PCB Quadrotor (borstlösa) (16 / 20 steg)
Steg 16: Att sätta det tillsammans: The Code
Den mest intressanta delen av koden, som implementerar begrepp från de senaste fyra stegen, passar på en sida (eller de två bilderna i det här steget). Här är det, med korta beskrivningar av vad varje del gör:
readGyro();
readAcc();
// START OF + MODE ----------------------------------------------------------------------
a_pitch = (accel_x_zero - accel_x);
a_roll = (accel_y - accel_y_zero);
a_z = (signerad 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);
Denna del griper raw accelerometer och gyro signalerna. Det gör också nollställning, även om jag har alla mina nollor inställd... noll... eftersom Pololu minIMU-9 gör ett ganska bra jobb med nollställning dess resultat redan. Om det behövs kan nollvärden ändras för att trimma quadrotor. Obs: a_pitch är omvänd. Detta är en fysisk fråga. Under positiva pitch, gravitation vektorn pekar mot X-axeln i IMU. Registreras som negativa acceleration i X, eftersom accelrometer inte kan skilja mellan lutar framåt och påskynda bakåt. a_roll har inte denna omsvängning.
rate_pitch = (float) g_pitch * G_GAIN;
rate_roll = (float) g_roll * G_GAIN;
rate_yaw = (float) g_yaw * G_GAIN;
Konvertera raw gyro signalerna till fysiska enheter på º / 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;
om (pitch_error_integral > = INT_SAT) {pitch_error_integral = INT_SAT;}
om (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;
om (roll_error_integral > = INT_SAT) {roll_error_integral = INT_SAT;}
om (roll_error_inttegral < = - INT_SAT) {roll_error_integral = - INT_SAT;}
Pitch och rulla kompletterande filter, samt vinkla felberäkning. pitch_command och roll_command kom från radion och överförs från marken stationen grundas på insatsvaror som användaren. MAX_ANGLE är en användaren-definiera konstant som anger maximal befallde pitch och rulle vinkeln i grader. Integrerad kontroll benämner alla kommenteras ut, men känn dig fri att spela med dem.
Kontroll
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 ------------------------------------------------------------------------
Detta är kommandot matris från steg 12. Yaw styrenheten är i-fodrade, vilket är typ av dumt. yaw_command kommer från radion, och överförs av marken stationen. MAX_RATE är en användardefinierad konstant som anger maximalt befallde girningshastighet rotationen i grader per sekund.