Crius All in one pro для внедорожника Krohpit ATV 8x8 - Multiwii в машине.
Мультикоптерный контроллер Crius AIO Pro для управление моей машиной внедорожник Krohpit 8x8 Gyro с бортовой трансмиссией. В частности БТР-80.
Контроллер управляет двумя силовыми моторами левого и правого борта, рулевыми сервами и сервами подвески.
В контроллере есть гироскопы и акксерерометры, с помощью которых можно решать задачу стабилизации кузова машины, удержание на траектории в поворотах, противобуксовочная система, антиблокировка (ABS). Вероятно можно будет решить задачу ровного приземления на колеса при прыжках на трамплинах за счет изменения момента на колеса.
Еще данный контроллер позволяет подключить модуль GPS и модуль телеметрии, для управления с компьютера.
Сервы подвески работают по принципу октокоптерной схемы X
Первая бета версия. За основу взял код 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:
Post a Comment