Jeanne Le Soudeer
/
PSL-VicTeam
fusion leopold sasa
main.cpp
- Committer:
- leocle
- Date:
- 2020-04-24
- Revision:
- 1:c5e9c436f8d0
- Parent:
- 0:8762773cddf9
- Child:
- 2:223d6363315b
File content as of revision 1:c5e9c436f8d0:
/* #include <mbed_events.h> */ #include "mbed.h" #include "AX12.h" DigitalOut alivenessLED(LED1); DigitalOut testLed(LED2); Serial coach(D1, D0); const float rapport_vitesse = 0.019383809*0.026315; const float root3 = 1.7321; AX12 myax12a (PC_4, PC_5, 2); AX12 myax12b (PC_4, PC_5, 3); AX12 myax12c (PC_4, PC_5, 4); int i = 0; char last_read; int indice_a_modifie = 1; char message[50]; char message_vide = '0'; float front, lat, rot, T_front, T_lat = 0; /*typedef struct { float front; float lat; float rot ; } dep; typedef struct { float front; float lat; } tire;*/ /* void blinkCallback(void) { alivenessLED = !alivenessLED; }*/ void setup() { //eventQueue.call_every(500, blinkCallback); coach.baud(115200); myax12a.SetMode(1); myax12b.SetMode(1); myax12c.SetMode(1); } void asser(float Vx, float Vy, float gammaz) { float wa = rapport_vitesse*(Vx + 100 * gammaz); float wb = rapport_vitesse*( -0.5 * Vx - (root3 / 2) * Vy + 100 * gammaz); float wc = rapport_vitesse*( -0.5 * Vx + (root3 / 2) * Vy + 100 * gammaz); myax12a.SetCRSpeed(wa); myax12b.SetCRSpeed(wb); myax12c.SetCRSpeed(wc); } void send_stat() { coach.printf("S%f,%f,%f,%f,%f \n", front, lat, rot, T_front, T_lat ); } void mise_a_zero_msg() { for(i=0; i<50; i++) { //On réinitialise la chaîne de caractères message[i] = message_vide; } indice_a_modifie = 0; } void reception_com() { if (coach.readable()) { last_read = coach.getc(); /* if (last_read == 'A') { wait(0.05); indice_a_modifie = 1; last_read = coach.getc(); while ((last_read != ';')) { message[indice_a_modifie] = last_read; indice_a_modifie ++; coach.printf("|%i|%s|", indice_a_modifie, message); last_read = coach.getc(); } message[indice_a_modifie] = last_read; sscanf(message,"%f,%f,%f,%f,%f;", &front, &lat, &rot, &front, &lat ); mise_a_zero_msg(); }*/ switch(last_read){ case 'Z': front = 0; lat = 0; rot = 0; T_front = 0; T_lat = 0; break; case 'a': front ++; break; case 'b': front --; break; case 'A': front = front + 10; break; case 'B': front = front - 10; break; case 'c': lat ++; break; case 'd': lat --; break; case 'C': lat = lat + 10; break; case 'D': lat = lat - 10; break; case 'e': rot ++; break; case 'f': rot --; break; case 'E': rot = rot + 10; break; case 'F': rot = rot - 10; break; case 'S' : send_stat(); } } } int main() { setup(); while (true) { reception_com(); asser(front, lat, rot); } }