Yuri De Stefani / CrazyflieController

controlador de atitude

Mixer/Mixer.cpp@0:3871dc7bedf7, 2018-10-10 (annotated)

Committer:
yvesyuzo
Date:
Wed Oct 10 11:13:07 2018 +0000
Revision:
0:3871dc7bedf7
Child:
2:54fabc943f25
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yvesyuzo 0:3871dc7bedf7 1 #include "mbed.h"
yvesyuzo 0:3871dc7bedf7 2 #include "Mixer.h"
yvesyuzo 0:3871dc7bedf7 3 #include <math.h> /* sqrt */
yvesyuzo 0:3871dc7bedf7 4 #include "Library.h"
yvesyuzo 0:3871dc7bedf7 5
yvesyuzo 0:3871dc7bedf7 6 //Declare motors as PWM outputs
yvesyuzo 0:3871dc7bedf7 7 PwmOut motor[4] = {(PA_1),(PB_11),(PA_15),(PB_9_ALT1)};
yvesyuzo 0:3871dc7bedf7 8
yvesyuzo 0:3871dc7bedf7 9 // Class constructor
yvesyuzo 0:3871dc7bedf7 10 Mixer :: Mixer () : motor_1 ( PA_1 ) , motor_2 ( PB_11 ) , motor_3 ( PA_15 ) , motor_4 (
yvesyuzo 0:3871dc7bedf7 11 PB_9_ALT1 )
yvesyuzo 0:3871dc7bedf7 12 {
yvesyuzo 0:3871dc7bedf7 13 }
yvesyuzo 0:3871dc7bedf7 14
yvesyuzo 0:3871dc7bedf7 15
yvesyuzo 0:3871dc7bedf7 16 void Mixer :: actuate ( float f_t , float tau_phi , float tau_theta , float tau_psi )
yvesyuzo 0:3871dc7bedf7 17 {
yvesyuzo 0:3871dc7bedf7 18
yvesyuzo 0:3871dc7bedf7 19 // Define parameters
yvesyuzo 0:3871dc7bedf7 20 float const alpha = 1.081e-7;
yvesyuzo 0:3871dc7bedf7 21 float const beta = 2.678e-11;
yvesyuzo 0:3871dc7bedf7 22
yvesyuzo 0:3871dc7bedf7 23 //calcula as velocidades angulares conforme a relacao entre toque e forca e velocidade angular
yvesyuzo 0:3871dc7bedf7 24 w1 = ((1/(4*kl))* f_t) - ((1/(4*kl*l))* tau_phi) - ((1/(4*kl))* tau_theta) - ((1/(4*kd))* tau_psi) ;
yvesyuzo 0:3871dc7bedf7 25 w2 = ((1/(4*kl))* f_t) - ((1/(4*kl*l))* tau_phi) + ((1/(4*kl*l))* tau_theta) + ((1/(4*kd))* tau_psi);
yvesyuzo 0:3871dc7bedf7 26 w3 = ((1/(4*kl))* f_t) + ((1/(4*kl*l))* tau_phi) + ((1/(4*kl*l))* tau_theta) - ((1/(4*kd))* tau_psi);
yvesyuzo 0:3871dc7bedf7 27 w4 = ((1/(4*kl))* f_t) + ((1/(4*kl*l))* tau_phi) - ((1/(4*kl*l))* tau_theta) + ((1/(4*kd))* tau_psi);
yvesyuzo 0:3871dc7bedf7 28
yvesyuzo 0:3871dc7bedf7 29 //confere se nao existe algum valor de velocidade menor que 0 para nao resultar em raiz de numero negativo
yvesyuzo 0:3871dc7bedf7 30 if (w1 < 0) {
yvesyuzo 0:3871dc7bedf7 31 w1 = 0;
yvesyuzo 0:3871dc7bedf7 32 }
yvesyuzo 0:3871dc7bedf7 33 if (w2 < 0) {
yvesyuzo 0:3871dc7bedf7 34 w2 = 0;
yvesyuzo 0:3871dc7bedf7 35 }
yvesyuzo 0:3871dc7bedf7 36 if (w3 < 0) {
yvesyuzo 0:3871dc7bedf7 37 w3 = 0;
yvesyuzo 0:3871dc7bedf7 38 }
yvesyuzo 0:3871dc7bedf7 39 if (w4 < 0) {
yvesyuzo 0:3871dc7bedf7 40 w4 = 0;
yvesyuzo 0:3871dc7bedf7 41 }
yvesyuzo 0:3871dc7bedf7 42 //Valores de velocidade angular descoberto com os torques e forca resultante fornecidos
yvesyuzo 0:3871dc7bedf7 43 w1 = sqrt(w1);
yvesyuzo 0:3871dc7bedf7 44 w2 = sqrt(w2);
yvesyuzo 0:3871dc7bedf7 45 w3 = sqrt(w3);
yvesyuzo 0:3871dc7bedf7 46 w4 = sqrt(w4);
yvesyuzo 0:3871dc7bedf7 47
yvesyuzo 0:3871dc7bedf7 48
yvesyuzo 0:3871dc7bedf7 49
yvesyuzo 0:3871dc7bedf7 50 wait (5) ;
yvesyuzo 0:3871dc7bedf7 51 // Turn on all motors for 5s
yvesyuzo 0:3871dc7bedf7 52 motor [0] = alpha *pow(w1 ,2) + beta * w1;
yvesyuzo 0:3871dc7bedf7 53 motor [1] = alpha *pow(w2 ,2) + beta * w2;
yvesyuzo 0:3871dc7bedf7 54 motor [2] = alpha *pow(w3 ,2) + beta * w3;
yvesyuzo 0:3871dc7bedf7 55 motor [3] = alpha *pow(w4 ,2) + beta * w4;
yvesyuzo 0:3871dc7bedf7 56
yvesyuzo 0:3871dc7bedf7 57
yvesyuzo 0:3871dc7bedf7 58 }
yvesyuzo 0:3871dc7bedf7 59
yvesyuzo 0:3871dc7bedf7 60
yvesyuzo 0:3871dc7bedf7 61
yvesyuzo 0:3871dc7bedf7 62
yvesyuzo 0:3871dc7bedf7 63
yvesyuzo 0:3871dc7bedf7 64