Leopold Clement / Mbed 2 deprecated PSL-VicTeam

faut que Val répare le truc

Dependencies:   mbed AX12

main.cpp@2:223d6363315b, 2020-05-15 (annotated)

Committer:
leocle
Date:
Fri May 15 09:51:02 2020 +0000
Revision:
2:223d6363315b
Parent:
1:c5e9c436f8d0
Child:
3:c2c22b4f984b
communication qui marche vraiment

Who changed what in which revision?

UserRevisionLine numberNew contents of line
leocle 0:8762773cddf9 1
leocle 0:8762773cddf9 2 /* #include <mbed_events.h> */
leocle 0:8762773cddf9 3 #include "mbed.h"
leocle 0:8762773cddf9 4 #include "AX12.h"
leocle 0:8762773cddf9 5
leocle 0:8762773cddf9 6 DigitalOut alivenessLED(LED1);
leocle 0:8762773cddf9 7 DigitalOut testLed(LED2);
leocle 0:8762773cddf9 8
leocle 2:223d6363315b 9 /* Serial coach(D1, D0, 115200); */
leocle 2:223d6363315b 10 Serial coach(USBTX, USBRX);
leocle 0:8762773cddf9 11
leocle 0:8762773cddf9 12 const float rapport_vitesse = 0.019383809*0.026315;
leocle 0:8762773cddf9 13 const float root3 = 1.7321;
leocle 0:8762773cddf9 14 AX12 myax12a (PC_4, PC_5, 2);
leocle 0:8762773cddf9 15 AX12 myax12b (PC_4, PC_5, 3);
leocle 0:8762773cddf9 16 AX12 myax12c (PC_4, PC_5, 4);
leocle 0:8762773cddf9 17
leocle 0:8762773cddf9 18 int i = 0;
leocle 0:8762773cddf9 19 char last_read;
leocle 1:c5e9c436f8d0 20 int indice_a_modifie = 1;
leocle 2:223d6363315b 21 char message_0[50] = {};
leocle 2:223d6363315b 22 char message_1[50] = {};
leocle 1:c5e9c436f8d0 23 char message_vide = '0';
leocle 0:8762773cddf9 24 float front, lat, rot, T_front, T_lat = 0;
leocle 2:223d6363315b 25 int flag_msg = 0;
leocle 2:223d6363315b 26 int flag_a_traite = 0;
leocle 1:c5e9c436f8d0 27
leocle 1:c5e9c436f8d0 28 void asser(float Vx, float Vy, float gammaz)
leocle 1:c5e9c436f8d0 29 {
leocle 0:8762773cddf9 30 float wa = rapport_vitesse*(Vx + 100 * gammaz);
leocle 0:8762773cddf9 31 float wb = rapport_vitesse*( -0.5 * Vx - (root3 / 2) * Vy + 100 * gammaz);
leocle 0:8762773cddf9 32 float wc = rapport_vitesse*( -0.5 * Vx + (root3 / 2) * Vy + 100 * gammaz);
leocle 0:8762773cddf9 33 myax12a.SetCRSpeed(wa);
leocle 0:8762773cddf9 34 myax12b.SetCRSpeed(wb);
leocle 0:8762773cddf9 35 myax12c.SetCRSpeed(wc);
leocle 1:c5e9c436f8d0 36 }
leocle 0:8762773cddf9 37
leocle 1:c5e9c436f8d0 38 void send_stat()
leocle 1:c5e9c436f8d0 39 {
leocle 1:c5e9c436f8d0 40 coach.printf("S%f,%f,%f,%f,%f \n", front, lat, rot, T_front, T_lat );
leocle 1:c5e9c436f8d0 41 }
leocle 0:8762773cddf9 42
leocle 2:223d6363315b 43 void mise_a_zero_msg_0()
leocle 1:c5e9c436f8d0 44 {
leocle 1:c5e9c436f8d0 45 for(i=0; i<50; i++) { //On réinitialise la chaîne de caractères
leocle 2:223d6363315b 46 message_0[i] = message_vide;
leocle 1:c5e9c436f8d0 47 }
leocle 2:223d6363315b 48 }
leocle 2:223d6363315b 49
leocle 2:223d6363315b 50
leocle 2:223d6363315b 51 void mise_a_zero_msg_1()
leocle 2:223d6363315b 52 {
leocle 2:223d6363315b 53 for(i=0; i<50; i++) { //On réinitialise la chaîne de caractères
leocle 2:223d6363315b 54 message_1[i] = message_vide;
leocle 2:223d6363315b 55 }
leocle 1:c5e9c436f8d0 56 }
leocle 1:c5e9c436f8d0 57
leocle 2:223d6363315b 58 void callback_serial()
leocle 1:c5e9c436f8d0 59 {
leocle 2:223d6363315b 60 last_read = coach.getc();
leocle 2:223d6363315b 61 if (last_read == 'S') {
leocle 2:223d6363315b 62 send_stat();
leocle 2:223d6363315b 63 } else if (last_read == ';') {
leocle 2:223d6363315b 64 coach.puts("EOL");
leocle 2:223d6363315b 65 coach.puts(message_0);
leocle 2:223d6363315b 66 coach.puts(message_1);
leocle 2:223d6363315b 67 if (flag_msg == 0) {
leocle 2:223d6363315b 68 message_0[indice_a_modifie] = last_read;
leocle 2:223d6363315b 69 mise_a_zero_msg_1();
leocle 2:223d6363315b 70 flag_msg = 1;
leocle 2:223d6363315b 71 } else {
leocle 2:223d6363315b 72 message_1[indice_a_modifie] = last_read;
leocle 2:223d6363315b 73 mise_a_zero_msg_0();
leocle 2:223d6363315b 74 flag_msg = 0;
leocle 2:223d6363315b 75 }
leocle 2:223d6363315b 76 flag_a_traite = 1;
leocle 2:223d6363315b 77 indice_a_modifie = 0;
leocle 2:223d6363315b 78 } else {
leocle 2:223d6363315b 79 if (flag_msg == 0) {
leocle 2:223d6363315b 80 message_0[indice_a_modifie] = last_read;
leocle 2:223d6363315b 81 } else {
leocle 2:223d6363315b 82 message_1[indice_a_modifie] = last_read;
leocle 2:223d6363315b 83 }
leocle 2:223d6363315b 84 indice_a_modifie++;
leocle 2:223d6363315b 85 }
leocle 2:223d6363315b 86 }
leocle 1:c5e9c436f8d0 87
leocle 2:223d6363315b 88 void reception_com()
leocle 2:223d6363315b 89 {
leocle 2:223d6363315b 90 if (flag_a_traite == 1) {
leocle 2:223d6363315b 91 coach.puts("traitement");
leocle 2:223d6363315b 92 if (flag_msg == 0) {
leocle 2:223d6363315b 93 sscanf(message_1,"%f,%f,%f,%f,%f", &front, &lat, &rot, &T_front, &T_lat );
leocle 2:223d6363315b 94 } else {
leocle 2:223d6363315b 95 sscanf(message_0,"%f,%f,%f,%f,%f", &front, &lat, &rot, &T_front, &T_lat );
leocle 1:c5e9c436f8d0 96 }
leocle 2:223d6363315b 97 flag_a_traite = 0;
leocle 1:c5e9c436f8d0 98 }
leocle 1:c5e9c436f8d0 99 }
leocle 0:8762773cddf9 100
leocle 2:223d6363315b 101 void setup()
leocle 2:223d6363315b 102 {
leocle 2:223d6363315b 103 coach.baud(115200);
leocle 2:223d6363315b 104 myax12a.SetMode(1);
leocle 2:223d6363315b 105 myax12b.SetMode(1);
leocle 2:223d6363315b 106 myax12c.SetMode(1);
leocle 2:223d6363315b 107 coach.puts("stl\n\r");
leocle 2:223d6363315b 108 coach.attach(&callback_serial, Serial::RxIrq);
leocle 2:223d6363315b 109 }
leocle 2:223d6363315b 110
leocle 1:c5e9c436f8d0 111 int main()
leocle 1:c5e9c436f8d0 112 {
leocle 0:8762773cddf9 113 setup();
leocle 1:c5e9c436f8d0 114 while (true) {
leocle 0:8762773cddf9 115 reception_com();
leocle 0:8762773cddf9 116 asser(front, lat, rot);
leocle 1:c5e9c436f8d0 117 }
leocle 0:8762773cddf9 118
leocle 2:223d6363315b 119 }