Leopold Clement / Mbed 2 deprecated PSL-VicTeam

faut que Val répare le truc

Dependencies:   mbed AX12

main.cpp@5:86f9ec683ad5, 2020-05-22 (annotated)

Committer:
leocle
Date:
Fri May 22 07:33:50 2020 +0000
Revision:
5:86f9ec683ad5
Parent:
4:05d5fe68173b
Child:
6:b32a24905cd6
avec les bon numero de moteur

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