Leopold Clement / Mbed 2 deprecated PSL-VicTeam

faut que Val répare le truc

Dependencies:   mbed AX12

main.cpp@6:b32a24905cd6, 2020-06-05 (annotated)

Committer:
leocle
Date:
Fri Jun 05 08:47:26 2020 +0000
Revision:
6:b32a24905cd6
Parent:
5:86f9ec683ad5
Child:
7:49ef78d60837
Child:
8:922178e5d286
Rajout du retour de position des roues

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