Crius All in one pro для внедорожника Krohpit 8x8

Crius All in one pro для внедорожника Krohpit ATV 8x8 - Multiwii в машине.

Мультикоптерный контроллер Crius AIO Pro для управление моей машиной внедорожник Krohpit 8x8 Gyro с бортовой трансмиссией. В частности БТР-80.



Контроллер управляет двумя силовыми моторами левого и правого борта, рулевыми сервами и сервами подвески.
В контроллере есть гироскопы и акксерерометры, с помощью которых можно решать задачу стабилизации кузова машины, удержание на траектории в поворотах, противобуксовочная система, антиблокировка (ABS). Вероятно можно будет решить задачу ровного приземления на колеса при прыжках на трамплинах за счет изменения момента на колеса.
Еще данный контроллер позволяет подключить модуль GPS и модуль телеметрии, для управления с компьютера.
Сервы подвески работают по принципу октокоптерной схемы X

09.11.2013
Первая бета версия. За основу взял код Arduino Multiwii 2.2. Увеличил количество количество каналов серв до 15. Пока использую софтовый метод генерации ШИМ сигнала для сервоприводов. Частота ШИМ 50 Гц.

Сделал четыре функции в рулевом управлении:
1. Бортовое управление, при нулевой скорости разворот на месте, все колеса повернуты плугом.
2. Бортовое управление, при нулевой скорости разворот на месте, все колеса по центру.
3. Поворот передних колес, дифференциал мотора.
4. Поворот всех колес, задние зависят от скорости, дифференциал мотора.

И четыре функции работы электроподвески:
1. Все сервоприводы подвески отключены.
2. Статический клиренс и горизонт шасси.
3. Прогрессивный наклон шасси в повороте.
4. Гиростабилизация шасси.

Привожу основную часть кода отвечающего за управление каналами в Output.ino
#if defined(Krohpit_ATV)
       
      static int16_t   servoMid[15];                        // Midpoint on servo
      static uint8_t   servoTravel[15] = SERVO_RATES;       // Rates in 0-100%
      static int8_t    servoReverse[15] = SERVO_DIRECTION ; // Inverted servos
      static int16_t   servoLimit[15][2]; // Holds servoLimit data
      static int16_t   servoOffset[15] = SERVO_OFFSET;
      static int16_t   Clearance = 0;
      
      #define SERVO_MIN 1000           // limit servo travel range must be inside [1000;2000]
      #define SERVO_MAX 2000           // limit servo travel range must be inside [1000;2000]
      for(i=0; i<15; i++){  //  Set rates with 0 - 100%.
      servoMid[i]     =MIDRC + servoOffset[i];
      servoLimit[i][0]=servoMid[i]-((servoMid[i]-SERVO_MIN)   *(servoTravel[i]*0.01));
      servoLimit[i][1]=servoMid[i]+((SERVO_MAX - servoMid[i]) *(servoTravel[i]*0.01));
      }
  
     
      if (f.RRSTR_MODE){   //RRSTR Поворот всех колес, задние зависят от скорости, дифференциал мотора 
      if (rcCommand[AUX1] >50) {
      servo[0]  = servoMid[0] + rcCommand[AUX1] - 0.1*rcCommand[YAW]; //Motor L
      servo[1]  = servoMid[1] + rcCommand[AUX1] + 0.1*rcCommand[YAW]; //Motor R
        } else {
          if (rcCommand[AUX1] <-50) {
      servo[0]  = servoMid[0] + rcCommand[AUX1] + 0.1*rcCommand[YAW]; //Motor L
      servo[1]  = servoMid[1] + rcCommand[AUX1] - 0.1*rcCommand[YAW]; //Motor R
        }
        else {
      servo[0]  = servoMid[0] + rcCommand[AUX1]; //Motor L
      servo[1]  = servoMid[1] + rcCommand[AUX1]; //Motor R
        }
        }
      servo[2]  = servoMid[2] + rcCommand[YAW]*servoReverse[2]; //Steering FL
      servo[3]  = servoMid[3] + rcCommand[YAW]*servoReverse[3]; //Steering FR
      servo[4]  = servoMid[4] + int(float(rcCommand[YAW])*(1-abs(float(rcCommand[AUX1]))/500))*servoReverse[4]; //Steering RL
      servo[5]  = servoMid[5] + int(float(rcCommand[YAW])*(1-abs(float(rcCommand[AUX1]))/500))*servoReverse[5]; //Steering RR
           }else{
        if(f.YAWSTR_MODE){   //YAWSTR Бортовое управление, при нулевой скорости разворот на месте 
      if (rcCommand[AUX1] >5) {
      servo[0]  = servoMid[0] + rcCommand[AUX1] - rcCommand[YAW]; //Motor L
      servo[1]  = servoMid[1] + rcCommand[AUX1] + rcCommand[YAW]; //Motor R
        } else {
          if (rcCommand[AUX1] <-5) {
      servo[0]  = servoMid[0] + rcCommand[AUX1] + rcCommand[YAW]; //Motor L
      servo[1]  = servoMid[1] + rcCommand[AUX1] - rcCommand[YAW]; //Motor R
        }
        else {
      servo[0]  = servoMid[0] - rcCommand[YAW]; //Motor L
      servo[1]  = servoMid[1]+ rcCommand[YAW]; //Motor R
        }
        }
      if (rcData[AUX3]>1850)   {
      servo[2]  = 1950; //Steering FL
      servo[3]  = 1050; //Steering FR
      servo[4]  = 1950; //Steering RL
      servo[5]  = 1050; //Steering RR
      }
      else  {
      servo[2]  = servoMid[2]; //Steering FL
      servo[3]  = servoMid[3]; //Steering FR
      servo[4]  = servoMid[4]; //Steering RL
      servo[5]  = servoMid[5]; //Steering RR
       }
      else{  //Поворот передних колес, дифференциал мотора
      if (rcCommand[AUX1] >50) {
      servo[0]  = servoMid[0] + rcCommand[AUX1] - 0.1*rcCommand[YAW]; //Motor L
      servo[1]  = servoMid[1] + rcCommand[AUX1] + 0.1*rcCommand[YAW]; //Motor R
        } 
      else {
          if (rcCommand[AUX1] <-50) {
      servo[0]  = servoMid[0] + rcCommand[AUX1] + 0.1*rcCommand[YAW]; //Motor L
      servo[1]  = servoMid[1] + rcCommand[AUX1] - 0.1*rcCommand[YAW]; //Motor R
        }
        else {
      servo[0]  = servoMid[0] + rcCommand[AUX1]; //Motor L
      servo[1]  = servoMid[1] + rcCommand[AUX1]; //Motor R
        }
        }
      servo[2]  = servoMid[2] + rcCommand[YAW]*servoReverse[2]; //Steering FL
      servo[3]  = servoMid[3] + rcCommand[YAW]*servoReverse[3]; //Steering FR
      servo[4]  = servoMid[4]; //Steering RL
      servo[5]  = servoMid[5]; //Steering RR
       }
      }
      if  (rcData[THROTTLE]<1060)     {
  pinMode(3,INPUT);     //6 Suspension     
  pinMode(5,INPUT);     //7 Suspension     
  pinMode(6,INPUT);     //8 Suspension                 
  pinMode(2,INPUT);     //9 Suspension     
  pinMode(7,INPUT);     //10 Suspension
  pinMode(8,INPUT);     //11 Suspension
  pinMode(9,INPUT);     //12 Suspension
  pinMode(10,INPUT);    //13 Suspension
  }
else  {  
  pinMode(3,OUTPUT);     //6 Suspension     
  pinMode(5,OUTPUT);     //7 Suspension     
  pinMode(6,OUTPUT);     //8 Suspension                 
  pinMode(2,OUTPUT);     //9 Suspension     
  pinMode(7,OUTPUT);     //10 Suspension
  pinMode(8,OUTPUT);     //11 Suspension
  pinMode(9,OUTPUT);     //12 Suspension
  pinMode(10,OUTPUT);    //13 Suspension
        
  Clearance = min(abs(rcData[THROTTLE]-MIDRC),500);
  if (rcData[THROTTLE]<MIDRC) Clearance = -Clearance;
      
      #define PIDMIX(X,Y,Z,U,V,W) Clearance + axisPID[ROLL]*X + axisPID[PITCH]*Y + axisPID[YAW]*Z + rcCommand[ROLL]*U + rcCommand[PITCH]*V + rcCommand[YAW]*W
      if(f.PASSTHRU_MODE){   //PASSTHRU Статический клиренс и горизонт шасси
      servo[6] = servoMid[6] + PIDMIX(0,0,0,+1  ,-1/2,0); //MIDFRONT_L
      servo[7] = servoMid[7] + PIDMIX(0,0,0,-1,-1  ,0); //FRONT_R
      servo[8] = servoMid[8] + PIDMIX(0,0,0,-1  ,+1/2,0); //MIDREAR_R
      servo[9] = servoMid[9] + PIDMIX(0,0,0,+1,+1  ,0); //REAR_L
      servo[10] =servoMid[10] -  PIDMIX(0,0,0,-1,+1  ,0); //FRONT_L
      servo[11] =servoMid[11] -  PIDMIX(0,0,0,+1  ,+1/2,0); //MIDFRONT_R
      servo[12] =servoMid[12] -  PIDMIX(0,0,0,+1,-1  ,0); //REAR_R
      servo[13] =servoMid[13] -  PIDMIX(0,0,0,-1  ,-1/2,0); //MIDREAR_L
      }else{
        if(f.PROGRES_MODE){   //PROGRES Прогрессивный наклон шасси в повороте 
      servo[6] = servoMid[6] + PIDMIX(0,0,0,+1  ,-1/2,1); //MIDFRONT_L
      servo[7] = servoMid[7] + PIDMIX(0,0,0,-1,-1  ,-1); //FRONT_R
      servo[8] = servoMid[8] + PIDMIX(0,0,0,-1  ,+1/2,-1); //MIDREAR_R
      servo[9] = servoMid[9] + PIDMIX(0,0,0,+1,+1  ,1); //REAR_L
      servo[10] =servoMid[10] -  PIDMIX(0,0,0,-1,+1  ,-1); //FRONT_L
      servo[11] =servoMid[11] -  PIDMIX(0,0,0,+1  ,+1/2,1); //MIDFRONT_R
      servo[12] =servoMid[12] -  PIDMIX(0,0,0,+1,-1  ,1); //REAR_R
      servo[13] =servoMid[13] -  PIDMIX(0,0,0,-1  ,-1/2,-1); //MIDREAR_L
       }else{        //Гиростабилизация шасси
      servo[6] = servoMid[6] + PIDMIX(+1  ,-1/2,0,0,0,0); //MIDFRONT_L
      servo[7] = servoMid[7] + PIDMIX(-1,-1  ,0,0,0,0); //FRONT_R
      servo[8] = servoMid[8] + PIDMIX(-1  ,+1/2,0,0,0,0); //MIDREAR_R
      servo[9] = servoMid[9] + PIDMIX(+1,+1  ,0,0,0,0); //REAR_L
      servo[10] =servoMid[10] -  PIDMIX(-1,+1  ,0,0,0,0); //FRONT_L
      servo[11] =servoMid[11] -  PIDMIX(+1  ,+1/2,0,0,0,0); //MIDFRONT_R
      servo[12] =servoMid[12] -  PIDMIX(+1,-1  ,0,0,0,0); //REAR_R
      servo[13] =servoMid[13] -  PIDMIX(-1  ,-1/2,0,0,0,0); //MIDREAR_L
       }
      }
}
      servo[14]=servoMid[14];
     
       for(i=0;i<15;i++){
        servo[i]  = map(servo[i], SERVO_MIN, SERVO_MAX,servoLimit[i][0],servoLimit[i][1]);
        servo[i]  = constrain( servo[i], SERVO_MIN, SERVO_MAX);
      }
     
      /*
      rcCommand[THROTTLE] - клиренс 1000:2000
      Clearance - клиренс -500:500
      rcCommand[ROLL] - наклоны влево/вправо -500:500
      rcCommand[PITCH] - наклоны вперед/назад -500:500
      rcCommand[YAW] - руль -500:500
      rcCommand[AUX1] - газ/тормоз -500:500
      rcData[AUX1] - газ/тормоз 1000:2000
      rcData[AUX2] - режим подвески 1000:2000
      rcData[AUX3] - режим руля 1000:2000
      rcData[AUX4] - переключение передач 1000:2000
       */ 
 #endif //Krohpit_ATV

No comments: