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@23:4905fbc2b31a, 2018-10-31 (annotated)

Committer:
fbob
Date:
Wed Oct 31 13:21:09 2018 +0000
Revision:
23:4905fbc2b31a
Parent:
22:5f2323e30cdc
Child:
24:7b9e3beb61d5
v2

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 6:3188b00263e8 7 led_1_red = 1;
fbob 17:f682b4a5686d 8 led_1_green = 0;
fbob 6:3188b00263e8 9 led_4_red = 1;
fbob 17:f682b4a5686d 10 led_4_green = 0;
fbob 17:f682b4a5686d 11 motor_1 = 0.0f;
fbob 17:f682b4a5686d 12 motor_2 = 0.0f;
fbob 17:f682b4a5686d 13 motor_3 = 0.0f;
fbob 17:f682b4a5686d 14 motor_4 = 0.0f;
fbob 23:4905fbc2b31a 15 }
fbob 0:b1f2c9e88e32 16
fbob 5:b9947e3d20cf 17 // Actuate motors with desired total trust force (N) and torques (N.m)
fbob 0:b1f2c9e88e32 18 void Mixer::actuate(float f_t, float tau_phi, float tau_theta, float tau_psi)
fbob 0:b1f2c9e88e32 19 {
fbob 0:b1f2c9e88e32 20 force_and_torques_to_angular_velocities(f_t,tau_phi,tau_theta,tau_psi);
fbob 0:b1f2c9e88e32 21 motor_1 = angular_velocity_to_pwm(omega_1);
fbob 0:b1f2c9e88e32 22 motor_2 = angular_velocity_to_pwm(omega_2);
fbob 0:b1f2c9e88e32 23 motor_3 = angular_velocity_to_pwm(omega_3);
fbob 0:b1f2c9e88e32 24 motor_4 = angular_velocity_to_pwm(omega_4);
fbob 6:3188b00263e8 25 if ((omega_1 == 0) & (omega_2 == 0) & (omega_3 == 0) & (omega_4 == 0))
fbob 6:3188b00263e8 26 {
fbob 6:3188b00263e8 27 led_1_red = 1;
fbob 6:3188b00263e8 28 led_1_green = 0;
fbob 6:3188b00263e8 29 led_4_red = 1;
fbob 6:3188b00263e8 30 led_4_green = 0;
fbob 6:3188b00263e8 31 }
fbob 6:3188b00263e8 32 else
fbob 6:3188b00263e8 33 {
fbob 17:f682b4a5686d 34 led_1_red = 0;
fbob 6:3188b00263e8 35 led_1_green = 1;
fbob 17:f682b4a5686d 36 led_4_red = 0;
fbob 6:3188b00263e8 37 led_4_green = 1;
fbob 6:3188b00263e8 38 }
fbob 0:b1f2c9e88e32 39 }
fbob 0:b1f2c9e88e32 40
fbob 5:b9947e3d20cf 41 // Converts total trust force (N) and torques (N.m) to angular velocities (rad/s)
fbob 0:b1f2c9e88e32 42 void Mixer::force_and_torques_to_angular_velocities(float f_t, float tau_phi, float tau_theta, float tau_psi)
fbob 0:b1f2c9e88e32 43 {
fbob 0:b1f2c9e88e32 44 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 45 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 46 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 47 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 48 if(omega_1_squared>=0) {
fbob 0:b1f2c9e88e32 49 omega_1 = sqrt(omega_1_squared);
fbob 0:b1f2c9e88e32 50 } else {
fbob 0:b1f2c9e88e32 51 omega_1 = 0.0f;
fbob 0:b1f2c9e88e32 52 }
fbob 0:b1f2c9e88e32 53 if(omega_2_squared>=0) {
fbob 0:b1f2c9e88e32 54 omega_2 = sqrt(omega_2_squared);
fbob 0:b1f2c9e88e32 55 } else {
fbob 0:b1f2c9e88e32 56 omega_2 = 0.0f;
fbob 0:b1f2c9e88e32 57 }
fbob 0:b1f2c9e88e32 58 if(omega_3_squared>=0) {
fbob 0:b1f2c9e88e32 59 omega_3 = sqrt(omega_3_squared);
fbob 0:b1f2c9e88e32 60 } else {
fbob 0:b1f2c9e88e32 61 omega_3 = 0.0f;
fbob 0:b1f2c9e88e32 62 }
fbob 0:b1f2c9e88e32 63 if(omega_4_squared>=0) {
fbob 0:b1f2c9e88e32 64 omega_4 = sqrt(omega_4_squared);
fbob 0:b1f2c9e88e32 65 } else {
fbob 0:b1f2c9e88e32 66 omega_4 = 0.0f;
fbob 0:b1f2c9e88e32 67 }
fbob 0:b1f2c9e88e32 68 }
fbob 0:b1f2c9e88e32 69
fbob 5:b9947e3d20cf 70 // Converts desired angular velocity (rad/s) to PWM signal (%)
fbob 0:b1f2c9e88e32 71 float Mixer::angular_velocity_to_pwm(float omega)
fbob 0:b1f2c9e88e32 72 {
fbob 0:b1f2c9e88e32 73 float pwm = alpha*pow(omega,2)+beta*omega;
fbob 0:b1f2c9e88e32 74 return pwm;
fbob 0:b1f2c9e88e32 75 }