File content as of revision 0:332926683ee3:

#include "mbed.h"
#include "Mixer.h"

// Class constructor
Mixer::Mixer() : motor0(PA_1), motor1(PB_11), motor2(PA_15), motor3(PB_9_ALT1)
{
}

// Actuate motors with the desired force (N) and torques (N.m)
void Mixer::actuate( float ft , float tau_phi , float tau_theta , float tau_psi )
{
    ft_to_omega(ft,tau_phi,tau_theta,tau_psi);
    motor0 = omega_to_pwm(omega0);
    motor1 = omega_to_pwm(omega1);
    motor2 = omega_to_pwm(omega2);
    motor3 = omega_to_pwm(omega3);
}

// Converts desired force (N) and torques (N.m) to angular velocities ( rad /s)
void Mixer::ft_to_omega( float ft , float tau_phi , float tau_theta , float tau_psi )
{
    float omega02 = (1/(4*kl))*ft - (1/(4*kl*l))*tau_phi - (1/(4*kl*l))*tau_theta - (1/(4*kd))*tau_psi;
    float omega12 = (1/(4*kl))*ft - (1/(4*kl*l))*tau_phi + (1/(4*kl*l))*tau_theta + (1/(4*kd))*tau_psi;
    float omega22 = (1/(4*kl))*ft + (1/(4*kl*l))*tau_phi + (1/(4*kl*l))*tau_theta - (1/(4*kd))*tau_psi;
    float omega32 = (1/(4*kl))*ft + (1/(4*kl*l))*tau_phi - (1/(4*kl*l))*tau_theta + (1/(4*kd))*tau_psi;
    
    if (omega02 < 0){
        omega02 = 0;
    }
    if (omega12 < 0){
        omega12 = 0;
    }
    if (omega22 < 0){
        omega22 = 0;
    }
    if (omega32 < 0){
        omega32 = 0;
    }
    
    omega0 = sqrt(omega02);
    omega1 = sqrt(omega12);
    omega2 = sqrt(omega22);
    omega3 = sqrt(omega32);
}

// Converts desired angular velocity (rad/s) to PWM signal
float Mixer::omega_to_pwm( float omega )
{
    float pwm = alpha*pow(omega,2) + beta*omega;
    return pwm;
}