Sébastien Posada Ramirez
/
Herkulex_Library_2021_phare
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
fonctions_herkulex.cpp
00001 #include "mbed.h" 00002 #include "fonctions_herkulex.h" 00003 //#include "ident_crac.h" 00004 00005 Timer Time_out; 00006 00007 //-------------------------Définition des ports série--------------------------- 00008 //RawSerial pc(USBTX, USBRX,115200); 00009 00010 RawSerial serial1(PB_6,PB_7,115200); // P : 41 et 42 00011 RawSerial serial2(PA_2,PA_3,115200); // P : 43 et 47 00012 RawSerial serial3(PA_2,PA_3,115200); // P : 44 et 48 00013 RawSerial serial4(PA_2,PA_3,115200); // P : 45 00014 RawSerial serial5(PA_2,PA_3,115200); // P : 46 00015 00016 00017 //----------------------------variables de reception---------------------------- 00018 uint8_t rx[300]; 00019 uint8_t rx2[256]; 00020 unsigned char size_reponse=100; 00021 unsigned char recevoir = 0; 00022 unsigned char i2 = 0; 00023 unsigned char flag_serial1_receive2 = 0; 00024 //--------------------variables et fonction de verification--------------------- 00025 #define tolerance_en_position 16 //1 degre=(1002-21)/320=3.066position 00026 #define tolerance_en_position_negatif -16 00027 #define B_tolerance_en_position 16 //1 degre=(1002-21)/320=3.066position 00028 #define B_tolerance_en_position_negatif -16 00029 #define V_b 45 //temps d'attente de bras 00030 #define V_m 45 //temps d'attente de bras 00031 #define V_h 45 //temps d'attente de bras 00032 #define TEMPO_R 16 //temps d'attente de reception 00033 #define PWM_recl 0.6 00034 #define temps_servo_G 65 //temps d'attente des servos sauf 2 bras 00035 #define new_tempoX 45 //temps d'attente de correction double 00036 00037 00038 int16_t pos_position = 0, get_pos = 0, pos_ID = 0; 00039 uint8_t pos_led = 0, Status = 0,iID = 0; 00040 uint8_t nombre_servo = 0; 00041 uint8_t pos_time = 0; 00042 uint16_t position_servo_mul[20]; 00043 uint8_t data_servo_mul[40]; 00044 uint8_t flag_correction = 0; 00045 float new_tempo=0; 00046 float tab_tempo[20]; 00047 uint16_t position_servo_mul_different[20]; 00048 uint8_t data_servo_mul_different[60]; 00049 int8_t my_Tor = 0; 00050 int8_t Tension_inter = 0; 00051 double Tension = 0; 00052 uint8_t coeffient_time = 1; 00053 uint8_t veri = 0; 00054 typedef enum {pos,vitesse,pos_mul_complex,pos_mul_complex_different} type_etat ; 00055 unsigned char serial_numero = 0; 00056 static type_etat etat=pos; 00057 int nb_erreur_pas_de_couple = 0; 00058 int nb_erreur_pos = 0; 00059 00060 00061 00062 //---------------------fonction d'interruption de reception de serial1--------------------- 00063 /* 00064 *Ici on crée une interruption afin de rendre prioritaire la réception de données 00065 *!!! Pour utiliser les fonctions utilisant les interruptions, il faut 'activer' ces dernières 00066 *!!!avec la fonction servo_interrupt_en(); 00067 */ 00068 unsigned char flag_perdu_info_serial1 = 0, indicateur_serial1 = 0, Size_trame_serial1 = 0, old_valueserial1 = 0; 00069 unsigned char char_receive_pc[100]; 00070 unsigned char char_receive_serial1[100]; 00071 unsigned char valueserial1=0; 00072 unsigned char valuepc=0,flag_seconde=0,flag_pc_receive=0,flag_serial1_receive=0; 00073 int pospos; 00074 00075 // 00076 //La fonction receive_serial1() est appelée par la fonction d'interruption 00077 //elle appelle la fonction automate_serial() et range les données reçues dans un tableau 00078 // 00079 void receive_serial1() 00080 { 00081 char_receive_serial1[valueserial1]=serial1.getc(); 00082 automate_serial1(); 00083 } 00084 // 00085 //fonction d'interruption 00086 // elle se déclenche dès que des données se trouvent sur le port de réception 00087 // elle appelle alors immédiatement la fonction receive_serial1(); 00088 // 00089 void Interrupt1_en(void) 00090 { 00091 serial1.attach(&receive_serial1,Serial::RxIrq); 00092 } 00093 bool verification() 00094 { 00095 uint8_t i = 0; 00096 switch(etat) { 00097 case pos: 00098 //------------------------Status-------------------- 00099 Status = getStatus(pos_ID,serial_numero); 00100 wait_ms(3); 00101 ////pc.printf("status = %d",Status); 00102 switch(Status) { 00103 case 0: 00104 break; 00105 00106 case 2: //Exceed allowed POT limit 00107 //pc.printf("ERR-Depasse la limite de position\n"); 00108 //clean_ERR(pos_ID); 00109 //wait_ms(500); 00110 clear(pos_ID,serial_numero); 00111 //positionControl(pos_ID, 1000, 3, GLED_ON); 00112 wait_ms(3); 00113 Status = getStatus(pos_ID,serial_numero); 00114 wait_ms(3); 00115 ////pc.printf("status = %d",Status); 00116 break; 00117 } 00118 //------------------Torque et position------------------------------ 00119 my_Tor = Get_Torque(pos_ID,serial_numero); 00120 wait_ms(5); 00121 ////pc.printf("my_Tor = %x\n",my_Tor); 00122 while(my_Tor != 0x60) { 00123 setTorque(pos_ID,TORQUE_ON,serial_numero); 00124 my_Tor = Get_Torque(pos_ID,serial_numero); 00125 wait_ms(5); 00126 } 00127 Tension_inter = Get_Tension_actuelle(pos_ID,serial_numero); 00128 Tension = Tension_inter*0.074; 00129 if(Tension <=6.60) { 00130 coeffient_time = 6; 00131 } else if(Tension <= 6.90) { 00132 coeffient_time = 4; 00133 } else if(Tension <= 7.10) { 00134 coeffient_time = 2; 00135 } else if(Tension > 7.10) { 00136 coeffient_time = 1; 00137 } 00138 get_pos = getPos(pos_ID,serial_numero); 00139 //pc.printf("P4=%d ",get_pos); 00140 if(((get_pos - pos_position)>tolerance_en_position)||((get_pos - pos_position)<tolerance_en_position_negatif)) { 00141 if((get_pos - pos_position)>tolerance_en_position) { 00142 new_tempo=(get_pos - pos_position)*0.084*coeffient_time + 1; 00143 if (new_tempo > 254) new_tempo = 254; 00144 } else if((get_pos - pos_position)<tolerance_en_position_negatif) { 00145 new_tempo=(get_pos - pos_position)*0.084*coeffient_time +1; 00146 if (new_tempo > 254) new_tempo = 254; 00147 } 00148 positionControl(pos_ID, pos_position, new_tempo, pos_led,serial_numero); 00149 //pc.printf("Correction!\n"); 00150 } 00151 break; 00152 case pos_mul_complex: 00153 //---------------------------Status--------------------------- 00154 for(i=0; i<nombre_servo; i++) 00155 { 00156 Status = getStatus(data_servo_mul[1+2*i],serial_numero); 00157 //pc.printf("status = %d",Status); 00158 switch(Status) { 00159 case 0: 00160 break; 00161 00162 case 2: //Exceed allowed POT limit 00163 ////pc.printf("ERR-Depasse la limite de position\n"); 00164 //clean_ERR(id); 00165 //wait_ms(500); 00166 clear(data_servo_mul[1+2*i],serial_numero); 00167 //positionControl(id, 1000, 3, GLED_ON); 00168 //wait_ms(3); 00169 //Status = getStatus(data_servo_mul[1+2*i]); 00170 //wait_ms(3); 00171 ////pc.printf("status = %d",Status); 00172 break; 00173 } 00174 } 00175 //----------------------Torque et position-------------------------- 00176 for(i=0; i<nombre_servo; i++) 00177 { 00178 my_Tor = Get_Torque(data_servo_mul[1+2*i],serial_numero); 00179 //Time_out.reset(); 00180 //Time_out.start(); 00181 while((my_Tor != 0x60))//&&(Time_out.read_ms()<100) ) 00182 { 00183 setTorque(data_servo_mul[1+2*i],TORQUE_ON,serial_numero); 00184 my_Tor = Get_Torque(data_servo_mul[1+2*i],serial_numero); 00185 ////pc.printf(" SET_TORQUE "); 00186 00187 Status = getStatus(data_servo_mul[1+2*i],serial_numero); 00188 clear(data_servo_mul[1+2*i],serial_numero); 00189 Status = getStatus(data_servo_mul[1+2*i],serial_numero); 00190 } 00191 //Time_out.stop(); 00192 } 00193 veri = 0; 00194 //Time_out.reset(); 00195 //Time_out.start(); 00196 while((veri < nombre_servo))//&&(Time_out.read_ms()<100)) 00197 { 00198 for(i=0; i<nombre_servo; i++) 00199 { 00200 my_Tor = Get_Torque(data_servo_mul[1+2*i],serial_numero); 00201 //Time_out.reset(); 00202 //Time_out.start(); 00203 while((my_Tor != 0x60))//&&(Time_out.read_ms()<100)) 00204 { 00205 setTorque(data_servo_mul[1+2*i],TORQUE_ON,serial_numero); 00206 my_Tor = Get_Torque(data_servo_mul[1+2*i],serial_numero); 00207 ////pc.printf(" SET_TORQUE "); 00208 00209 Status = getStatus(data_servo_mul[1+2*i],serial_numero); 00210 clear(data_servo_mul[1+2*i],serial_numero); 00211 Status = getStatus(data_servo_mul[1+2*i],serial_numero); 00212 } 00213 //Time_out.stop(); 00214 } 00215 for(i=0; i<nombre_servo; i++) 00216 { 00217 Tension_inter = Get_Tension_actuelle(data_servo_mul[1+2*i],serial_numero); 00218 Tension = Tension_inter*0.074; 00219 if(Tension <=6.60) { 00220 coeffient_time = 6; 00221 } else if(Tension <= 6.90) { 00222 coeffient_time = 4; 00223 } else if(Tension <= 7.10) { 00224 coeffient_time = 2; 00225 } else if(Tension > 7.10) { 00226 coeffient_time = 1; 00227 } 00228 get_pos = getPos(data_servo_mul[1+2*i],serial_numero); 00229 //pc.printf("PosiM=%d ",get_pos); 00230 if((get_pos - position_servo_mul[i])>tolerance_en_position) 00231 { 00232 tab_tempo[i]=(get_pos - position_servo_mul[i])*0.084*coeffient_time+1; //MinTempo pour 1 position:((320/60)*0.17)/(1000-20)=0.94ms MinPlayTime pour 1 position:0.94ms/11.2ms=0.084 00233 if (tab_tempo[i] > 254) tab_tempo[i] = 254; 00234 flag_correction = 1; 00235 } 00236 else if((get_pos - position_servo_mul[i])<tolerance_en_position_negatif) 00237 { 00238 tab_tempo[i]=(position_servo_mul[i] - get_pos)*0.084*coeffient_time+1; 00239 if (tab_tempo[i] > 254) tab_tempo[i] = 254; 00240 flag_correction = 1; 00241 } 00242 } 00243 if(flag_correction == 1) 00244 { 00245 new_tempo = 0; 00246 for(i=0; i<nombre_servo; i++) 00247 { 00248 if(tab_tempo[i]>new_tempo) 00249 { 00250 new_tempo = tab_tempo[i]; 00251 } 00252 } 00253 flag_correction = 0; 00254 positionControl_Mul_ensemble_complex(nombre_servo,new_tempo,data_servo_mul, position_servo_mul,serial_numero); 00255 //pc.printf("Correction!\n"); 00256 } 00257 veri=0; 00258 for(i=0; i<nombre_servo; i++) 00259 { 00260 get_pos = getPos(data_servo_mul[1+2*i],serial_numero); 00261 //pc.printf("PosiM=%d ",get_pos); 00262 if((get_pos - position_servo_mul[i])>tolerance_en_position) 00263 { 00264 tab_tempo[i]=(get_pos - position_servo_mul[i])*0.084*coeffient_time+1; //MinTempo pour 1 position:((320/60)*0.17)/(1000-20)=0.94ms MinPlayTime pour 1 position:0.94ms/11.2ms=0.084 00265 if (tab_tempo[i] > 254) tab_tempo[i] = 254; 00266 flag_correction = 1; 00267 } 00268 else if((get_pos - position_servo_mul[i])<tolerance_en_position_negatif) 00269 { 00270 tab_tempo[i]=(position_servo_mul[i] - get_pos)*0.084*coeffient_time+1; 00271 if (tab_tempo[i] > 254) tab_tempo[i] = 254; 00272 flag_correction = 1; 00273 } 00274 else { //if(((get_pos - position_servo_mul[i])<tolerance_en_position)&&((get_pos - position_servo_mul[i])>tolerance_en_position_negatif)) 00275 veri++; 00276 } 00277 } 00278 } 00279 //Time_out.stop(); 00280 break; 00281 case pos_mul_complex_different: 00282 //---------------------------Status--------------------------- 00283 for(i=0; i<nombre_servo; i++) { 00284 Status = getStatus(data_servo_mul_different[1+3*i],serial_numero); 00285 ////pc.printf("status = %d",Status); 00286 switch(Status) { 00287 case 0: 00288 break; 00289 00290 case 2: //Exceed allowed POT limit 00291 ////pc.printf("ERR-Depasse la limite de position\n"); 00292 //clean_ERR(id); 00293 //wait_ms(500); 00294 clear(data_servo_mul_different[1+3*i],serial_numero); 00295 //positionControl(id, 1000, 3, GLED_ON); 00296 //wait_ms(3); 00297 //Status = getStatus(data_servo_mul_different[1+2*i]); 00298 //wait_ms(3); 00299 ////pc.printf("status = %d",Status); 00300 break; 00301 } 00302 } 00303 //-------------------Torque et position----------------------------- 00304 for(i=0; i<nombre_servo; i++) { 00305 my_Tor = Get_Torque(data_servo_mul_different[1+3*i],serial_numero); 00306 while(my_Tor != 0x60) { 00307 setTorque(data_servo_mul_different[1+3*i],TORQUE_ON,serial_numero); 00308 my_Tor = Get_Torque(data_servo_mul_different[1+3*i],serial_numero); 00309 //wait_ms(5); 00310 ////pc.printf(" SET_TORQUE "); 00311 } 00312 } 00313 for(i=0; i<nombre_servo; i++) { 00314 Tension_inter = Get_Tension_actuelle(data_servo_mul_different[1+3*i],serial_numero); 00315 Tension = Tension_inter*0.074; 00316 if(Tension <=6.60) { 00317 coeffient_time = 6; 00318 } else if(Tension <= 6.90) { 00319 coeffient_time = 4; 00320 } else if(Tension <= 7.10) { 00321 coeffient_time = 2; 00322 } else if(Tension > 7.10) { 00323 coeffient_time = 1; 00324 } 00325 get_pos = getPos(data_servo_mul_different[1+3*i],serial_numero); 00326 //pc.printf("PosiM=%d ",get_pos); 00327 if((get_pos - position_servo_mul_different[i])>tolerance_en_position) { 00328 tab_tempo[i]=(get_pos - position_servo_mul_different[i])*0.084*coeffient_time+1; //MinTempo pour 1 position:((320/60)*0.17)/(1000-20)=0.94ms MinPlayTime pour 1 position:0.94ms/11.2ms=0.084 00329 if (tab_tempo[i] > 254) tab_tempo[i] = 254; 00330 data_servo_mul_different[2+3*i] = tab_tempo[i]; 00331 flag_correction = 1; 00332 } else if((get_pos - position_servo_mul_different[i])<tolerance_en_position_negatif) { 00333 tab_tempo[i]=(position_servo_mul_different[i] - get_pos)*0.084*coeffient_time+1; 00334 if (tab_tempo[i] > 254) tab_tempo[i] = 254; 00335 data_servo_mul_different[2+3*i] = tab_tempo[i]; 00336 flag_correction = 1; 00337 } 00338 } 00339 if(flag_correction == 1) { 00340 flag_correction = 0; 00341 positionControl_Mul_ensemble_different_complex(nombre_servo,data_servo_mul_different, position_servo_mul_different,serial_numero); 00342 //pc.printf("Correction!\n"); 00343 } 00344 break; 00345 } 00346 return true; 00347 } 00348 // 00349 //La fonction verification_3_bras() sert à vérifier les bras utilisés après un mouvement groupé 00350 //Elle change les numéros de sérial et des cerveaux moteurs 00351 // 00352 void verification_3_bras(uint8_t sens_avant) 00353 { 00354 uint8_t i=0, idata=0; 00355 uint8_t servos_bras_avant[6] = {RLED_ON, 1, GLED_ON, 2, BLED_ON, 3}; 00356 uint8_t servos_bras_arriere[6] = {RLED_ON, 4, GLED_ON, 5, BLED_ON, 6}; 00357 if(sens_avant==1) 00358 { 00359 for(idata=0;idata<6;idata++) 00360 { 00361 data_servo_mul[idata] = servos_bras_avant[idata]; 00362 } 00363 } 00364 else if(sens_avant==0) 00365 { 00366 for(idata=0;idata<6;idata++) 00367 { 00368 data_servo_mul[idata] = servos_bras_arriere[idata]; 00369 } 00370 } 00371 00372 for(i=0; i<3; i++) 00373 { 00374 serial_numero=i+1; 00375 verification(); 00376 } 00377 } 00378 00379 // 00380 //La fonction automate_serial1() sert à vérifier la bonne réception des données 00381 //elle est automatiquement appelée par la fonction receive_serial1() 00382 // 00383 void automate_serial1() 00384 { 00385 typedef enum {Attente,FF,Size,Data} type_etat1; 00386 static type_etat1 etat1=Attente; 00387 ///////pc.printf("coucou"); 00388 00389 //////pc.printf("%d\r\n", char_receive_serial1[valueserial1]); 00390 00391 switch (etat1) { 00392 // état Attente 00393 //on attend la réception des données 00394 //si on reçois l'octet 0xFF, il s'agit d'un début de trame 00395 //on passe à l'état suivant 00396 // 00397 case Attente: 00398 if(char_receive_serial1[0] == 0xFF) { 00399 etat1 = FF; 00400 valueserial1 = 1; 00401 } 00402 break; 00403 // état FF 00404 //on attend le second octet 0xFF pour confirmer qu'il s'agit d'une trame 00405 //si on reçoit l'octet 0xFF, il s'agit bien d'une trame Herkulex 00406 //on passe à l'état suivant 00407 //Sinon on retourne à l'état précédent 00408 // 00409 case FF: 00410 if(char_receive_serial1[1] == 0xFF) { 00411 etat1 = Size; 00412 valueserial1 = 2; 00413 } else { 00414 etat1 = Attente; 00415 valueserial1 = 0; 00416 flag_perdu_info_serial1 = 1; //flag_perdu_info_serial1 00417 } 00418 break; 00419 // état size 00420 //On vérifie si l'octet size est supérieur à la taille minimale d'une trame Herkulex, 00421 //Si oui on passe à l'état suivant 00422 //Si non on passe à l'état attente et flag_perdu_info_serial1 passe à 1 pour signaler la perte d'information 00423 case Size: 00424 if(char_receive_serial1[2] < 7) { 00425 etat1 = Attente; 00426 valueserial1 = 0; 00427 flag_perdu_info_serial1 = 1; //flag_perdu_info_serial1 00428 } else { 00429 etat1 = Data; 00430 old_valueserial1 = 2; 00431 valueserial1 = 3; 00432 } 00433 Size_trame_serial1 = char_receive_serial1[2]; 00434 break; 00435 //état data 00436 //on verifie que la taille de la trame reçue correspond à celle indiquée dans l'octet 'size' 00437 //si oui 00438 //flag_serial1_receive passe à 1 pour indiquer que la trame à bien été transmise 00439 case Data: 00440 if((valueserial1-2)==(Size_trame_serial1-3)) { 00441 flag_serial1_receive = 1; 00442 etat1 = Attente; 00443 valueserial1 = 0; 00444 } else { 00445 valueserial1++; 00446 } 00447 break; 00448 00449 default: 00450 break; 00451 } 00452 } 00453 //---------------------fonction d'interruption de reception de serial2--------------------- 00454 //même principe que la fonction d'interrutpion de serial1 00455 unsigned char flag_perdu_info_serial2 = 0, indicateur_serial2 = 0, Size_trame_serial2 = 0, old_valueserial2 = 0; 00456 unsigned char char_receive_serial2[100]; 00457 unsigned char valueserial2=0; 00458 unsigned char flag_serial2_receive=0; 00459 void receive_serial2() 00460 { 00461 char_receive_serial2[valueserial2]=serial2.getc(); 00462 automate_serial2(); 00463 } 00464 00465 void Interrupt2_en(void) 00466 { 00467 serial2.attach(&receive_serial2,Serial::RxIrq); 00468 } 00469 00470 void automate_serial2() 00471 { 00472 typedef enum {Attente,FF,Size,Data} type_etat2; 00473 static type_etat2 etat2=Attente; 00474 //////////pc.printf("coucou"); 00475 00476 //////pc.printf("%d\r\n", char_receive_serial2[valueserial2]); 00477 00478 switch (etat2) { 00479 case Attente: 00480 if(char_receive_serial2[0] == 0xFF) { 00481 etat2 = FF; 00482 valueserial2 = 1; 00483 } 00484 break; 00485 case FF: 00486 if(char_receive_serial2[1] == 0xFF) { 00487 etat2 = Size; 00488 valueserial2 = 2; 00489 } else { 00490 etat2 = Attente; 00491 valueserial2 = 0; 00492 flag_perdu_info_serial2 = 1; //flag_perdu_info_serial1 00493 } 00494 break; 00495 case Size: 00496 if(char_receive_serial2[2] < 7) { 00497 etat2 = Attente; 00498 valueserial2 = 0; 00499 flag_perdu_info_serial2 = 1; //flag_perdu_info_serial1 00500 } else { 00501 etat2 = Data; 00502 old_valueserial2 = 2; 00503 valueserial2 = 3; 00504 } 00505 Size_trame_serial2 = char_receive_serial2[2]; 00506 break; 00507 00508 case Data: 00509 if((valueserial2-2)==(Size_trame_serial2-3)) { 00510 flag_serial2_receive = 1; 00511 etat2 = Attente; 00512 valueserial2 = 0; 00513 } else { 00514 valueserial2++; 00515 } 00516 break; 00517 00518 default: 00519 break; 00520 } 00521 } 00522 //---------------------fonction d'interruption de reception de serial3--------------------- 00523 //même principe que la fonction d'interrutpion de serial1 00524 unsigned char flag_perdu_info_serial3 = 0, indicateur_serial3 = 0, Size_trame_serial3 = 0, old_valueserial3 = 0; 00525 unsigned char char_receive_serial3[100]; 00526 unsigned char valueserial3=0; 00527 unsigned char flag_serial3_receive=0; 00528 void receive_serial3() 00529 { 00530 char_receive_serial3[valueserial3]=serial3.getc(); 00531 automate_serial3(); 00532 } 00533 00534 void Interrupt3_en(void) 00535 { 00536 serial3.attach(&receive_serial3,Serial::RxIrq); 00537 } 00538 00539 void automate_serial3() 00540 { 00541 typedef enum {Attente,FF,Size,Data} type_etat3; 00542 static type_etat3 etat3=Attente; 00543 //////////pc.printf("coucou"); 00544 00545 //////pc.printf("%d\r\n", char_receive_serial3[valueserial3]); 00546 00547 switch (etat3) { 00548 case Attente: 00549 if(char_receive_serial3[0] == 0xFF) { 00550 etat3 = FF; 00551 valueserial3 = 1; 00552 } 00553 break; 00554 case FF: 00555 if(char_receive_serial3[1] == 0xFF) { 00556 etat3 = Size; 00557 valueserial3 = 2; 00558 } else { 00559 etat3 = Attente; 00560 valueserial3 = 0; 00561 flag_perdu_info_serial3 = 1; //flag_perdu_info_serial1 00562 } 00563 break; 00564 case Size: 00565 if(char_receive_serial3[2] < 7) { 00566 etat3 = Attente; 00567 valueserial3 = 0; 00568 flag_perdu_info_serial3 = 1; //flag_perdu_info_serial1 00569 } else { 00570 etat3 = Data; 00571 old_valueserial3 = 2; 00572 valueserial3 = 3; 00573 } 00574 Size_trame_serial3 = char_receive_serial3[2]; 00575 break; 00576 00577 case Data: 00578 if((valueserial3-2)==(Size_trame_serial3-3)) { 00579 flag_serial3_receive = 1; 00580 etat3 = Attente; 00581 valueserial3 = 0; 00582 } else { 00583 valueserial3++; 00584 } 00585 break; 00586 00587 default: 00588 break; 00589 } 00590 } 00591 //---------------------fonction d'interruption de reception de serial4--------------------- 00592 //même principe que la fonction d'interrutpion de serial1 00593 unsigned char flag_perdu_info_serial4 = 0, indicateur_serial4 = 0, Size_trame_serial4 = 0, old_valueserial4 = 0; 00594 unsigned char char_receive_serial4[100]; 00595 unsigned char valueserial4=0; 00596 unsigned char flag_serial4_receive=0; 00597 void receive_serial4() 00598 { 00599 char_receive_serial4[valueserial4]=serial4.getc(); 00600 automate_serial4(); 00601 } 00602 00603 void Interrupt4_en(void) 00604 { 00605 serial4.attach(&receive_serial4,Serial::RxIrq); 00606 } 00607 00608 void automate_serial4() 00609 { 00610 typedef enum {Attente,FF,Size,Data} type_etat4; 00611 static type_etat4 etat4=Attente; 00612 //////////pc.printf("coucou"); 00613 00614 //////pc.printf("%d\r\n", char_receive_serial4[valueserial4]); 00615 00616 switch (etat4) { 00617 case Attente: 00618 if(char_receive_serial4[0] == 0xFF) { 00619 etat4 = FF; 00620 valueserial4 = 1; 00621 } 00622 break; 00623 case FF: 00624 if(char_receive_serial4[1] == 0xFF) { 00625 etat4 = Size; 00626 valueserial4 = 2; 00627 } else { 00628 etat4 = Attente; 00629 valueserial4 = 0; 00630 flag_perdu_info_serial4 = 1; //flag_perdu_info_serial1 00631 } 00632 break; 00633 case Size: 00634 if(char_receive_serial4[2] < 7) { 00635 etat4 = Attente; 00636 valueserial4 = 0; 00637 flag_perdu_info_serial4 = 1; //flag_perdu_info_serial1 00638 } else { 00639 etat4 = Data; 00640 old_valueserial4 = 2; 00641 valueserial4 = 3; 00642 } 00643 Size_trame_serial4 = char_receive_serial4[2]; 00644 break; 00645 00646 case Data: 00647 if((valueserial4-2)==(Size_trame_serial4-3)) { 00648 flag_serial4_receive = 1; 00649 etat4 = Attente; 00650 valueserial4 = 0; 00651 } else { 00652 valueserial4++; 00653 } 00654 break; 00655 00656 default: 00657 break; 00658 } 00659 } 00660 //---------------------fonction d'interruption de reception de serial5--------------------- 00661 //même principe que la fonction d'interrutpion de serial1 00662 unsigned char flag_perdu_info_serial5 = 0, indicateur_serial5 = 0, Size_trame_serial5 = 0, old_valueserial5 = 0; 00663 unsigned char char_receive_serial5[100]; 00664 unsigned char valueserial5=0; 00665 unsigned char flag_serial5_receive=0; 00666 void receive_serial5() 00667 { 00668 char_receive_serial5[valueserial5]=serial5.getc(); 00669 automate_serial5(); 00670 } 00671 00672 void Interrupt5_en(void) 00673 { 00674 serial5.attach(&receive_serial5,Serial::RxIrq); 00675 } 00676 00677 void automate_serial5() 00678 { 00679 typedef enum {Attente,FF,Size,Data} type_etat5; 00680 static type_etat5 etat5=Attente; 00681 //////////pc.printf("coucou"); 00682 00683 //////pc.printf("%d\r\n", char_receive_serial5[valueserial5]); 00684 00685 switch (etat5) { 00686 case Attente: 00687 if(char_receive_serial5[0] == 0xFF) { 00688 etat5 = FF; 00689 valueserial5 = 1; 00690 } 00691 break; 00692 case FF: 00693 if(char_receive_serial5[1] == 0xFF) { 00694 etat5 = Size; 00695 valueserial5 = 2; 00696 } else { 00697 etat5 = Attente; 00698 valueserial5 = 0; 00699 flag_perdu_info_serial5 = 1; //flag_perdu_info_serial1 00700 } 00701 break; 00702 case Size: 00703 if(char_receive_serial5[2] < 7) { 00704 etat5 = Attente; 00705 valueserial5 = 0; 00706 flag_perdu_info_serial5 = 1; //flag_perdu_info_serial1 00707 } else { 00708 etat5 = Data; 00709 old_valueserial5 = 2; 00710 valueserial5 = 3; 00711 } 00712 Size_trame_serial5 = char_receive_serial5[2]; 00713 break; 00714 00715 case Data: 00716 if((valueserial5-2)==(Size_trame_serial5-3)) { 00717 flag_serial5_receive = 1; 00718 etat5 = Attente; 00719 valueserial5 = 0; 00720 } else { 00721 valueserial5++; 00722 } 00723 break; 00724 00725 default: 00726 break; 00727 } 00728 } 00729 //----------------xxxxx----fonction de fermture de serial----------------------- 00730 /*void N_Herkulex() 00731 { 00732 00733 if(Sv != NULL) 00734 delete Sv; 00735 if(recevoir==2) { 00736 size_reponse = rx2[recevoir]; 00737 } 00738 }*/ 00739 //-------------------------fonction de transmission----------------------------- 00740 // 00741 //Permet de transmettre une trame manuellement sur une liaison série choisie 00742 // 00743 //packetSize ==> Taille totale de la trame en octets 00744 // en-têtes (HEADER) et données (data) inclus 00745 // 00746 //data ==> Données ( Ici il d'agit de la trame en entier) à rentrer sous forme de tableau (1 octet par case!) 00747 // 00748 //numero_serial ==> Numéro de la liaison série sur laquelle on 00749 // envoie la trame 00750 void txPacket(uint8_t packetSize, uint8_t* data, uint8_t numero_serial) 00751 00752 00753 /*#ifdef HERKULEX_DEBUG 00754 pc->printf("[TX]"); 00755 for(uint8_t i = 0; i < packetSize ; i++) 00756 { 00757 #ifdef HERKULEX_DEBUG 00758 pc->printf("%02X ",data[i]); 00759 #endif 00760 txd->putc(data[i]); 00761 } 00762 #ifdef HERKULEX_DEBUG 00763 pc->printf("\n"); 00764 #endif*/ 00765 { 00766 serial_numero = numero_serial; 00767 if(numero_serial == 1) { //Envoi sur la liaison série 1 00768 for(uint8_t i = 0; i < packetSize ; i++) { // 00769 while(serial1.writeable() == 0); //On envoie 1 octet toute les 100 us 00770 serial1.putc(data[i]); // 00771 wait_us(100); // 00772 } 00773 } else if(numero_serial == 2) { //Envoi sur la liaison série 2 00774 for(uint8_t i = 0; i < packetSize ; i++) { 00775 while(serial2.writeable() == 0); 00776 serial2.putc(data[i]); 00777 wait_us(100); 00778 } 00779 } else if(numero_serial == 3) { //Envoi sur la liaison série 3 00780 for(uint8_t i = 0; i < packetSize ; i++) { 00781 while(serial3.writeable() == 0); 00782 serial3.putc(data[i]); 00783 wait_us(100); 00784 } 00785 } else if(numero_serial == 4) { //Envoi sur la liaison série 4 00786 for(uint8_t i = 0; i < packetSize ; i++) { 00787 while(serial4.writeable() == 0); 00788 serial4.putc(data[i]); 00789 wait_us(100); 00790 } 00791 } else if(numero_serial == 5) { //Envoi sur la liaison série 5 00792 for(uint8_t i = 0; i < packetSize ; i++) { 00793 while(serial5.writeable() == 0); 00794 serial5.putc(data[i]); 00795 wait_us(100); 00796 } 00797 } 00798 wait_ms(TEMPO_R); 00799 } 00800 //----------------------------fonction de reception----------------------------- 00801 //Permet de recevoir une trame 00802 void rxPacket(uint8_t packetSize, uint8_t* data, uint8_t numero_serial) 00803 // 00804 //packetSize ==> taille de la trame à recevoir 00805 //data ==> Données 00806 // 00807 { 00808 00809 /*#ifdef HERKULEX_DEBUG 00810 pc->printf("[RX]"); 00811 #endif 00812 for (uint8_t i=0; i < packetSize; i++) 00813 { 00814 data[i] = rxd->getc(); 00815 #ifdef HERKULEX_DEBUG 00816 pc->printf("%02X ",data[i]); 00817 #endif 00818 } 00819 #ifdef HERKULEX_DEBUG 00820 pc->printf("\n"); 00821 #endif*/ 00822 serial_numero = numero_serial; 00823 if(numero_serial == 1) { 00824 if(flag_serial1_receive) { 00825 for(unsigned char i4=0; i4<Size_trame_serial1; i4++) { 00826 data[i4] = char_receive_serial1[i4]; 00827 //////pc.printf("%d ",(int)char_receive_serial1[i4]); 00828 } 00829 flag_serial1_receive=0; 00830 valueserial1=0; 00831 } 00832 } else if(numero_serial == 2) { 00833 if(flag_serial2_receive) { 00834 for(unsigned char i4=0; i4<Size_trame_serial2; i4++) { 00835 data[i4] = char_receive_serial2[i4]; 00836 //////pc.printf("%d ",(int)char_receive_serial2[i4]); 00837 } 00838 flag_serial2_receive=0; 00839 valueserial2=0; 00840 } 00841 } else if(numero_serial == 3) { 00842 if(flag_serial3_receive) { 00843 for(unsigned char i4=0; i4<Size_trame_serial3; i4++) { 00844 data[i4] = char_receive_serial3[i4]; 00845 //////pc.printf("%d ",(int)char_receive_serial3[i4]); 00846 } 00847 flag_serial3_receive=0; 00848 valueserial3=0; 00849 } 00850 } else if(numero_serial == 4) { 00851 if(flag_serial4_receive) { 00852 for(unsigned char i4=0; i4<Size_trame_serial4; i4++) { 00853 data[i4] = char_receive_serial4[i4]; 00854 //////pc.printf("%d ",(int)char_receive_serial4[i4]); 00855 } 00856 flag_serial4_receive=0; 00857 valueserial4=0; 00858 } 00859 } else if(numero_serial == 5) { 00860 if(flag_serial5_receive) { 00861 for(unsigned char i4=0; i4<Size_trame_serial5; i4++) { 00862 data[i4] = char_receive_serial5[i4]; 00863 //////pc.printf("%d ",(int)char_receive_serial5[i4]); 00864 } 00865 flag_serial5_receive=0; 00866 valueserial5=0; 00867 } 00868 } 00869 } 00870 //----------------------fonction pour sortir de l'état d'erreur------------------------- 00871 // 00872 //Permet de "sortir" de la mise en erreur d'un servomoteur 00873 // 00874 void clear(uint8_t id, uint8_t numero_serial) 00875 // 00876 // id ==> On entre l'ID du servomoteur que l'on souhaite sortir de l'état d'erreur 00877 // numero serial ==> On entre le numéro de la liaison série sur laquelle se trouve le servomoteur concerné 00878 { 00879 uint8_t txBuf[11]; 00880 serial_numero = numero_serial; 00881 txBuf[0] = HEADER; // Packet Header (0xFF) 00882 txBuf[1] = HEADER; // Packet Header (0xFF) 00883 txBuf[2] = MIN_PACKET_SIZE + 4; // Packet Size 00884 txBuf[3] = id; // Servo ID 00885 txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03) *On choisi le CMD pour écrire dans un registre 00886 txBuf[5] = 0; // Checksum1 00887 txBuf[6] = 0; // Checksum2 00888 txBuf[7] = RAM_STATUS_ERROR; // Address *On écrit dans le registre RAM_STATUS_ERROR 00889 txBuf[8] = BYTE2; // Length * 00890 txBuf[9] = 0; // Clear RAM_STATUS_ERROR 00891 txBuf[10]= 0; // Clear RAM_STATUS_DETAIL 00892 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE 00893 // Checksum2 = (~Checksum1)&0xFE 00894 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]) & 0xFE; //calcul de checksum1 00895 txBuf[6] = (~txBuf[5])&0xFE; //calcul de checksum2 00896 // send packet (mbed -> herkulex) 00897 if(numero_serial == 1) { 00898 for(uint8_t i = 0; i < 11 ; i++) { 00899 while(serial1.writeable() == 0); 00900 serial1.putc(txBuf[i]); 00901 wait_us(100); 00902 } 00903 } else if(numero_serial == 2) { 00904 for(uint8_t i = 0; i < 11 ; i++) { 00905 while(serial2.writeable() == 0); 00906 serial2.putc(txBuf[i]); 00907 wait_us(100); 00908 } 00909 } else if(numero_serial == 3) { 00910 for(uint8_t i = 0; i < 11 ; i++) { 00911 while(serial3.writeable() == 0); 00912 serial3.putc(txBuf[i]); 00913 wait_us(100); 00914 } 00915 } else if(numero_serial == 4) { 00916 for(uint8_t i = 0; i < 11 ; i++) { 00917 while(serial4.writeable() == 0); 00918 serial4.putc(txBuf[i]); 00919 wait_us(100); 00920 } 00921 } else if(numero_serial == 5) { 00922 for(uint8_t i = 0; i < 11 ; i++) { 00923 while(serial5.writeable() == 0); 00924 serial5.putc(txBuf[i]); 00925 wait_us(100); 00926 } 00927 } 00928 wait_ms(TEMPO_R); 00929 } 00930 //----------------fonction de mise à jour du couple du servo--------------------- 00931 void setTorque(uint8_t id, uint8_t cmdTorque, uint8_t numero_serial) 00932 // Permet de modifier l'état du couple d'un servo------------ 00933 // id ==> ID du servomoteur 00934 //cmdTorque ==> état souhaité pour le couple 00935 //numero_serial ==> Numéro de la liaison série sur laquelle se trouve le servo 00936 00937 // valeurs posssibles pour cmdTorque 00938 // 0x40 Break On Opérations commandes impossibles 00939 // 0x60 Torque On Fontionnement normal 00940 // 0x00 Torque Free Opérations commandes impossibles + possibilité de déplacer le servo manuellement 00941 { 00942 uint8_t txBuf[10]; 00943 serial_numero = numero_serial; 00944 txBuf[0] = HEADER; // Packet Header (0xFF) 00945 txBuf[1] = HEADER; // Packet Header (0xFF) 00946 txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size 00947 txBuf[3] = id; // Servo ID 00948 txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03) 00949 txBuf[5] = 0; // Checksum1 00950 txBuf[6] = 0; // Checksum2 00951 txBuf[7] = RAM_TORQUE_CONTROL; // Address 00952 txBuf[8] = BYTE1; // Length 00953 txBuf[9] = cmdTorque; // Torque ON 00954 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE 00955 // Checksum2 = (~Checksum1)&0xFE 00956 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE; 00957 txBuf[6] = (~txBuf[5])&0xFE; 00958 // send packet (mbed -> herkulex) 00959 if(numero_serial == 1) { 00960 for(uint8_t i = 0; i < 10 ; i++) { 00961 while(serial1.writeable() == 0); 00962 serial1.putc(txBuf[i]); 00963 wait_us(100); 00964 } 00965 } else if(numero_serial == 2) { 00966 for(uint8_t i = 0; i < 10 ; i++) { 00967 while(serial2.writeable() == 0); 00968 serial2.putc(txBuf[i]); 00969 wait_us(100); 00970 } 00971 } else if(numero_serial == 3) { 00972 for(uint8_t i = 0; i < 10 ; i++) { 00973 while(serial3.writeable() == 0); 00974 serial3.putc(txBuf[i]); 00975 wait_us(100); 00976 } 00977 } else if(numero_serial == 4) { 00978 for(uint8_t i = 0; i < 10 ; i++) { 00979 while(serial4.writeable() == 0); 00980 serial4.putc(txBuf[i]); 00981 wait_us(100); 00982 } 00983 } else if(numero_serial == 5) { 00984 for(uint8_t i = 0; i < 10 ; i++) { 00985 while(serial5.writeable() == 0); 00986 serial5.putc(txBuf[i]); 00987 wait_us(100); 00988 } 00989 } 00990 wait_ms(TEMPO_R); 00991 } 00992 //-------------fonction de contrôle de position pour un seul servo-------------- 00993 //Permet de controler un servomoteur en position 00994 void positionControl(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED, uint8_t numero_serial) 00995 // 00996 //id ==> id du servo à déplacer 00997 //position ==> position à atteindre 00998 //playtime ==> temps à mettre pour effectuer le déplacement 00999 //setLED ==> LED à allumer 01000 //numero-serial ==> numéro de la liaison série 01001 { 01002 float tempo=0; 01003 serial_numero = numero_serial; 01004 //if (position > 1023) return; //1002-21 01005 if (playtime > 254) playtime = 254; //return; //1-254 == 11.2ms-2.844sec. 01006 tempo=playtime*0.012; 01007 pos_ID = id; 01008 uint8_t txBuf[12]; 01009 etat = pos; 01010 pos_position = position; 01011 pos_time = playtime; 01012 pos_led = setLED; 01013 txBuf[0] = HEADER; // Packet Header (0xFF) 01014 txBuf[1] = HEADER; // Packet Header (0xFF) 01015 txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size 01016 //txBuf[3] = MAX_PID; 01017 txBuf[3] = id; // pID is total number of servos in the network (0 ~ 253) 01018 txBuf[4] = CMD_S_JOG; // Command S JOG (0x06) 01019 txBuf[5] = 0; // Checksum1 01020 txBuf[6] = 0; // Checksum2 01021 txBuf[7] = playtime; // Playtime 01022 txBuf[8] = position & 0x00FF; // Position (LSB, Least Significant Bit) 01023 txBuf[9] =(position & 0xFF00) >> 8; // position (MSB, Most Significanct Bit) 01024 txBuf[10] = POS_MODE | setLED; // Pos Mode and LED on/off 01025 txBuf[11] = id; // Servo ID 01026 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE 01027 // Checksum2 = (~Checksum1)&0xFE 01028 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE; 01029 txBuf[6] = (~txBuf[5])&0xFE; 01030 // send packet (mbed -> herkulex) 01031 //txPacket(12, txBuf); 01032 if(numero_serial == 1) { 01033 for(uint8_t i = 0; i < 12 ; i++) { 01034 while(serial1.writeable() == 0); 01035 serial1.putc(txBuf[i]); 01036 //pc.printf("%d/",txBuf[i]); 01037 wait_us(100); 01038 } 01039 } else if(numero_serial == 2) { 01040 for(uint8_t i = 0; i < 12 ; i++) { 01041 while(serial2.writeable() == 0); 01042 serial2.putc(txBuf[i]); 01043 01044 wait_us(100); 01045 } 01046 } else if(numero_serial == 3) { 01047 for(uint8_t i = 0; i < 12 ; i++) { 01048 while(serial3.writeable() == 0); 01049 serial3.putc(txBuf[i]); 01050 wait_us(100); 01051 } 01052 } else if(numero_serial == 4) { 01053 for(uint8_t i = 0; i < 12 ; i++) { 01054 while(serial4.writeable() == 0); 01055 serial4.putc(txBuf[i]); 01056 wait_us(100); 01057 } 01058 } else if(numero_serial == 5) { 01059 for(uint8_t i = 0; i < 12 ; i++) { 01060 while(serial5.writeable() == 0); 01061 serial5.putc(txBuf[i]); 01062 wait_us(100); 01063 } 01064 } 01065 wait(tempo); 01066 wait_ms(TEMPO_R); 01067 } 01068 //-------------fonction de controle de vitesse pour un seul servo--------------- 01069 01070 void velocityControl(uint8_t id, int16_t speed, uint8_t setLED, uint8_t numero_serial) 01071 // 01072 //id ==> id du servo à déplacer 01073 //speed ==> vitesse (sans dec) 01074 //setLED ==> LED à allumer 01075 //numero_serial ==> numéro de la liaison série 01076 // 01077 { 01078 serial_numero = numero_serial; 01079 if (speed > 1023 || speed < -1023) return; 01080 uint8_t txBuf[12]; 01081 txBuf[0] = HEADER; // Packet Header (0xFF) 01082 txBuf[1] = HEADER; // Packet Header (0xFF) 01083 txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size 01084 txBuf[3] = id; // pID is total number of servos in the network (0 ~ 253) 01085 txBuf[4] = CMD_S_JOG; // Command S JOG (0x06) 01086 txBuf[5] = 0; // Checksum1 01087 txBuf[6] = 0; // Checksum2 01088 txBuf[7] = 0; // Playtime, unmeaningful in turn mode 01089 if (speed >= 0) { //On gère la vitesse positive 01090 txBuf[8] = speed & 0x00FF; // Speed (LSB, Least Significant Bit) 01091 txBuf[9] =(speed & 0xFF00) >> 8; // Speed (MSB, Most Significanct Bit) 01092 } else if(speed < 0) { //On gère la vitesse négative (voir pg.48 de la documentation herkulex) 01093 speed= abs(speed); 01094 txBuf[8] = speed & 0x00FF; // Speed (LSB, Least Significant Bit) 01095 txBuf[9] =((speed|0x4000) & 0xFF00) >> 8; // Speed (MSB, Most Significanct Bit) 01096 } 01097 01098 txBuf[10] = TURN_MODE | setLED; // Turn Mode and LED on/off 01099 txBuf[11] = id; // Servo ID 01100 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE 01101 // Checksum2 = (~Checksum1)&0xFE 01102 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE; 01103 txBuf[6] = (~txBuf[5])&0xFE; 01104 // send packet (mbed -> herkulex) 01105 if(numero_serial == 1) { 01106 txPacket(12, txBuf,1); 01107 } else if(numero_serial == 2) { 01108 txPacket(12, txBuf,2); 01109 } else if(numero_serial == 3) { 01110 txPacket(12, txBuf,3); 01111 } else if(numero_serial == 4) { 01112 txPacket(12, txBuf,4); 01113 } else if(numero_serial == 5) { 01114 txPacket(12, txBuf,5); 01115 } 01116 wait_ms(TEMPO_R); 01117 } 01118 //--------------------Compteur de tour------------------------------------------------------------------------ 01119 //c'est un compteur de tour (CQFD) 01120 void compteTour(int ID,int16_t speed,uint8_t tour, uint16_t position,uint8_t setLED,uint8_t serial) 01121 // 01122 //id ==> id du servo à déplacer 01123 //speed ==> vitesse 01124 //tour ==> nombre de tour à effectuer 01125 //position ==> position finale 01126 //setLED ==> LED à allumer 01127 //numero_serial ==> numéro de la liaison série 01128 // 01129 // 01130 01131 { 01132 int etat =0; 01133 int flagTour=0; 01134 int flagDernierTour = 0; 01135 int end = 0; 01136 int posAct, posCible, pos; 01137 servo_interrupt_en(); 01138 clear(ID,serial); 01139 setTorque(ID, TORQUE_ON,serial); 01140 01141 posCible=getPos(ID,serial);//position; 01142 01143 //pc.printf("\nPos depart :%d",posCible); 01144 velocityControl(ID,speed,setLED,serial); 01145 wait_ms(100); 01146 if(tour == 0){ 01147 etat = 2; 01148 tour = 1; 01149 } 01150 01151 01152 if(speed > 0) { 01153 while(end != 1) { 01154 switch (etat) { 01155 case 0 : 01156 pos = getPos(ID,serial); 01157 posAct = pos - posCible; 01158 if(posAct<0){ 01159 //pc.printf("\n0 pos :%d",pos); 01160 clear(ID,serial); 01161 setTorque(ID, TORQUE_ON,serial); 01162 etat=1; 01163 } 01164 01165 break; 01166 case 1 : 01167 pos = getPos(ID,serial); 01168 velocityControl(ID,speed,RLED_ON,serial); 01169 posAct = posCible - pos; 01170 if (posAct < 0 ){ 01171 //pc.printf("\n1 pos :%d",pos); 01172 etat = 2; 01173 } 01174 break; 01175 01176 case 2 : 01177 01178 clear(ID,serial); 01179 setTorque(ID, TORQUE_ON,serial); 01180 01181 if (flagTour == tour-1) { 01182 if (getPos(ID,serial) > position && !flagDernierTour){ 01183 //pc.printf("\nTour sup"); 01184 // flagTour++; 01185 flagDernierTour =1; 01186 etat =0; 01187 posCible = position; 01188 }else{ 01189 //pc.printf("\ngoto pos finale"); 01190 velocityControl(ID,0,setLED,serial); 01191 positionControl(ID,position,1,setLED,serial); 01192 end = 1; 01193 } 01194 } 01195 01196 else { 01197 flagTour=flagTour+1; 01198 //pc.printf("\ntours execute : %d",flagTour); 01199 etat = 0; 01200 } 01201 break; 01202 } 01203 } 01204 } else if(speed < 0) { 01205 while(end != 1) { 01206 switch (etat) { 01207 case 0 : 01208 pos = getPos(ID,serial); 01209 posAct = pos - posCible; 01210 if(posAct>0){ 01211 //pc.printf("\n0 pos :%d",pos); 01212 clear(ID,serial); 01213 setTorque(ID, TORQUE_ON,serial); 01214 etat=1; 01215 } 01216 01217 break; 01218 01219 case 1 : 01220 pos = getPos(ID,serial); 01221 velocityControl(ID,speed,RLED_ON,serial); 01222 posAct = posCible - pos; 01223 if (posAct > 0 ){ 01224 //pc.printf("\n1 pos :%d",pos); 01225 etat = 2; 01226 } 01227 break; 01228 01229 case 2 : 01230 01231 clear(ID,serial); 01232 setTorque(ID, TORQUE_ON,serial); 01233 01234 if (flagTour == tour-1) { 01235 if (getPos(ID,serial) < position && !flagDernierTour){ 01236 //pc.printf("\nTour sup"); 01237 // flagTour++; 01238 flagDernierTour =1; 01239 etat =0; 01240 posCible = position; 01241 }else{ 01242 //pc.printf("\ngoto pos finale"); 01243 velocityControl(ID,0,setLED,serial); 01244 positionControl(ID,position,1,setLED,serial); 01245 end = 1; 01246 } 01247 } 01248 01249 else { 01250 flagTour=flagTour+1; 01251 //pc.printf("\ntours execute : %d",flagTour); 01252 etat = 0; 01253 } 01254 break; 01255 } 01256 } 01257 } 01258 } 01259 /* 01260 01261 { 01262 int etat =0; 01263 int flagTour=0; 01264 int end = 0; 01265 int posAct, posCible; 01266 servo_interrupt_en(); 01267 clear(ID,serial); 01268 setTorque(ID, TORQUE_ON,serial); 01269 01270 posCible=position; 01271 velocityControl(ID,speed,setLED,serial); 01272 wait_ms(100); 01273 01274 if(speed > 0) { 01275 while(end != 1) { 01276 switch (etat) { 01277 case 0 : 01278 01279 posAct = getPos(ID,serial); 01280 posAct = posAct-posCible; 01281 //pc.printf("%d",posAct); 01282 if (posAct < 0) { 01283 clear(ID,serial); 01284 setTorque(ID, TORQUE_ON,serial); 01285 etat=1; 01286 } 01287 break; 01288 01289 case 1 : 01290 01291 velocityControl(ID,speed,RLED_ON,serial); 01292 posAct = getPos(ID,serial); 01293 posAct = posCible-posAct; 01294 if (posAct < 0) etat = 2; 01295 break; 01296 01297 case 2 : 01298 01299 clear(ID,serial); 01300 setTorque(ID, TORQUE_ON,serial); 01301 if (flagTour == tour-1 | tour == 0) { 01302 velocityControl(ID,0,setLED,serial); 01303 positionControl(ID,posCible,1,setLED,serial); 01304 end = 1; 01305 01306 } else { 01307 flagTour=flagTour+1; 01308 etat = 0; 01309 } 01310 break; 01311 } 01312 } 01313 } else if(speed < 0) { 01314 while(end != 1) { 01315 switch (etat) { 01316 case 0 : 01317 01318 posAct = getPos(ID,serial); 01319 posAct = posCible-posAct; 01320 //pc.printf("%d",posAct); 01321 if (posAct < 0) { 01322 clear(ID,serial); 01323 setTorque(ID, TORQUE_ON,serial); 01324 etat=1; 01325 } 01326 break; 01327 01328 case 1 : 01329 01330 velocityControl(ID,speed,RLED_ON,serial); 01331 posAct = getPos(ID,serial); 01332 posAct = posAct-posCible; 01333 if (posAct < 0) etat = 2; 01334 break; 01335 01336 case 2 : 01337 01338 clear(ID,serial); 01339 setTorque(ID, TORQUE_ON,serial); 01340 if (flagTour == tour-1 | tour == 0) { 01341 velocityControl(ID,0,setLED,serial); 01342 positionControl(ID,posCible,1,setLED,serial); 01343 end =1; 01344 } else { 01345 flagTour=flagTour+1; 01346 etat = 0; 01347 } 01348 break; 01349 } 01350 } 01351 } 01352 }*/ 01353 01354 //--------------------fonction d'acquisition d'etat d'un servo----------------------- 01355 int8_t getStatus(uint8_t id, uint8_t numero_serial) 01356 // 01357 // renvoi l'état du servomoteur (doc pg 39) 01358 // 01359 //id ==> Id du servo concerné 01360 //numero-serial ==> numéro de la liaison série du servo 01361 // 01362 { 01363 serial_numero = numero_serial; 01364 uint8_t status; 01365 uint8_t txBuf[7]; 01366 size_reponse = 9; 01367 01368 txBuf[0] = HEADER; // Packet Header (0xFF) 01369 txBuf[1] = HEADER; // Packet Header (0xFF) 01370 txBuf[2] = MIN_PACKET_SIZE; // Packet Size 01371 txBuf[3] = id; // Servo ID 01372 txBuf[4] = CMD_STAT; // Status Error, Status Detail request 01373 // Check Sum1 and Check Sum2 01374 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]) & 0xFE; 01375 txBuf[6] = (~txBuf[5])&0xFE; 01376 01377 uint8_t rxBuf[9]; 01378 01379 if(numero_serial == 1) { 01380 for(uint8_t i = 0; i < 7 ; i++) { 01381 while(serial1.writeable() == 0); 01382 serial1.putc(txBuf[i]); 01383 wait_us(100); 01384 } 01385 wait_ms(TEMPO_R); 01386 if(flag_serial1_receive) { 01387 for(unsigned char i4=0; i4<Size_trame_serial1; i4++) { 01388 rxBuf[i4] = char_receive_serial1[i4]; 01389 ////////////pc.printf("%d ",(int)char_receive_serial1[i4]); 01390 } 01391 flag_serial1_receive=0; 01392 valueserial1=0; 01393 } 01394 } else if(numero_serial == 2) { 01395 for(uint8_t i = 0; i < 7 ; i++) { 01396 while(serial2.writeable() == 0); 01397 serial2.putc(txBuf[i]); 01398 wait_us(100); 01399 } 01400 wait_ms(TEMPO_R); 01401 if(flag_serial2_receive) { 01402 for(unsigned char i4=0; i4<Size_trame_serial2; i4++) { 01403 rxBuf[i4] = char_receive_serial2[i4]; 01404 //////pc.printf("%d ",(int)char_receive_serial2[i4]); 01405 } 01406 flag_serial2_receive=0; 01407 valueserial2=0; 01408 } 01409 } else if(numero_serial == 3) { 01410 for(uint8_t i = 0; i < 7 ; i++) { 01411 while(serial3.writeable() == 0); 01412 serial3.putc(txBuf[i]); 01413 wait_us(100); 01414 } 01415 wait_ms(TEMPO_R); 01416 if(flag_serial3_receive) { 01417 for(unsigned char i4=0; i4<Size_trame_serial3; i4++) { 01418 rxBuf[i4] = char_receive_serial3[i4]; 01419 //////pc.printf("%d ",(int)char_receive_serial3[i4]); 01420 } 01421 flag_serial3_receive=0; 01422 valueserial3=0; 01423 } 01424 } else if(numero_serial == 4) { 01425 for(uint8_t i = 0; i < 7 ; i++) { 01426 while(serial4.writeable() == 0); 01427 serial4.putc(txBuf[i]); 01428 wait_us(100); 01429 } 01430 wait_ms(TEMPO_R); 01431 if(flag_serial4_receive) { 01432 for(unsigned char i4=0; i4<Size_trame_serial4; i4++) { 01433 rxBuf[i4] = char_receive_serial4[i4]; 01434 //////pc.printf("%d ",(int)char_receive_serial4[i4]); 01435 } 01436 flag_serial4_receive=0; 01437 valueserial4=0; 01438 } 01439 } else if(numero_serial == 5) { 01440 for(uint8_t i = 0; i < 7 ; i++) { 01441 while(serial5.writeable() == 0); 01442 serial5.putc(txBuf[i]); 01443 wait_us(100); 01444 } 01445 wait_ms(TEMPO_R); 01446 if(flag_serial5_receive) { 01447 for(unsigned char i4=0; i4<Size_trame_serial5; i4++) { 01448 rxBuf[i4] = char_receive_serial5[i4]; 01449 //////pc.printf("%d ",(int)char_receive_serial5[i4]); 01450 } 01451 flag_serial5_receive=0; 01452 valueserial5=0; 01453 } 01454 } 01455 01456 // Checksum1 01457 uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]) & 0xFE; 01458 if (chksum1 != rxBuf[5]) { 01459 if(numero_serial == 1) { 01460 flag_serial1_receive=0; 01461 } else if(numero_serial == 2) { 01462 flag_serial2_receive=0; 01463 } else if(numero_serial == 3) { 01464 flag_serial3_receive=0; 01465 } else if(numero_serial == 4) { 01466 flag_serial4_receive=0; 01467 } else if(numero_serial == 5) { 01468 flag_serial5_receive=0; 01469 } 01470 return -1; 01471 } 01472 // Checksum2 01473 uint8_t chksum2 = (~rxBuf[5]&0xFE); 01474 if (chksum2 != rxBuf[6]) { 01475 if(numero_serial == 1) { 01476 flag_serial1_receive=0; 01477 } else if(numero_serial == 2) { 01478 flag_serial2_receive=0; 01479 } else if(numero_serial == 3) { 01480 flag_serial3_receive=0; 01481 } else if(numero_serial == 4) { 01482 flag_serial4_receive=0; 01483 } else if(numero_serial == 5) { 01484 flag_serial5_receive=0; 01485 } 01486 return -1; 01487 } 01488 status = rxBuf[7]; // Status Error 01489 //status = rxBuf[8]; // Status Detail 01490 01491 01492 return status; 01493 } 01494 //------------------fonction pour lire la position actuelle----------------------- 01495 int16_t getPos(uint8_t id, uint8_t numero_serial) 01496 // 01497 //renvoie la position d'un servo 01498 // 01499 //!!!ne pas oublier d'utiliser servo_interrupt_en();!!! 01500 // 01501 //id ==> id d'un servomoteur 01502 //numero_serial==> numéro de la liaison série du servo 01503 // 01504 { 01505 serial_numero = numero_serial; 01506 uint16_t position = 0; 01507 uint8_t txBuf[9]; 01508 size_reponse = 13; 01509 01510 txBuf[0] = HEADER; // Packet Header (0xFF) 01511 txBuf[1] = HEADER; // Packet Header (0xFF) 01512 txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size 01513 txBuf[3] = id; // Servo ID 01514 txBuf[4] = CMD_RAM_READ; // Command Ram Read 01515 txBuf[5] = 0; // Checksum1 01516 txBuf[6] = 0; // Checksum2 01517 txBuf[7] = RAM_CALIBRATED_POSITION; // Address 01518 txBuf[8] = BYTE2; 01519 // Check Sum1 and Check Sum2 01520 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE; 01521 txBuf[6] = (~txBuf[5])&0xFE; 01522 01523 uint8_t rxBuf[13]; 01524 01525 if(numero_serial == 1) { 01526 for(uint8_t i = 0; i < 9 ; i++) { 01527 while(serial1.writeable() == 0); 01528 serial1.putc(txBuf[i]); 01529 wait_us(100); 01530 } 01531 wait_ms(TEMPO_R); 01532 if(flag_serial1_receive) { 01533 for(unsigned char i4=0; i4<Size_trame_serial1; i4++) { 01534 rxBuf[i4] = char_receive_serial1[i4]; 01535 //pc.printf("%d ",(int)char_receive_serial1[i4]); 01536 } 01537 flag_serial1_receive=0; 01538 valueserial1=0; 01539 } 01540 } else if(numero_serial == 2) { 01541 for(uint8_t i = 0; i < 9 ; i++) { 01542 while(serial2.writeable() == 0); 01543 serial2.putc(txBuf[i]); 01544 wait_us(100); 01545 } 01546 wait_ms(TEMPO_R); 01547 if(flag_serial2_receive) { 01548 for(unsigned char i4=0; i4<Size_trame_serial2; i4++) { 01549 rxBuf[i4] = char_receive_serial2[i4]; 01550 //////pc.printf("%d ",(int)char_receive_serial2[i4]); 01551 } 01552 flag_serial2_receive=0; 01553 valueserial2=0; 01554 } 01555 } else if(numero_serial == 3) { 01556 for(uint8_t i = 0; i < 9 ; i++) { 01557 while(serial3.writeable() == 0); 01558 serial3.putc(txBuf[i]); 01559 wait_us(100); 01560 } 01561 wait_ms(TEMPO_R); 01562 if(flag_serial3_receive) { 01563 for(unsigned char i4=0; i4<Size_trame_serial3; i4++) { 01564 rxBuf[i4] = char_receive_serial3[i4]; 01565 //////pc.printf("%d ",(int)char_receive_serial3[i4]); 01566 } 01567 flag_serial3_receive=0; 01568 valueserial3=0; 01569 } 01570 } else if(numero_serial == 4) { 01571 for(uint8_t i = 0; i < 9 ; i++) { 01572 while(serial4.writeable() == 0); 01573 serial4.putc(txBuf[i]); 01574 wait_us(100); 01575 } 01576 wait_ms(TEMPO_R); 01577 if(flag_serial4_receive) { 01578 for(unsigned char i4=0; i4<Size_trame_serial4; i4++) { 01579 rxBuf[i4] = char_receive_serial4[i4]; 01580 //////pc.printf("%d ",(int)char_receive_serial4[i4]); 01581 } 01582 flag_serial4_receive=0; 01583 valueserial4=0; 01584 } 01585 } else if(numero_serial == 5) { 01586 for(uint8_t i = 0; i < 9 ; i++) { 01587 while(serial5.writeable() == 0); 01588 serial5.putc(txBuf[i]); 01589 wait_us(100); 01590 } 01591 wait_ms(TEMPO_R); 01592 if(flag_serial5_receive) { 01593 for(unsigned char i4=0; i4<Size_trame_serial5; i4++) { 01594 rxBuf[i4] = char_receive_serial5[i4]; 01595 //////pc.printf("%d ",(int)char_receive_serial5[i4]); 01596 } 01597 flag_serial5_receive=0; 01598 valueserial5=0; 01599 } 01600 } 01601 01602 01603 // Checksum1 01604 uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE; 01605 if (chksum1 != rxBuf[5]) { 01606 /*#ifdef HERKULEX_DEBUG 01607 pc->printf("Checksum1 fault\n"); 01608 #endif*/ 01609 if(numero_serial == 1) { 01610 flag_serial1_receive=0; 01611 } else if(numero_serial == 2) { 01612 flag_serial2_receive=0; 01613 } else if(numero_serial == 3) { 01614 flag_serial3_receive=0; 01615 } else if(numero_serial == 4) { 01616 flag_serial4_receive=0; 01617 } else if(numero_serial == 5) { 01618 flag_serial5_receive=0; 01619 } 01620 return -1; 01621 }// Checksum2 01622 uint8_t chksum2 = (~rxBuf[5]&0xFE); 01623 if (chksum2 != rxBuf[6]) { 01624 /* #ifdef HERKULEX_DEBUG 01625 pc->printf("Checksum2 fault\n"); 01626 #endif*/ 01627 if(numero_serial == 1) { 01628 flag_serial1_receive=0; 01629 } else if(numero_serial == 2) { 01630 flag_serial2_receive=0; 01631 } else if(numero_serial == 3) { 01632 flag_serial3_receive=0; 01633 } else if(numero_serial == 4) { 01634 flag_serial4_receive=0; 01635 } else if(numero_serial == 5) { 01636 flag_serial5_receive=0; 01637 } 01638 return -1; 01639 } position = ((rxBuf[10]&0x03)<<8) | rxBuf[9]; 01640 01641 01642 //} 01643 return position; 01644 } 01645 //---------------fonction d'acquis d'etat de couple d'un servo------------------ 01646 //Obtenir la valeur du couple d'un servo 01647 int8_t Get_Torque(int8_t id, uint8_t numero_serial) 01648 // 01649 //id ==> id du servomoteur sur la liaison série 01650 //numero_serial ==> numéro de la liaison série sur laquelle se trouve le servomoteur 01651 // 01652 { 01653 serial_numero = numero_serial; 01654 uint8_t txBuf[9]; 01655 int8_t Tor = 0; 01656 01657 uint8_t iv=0; 01658 for(iv=0; iv<20; iv++) { 01659 rx2[iv] = 0; 01660 } 01661 01662 txBuf[0] = HEADER; // Packet Header (0xFF) 01663 txBuf[1] = HEADER; // Packet Header (0xFF) 01664 txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size 01665 txBuf[3] = id; // Servo ID 01666 txBuf[4] = CMD_RAM_READ; // Command Ram Read 01667 txBuf[5] = 0; // Checksum1 01668 txBuf[6] = 0; // Checksum2 01669 txBuf[7] = RAM_TORQUE_CONTROL; 01670 txBuf[8] = BYTE1; // Length 01671 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;//Checksum2 01672 txBuf[6] = (~txBuf[5])&0xFE; // CheckSum2 01673 01674 //pc.printf(" Torque "); 01675 01676 uint8_t rxBuf[12]; 01677 01678 if(numero_serial == 1) { 01679 //send packet (mbed -> herkulex) 01680 for(uint8_t i = 0; i < 9 ; i++) { 01681 while(serial1.writeable() == 0); 01682 serial1.putc(txBuf[i]); 01683 wait_us(100); 01684 } 01685 01686 //send packet (mbed -> herkulex) 01687 //uint8_t rxBuf[12]; 01688 wait_ms(TEMPO_R); 01689 if(flag_serial1_receive) { 01690 for(unsigned char i4=0; i4<Size_trame_serial1; i4++) { 01691 rxBuf[i4] = char_receive_serial1[i4]; 01692 //////pc.printf("%d ",(int)char_receive_serial1[i4]); 01693 } 01694 flag_serial1_receive=0; 01695 valueserial1=0; 01696 } 01697 } else if(numero_serial == 2) { 01698 for(uint8_t i = 0; i < 9 ; i++) { 01699 while(serial2.writeable() == 0); 01700 serial2.putc(txBuf[i]); 01701 wait_us(100); 01702 } 01703 wait_ms(TEMPO_R); 01704 if(flag_serial2_receive) { 01705 for(unsigned char i4=0; i4<Size_trame_serial2; i4++) { 01706 rxBuf[i4] = char_receive_serial2[i4]; 01707 //////pc.printf("%d ",(int)char_receive_serial2[i4]); 01708 } 01709 flag_serial2_receive=0; 01710 valueserial2=0; 01711 } 01712 } else if(numero_serial == 3) { 01713 for(uint8_t i = 0; i < 9 ; i++) { 01714 while(serial3.writeable() == 0); 01715 serial3.putc(txBuf[i]); 01716 wait_us(100); 01717 } 01718 wait_ms(TEMPO_R); 01719 if(flag_serial3_receive) { 01720 for(unsigned char i4=0; i4<Size_trame_serial3; i4++) { 01721 rxBuf[i4] = char_receive_serial3[i4]; 01722 //////pc.printf("%d ",(int)char_receive_serial3[i4]); 01723 } 01724 flag_serial3_receive=0; 01725 valueserial3=0; 01726 } 01727 } else if(numero_serial == 4) { 01728 for(uint8_t i = 0; i < 9 ; i++) { 01729 while(serial4.writeable() == 0); 01730 serial4.putc(txBuf[i]); 01731 wait_us(100); 01732 } 01733 wait_ms(TEMPO_R); 01734 if(flag_serial4_receive) { 01735 for(unsigned char i4=0; i4<Size_trame_serial4; i4++) { 01736 rxBuf[i4] = char_receive_serial4[i4]; 01737 //////pc.printf("%d ",(int)char_receive_serial4[i4]); 01738 } 01739 flag_serial4_receive=0; 01740 valueserial4=0; 01741 } 01742 } else if(numero_serial == 5) { 01743 for(uint8_t i = 0; i < 9 ; i++) { 01744 while(serial5.writeable() == 0); 01745 serial5.putc(txBuf[i]); 01746 wait_us(100); 01747 } 01748 wait_ms(TEMPO_R); 01749 if(flag_serial5_receive) { 01750 for(unsigned char i4=0; i4<Size_trame_serial5; i4++) { 01751 rxBuf[i4] = char_receive_serial5[i4]; 01752 //////pc.printf("%d ",(int)char_receive_serial5[i4]); 01753 } 01754 flag_serial5_receive=0; 01755 valueserial5=0; 01756 } 01757 } 01758 01759 uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]) & 0xFE; 01760 if (chksum1 != rxBuf[5]) { 01761 /*#ifdef HERKULEX_DEBUG 01762 pc->printf("Checksum1 fault\n"); 01763 #endif*/ 01764 if(numero_serial == 1) { 01765 flag_serial1_receive=0; 01766 } else if(numero_serial == 2) { 01767 flag_serial2_receive=0; 01768 } else if(numero_serial == 3) { 01769 flag_serial3_receive=0; 01770 } else if(numero_serial == 4) { 01771 flag_serial4_receive=0; 01772 } else if(numero_serial == 5) { 01773 flag_serial5_receive=0; 01774 } 01775 return -1; 01776 }// Checksum2 01777 uint8_t chksum2 = (~rxBuf[5]&0xFE); 01778 if (chksum2 != rxBuf[6]) { 01779 /* #ifdef HERKULEX_DEBUG 01780 pc->printf("Checksum2 fault\n"); 01781 #endif*/ 01782 if(numero_serial == 1) { 01783 flag_serial1_receive=0; 01784 } else if(numero_serial == 2) { 01785 flag_serial2_receive=0; 01786 } else if(numero_serial == 3) { 01787 flag_serial3_receive=0; 01788 } else if(numero_serial == 4) { 01789 flag_serial4_receive=0; 01790 } else if(numero_serial == 5) { 01791 flag_serial5_receive=0; 01792 } 01793 return -1; 01794 } 01795 Tor = rxBuf[9]; 01796 /* #ifdef HERKULEX_DEBUG 01797 pc->printf("position = %04X(%d)\n", position, position); 01798 #endif*/ 01799 //} 01800 return Tor; 01801 } 01802 //---------------fonction pour lire le temperature max pour un servo------------ 01803 //obtenir la valeur de température maximum tolérée par le servomoteur 01804 int8_t Get_Temperature_MAX(int8_t id, uint8_t numero_serial) 01805 // 01806 //id ==> id du servomoteur sur la liaison série 01807 //numero_serial ==> numéro de la liaison série sur laquelle se trouve le servomoteur 01808 // 01809 { 01810 serial_numero = numero_serial; 01811 uint8_t txBuf[9]; 01812 int8_t tempeMAX = 0; 01813 01814 txBuf[0] = HEADER; // Packet Header (0xFF) 01815 txBuf[1] = HEADER; // Packet Header (0xFF) 01816 txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size 01817 txBuf[3] = id; // Servo ID 01818 txBuf[4] = CMD_RAM_READ; // Command Ram Read 01819 txBuf[5] = 0; // Checksum1 01820 txBuf[6] = 0; // Checksum2 01821 txBuf[7] = RAM_MAX_TEMPERATURE; 01822 txBuf[8] = BYTE1; // Length 01823 // Check Sum1 and Check Sum2 01824 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE; 01825 txBuf[6] = (~txBuf[5])&0xFE; 01826 01827 //pc.printf(" tempeMAX "); 01828 01829 uint8_t rxBuf[12]; 01830 01831 if(numero_serial == 1) { 01832 //send packet (mbed -> herkulex) 01833 for(uint8_t i = 0; i < 9 ; i++) { 01834 while(serial1.writeable() == 0); 01835 serial1.putc(txBuf[i]); 01836 wait_us(100); 01837 } 01838 01839 //send packet (mbed -> herkulex) 01840 //uint8_t rxBuf[12]; 01841 wait_ms(TEMPO_R); 01842 if(flag_serial1_receive) { 01843 for(unsigned char i4=0; i4<Size_trame_serial1; i4++) { 01844 rxBuf[i4] = char_receive_serial1[i4]; 01845 //////pc.printf("%d ",(int)char_receive_serial1[i4]); 01846 } 01847 flag_serial1_receive=0; 01848 valueserial1=0; 01849 } 01850 } else if(numero_serial == 2) { 01851 for(uint8_t i = 0; i < 9 ; i++) { 01852 while(serial2.writeable() == 0); 01853 serial2.putc(txBuf[i]); 01854 wait_us(100); 01855 } 01856 wait_ms(TEMPO_R); 01857 if(flag_serial2_receive) { 01858 for(unsigned char i4=0; i4<Size_trame_serial2; i4++) { 01859 rxBuf[i4] = char_receive_serial2[i4]; 01860 //////pc.printf("%d ",(int)char_receive_serial2[i4]); 01861 } 01862 flag_serial2_receive=0; 01863 valueserial2=0; 01864 } 01865 } else if(numero_serial == 3) { 01866 for(uint8_t i = 0; i < 9 ; i++) { 01867 while(serial3.writeable() == 0); 01868 serial3.putc(txBuf[i]); 01869 wait_us(100); 01870 } 01871 wait_ms(TEMPO_R); 01872 if(flag_serial3_receive) { 01873 for(unsigned char i4=0; i4<Size_trame_serial3; i4++) { 01874 rxBuf[i4] = char_receive_serial3[i4]; 01875 //////pc.printf("%d ",(int)char_receive_serial3[i4]); 01876 } 01877 flag_serial3_receive=0; 01878 valueserial3=0; 01879 } 01880 } else if(numero_serial == 4) { 01881 for(uint8_t i = 0; i < 9 ; i++) { 01882 while(serial4.writeable() == 0); 01883 serial4.putc(txBuf[i]); 01884 wait_us(100); 01885 } 01886 wait_ms(TEMPO_R); 01887 if(flag_serial4_receive) { 01888 for(unsigned char i4=0; i4<Size_trame_serial4; i4++) { 01889 rxBuf[i4] = char_receive_serial4[i4]; 01890 //////pc.printf("%d ",(int)char_receive_serial4[i4]); 01891 } 01892 flag_serial4_receive=0; 01893 valueserial4=0; 01894 } 01895 } else if(numero_serial == 5) { 01896 for(uint8_t i = 0; i < 9 ; i++) { 01897 while(serial5.writeable() == 0); 01898 serial5.putc(txBuf[i]); 01899 wait_us(100); 01900 } 01901 wait_ms(TEMPO_R); 01902 if(flag_serial5_receive) { 01903 for(unsigned char i4=0; i4<Size_trame_serial5; i4++) { 01904 rxBuf[i4] = char_receive_serial5[i4]; 01905 //////pc.printf("%d ",(int)char_receive_serial5[i4]); 01906 } 01907 flag_serial5_receive=0; 01908 valueserial5=0; 01909 } 01910 } 01911 01912 uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]) & 0xFE; 01913 if (chksum1 != rxBuf[5]) { 01914 /*#ifdef HERKULEX_DEBUG 01915 pc->printf("Checksum1 fault\n"); 01916 #endif*/ 01917 if(numero_serial == 1) { 01918 flag_serial1_receive=0; 01919 } else if(numero_serial == 2) { 01920 flag_serial2_receive=0; 01921 } else if(numero_serial == 3) { 01922 flag_serial3_receive=0; 01923 } else if(numero_serial == 4) { 01924 flag_serial4_receive=0; 01925 } else if(numero_serial == 5) { 01926 flag_serial5_receive=0; 01927 } 01928 return -1; 01929 }// Checksum2 01930 uint8_t chksum2 = (~rxBuf[5]&0xFE); 01931 if (chksum2 != rxBuf[6]) { 01932 /* #ifdef HERKULEX_DEBUG 01933 pc->printf("Checksum2 fault\n"); 01934 #endif*/ 01935 if(numero_serial == 1) { 01936 flag_serial1_receive=0; 01937 } else if(numero_serial == 2) { 01938 flag_serial2_receive=0; 01939 } else if(numero_serial == 3) { 01940 flag_serial3_receive=0; 01941 } else if(numero_serial == 4) { 01942 flag_serial4_receive=0; 01943 } else if(numero_serial == 5) { 01944 flag_serial5_receive=0; 01945 } 01946 return -1; 01947 } 01948 tempeMAX = rxBuf[9]; 01949 /* #ifdef HERKULEX_DEBUG 01950 pc->printf("position = %04X(%d)\n", position, position); 01951 #endif*/ 01952 //} 01953 return tempeMAX; 01954 } 01955 //--------fonction de controle de position pour deux servo(same playtime)------- 01956 //permet de déplacer deux servomoteurs sur la même liaison série avec le même temps d'execution 01957 void positionControl_Mul_ensemble(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED,uint8_t id2, uint16_t position2, uint8_t setLED2, uint8_t numero_serial) 01958 // 01959 //id 01960 // 01961 01962 { 01963 serial_numero = numero_serial; 01964 float tempo=0; 01965 //if (position > 1023) return; //1002-21 01966 if (playtime > 254) return; //1-254 == 11.2ms-2.844sec. 01967 tempo=playtime*0.012; 01968 uint8_t txBuf[16]; 01969 etat = pos; 01970 pos_position = position; 01971 pos_time = playtime; 01972 pos_led = setLED; 01973 txBuf[0] = HEADER; // Packet Header (0xFF) 01974 txBuf[1] = HEADER; // Packet Header (0xFF) 01975 txBuf[2] = MIN_PACKET_SIZE + 9; // Packet Size 01976 //txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253) 01977 txBuf[3] = 254; // broadcast ID 01978 txBuf[4] = CMD_S_JOG; // Command S JOG (0x06) 01979 txBuf[5] = 0; // Checksum1 01980 txBuf[6] = 0; // Checksum2 01981 txBuf[7] = playtime; // Playtime 01982 txBuf[8] = position & 0x00FF; // Position (LSB, Least Significant Bit) 01983 txBuf[9] =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit) 01984 txBuf[10] = POS_MODE | setLED; // Pos Mode and LED on/off 01985 txBuf[11] = id; // Servo ID 01986 txBuf[12] = position2 & 0x00FF; // Position (LSB, Least Significant Bit) 01987 txBuf[13] =(position2 & 0xFF00) >> 8;// position (MSB, Most Significanct Bit) 01988 txBuf[14] = POS_MODE | setLED2; // Pos Mode and LED on/off 01989 txBuf[15] = id2; // Servo ID 01990 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE 01991 // Checksum2 = (~Checksum1)&0xFE 01992 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]^txBuf[12]^txBuf[13]^txBuf[14]^txBuf[15]) & 0xFE; 01993 txBuf[6] = (~txBuf[5])&0xFE; 01994 // send packet (mbed -> herkulex) 01995 01996 if(numero_serial == 1) { 01997 for(uint8_t i = 0; i < 16 ; i++) { 01998 while(serial1.writeable() == 0); 01999 serial1.putc(txBuf[i]); 02000 wait_us(100); 02001 } 02002 } else if(numero_serial == 2) { 02003 for(uint8_t i = 0; i < 16 ; i++) { 02004 while(serial2.writeable() == 0); 02005 serial2.putc(txBuf[i]); 02006 wait_us(100); 02007 } 02008 } else if(numero_serial == 3) { 02009 for(uint8_t i = 0; i < 16 ; i++) { 02010 while(serial3.writeable() == 0); 02011 serial3.putc(txBuf[i]); 02012 wait_us(100); 02013 } 02014 } else if(numero_serial == 4) { 02015 for(uint8_t i = 0; i < 16 ; i++) { 02016 while(serial4.writeable() == 0); 02017 serial4.putc(txBuf[i]); 02018 wait_us(100); 02019 } 02020 } else if(numero_serial == 5) { 02021 for(uint8_t i = 0; i < 16 ; i++) { 02022 while(serial5.writeable() == 0); 02023 serial5.putc(txBuf[i]); 02024 wait_us(100); 02025 } 02026 } 02027 wait(tempo); 02028 wait_ms(TEMPO_R); 02029 } 02030 //-----fonction de controle de position pour deux servo(different playtime)----- //a changer... 02031 void positionControl_Mul_playtime_different(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED,uint8_t id2, uint16_t position2, uint8_t playtime2, uint8_t setLED2, uint8_t numero_serial) 02032 // 02033 //permet de controler deux servomoteurs avec des temps d'execution différents 02034 // 02035 { 02036 serial_numero = numero_serial; 02037 float tempo=0; 02038 //if (position > 1023) return; //1002-21 02039 if (playtime > 254) playtime = 254; //return; //1-254 == 11.2ms-2.844sec. 02040 if(playtime>playtime2) { 02041 tempo=playtime*0.012; 02042 } else if(playtime<playtime2) { 02043 tempo=playtime2*0.012; 02044 } 02045 uint8_t txBuf[17]; 02046 etat = pos; 02047 pos_position = position; 02048 pos_time = playtime; 02049 pos_led = setLED; 02050 txBuf[0] = HEADER; // Packet Header (0xFF) 02051 txBuf[1] = HEADER; // Packet Header (0xFF) 02052 txBuf[2] = MIN_PACKET_SIZE + 9; // Packet Size 02053 //txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253) 02054 txBuf[3] = 254; // broadcast ID 02055 txBuf[4] = CMD_I_JOG; // Command I JOG 02056 txBuf[5] = 0; // Checksum1 02057 txBuf[6] = 0; // Checksum2 02058 txBuf[7] = position & 0x00FF; // Position (LSB, Least Significant Bit) 02059 txBuf[8] =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit) 02060 txBuf[9] = POS_MODE | setLED; // Pos Mode and LED on/off 02061 txBuf[10] = id; // Servo ID 02062 txBuf[11] = playtime; // Playtime 02063 txBuf[12] = position2 & 0x00FF; // Position (LSB, Least Significant Bit) 02064 txBuf[13] =(position2 & 0xFF00) >> 8;// position (MSB, Most Significanct Bit) 02065 txBuf[14] = POS_MODE | setLED2; // Pos Mode and LED on/off 02066 txBuf[15] = id2; // Servo ID 02067 txBuf[16] = playtime2; // Playtime 02068 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE 02069 // Checksum2 = (~Checksum1)&0xFE 02070 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]^txBuf[12]^txBuf[13]^txBuf[14]^txBuf[15]) & 0xFE; 02071 txBuf[6] = (~txBuf[5])&0xFE; 02072 // send packet (mbed -> herkulex) 02073 //txPacket(12, txBuf); 02074 02075 if(numero_serial == 1) { 02076 for(uint8_t i = 0; i < 17 ; i++) { 02077 while(serial1.writeable() == 0); 02078 serial1.putc(txBuf[i]); 02079 wait_us(100); 02080 } 02081 } else if(numero_serial == 2) { 02082 for(uint8_t i = 0; i < 17 ; i++) { 02083 while(serial2.writeable() == 0); 02084 serial2.putc(txBuf[i]); 02085 wait_us(100); 02086 } 02087 } else if(numero_serial == 3) { 02088 for(uint8_t i = 0; i < 17 ; i++) { 02089 while(serial3.writeable() == 0); 02090 serial3.putc(txBuf[i]); 02091 wait_us(100); 02092 } 02093 } else if(numero_serial == 4) { 02094 for(uint8_t i = 0; i < 17 ; i++) { 02095 while(serial4.writeable() == 0); 02096 serial4.putc(txBuf[i]); 02097 wait_us(100); 02098 } 02099 } else if(numero_serial == 5) { 02100 for(uint8_t i = 0; i < 17 ; i++) { 02101 while(serial5.writeable() == 0); 02102 serial5.putc(txBuf[i]); 02103 wait_us(100); 02104 } 02105 } 02106 02107 wait_ms(TEMPO_R); 02108 } 02109 //-----fonction de controle de position pour plusieurs servo(same playtime)----- 02110 void positionControl_Mul_ensemble_complex(uint8_t nb_servo, uint8_t playtime, uint8_t* data, uint16_t* pos, uint8_t numero_serial) // uint16_t position, uint8_t setLED, uint8_t id 02111 // 02112 //Permet de controler tout les servos de la même liaison série avec le même temps d'execution 02113 // 02114 // 02115 { 02116 serial_numero = numero_serial; 02117 //float tempo=0; 02118 uint8_t taille = 0,i = 0,idata = 0, ipos = 0; 02119 //if (position > 1023) return; //1002-21 02120 if (playtime > 254) return; //1-254 == 11.2ms-2.844sec. 02121 //tempo=playtime*0.012; 02122 taille = 7 + 1 + 4 * nb_servo; 02123 nombre_servo = nb_servo; 02124 pos_time = playtime; 02125 uint8_t txBuf[taille]; 02126 etat = pos_mul_complex; 02127 02128 txBuf[0] = HEADER; // Packet Header (0xFF) 02129 txBuf[1] = HEADER; // Packet Header (0xFF) 02130 txBuf[2] = MIN_PACKET_SIZE + 1 + 4 * nb_servo; // Packet Size 02131 //txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253) 02132 txBuf[3] = 254; // broadcast ID 02133 txBuf[4] = CMD_S_JOG; // Command S JOG (0x06) 02134 txBuf[5] = 0; // Checksum1 02135 txBuf[6] = 0; // Checksum2 02136 txBuf[7] = playtime; // Playtime 02137 02138 for(i=0; i<nb_servo; i++) { 02139 txBuf[8+i*4] = pos[ipos] & 0x00FF; // Position (LSB, Least Significant Bit) 02140 txBuf[9+i*4] =(pos[ipos] & 0xFF00) >> 8;// position (MSB, Most Significanct Bit) 02141 position_servo_mul[ipos] = pos[ipos]; 02142 ipos++; 02143 txBuf[10+i*4] = POS_MODE | data[idata]; // Pos Mode and LED on/off 02144 data_servo_mul[idata] = data[idata]; 02145 idata++; 02146 txBuf[11+i*4] = data[idata]; // Servo ID 02147 data_servo_mul[idata] = data[idata]; 02148 idata++; 02149 } 02150 02151 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE 02152 // Checksum2 = (~Checksum1)&0xFE 02153 txBuf[5] = txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]; 02154 02155 for(i=1; i<(taille-7); i++) { 02156 txBuf[5]=txBuf[5]^txBuf[7+i]; 02157 } 02158 txBuf[5] = txBuf[5]& 0xFE; 02159 txBuf[6] = (~txBuf[5])&0xFE; 02160 02161 // send packet (mbed -> herkulex) 02162 if(numero_serial == 1) { 02163 for(uint8_t i = 0; i < taille ; i++) { 02164 while(serial1.writeable() == 0); 02165 serial1.putc(txBuf[i]); 02166 wait_us(100); 02167 } 02168 } else if(numero_serial == 2) { 02169 for(uint8_t i = 0; i < taille ; i++) { 02170 while(serial2.writeable() == 0); 02171 serial2.putc(txBuf[i]); 02172 wait_us(100); 02173 } 02174 } else if(numero_serial == 3) { 02175 for(uint8_t i = 0; i < taille ; i++) { 02176 while(serial3.writeable() == 0); 02177 serial3.putc(txBuf[i]); 02178 wait_us(100); 02179 } 02180 } else if(numero_serial == 4) { 02181 for(uint8_t i = 0; i < taille ; i++) { 02182 while(serial4.writeable() == 0); 02183 serial4.putc(txBuf[i]); 02184 wait_us(100); 02185 } 02186 } else if(numero_serial == 5) { 02187 for(uint8_t i = 0; i < taille ; i++) { 02188 while(serial5.writeable() == 0); 02189 serial5.putc(txBuf[i]); 02190 wait_us(100); 02191 } 02192 } 02193 02194 /*for(uint8_t i = 0; i < taille ; i++) 02195 { 02196 while(serial1.writeable() == 0); 02197 serial1.putc(txBuf[i]); 02198 wait_us(100); 02199 }*/ 02200 wait_ms(TEMPO_R); 02201 } 02202 //--fonction de controle de position pour plusieurs servo(different playtime)--- 02203 void positionControl_Mul_ensemble_different_complex(uint8_t nb_servo, uint8_t* data, uint16_t* pos, uint8_t numero_serial) // uint16_t position, uint8_t setLED, uint8_t id, uint8_t playtime 02204 // 02205 //Permet de controler tout les servos de la même liaison série avec un temps d'execution différent 02206 // 02207 { 02208 serial_numero = numero_serial; 02209 float tempo=0; 02210 uint8_t Max_playtime = 0; 02211 uint8_t taille = 0,i = 0,idata = 0, ipos = 0,iplay_time = 0; 02212 //if (position > 1023) return; //1002-21 02213 //if (playtime > 254) return; //1-254 == 11.2ms-2.844sec. 02214 02215 for(iplay_time=0; iplay_time<nb_servo; iplay_time++) { 02216 if(Max_playtime<data[2+3*iplay_time]) { 02217 Max_playtime=data[2+3*iplay_time]; 02218 } 02219 } 02220 tempo=Max_playtime*0.012; 02221 taille = 7 + 5 * nb_servo; 02222 nombre_servo = nb_servo; 02223 uint8_t txBuf[taille]; 02224 etat = pos_mul_complex_different; 02225 02226 txBuf[0] = HEADER; // Packet Header (0xFF) 02227 txBuf[1] = HEADER; // Packet Header (0xFF) 02228 txBuf[2] = MIN_PACKET_SIZE + 5 * nb_servo; // Packet Size 02229 //txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253) 02230 txBuf[3] = 254; // broadcast ID 02231 txBuf[4] = CMD_I_JOG; // Command I JOG (0x06) 02232 txBuf[5] = 0; // Checksum1 02233 txBuf[6] = 0; // Checksum2 02234 02235 for(i=0; i<nb_servo; i++) { 02236 txBuf[7+i*5] = pos[ipos] & 0x00FF; // Position (LSB, Least Significant Bit) 02237 txBuf[8+i*5] =(pos[ipos] & 0xFF00) >> 8;// position (MSB, Most Significanct Bit) 02238 position_servo_mul_different[ipos] = pos[ipos]; 02239 ipos++; 02240 txBuf[9+i*5] = POS_MODE | data[idata]; // Pos Mode and LED on/off 02241 data_servo_mul_different[idata] = data[idata]; 02242 idata++; 02243 txBuf[10+i*5] = data[idata]; // Servo ID 02244 data_servo_mul_different[idata] = data[idata]; 02245 idata++; 02246 txBuf[11+i*5] = data[idata]; // Playtime 02247 data_servo_mul_different[idata] = data[idata]; 02248 idata++; 02249 } 02250 02251 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE 02252 // Checksum2 = (~Checksum1)&0xFE 02253 txBuf[5] = txBuf[2]^txBuf[3]^txBuf[4]; 02254 02255 for(i=1; i<(taille-6); i++) { 02256 txBuf[5]=txBuf[5]^txBuf[6+i]; 02257 } 02258 txBuf[5] = txBuf[5]& 0xFE; 02259 txBuf[6] = (~txBuf[5])&0xFE; 02260 02261 // send packet (mbed -> herkulex) 02262 if(numero_serial == 1) { 02263 for(uint8_t i = 0; i < taille ; i++) { 02264 while(serial1.writeable() == 0); 02265 serial1.putc(txBuf[i]); 02266 wait_us(100); 02267 } 02268 } else if(numero_serial == 2) { 02269 for(uint8_t i = 0; i < taille ; i++) { 02270 while(serial2.writeable() == 0); 02271 serial2.putc(txBuf[i]); 02272 wait_us(100); 02273 } 02274 } else if(numero_serial == 3) { 02275 for(uint8_t i = 0; i < taille ; i++) { 02276 while(serial3.writeable() == 0); 02277 serial3.putc(txBuf[i]); 02278 wait_us(100); 02279 } 02280 } else if(numero_serial == 4) { 02281 for(uint8_t i = 0; i < taille ; i++) { 02282 while(serial4.writeable() == 0); 02283 serial4.putc(txBuf[i]); 02284 wait_us(100); 02285 } 02286 } else if(numero_serial == 5) { 02287 for(uint8_t i = 0; i < taille ; i++) { 02288 while(serial5.writeable() == 0); 02289 serial5.putc(txBuf[i]); 02290 wait_us(100); 02291 } 02292 } 02293 02294 /*for(uint8_t i = 0; i < taille ; i++) 02295 { 02296 while(serial1.writeable() == 0); 02297 serial1.putc(txBuf[i]); 02298 wait_us(100); 02299 }*/ 02300 wait(tempo); 02301 wait_ms(TEMPO_R); 02302 } 02303 //---------------fonction pour lire la tension minimale pour un servo---------------- 02304 int8_t Get_Tension_MIN(int8_t id, uint8_t numero_serial) 02305 { 02306 serial_numero = numero_serial; 02307 uint8_t txBuf[9]; 02308 int8_t tensionMIN = 0; 02309 02310 txBuf[0] = HEADER; // Packet Header (0xFF) 02311 txBuf[1] = HEADER; // Packet Header (0xFF) 02312 txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size 02313 txBuf[3] = id; // Servo ID 02314 txBuf[4] = CMD_RAM_READ; // Command Ram Read 02315 txBuf[5] = 0; // Checksum1 02316 txBuf[6] = 0; // Checksum2 02317 txBuf[7] = RAM_MIN_VOLTAGE; 02318 txBuf[8] = BYTE1; // Length 02319 // Check Sum1 and Check Sum2 02320 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE; 02321 txBuf[6] = (~txBuf[5])&0xFE; 02322 02323 //pc.printf(" tensionMIN "); 02324 02325 uint8_t rxBuf[12]; 02326 02327 if(numero_serial == 1) { 02328 //send packet (mbed -> herkulex) 02329 for(uint8_t i = 0; i < 9 ; i++) { 02330 while(serial1.writeable() == 0); 02331 serial1.putc(txBuf[i]); 02332 wait_us(100); 02333 } 02334 02335 //send packet (mbed -> herkulex) 02336 //uint8_t rxBuf[12]; 02337 wait_ms(TEMPO_R); 02338 if(flag_serial1_receive) { 02339 for(unsigned char i4=0; i4<Size_trame_serial1; i4++) { 02340 rxBuf[i4] = char_receive_serial1[i4]; 02341 //////pc.printf("%d ",(int)char_receive_serial1[i4]); 02342 } 02343 flag_serial1_receive=0; 02344 valueserial1=0; 02345 } 02346 } else if(numero_serial == 2) { 02347 for(uint8_t i = 0; i < 9 ; i++) { 02348 while(serial2.writeable() == 0); 02349 serial2.putc(txBuf[i]); 02350 wait_us(100); 02351 } 02352 wait_ms(TEMPO_R); 02353 if(flag_serial2_receive) { 02354 for(unsigned char i4=0; i4<Size_trame_serial2; i4++) { 02355 rxBuf[i4] = char_receive_serial2[i4]; 02356 //////pc.printf("%d ",(int)char_receive_serial2[i4]); 02357 } 02358 flag_serial2_receive=0; 02359 valueserial2=0; 02360 } 02361 } else if(numero_serial == 3) { 02362 for(uint8_t i = 0; i < 9 ; i++) { 02363 while(serial3.writeable() == 0); 02364 serial3.putc(txBuf[i]); 02365 wait_us(100); 02366 } 02367 wait_ms(TEMPO_R); 02368 if(flag_serial3_receive) { 02369 for(unsigned char i4=0; i4<Size_trame_serial3; i4++) { 02370 rxBuf[i4] = char_receive_serial3[i4]; 02371 //////pc.printf("%d ",(int)char_receive_serial3[i4]); 02372 } 02373 flag_serial3_receive=0; 02374 valueserial3=0; 02375 } 02376 } else if(numero_serial == 4) { 02377 for(uint8_t i = 0; i < 9 ; i++) { 02378 while(serial4.writeable() == 0); 02379 serial4.putc(txBuf[i]); 02380 wait_us(100); 02381 } 02382 wait_ms(TEMPO_R); 02383 if(flag_serial4_receive) { 02384 for(unsigned char i4=0; i4<Size_trame_serial4; i4++) { 02385 rxBuf[i4] = char_receive_serial4[i4]; 02386 //////pc.printf("%d ",(int)char_receive_serial4[i4]); 02387 } 02388 flag_serial4_receive=0; 02389 valueserial4=0; 02390 } 02391 } else if(numero_serial == 5) { 02392 for(uint8_t i = 0; i < 9 ; i++) { 02393 while(serial5.writeable() == 0); 02394 serial5.putc(txBuf[i]); 02395 wait_us(100); 02396 } 02397 wait_ms(TEMPO_R); 02398 if(flag_serial5_receive) { 02399 for(unsigned char i4=0; i4<Size_trame_serial5; i4++) { 02400 rxBuf[i4] = char_receive_serial5[i4]; 02401 //////pc.printf("%d ",(int)char_receive_serial5[i4]); 02402 } 02403 flag_serial5_receive=0; 02404 valueserial5=0; 02405 } 02406 } 02407 02408 /*for(uint8_t i = 0; i < 9 ; i++) 02409 { 02410 while(serial1.writeable() == 0); 02411 serial1.putc(txBuf[i]); 02412 wait_us(100); 02413 } 02414 02415 // send packet (mbed -> herkulex) 02416 uint8_t rxBuf[12]; 02417 //wait_ms(3); 02418 wait_ms(TEMPO_R); 02419 if(flag_serial1_receive) 02420 { 02421 for(unsigned char i4=0;i4<Size_trame_serial1; i4++) 02422 { 02423 rxBuf[i4] = char_receive_serial1[i4]; 02424 ////pc.printf("%d ",(int)char_receive_serial1[i4]); 02425 } 02426 flag_serial1_receive=0; 02427 valueserial1=0; 02428 }*/ 02429 02430 uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]) & 0xFE; 02431 if (chksum1 != rxBuf[5]) { 02432 /*#ifdef HERKULEX_DEBUG 02433 pc->printf("Checksum1 fault\n"); 02434 #endif*/ 02435 if(numero_serial == 1) { 02436 flag_serial1_receive=0; 02437 } else if(numero_serial == 2) { 02438 flag_serial2_receive=0; 02439 } else if(numero_serial == 3) { 02440 flag_serial3_receive=0; 02441 } else if(numero_serial == 4) { 02442 flag_serial4_receive=0; 02443 } else if(numero_serial == 5) { 02444 flag_serial5_receive=0; 02445 } 02446 return -1; 02447 }// Checksum2 02448 uint8_t chksum2 = (~rxBuf[5]&0xFE); 02449 if (chksum2 != rxBuf[6]) { 02450 /* #ifdef HERKULEX_DEBUG 02451 pc->printf("Checksum2 fault\n"); 02452 #endif*/ 02453 if(numero_serial == 1) { 02454 flag_serial1_receive=0; 02455 } else if(numero_serial == 2) { 02456 flag_serial2_receive=0; 02457 } else if(numero_serial == 3) { 02458 flag_serial3_receive=0; 02459 } else if(numero_serial == 4) { 02460 flag_serial4_receive=0; 02461 } else if(numero_serial == 5) { 02462 flag_serial5_receive=0; 02463 } 02464 return -1; 02465 } 02466 tensionMIN = rxBuf[9]; 02467 /* #ifdef HERKULEX_DEBUG 02468 pc->printf("position = %04X(%d)\n", position, position); 02469 #endif*/ 02470 //} 02471 return tensionMIN; 02472 } 02473 //-------------fonction pour controle la tension min pour un servo-------------- 02474 void Set_Tension_MIN(int8_t id,uint8_t Tension_Min, uint8_t numero_serial) 02475 { 02476 serial_numero = numero_serial; 02477 uint8_t txBuf[10]; 02478 02479 txBuf[0] = HEADER; // Packet Header (0xFF) 02480 txBuf[1] = HEADER; // Packet Header (0xFF) 02481 txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size 02482 txBuf[3] = id; // Servo ID 02483 txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03) 02484 txBuf[5] = 0; // Checksum1 02485 txBuf[6] = 0; // Checksum2 02486 txBuf[7] = RAM_MIN_VOLTAGE; 02487 txBuf[8] = BYTE1; // Length 02488 txBuf[9] = Tension_Min; 02489 // Check Sum1 and Check Sum2 02490 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE; 02491 txBuf[6] = (~txBuf[5])&0xFE; 02492 02493 //pc.printf(" tensionMIN "); 02494 /*for(uint8_t i = 0; i < 10 ; i++) 02495 { 02496 while(serial1.writeable() == 0); 02497 serial1.putc(txBuf[i]); 02498 wait_us(100); 02499 }*/ 02500 if(numero_serial == 1) { 02501 for(uint8_t i = 0; i < 10 ; i++) { 02502 while(serial1.writeable() == 0); 02503 serial1.putc(txBuf[i]); 02504 wait_us(100); 02505 } 02506 } else if(numero_serial == 2) { 02507 for(uint8_t i = 0; i < 10 ; i++) { 02508 while(serial2.writeable() == 0); 02509 serial2.putc(txBuf[i]); 02510 wait_us(100); 02511 } 02512 } else if(numero_serial == 3) { 02513 for(uint8_t i = 0; i < 10 ; i++) { 02514 while(serial3.writeable() == 0); 02515 serial3.putc(txBuf[i]); 02516 wait_us(100); 02517 } 02518 } else if(numero_serial == 4) { 02519 for(uint8_t i = 0; i < 10 ; i++) { 02520 while(serial4.writeable() == 0); 02521 serial4.putc(txBuf[i]); 02522 wait_us(100); 02523 } 02524 } else if(numero_serial == 5) { 02525 for(uint8_t i = 0; i < 10 ; i++) { 02526 while(serial5.writeable() == 0); 02527 serial5.putc(txBuf[i]); 02528 wait_us(100); 02529 } 02530 } 02531 //wait_ms(3); 02532 wait_ms(TEMPO_R); 02533 } 02534 //------------fonction pour lire la tension d'un servo------------- 02535 int8_t Get_Tension_actuelle(int8_t id, uint8_t numero_serial) 02536 { 02537 serial_numero = numero_serial; 02538 uint8_t txBuf[9]; 02539 int8_t tension = 0; 02540 02541 txBuf[0] = HEADER; // Packet Header (0xFF) 02542 txBuf[1] = HEADER; // Packet Header (0xFF) 02543 txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size 02544 txBuf[3] = id; // Servo ID 02545 txBuf[4] = CMD_RAM_READ; // Command Ram Read (0x03) 02546 txBuf[5] = 0; // Checksum1 02547 txBuf[6] = 0; // Checksum2 02548 txBuf[7] = RAM_VOLTAGE; 02549 txBuf[8] = BYTE2; // Length 02550 // Check Sum1 and Check Sum2 02551 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE; 02552 txBuf[6] = (~txBuf[5])&0xFE; 02553 02554 //pc.printf(" tension "); 02555 02556 uint8_t rxBuf[13]; 02557 02558 if(numero_serial == 1) { 02559 //send packet (mbed -> herkulex) 02560 for(uint8_t i = 0; i < 9 ; i++) { 02561 while(serial1.writeable() == 0); 02562 serial1.putc(txBuf[i]); 02563 wait_us(100); 02564 } 02565 02566 //send packet (mbed -> herkulex) 02567 //uint8_t rxBuf[13]; 02568 wait_ms(TEMPO_R); 02569 if(flag_serial1_receive) { 02570 for(unsigned char i4=0; i4<Size_trame_serial1; i4++) { 02571 rxBuf[i4] = char_receive_serial1[i4]; 02572 //////pc.printf("%d ",(int)char_receive_serial1[i4]); 02573 } 02574 flag_serial1_receive=0; 02575 valueserial1=0; 02576 } 02577 } else if(numero_serial == 2) { 02578 for(uint8_t i = 0; i < 9 ; i++) { 02579 while(serial2.writeable() == 0); 02580 serial2.putc(txBuf[i]); 02581 wait_us(100); 02582 } 02583 wait_ms(TEMPO_R); 02584 if(flag_serial2_receive) { 02585 for(unsigned char i4=0; i4<Size_trame_serial2; i4++) { 02586 rxBuf[i4] = char_receive_serial2[i4]; 02587 //////pc.printf("%d ",(int)char_receive_serial2[i4]); 02588 } 02589 flag_serial2_receive=0; 02590 valueserial2=0; 02591 } 02592 } else if(numero_serial == 3) { 02593 for(uint8_t i = 0; i < 9 ; i++) { 02594 while(serial3.writeable() == 0); 02595 serial3.putc(txBuf[i]); 02596 wait_us(100); 02597 } 02598 wait_ms(TEMPO_R); 02599 if(flag_serial3_receive) { 02600 for(unsigned char i4=0; i4<Size_trame_serial3; i4++) { 02601 rxBuf[i4] = char_receive_serial3[i4]; 02602 //////pc.printf("%d ",(int)char_receive_serial3[i4]); 02603 } 02604 flag_serial3_receive=0; 02605 valueserial3=0; 02606 } 02607 } else if(numero_serial == 4) { 02608 for(uint8_t i = 0; i < 9 ; i++) { 02609 while(serial4.writeable() == 0); 02610 serial4.putc(txBuf[i]); 02611 wait_us(100); 02612 } 02613 wait_ms(TEMPO_R); 02614 if(flag_serial4_receive) { 02615 for(unsigned char i4=0; i4<Size_trame_serial4; i4++) { 02616 rxBuf[i4] = char_receive_serial4[i4]; 02617 //////pc.printf("%d ",(int)char_receive_serial4[i4]); 02618 } 02619 flag_serial4_receive=0; 02620 valueserial4=0; 02621 } 02622 } else if(numero_serial == 5) { 02623 for(uint8_t i = 0; i < 9 ; i++) { 02624 while(serial5.writeable() == 0); 02625 serial5.putc(txBuf[i]); 02626 wait_us(100); 02627 } 02628 wait_ms(TEMPO_R); 02629 if(flag_serial5_receive) { 02630 for(unsigned char i4=0; i4<Size_trame_serial5; i4++) { 02631 rxBuf[i4] = char_receive_serial5[i4]; 02632 //////pc.printf("%d ",(int)char_receive_serial5[i4]); 02633 } 02634 flag_serial5_receive=0; 02635 valueserial5=0; 02636 } 02637 } 02638 /* 02639 for(uint8_t i = 0; i < 9 ; i++) 02640 { 02641 while(serial1.writeable() == 0); 02642 serial1.putc(txBuf[i]); 02643 wait_us(100); 02644 } 02645 02646 // send packet (mbed -> herkulex) 02647 uint8_t rxBuf[13]; 02648 //wait_ms(3); 02649 wait_ms(TEMPO_R); 02650 if(flag_serial1_receive) 02651 { 02652 for(unsigned char i4=0;i4<Size_trame_serial1; i4++) 02653 { 02654 rxBuf[i4] = char_receive_serial1[i4]; 02655 ////pc.printf("%d ",(int)char_receive_serial1[i4]); 02656 } 02657 flag_serial1_receive=0; 02658 valueserial1=0; 02659 } 02660 */ 02661 02662 uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE; 02663 if (chksum1 != rxBuf[5]) { 02664 /*#ifdef HERKULEX_DEBUG 02665 pc->printf("Checksum1 fault\n"); 02666 #endif*/ 02667 if(numero_serial == 1) { 02668 flag_serial1_receive=0; 02669 } else if(numero_serial == 2) { 02670 flag_serial2_receive=0; 02671 } else if(numero_serial == 3) { 02672 flag_serial3_receive=0; 02673 } else if(numero_serial == 4) { 02674 flag_serial4_receive=0; 02675 } else if(numero_serial == 5) { 02676 flag_serial5_receive=0; 02677 } 02678 return -1; 02679 }// Checksum2 02680 uint8_t chksum2 = (~rxBuf[5]&0xFE); 02681 if (chksum2 != rxBuf[6]) { 02682 /* #ifdef HERKULEX_DEBUG 02683 pc->printf("Checksum2 fault\n"); 02684 #endif*/ 02685 if(numero_serial == 1) { 02686 flag_serial1_receive=0; 02687 } else if(numero_serial == 2) { 02688 flag_serial2_receive=0; 02689 } else if(numero_serial == 3) { 02690 flag_serial3_receive=0; 02691 } else if(numero_serial == 4) { 02692 flag_serial4_receive=0; 02693 } else if(numero_serial == 5) { 02694 flag_serial5_receive=0; 02695 } 02696 return -1; 02697 } tension = rxBuf[9]; 02698 /* #ifdef HERKULEX_DEBUG 02699 pc->printf("position = %04X(%d)\n", position, position); 02700 #endif*/ 02701 //} 02702 return tension; 02703 } 02704 02705 02706 //----------------------------------------------------------------------------------------- 02707 void servo_interrupt_en(void) 02708 { 02709 Interrupt1_en(); 02710 Interrupt2_en(); 02711 Interrupt3_en(); 02712 Interrupt4_en(); 02713 Interrupt5_en(); 02714 } 02715 02716 int8_t Get_Led(uint8_t id, uint8_t numero_serie) 02717 { 02718 char numero_serial = numero_serie; 02719 uint8_t txBuf[7]; 02720 txBuf[0] = HEADER; // Packet Header (0xFF) 02721 txBuf[1] = HEADER; // Packet Header (0xFF) 02722 txBuf[2] = 0x07; // Packet Size 02723 txBuf[3] = id; // Servo ID 02724 txBuf[4] = 0x05; // Command Ram Write (0x03) *On choisi le CMD pour écrire dans un registre 02725 txBuf[5] = 0; // Checksum1 02726 txBuf[6] = 0; // Checksum2 02727 02728 // calcul des checksums // 02729 txBuf[5]=(txBuf[2]^txBuf[3]^txBuf[4]) & 0xFE; // checksum1 02730 txBuf[5] &= 0xFE; // checksum1 02731 txBuf[6] = (~txBuf[5] & 0xFE);//checksum2 02732 02733 02734 // envoi // 02735 //txPacket(0x07,txBuf,numero_serial); 02736 //reception 02737 02738 02739 02740 uint8_t rxBuf[9]; 02741 02742 if(numero_serial == 1) { 02743 for(uint8_t i = 0; i < 7 ; i++) { 02744 while(serial1.writeable() == 0); 02745 serial1.putc(txBuf[i]); 02746 wait_us(100); 02747 } 02748 wait_ms(TEMPO_R); 02749 if(flag_serial1_receive) { 02750 for(unsigned char i4=0; i4<8; i4++) { 02751 rxBuf[i4] = char_receive_serial1[i4]; 02752 } 02753 flag_serial1_receive=0; 02754 valueserial1=0; 02755 } 02756 } else if(numero_serial == 2) { 02757 for(uint8_t i = 0; i < 7 ; i++) { 02758 while(serial2.writeable() == 0); 02759 serial2.putc(txBuf[i]); 02760 wait_us(100); 02761 } 02762 wait_ms(TEMPO_R); 02763 if(flag_serial2_receive) { 02764 for(unsigned char i4=0; i4<8; i4++) { 02765 rxBuf[i4] = char_receive_serial2[i4]; 02766 } 02767 flag_serial2_receive=0; 02768 valueserial2=0; 02769 } 02770 } else if(numero_serial == 3) { 02771 for(uint8_t i = 0; i < 7 ; i++) { 02772 while(serial3.writeable() == 0); 02773 serial3.putc(txBuf[i]); 02774 wait_us(100); 02775 } 02776 wait_ms(TEMPO_R); 02777 if(flag_serial3_receive) { 02778 for(unsigned char i4=0; i4<8; i4++) { 02779 rxBuf[i4] = char_receive_serial3[i4]; 02780 } 02781 flag_serial3_receive=0; 02782 valueserial3=0; 02783 } 02784 } else if(numero_serial == 4) { 02785 for(uint8_t i = 0; i < 7 ; i++) { 02786 while(serial4.writeable() == 0); 02787 serial4.putc(txBuf[i]); 02788 wait_us(100); 02789 } 02790 wait_ms(TEMPO_R); 02791 if(flag_serial4_receive) { 02792 for(unsigned char i4=0; i4<8; i4++) { 02793 rxBuf[i4] = char_receive_serial4[i4]; 02794 } 02795 flag_serial4_receive=0; 02796 valueserial4=0; 02797 } 02798 } else if(numero_serial == 5) { 02799 for(uint8_t i = 0; i < 7 ; i++) { 02800 while(serial5.writeable() == 0); 02801 serial5.putc(txBuf[i]); 02802 wait_us(100); 02803 } 02804 wait_ms(TEMPO_R); 02805 if(flag_serial5_receive) { 02806 for(unsigned char i4=0; i4<8; i4++) { 02807 rxBuf[i4] = char_receive_serial5[i4]; 02808 } 02809 flag_serial5_receive=0; 02810 valueserial5=0; 02811 } 02812 } 02813 02814 // Checksum1 02815 uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]) & 0xFE; 02816 if (chksum1 != rxBuf[5]) { 02817 if(numero_serial == 1) { 02818 flag_serial1_receive=0; 02819 } else if(numero_serial == 2) { 02820 flag_serial2_receive=0; 02821 } else if(numero_serial == 3) { 02822 flag_serial3_receive=0; 02823 } else if(numero_serial == 4) { 02824 flag_serial4_receive=0; 02825 } else if(numero_serial == 5) { 02826 flag_serial5_receive=0; 02827 } 02828 return -1; 02829 }// Checksum2 02830 uint8_t chksum2 = (~rxBuf[5]&0xFE); 02831 if (chksum2 != rxBuf[6]) { 02832 if(numero_serial == 1) { 02833 flag_serial1_receive=0; 02834 } else if(numero_serial == 2) { 02835 flag_serial2_receive=0; 02836 } else if(numero_serial == 3) { 02837 flag_serial3_receive=0; 02838 } else if(numero_serial == 4) { 02839 flag_serial4_receive=0; 02840 } else if(numero_serial == 5) { 02841 flag_serial5_receive=0; 02842 } 02843 return -1; 02844 } int8_t couleur_hrk = rxBuf[7]; // Couleur 02845 02846 return couleur_hrk; 02847 }
Generated on Fri Jul 29 2022 08:50:13 by
1.7.2