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@9:15058b4fa090, 2018-09-28 (annotated)

Committer:
fbob
Date:
Fri Sep 28 15:08:41 2018 +0000
Revision:
9:15058b4fa090
Parent:
6:3188b00263e8
Child:
10:7074bc7038d6
Vertical estimator improvements

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