Fabio Bobrow / CrazyflieController

Library containing Crazyflie 2.0 controller classes: - Attitude estimator - Horizontal estimator - Vertical estimator - Attitude controller - Horizontal controller - Vertical controller - Mixer

Mixer/Mixer.cpp@24:7b9e3beb61d5, 2018-12-06 (annotated)

Committer:
fbob
Date:
Thu Dec 06 16:44:40 2018 +0000
Revision:
24:7b9e3beb61d5
Parent:
23:4905fbc2b31a 
Adjust

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fbob 0:b1f2c9e88e32 1 #include "mbed.h"
fbob 0:b1f2c9e88e32 2 #include "Mixer.h"
fbob 0:b1f2c9e88e32 3
fbob 23:4905fbc2b31a 4 // Class constructor
fbob 12:9fed6f656f88 5 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)
fbob 0:b1f2c9e88e32 6 {
fbob 24:7b9e3beb61d5 7 armed = false;
fbob 24:7b9e3beb61d5 8 led_1_red = 1;
fbob 24:7b9e3beb61d5 9 led_1_green = 0;
fbob 24:7b9e3beb61d5 10 led_4_red = 1;
fbob 24:7b9e3beb61d5 11 led_4_green = 0;
fbob 24:7b9e3beb61d5 12 motor_1.period(period_pwm);
fbob 24:7b9e3beb61d5 13 motor_2.period(period_pwm);
fbob 24:7b9e3beb61d5 14 motor_3.period(period_pwm);
fbob 24:7b9e3beb61d5 15 motor_4.period(period_pwm);
fbob 24:7b9e3beb61d5 16 motor_1 = 0.0f;
fbob 24:7b9e3beb61d5 17 motor_2 = 0.0f;
fbob 24:7b9e3beb61d5 18 motor_3 = 0.0f;
fbob 24:7b9e3beb61d5 19 motor_4 = 0.0f;
fbob 24:7b9e3beb61d5 20 }
fbob 24:7b9e3beb61d5 21
fbob 24:7b9e3beb61d5 22 void Mixer::arm()
fbob 24:7b9e3beb61d5 23 {
fbob 24:7b9e3beb61d5 24 led_1_red = 0;
fbob 24:7b9e3beb61d5 25 led_1_green = 1;
fbob 24:7b9e3beb61d5 26 led_4_red = 0;
fbob 24:7b9e3beb61d5 27 led_4_green = 1;
fbob 24:7b9e3beb61d5 28 motor_1.period(1.0/440.0);
fbob 24:7b9e3beb61d5 29 motor_1 = 0.2f;
fbob 24:7b9e3beb61d5 30 wait(motor_test_on_time);
fbob 24:7b9e3beb61d5 31 motor_1.period(period_pwm);
fbob 24:7b9e3beb61d5 32 motor_1 = 0.0f;
fbob 24:7b9e3beb61d5 33 wait(motor_test_off_time);
fbob 24:7b9e3beb61d5 34 motor_2.period(1.0/880.0);
fbob 24:7b9e3beb61d5 35 motor_2 = 0.2f;
fbob 24:7b9e3beb61d5 36 wait(motor_test_on_time);
fbob 24:7b9e3beb61d5 37 motor_2.period(period_pwm);
fbob 24:7b9e3beb61d5 38 motor_2 = 0.0f;
fbob 24:7b9e3beb61d5 39 wait(motor_test_off_time);
fbob 24:7b9e3beb61d5 40 motor_3.period(1.0/660.0);
fbob 24:7b9e3beb61d5 41 motor_3 = 0.2f;
fbob 24:7b9e3beb61d5 42 wait(motor_test_on_time);
fbob 24:7b9e3beb61d5 43 motor_3.period(period_pwm);
fbob 24:7b9e3beb61d5 44 motor_3 = 0.0f;
fbob 24:7b9e3beb61d5 45 wait(motor_test_off_time);
fbob 24:7b9e3beb61d5 46 motor_4.period(1.0/550.0);
fbob 24:7b9e3beb61d5 47 motor_4 = 0.2f;
fbob 24:7b9e3beb61d5 48 wait(motor_test_on_time);
fbob 24:7b9e3beb61d5 49 motor_4.period(period_pwm);
fbob 24:7b9e3beb61d5 50 motor_4 = 0.0f;
fbob 24:7b9e3beb61d5 51 wait(motor_test_off_time);
fbob 24:7b9e3beb61d5 52 armed = true;
fbob 24:7b9e3beb61d5 53 }
fbob 24:7b9e3beb61d5 54
fbob 24:7b9e3beb61d5 55 void Mixer::disarm()
fbob 24:7b9e3beb61d5 56 {
fbob 6:3188b00263e8 57 led_1_red = 1;
fbob 17:f682b4a5686d 58 led_1_green = 0;
fbob 6:3188b00263e8 59 led_4_red = 1;
fbob 17:f682b4a5686d 60 led_4_green = 0;
fbob 17:f682b4a5686d 61 motor_1 = 0.0f;
fbob 17:f682b4a5686d 62 motor_2 = 0.0f;
fbob 17:f682b4a5686d 63 motor_3 = 0.0f;
fbob 17:f682b4a5686d 64 motor_4 = 0.0f;
fbob 24:7b9e3beb61d5 65 armed = false;
fbob 23:4905fbc2b31a 66 }
fbob 0:b1f2c9e88e32 67
fbob 5:b9947e3d20cf 68 // Actuate motors with desired total trust force (N) and torques (N.m)
fbob 0:b1f2c9e88e32 69 void Mixer::actuate(float f_t, float tau_phi, float tau_theta, float tau_psi)
fbob 0:b1f2c9e88e32 70 {
fbob 24:7b9e3beb61d5 71 if(armed)
fbob 6:3188b00263e8 72 {
fbob 24:7b9e3beb61d5 73 force_and_torques_to_angular_velocities(f_t,tau_phi,tau_theta,tau_psi);
fbob 24:7b9e3beb61d5 74 motor_1 = angular_velocity_to_pwm(omega_1);
fbob 24:7b9e3beb61d5 75 motor_2 = angular_velocity_to_pwm(omega_2);
fbob 24:7b9e3beb61d5 76 motor_3 = angular_velocity_to_pwm(omega_3);
fbob 24:7b9e3beb61d5 77 motor_4 = angular_velocity_to_pwm(omega_4);
fbob 6:3188b00263e8 78 }
fbob 0:b1f2c9e88e32 79 }
fbob 0:b1f2c9e88e32 80
fbob 5:b9947e3d20cf 81 // Converts total trust force (N) and torques (N.m) to angular velocities (rad/s)
fbob 0:b1f2c9e88e32 82 void Mixer::force_and_torques_to_angular_velocities(float f_t, float tau_phi, float tau_theta, float tau_psi)
fbob 0:b1f2c9e88e32 83 {
fbob 0:b1f2c9e88e32 84 float omega_1_squared = (1.0f/4.0f)*(f_t/kl-tau_phi/(kl*l)-tau_theta/(kl*l)-tau_psi/kd);
fbob 0:b1f2c9e88e32 85 float omega_2_squared = (1.0f/4.0f)*(f_t/kl-tau_phi/(kl*l)+tau_theta/(kl*l)+tau_psi/kd);
fbob 0:b1f2c9e88e32 86 float omega_3_squared = (1.0f/4.0f)*(f_t/kl+tau_phi/(kl*l)+tau_theta/(kl*l)-tau_psi/kd);
fbob 0:b1f2c9e88e32 87 float omega_4_squared = (1.0f/4.0f)*(f_t/kl+tau_phi/(kl*l)-tau_theta/(kl*l)+tau_psi/kd);
fbob 0:b1f2c9e88e32 88 if(omega_1_squared>=0) {
fbob 0:b1f2c9e88e32 89 omega_1 = sqrt(omega_1_squared);
fbob 0:b1f2c9e88e32 90 } else {
fbob 0:b1f2c9e88e32 91 omega_1 = 0.0f;
fbob 0:b1f2c9e88e32 92 }
fbob 0:b1f2c9e88e32 93 if(omega_2_squared>=0) {
fbob 0:b1f2c9e88e32 94 omega_2 = sqrt(omega_2_squared);
fbob 0:b1f2c9e88e32 95 } else {
fbob 0:b1f2c9e88e32 96 omega_2 = 0.0f;
fbob 0:b1f2c9e88e32 97 }
fbob 0:b1f2c9e88e32 98 if(omega_3_squared>=0) {
fbob 0:b1f2c9e88e32 99 omega_3 = sqrt(omega_3_squared);
fbob 0:b1f2c9e88e32 100 } else {
fbob 0:b1f2c9e88e32 101 omega_3 = 0.0f;
fbob 0:b1f2c9e88e32 102 }
fbob 0:b1f2c9e88e32 103 if(omega_4_squared>=0) {
fbob 0:b1f2c9e88e32 104 omega_4 = sqrt(omega_4_squared);
fbob 0:b1f2c9e88e32 105 } else {
fbob 0:b1f2c9e88e32 106 omega_4 = 0.0f;
fbob 0:b1f2c9e88e32 107 }
fbob 0:b1f2c9e88e32 108 }
fbob 0:b1f2c9e88e32 109
fbob 5:b9947e3d20cf 110 // Converts desired angular velocity (rad/s) to PWM signal (%)
fbob 0:b1f2c9e88e32 111 float Mixer::angular_velocity_to_pwm(float omega)
fbob 0:b1f2c9e88e32 112 {
fbob 0:b1f2c9e88e32 113 float pwm = alpha*pow(omega,2)+beta*omega;
fbob 0:b1f2c9e88e32 114 return pwm;
fbob 0:b1f2c9e88e32 115 }