Jeanne Le Soudeer
/
PSL-VicTeam
fusion leopold sasa
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?
User | Revision | Line number | New 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 | } |