Leopold Clement / Mbed 2 deprecated PSL-VicTeam

faut que Val répare le truc

Dependencies:   mbed AX12

main.cpp@4:05d5fe68173b, 2020-05-22 (annotated)

Committer:
leocle
Date:
Fri May 22 07:25:44 2020 +0000
Revision:
4:05d5fe68173b
Parent:
3:c2c22b4f984b
Child:
5:86f9ec683ad5
Jeanne ...

Who changed what in which revision?

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