controlador de atitude
Mixer/Mixer.cpp
- Committer:
- yvesyuzo
- Date:
- 2018-10-10
- Revision:
- 0:3871dc7bedf7
- Child:
- 2:54fabc943f25
File content as of revision 0:3871dc7bedf7:
#include "mbed.h" #include "Mixer.h" #include <math.h> /* sqrt */ #include "Library.h" //Declare motors as PWM outputs PwmOut motor[4] = {(PA_1),(PB_11),(PA_15),(PB_9_ALT1)}; // Class constructor Mixer :: Mixer () : motor_1 ( PA_1 ) , motor_2 ( PB_11 ) , motor_3 ( PA_15 ) , motor_4 ( PB_9_ALT1 ) { } void Mixer :: actuate ( float f_t , float tau_phi , float tau_theta , float tau_psi ) { // Define parameters float const alpha = 1.081e-7; float const beta = 2.678e-11; //calcula as velocidades angulares conforme a relacao entre toque e forca e velocidade angular w1 = ((1/(4*kl))* f_t) - ((1/(4*kl*l))* tau_phi) - ((1/(4*kl))* tau_theta) - ((1/(4*kd))* tau_psi) ; w2 = ((1/(4*kl))* f_t) - ((1/(4*kl*l))* tau_phi) + ((1/(4*kl*l))* tau_theta) + ((1/(4*kd))* tau_psi); w3 = ((1/(4*kl))* f_t) + ((1/(4*kl*l))* tau_phi) + ((1/(4*kl*l))* tau_theta) - ((1/(4*kd))* tau_psi); w4 = ((1/(4*kl))* f_t) + ((1/(4*kl*l))* tau_phi) - ((1/(4*kl*l))* tau_theta) + ((1/(4*kd))* tau_psi); //confere se nao existe algum valor de velocidade menor que 0 para nao resultar em raiz de numero negativo if (w1 < 0) { w1 = 0; } if (w2 < 0) { w2 = 0; } if (w3 < 0) { w3 = 0; } if (w4 < 0) { w4 = 0; } //Valores de velocidade angular descoberto com os torques e forca resultante fornecidos w1 = sqrt(w1); w2 = sqrt(w2); w3 = sqrt(w3); w4 = sqrt(w4); wait (5) ; // Turn on all motors for 5s motor [0] = alpha *pow(w1 ,2) + beta * w1; motor [1] = alpha *pow(w2 ,2) + beta * w2; motor [2] = alpha *pow(w3 ,2) + beta * w3; motor [3] = alpha *pow(w4 ,2) + beta * w4; }