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@0:b1f2c9e88e32, 2018-08-31 (annotated)

Committer:
fbob
Date:
Fri Aug 31 18:41:31 2018 +0000
Revision:
0:b1f2c9e88e32
Child:
5:b9947e3d20cf
Include attitude controller, attitude estimator and mixer

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 0:b1f2c9e88e32 4 // Class constructor
fbob 0:b1f2c9e88e32 5 //Mixer::Mixer(PinName pin_1, PinName pin_2, PinName pin_3, PinName pin_4) : motor_1(pin_1) , motor_2(pin_2) , motor_3(pin_3) , motor_4(pin_4)
fbob 0:b1f2c9e88e32 6 Mixer::Mixer() : motor_1(PA_1), motor_2(PB_11), motor_3(PA_15), motor_4(PB_9_ALT1)
fbob 0:b1f2c9e88e32 7 {
fbob 0:b1f2c9e88e32 8 }
fbob 0:b1f2c9e88e32 9
fbob 0:b1f2c9e88e32 10 // Actuate motors with the desired force (N) and torques (N.m)
fbob 0:b1f2c9e88e32 11 void Mixer::actuate(float f_t, float tau_phi, float tau_theta, float tau_psi)
fbob 0:b1f2c9e88e32 12 {
fbob 0:b1f2c9e88e32 13 // Convert desired force (N) and torques (N.m) to angular velocities (rad/s)
fbob 0:b1f2c9e88e32 14 force_and_torques_to_angular_velocities(f_t,tau_phi,tau_theta,tau_psi);
fbob 0:b1f2c9e88e32 15 // Convert desired angular velocity (rad/s) to PWM signal and actuate motors
fbob 0:b1f2c9e88e32 16 motor_1 = angular_velocity_to_pwm(omega_1);
fbob 0:b1f2c9e88e32 17 motor_2 = angular_velocity_to_pwm(omega_2);
fbob 0:b1f2c9e88e32 18 motor_3 = angular_velocity_to_pwm(omega_3);
fbob 0:b1f2c9e88e32 19 motor_4 = angular_velocity_to_pwm(omega_4);
fbob 0:b1f2c9e88e32 20 }
fbob 0:b1f2c9e88e32 21
fbob 0:b1f2c9e88e32 22 // Converts desired force (N) and torques (N.m) to angular velocities (rad/s)
fbob 0:b1f2c9e88e32 23 void Mixer::force_and_torques_to_angular_velocities(float f_t, float tau_phi, float tau_theta, float tau_psi)
fbob 0:b1f2c9e88e32 24 {
fbob 0:b1f2c9e88e32 25 // Convert desired force (N) and torques (N.m) to angular velocities squared (rad^2/s^2)
fbob 0:b1f2c9e88e32 26 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 27 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 28 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 29 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 30 // Convert angular velocities squared (rad^2/s^2) to angular velocities (rad/s) only if value is positive
fbob 0:b1f2c9e88e32 31 if(omega_1_squared>=0) {
fbob 0:b1f2c9e88e32 32 omega_1 = sqrt(omega_1_squared);
fbob 0:b1f2c9e88e32 33 } else {
fbob 0:b1f2c9e88e32 34 omega_1 = 0.0f;
fbob 0:b1f2c9e88e32 35 }
fbob 0:b1f2c9e88e32 36 if(omega_2_squared>=0) {
fbob 0:b1f2c9e88e32 37 omega_2 = sqrt(omega_2_squared);
fbob 0:b1f2c9e88e32 38 } else {
fbob 0:b1f2c9e88e32 39 omega_2 = 0.0f;
fbob 0:b1f2c9e88e32 40 }
fbob 0:b1f2c9e88e32 41 if(omega_3_squared>=0) {
fbob 0:b1f2c9e88e32 42 omega_3 = sqrt(omega_3_squared);
fbob 0:b1f2c9e88e32 43 } else {
fbob 0:b1f2c9e88e32 44 omega_3 = 0.0f;
fbob 0:b1f2c9e88e32 45 }
fbob 0:b1f2c9e88e32 46 if(omega_4_squared>=0) {
fbob 0:b1f2c9e88e32 47 omega_4 = sqrt(omega_4_squared);
fbob 0:b1f2c9e88e32 48 } else {
fbob 0:b1f2c9e88e32 49 omega_4 = 0.0f;
fbob 0:b1f2c9e88e32 50 }
fbob 0:b1f2c9e88e32 51 }
fbob 0:b1f2c9e88e32 52
fbob 0:b1f2c9e88e32 53 // Converts desired angular velocity (rad/s) to PWM signal
fbob 0:b1f2c9e88e32 54 float Mixer::angular_velocity_to_pwm(float omega)
fbob 0:b1f2c9e88e32 55 {
fbob 0:b1f2c9e88e32 56 // Convert desired angular velocity (rad/s) to PWM signal
fbob 0:b1f2c9e88e32 57 float pwm = alpha*pow(omega,2)+beta*omega;
fbob 0:b1f2c9e88e32 58 return pwm;
fbob 0:b1f2c9e88e32 59 }