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;
    

}