Роботизированная КПП из МКПП под управлением Ардуино

[kadushkin]

Реализованный проект: http://compcar.ru/forum/showthread.p...l=1#post109972

Собственно задача, переделать механическую КПП в роботизированную.
1ый этап: замена ручного привода (тросов переключения) КПП на эл. приводы под управлением ардуино.
В качестве приводов используются шаговые двигатели и контроллеры к ним, в качестве датчиков положения - датчики холла. Управляет переключением шифтер от игрового руля Logitech.

Механизм переключения до переделки:


После переделки с управлением от ардуины


Код:

#include <EEPROM.h>
#include <SPI.h>
#include <Mystepper.h>
#include <MsTimer2.h>
#include <avr/wdt.h>  

// Программа для управления МКПП FM42 автомобиля
// В качестве привода используются: шаговый двигатель с редуктором (первый привод) и шаговый двигатель с винтовой передачей (второй привод)
// Первый привод необходим непосредственно для переключения вилок КПП, он является силовым
// Второй привод - для перемещения между вилками КПП
// Каждый привод контролируется тремя датчиками холла - для отстеживания положений
// Кроме того, для блокировки включения задней и пятой передачи (от случайного попадания) имеется актуатор

#define DEBUG 1  // используется при отладке

////// ********Описание периферии********** ///////////
// Контакт 0 для  - зарезервировано, не использовать!!!
// Контакт 1 для  - зарезервировано, не использовать!!!
#define  TahometrPin 2  // Контакт 2 для тахометра !!!!!!!!! ПРЕРЫВАНИЕ 0 
#define  SpeedSensorPin 3  // Контакт 3 для датчика скорости !!!!!!!!!ПРЕРЫВАНИЕ 1
#define  Motor1_RelayPin 4// Контакт 4 для  реле 1 блока-реле1 - реле включения контроллера шаг. двигателя первого привода
#define  GearBlock5R_RelayPin 5// Контакт 5 для  реле 2 блока-реле1
// Контакт 6 для  реле 3 блока-реле1
// Контакт 7 для  реле 4 блока-реле1
// Контакт 8 для !!!!!!!!! свободно 
// Контакт 9 для !!!!!!!!! свободно 
// Контакт 10 для динамика
// Контакт 11 для !!!!!!!!! свободно 
// Контакт 12 для !!!!!!!!! свободно 
#define  LedPin 13  // Контакт 13 для светодиода
#define  HandBreakPin 14  // Контакт 4 для сигнала от ручного тормоза
#define  SmallLightPin 15  // Контакт 5 для сигнала от габаритов
#define  LeftTurnPin 16  // Контакт 6 для сигнала от левого поворотника
#define  RightTurnPin 17  // Контакт 7 для сигнала от правого поворотника
// Контакт 18 для !!!!!!!!!ПРЕРЫВАНИЕ 5 !!!!!!!!! свободно 
// Контакт 19 для !!!!!!!!!ПРЕРЫВАНИЕ 4 !!!!!!!!! свободно 
#define  ClutchONButtonPin 20 // Контакт 20 для кнопки выжима сцепления NO !!!!!!!!!ПРЕРЫВАНИЕ 3
#define  ClutchOFFButtonPin 21 // Контакт 21 для кнопки отжима сцепления NC !!!!!!!!!ПРЕРЫВАНИЕ 2
#define  Motor1HallSensor1Pin 22 // Контакт 22 для датчика холла1 первого привода
#define  Motor1HallSensor2Pin 23 // Контакт 23 для датчика холла2 первого привода
#define  Motor1HallSensor3Pin 24 // Контакт 24 для датчика холла3 первого привода
// Контакт 25 для !!!!!!!!! свободно 
#define  REG_LATCH 26 // Контакт 26 для "Защелка" для опроса кнопок шифтера 
#define  Motor2HallSensor4Pin 27 // Контакт 27 для датчика холла4 второго привода
#define  Motor2HallSensor5Pin 28 // Контакт 28 для датчика холла5 второго привода
#define  Motor2HallSensor6Pin 29 // Контакт 29 для датчика холла6 второго привода
#define  Motor2A1Pin 30 // Контакт 30 для второго привода
#define  Motor2B1Pin 31 // Контакт 31 для второго привода
#define  Motor2A2Pin 32 // Контакт 32 для второго привода
#define  Motor2B2Pin 33 // Контакт 33 для второго привода
// Контакт 34 для реле 5 блока-реле1
// Контакт 35 для реле 6 блока-реле1
// Контакт 36 для реле 7 блока-реле1
// Контакт 37 для реле 8 блока-реле1
#define  LCDGear1_Indicate_Pin 38 // Контакт 38 для индикации Передачи 1 на ЖКИ
#define  LCDGear2_Indicate_Pin 39 // Контакт 39 для индикации Передачи 2 на ЖКИ
#define  LCDGear3_Indicate_Pin 40 // Контакт 40 для индикации Передачи 3 на ЖКИ
#define  LCDGear4_Indicate_Pin 41 // Контакт 41 для индикации Передачи 4 на ЖКИ
#define  LCDGearD_Indicate_Pin 42 // Контакт 42 для индикации Передачи D на ЖКИ
#define  LCDGearN_Indicate_Pin 43 // Контакт 43 для индикации Передачи N на ЖКИ
#define  LCDGearR_Indicate_Pin 44 // Контакт 44 для индикации Передачи R на ЖКИ
#define  LCDGearP_Indicate_Pin 45 // Контакт 45 для индикации Передачи P на ЖКИ
#define  Motor1_Rotate_Side_Pin 46 // Контакт 46 для первого привода - направление вращения
#define  Motor1_Online_Direct_Pin 47 // Контакт 47 для первого привода - прямое управление
#define  Motor1_Frequency_Pin 48 // Контакт 48 для первого привода - частота импульсов
// Контакт 49 !!!!!!!!! свободно 
// Контакт 50 MISO - занято для опроса кнопок шифтера
// Контакт 51 MOSI - занято для опроса кнопок шифтера  - зарезервировано, не использовать!!!
// Контакт 52 SCK - занято для опроса кнопок шифтера 
// Контакт 53 SS - занято для опроса кнопок шифтера - зарезервировано, не использовать!!!

//Stepper Motor1(200, Motor1_Rotate_Side_Pin, Motor1_Frequency_Pin, Motor1_Online_Direct_Pin,   Motor1B2Pin);
//Stepper Motor1(200, Motor1_Rotate_Side_Pin,  Motor1_Online_Direct_Pin, Motor1_Frequency_Pin,  Motor1B2Pin);
Stepper Motor2(200, Motor2A1Pin, Motor2B1Pin, Motor2A2Pin,   Motor2B2Pin);

//аналоговые входы

#define  Shifter_AxeY 0 //A0 // Шифтер ось 1 Y
#define  Shifter_AxeX 1 //A1 // Шифтер ось 2 X
#define  CurrentSensor1Pin 2 //A2 //датчик тока1
#define  CurrentSensor2Pin 3 //A3 //датчик тока2
#define  WheelButtonsPin 6 //A6 // кнопки руля

//кнопки шифтера
byte Shifter_Button_Array[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; 
byte Shifter_Button_UP=0;
byte Shifter_Button_DOWN=1;
byte Shifter_Button_LEFT=2;
byte Shifter_Button_RIGHT=3;
byte Shifter_Button_Black3=4;
byte Shifter_Button_Black1=5;
byte Shifter_Button_Black4=6;
byte Shifter_Button_Black2=7;
byte Shifter_Button_Red1=8;
byte Shifter_Button_Red4=9;
byte Shifter_Button_Red2=10;
byte Shifter_Button_Red3=11;
byte Shifter_PlusMinusMode=12; //режим +-
byte Shifter_Button_Uknwn1=13;
byte Shifter_RearGearMode=14; //режим задней передачи (рычаг утоплен)
byte Shifter_Button_Uknwn2=15;

int CurrentSensor1Value=0;
int CurrentSensor2Value=0;

//описание переменных
//boolean RelayOn = false;

volatile unsigned long Speed_ImpulseTime = 0; // время между импульсами датчика скорости
volatile unsigned long Speed_time=0;  // время для отсчета импульсов
volatile int Speed_impulse_count = 0;  // кол-во импульсов датчика скорости
volatile float Speed=0; // скорость с датчика скорости

volatile unsigned long Taho_ImpulseTime = 0; // время между импульсами тахометра
volatile unsigned long Taho_time=0;  // время для отсчета импульсов
volatile int Tahometr_impulse_count = 0;  // кол-во импульсов тахометра
volatile float Taho=0; // обороты двигателя

char CurGear='0';
char LastGear='0';
char RecommendGear='0';

//byte GearLastName=0;
byte P_1=1;
byte P_2=2;
byte P_3=3;
byte P_4=4;
byte P_5=5;
byte P_R=255;
byte P_N12=12;
byte P_N34=34;
byte P_N5R=250;
byte P_MINUS=127;
byte P_PLUS=128;

int EEPROMaddrLastMotor1HallSensorNum=0; //адрес в энергонезавсимой памяти куда заносится значение положение привода 1 согласно датчикам холла 1,2 или 3 
int EEPROMaddrLastMotor2HallSensorNum=1; //адрес в энергонезавсимой памяти куда заносится значение положение привода 2 согласно датчикам холла 4,5 или 6
int EEPROMaddrLastSuccessGearPosition=2; //адрес в энергонезавсимой памяти куда заносится значение последней удачно включенной передачи
int EEPROMaddrLastFrontGearPosition=3; //адрес в энергонезавсимой памяти куда заносится значение последней удачно включенной передачи в режиме ( без нейтрали и задней передачи)

// переменные нажатия рулевых кнопок
unsigned long math_time,old_time,real_time, res_dt1, res_d;     
int LIMIT=10;
char ButtonName='X'; // +, -, >, R=>>|, L=|<<, S=SRC, X=None
// переменные нажатия рулевых кнопок - конец



//###################################################################
//общие настройки 
void setup() 
{
  wdt_enable (WDTO_8S); // сторожевой таймер от зависания
 // Shifter_Button_Array[Shifter_Button_UP]=1;
 
// MsTimer2::set(500, Timer2_Timer); // таймер будет срабатывать через каждые 500ms 
// MsTimer2::start();//запустить таймер

//*************************Описание пинов**************************
// Контакт 0 для  - зарезервировано, не использовать!!!
// Контакт 1 для  - зарезервировано, не использовать!!!
//2
pinMode(TahometrPin, INPUT); // Контакт для тахометра !!!!!!!!!ПРЕРЫВАНИЕ 0
digitalWrite(TahometrPin, HIGH); //внутренняя подтяжка входа тахометра 
attachInterrupt(0, TahometrImpulse_on, FALLING); // настройка прерывания и функции на него
//3
pinMode(SpeedSensorPin, INPUT); // Контакт для датчика скорости !!!!!!!!!ПРЕРЫВАНИЕ 1
digitalWrite(SpeedSensorPin, HIGH); //внутренняя подтяжка входа датчика скорости 
attachInterrupt(1, SpeedImpulse_on, FALLING); // настройка прерывания и функции на него

// Контакт 4 для реле 1 блока-реле1
pinMode(Motor1_RelayPin, OUTPUT);
// Контакт 5 для реле 2 блока-реле1
pinMode(GearBlock5R_RelayPin, OUTPUT);
// Контакт 6 для реле 3 блока-реле1
// Контакт 7 для реле 4 блока-реле1
// Контакт 8 для !!!!!!!!! свободно 
// Контакт 9 для !!!!!!!!! свободно 
// Контакт 10 для динамика
// Контакт 11 для !!!!!!!!! свободно 
// Контакт 12 для !!!!!!!!! свободно 
//13
pinMode(LedPin, OUTPUT); // Контакт для светодиода
//14
pinMode(HandBreakPin, INPUT);// Контакт для сигнала от ручного тормоза
digitalWrite(HandBreakPin, HIGH); //внутренняя подтяжка входа +++++ 
//15
pinMode(SmallLightPin, INPUT);// Контакт для сигнала от габаритов
digitalWrite(SmallLightPin, HIGH); //внутренняя подтяжка входа +++++ 
//16
pinMode(LeftTurnPin, INPUT);// Контакт 6 для сигнала от левого поворотника
digitalWrite(LeftTurnPin, HIGH); //внутренняя подтяжка входа +++++ 
//17
pinMode(RightTurnPin, INPUT);// Контакт 7 для сигнала от правого поворотника
digitalWrite(RightTurnPin, HIGH); //внутренняя подтяжка входа +++++ 
// Контакт 18 для !!!!!!!!!ПРЕРЫВАНИЕ 5 !!!!!!!!! свободно 
// Контакт 19 для !!!!!!!!!ПРЕРЫВАНИЕ 4 !!!!!!!!! свободно 
//20
pinMode(ClutchONButtonPin, INPUT);// Контакт для кнопки выжима сцепления NO !!!!!!!!!ПРЕРЫВАНИЕ 3
digitalWrite(ClutchONButtonPin, HIGH); //внутренняя подтяжка входа +++++ 
//21
pinMode(ClutchOFFButtonPin, INPUT);// Контакт для кнопки отжима сцепления NC !!!!!!!!!ПРЕРЫВАНИЕ 2
digitalWrite(ClutchOFFButtonPin, HIGH); //внутренняя подтяжка входа +++++ 
//22
pinMode(Motor1HallSensor1Pin, INPUT);// Контакт для датчика холла1 первого привода
digitalWrite(Motor1HallSensor1Pin, HIGH); //внутренняя подтяжка входа +++++ 
//23
pinMode(Motor1HallSensor2Pin, INPUT);// Контакт для датчика холла2 первого привода
digitalWrite(Motor1HallSensor2Pin, HIGH); //внутренняя подтяжка входа +++++ 
//24
pinMode(Motor1HallSensor3Pin, INPUT);// Контакт для датчика холла3 первого привода
digitalWrite(Motor1HallSensor3Pin, HIGH); //внутренняя подтяжка входа +++++ 
// Контакт 25 для !!!!!!!!! свободно 
//26 
pinMode(REG_LATCH, OUTPUT); // Контакт 26 для "Защелка" для опроса кнопок шифтера 
digitalWrite(REG_LATCH, HIGH); //внутренняя подтяжка входа +++++ 
//27
pinMode(Motor2HallSensor4Pin, INPUT);// Контакт для датчика холла4 второго привода
digitalWrite(Motor2HallSensor4Pin, HIGH); //внутренняя подтяжка входа +++++ 
//28
pinMode(Motor2HallSensor5Pin, INPUT);// Контакт для датчика холла5 второго привода
digitalWrite(Motor2HallSensor5Pin, HIGH); //внутренняя подтяжка входа +++++ 
//29
pinMode(Motor2HallSensor6Pin, INPUT);// Контакт для датчика холла6 второго привода
digitalWrite(Motor2HallSensor6Pin, HIGH); //внутренняя подтяжка входа +++++ 
//30
pinMode(Motor2A1Pin, OUTPUT);// Контакт для второго привода
//31
pinMode(Motor2B1Pin, OUTPUT);// Контакт для второго привода
//32
pinMode(Motor2A2Pin, OUTPUT);// Контакт для второго привода
//33
pinMode(Motor2B2Pin, OUTPUT);// Контакт для второго привода
// Контакт 34 для реле 5 блока-реле1
// Контакт 35 для реле 6 блока-реле1
// Контакт 36 для реле 7 блока-реле1
// Контакт 37 для реле 8 блока-реле1
//38
pinMode(LCDGear1_Indicate_Pin, OUTPUT);// Контакт для индикации Передачи 1 на ЖКИ
//39
pinMode(LCDGear2_Indicate_Pin, OUTPUT);// Контакт для индикации Передачи 2 на ЖКИ
//40
pinMode(LCDGear3_Indicate_Pin, OUTPUT);// Контакт для индикации Передачи 3 на ЖКИ
//41
pinMode(LCDGear4_Indicate_Pin, OUTPUT);// Контакт для индикации Передачи 4 на ЖКИ
//42
pinMode(LCDGearD_Indicate_Pin, OUTPUT);// Контакт для индикации Передачи D на ЖКИ
//43
pinMode(LCDGearN_Indicate_Pin, OUTPUT);// Контакт для индикации Передачи N на ЖКИ
//44
pinMode(LCDGearR_Indicate_Pin, OUTPUT);// Контакт для индикации Передачи R на ЖКИ
//45
pinMode(LCDGearP_Indicate_Pin, OUTPUT);// Контакт для индикации Передачи P на ЖКИ
//46
pinMode(Motor1_Rotate_Side_Pin, OUTPUT);// Контакт  для первого привода - направление вращения
//47
pinMode(Motor1_Online_Direct_Pin, OUTPUT);// Контакт  для первого привода - прямое управление
//48
pinMode(Motor1_Frequency_Pin, OUTPUT);// Контакт  для первого привода - частота импульсов
// Контакт 49 !!!!!!!!! свободно 
SPI.begin(); //задействуем пины 50-53
// Контакт 50 MISO - занято для опроса кнопок шифтера
// Контакт 51 MOSI - занято для опроса кнопок шифтера  - зарезервировано, не использовать!!!
// Контакт 52 SCK - занято для опроса кнопок шифтера 
// Контакт 53 SS - занято для опроса кнопок шифтера - зарезервировано, не использовать!!!


// параметры моторов, скорости вращения об/мин
//Motor1.setSpeed(100);  
Motor2.setSpeed(300);  

//гасим ЖКИ 
Gear_LCD_Clear();
//


#if DEBUG
Serial.begin(115200); 
#endif
  
}

//###################################################################       
//функция возвращает положение рычага шифтера КПП       
byte GetShifterPosition()
{
uint16_t ShifterButtonsStatus=Read_ShifterButtonsStatus();
ShifterButtonsStatus >>= Shifter_PlusMinusMode; 
byte Shifter_PlusMinusModeStatus=ShifterButtonsStatus & 1;  
//Serial.println(analogRead(0)); //Y
// Serial.println(analogRead(1)); //X
byte ShifterPosition=0;
int X_Position=analogRead(Shifter_AxeX); 
int Y_Position=analogRead(Shifter_AxeY);

/*
Serial.print("X=");
Serial.println(X_Position);
Serial.print("Y=");
Serial.println(Y_Position);
*/

//uint16_t ButtonsStatus=Read_ShifterButtonsStatus();
//ButtonsStatus[]
if (Shifter_PlusMinusModeStatus==0) // обычный режим переключения КПП - ручной (не +-)
{
if ((X_Position<320) && (Y_Position>600)) {ShifterPosition=P_1;}
if ((X_Position<320) && (Y_Position<100)) {ShifterPosition=P_2;}
if (((X_Position>320) && (X_Position<520)) && (Y_Position>600)) {ShifterPosition=P_3;}
if (((X_Position>320) && (X_Position<520)) && (Y_Position<100)) {ShifterPosition=P_4;}
if ((X_Position>520) && (Y_Position>600)) {ShifterPosition=P_5;}
if ((X_Position>520) && (Y_Position<100)) {ShifterPosition=P_R;}
//if ((Y_Position>100) && (Y_Position<600)) {ShifterPosition=P_N;}
if ((X_Position<320) && ((Y_Position>100) && (Y_Position<600))) {ShifterPosition=P_N12;}
if (((X_Position>320) && (X_Position<520)) && (Y_Position>100) && (Y_Position<600)) {ShifterPosition=P_N34;}
if ((X_Position>520) && ((Y_Position>100) && (Y_Position<600))) {ShifterPosition=P_N5R;}
}

//+-
if (Shifter_PlusMinusModeStatus==1) // режим переключения КПП - спортивный (+-)
{
if (Y_Position>500) {ShifterPosition=P_MINUS;}
if (Y_Position<300) {ShifterPosition=P_PLUS;}
//if ((Y_Position>300) && (Y_Position<500)) {ShifterPosition='N';}
}

return ShifterPosition;
} 

//###################################################################
//функция возвращает состояние изменившихся кнопок шифтера
uint16_t Read_ShifterButtonsStatus() 
  {

static  uint16_t Last_Shifter_Buttons_States = 0; // статус четырех нижних (красных) кнопок и восьми верхних и средних (черных) кнопок
  uint16_t Shifter_Buttons_Changes =0;
  
  digitalWrite(REG_LATCH, LOW);  //Выставим на защёлке сначала низкий, потом - высокий уровни.
  digitalWrite(REG_LATCH, HIGH); //Сдвиговый регистр запомнит уровни сигналов на входах и сможет их нам потом отдать бит за би

  uint16_t Shifter_Buttons_States= (SPI.transfer(0) <<8 ) | SPI.transfer(0); //Читаем запомненные состояния четырех нижних (красных) кнопок и восьми верхних и средних (черных) кнопок

  if (Shifter_Buttons_States != Last_Shifter_Buttons_States)
  {
    Shifter_Buttons_Changes = Shifter_Buttons_States ^ Last_Shifter_Buttons_States;
    Last_Shifter_Buttons_States = Shifter_Buttons_States; // запоминаем текущие состояния
    
//#if DEBUG      
//Serial.println(Shifter_Buttons_States, BIN);
//#endif


  /*
    for (int i = 0; i < 16; ++i)
    {
      if (Shifter_Buttons_Changes & 1) // если состояние кнопки изменилось…
      {
        
     /*  Serial.print("#");
        Serial.print(i); // шлём номер кнопки
        Serial.print(" -> ");
        Serial.println(states & 1); // … и новое состояние
      }

      Shifter_Buttons_Changes >>= 1; // Сдвигаем биты вправо
      Shifter_Buttons_States >>= 1; // Сдвигаем биты вправо
    }
    */
  }
  return Shifter_Buttons_States; //Shifter_Buttons_Changes;
 }

//################################################################### 
//гасим ЖКИ  
void Gear_LCD_Clear()
{
digitalWrite(LCDGear1_Indicate_Pin, LOW);
digitalWrite(LCDGear2_Indicate_Pin, LOW);
digitalWrite(LCDGear3_Indicate_Pin, LOW);
digitalWrite(LCDGear4_Indicate_Pin, LOW);
digitalWrite(LCDGearD_Indicate_Pin, LOW);
digitalWrite(LCDGearN_Indicate_Pin, LOW);
digitalWrite(LCDGearR_Indicate_Pin, LOW);
digitalWrite(LCDGearP_Indicate_Pin, LOW);
}
 
//################################################################### 
 void Indicate_Gear_On_LCD(byte GearName)
{
Gear_LCD_Clear();  
if (GearName==P_1){digitalWrite(LCDGear1_Indicate_Pin, HIGH);}
if (GearName==P_2){digitalWrite(LCDGear2_Indicate_Pin, HIGH);}
if (GearName==P_3){digitalWrite(LCDGear3_Indicate_Pin, HIGH);}
if (GearName==P_4){digitalWrite(LCDGear4_Indicate_Pin, HIGH);}
if (GearName==P_5){digitalWrite(LCDGearD_Indicate_Pin, HIGH);}
if (GearName==P_R){digitalWrite(LCDGearR_Indicate_Pin, HIGH);}
if (GearName==P_N12){digitalWrite(LCDGearN_Indicate_Pin, HIGH);}
if (GearName==P_N34){digitalWrite(LCDGearN_Indicate_Pin, HIGH);}
if (GearName==P_N5R){digitalWrite(LCDGearN_Indicate_Pin, HIGH);}
}

//###################################################################
void Indicate_Recommended_Gear(float Speed_kmh)
{
  
if (Speed_kmh<=0) {RecommendGear='N';} // реле N
if ((Speed_kmh>0) && (Speed_kmh<=7)) {RecommendGear='1';} // реле   1 
if ((Speed_kmh>10) && (Speed_kmh<=22)) {RecommendGear='2';} // реле   2
if ((Speed_kmh>25) && (Speed_kmh<=40)) {RecommendGear='3';} // реле   3
if ((Speed_kmh>43) && (Speed_kmh<=85)) {RecommendGear='4';} // реле   4
if (Speed_kmh>90) {RecommendGear='D';} // реле   5


if (LastGear!=RecommendGear)
{
if (LastGear=='1'){digitalWrite(LCDGear1_Indicate_Pin, HIGH);}
if (LastGear=='2'){digitalWrite(LCDGear2_Indicate_Pin, HIGH);}
if (LastGear=='3'){digitalWrite(LCDGear3_Indicate_Pin, HIGH);}
if (LastGear=='4'){digitalWrite(LCDGear4_Indicate_Pin, HIGH);}
if (LastGear=='D'){digitalWrite(LCDGearD_Indicate_Pin, HIGH);}
if (LastGear=='N'){digitalWrite(LCDGearN_Indicate_Pin, HIGH);}
if (LastGear=='R'){digitalWrite(LCDGearR_Indicate_Pin, HIGH);}
if (LastGear=='P'){digitalWrite(LCDGearP_Indicate_Pin, HIGH);}

if (RecommendGear=='1'){digitalWrite(LCDGear1_Indicate_Pin, LOW);}
if (RecommendGear=='2'){digitalWrite(LCDGear2_Indicate_Pin, LOW);}
if (RecommendGear=='3'){digitalWrite(LCDGear3_Indicate_Pin, LOW);}
if (RecommendGear=='4'){digitalWrite(LCDGear4_Indicate_Pin, LOW);}
if (RecommendGear=='D'){digitalWrite(LCDGearD_Indicate_Pin, LOW);}
if (RecommendGear=='N'){digitalWrite(LCDGearN_Indicate_Pin, LOW);}
if (RecommendGear=='R'){digitalWrite(LCDGearR_Indicate_Pin, LOW);}
if (RecommendGear=='P'){digitalWrite(LCDGearP_Indicate_Pin, LOW);}

LastGear=RecommendGear;
}  
}



//###################################################################
// Функция привода переключения передач (1ый привод), можно передавать значения только Motor1HallSensor1Pin, Motor1HallSensor2Pin, Motor1HallSensor3Pin
// runaway - шаги для перебега (точное позиционирование после срабатывания датчика холла) 
// holding_time_msec - время (мс) удержания привода после прихода в нужное положение
byte Change_Gear_Motor_On(int steps, int to_Position, int runaway, int holding_time_msec)
{ 
int i_runaway=0;
byte Rezult=0;
int ttime=millis(); 
boolean RelayToDo=true;
int stepped=1; 
int stepcount=abs(steps);
if ((steps>0) && (digitalRead(Motor1HallSensor2Pin)==0)) {RelayToDo=false;} // дальше крутить в эту сторону нельзя - конечное положение
if ((steps<0) && (digitalRead(Motor1HallSensor1Pin)==0)) {RelayToDo=false;} // дальше крутить в эту сторону нельзя - конечное положение
if (digitalRead(to_Position)==0) {RelayToDo=false;} //уже достигли нужного положения по датчику холла

if (RelayToDo) 
    { //1
digitalWrite(Motor1_RelayPin, HIGH); //включаем реле 
if (steps>0) {digitalWrite(Motor1_Rotate_Side_Pin, HIGH);} // задаем сторону вращения
if (steps<0) {digitalWrite(Motor1_Rotate_Side_Pin, LOW);} // задаем сторону вращения
digitalWrite(Motor1_Online_Direct_Pin, HIGH); // прямое управление контроллером шагового двигателя с ардуино
//int TimeBTWNImpulse=3000; //время между импульсами (шагами) - скорость вращения
int TimeBTWNImpulse=1000; //время между импульсами (шагами) - скорость вращения
  
  
while (stepped<=stepcount)
  {
//if (stepped>50) {TimeBTWNImpulse=2000;} //ускорение 1ая ступень
//if (stepped>100) {TimeBTWNImpulse=1500;} //ускорение 2ая ступень
//if (stepped>150) {TimeBTWNImpulse=1000;} //ускорение 3ья ступень
digitalWrite(Motor1_Frequency_Pin, HIGH); //импульс
 delayMicroseconds(TimeBTWNImpulse);
 digitalWrite(Motor1_Frequency_Pin, LOW);
 stepped++; 
if ((steps>0) && (digitalRead(Motor1HallSensor2Pin)==0)) {stepped=stepcount+1;} // дальше крутить в эту сторону нельзя - конечное положение
if ((steps<0) && (digitalRead(Motor1HallSensor1Pin)==0)) {stepped=stepcount+1;} // дальше крутить в эту сторону нельзя - конечное положение
if (digitalRead(to_Position)==0) {i_runaway++; if (i_runaway>=runaway) {stepped=stepcount+1;}} //уже достигли нужного положения по датчику холла
  }
    } //1  
 digitalWrite(Motor1_Frequency_Pin, LOW);
 delay(holding_time_msec); // выдержка (удержание) привода
 digitalWrite(Motor1_RelayPin, LOW); //выключаем реле
//Serial.println( millis()-ttime);  
delay(10); //даем время для адаптации датчикам холла

Rezult=GetCurrentGearPosition();

if (Rezult!=0) {SetSuccessGearPosition_to_EEPROM(Rezult);}

if ((Rezult>=P_1) && (Rezult<=P_5)) {SetFrontGearPosition_to_EEPROM(Rezult);}

return Rezult;
       	  }
       
//###################################################################       
// Функция привода выбора передач (2ой привод), можно передавать значения только Motor2HallSensor4Pin, Motor2HallSensor5Pin, Motor2HallSensor6Pin 
// runaway - шаги для перебега (точное позиционирование после срабатывания датчика холла) 
void Select_Gear_Motor_On(int steps, int to_Position, int runaway)
{ 
boolean NeedToDo=true;   
if ((steps>0) && (digitalRead(Motor2HallSensor5Pin)==0)) {NeedToDo=false;} // дальше крутить в эту сторону нельзя - конечное положение
if ((steps<0) && (digitalRead(Motor2HallSensor6Pin)==0)) {NeedToDo=false;} // дальше крутить в эту сторону нельзя - конечное положение
if (digitalRead(to_Position)==0) {NeedToDo=false;} //уже достигли нужного положения по датчику холла
  
if (NeedToDo) {Motor2.step(steps, to_Position, runaway);}
}  
       

      

//###################################################################
//функция возвращает текущую позицию КПП по датчикам 
//возвращает значения 1,2,3,4,5,R=255,12,34,250
// при неудаче возвращает 0
byte GetCurrentGearPosition()
{
 
byte Rezult=0; //неопределенное положение
int c=0; // количество сработавших датчиков положения (холла)
if ((digitalRead(Motor1HallSensor1Pin)==0) && (digitalRead(Motor2HallSensor6Pin)==0)) {Rezult=P_1;}
if ((digitalRead(Motor1HallSensor2Pin)==0) && (digitalRead(Motor2HallSensor6Pin)==0)) {Rezult=P_2;}
if ((digitalRead(Motor1HallSensor1Pin)==0) && (digitalRead(Motor2HallSensor4Pin)==0)) {Rezult=P_3;}
if ((digitalRead(Motor1HallSensor2Pin)==0) && (digitalRead(Motor2HallSensor4Pin)==0)) {Rezult=P_4;}
if ((digitalRead(Motor1HallSensor1Pin)==0) && (digitalRead(Motor2HallSensor5Pin)==0)) {Rezult=P_5;}
if ((digitalRead(Motor1HallSensor2Pin)==0) && (digitalRead(Motor2HallSensor5Pin)==0)) {Rezult=P_R;} // задняя передача

if ((digitalRead(Motor1HallSensor3Pin)==0) && (digitalRead(Motor2HallSensor6Pin)==0)) {Rezult=P_N12;}
if ((digitalRead(Motor1HallSensor3Pin)==0) && (digitalRead(Motor2HallSensor4Pin)==0)) {Rezult=P_N34;}
if ((digitalRead(Motor1HallSensor3Pin)==0) && (digitalRead(Motor2HallSensor5Pin)==0)) {Rezult=P_N5R;}

if (digitalRead(Motor1HallSensor1Pin)==0) {c++;}
if (digitalRead(Motor1HallSensor2Pin)==0) {c++;}
if (digitalRead(Motor1HallSensor3Pin)==0) {c++;}
if (digitalRead(Motor2HallSensor4Pin)==0) {c++;}
if (digitalRead(Motor2HallSensor5Pin)==0) {c++;}
if (digitalRead(Motor2HallSensor6Pin)==0) {c++;}

if (c!=2) {Rezult=0;} //неопределенное положение
//Serial.println(c); /////////////
return Rezult;  
}


//###################################################################
//функция извлекающая последнюю успешную передачу КПП, последнее определенное положение КПП
//возвращает значения 1,2,3,4,5,R=255,12,34,250
// 0 при отсутствии информации о последнем положении
byte GetLastSuccessGearPosition()
{
byte Rezult=0;
Rezult=EEPROM.read(EEPROMaddrLastSuccessGearPosition);
return Rezult;  
}

byte GetLastFrontGearPosition()
{
byte Rezult=0;
Rezult=EEPROM.read(EEPROMaddrLastFrontGearPosition);
return Rezult;  
}

//процедура записывающая в память включенную передачу
void SetSuccessGearPosition_to_EEPROM(byte Gearvalue)
{
EEPROM.write(EEPROMaddrLastSuccessGearPosition, Gearvalue);
}

void SetFrontGearPosition_to_EEPROM(byte Gearvalue)
{
EEPROM.write(EEPROMaddrLastFrontGearPosition, Gearvalue);
}




//###################################################################  
//функция перевода в нейтральную поперечную линию между 1-3-5 и 2-4-R
//возвращает 0 при неудаче
// 12 при переходе в продольную позицию между 1-2
// 34 при переходе в продольную позицию между 3-4
// 250 при переходе в продольную позицию между 5-R
byte Go_To_Neutral_Line()
{
byte Rezult=0;

byte CurrentGearPosition=GetCurrentGearPosition();
if (CurrentGearPosition==0) {CurrentGearPosition=SearchingGear();}

if ((CurrentGearPosition==P_1) || (CurrentGearPosition==P_3)) {Change_Gear_Motor_On(200, Motor1HallSensor3Pin,18, 10);}
if ((CurrentGearPosition==P_2) || (CurrentGearPosition==P_4)) {Change_Gear_Motor_On(-200, Motor1HallSensor3Pin,14, 10);}
if (CurrentGearPosition==P_5) { if (Gear_Unlock_R_Block_5(false)) {Change_Gear_Motor_On(200, Motor1HallSensor3Pin,18, 10);}}
if (CurrentGearPosition==P_R) { if (Gear_Unlock_R_Block_5(true)) {Change_Gear_Motor_On(-200, Motor1HallSensor3Pin,14, 10); Gear_Unlock_R_Block_5(false);}}

if (CurrentGearPosition==0)
{
byte LastGearPosition=GetLastSuccessGearPosition();
if ((LastGearPosition==P_1) || (LastGearPosition==P_3)) {Change_Gear_Motor_On(200, Motor1HallSensor3Pin,18, 10);}
if ((LastGearPosition==P_2) || (LastGearPosition==P_4)) {Change_Gear_Motor_On(-200, Motor1HallSensor3Pin,14, 10);}
if (LastGearPosition==P_5) { if (Gear_Unlock_R_Block_5(false)) {Change_Gear_Motor_On(200, Motor1HallSensor3Pin,18, 10);}}
if (LastGearPosition==P_R) { if (Gear_Unlock_R_Block_5(true)) {Change_Gear_Motor_On(-200, Motor1HallSensor3Pin,14, 10); Gear_Unlock_R_Block_5(false);}}

}

Rezult=GetCurrentGearPosition();

if (Rezult!=0) {SetSuccessGearPosition_to_EEPROM(Rezult);}
return Rezult;
} 


// функция поиска передач - при неопределенном положении
byte SearchingGear()
{
byte CurrentGearPosition=GetCurrentGearPosition();  
if (CurrentGearPosition!=0)  {return CurrentGearPosition;}


int c=0;
if (digitalRead(Motor1HallSensor1Pin)==0) {c++;}
if (digitalRead(Motor1HallSensor2Pin)==0) {c++;}
if (digitalRead(Motor1HallSensor3Pin)==0) {c++;}
if (digitalRead(Motor2HallSensor4Pin)==0) {c++;}
if (digitalRead(Motor2HallSensor5Pin)==0) {c++;}
if (digitalRead(Motor2HallSensor6Pin)==0) {c++;}
if (c>2) {return 0;} //неопределенное положение из-за залипшего датчика холла - тупиковая ситуация

byte GearLastName=GetLastSuccessGearPosition();

if (c<=0) //*************** самый сложный случай ни один датчик не сработал, положение потеряно полностью
{
if ((GearLastName==P_1) || (GearLastName==P_3) || (GearLastName==P_5)) {Change_Gear_Motor_On(200, Motor1HallSensor3Pin,18, 10); }
if ((GearLastName==P_2) || (GearLastName==P_5) || (GearLastName==P_R)) {Change_Gear_Motor_On(-200, Motor1HallSensor3Pin,14, 10); }
if ((GearLastName==P_N12) || (GearLastName==P_N34) || (GearLastName==P_N5R)) {Change_Gear_Motor_On(200, Motor1HallSensor3Pin,18, 10); }
}

if (digitalRead(Motor1HallSensor1Pin)==0)  
{
Change_Gear_Motor_On(200, Motor1HallSensor3Pin,18, 10); 
if ((GearLastName==P_1) || (GearLastName==P_2) || (GearLastName==P_N12) ) {Select_Gear_Motor_On(-500, Motor2HallSensor6Pin, 30);}
if ((GearLastName==P_3) || (GearLastName==P_4) || (GearLastName==P_N34) ) {Select_Gear_Motor_On(-250, Motor2HallSensor4Pin, 40);}
if ((GearLastName==P_5) || (GearLastName==P_R) || (GearLastName==P_N5R) ) {Select_Gear_Motor_On(500, Motor2HallSensor5Pin, 30);}
return GetCurrentGearPosition();
} 

if (digitalRead(Motor1HallSensor2Pin)==0)  
{
Change_Gear_Motor_On(-200, Motor1HallSensor3Pin,14, 10); 
if ((GearLastName==P_1) || (GearLastName==P_2) || (GearLastName==P_N12) ) {Select_Gear_Motor_On(-500, Motor2HallSensor6Pin, 30);}
if ((GearLastName==P_3) || (GearLastName==P_4) || (GearLastName==P_N34) ) {Select_Gear_Motor_On(-250, Motor2HallSensor4Pin, 40);}
if ((GearLastName==P_5) || (GearLastName==P_R) || (GearLastName==P_N5R) ) {Select_Gear_Motor_On(500, Motor2HallSensor5Pin, 30);}
return GetCurrentGearPosition();
} 

if (digitalRead(Motor1HallSensor3Pin)==0)  
{
if ((GearLastName==P_1) || (GearLastName==P_2) || (GearLastName==P_N12) ) {Select_Gear_Motor_On(-500, Motor2HallSensor6Pin, 30);}
if ((GearLastName==P_3) || (GearLastName==P_4) || (GearLastName==P_N34) ) {Select_Gear_Motor_On(-250, Motor2HallSensor4Pin, 40);}
if ((GearLastName==P_5) || (GearLastName==P_R) || (GearLastName==P_N5R) ) {Select_Gear_Motor_On(500, Motor2HallSensor5Pin, 30);}

return GetCurrentGearPosition();
} 

if ((digitalRead(Motor2HallSensor4Pin)==0) || (digitalRead(Motor2HallSensor5Pin)==0) || (digitalRead(Motor2HallSensor6Pin)==0))  //12 34 5R
{
if ((GearLastName==P_1) || (GearLastName==P_3) || (GearLastName==P_5)) {Change_Gear_Motor_On(200, Motor1HallSensor3Pin,18, 10); }
if ((GearLastName==P_2) || (GearLastName==P_5) || (GearLastName==P_R)) {Change_Gear_Motor_On(-200, Motor1HallSensor3Pin,14, 10); }
if ((GearLastName==P_N12) || (GearLastName==P_N34) || (GearLastName==P_N5R)) {Change_Gear_Motor_On(200, Motor1HallSensor3Pin,18, 10); }
return GetCurrentGearPosition();
}  



}

//###################################################################
//функция выбора передач по линии нейтрали (влево-вправо)
// самостоятельно не вызывать - вызывается из функции Go_To_Gear
byte SelectGear (byte ToNeutralLine)
{
byte Rezult=0;
byte CurrentGearPosition=GetCurrentGearPosition();

if (CurrentGearPosition==0) {CurrentGearPosition=SearchingGear();}

if (CurrentGearPosition==ToNeutralLine) {return CurrentGearPosition;}
if (ToNeutralLine==P_N12)
 {
if (CurrentGearPosition==P_N34) {Select_Gear_Motor_On(-250, Motor2HallSensor6Pin, 30);}
if (CurrentGearPosition==P_N5R) {Select_Gear_Motor_On(-500, Motor2HallSensor6Pin, 30);}

if (CurrentGearPosition==0)
{
byte LastGearPosition=GetLastSuccessGearPosition();
if (LastGearPosition==P_N34) {Select_Gear_Motor_On(-250, Motor2HallSensor6Pin, 30);}
if (LastGearPosition==P_N5R) {Select_Gear_Motor_On(-500, Motor2HallSensor6Pin, 30);}
} 
 }
 
 if (ToNeutralLine==P_N34)
 {
if (CurrentGearPosition==P_N12) {Select_Gear_Motor_On(250, Motor2HallSensor4Pin, 40);}
if (CurrentGearPosition==P_N5R) {Select_Gear_Motor_On(-250, Motor2HallSensor4Pin, 40);}

if (CurrentGearPosition==0)
{
byte LastGearPosition=GetLastSuccessGearPosition();
if (LastGearPosition==P_N12) {Select_Gear_Motor_On(250, Motor2HallSensor4Pin, 40);}
if (LastGearPosition==P_N5R) {Select_Gear_Motor_On(-250, Motor2HallSensor4Pin, 40);}
} 
 }
 
 if (ToNeutralLine==P_N5R)
 {
if (CurrentGearPosition==P_N12) {Select_Gear_Motor_On(500, Motor2HallSensor5Pin, 30);}
if (CurrentGearPosition==P_N34) {Select_Gear_Motor_On(250, Motor2HallSensor5Pin, 30);}

if (CurrentGearPosition==0)
{
byte LastGearPosition=GetLastSuccessGearPosition();
if (LastGearPosition==P_N12) {Select_Gear_Motor_On(500, Motor2HallSensor5Pin, 30);}
if (LastGearPosition==P_N34) {Select_Gear_Motor_On(250, Motor2HallSensor5Pin, 30);}
} 
 }

Rezult=GetCurrentGearPosition();
if (Rezult!=0) {SetSuccessGearPosition_to_EEPROM(Rezult);}
return Rezult;
}

//###################################################################
//функция разблокировки задней передачи (блокировки 5ой)
boolean Gear_Unlock_R_Block_5(boolean Unlock_R_block_5)
{
if (Unlock_R_block_5) {digitalWrite(GearBlock5R_RelayPin, HIGH);}
if (!Unlock_R_block_5) {digitalWrite(GearBlock5R_RelayPin, LOW);}
return true; // встроить сюда проверку при срабатывании актуатора
}

//###################################################################
//функция переключения передачи  
//сначала производится перевод в нейтральную линию
//затем выбор передачи по нейтральной линии
//затем при необходимости разблокировки аередачи (задней или пятой)
//и уже потом перевод в нужное положение
boolean Go_To_Gear(byte GearName)
{
byte CurrentGearPosition=GetCurrentGearPosition();
if (CurrentGearPosition==GearName) {return true;}
if (CurrentGearPosition==0) {CurrentGearPosition=SearchingGear();}
  
byte SetNeutral=Go_To_Neutral_Line(); //переводим в нейтраль
if ((SetNeutral==P_N12) || (SetNeutral==P_N34) || (SetNeutral==P_N5R))
{ //1
if (GearName==P_1) {if (SelectGear(P_N12)==P_N12) {if (Change_Gear_Motor_On(-400, Motor1HallSensor1Pin,0, 100)==GearName){return true;}}}
if (GearName==P_2) {if (SelectGear(P_N12)==P_N12) {if (Change_Gear_Motor_On(400, Motor1HallSensor2Pin,0, 100)==GearName){return true;}}}

if (GearName==P_3) {if (SelectGear(P_N34)==P_N34) {if (Change_Gear_Motor_On(-400, Motor1HallSensor1Pin,0, 100)==GearName){return true;}}}
if (GearName==P_4) {if (SelectGear(P_N34)==P_N34) {if (Change_Gear_Motor_On(400, Motor1HallSensor2Pin,0, 100)==GearName){return true;}}}

if (GearName==P_5) {if (SelectGear(P_N5R)==P_N5R) {if (Gear_Unlock_R_Block_5(false)) {if (Change_Gear_Motor_On(-400, Motor1HallSensor1Pin,0, 100)==GearName){return true;}}}}
if (GearName==P_R) {if (SelectGear(P_N5R)==P_N5R) {if (Gear_Unlock_R_Block_5(true)) {if (Change_Gear_Motor_On(400, Motor1HallSensor2Pin,0, 100)==GearName){Gear_Unlock_R_Block_5(false);return true;}}}}
  
  
}//1
return false;
}  
       
//###################################################################
//###################################################################
// главный цикл
void loop()
{
wdt_reset();
KPP_working();  
}

// процедура работы КПП - самодостаточная
void KPP_working()
{
delay(100);  
uint16_t ShifterButtonsStatus=Read_ShifterButtonsStatus();
ShifterButtonsStatus >>= Shifter_RearGearMode; 
byte Shifter_RearGearModeStatus=ShifterButtonsStatus & 1;
ShifterButtonsStatus=Read_ShifterButtonsStatus();
ShifterButtonsStatus >>= Shifter_PlusMinusMode; 
byte Shifter_PlusMinusModeStatus=ShifterButtonsStatus & 1;  

byte ToGear=GetShifterPosition();
byte GearName=GetCurrentGearPosition();
byte GearNameToIndicate=GearName;

// обычный режим
//*******************
byte GearLastName=GetLastSuccessGearPosition();
if ((GearLastName!=ToGear) && (Shifter_PlusMinusModeStatus==0))
{
if ((ToGear==P_1)&& (Shifter_RearGearModeStatus==0))  {GearNameToIndicate=Go_To_Gear(ToGear);}
if ((ToGear==P_2)&& (Shifter_RearGearModeStatus==0))  {GearNameToIndicate=Go_To_Gear(ToGear);}
if ((ToGear==P_3)&& (Shifter_RearGearModeStatus==0))  {GearNameToIndicate=Go_To_Gear(ToGear);}
if ((ToGear==P_4)&& (Shifter_RearGearModeStatus==0))  {GearNameToIndicate=Go_To_Gear(ToGear);}
if ((ToGear==P_5)&& (Shifter_RearGearModeStatus==0))  {GearNameToIndicate=Go_To_Gear(ToGear);}
if ((ToGear==P_R) && (Shifter_RearGearModeStatus==1)) {GearNameToIndicate=Go_To_Gear(ToGear);}
if ((ToGear==P_N12)||(ToGear==P_N34) ||(ToGear==P_N5R)){GearNameToIndicate=Go_To_Neutral_Line();}
}
//*******************



// спортивный режим +-
//*******************
//byte GearLastName_withoutN=0;
//GearLastName
byte FrontGearLastName=GetLastFrontGearPosition();
if ((FrontGearLastName<P_1) || (FrontGearLastName>P_5)) {FrontGearLastName=P_1;}
if (Shifter_PlusMinusModeStatus==1)
   {
if (ToGear==P_PLUS)
{
if ((GearLastName==P_N5R)&& (FrontGearLastName==P_5)) {GearNameToIndicate=Go_To_Gear(FrontGearLastName);}  
if ((FrontGearLastName>=P_1) && (FrontGearLastName<P_5)) {GearNameToIndicate=Go_To_Gear(FrontGearLastName+1);}
}

if (ToGear==P_MINUS)
{
//if (GearLastName==P_1) {GearName=Go_To_Neutral_Line();}  
if ((GearLastName==P_N12)&& (FrontGearLastName==P_1)) {GearNameToIndicate=Go_To_Gear(FrontGearLastName);}
if ((FrontGearLastName>P_1) && (FrontGearLastName<=P_5))  {GearNameToIndicate=Go_To_Gear(FrontGearLastName-1);}
}

if (Shifter_RearGearModeStatus==1) {GearNameToIndicate=Go_To_Neutral_Line();} 
   }
//*******************

#if DEBUG  
//Serial.println(ToGear);    
//delay(1000);
#endif


Indicate_Gear_On_LCD(GearNameToIndicate);


}





//###################################################################
// обработчик событий тахометра
void  TahometrImpulse_on() //сидит на прерывании 0, срабатывает при возникновении событий на 2 цифровом входе
{ 
//TimeImpulse1=
Tahometr_impulse_count++; //увеличить счетчик импульсов
//Speed=0;
if (Tahometr_impulse_count >=2) //2имп=1 оборот коленвала
  {
Taho_ImpulseTime = micros()-Taho_time;  //время прихода между импульсами
Taho_time = micros(); //обнулить
Taho=((Tahometr_impulse_count*60*1000000)/(2*Taho_ImpulseTime));   // перевод в об/мин, формула универсальна для Тигго, 1 об на 2 ммпульса,1000000 - перевод мкс в сек., 60 - перевод сек в мин.

#if DEBUG
//Serial.println(Taho_ImpulseTime);
//Serial.println(Taho); 
#endif

Tahometr_impulse_count = 0;
  }  
} 

//###################################################################
// обработчик событий датчика скорости
void  SpeedImpulse_on() //сидит на прерывании 1, срабатывает при возникновении событий на 3 цифровом входе
{ 
//TimeImpulse1=
Speed_impulse_count++; //увеличить счетчик импульсов
//Speed=0;
if (Speed_impulse_count >=2) //2имп=0,8м    5имп=2м
  {
Speed_ImpulseTime = micros()-Speed_time;  //время прихода между импульсами
Speed_time = micros(); //обнулить
Speed=((Speed_impulse_count*2*3.6*1000000)/(5*Speed_ImpulseTime));   // перевод в км/ч, формула универсальна для Тигго, 2=2м за 5 ммпульсов, 3.6 - перевод м/с в км/ч, 1000000 - перевод мкс в сек., 5 - 5 импульсов

#if DEBUG
//Serial.println(Speed_ImpulseTime);
//Serial.println(Speed); 
#endif

Speed_impulse_count = 0;
  }  
} 

//###################################################################
// Функция проверки нажатия рулевой кнопки
// Возвращает false если кнопка не была нажата 
// и true если нажата
// ButtonName принимает значения в зависимости от нажатой кнопки +, -, >, R=>>|, L=|<<, S=SRC, X=None
boolean WheelButonPush_IsPushed()    
{    
bool result; 
result=false;
unsigned long buttonVal;
buttonVal=0; 
ButtonName='X';
 res_d = analogRead(WheelButtonsPin);  
  if( res_d < LIMIT) res_d = LIMIT; 
  delay(10);   
  res_dt1 = analogRead(WheelButtonsPin);   
        old_time = millis();    
        math_time = 0;     
        while(math_time < 200)    
          {    
             real_time = millis();                 
             math_time = real_time - old_time;   
            res_dt1 = analogRead(WheelButtonsPin);  
            if(res_dt1 < res_d-LIMIT || res_dt1 > res_d+LIMIT)break; 
          }    
       
    if( math_time > 70)  
      {   
  buttonVal=res_d;
  ButtonName='X';
   //   Serial.println (res_d);
              
        if ((buttonVal>25) && (buttonVal<45))
       {
ButtonName='-'; //35-36
result=true;
        }
    
        if ((buttonVal>60) && (buttonVal<85))
       {
ButtonName='+'; //72-73
result=true;
        }

        if ((buttonVal>110) && (buttonVal<135))
       {
ButtonName='S'; //122
result=true;
        }

        if ((buttonVal>175) && (buttonVal<200))
       {
ButtonName='R'; //187
result=true;
        }

        if ((buttonVal>275) && (buttonVal<295))
       {
ButtonName='L'; //285
result=true;
        }

        if ((buttonVal>415) && (buttonVal<440))
       {
ButtonName='>'; //429
result=true;
        }
        
        if ((buttonVal>645) && (buttonVal<670))
       {
ButtonName='X'; //658-659
result=false;
        }          

 buttonVal=0;    
    }  
       return result;
}  



//###################################################################
 int sensorValue = 0;  // variable to store the value coming from the sensor
 int countvalues = 5;      // how many values must be averaged
 int ZeroLevel = 520;    // Zero level
 float kVolt = 0.0506;      // conversion factor

int Current(int sensorPin) {
  int TMPsensorValue = 0;
  int count = 0;
 
  for (count =0; count < countvalues; count++) {
    // read the value from the sensor:
    TMPsensorValue = analogRead(sensorPin);
    delay(30);
    // make average value
    sensorValue = (sensorValue+TMPsensorValue)/2;
    }
  return sensorValue;
  }

//###################################################################
void Timer2_Timer() //обработка событий в таймере
{

//CurrentSensor1Value = analogRead(CurrentSensor1Pin);       // read analog input pin CurrentSensor1Pin

//int CurrentSensor1Value = Current(CurrentSensor1Pin);
//  Serial.println(kVolt * (CurrentSensor1Value - ZeroLevel));
/*  
#if DEBUG
    //  Serial.println(CurrentSensor1Value);  // prints the value read
 Serial.println((CurrentSensor1Value-512)*1000/20);  // prints the value read
  delay(100);                        // wait 100ms for next reading
#endif
*/   
}

С благодарностью приму любые замечания по коду.

Фотки:
http://big-e.ru/images/Gallery/preview_ex.php?dir=Robot
Связанные ссылки по теме:
http://big-e.ru/news.php?readmore=21 - родной механизм МКПП
http://big-e.ru/images/Gallery/Robot/P1020200.avi - переделанный механизм

Вопросы решенные в ходе работ:
Индикация передач на ЖКИ: http://www.compcar.ru/forum/showpost...35&postcount=7
Подключение датчиков холла, скорости и тахометра: http://www.compcar.ru/forum/showthread.php?t=8566
Подключение рулевых кнопок (мультируля): http://www.compcar.ru/forum/showpost...&postcount=421
Подключение шаговых двигателей: http://www.compcar.ru/forum/showpost...8&postcount=81

Надо отдать должное этому форуму и активным его членам - без их поддержки ничего бы не вышло.

[kadushkin]

Помогите решить этот вопрос:
http://www.compcar.ru/forum/showpost...6&postcount=39

[Murkur]

Вообще-то очень даже интересная тема - я бы себе такое сделал. Но пока не понял, а как же педаль сцепления? Или я что-то пропустил? В принципе, можно и на педаль сцепления поставить привод, но тут уже нужно будет и педаль тормоза задействовать. И ещё... А как быть на случай отказа робота? Полюбому нужно придумать, как в дорожных условиях вернуть управление на механику. Для этого продольный двигатель можно так и оставить, а поворот вала сделать через шестерню, одетую на вал. При этом механику можно будет сохранить.

[kadushkin]

Ну я же написал, что это 1ый этап, хотя возможно второго и не будет...
В случае отказа робота, с помощью отвертки (под капотом, сняв АКБ) можно будет врубить первую передачу, ну или вторую.

[Murkur]

Ну я же написал, что это 1ый этап, хотя возможно второго и не будет...
В случае отказа робота, с помощью отвертки (под капотом, сняв АКБ) можно будет врубить первую передачу, ну или вторую.

А какой должен был быть 2-й этап?

[kadushkin]

Этап 2:
Установка рабочего электроцилиндра сцепления, выброс педали , со всеми вытекающими последствиями.

[Murkur]

Этап 2:
Установка рабочего электроцилиндра сцепления, выброс педали , со всеми вытекающими последствиями.

Огоо, становится интересно... Хотя, я думаю попробовать себе сделать пока 1-й этап. Спасибо за наводку.

[sandos]

зачем на дуино .... сразу взвалить на carpc ... и сразу запись в испытатели надо открывать ... интересно много желающих будет ))))))))

[Murkur]

зачем на дуино .... сразу взвалить на carpc ... и сразу запись в испытатели надо открывать ... интересно много желающих будет ))))))))

А в чём проблема? Работают же нормально устройства на дуино и ничего. А потом всё это можно прописать в чип.

[poraboloid]

Очень интересная тема.
Насчет педали сцепления можно погуглить, были разработки чисто механические для Тазиков, на основе стандартного вакумника.
А есть более детальные фото блока выбора и движков ?