Étape 16 : Mettre ensemble : le Code
La partie la plus intéressante du code, qui met en œuvre les concepts depuis les quatre dernières étapes, s’adapte sur une seule page (ou les deux images dans cette étape). C’est ici, avec de brèves descriptions de ce que fait chaque partie :
readGyro() ;
readAcc() ;
// START OF + MODE ----------------------------------------------------------------------
a_pitch = (accel_x_zero - accel_x) ;
a_roll = (accel_y - accel_y_zero) ;
a_z = (int signé) analogRead(A_Z) - a_z_z ;
g_pitch = (gyro_y - gyro_y_zero) ;
g_roll = (gyro_x - gyro_x_zero) ;
g_yaw = (gyro_z - gyro_z_zero) ;
Cette partie s’empare des signaux bruts d’accéléromètre et gyroscope. Il fait aussi la réduction à zéro, même si j’ai tous mes zéros... la valeur zéro... puisque le minIMU Pololu-9 fait un bon travail de remise à zéro de ses sorties déjà. Si nécessaire, les valeurs zéro pourraient être modifiées pour couper la quadrotor. Remarque : a_pitch est contrepassée. Il s’agit d’un problème physique. Sous des pas positifs, le vecteur de la gravité soit du côté de l’axe X de l’IMU. Cela enregistre comme accélération négative en X, étant donné que l’accelrometer ne peut pas distinguer entre se pencher vers l’avant et l’accélération vers l’arrière. a_roll n’a pas ce revirement.
rate_pitch = (float) g_pitch * G_GAIN ;
rate_roll = (float) g_roll * G_GAIN ;
rate_yaw = (float) g_yaw * G_GAIN ;
Convertissent les signaux de gyro brutes en unités physiques de º / s.
angle_pitch = AA * (angle_pitch + rate_pitch * DT) ;
angle_pitch += (1,0 - AA) * (float) a_pitch * A_GAIN ;
pitch_error = (float) (pitch_command - 127) / 127,0 * MAX_ANGLE - angle_pitch ;
pitch_error_integral += pitch_error * DT ;
Si (pitch_error_integral > = INT_SAT) {pitch_error_integral = INT_SAT;}
Si (pitch_error_integral < = - INT_SAT) {pitch_error_integral = - INT_SAT;}
angle_roll = AA * (angle_roll + rate_roll * DT) ;
angle_roll += (1,0 - AA) * (float) a_roll * A_GAIN ;
roll_error = (float) (roll_command - 127) / 127,0 * MAX_ANGLE - angle_roll ;
roll_error_integral += roll_error * DT ;
Si (roll_error_integral > = INT_SAT) {roll_error_integral = INT_SAT;}
Si (roll_error_inttegral < = - INT_SAT) {roll_error_integral = - INT_SAT;}
Pitch et rouler les filtres complémentaires, ainsi que calcul de l’erreur d’angle. pitch_command et roll_command viennent de la radio et sont transmises par la station au sol issue des entrées d’utilisateur. MAX_ANGLE est une constante qui définit l’angle de tangage et de roulis commandée maximale exprimée en degrés défini par l’utilisateur. Les modalités de commande incorporé sont toutes commentées, mais n’hésitez pas à jouer avec eux.
Contrôle
front_command_f = pitch_error * KP ;
front_command_f = pitch_error_integral * KI ;
front_command_f += rate_pitch * KD ;
front_command_f-= ((float) (yaw_command - 127) / 127,0 * MAX_RATE - rate_yaw) * KY ;
rear_command_f += pitch_error * KP ;
rear_command_f += pitch_error_integral * KI ;
rear_command_f = rate_pitch * KD ;
rear_command_f-= ((float) (yaw_command - 127) / 127,0 * MAX_RATE - rate_yaw) * KY ;
right_command_f = roll_error * KP ;
right_command_f = roll_error_integral * KI ;
right_command_f += 1.3 * rate_roll * KD ;
right_command_f += ((float) (yaw_command - 127) / 127,0 * MAX_RATE - rate_yaw) * KY ;
left_command_f += roll_error * KP ;
left_command_f += roll_error_integral * KI ;
left_command_f = rate_roll * KD ;
left_command_f += ((float) (yaw_command - 127) / 127,0 * MAX_RATE - rate_yaw) * KY ;
// END OF + MODE ------------------------------------------------------------------------
Il s’agit de la matrice de commande de l’étape 12. Le contrôleur de mouvement de lacet est en doublé, ce qui est peu ridicule. yaw_command vient de la radio et est transmise par la station au sol. MAX_RATE est une constante définie par l’utilisateur qui définit la valeur maximale commanda la vitesse de rotation de lacet en degrés par seconde.
