Library containing Crazyflie 2.0 controller classes: - Attitude estimator - Horizontal estimator - Vertical estimator - Attitude controller - Horizontal controller - Vertical controller - Mixer
Revision 24:7b9e3beb61d5, committed 2018-12-06
- Comitter:
- fbob
- Date:
- Thu Dec 06 16:44:40 2018 +0000
- Parent:
- 23:4905fbc2b31a
- Commit message:
- Adjust
Changed in this revision
--- a/Mixer/Mixer.cpp Wed Oct 31 13:21:09 2018 +0000 +++ b/Mixer/Mixer.cpp Thu Dec 06 16:44:40 2018 +0000 @@ -4,6 +4,56 @@ // Class constructor Mixer::Mixer() : motor_1(PA_1), motor_2(PB_11), motor_3(PA_15), motor_4(PB_9_ALT1), led_1_red(PC_3), led_1_green(PC_2), led_4_red(PC_0), led_4_green(PC_1) { + armed = false; + led_1_red = 1; + led_1_green = 0; + led_4_red = 1; + led_4_green = 0; + motor_1.period(period_pwm); + motor_2.period(period_pwm); + motor_3.period(period_pwm); + motor_4.period(period_pwm); + motor_1 = 0.0f; + motor_2 = 0.0f; + motor_3 = 0.0f; + motor_4 = 0.0f; +} + +void Mixer::arm() +{ + led_1_red = 0; + led_1_green = 1; + led_4_red = 0; + led_4_green = 1; + motor_1.period(1.0/440.0); + motor_1 = 0.2f; + wait(motor_test_on_time); + motor_1.period(period_pwm); + motor_1 = 0.0f; + wait(motor_test_off_time); + motor_2.period(1.0/880.0); + motor_2 = 0.2f; + wait(motor_test_on_time); + motor_2.period(period_pwm); + motor_2 = 0.0f; + wait(motor_test_off_time); + motor_3.period(1.0/660.0); + motor_3 = 0.2f; + wait(motor_test_on_time); + motor_3.period(period_pwm); + motor_3 = 0.0f; + wait(motor_test_off_time); + motor_4.period(1.0/550.0); + motor_4 = 0.2f; + wait(motor_test_on_time); + motor_4.period(period_pwm); + motor_4 = 0.0f; + wait(motor_test_off_time); + armed = true; +} + +void Mixer::disarm() +{ led_1_red = 1; led_1_green = 0; led_4_red = 1; @@ -12,29 +62,19 @@ motor_2 = 0.0f; motor_3 = 0.0f; motor_4 = 0.0f; + armed = false; } // Actuate motors with desired total trust force (N) and torques (N.m) void Mixer::actuate(float f_t, float tau_phi, float tau_theta, float tau_psi) { - force_and_torques_to_angular_velocities(f_t,tau_phi,tau_theta,tau_psi); - motor_1 = angular_velocity_to_pwm(omega_1); - motor_2 = angular_velocity_to_pwm(omega_2); - motor_3 = angular_velocity_to_pwm(omega_3); - motor_4 = angular_velocity_to_pwm(omega_4); - if ((omega_1 == 0) & (omega_2 == 0) & (omega_3 == 0) & (omega_4 == 0)) + if(armed) { - led_1_red = 1; - led_1_green = 0; - led_4_red = 1; - led_4_green = 0; - } - else - { - led_1_red = 0; - led_1_green = 1; - led_4_red = 0; - led_4_green = 1; + force_and_torques_to_angular_velocities(f_t,tau_phi,tau_theta,tau_psi); + motor_1 = angular_velocity_to_pwm(omega_1); + motor_2 = angular_velocity_to_pwm(omega_2); + motor_3 = angular_velocity_to_pwm(omega_3); + motor_4 = angular_velocity_to_pwm(omega_4); } }
--- a/Mixer/Mixer.h Wed Oct 31 13:21:09 2018 +0000 +++ b/Mixer/Mixer.h Thu Dec 06 16:44:40 2018 +0000 @@ -12,11 +12,17 @@ Mixer(); // Actuate motors with desired total trust force (N) and torques (N.m) void actuate(float f_t, float tau_phi, float tau_theta, float tau_psi); + + void arm(); + + void disarm(); private: // Motors PWM outputs PwmOut motor_1, motor_2, motor_3, motor_4; // LED digital outputs DigitalOut led_1_red, led_1_green, led_4_red, led_4_green; + // + bool armed; // Angular velocities (rad/s) float omega_1, omega_2, omega_3, omega_4; // Converts total trust force (N) and torques (N.m) to angular velocities (rad/s)
--- a/Parameters/Parameters.h Wed Oct 31 13:21:09 2018 +0000 +++ b/Parameters/Parameters.h Thu Dec 06 16:44:40 2018 +0000 @@ -1,6 +1,13 @@ #ifndef Parameters_h #define Parameters_h +// PWM frequencies +const float f_pwm = 500.0f; +const float period_pwm = 1.0f/f_pwm; + +const float motor_test_on_time = 0.05f; +const float motor_test_off_time = 0.15f; + // Interrupt frequencies const float f = 500.0f; // Hz const float f_range = 20.0f; // Hz