Mixer.cpp

00001 #include "mbed.h"
00002 #include "Mixer.h"
00003 #include <math.h>       /* sqrt */
00004 #include "Library.h"
00005 
00006 //Declare motors as PWM outputs
00007 PwmOut motor[4] = {(PA_1),(PB_11),(PA_15),(PB_9_ALT1)};
00008 
00009 // Class constructor
00010 Mixer :: Mixer () : motor_1 ( PA_1 ) , motor_2 ( PB_11 ) , motor_3 ( PA_15 ) , motor_4 (PB_9_ALT1 )
00011 {
00012 }
00013 
00014 void Mixer :: actuate ( float f_t , float tau_phi , float tau_theta , float tau_psi )
00015 {
00016     
00017     // Define parameters
00018     float const alpha = 1.081e-7;
00019     float const beta = 2.678e-11;
00020     
00021     //calcula as velocidades angulares conforme a relacao entre toque e forca e velocidade angular
00022     w1 = ((1/(4*kl))* f_t) - ((1/(4*kl*l))* tau_phi) - ((1/(4*kl)) * tau_theta) - ((1/(4*kd))* tau_psi)  ;
00023     w2 = ((1/(4*kl))* f_t) - ((1/(4*kl*l))* tau_phi) + ((1/(4*kl*l))* tau_theta) + ((1/(4*kd))* tau_psi);
00024     w3 = ((1/(4*kl))* f_t) + ((1/(4*kl*l))* tau_phi) + ((1/(4*kl*l))* tau_theta) - ((1/(4*kd))* tau_psi);
00025     w4 = ((1/(4*kl))* f_t) + ((1/(4*kl*l))* tau_phi) - ((1/(4*kl*l))* tau_theta) + ((1/(4*kd))* tau_psi);
00026 
00027     //confere se nao existe algum valor de velocidade menor que 0 para nao resultar em raiz de numero negativo
00028     if (w1 < 0) {
00029         w1 = 0;
00030     }
00031     if (w2 < 0) {
00032         w2 = 0;
00033     }
00034     if (w3 < 0) {
00035         w3 = 0;
00036     }
00037     if (w4 < 0) {
00038         w4 = 0;
00039     }
00040     //Valores de velocidade angular descoberto com os torques e forca resultante fornecidos
00041     w1 = sqrt(w1);
00042     w2 = sqrt(w2);
00043     w3 = sqrt(w3);
00044     w4 = sqrt(w4);
00045 
00046     // Turn on all motors for 5s
00047     motor [0] = alpha *pow(w1 ,2) + beta * w1;
00048     motor [1] = alpha *pow(w2 ,2) + beta * w2;
00049     motor [2] = alpha *pow(w3 ,2) + beta * w3;
00050     motor [3] = alpha *pow(w4 ,2) + beta * w4;
00051     
00052 
00053 }