Stap 16: Puttees op tezamen: de Code
Het meest interessante deel van de code, die uitvoering geeft aan de concepten van de afgelopen vier stappen, past op één pagina (of de twee beelden in deze stap). Hier is het, met korte beschrijvingen van wat elk onderdeel doet:
readGyro();
readAcc();
// START OF + MODE ----------------------------------------------------------------------
a_pitch = (accel_x_zero - accel_x);
a_roll = (accel_y - accel_y_zero);
a_z = (ondertekende int) 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);
Dit deel pakt de ruwe versnellingsmeter en gyro signalen. Het doet ook zeroing, hoewel ik heb alle mijn nullen... ingesteld op nul... aangezien de Pololu minIMU-9 heeft een goede baan van de resultaten al zeroing. Indien nodig, kunnen de nulwaarden worden gewijzigd als u wilt bijsnijden van de quadrotor. Opmerking: a_pitch wordt omgekeerd. Dit is een fysieke kwestie. Onder positieve pitch, de zwaartekracht vector wijst naar de X-as van de IMU. Dit registreert als negatieve versnelling in X, aangezien de accelrometer geen onderscheid tussen leunend vooruit en achteruit versnelling. a_roll hoeft niet deze omkering.
rate_pitch = (float) g_pitch * G_GAIN;
rate_roll = (float) g_roll * G_GAIN;
rate_yaw = (float) g_yaw * G_GAIN;
De ruwe gyro-signalen converteren naar fysieke eenheden van º / 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;
Als (pitch_error_integral > = INT_SAT) {pitch_error_integral = INT_SAT;}
Als (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;
Als (roll_error_integral > = INT_SAT) {roll_error_integral = INT_SAT;}
Als (roll_error_inttegral < = - INT_SAT) {roll_error_integral = - INT_SAT;}
Pitch en roll aanvullende Filters, evenals Foutberekening hoek. pitch_command en roll_command komen uit de radio, en worden verzonden vanaf het grondstation op basis van gebruikersinvoer. MAX_ANGLE is een user-define-constante die de maximale bevolen pitch en roll hoek in graden ingesteld. De integrale controle zijn alle commentaar uit, maar voel je vrij om te spelen met hen.
Controle
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 ------------------------------------------------------------------------
Dit is de matrijs van de opdracht van stap 12. De yaw controller is in omzoomde, dat is soort van dom. yaw_command komt van de radio, en wordt overgedragen door het grondstation. MAX_RATE is een door de gebruiker gedefinieerde constante waarin de maximale geboden yaw tarief van de rotatie in graden per seconde.
