File content as of revision 7:fb6174d2ea78:

#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) //para rodar comando serial é necessario comentar essa linha
{
}
*/
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);

    // Turn on all motors for 5s
    motor_1 = alpha *pow(w1 ,2) + beta * w1;
    motor_2 = alpha *pow(w2 ,2) + beta * w2;
    motor_3 = alpha *pow(w3 ,2) + beta * w3;
    motor_4 = alpha *pow(w4 ,2) + beta * w4;

}