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?
User | Revision | Line number | New 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 | } |