Projet.cpp

00001 #include "Projet.h"
00002 
00003 /* description de la fonction
00004 
00005     Entrée : vitesses dans le repère du terrain + vitesse angulaire robot
00006     Sortie : vitesse de rotation de chaque moteur
00007 
00008 */
00009 
00010 #define PI 3.14159
00011 
00012 void cmd_inverse(float Vx_m_s,float Vy_m_s,float Omega_rad_s,float Theta_rad, MX12 *pmx)
00013 
00014 {
00015     
00016     float O1;
00017     float O2;
00018     float O3;
00019     
00020 
00021     float norme_vitesse = pow((pow(Vx_m_s,2)+pow(Vy_m_s,2)),0.5);
00022     float Alpha_rad;
00023 
00024     Alpha_rad = atan2(Vy_m_s,Vx_m_s);
00025     
00026         if(Alpha_rad<0) {
00027         Alpha_rad=2*PI+Alpha_rad;
00028         }
00029 
00030     float Vrx_m_s = norme_vitesse*cosf(Alpha_rad-Theta_rad);
00031     float Vry_m_s = norme_vitesse*sinf(Alpha_rad-Theta_rad);
00032 
00033     O1=-(Vrx_m_s*pow(3,0.5)/2+Vry_m_s*0.5)/0.019+Omega_rad_s*0.078/0.019;
00034     O3=(Vrx_m_s*pow(3,0.5)/2-Vry_m_s*0.5)/0.019+Omega_rad_s*0.078/0.019;
00035     O2=Vry_m_s/0.019+Omega_rad_s*0.078/0.019;
00036 
00037     float PO1=O1*1/(470*2*PI/60)/10/2;
00038     float PO2=O2*1/(470*2*PI/60)/10/2;
00039     float PO3=O3*1/(470*2*PI/60)/10/2;
00040 
00041     printf(" %d, %d, %d \n", int(O1*100*0.019),int(O2*100*0.019),int(O3*100*0.019));
00042 
00043     pmx->SetSpeed(1,PO1);
00044     pmx->SetSpeed(2,PO2);
00045     pmx->SetSpeed(3,PO3);
00046 
00047 }