File content as of revision 1:2bc2848b78d1:

#include "Projet.h"

/* description de la fonction

    Entrée : vitesses dans le repère du terrain + vitesse angulaire robot
    Sortie : vitesse de rotation de chaque moteur

*/

#define PI 3.14159

void cmd_inverse(float Vx_m_s,float Vy_m_s,float Omega_rad_s,float Theta_rad, MX12 *pmx)

{
    
    float O1;
    float O2;
    float O3;
    

    float norme_vitesse = pow((pow(Vx_m_s,2)+pow(Vy_m_s,2)),0.5);
    float Alpha_rad;

    Alpha_rad = atan2(Vy_m_s,Vx_m_s);
    
        if(Alpha_rad<0) {
        Alpha_rad=2*PI+Alpha_rad;
        }

    float Vrx_m_s = norme_vitesse*cosf(Alpha_rad-Theta_rad);
    float Vry_m_s = norme_vitesse*sinf(Alpha_rad-Theta_rad);

    O1=-(Vrx_m_s*pow(3,0.5)/2+Vry_m_s*0.5)/0.019+Omega_rad_s*0.078/0.019;
    O3=(Vrx_m_s*pow(3,0.5)/2-Vry_m_s*0.5)/0.019+Omega_rad_s*0.078/0.019;
    O2=Vry_m_s/0.019+Omega_rad_s*0.078/0.019;

    float PO1=O1*1/(470*2*PI/60)/10/2;
    float PO2=O2*1/(470*2*PI/60)/10/2;
    float PO3=O3*1/(470*2*PI/60)/10/2;

    printf(" %d, %d, %d \n", int(O1*100*0.019),int(O2*100*0.019),int(O3*100*0.019));

    pmx->SetSpeed(1,PO1);
    pmx->SetSpeed(2,PO2);
    pmx->SetSpeed(3,PO3);

}