Leopold Clement / Mbed 2 deprecated PSL-VicTeam

faut que Val répare le truc

Dependencies:   mbed AX12

main.cpp@19:6338e004c566, 2020-06-10 (annotated)

Committer:
leocle
Date:
Wed Jun 10 14:00:41 2020 +0000
Revision:
19:6338e004c566
Parent:
18:68a8f70707e6 
Rajout de la commande Z

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 14:b5e4219b01d9 3
jlesoudeer 13:dfbfefe2f0a8 4 #define adrr_queue 2
jlesoudeer 13:dfbfefe2f0a8 5 #define adrr_gauche 1
jlesoudeer 13:dfbfefe2f0a8 6 #define adrr_droite 3
leocle 0:8762773cddf9 7
leocle 16:1bbf7e4249d7 8 #define hex_queue 0x02
leocle 16:1bbf7e4249d7 9 #define hex_gauche 0x01
leocle 16:1bbf7e4249d7 10 #define hex_droite 0x03
leocle 14:b5e4219b01d9 11
leocle 0:8762773cddf9 12
leocle 0:8762773cddf9 13 DigitalOut alivenessLED(LED1);
leocle 0:8762773cddf9 14 DigitalOut testLed(LED2);
leocle 0:8762773cddf9 15
jlesoudeer 13:dfbfefe2f0a8 16 Serial coach(D1, D0);
jlesoudeer 13:dfbfefe2f0a8 17 /*Serial coach(USBTX, USBRX);*/
leocle 6:b32a24905cd6 18 Serial myserial_ax12(PC_4,PC_5,115200); // Modifier le baud rate en fonction de votre utilisation des moteurs
leocle 0:8762773cddf9 19
leocle 0:8762773cddf9 20 const float rapport_vitesse = 0.019383809*0.026315;
leocle 0:8762773cddf9 21 const float root3 = 1.7321;
leocle 6:b32a24905cd6 22 /*a=moteur de queue
leocle 5:86f9ec683ad5 23 AX12 myax12a (PC_4, PC_5, 2,115200);
leocle 6:b32a24905cd6 24 b : moteur de gauche
leocle 5:86f9ec683ad5 25 AX12 myax12b (PC_4, PC_5, 1,115200);
leocle 6:b32a24905cd6 26 c : moteur de droite
leocle 6:b32a24905cd6 27 AX12 myax12c (PC_4, PC_5, 3,115200);*/
leocle 0:8762773cddf9 28
leocle 0:8762773cddf9 29 int i = 0;
leocle 0:8762773cddf9 30 char last_read;
leocle 1:c5e9c436f8d0 31 int indice_a_modifie = 1;
leocle 2:223d6363315b 32 char message_0[50] = {};
leocle 2:223d6363315b 33 char message_1[50] = {};
leocle 1:c5e9c436f8d0 34 char message_vide = '0';
leocle 0:8762773cddf9 35 float front, lat, rot, T_front, T_lat = 0;
leocle 2:223d6363315b 36 int flag_msg = 0;
leocle 2:223d6363315b 37 int flag_a_traite = 0;
leocle 1:c5e9c436f8d0 38
leocle 6:b32a24905cd6 39
leocle 6:b32a24905cd6 40 DigitalOut my_led1(LED1);
leocle 6:b32a24905cd6 41 DigitalOut my_led2(LED2);
leocle 6:b32a24905cd6 42 DigitalOut my_led3(LED3);
leocle 6:b32a24905cd6 43 DigitalIn bp(USER_BUTTON);
leocle 6:b32a24905cd6 44
leocle 6:b32a24905cd6 45 void SetCRSpeed(int ID, float speed);
leocle 6:b32a24905cd6 46 void SetMode(int ID, int mode);
leocle 6:b32a24905cd6 47 void SetCWLimit (int ID, int degrees);
leocle 6:b32a24905cd6 48 void SetCCWLimit (int ID, int degrees);
leocle 6:b32a24905cd6 49 void Write(int ID, int start, int bytes, char* data);
leocle 6:b32a24905cd6 50
leocle 6:b32a24905cd6 51 char tata[10] = {};
leocle 6:b32a24905cd6 52 uint16_t position_1;
leocle 6:b32a24905cd6 53 uint16_t position_2;
leocle 6:b32a24905cd6 54 uint16_t position_3;
leocle 6:b32a24905cd6 55 int num_moteur = 1;
leocle 6:b32a24905cd6 56 int vitesse;
leocle 6:b32a24905cd6 57 char sens;
leocle 6:b32a24905cd6 58 int temps_ms = 0;
leocle 6:b32a24905cd6 59 //pour connaitre le nombre de tours
leocle 6:b32a24905cd6 60 int tour_1 = 0;
leocle 6:b32a24905cd6 61 int tour_2 = 0;
leocle 6:b32a24905cd6 62 int tour_3 = 0;
leocle 6:b32a24905cd6 63
leocle 6:b32a24905cd6 64 Ticker ticker_1ms;
leocle 6:b32a24905cd6 65
leocle 6:b32a24905cd6 66 uint16_t position_1_old;
leocle 6:b32a24905cd6 67 uint16_t position_2_old;
leocle 6:b32a24905cd6 68 uint16_t position_3_old;
leocle 6:b32a24905cd6 69 int16_t delta_position_1;
leocle 6:b32a24905cd6 70 int16_t delta_position_2;
leocle 6:b32a24905cd6 71 int16_t delta_position_3;
leocle 6:b32a24905cd6 72
leocle 6:b32a24905cd6 73 //Définition des trames de requête
leocle 14:b5e4219b01d9 74 char trame_demande_pos_1[8]= {0xFF,0xFF,hex_queue,0x4,0x2,0x24,2,0xD2};
leocle 14:b5e4219b01d9 75 char trame_demande_pos_2[8]= {0xFF,0xFF,hex_gauche,0x4,0x2,0x24,2,0xD1};
leocle 14:b5e4219b01d9 76 char trame_demande_pos_3[8]= {0xFF,0xFF,hex_droite,0x4,0x2,0x24,2,0xD0};
leocle 6:b32a24905cd6 77
leocle 6:b32a24905cd6 78
leocle 6:b32a24905cd6 79 //Fonction d'interruption sur réception de caractère sur liaison série
leocle 6:b32a24905cd6 80 void Rx_Irq_fonction(void)
leocle 6:b32a24905cd6 81 {
leocle 6:b32a24905cd6 82 uint16_t position_lu = 0;
leocle 6:b32a24905cd6 83 my_led1 = !my_led1;
leocle 6:b32a24905cd6 84
leocle 6:b32a24905cd6 85 static int i_rx=0;
leocle 6:b32a24905cd6 86 tata[i_rx]=myserial_ax12.getc();
leocle 6:b32a24905cd6 87
leocle 6:b32a24905cd6 88 if (tata[0]!=0xFF) // On vérifie que le premier et le deuxième octet sont biens 0xFF
leocle 6:b32a24905cd6 89 i_rx=0;
leocle 6:b32a24905cd6 90 else i_rx++;
leocle 6:b32a24905cd6 91 if((i_rx==2)&&(tata[1] != 0xFF)) i_rx=0;
leocle 6:b32a24905cd6 92
leocle 6:b32a24905cd6 93 //Une fois qu'on sait que la trame est "juste" on récupère le reste
leocle 6:b32a24905cd6 94 if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x01) && i_rx==7) {
leocle 6:b32a24905cd6 95 my_led3 = !my_led3;
leocle 6:b32a24905cd6 96 i_rx=0;
leocle 6:b32a24905cd6 97
leocle 6:b32a24905cd6 98 if(tata[4] == 0) { //si erreur est nulle
leocle 6:b32a24905cd6 99 //On transforme les données en fonction de ce qu'on lit depuis les moteurs
leocle 6:b32a24905cd6 100 position_lu = tata[5]+((uint16_t)tata[6]<<8);
leocle 6:b32a24905cd6 101 if((0<= position_lu)&&(position_lu<=4095)) {
leocle 6:b32a24905cd6 102 position_1 = position_lu;
leocle 6:b32a24905cd6 103 }
leocle 6:b32a24905cd6 104 }
leocle 6:b32a24905cd6 105 for(int i=0; i<10; i++) {
leocle 6:b32a24905cd6 106 tata[i]=0;
leocle 6:b32a24905cd6 107 }
leocle 6:b32a24905cd6 108 }
leocle 6:b32a24905cd6 109
leocle 6:b32a24905cd6 110 if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x02) && i_rx==7) {
leocle 6:b32a24905cd6 111 my_led3 = !my_led3;
leocle 6:b32a24905cd6 112 i_rx=0;
leocle 6:b32a24905cd6 113
leocle 6:b32a24905cd6 114 if(tata[4]==00) { //si erreur nulle
leocle 6:b32a24905cd6 115 //On transforme les données en fonction de ce qu'on lit depuis les moteurs
leocle 6:b32a24905cd6 116 position_lu = tata[5]+((uint16_t)tata[6]<<8);
leocle 6:b32a24905cd6 117 if((0<= position_lu)&&(position_lu<=4095)) {
leocle 6:b32a24905cd6 118 position_2 = position_lu;
leocle 6:b32a24905cd6 119 }
leocle 6:b32a24905cd6 120
leocle 6:b32a24905cd6 121 }
leocle 6:b32a24905cd6 122 for(int i=0; i<10; i++) {
leocle 6:b32a24905cd6 123 tata[i]=0;
leocle 6:b32a24905cd6 124 }
leocle 6:b32a24905cd6 125 }
leocle 6:b32a24905cd6 126
leocle 6:b32a24905cd6 127 if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x03) && i_rx==7) {
leocle 6:b32a24905cd6 128 my_led3 = !my_led3;
leocle 6:b32a24905cd6 129 i_rx=0;
leocle 6:b32a24905cd6 130
leocle 6:b32a24905cd6 131 if(tata[4]==00) { //si erreur nulle
leocle 6:b32a24905cd6 132 //On transforme les données en fonction de ce qu'on lit depuis les moteurs
leocle 6:b32a24905cd6 133 position_lu = tata[5]+((uint16_t)tata[6]<<8);
leocle 6:b32a24905cd6 134 if((0<= position_lu)&&(position_lu<=4095)) {
leocle 6:b32a24905cd6 135 position_3 = position_lu;
leocle 6:b32a24905cd6 136 }
leocle 6:b32a24905cd6 137
leocle 6:b32a24905cd6 138 }
leocle 6:b32a24905cd6 139 for(int i=0; i<10; i++) {
leocle 6:b32a24905cd6 140 tata[i]=0;
leocle 6:b32a24905cd6 141 }
leocle 6:b32a24905cd6 142 }
leocle 6:b32a24905cd6 143
leocle 6:b32a24905cd6 144 if (i_rx>9) {
leocle 6:b32a24905cd6 145 for(int i=0; i<10; i++)
leocle 6:b32a24905cd6 146 tata[i]=0;
leocle 6:b32a24905cd6 147 i_rx=0;
leocle 6:b32a24905cd6 148 }
leocle 6:b32a24905cd6 149 }
leocle 6:b32a24905cd6 150
leocle 6:b32a24905cd6 151 void fonction_1ms(void)
leocle 6:b32a24905cd6 152 {
leocle 6:b32a24905cd6 153 temps_ms++;
leocle 6:b32a24905cd6 154 }
leocle 6:b32a24905cd6 155
leocle 6:b32a24905cd6 156
leocle 6:b32a24905cd6 157
leocle 1:c5e9c436f8d0 158 void asser(float Vx, float Vy, float gammaz)
leocle 1:c5e9c436f8d0 159 {
leocle 18:68a8f70707e6 160 float wa = rapport_vitesse*(Vy + gammaz);
leocle 18:68a8f70707e6 161 float wb = rapport_vitesse*( -0.5 * Vy + (root3 / 2) * Vx + gammaz);
leocle 18:68a8f70707e6 162 float wc = rapport_vitesse*( -0.5 * Vy - (root3 / 2) * Vx + gammaz);
leocle 12:7d61181d7ac7 163 /*moteur a */
leocle 12:7d61181d7ac7 164 SetCRSpeed(adrr_queue, wa);
leocle 16:1bbf7e4249d7 165 wait(0.1)
leocle 12:7d61181d7ac7 166 /*moteur b */
leocle 12:7d61181d7ac7 167 SetCRSpeed(adrr_gauche, wb);
leocle 16:1bbf7e4249d7 168 wait(0.1)
leocle 12:7d61181d7ac7 169 /* moteur c */
leocle 12:7d61181d7ac7 170 SetCRSpeed(adrr_droite, wc);
leocle 16:1bbf7e4249d7 171 wait(0.1)
leocle 1:c5e9c436f8d0 172 }
leocle 0:8762773cddf9 173
leocle 1:c5e9c436f8d0 174 void send_stat()
leocle 1:c5e9c436f8d0 175 {
leocle 16:1bbf7e4249d7 176 coach.printf("S%f,%f,%f,%f,%f \r\n", front, lat, rot, T_front, T_lat );
leocle 1:c5e9c436f8d0 177 }
leocle 0:8762773cddf9 178
leocle 6:b32a24905cd6 179 void send_roues()
leocle 6:b32a24905cd6 180 {
leocle 16:1bbf7e4249d7 181 coach.printf("\n\rR %5d ; %5d ; %5d ; %5d ; %5d ; %5d\r\n", position_1, delta_position_1, position_2, delta_position_2, position_3, delta_position_3);
leocle 6:b32a24905cd6 182 }
leocle 6:b32a24905cd6 183
leocle 2:223d6363315b 184 void mise_a_zero_msg_0()
leocle 1:c5e9c436f8d0 185 {
leocle 1:c5e9c436f8d0 186 for(i=0; i<50; i++) { //On réinitialise la chaîne de caractères
leocle 2:223d6363315b 187 message_0[i] = message_vide;
leocle 1:c5e9c436f8d0 188 }
leocle 2:223d6363315b 189 }
leocle 2:223d6363315b 190
leocle 2:223d6363315b 191
leocle 2:223d6363315b 192 void mise_a_zero_msg_1()
leocle 2:223d6363315b 193 {
leocle 2:223d6363315b 194 for(i=0; i<50; i++) { //On réinitialise la chaîne de caractères
leocle 2:223d6363315b 195 message_1[i] = message_vide;
leocle 2:223d6363315b 196 }
leocle 1:c5e9c436f8d0 197 }
leocle 1:c5e9c436f8d0 198
leocle 2:223d6363315b 199 void callback_serial()
leocle 1:c5e9c436f8d0 200 {
leocle 2:223d6363315b 201 last_read = coach.getc();
leocle 2:223d6363315b 202 if (last_read == 'S') {
leocle 2:223d6363315b 203 send_stat();
leocle 6:b32a24905cd6 204 } else if (last_read == 'R') {
leocle 6:b32a24905cd6 205 send_roues();
leocle 19:6338e004c566 206 } else if (last_read == 'Z'){
leocle 19:6338e004c566 207 front = 0;
leocle 19:6338e004c566 208 lat = 0;
leocle 19:6338e004c566 209 rot = 0;
leocle 2:223d6363315b 210 } else if (last_read == ';') {
leocle 3:c2c22b4f984b 211 coach.puts("EOL\n");
leocle 3:c2c22b4f984b 212 /*coach.puts(message_0);
leocle 3:c2c22b4f984b 213 coach.puts(message_1); */
leocle 2:223d6363315b 214 if (flag_msg == 0) {
leocle 2:223d6363315b 215 message_0[indice_a_modifie] = last_read;
leocle 2:223d6363315b 216 mise_a_zero_msg_1();
leocle 2:223d6363315b 217 flag_msg = 1;
leocle 2:223d6363315b 218 } else {
leocle 2:223d6363315b 219 message_1[indice_a_modifie] = last_read;
leocle 2:223d6363315b 220 mise_a_zero_msg_0();
leocle 2:223d6363315b 221 flag_msg = 0;
leocle 2:223d6363315b 222 }
leocle 2:223d6363315b 223 flag_a_traite = 1;
leocle 2:223d6363315b 224 indice_a_modifie = 0;
leocle 2:223d6363315b 225 } else {
leocle 2:223d6363315b 226 if (flag_msg == 0) {
leocle 2:223d6363315b 227 message_0[indice_a_modifie] = last_read;
leocle 2:223d6363315b 228 } else {
leocle 2:223d6363315b 229 message_1[indice_a_modifie] = last_read;
leocle 2:223d6363315b 230 }
leocle 2:223d6363315b 231 indice_a_modifie++;
leocle 2:223d6363315b 232 }
leocle 2:223d6363315b 233 }
leocle 1:c5e9c436f8d0 234
leocle 2:223d6363315b 235 void reception_com()
leocle 2:223d6363315b 236 {
leocle 2:223d6363315b 237 if (flag_a_traite == 1) {
leocle 3:c2c22b4f984b 238 coach.puts("traitement\n");
leocle 2:223d6363315b 239 if (flag_msg == 0) {
leocle 2:223d6363315b 240 sscanf(message_1,"%f,%f,%f,%f,%f", &front, &lat, &rot, &T_front, &T_lat );
leocle 2:223d6363315b 241 } else {
leocle 2:223d6363315b 242 sscanf(message_0,"%f,%f,%f,%f,%f", &front, &lat, &rot, &T_front, &T_lat );
leocle 1:c5e9c436f8d0 243 }
leocle 2:223d6363315b 244 flag_a_traite = 0;
leocle 1:c5e9c436f8d0 245 }
leocle 1:c5e9c436f8d0 246 }
leocle 0:8762773cddf9 247
leocle 2:223d6363315b 248
leocle 1:c5e9c436f8d0 249 int main()
leocle 1:c5e9c436f8d0 250 {
leocle 11:75b6dde00307 251
jlesoudeer 8:922178e5d286 252 coach.baud(115200);
leocle 10:1675ab60cf43 253 coach.puts("slt\n\r");
leocle 7:49ef78d60837 254 coach.attach(&callback_serial, Serial::RxIrq);
leocle 7:49ef78d60837 255
leocle 6:b32a24905cd6 256 //myax12.SetMode(1); //passage en Continuous rotation
leocle 6:b32a24905cd6 257
leocle 6:b32a24905cd6 258 //char trame_cmd_speed_1[9]={0xFF,0xFF,0x01,0x5,0x3,0x20,0x10,0x0,0xC6};
leocle 6:b32a24905cd6 259 //char trame_cmd_speed_2[9]={0xFF,0xFF,0x02,0x5,0x3,0x20,0x10,0x0,0xC5};
leocle 6:b32a24905cd6 260 //char trame_cmd_speed_3[9]={0xFF,0xFF,0x03,0x5,0x3,0x20,0x10,0x0,0xC4};
leocle 6:b32a24905cd6 261
leocle 6:b32a24905cd6 262 //pour regarder les 3 moteurs les uns à la suite des autres
jlesoudeer 8:922178e5d286 263
leocle 6:b32a24905cd6 264 int etat=1;
leocle 6:b32a24905cd6 265 int fm_etat=1;
leocle 6:b32a24905cd6 266
leocle 12:7d61181d7ac7 267 SetMode(adrr_queue , 1);
leocle 6:b32a24905cd6 268 wait(0.1);
leocle 12:7d61181d7ac7 269 SetMode(adrr_droite, 1);
leocle 6:b32a24905cd6 270 wait(0.1);
leocle 12:7d61181d7ac7 271 SetMode(adrr_gauche, 1);
leocle 6:b32a24905cd6 272
leocle 6:b32a24905cd6 273 /* SetCRSpeed(1,0.05);
leocle 6:b32a24905cd6 274 wait(0.1);
leocle 6:b32a24905cd6 275 SetCRSpeed(2,0.05);
leocle 6:b32a24905cd6 276 wait(0.1);
leocle 6:b32a24905cd6 277 SetCRSpeed(3,0.05); */
leocle 6:b32a24905cd6 278
leocle 6:b32a24905cd6 279 char a;
leocle 6:b32a24905cd6 280
leocle 6:b32a24905cd6 281 //On passe le mode retour en 1 : retour seulement sur demande de lecture
leocle 6:b32a24905cd6 282 a=1;
leocle 12:7d61181d7ac7 283 Write(adrr_queue,16,1,&a);
leocle 12:7d61181d7ac7 284 Write(adrr_droite,16,1,&a);
leocle 12:7d61181d7ac7 285 Write(adrr_gauche,16,1,&a);
leocle 6:b32a24905cd6 286
leocle 6:b32a24905cd6 287
leocle 6:b32a24905cd6 288 //On passe le délai de retour à 250µs. C'est bien de le diminuer ensuite.
leocle 6:b32a24905cd6 289 a=250;
leocle 12:7d61181d7ac7 290 Write(adrr_queue,5,1,&a);
leocle 12:7d61181d7ac7 291 Write(adrr_droite,5,1,&a);
leocle 12:7d61181d7ac7 292 Write(adrr_gauche,5,1,&a);
leocle 6:b32a24905cd6 293
leocle 6:b32a24905cd6 294 /* On coupe de le couple du moteur pour pouvoir le faire tourner à la main
leocle 6:b32a24905cd6 295 a=0;
leocle 6:b32a24905cd6 296 Write(2,24,1,&a);
leocle 6:b32a24905cd6 297 Write(1,24,1,&a);
leocle 6:b32a24905cd6 298 Write(3,24,1,&a); */
leocle 6:b32a24905cd6 299
leocle 6:b32a24905cd6 300 myserial_ax12.attach(&Rx_Irq_fonction, Serial::RxIrq);
leocle 6:b32a24905cd6 301 ticker_1ms.attach(&fonction_1ms,0.001);
leocle 6:b32a24905cd6 302 etat=2;
leocle 6:b32a24905cd6 303
jlesoudeer 8:922178e5d286 304 coach.printf("fin_initialisation");
jlesoudeer 8:922178e5d286 305 position_1 = 0;
jlesoudeer 8:922178e5d286 306 position_2 = 0;
jlesoudeer 8:922178e5d286 307 position_3 = 0;
jlesoudeer 8:922178e5d286 308 position_1_old = 0;
jlesoudeer 8:922178e5d286 309 position_2_old = 0;
jlesoudeer 8:922178e5d286 310 position_3_old = 0;
jlesoudeer 8:922178e5d286 311 delta_position_1 = 0;
jlesoudeer 8:922178e5d286 312 delta_position_2 = 0;
jlesoudeer 8:922178e5d286 313 delta_position_3 = 0;
leocle 16:1bbf7e4249d7 314
leocle 1:c5e9c436f8d0 315 while (true) {
leocle 6:b32a24905cd6 316 //on regarde la position des moteurs
leocle 6:b32a24905cd6 317 switch(etat) {
leocle 6:b32a24905cd6 318 case 2 :
leocle 6:b32a24905cd6 319 if(fm_etat) {
leocle 6:b32a24905cd6 320 position_1_old = position_1;
leocle 6:b32a24905cd6 321 for(int i=0; i<=7; i++) {
leocle 6:b32a24905cd6 322 //On envoi la trame que l'on souhaite
leocle 6:b32a24905cd6 323
leocle 6:b32a24905cd6 324 myserial_ax12.putc(trame_demande_pos_1[i]);
leocle 6:b32a24905cd6 325 //hypothese marche avant et il ne fait pas plus d'un tour entre deux mesures
leocle 6:b32a24905cd6 326 }
leocle 6:b32a24905cd6 327 fm_etat=0;
leocle 6:b32a24905cd6 328 }
leocle 6:b32a24905cd6 329
leocle 6:b32a24905cd6 330 if(temps_ms>=3) {
leocle 6:b32a24905cd6 331 etat=3;
leocle 6:b32a24905cd6 332 fm_etat=1;
leocle 6:b32a24905cd6 333 }
leocle 6:b32a24905cd6 334 break;
leocle 6:b32a24905cd6 335
leocle 6:b32a24905cd6 336 case 3 :
leocle 6:b32a24905cd6 337
leocle 6:b32a24905cd6 338 if(fm_etat) {
leocle 6:b32a24905cd6 339 delta_position_1 = (int16_t)position_1 - (int16_t)position_1_old;
leocle 6:b32a24905cd6 340 if(delta_position_1 > 2048) {
leocle 6:b32a24905cd6 341 delta_position_1 = delta_position_1 - 4096;
leocle 6:b32a24905cd6 342 }
leocle 6:b32a24905cd6 343
leocle 6:b32a24905cd6 344 if(delta_position_1 < -2048) {
leocle 6:b32a24905cd6 345 delta_position_1 = delta_position_1 + 4096;
leocle 6:b32a24905cd6 346 }
leocle 6:b32a24905cd6 347 position_2_old = position_2;
leocle 6:b32a24905cd6 348 for(int i=0; i<=7; i++) {
leocle 6:b32a24905cd6 349 //On envoi la trame que l'on souhaite
leocle 6:b32a24905cd6 350 myserial_ax12.putc(trame_demande_pos_2[i]);
leocle 6:b32a24905cd6 351 //hypothese marche avant et il ne fait pas plus d'un tour entre deux mesures
leocle 6:b32a24905cd6 352
leocle 6:b32a24905cd6 353 }
leocle 6:b32a24905cd6 354 fm_etat=0;
leocle 6:b32a24905cd6 355 }
leocle 6:b32a24905cd6 356
leocle 6:b32a24905cd6 357 if(temps_ms>=6) {
leocle 6:b32a24905cd6 358 etat=4;
leocle 6:b32a24905cd6 359 fm_etat=1;
leocle 6:b32a24905cd6 360 }
leocle 6:b32a24905cd6 361 break;
leocle 6:b32a24905cd6 362
leocle 6:b32a24905cd6 363 case 4 :
leocle 6:b32a24905cd6 364 if(fm_etat) {
leocle 6:b32a24905cd6 365 delta_position_2 = (int16_t)position_2 - (int16_t)position_2_old;
leocle 6:b32a24905cd6 366 if(delta_position_2 > 2048) {
leocle 6:b32a24905cd6 367 delta_position_2 = delta_position_2 - 4096;
leocle 6:b32a24905cd6 368 }
leocle 6:b32a24905cd6 369
leocle 6:b32a24905cd6 370 if(delta_position_2 < -2048) {
leocle 6:b32a24905cd6 371 delta_position_2 = delta_position_2 + 4096;
leocle 6:b32a24905cd6 372 }
leocle 6:b32a24905cd6 373 position_3_old = position_3;
leocle 6:b32a24905cd6 374 for(int i=0; i<=7; i++) {
leocle 6:b32a24905cd6 375 //On envoi la trame que l'on souhaite
leocle 6:b32a24905cd6 376 myserial_ax12.putc(trame_demande_pos_3[i]);
leocle 6:b32a24905cd6 377
leocle 6:b32a24905cd6 378 }
leocle 6:b32a24905cd6 379 fm_etat=0;
leocle 6:b32a24905cd6 380 }
leocle 6:b32a24905cd6 381
leocle 6:b32a24905cd6 382 if(temps_ms>=9) {
leocle 6:b32a24905cd6 383 etat=5;
leocle 6:b32a24905cd6 384 fm_etat=1;
leocle 6:b32a24905cd6 385 }
leocle 6:b32a24905cd6 386 break;
leocle 6:b32a24905cd6 387
leocle 6:b32a24905cd6 388 case 5 :
jlesoudeer 8:922178e5d286 389 if(fm_etat == 1) {
jlesoudeer 8:922178e5d286 390 delta_position_3 = (int16_t)position_3 - (int16_t)position_3_old;
jlesoudeer 8:922178e5d286 391 if(delta_position_3 > 2048) {
jlesoudeer 8:922178e5d286 392 delta_position_3 = delta_position_3 - 4096;
jlesoudeer 8:922178e5d286 393 }
leocle 6:b32a24905cd6 394
jlesoudeer 8:922178e5d286 395 if(delta_position_3 < -2048) {
jlesoudeer 8:922178e5d286 396 delta_position_3 = delta_position_3 + 4096;
jlesoudeer 8:922178e5d286 397 }
jlesoudeer 8:922178e5d286 398 send_roues();
leocle 6:b32a24905cd6 399 }
leocle 6:b32a24905cd6 400
jlesoudeer 8:922178e5d286 401 fm_etat =0;
jlesoudeer 8:922178e5d286 402
jlesoudeer 8:922178e5d286 403 if (flag_a_traite == 1) {
jlesoudeer 8:922178e5d286 404 etat = 6;
jlesoudeer 8:922178e5d286 405 temps_ms = 0;
jlesoudeer 8:922178e5d286 406 fm_etat =1;
leocle 6:b32a24905cd6 407 }
jlesoudeer 8:922178e5d286 408 break;
jlesoudeer 8:922178e5d286 409
jlesoudeer 8:922178e5d286 410 case 6 :
jlesoudeer 8:922178e5d286 411 if(fm_etat ==1){
jlesoudeer 8:922178e5d286 412 coach.puts("traitement\n");
jlesoudeer 8:922178e5d286 413 if (flag_msg == 0) {
jlesoudeer 8:922178e5d286 414 sscanf(message_1,"%f,%f,%f,%f,%f", &front, &lat, &rot, &T_front, &T_lat );
jlesoudeer 8:922178e5d286 415 } else {
jlesoudeer 8:922178e5d286 416 sscanf(message_0,"%f,%f,%f,%f,%f", &front, &lat, &rot, &T_front, &T_lat );
jlesoudeer 8:922178e5d286 417 }
jlesoudeer 8:922178e5d286 418 flag_a_traite = 0;
leocle 6:b32a24905cd6 419 temps_ms = 0;
jlesoudeer 8:922178e5d286 420 asser(front, lat, rot);
jlesoudeer 8:922178e5d286 421 fm_etat = 0;
jlesoudeer 8:922178e5d286 422 }
jlesoudeer 8:922178e5d286 423 if(temps_ms>= 5) {
jlesoudeer 8:922178e5d286 424
jlesoudeer 8:922178e5d286 425 etat = 2;
jlesoudeer 8:922178e5d286 426 fm_etat =1;
jlesoudeer 8:922178e5d286 427 temps_ms = 0;
jlesoudeer 8:922178e5d286 428 }
leocle 6:b32a24905cd6 429 break;
leocle 6:b32a24905cd6 430 }
leocle 1:c5e9c436f8d0 431 }
leocle 0:8762773cddf9 432
leocle 2:223d6363315b 433 }
leocle 6:b32a24905cd6 434
leocle 6:b32a24905cd6 435 void SetCRSpeed(int ID, float speed)
leocle 6:b32a24905cd6 436 {
leocle 6:b32a24905cd6 437
leocle 6:b32a24905cd6 438 // bit 10 = direction, 0 = CCW, 1=CW
leocle 6:b32a24905cd6 439 // bits 9-0 = Speed
leocle 6:b32a24905cd6 440 char data[2];
leocle 6:b32a24905cd6 441
leocle 6:b32a24905cd6 442 int goal = (0x3ff * abs(speed));
leocle 6:b32a24905cd6 443
leocle 6:b32a24905cd6 444 // Set direction CW if we have a negative speed
leocle 6:b32a24905cd6 445 if (speed < 0) {
leocle 6:b32a24905cd6 446 goal |= (0x1 << 10);
leocle 6:b32a24905cd6 447 }
leocle 6:b32a24905cd6 448
leocle 6:b32a24905cd6 449 data[0] = goal & 0xff; // bottom 8 bits
leocle 6:b32a24905cd6 450 data[1] = goal >> 8; // top 8 bits
leocle 6:b32a24905cd6 451
leocle 6:b32a24905cd6 452 // write the packet, return the error code
leocle 6:b32a24905cd6 453 Write(ID, 0x20, 2, data);
leocle 6:b32a24905cd6 454 }
leocle 6:b32a24905cd6 455
leocle 6:b32a24905cd6 456 void SetMode(int ID, int mode)
leocle 6:b32a24905cd6 457 {
leocle 6:b32a24905cd6 458
leocle 6:b32a24905cd6 459 if (mode == 1) { // set CR
leocle 6:b32a24905cd6 460 SetCWLimit(ID,0);
leocle 6:b32a24905cd6 461 SetCCWLimit(ID,0);
leocle 6:b32a24905cd6 462 SetCRSpeed(ID,0.0);
leocle 6:b32a24905cd6 463 } else {
leocle 6:b32a24905cd6 464 SetCWLimit(ID,0);
leocle 6:b32a24905cd6 465 SetCCWLimit(ID,360);
leocle 6:b32a24905cd6 466 SetCRSpeed(ID,0.0);
leocle 6:b32a24905cd6 467 }
leocle 6:b32a24905cd6 468 }
leocle 6:b32a24905cd6 469
leocle 6:b32a24905cd6 470 void SetCWLimit (int ID, int degrees)
leocle 6:b32a24905cd6 471 {
leocle 6:b32a24905cd6 472
leocle 6:b32a24905cd6 473 char data[2];
leocle 6:b32a24905cd6 474 short limit = (4095 * degrees) / 360;
leocle 6:b32a24905cd6 475
leocle 6:b32a24905cd6 476 data[0] = limit & 0xff; // bottom 8 bits
leocle 6:b32a24905cd6 477 data[1] = limit >> 8; // top 8 bits
leocle 6:b32a24905cd6 478
leocle 6:b32a24905cd6 479 Write(ID, 0x06, 2, data);
leocle 6:b32a24905cd6 480 }
leocle 6:b32a24905cd6 481
leocle 6:b32a24905cd6 482 void SetCCWLimit (int ID, int degrees)
leocle 6:b32a24905cd6 483 {
leocle 6:b32a24905cd6 484
leocle 6:b32a24905cd6 485 char data[2];
leocle 6:b32a24905cd6 486 short limit = (4095 * degrees) / 360;
leocle 6:b32a24905cd6 487
leocle 6:b32a24905cd6 488 data[0] = limit & 0xff; // bottom 8 bits
leocle 6:b32a24905cd6 489 data[1] = limit >> 8; // top 8 bits
leocle 6:b32a24905cd6 490
leocle 6:b32a24905cd6 491 Write(ID, 0x08, 2, data);
leocle 6:b32a24905cd6 492 }
leocle 6:b32a24905cd6 493
leocle 6:b32a24905cd6 494 void Write(int ID, int start, int bytes, char* data)
leocle 6:b32a24905cd6 495 {
leocle 6:b32a24905cd6 496 char TxBuf[16];
leocle 6:b32a24905cd6 497 char sum = 0;
leocle 6:b32a24905cd6 498
leocle 6:b32a24905cd6 499 TxBuf[0] = 0xff;
leocle 6:b32a24905cd6 500 TxBuf[1] = 0xff;
leocle 6:b32a24905cd6 501
leocle 6:b32a24905cd6 502 // ID
leocle 6:b32a24905cd6 503 TxBuf[2] = ID;
leocle 6:b32a24905cd6 504 sum += TxBuf[2];
leocle 6:b32a24905cd6 505
leocle 6:b32a24905cd6 506
leocle 6:b32a24905cd6 507 // packet Length
leocle 6:b32a24905cd6 508 TxBuf[3] = 3+bytes;
leocle 6:b32a24905cd6 509 sum += TxBuf[3];
leocle 6:b32a24905cd6 510
leocle 6:b32a24905cd6 511 // Instruction
leocle 6:b32a24905cd6 512 TxBuf[4]=0x03;
leocle 6:b32a24905cd6 513 sum += TxBuf[4];
leocle 6:b32a24905cd6 514
leocle 6:b32a24905cd6 515 // Start Address
leocle 6:b32a24905cd6 516 TxBuf[5] = start;
leocle 6:b32a24905cd6 517 sum += TxBuf[5];
leocle 6:b32a24905cd6 518
leocle 6:b32a24905cd6 519 // data
leocle 6:b32a24905cd6 520 for (char i=0; i<bytes ; i++) {
leocle 6:b32a24905cd6 521 TxBuf[6+i] = data[i];
leocle 6:b32a24905cd6 522 sum += TxBuf[6+i];
leocle 6:b32a24905cd6 523
leocle 6:b32a24905cd6 524 }
leocle 6:b32a24905cd6 525
leocle 6:b32a24905cd6 526 // checksum
leocle 6:b32a24905cd6 527 TxBuf[6+bytes] = 0xFF - sum;
leocle 6:b32a24905cd6 528
leocle 6:b32a24905cd6 529 // Transmit the packet in one burst with no pausing
leocle 6:b32a24905cd6 530 for (int i = 0; i < (7 + bytes) ; i++) {
leocle 6:b32a24905cd6 531 myserial_ax12.putc(TxBuf[i]);
leocle 6:b32a24905cd6 532 }
leocle 6:b32a24905cd6 533
leocle 6:b32a24905cd6 534 wait(0.00002);
leocle 6:b32a24905cd6 535
leocle 6:b32a24905cd6 536 }