Yuri De Stefani / CrazyflieController

controlador de atitude

Mixer/Mixer.cpp@8:c96125e9ac70, 2018-11-21 (annotated)

Committer:
yurindes
Date:
Wed Nov 21 10:07:00 2018 +0000
Branch:
yuri
Revision:
8:c96125e9ac70
Parent:
2:54fabc943f25 
teste controlador atitude;

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 2:54fabc943f25 10 Mixer :: Mixer () : motor_1 ( PA_1 ) , motor_2 ( PB_11 ) , motor_3 ( PA_15 ) , motor_4 (PB_9_ALT1 )
yvesyuzo 0:3871dc7bedf7 11 {
yvesyuzo 0:3871dc7bedf7 12 }
yvesyuzo 0:3871dc7bedf7 13
yvesyuzo 0:3871dc7bedf7 14 void Mixer :: actuate ( float f_t , float tau_phi , float tau_theta , float tau_psi )
yvesyuzo 0:3871dc7bedf7 15 {
yvesyuzo 0:3871dc7bedf7 16
yvesyuzo 0:3871dc7bedf7 17 // Define parameters
yvesyuzo 0:3871dc7bedf7 18 float const alpha = 1.081e-7;
yvesyuzo 0:3871dc7bedf7 19 float const beta = 2.678e-11;
yvesyuzo 0:3871dc7bedf7 20
yvesyuzo 0:3871dc7bedf7 21 //calcula as velocidades angulares conforme a relacao entre toque e forca e velocidade angular
yurindes 8:c96125e9ac70 22 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 23 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 24 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 25 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 26
yvesyuzo 0:3871dc7bedf7 27 //confere se nao existe algum valor de velocidade menor que 0 para nao resultar em raiz de numero negativo
yvesyuzo 0:3871dc7bedf7 28 if (w1 < 0) {
yvesyuzo 0:3871dc7bedf7 29 w1 = 0;
yvesyuzo 0:3871dc7bedf7 30 }
yvesyuzo 0:3871dc7bedf7 31 if (w2 < 0) {
yvesyuzo 0:3871dc7bedf7 32 w2 = 0;
yvesyuzo 0:3871dc7bedf7 33 }
yvesyuzo 0:3871dc7bedf7 34 if (w3 < 0) {
yvesyuzo 0:3871dc7bedf7 35 w3 = 0;
yvesyuzo 0:3871dc7bedf7 36 }
yvesyuzo 0:3871dc7bedf7 37 if (w4 < 0) {
yvesyuzo 0:3871dc7bedf7 38 w4 = 0;
yvesyuzo 0:3871dc7bedf7 39 }
yvesyuzo 0:3871dc7bedf7 40 //Valores de velocidade angular descoberto com os torques e forca resultante fornecidos
yvesyuzo 0:3871dc7bedf7 41 w1 = sqrt(w1);
yvesyuzo 0:3871dc7bedf7 42 w2 = sqrt(w2);
yvesyuzo 0:3871dc7bedf7 43 w3 = sqrt(w3);
yvesyuzo 0:3871dc7bedf7 44 w4 = sqrt(w4);
yvesyuzo 2:54fabc943f25 45
yvesyuzo 0:3871dc7bedf7 46 // Turn on all motors for 5s
yvesyuzo 0:3871dc7bedf7 47 motor [0] = alpha *pow(w1 ,2) + beta * w1;
yvesyuzo 0:3871dc7bedf7 48 motor [1] = alpha *pow(w2 ,2) + beta * w2;
yvesyuzo 0:3871dc7bedf7 49 motor [2] = alpha *pow(w3 ,2) + beta * w3;
yvesyuzo 0:3871dc7bedf7 50 motor [3] = alpha *pow(w4 ,2) + beta * w4;
yvesyuzo 0:3871dc7bedf7 51
yvesyuzo 0:3871dc7bedf7 52
yurindes 8:c96125e9ac70 53 }