Robocup5 / Mbed OS DISCO_L475VG_IOT01-Sensors-BSP

prog robocup equip2

Dependencies:   nRF24L01P MX12 BSP_B-L475E-IOT01

Projet.cpp@1:2bc2848b78d1, 2021-04-23 (annotated)

Committer:
chedozeauclement
Date:
Fri Apr 23 09:08:02 2021 +0000
Revision:
1:2bc2848b78d1
Parent:
0:7b8f377e2077 
Prog comm/mvt;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chedozeauclement 0:7b8f377e2077 1 #include "Projet.h"
chedozeauclement 0:7b8f377e2077 2
chedozeauclement 0:7b8f377e2077 3 /* description de la fonction
chedozeauclement 0:7b8f377e2077 4
chedozeauclement 0:7b8f377e2077 5 Entrée : vitesses dans le repère du terrain + vitesse angulaire robot
chedozeauclement 0:7b8f377e2077 6 Sortie : vitesse de rotation de chaque moteur
chedozeauclement 0:7b8f377e2077 7
chedozeauclement 0:7b8f377e2077 8 */
chedozeauclement 0:7b8f377e2077 9
chedozeauclement 0:7b8f377e2077 10 #define PI 3.14159
chedozeauclement 0:7b8f377e2077 11
chedozeauclement 0:7b8f377e2077 12 void cmd_inverse(float Vx_m_s,float Vy_m_s,float Omega_rad_s,float Theta_rad, MX12 *pmx)
chedozeauclement 0:7b8f377e2077 13
chedozeauclement 0:7b8f377e2077 14 {
chedozeauclement 0:7b8f377e2077 15
chedozeauclement 0:7b8f377e2077 16 float O1;
chedozeauclement 0:7b8f377e2077 17 float O2;
chedozeauclement 0:7b8f377e2077 18 float O3;
chedozeauclement 0:7b8f377e2077 19
chedozeauclement 0:7b8f377e2077 20
chedozeauclement 0:7b8f377e2077 21 float norme_vitesse = pow((pow(Vx_m_s,2)+pow(Vy_m_s,2)),0.5);
chedozeauclement 0:7b8f377e2077 22 float Alpha_rad;
chedozeauclement 0:7b8f377e2077 23
chedozeauclement 0:7b8f377e2077 24 Alpha_rad = atan2(Vy_m_s,Vx_m_s);
chedozeauclement 0:7b8f377e2077 25
chedozeauclement 0:7b8f377e2077 26 if(Alpha_rad<0) {
chedozeauclement 0:7b8f377e2077 27 Alpha_rad=2*PI+Alpha_rad;
chedozeauclement 0:7b8f377e2077 28 }
chedozeauclement 0:7b8f377e2077 29
chedozeauclement 0:7b8f377e2077 30 float Vrx_m_s = norme_vitesse*cosf(Alpha_rad-Theta_rad);
chedozeauclement 0:7b8f377e2077 31 float Vry_m_s = norme_vitesse*sinf(Alpha_rad-Theta_rad);
chedozeauclement 0:7b8f377e2077 32
chedozeauclement 0:7b8f377e2077 33 O1=-(Vrx_m_s*pow(3,0.5)/2+Vry_m_s*0.5)/0.019+Omega_rad_s*0.078/0.019;
chedozeauclement 0:7b8f377e2077 34 O3=(Vrx_m_s*pow(3,0.5)/2-Vry_m_s*0.5)/0.019+Omega_rad_s*0.078/0.019;
chedozeauclement 0:7b8f377e2077 35 O2=Vry_m_s/0.019+Omega_rad_s*0.078/0.019;
chedozeauclement 0:7b8f377e2077 36
chedozeauclement 0:7b8f377e2077 37 float PO1=O1*1/(470*2*PI/60)/10/2;
chedozeauclement 0:7b8f377e2077 38 float PO2=O2*1/(470*2*PI/60)/10/2;
chedozeauclement 0:7b8f377e2077 39 float PO3=O3*1/(470*2*PI/60)/10/2;
chedozeauclement 0:7b8f377e2077 40
chedozeauclement 0:7b8f377e2077 41 printf(" %d, %d, %d \n", int(O1*100*0.019),int(O2*100*0.019),int(O3*100*0.019));
chedozeauclement 0:7b8f377e2077 42
chedozeauclement 0:7b8f377e2077 43 pmx->SetSpeed(1,PO1);
chedozeauclement 0:7b8f377e2077 44 pmx->SetSpeed(2,PO2);
chedozeauclement 0:7b8f377e2077 45 pmx->SetSpeed(3,PO3);
chedozeauclement 0:7b8f377e2077 46
chedozeauclement 0:7b8f377e2077 47 }