CRAC Team
CAN_Servomoteur_V1_Haoxuan avec lib de servo
Dependencies: mbed
Embed:
(wiki syntax)
Show/hide line numbers
lib.cpp
00001 #include "mbed.h" 00002 #include "lib.h" 00003 00004 //RawSerial pc(SERIAL_TX, SERIAL_RX, 57600); 00005 //RawSerial serial4(PB_6,PB_7,57600); 00006 00007 00008 #define tolerance_en_position 16 //1 degre=(1002-21)/320=3.066position 00009 #define tolerance_en_position_negatif -16 00010 #define V_b 45 00011 #define V_m 45 00012 #define V_h 45 00013 #define TEMPO_R 30 00014 00015 //------------------------------------------a choisir--------------------------- 00016 #define nombre_servomoteur 3 // 00017 // 00018 #define ID_1 0xFD //=253 premiere // 00019 #define ID_3 0x0D //=13 troixieme // 00020 #define ID_2 0x0C //=12 deuxieme // 00021 #define ID_4 0x09 //Avec engrenage 00022 #define Pompe_pin1 PA_15 //Pin de Pompe 00023 //------------------------------------------------------------------------------ 00024 00025 uint16_t Pos_Engrenage_centre = 351; 00026 uint16_t Pos_Engrenage_gauche = 607; 00027 uint16_t Pos_Engrenage_droit = 108; 00028 uint16_t Pos_Engrenage_Vers_Droit = 62; 00029 uint16_t Pos_Engrenage_Vers_Gauche = 630; 00030 00031 //----------------------------variables de reception---------------------------- 00032 uint8_t rx[300]; 00033 uint8_t rx2[256]; 00034 unsigned char size_reponse=100; 00035 unsigned char recevoir = 0; 00036 unsigned char i2 = 0; 00037 unsigned char flag_serial4_receive2 = 0; 00038 //--------------------variables et fonction de verification--------------------- 00039 int16_t pos_position = 0, get_pos = 0, pos_ID = 0; 00040 uint8_t pos_led = 0, Status = 0,iID = 0; 00041 uint8_t nombre_servo = 0; 00042 uint8_t pos_time = 0; 00043 uint16_t position_servo_mul[20]; 00044 uint8_t data_servo_mul[40]; 00045 uint8_t flag_correction = 0; 00046 float new_tempo=0; 00047 float tab_tempo[20]; 00048 uint16_t position_servo_mul_different[20]; 00049 uint8_t data_servo_mul_different[60]; 00050 int8_t my_Tor = 0; 00051 int8_t Tension_inter = 0; 00052 float Tension = 0; 00053 uint8_t coeffient_time = 1; 00054 uint8_t veri = 0; 00055 typedef enum {pos,vitesse,pos_mul_complex,pos_mul_complex_different} type_etat ; 00056 static type_etat etat=pos; 00057 void verifacation() 00058 { 00059 uint8_t i = 0; 00060 switch(etat) { 00061 case pos: 00062 //------------------------Status-------------------- 00063 Status = getStatus(pos_ID); 00064 wait_ms(3); 00065 pc.printf("status = %d",Status); 00066 switch(Status) { 00067 case 0: 00068 break; 00069 00070 case 2: //Exceed allowed POT limit 00071 pc.printf("ERR-Depasse la limite de position\n"); 00072 //clean_ERR(pos_ID); 00073 //wait_ms(500); 00074 clear(pos_ID); 00075 //positionControl(pos_ID, 1000, 3, GLED_ON); 00076 wait_ms(3); 00077 Status = getStatus(pos_ID); 00078 wait_ms(3); 00079 pc.printf("status = %d",Status); 00080 break; 00081 } 00082 //------------------Torque et position------------------------------ 00083 my_Tor = Get_Torque(pos_ID); 00084 wait_ms(5); 00085 //pc.printf("my_Tor = %x\n",my_Tor); 00086 while(my_Tor != 0x60) { 00087 setTorque(pos_ID,TORQUE_ON); 00088 my_Tor = Get_Torque(pos_ID); 00089 wait_ms(5); 00090 } 00091 Tension_inter = Get_Tension_actuelle(pos_ID); 00092 Tension = Tension_inter*0.074; 00093 if(Tension <=6.60) { 00094 coeffient_time = 6; 00095 } else if(Tension <= 6.90) { 00096 coeffient_time = 4; 00097 } else if(Tension <= 7.10) { 00098 coeffient_time = 2; 00099 } else if(Tension > 7.10) { 00100 coeffient_time = 1; 00101 } 00102 get_pos = getPos(pos_ID); 00103 pc.printf("P4=%d ",get_pos); 00104 if(((get_pos - pos_position)>tolerance_en_position)||((get_pos - pos_position)<tolerance_en_position_negatif)) { 00105 if((get_pos - pos_position)>tolerance_en_position) { 00106 new_tempo=(get_pos - pos_position)*0.084*coeffient_time + 1; 00107 if (new_tempo > 254) new_tempo = 254; 00108 } else if((get_pos - pos_position)<tolerance_en_position_negatif) { 00109 new_tempo=(get_pos - pos_position)*0.084*coeffient_time +1; 00110 if (new_tempo > 254) new_tempo = 254; 00111 } 00112 positionControl(pos_ID, pos_position, new_tempo, pos_led); 00113 pc.printf("Correction!\n"); 00114 } 00115 break; 00116 case pos_mul_complex: 00117 //---------------------------Status--------------------------- 00118 for(i=0; i<nombre_servo; i++) { 00119 Status = getStatus(data_servo_mul[1+2*i]); 00120 pc.printf("status = %d",Status); 00121 switch(Status) { 00122 case 0: 00123 break; 00124 00125 case 2: //Exceed allowed POT limit 00126 //pc.printf("ERR-Depasse la limite de position\n"); 00127 //clean_ERR(id); 00128 //wait_ms(500); 00129 clear(data_servo_mul[1+2*i]); 00130 //positionControl(id, 1000, 3, GLED_ON); 00131 //wait_ms(3); 00132 //Status = getStatus(data_servo_mul[1+2*i]); 00133 //wait_ms(3); 00134 //pc.printf("status = %d",Status); 00135 break; 00136 } 00137 } 00138 //----------------------Torque et position-------------------------- 00139 for(i=0; i<nombre_servo; i++) { 00140 my_Tor = Get_Torque(data_servo_mul[1+2*i]); 00141 while(my_Tor != 0x60) { 00142 setTorque(data_servo_mul[1+2*i],TORQUE_ON); 00143 my_Tor = Get_Torque(data_servo_mul[1+2*i]); 00144 //pc.printf(" SET_TORQUE "); 00145 00146 Status = getStatus(data_servo_mul[1+2*i]); 00147 clear(data_servo_mul[1+2*i]); 00148 Status = getStatus(data_servo_mul[1+2*i]); 00149 } 00150 } 00151 veri = 0; 00152 while(veri < nombre_servo){ 00153 for(i=0; i<nombre_servo; i++) { 00154 my_Tor = Get_Torque(data_servo_mul[1+2*i]); 00155 while(my_Tor != 0x60) { 00156 setTorque(data_servo_mul[1+2*i],TORQUE_ON); 00157 my_Tor = Get_Torque(data_servo_mul[1+2*i]); 00158 //pc.printf(" SET_TORQUE "); 00159 00160 Status = getStatus(data_servo_mul[1+2*i]); 00161 clear(data_servo_mul[1+2*i]); 00162 Status = getStatus(data_servo_mul[1+2*i]); 00163 } 00164 } 00165 for(i=0; i<nombre_servo; i++) { 00166 Tension_inter = Get_Tension_actuelle(data_servo_mul[1+2*i]); 00167 Tension = Tension_inter*0.074; 00168 if(Tension <=6.60) { 00169 coeffient_time = 6; 00170 } else if(Tension <= 6.90) { 00171 coeffient_time = 4; 00172 } else if(Tension <= 7.10) { 00173 coeffient_time = 2; 00174 } else if(Tension > 7.10) { 00175 coeffient_time = 1; 00176 } 00177 get_pos = getPos(data_servo_mul[1+2*i]); 00178 pc.printf("PosiM=%d ",get_pos); 00179 if((get_pos - position_servo_mul[i])>tolerance_en_position) { 00180 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 00181 if (tab_tempo[i] > 254) tab_tempo[i] = 254; 00182 flag_correction = 1; 00183 } else if((get_pos - position_servo_mul[i])<tolerance_en_position_negatif) { 00184 tab_tempo[i]=(position_servo_mul[i] - get_pos)*0.084*coeffient_time+1; 00185 if (tab_tempo[i] > 254) tab_tempo[i] = 254; 00186 flag_correction = 1; 00187 } 00188 } 00189 if(flag_correction == 1) { 00190 new_tempo = 0; 00191 for(i=0; i<nombre_servo; i++) { 00192 if(tab_tempo[i]>new_tempo) { 00193 new_tempo = tab_tempo[i]; 00194 } 00195 } 00196 flag_correction = 0; 00197 positionControl_Mul_ensemble_complex(nombre_servo,new_tempo,data_servo_mul, position_servo_mul); 00198 pc.printf("Correction!\n"); 00199 } 00200 veri = 0; 00201 for(i=0; i<nombre_servo; i++) { 00202 get_pos = getPos(data_servo_mul[1+2*i]); 00203 pc.printf("PosiM=%d ",get_pos); 00204 if((get_pos - position_servo_mul[i])>tolerance_en_position) { 00205 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 00206 if (tab_tempo[i] > 254) tab_tempo[i] = 254; 00207 flag_correction = 1; 00208 } else if((get_pos - position_servo_mul[i])<tolerance_en_position_negatif) { 00209 tab_tempo[i]=(position_servo_mul[i] - get_pos)*0.084*coeffient_time+1; 00210 if (tab_tempo[i] > 254) tab_tempo[i] = 254; 00211 flag_correction = 1; 00212 }else //if(((get_pos - position_servo_mul[i])<tolerance_en_position)&&((get_pos - position_servo_mul[i])>tolerance_en_position_negatif)) 00213 { 00214 veri++; 00215 } 00216 } 00217 } 00218 break; 00219 case pos_mul_complex_different: 00220 //---------------------------Status--------------------------- 00221 for(i=0; i<nombre_servo; i++) { 00222 Status = getStatus(data_servo_mul_different[1+3*i]); 00223 //pc.printf("status = %d",Status); 00224 switch(Status) { 00225 case 0: 00226 break; 00227 00228 case 2: //Exceed allowed POT limit 00229 //pc.printf("ERR-Depasse la limite de position\n"); 00230 //clean_ERR(id); 00231 //wait_ms(500); 00232 clear(data_servo_mul_different[1+3*i]); 00233 //positionControl(id, 1000, 3, GLED_ON); 00234 //wait_ms(3); 00235 //Status = getStatus(data_servo_mul_different[1+2*i]); 00236 //wait_ms(3); 00237 //pc.printf("status = %d",Status); 00238 break; 00239 } 00240 } 00241 //-------------------Torque et position----------------------------- 00242 for(i=0; i<nombre_servo; i++) { 00243 my_Tor = Get_Torque(data_servo_mul_different[1+3*i]); 00244 while(my_Tor != 0x60) { 00245 setTorque(data_servo_mul_different[1+3*i],TORQUE_ON); 00246 my_Tor = Get_Torque(data_servo_mul_different[1+3*i]); 00247 //wait_ms(5); 00248 //pc.printf(" SET_TORQUE "); 00249 } 00250 } 00251 for(i=0; i<nombre_servo; i++) { 00252 Tension_inter = Get_Tension_actuelle(data_servo_mul_different[1+3*i]); 00253 Tension = Tension_inter*0.074; 00254 if(Tension <=6.60) { 00255 coeffient_time = 6; 00256 } else if(Tension <= 6.90) { 00257 coeffient_time = 4; 00258 } else if(Tension <= 7.10) { 00259 coeffient_time = 2; 00260 } else if(Tension > 7.10) { 00261 coeffient_time = 1; 00262 } 00263 get_pos = getPos(data_servo_mul_different[1+3*i]); 00264 pc.printf("PosiM=%d ",get_pos); 00265 if((get_pos - position_servo_mul_different[i])>tolerance_en_position) { 00266 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 00267 if (tab_tempo[i] > 254) tab_tempo[i] = 254; 00268 data_servo_mul_different[2+3*i] = tab_tempo[i]; 00269 flag_correction = 1; 00270 } else if((get_pos - position_servo_mul_different[i])<tolerance_en_position_negatif) { 00271 tab_tempo[i]=(position_servo_mul_different[i] - get_pos)*0.084*coeffient_time+1; 00272 if (tab_tempo[i] > 254) tab_tempo[i] = 254; 00273 data_servo_mul_different[2+3*i] = tab_tempo[i]; 00274 flag_correction = 1; 00275 } 00276 } 00277 if(flag_correction == 1) { 00278 flag_correction = 0; 00279 positionControl_Mul_ensemble_different_complex(nombre_servo,data_servo_mul_different, position_servo_mul_different); 00280 pc.printf("Correction!\n"); 00281 } 00282 break; 00283 } 00284 } 00285 //---------------------fonction d'interruption de reception--------------------- 00286 unsigned char flag_perdu_info = 0, indicateur = 0, Size_trame = 0, old_valueserial4 = 0; 00287 unsigned char char_receive_pc[100]; 00288 unsigned char char_receive_serial4[100]; 00289 unsigned char valueserial4=0; 00290 unsigned char valuepc=0,flag_seconde=0,flag_pc_receive=0,flag_serial4_receive=0; 00291 void receive_serial4() 00292 { 00293 char_receive_serial4[valueserial4]=serial4.getc(); 00294 automate(); 00295 } 00296 00297 void Interrupt4_en(void){ 00298 serial4.attach(&receive_serial4,Serial::RxIrq); 00299 } 00300 00301 void automate() 00302 { 00303 typedef enum {Attente,FF,Size,Data} type_etat; 00304 static type_etat etat=Attente; 00305 //pc.printf("coucou"); 00306 00307 //pc.printf("%d\r\n", char_receive_serial4[valueserial4]); 00308 00309 switch (etat) { 00310 case Attente: 00311 if(char_receive_serial4[0] == 0xFF) 00312 { 00313 etat = FF; 00314 valueserial4 = 1; 00315 } 00316 break; 00317 case FF: 00318 if(char_receive_serial4[1] == 0xFF) 00319 { 00320 etat = Size; 00321 valueserial4 = 2; 00322 } 00323 else 00324 { 00325 etat = Attente; 00326 valueserial4 = 0; 00327 flag_perdu_info = 1; //flag_perdu_info 00328 } 00329 break; 00330 case Size: 00331 if(char_receive_serial4[2] < 7) 00332 { 00333 etat = Attente; 00334 valueserial4 = 0; 00335 flag_perdu_info = 1; //flag_perdu_info 00336 } else { 00337 etat = Data; 00338 old_valueserial4 = 2; 00339 valueserial4 = 3; 00340 } 00341 Size_trame = char_receive_serial4[2]; 00342 break; 00343 00344 case Data: 00345 if((valueserial4-2)==(Size_trame-3)) 00346 { 00347 flag_serial4_receive = 1; 00348 etat = Attente; 00349 valueserial4 = 0; 00350 } else { 00351 valueserial4++; 00352 } 00353 break; 00354 00355 default:break; 00356 } 00357 } 00358 00359 //----------------xxxxx----fonction de fermture de serial----------------------- 00360 /*void N_Herkulex() 00361 { 00362 00363 if(Sv != NULL) 00364 delete Sv; 00365 if(recevoir==2) { 00366 size_reponse = rx2[recevoir]; 00367 } 00368 }*/ 00369 //--------------2e fonction d'effacer les erreurs (pas utilisee)---------------- 00370 int8_t clean_ERR(uint8_t id) 00371 { 00372 uint8_t Cx[11]; //{0xFF,0xFF,0x0B,0xFD,0x03,0xC6,0x38,0x30,0x02,0x00,0x00}; 00373 uint8_t Cr[9]; //{0xFF,0xFF,0x09,0xFD,0x04,0xC2,0x3C,0x30,0x02}; 00374 00375 Cx[0] = HEADER; // Packet Header (0xFF) 00376 Cx[1] = HEADER; // Packet Header (0xFF) 00377 Cx[2] = MIN_PACKET_SIZE + 4; // Packet Size 00378 Cx[3] = id; // Servo ID 00379 Cx[4] = CMD_RAM_WRITE; // Command Ram Write (0x03) 00380 Cx[5] = 0xC6; // Checksum1 00381 Cx[6] = 0x38; // Checksum2 00382 Cx[7] = RAM_POSITION_FEEDFORWARD_1ST_GAIN; // Address 00383 Cx[8] = BYTE2; // Length 00384 Cx[9] = 0; 00385 Cx[10]= 0; 00386 00387 Cr[0] = HEADER; // Packet Header (0xFF) 00388 Cr[1] = HEADER; // Packet Header (0xFF) 00389 Cr[2] = MIN_PACKET_SIZE + 2; // Packet Size 00390 Cr[3] = id; // Servo ID 00391 Cr[4] = CMD_RAM_READ; // Command Ram Read 00392 Cr[5] = 0xC2; // Checksum1 00393 Cr[6] = 0x3C; // Checksum2 00394 Cr[7] = RAM_POSITION_FEEDFORWARD_1ST_GAIN; // Address 00395 Cr[8] = BYTE2; // Length 00396 00397 size_reponse = 13; 00398 for(uint8_t i = 0; i < 11 ; i++) { 00399 while(serial4.writeable() == 0); 00400 serial4.putc(Cx[i]); 00401 } 00402 wait_ms(0.3); 00403 for(uint8_t i = 0; i < 9 ; i++) { 00404 while(serial4.writeable() == 0); 00405 serial4.putc(Cr[i]); 00406 } 00407 wait_ms(0.5); 00408 00409 uint8_t rxBuf[13]; 00410 if(flag_serial4_receive2) { 00411 for(i2 = 0; i2<13; i2++) { 00412 rxBuf[i2] = rx2[i2]; 00413 pc.printf(" %x",rx2[i2]); 00414 } 00415 flag_serial4_receive2=0; 00416 } 00417 // Checksum1 00418 uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE; 00419 if (chksum1 != rxBuf[5]) { 00420 return -1; 00421 } 00422 // Checksum2 00423 uint8_t chksum2 = (~rxBuf[5]&0xFE); 00424 if (chksum2 != rxBuf[6]) { 00425 return -1; 00426 } 00427 return 0; 00428 } 00429 //-------------------------fonction de transmission----------------------------- 00430 //comme j'ecrit cette partie dans chaque conction, cette fonction n'est pas souvent utilisee mais on l'utilise 00431 void txPacket(uint8_t packetSize, uint8_t* data) 00432 { 00433 /*#ifdef HERKULEX_DEBUG 00434 pc->printf("[TX]"); 00435 #endif 00436 for(uint8_t i = 0; i < packetSize ; i++) 00437 { 00438 #ifdef HERKULEX_DEBUG 00439 pc->printf("%02X ",data[i]); 00440 #endif 00441 txd->putc(data[i]); 00442 } 00443 #ifdef HERKULEX_DEBUG 00444 pc->printf("\n"); 00445 #endif*/ 00446 00447 for(uint8_t i = 0; i < packetSize ; i++) 00448 { 00449 while(serial4.writeable() == 0); 00450 serial4.putc(data[i]); 00451 wait_us(100); 00452 } 00453 wait_ms(TEMPO_R); 00454 } 00455 //----------------------------fonction de reception----------------------------- 00456 //comme j'ecrit cette partie dans chaque conction, cette fonction n'est pas souvent utilisee mais on l'utilise 00457 void rxPacket(uint8_t packetSize, uint8_t* data) 00458 { 00459 /*#ifdef HERKULEX_DEBUG 00460 pc->printf("[RX]"); 00461 #endif 00462 for (uint8_t i=0; i < packetSize; i++) 00463 { 00464 data[i] = rxd->getc(); 00465 #ifdef HERKULEX_DEBUG 00466 pc->printf("%02X ",data[i]); 00467 #endif 00468 } 00469 #ifdef HERKULEX_DEBUG 00470 pc->printf("\n"); 00471 #endif*/ 00472 00473 if(flag_serial4_receive) 00474 { 00475 for(unsigned char i4=0;i4<Size_trame; i4++) 00476 { 00477 data[i4] = char_receive_serial4[i4]; 00478 pc.printf("%d ",(int)char_receive_serial4[i4]); 00479 } 00480 flag_serial4_receive=0; 00481 valueserial4=0; 00482 } 00483 } 00484 //----------------------fonction d'effacer les erreurs-------------------------- 00485 void clear(uint8_t id) 00486 { 00487 uint8_t txBuf[11]; 00488 txBuf[0] = HEADER; // Packet Header (0xFF) 00489 txBuf[1] = HEADER; // Packet Header (0xFF) 00490 txBuf[2] = MIN_PACKET_SIZE + 4; // Packet Size 00491 txBuf[3] = id; // Servo ID 00492 txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03) 00493 txBuf[5] = 0; // Checksum1 00494 txBuf[6] = 0; // Checksum2 00495 txBuf[7] = RAM_STATUS_ERROR; // Address 00496 txBuf[8] = BYTE2; // Length 00497 txBuf[9] = 0; // Clear RAM_STATUS_ERROR 00498 txBuf[10]= 0; // Clear RAM_STATUS_DETAIL 00499 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE 00500 // Checksum2 = (~Checksum1)&0xFE 00501 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]) & 0xFE; 00502 txBuf[6] = (~txBuf[5])&0xFE; 00503 // send packet (mbed -> herkulex) 00504 for(uint8_t i = 0; i < 11 ; i++) 00505 { 00506 while(serial4.writeable() == 0); 00507 serial4.putc(txBuf[i]); 00508 wait_us(100); 00509 } 00510 wait_ms(TEMPO_R); 00511 } 00512 //----------------fonction de mis a jour le couple de servo--------------------- 00513 void setTorque(uint8_t id, uint8_t cmdTorue) 00514 { 00515 uint8_t txBuf[10]; 00516 txBuf[0] = HEADER; // Packet Header (0xFF) 00517 txBuf[1] = HEADER; // Packet Header (0xFF) 00518 txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size 00519 txBuf[3] = id; // Servo ID 00520 txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03) 00521 txBuf[5] = 0; // Checksum1 00522 txBuf[6] = 0; // Checksum2 00523 txBuf[7] = RAM_TORQUE_CONTROL; // Address 00524 txBuf[8] = BYTE1; // Length 00525 txBuf[9] = cmdTorue; // Torque ON 00526 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE 00527 // Checksum2 = (~Checksum1)&0xFE 00528 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE; 00529 txBuf[6] = (~txBuf[5])&0xFE; 00530 // send packet (mbed -> herkulex) 00531 00532 for(uint8_t i = 0; i < 10 ; i++) 00533 { 00534 while(serial4.writeable() == 0); 00535 serial4.putc(txBuf[i]); 00536 wait_us(100); 00537 } 00538 wait_ms(TEMPO_R); 00539 } 00540 //-------------fonction de controle de position pour un seul servo-------------- 00541 void positionControl(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED) 00542 { 00543 float tempo=0; 00544 //if (position > 1023) return; //1002-21 00545 if (playtime > 254) playtime = 254; //return; //1-254 == 11.2ms-2.844sec. 00546 tempo=playtime*0.012; 00547 pos_ID = id; 00548 uint8_t txBuf[12]; 00549 etat = pos; 00550 pos_position = position; 00551 pos_time = playtime; 00552 pos_led = setLED; 00553 txBuf[0] = HEADER; // Packet Header (0xFF) 00554 txBuf[1] = HEADER; // Packet Header (0xFF) 00555 txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size 00556 //txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253) 00557 txBuf[3] = id; // pID is total number of servos in the network (0 ~ 253) 00558 txBuf[4] = CMD_S_JOG; // Command S JOG (0x06) 00559 txBuf[5] = 0; // Checksum1 00560 txBuf[6] = 0; // Checksum2 00561 txBuf[7] = playtime; // Playtime 00562 txBuf[8] = position & 0x00FF; // Position (LSB, Least Significant Bit) 00563 txBuf[9] =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit) 00564 txBuf[10] = POS_MODE | setLED; // Pos Mode and LED on/off 00565 txBuf[11] = id; // Servo ID 00566 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE 00567 // Checksum2 = (~Checksum1)&0xFE 00568 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE; 00569 txBuf[6] = (~txBuf[5])&0xFE; 00570 // send packet (mbed -> herkulex) 00571 //txPacket(12, txBuf); 00572 for(uint8_t i = 0; i < 12 ; i++) 00573 { 00574 while(serial4.writeable() == 0); 00575 serial4.putc(txBuf[i]); 00576 wait_us(100); 00577 } 00578 wait(tempo); 00579 wait_ms(TEMPO_R); 00580 } 00581 //-------------fonction de controle de vitesse pour un seul servo--------------- 00582 //Comme je n'ai pas utilise cette fonction, je ne l'ai pas encore change 00583 void velocityControl(uint8_t id, int16_t speed, uint8_t setLED) 00584 { 00585 if (speed > 1023 || speed < -1023) return; 00586 uint8_t txBuf[12]; 00587 txBuf[0] = HEADER; // Packet Header (0xFF) 00588 txBuf[1] = HEADER; // Packet Header (0xFF) 00589 txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size 00590 txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253) 00591 txBuf[4] = CMD_S_JOG; // Command S JOG (0x06) 00592 txBuf[5] = 0; // Checksum1 00593 txBuf[6] = 0; // Checksum2 00594 txBuf[7] = 0; // Playtime, unmeaningful in turn mode 00595 txBuf[8] = speed & 0x00FF; // Speed (LSB, Least Significant Bit) 00596 txBuf[9] =(speed & 0xFF00) >> 8; // Speed (MSB, Most Significanct Bit) 00597 txBuf[10] = TURN_MODE | setLED; // Turn Mode and LED on/off 00598 txBuf[11] = id; // Servo ID 00599 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE 00600 // Checksum2 = (~Checksum1)&0xFE 00601 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE; 00602 txBuf[6] = (~txBuf[5])&0xFE; 00603 // send packet (mbed -> herkulex) 00604 txPacket(12, txBuf); 00605 wait_ms(TEMPO_R); 00606 } 00607 //--------------------fonction d'acquis d'etat d'un servo----------------------- 00608 int8_t getStatus(uint8_t id) 00609 { 00610 uint8_t status; 00611 uint8_t txBuf[7]; 00612 size_reponse = 9; 00613 00614 txBuf[0] = HEADER; // Packet Header (0xFF) 00615 txBuf[1] = HEADER; // Packet Header (0xFF) 00616 txBuf[2] = MIN_PACKET_SIZE; // Packet Size 00617 txBuf[3] = id; // Servo ID 00618 txBuf[4] = CMD_STAT; // Status Error, Status Detail request 00619 // Check Sum1 and Check Sum2 00620 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]) & 0xFE; 00621 txBuf[6] = (~txBuf[5])&0xFE; 00622 00623 // send packet (mbed -> herkulex) 00624 for(uint8_t i = 0; i < 7 ; i++) 00625 { 00626 while(serial4.writeable() == 0); 00627 serial4.putc(txBuf[i]); 00628 wait_us(100); 00629 } 00630 00631 // send packet (mbed -> herkulex) 00632 uint8_t rxBuf[9]; 00633 wait_ms(TEMPO_R); 00634 if(flag_serial4_receive) 00635 { 00636 for(unsigned char i4=0;i4<Size_trame; i4++) 00637 { 00638 rxBuf[i4] = char_receive_serial4[i4]; 00639 pc.printf("%d ",(int)char_receive_serial4[i4]); 00640 } 00641 flag_serial4_receive=0; 00642 valueserial4=0; 00643 } 00644 00645 00646 // Checksum1 00647 uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]) & 0xFE; 00648 if (chksum1 != rxBuf[5]) { 00649 00650 return -1; 00651 } 00652 // Checksum2 00653 uint8_t chksum2 = (~rxBuf[5]&0xFE); 00654 if (chksum2 != rxBuf[6]) { 00655 00656 return -1; 00657 } 00658 status = rxBuf[7]; // Status Error 00659 //status = rxBuf[8]; // Status Detail 00660 00661 00662 return status; 00663 } 00664 //------------------fonction de lire la position actuelle----------------------- 00665 int16_t getPos(uint8_t id) 00666 { 00667 uint16_t position = 0; 00668 uint8_t txBuf[9]; 00669 size_reponse = 13; 00670 00671 txBuf[0] = HEADER; // Packet Header (0xFF) 00672 txBuf[1] = HEADER; // Packet Header (0xFF) 00673 txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size 00674 txBuf[3] = id; // Servo ID 00675 txBuf[4] = CMD_RAM_READ; // Command Ram Read 00676 txBuf[5] = 0; // Checksum1 00677 txBuf[6] = 0; // Checksum2 00678 txBuf[7] = RAM_CALIBRATED_POSITION; // Address 00679 txBuf[8] = BYTE2; 00680 // Check Sum1 and Check Sum2 00681 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE; 00682 txBuf[6] = (~txBuf[5])&0xFE; 00683 00684 for(uint8_t i = 0; i < 9 ; i++) 00685 { 00686 while(serial4.writeable() == 0); 00687 serial4.putc(txBuf[i]); 00688 wait_us(100); 00689 } 00690 00691 // send packet (mbed -> herkulex) 00692 uint8_t rxBuf[13]; 00693 wait_ms(TEMPO_R); 00694 if(flag_serial4_receive) 00695 { 00696 for(unsigned char i4=0;i4<Size_trame; i4++) 00697 { 00698 rxBuf[i4] = char_receive_serial4[i4]; 00699 pc.printf("%d ",(int)char_receive_serial4[i4]); 00700 } 00701 flag_serial4_receive=0; 00702 valueserial4=0; 00703 } 00704 00705 // Checksum1 00706 uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE; 00707 if (chksum1 != rxBuf[5]) { 00708 /*#ifdef HERKULEX_DEBUG 00709 pc->printf("Checksum1 fault\n"); 00710 #endif*/ 00711 return -1; 00712 } 00713 // Checksum2 00714 uint8_t chksum2 = (~rxBuf[5]&0xFE); 00715 if (chksum2 != rxBuf[6]) { 00716 /* #ifdef HERKULEX_DEBUG 00717 pc->printf("Checksum2 fault\n"); 00718 #endif*/ 00719 return -1; 00720 } 00721 position = ((rxBuf[10]&0x03)<<8) | rxBuf[9]; 00722 /* #ifdef HERKULEX_DEBUG 00723 pc->printf("position = %04X(%d)\n", position, position); 00724 #endif*/ 00725 //} 00726 return position; 00727 } 00728 //---------------fonction d'acquis d'etat de couple d'un servo------------------ 00729 int8_t Get_Torque(int8_t id) 00730 { 00731 uint8_t txBuf[9]; 00732 int8_t Tor = 0; 00733 00734 uint8_t iv=0; 00735 for(iv=0; iv<20; iv++) { 00736 rx2[iv] = 0; 00737 } 00738 00739 txBuf[0] = HEADER; // Packet Header (0xFF) 00740 txBuf[1] = HEADER; // Packet Header (0xFF) 00741 txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size 00742 txBuf[3] = id; // Servo ID 00743 txBuf[4] = CMD_RAM_READ; // Command Ram Read 00744 txBuf[5] = 0; // Checksum1 00745 txBuf[6] = 0; // Checksum2 00746 txBuf[7] = RAM_TORQUE_CONTROL; 00747 txBuf[8] = BYTE1; // Length 00748 // Check Sum1 and Check Sum2 00749 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE; 00750 txBuf[6] = (~txBuf[5])&0xFE; 00751 00752 pc.printf(" Torque "); 00753 00754 for(uint8_t i = 0; i < 9 ; i++) 00755 { 00756 while(serial4.writeable() == 0); 00757 serial4.putc(txBuf[i]); 00758 wait_us(100); 00759 } 00760 00761 // send packet (mbed -> herkulex) 00762 uint8_t rxBuf[12]; 00763 //wait_ms(3); 00764 wait_ms(TEMPO_R); 00765 if(flag_serial4_receive) 00766 { 00767 for(unsigned char i4=0;i4<Size_trame; i4++) 00768 { 00769 rxBuf[i4] = char_receive_serial4[i4]; 00770 pc.printf("%d ",(int)char_receive_serial4[i4]); 00771 } 00772 flag_serial4_receive=0; 00773 valueserial4=0; 00774 } 00775 00776 uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]) & 0xFE; 00777 if (chksum1 != rxBuf[5]) { 00778 /*#ifdef HERKULEX_DEBUG 00779 pc->printf("Checksum1 fault\n"); 00780 #endif*/ 00781 recevoir=0; 00782 return -1; 00783 } 00784 // Checksum2 00785 uint8_t chksum2 = (~rxBuf[5]&0xFE); 00786 if (chksum2 != rxBuf[6]) { 00787 /* #ifdef HERKULEX_DEBUG 00788 pc->printf("Checksum2 fault\n"); 00789 #endif*/ 00790 recevoir=0; 00791 return -1; 00792 } 00793 Tor = rxBuf[9]; 00794 /* #ifdef HERKULEX_DEBUG 00795 pc->printf("position = %04X(%d)\n", position, position); 00796 #endif*/ 00797 //} 00798 return Tor; 00799 } 00800 //---------------fonction pour lire le temperature max pour un servo------------ 00801 int8_t Get_Temperature_MAX(int8_t id) 00802 { 00803 00804 uint8_t txBuf[9]; 00805 int8_t tempeMAX = 0; 00806 00807 txBuf[0] = HEADER; // Packet Header (0xFF) 00808 txBuf[1] = HEADER; // Packet Header (0xFF) 00809 txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size 00810 txBuf[3] = id; // Servo ID 00811 txBuf[4] = CMD_RAM_READ; // Command Ram Read 00812 txBuf[5] = 0; // Checksum1 00813 txBuf[6] = 0; // Checksum2 00814 txBuf[7] = RAM_MAX_TEMPERATURE; 00815 txBuf[8] = BYTE1; // Length 00816 // Check Sum1 and Check Sum2 00817 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE; 00818 txBuf[6] = (~txBuf[5])&0xFE; 00819 00820 pc.printf(" tempeMAX "); 00821 00822 for(uint8_t i = 0; i < 9 ; i++) 00823 { 00824 while(serial4.writeable() == 0); 00825 serial4.putc(txBuf[i]); 00826 wait_us(100); 00827 } 00828 00829 // send packet (mbed -> herkulex) 00830 uint8_t rxBuf[12]; 00831 //wait_ms(3); 00832 wait_ms(TEMPO_R); 00833 if(flag_serial4_receive) 00834 { 00835 for(unsigned char i4=0;i4<Size_trame; i4++) 00836 { 00837 rxBuf[i4] = char_receive_serial4[i4]; 00838 pc.printf("%d ",(int)char_receive_serial4[i4]); 00839 } 00840 flag_serial4_receive=0; 00841 valueserial4=0; 00842 } 00843 00844 00845 uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]) & 0xFE; 00846 if (chksum1 != rxBuf[5]) { 00847 /*#ifdef HERKULEX_DEBUG 00848 pc->printf("Checksum1 fault\n"); 00849 #endif*/ 00850 recevoir=0; 00851 return -1; 00852 } 00853 // Checksum2 00854 uint8_t chksum2 = (~rxBuf[5]&0xFE); 00855 if (chksum2 != rxBuf[6]) { 00856 /* #ifdef HERKULEX_DEBUG 00857 pc->printf("Checksum2 fault\n"); 00858 #endif*/ 00859 recevoir=0; 00860 return -1; 00861 } 00862 tempeMAX = rxBuf[9]; 00863 /* #ifdef HERKULEX_DEBUG 00864 pc->printf("position = %04X(%d)\n", position, position); 00865 #endif*/ 00866 //} 00867 return tempeMAX; 00868 } 00869 //--------fonction de controle de position pour deux servo(same playtime)------- 00870 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) 00871 { 00872 float tempo=0; 00873 //if (position > 1023) return; //1002-21 00874 if (playtime > 254) return; //1-254 == 11.2ms-2.844sec. 00875 tempo=playtime*0.012; 00876 uint8_t txBuf[16]; 00877 etat = pos; 00878 pos_position = position; 00879 pos_time = playtime; 00880 pos_led = setLED; 00881 txBuf[0] = HEADER; // Packet Header (0xFF) 00882 txBuf[1] = HEADER; // Packet Header (0xFF) 00883 txBuf[2] = MIN_PACKET_SIZE + 9; // Packet Size 00884 //txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253) 00885 txBuf[3] = 254; // broadcast ID 00886 txBuf[4] = CMD_S_JOG; // Command S JOG (0x06) 00887 txBuf[5] = 0; // Checksum1 00888 txBuf[6] = 0; // Checksum2 00889 txBuf[7] = playtime; // Playtime 00890 txBuf[8] = position & 0x00FF; // Position (LSB, Least Significant Bit) 00891 txBuf[9] =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit) 00892 txBuf[10] = POS_MODE | setLED; // Pos Mode and LED on/off 00893 txBuf[11] = id; // Servo ID 00894 txBuf[12] = position2 & 0x00FF; // Position (LSB, Least Significant Bit) 00895 txBuf[13] =(position2 & 0xFF00) >> 8;// position (MSB, Most Significanct Bit) 00896 txBuf[14] = POS_MODE | setLED2; // Pos Mode and LED on/off 00897 txBuf[15] = id2; // Servo ID 00898 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE 00899 // Checksum2 = (~Checksum1)&0xFE 00900 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; 00901 txBuf[6] = (~txBuf[5])&0xFE; 00902 // send packet (mbed -> herkulex) 00903 00904 for(uint8_t i = 0; i < 16 ; i++) 00905 { 00906 while(serial4.writeable() == 0); 00907 serial4.putc(txBuf[i]); 00908 wait_us(100); 00909 } 00910 wait(tempo); 00911 wait_ms(TEMPO_R); 00912 } 00913 //-----fonction de controle de position pour deux servo(different playtime)----- //a changer... 00914 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) 00915 { 00916 float tempo=0; 00917 //if (position > 1023) return; //1002-21 00918 if (playtime > 254) playtime = 254; //return; //1-254 == 11.2ms-2.844sec. 00919 if(playtime>playtime2) { 00920 tempo=playtime*0.012; 00921 } else if(playtime<playtime2) { 00922 tempo=playtime2*0.012; 00923 } 00924 uint8_t txBuf[17]; 00925 etat = pos; 00926 pos_position = position; 00927 pos_time = playtime; 00928 pos_led = setLED; 00929 txBuf[0] = HEADER; // Packet Header (0xFF) 00930 txBuf[1] = HEADER; // Packet Header (0xFF) 00931 txBuf[2] = MIN_PACKET_SIZE + 9; // Packet Size 00932 //txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253) 00933 txBuf[3] = 254; // broadcast ID 00934 txBuf[4] = CMD_I_JOG; // Command I JOG 00935 txBuf[5] = 0; // Checksum1 00936 txBuf[6] = 0; // Checksum2 00937 txBuf[7] = position & 0x00FF; // Position (LSB, Least Significant Bit) 00938 txBuf[8] =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit) 00939 txBuf[9] = POS_MODE | setLED; // Pos Mode and LED on/off 00940 txBuf[10] = id; // Servo ID 00941 txBuf[11] = playtime; // Playtime 00942 txBuf[12] = position2 & 0x00FF; // Position (LSB, Least Significant Bit) 00943 txBuf[13] =(position2 & 0xFF00) >> 8;// position (MSB, Most Significanct Bit) 00944 txBuf[14] = POS_MODE | setLED2; // Pos Mode and LED on/off 00945 txBuf[15] = id2; // Servo ID 00946 txBuf[16] = playtime2; // Playtime 00947 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE 00948 // Checksum2 = (~Checksum1)&0xFE 00949 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; 00950 txBuf[6] = (~txBuf[5])&0xFE; 00951 // send packet (mbed -> herkulex) 00952 //txPacket(12, txBuf); 00953 00954 for(uint8_t i = 0; i < 17 ; i++) 00955 { 00956 while(serial4.writeable() == 0); 00957 serial4.putc(txBuf[i]); 00958 wait_us(100); 00959 } 00960 wait(tempo); 00961 wait_ms(TEMPO_R); 00962 } 00963 //-----fonction de controle de position pour plusieurs servo(same playtime)----- 00964 void positionControl_Mul_ensemble_complex(uint8_t nb_servo, uint8_t playtime, uint8_t* data, uint16_t* pos) // uint16_t position, uint8_t setLED, uint8_t id 00965 { 00966 float tempo=0; 00967 uint8_t taille = 0,i = 0,idata = 0, ipos = 0; 00968 //if (position > 1023) return; //1002-21 00969 if (playtime > 254) return; //1-254 == 11.2ms-2.844sec. 00970 tempo=playtime*0.012; 00971 taille = 7 + 1 + 4 * nb_servo; 00972 nombre_servo = nb_servo; 00973 pos_time = playtime; 00974 uint8_t txBuf[taille]; 00975 etat = pos_mul_complex; 00976 00977 txBuf[0] = HEADER; // Packet Header (0xFF) 00978 txBuf[1] = HEADER; // Packet Header (0xFF) 00979 txBuf[2] = MIN_PACKET_SIZE + 1 + 4 * nb_servo; // Packet Size 00980 //txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253) 00981 txBuf[3] = 254; // broadcast ID 00982 txBuf[4] = CMD_S_JOG; // Command S JOG (0x06) 00983 txBuf[5] = 0; // Checksum1 00984 txBuf[6] = 0; // Checksum2 00985 txBuf[7] = playtime; // Playtime 00986 00987 for(i=0; i<nb_servo; i++) { 00988 txBuf[8+i*4] = pos[ipos] & 0x00FF; // Position (LSB, Least Significant Bit) 00989 txBuf[9+i*4] =(pos[ipos] & 0xFF00) >> 8;// position (MSB, Most Significanct Bit) 00990 position_servo_mul[ipos] = pos[ipos]; 00991 ipos++; 00992 txBuf[10+i*4] = POS_MODE | data[idata]; // Pos Mode and LED on/off 00993 data_servo_mul[idata] = data[idata]; 00994 idata++; 00995 txBuf[11+i*4] = data[idata]; // Servo ID 00996 data_servo_mul[idata] = data[idata]; 00997 idata++; 00998 } 00999 01000 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE 01001 // Checksum2 = (~Checksum1)&0xFE 01002 txBuf[5] = txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]; 01003 01004 for(i=1; i<(taille-7); i++) { 01005 txBuf[5]=txBuf[5]^txBuf[7+i]; 01006 } 01007 txBuf[5] = txBuf[5]& 0xFE; 01008 txBuf[6] = (~txBuf[5])&0xFE; 01009 01010 // send packet (mbed -> herkulex) 01011 01012 for(uint8_t i = 0; i < taille ; i++) 01013 { 01014 while(serial4.writeable() == 0); 01015 serial4.putc(txBuf[i]); 01016 wait_us(100); 01017 } 01018 //wait(tempo); 01019 //wait_ms(TEMPO_R); 01020 } 01021 //--fonction de controle de position pour plusieurs servo(different playtime)--- 01022 void positionControl_Mul_ensemble_different_complex(uint8_t nb_servo, uint8_t* data, uint16_t* pos) // uint16_t position, uint8_t setLED, uint8_t id, uint8_t playtime 01023 { 01024 float tempo=0; 01025 uint8_t Max_playtime = 0; 01026 uint8_t taille = 0,i = 0,idata = 0, ipos = 0,iplay_time = 0; 01027 //if (position > 1023) return; //1002-21 01028 //if (playtime > 254) return; //1-254 == 11.2ms-2.844sec. 01029 01030 for(iplay_time=0; iplay_time<nb_servo; iplay_time++) { 01031 if(Max_playtime<data[2+3*iplay_time]) { 01032 Max_playtime=data[2+3*iplay_time]; 01033 } 01034 } 01035 tempo=Max_playtime*0.012; 01036 taille = 7 + 5 * nb_servo; 01037 nombre_servo = nb_servo; 01038 uint8_t txBuf[taille]; 01039 etat = pos_mul_complex_different; 01040 01041 txBuf[0] = HEADER; // Packet Header (0xFF) 01042 txBuf[1] = HEADER; // Packet Header (0xFF) 01043 txBuf[2] = MIN_PACKET_SIZE + 5 * nb_servo; // Packet Size 01044 //txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253) 01045 txBuf[3] = 254; // broadcast ID 01046 txBuf[4] = CMD_I_JOG; // Command I JOG (0x06) 01047 txBuf[5] = 0; // Checksum1 01048 txBuf[6] = 0; // Checksum2 01049 01050 for(i=0; i<nb_servo; i++) { 01051 txBuf[7+i*5] = pos[ipos] & 0x00FF; // Position (LSB, Least Significant Bit) 01052 txBuf[8+i*5] =(pos[ipos] & 0xFF00) >> 8;// position (MSB, Most Significanct Bit) 01053 position_servo_mul_different[ipos] = pos[ipos]; 01054 ipos++; 01055 txBuf[9+i*5] = POS_MODE | data[idata]; // Pos Mode and LED on/off 01056 data_servo_mul_different[idata] = data[idata]; 01057 idata++; 01058 txBuf[10+i*5] = data[idata]; // Servo ID 01059 data_servo_mul_different[idata] = data[idata]; 01060 idata++; 01061 txBuf[11+i*5] = data[idata]; // Playtime 01062 data_servo_mul_different[idata] = data[idata]; 01063 idata++; 01064 } 01065 01066 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE 01067 // Checksum2 = (~Checksum1)&0xFE 01068 txBuf[5] = txBuf[2]^txBuf[3]^txBuf[4]; 01069 01070 for(i=1; i<(taille-6); i++) { 01071 txBuf[5]=txBuf[5]^txBuf[6+i]; 01072 } 01073 txBuf[5] = txBuf[5]& 0xFE; 01074 txBuf[6] = (~txBuf[5])&0xFE; 01075 01076 // send packet (mbed -> herkulex) 01077 01078 for(uint8_t i = 0; i < taille ; i++) 01079 { 01080 while(serial4.writeable() == 0); 01081 serial4.putc(txBuf[i]); 01082 wait_us(100); 01083 } 01084 wait(tempo); 01085 wait_ms(TEMPO_R); 01086 } 01087 //---------------fonction pour lire la tension min pour un servo---------------- 01088 int8_t Get_Tension_MIN(int8_t id) 01089 { 01090 01091 uint8_t txBuf[9]; 01092 int8_t tensionMIN = 0; 01093 01094 txBuf[0] = HEADER; // Packet Header (0xFF) 01095 txBuf[1] = HEADER; // Packet Header (0xFF) 01096 txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size 01097 txBuf[3] = id; // Servo ID 01098 txBuf[4] = CMD_RAM_READ; // Command Ram Read 01099 txBuf[5] = 0; // Checksum1 01100 txBuf[6] = 0; // Checksum2 01101 txBuf[7] = RAM_MIN_VOLTAGE; 01102 txBuf[8] = BYTE1; // Length 01103 // Check Sum1 and Check Sum2 01104 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE; 01105 txBuf[6] = (~txBuf[5])&0xFE; 01106 01107 pc.printf(" tensionMIN "); 01108 01109 for(uint8_t i = 0; i < 9 ; i++) 01110 { 01111 while(serial4.writeable() == 0); 01112 serial4.putc(txBuf[i]); 01113 wait_us(100); 01114 } 01115 01116 // send packet (mbed -> herkulex) 01117 uint8_t rxBuf[12]; 01118 //wait_ms(3); 01119 wait_ms(TEMPO_R); 01120 if(flag_serial4_receive) 01121 { 01122 for(unsigned char i4=0;i4<Size_trame; i4++) 01123 { 01124 rxBuf[i4] = char_receive_serial4[i4]; 01125 pc.printf("%d ",(int)char_receive_serial4[i4]); 01126 } 01127 flag_serial4_receive=0; 01128 valueserial4=0; 01129 } 01130 01131 uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]) & 0xFE; 01132 if (chksum1 != rxBuf[5]) { 01133 /*#ifdef HERKULEX_DEBUG 01134 pc->printf("Checksum1 fault\n"); 01135 #endif*/ 01136 recevoir=0; 01137 return -1; 01138 } 01139 // Checksum2 01140 uint8_t chksum2 = (~rxBuf[5]&0xFE); 01141 if (chksum2 != rxBuf[6]) { 01142 /* #ifdef HERKULEX_DEBUG 01143 pc->printf("Checksum2 fault\n"); 01144 #endif*/ 01145 recevoir=0; 01146 return -1; 01147 } 01148 tensionMIN = rxBuf[9]; 01149 /* #ifdef HERKULEX_DEBUG 01150 pc->printf("position = %04X(%d)\n", position, position); 01151 #endif*/ 01152 //} 01153 return tensionMIN; 01154 } 01155 //-------------fonction pour controle la tension min pour un servo-------------- 01156 void Set_Tension_MIN(int8_t id,uint8_t Tension_Min) 01157 { 01158 uint8_t txBuf[10]; 01159 01160 txBuf[0] = HEADER; // Packet Header (0xFF) 01161 txBuf[1] = HEADER; // Packet Header (0xFF) 01162 txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size 01163 txBuf[3] = id; // Servo ID 01164 txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03) 01165 txBuf[5] = 0; // Checksum1 01166 txBuf[6] = 0; // Checksum2 01167 txBuf[7] = RAM_MIN_VOLTAGE; 01168 txBuf[8] = BYTE1; // Length 01169 txBuf[9] = Tension_Min; 01170 // Check Sum1 and Check Sum2 01171 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE; 01172 txBuf[6] = (~txBuf[5])&0xFE; 01173 01174 pc.printf(" tensionMIN "); 01175 for(uint8_t i = 0; i < 10 ; i++) 01176 { 01177 while(serial4.writeable() == 0); 01178 serial4.putc(txBuf[i]); 01179 wait_us(100); 01180 } 01181 //wait_ms(3); 01182 wait_ms(TEMPO_R); 01183 } 01184 //------------fonction pour lire la tension acttuelle pour un servo------------- 01185 int8_t Get_Tension_actuelle(int8_t id) 01186 { 01187 01188 uint8_t txBuf[9]; 01189 int8_t tension = 0; 01190 01191 txBuf[0] = HEADER; // Packet Header (0xFF) 01192 txBuf[1] = HEADER; // Packet Header (0xFF) 01193 txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size 01194 txBuf[3] = id; // Servo ID 01195 txBuf[4] = CMD_RAM_READ; // Command Ram Read (0x03) 01196 txBuf[5] = 0; // Checksum1 01197 txBuf[6] = 0; // Checksum2 01198 txBuf[7] = RAM_VOLTAGE; 01199 txBuf[8] = BYTE2; // Length 01200 // Check Sum1 and Check Sum2 01201 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE; 01202 txBuf[6] = (~txBuf[5])&0xFE; 01203 01204 pc.printf(" tension "); 01205 01206 for(uint8_t i = 0; i < 9 ; i++) 01207 { 01208 while(serial4.writeable() == 0); 01209 serial4.putc(txBuf[i]); 01210 wait_us(100); 01211 } 01212 01213 // send packet (mbed -> herkulex) 01214 uint8_t rxBuf[13]; 01215 //wait_ms(3); 01216 wait_ms(TEMPO_R); 01217 if(flag_serial4_receive) 01218 { 01219 for(unsigned char i4=0;i4<Size_trame; i4++) 01220 { 01221 rxBuf[i4] = char_receive_serial4[i4]; 01222 pc.printf("%d ",(int)char_receive_serial4[i4]); 01223 } 01224 flag_serial4_receive=0; 01225 valueserial4=0; 01226 } 01227 01228 01229 uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE; 01230 if (chksum1 != rxBuf[5]) { 01231 /*#ifdef HERKULEX_DEBUG 01232 pc->printf("Checksum1 fault\n"); 01233 #endif*/ 01234 recevoir=0; 01235 return -1; 01236 } 01237 // Checksum2 01238 uint8_t chksum2 = (~rxBuf[5]&0xFE); 01239 if (chksum2 != rxBuf[6]) { 01240 /* #ifdef HERKULEX_DEBUG 01241 pc->printf("Checksum2 fault\n"); 01242 #endif*/ 01243 recevoir=0; 01244 return -1; 01245 } 01246 tension = rxBuf[9]; 01247 /* #ifdef HERKULEX_DEBUG 01248 pc->printf("position = %04X(%d)\n", position, position); 01249 #endif*/ 01250 //} 01251 return tension; 01252 } 01253 01254 //-----------------deplacement des cubes--------------------- 01255 void deplacement_cubes(uint8_t poi_init, uint8_t poi_fini,int8_t ID,int8_t ID2,int8_t ID3,int8_t ID4,PinName Pompe_pin) 01256 { 01257 PwmOut Pompe(Pompe_pin); 01258 switch(poi_init) { 01259 case 5: //verifie X 01260 //uint8_t servo1[6] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3}; // uint8_t setLED, uint8_t id 01261 //uint8_t servo2[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3}; 01262 uint8_t servo3[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4}; 01263 uint8_t servo4[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01264 uint8_t servo5[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01265 //uint16_t position1[3] = {512, 236, 236}; 01266 //uint16_t position2[3] = {512, 512, 788}; 01267 uint16_t position3[4] = {374, 926, 788, Pos_Engrenage_centre}; 01268 uint16_t position6[4] = {481, 950, 714, Pos_Engrenage_centre}; 01269 uint16_t position4[4] = {650, 952, 511, Pos_Engrenage_centre}; 01270 uint16_t position5[4] = {451, 944, 720, Pos_Engrenage_centre}; 01271 01272 /*positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo1, position1); 01273 verifacation(); 01274 positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo2, position2); 01275 verifacation();*/ 01276 positionControl_Mul_ensemble_complex(4,V_b,servo3, position3); 01277 verifacation(); 01278 positionControl_Mul_ensemble_complex(4,V_b,servo3, position6); 01279 verifacation(); 01280 positionControl_Mul_ensemble_complex(4,V_h,servo4, position4); 01281 verifacation(); 01282 wait(0.3); 01283 Pompe = 1; 01284 wait(0.1); 01285 positionControl_Mul_ensemble_complex(4,V_m,servo5, position5); 01286 verifacation(); 01287 01288 break; 01289 case 3: //verifie X 01290 //uint8_t servo31[6] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3}; // uint8_t setLED, uint8_t id 01291 //uint8_t servo32[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3}; 01292 uint8_t servo33[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4}; 01293 uint8_t servo34[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, BLED_ON, ID4}; 01294 uint8_t servo35[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, BLED_ON, ID4}; 01295 //uint16_t position31[3] = {512, 236, 236}; 01296 //uint16_t position32[3] = {512, 512, 788}; 01297 uint16_t position33[4] = {374, 926, 788, Pos_Engrenage_centre}; 01298 uint16_t position34[4] = {673, 837, 595, Pos_Engrenage_centre}; //647, 842, 604 01299 uint16_t position36[4] = {547, 845, 702, Pos_Engrenage_centre}; 01300 uint16_t position35[4] = {451, 944, 720, Pos_Engrenage_centre}; 01301 01302 //positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo31, position31); 01303 //verifacation(); 01304 //positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo32, position32); 01305 //verifacation(); 01306 positionControl_Mul_ensemble_complex(4,V_b,servo33, position33); 01307 verifacation(); 01308 positionControl_Mul_ensemble_complex(4,V_h,servo34, position34); 01309 verifacation(); 01310 wait(0.3); 01311 Pompe = 1; 01312 wait(0.1); 01313 positionControl_Mul_ensemble_complex(4,V_m,servo33, position36); 01314 verifacation(); 01315 positionControl_Mul_ensemble_complex(4,V_m,servo35, position35); 01316 verifacation(); 01317 01318 break; 01319 case 2: 01320 //uint8_t servo20[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3}; 01321 //uint8_t servo21[6] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3}; // uint8_t setLED, uint8_t id 01322 //uint8_t servo22[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3}; 01323 uint8_t servo23[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4}; 01324 uint8_t servo24[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01325 uint8_t servo25[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01326 //uint16_t position20[3] = {512, 236, 236}; 01327 //uint16_t position21[3] = {512, 236, 236}; 01328 //uint16_t position22[3] = {512, 512, 788}; 01329 uint16_t position23[4] = {374, 926, 788, Pos_Engrenage_gauche}; 01330 uint16_t position24[4] = {673, 837, 595, Pos_Engrenage_gauche}; 01331 uint16_t position25[4] = {451, 944, 720, Pos_Engrenage_gauche}; 01332 01333 /*positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo20, position20); 01334 verifacation(); 01335 wait(0.2); 01336 positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo21, position21); 01337 verifacation(); 01338 positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo22, position22); 01339 verifacation();*/ 01340 positionControl_Mul_ensemble_complex(4,V_b,servo23, position23); 01341 verifacation(); 01342 positionControl_Mul_ensemble_complex(4,V_h,servo24, position24); 01343 verifacation(); 01344 wait(0.3); 01345 Pompe = 1; 01346 wait(0.1); 01347 positionControl_Mul_ensemble_complex(4,V_m,servo25, position25); 01348 verifacation(); 01349 01350 break; 01351 case 4: 01352 //uint8_t servo40[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3}; 01353 //uint8_t servo41[6] = {GLED_ON, ID4, BLED_ON, ID2, GLED_ON, ID3}; // uint8_t setLED, uint8_t id 01354 //uint8_t servo42[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3}; 01355 uint8_t servo43[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4}; 01356 uint8_t servo44[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01357 uint8_t servo45[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01358 //uint16_t position40[3] = {512, 236, 236}; 01359 //uint16_t position41[3] = {512, 236, 236}; 01360 //uint16_t position42[3] = {512, 512, 788}; 01361 uint16_t position43[4] = {374, 926, 788, Pos_Engrenage_droit}; 01362 uint16_t position44[4] = {673, 837, 595, Pos_Engrenage_droit}; 01363 uint16_t position45[4] = {451, 944, 720, Pos_Engrenage_droit}; 01364 01365 /*positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo40, position40); 01366 verifacation(); 01367 wait(0.2); 01368 positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo41, position41); 01369 verifacation(); 01370 positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo42, position42); 01371 verifacation();*/ 01372 positionControl_Mul_ensemble_complex(4,V_b,servo43, position43); 01373 verifacation(); 01374 positionControl_Mul_ensemble_complex(4,V_h,servo44, position44); 01375 verifacation(); 01376 wait(0.3); 01377 Pompe = 1; 01378 wait(0.1); 01379 positionControl_Mul_ensemble_complex(4,V_m,servo45, position45); 01380 verifacation(); 01381 01382 break; 01383 case 1: //verifie X 01384 //uint8_t servo11[6] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3}; // uint8_t setLED, uint8_t id 01385 //uint8_t servo12[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3}; 01386 uint8_t servo13[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4}; 01387 uint8_t servo14[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, BLED_ON, ID4}; 01388 uint8_t servo15[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, BLED_ON, ID4}; 01389 uint8_t servo16[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4}; 01390 //uint16_t position11[3] = {512, 236, 236}; 01391 //uint16_t position12[3] = {512, 512, 788}; 01392 uint16_t position13[4] = {374, 926, 788, Pos_Engrenage_centre}; 01393 uint16_t position17[4] = {658, 657, 790, Pos_Engrenage_centre}; 01394 uint16_t position14[4] = {731, 675, 695, Pos_Engrenage_centre}; 01395 uint16_t position15[4] = {658, 657, 790, Pos_Engrenage_centre}; 01396 uint16_t position16[4] = {417, 885, 796, Pos_Engrenage_centre}; 01397 01398 01399 //positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo11, position11); 01400 //verifacation(); 01401 //positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo12, position12); 01402 //verifacation(); 01403 positionControl_Mul_ensemble_complex(4,V_b,servo13, position13); 01404 verifacation(); 01405 positionControl_Mul_ensemble_complex(4,V_m,servo13, position17); 01406 verifacation(); 01407 positionControl_Mul_ensemble_complex(4,V_h,servo14, position14); 01408 verifacation(); 01409 wait(0.3); 01410 Pompe = 1; 01411 wait(0.1); 01412 positionControl_Mul_ensemble_complex(4,V_h,servo15, position15); 01413 verifacation(); 01414 //wait(2); 01415 positionControl_Mul_ensemble_complex(4,V_h,servo16, position16); 01416 verifacation(); 01417 01418 break; 01419 01420 /*case 22: 01421 uint8_t servo221[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id 01422 uint8_t servo222[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01423 uint8_t servo223[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; 01424 uint8_t servo224[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01425 01426 uint16_t position221[4] = {452, 801, 849, Pos_Engrenage_gauche}; 01427 uint16_t position222[4] = {535, 768, 805, Pos_Engrenage_gauche}; 01428 uint16_t position223[4] = {553, 819, 733, Pos_Engrenage_gauche}; 01429 uint16_t position224[4] = {395, 864, 832, Pos_Engrenage_gauche}; 01430 01431 positionControl_Mul_ensemble_complex(4,V_m,servo221, position221); 01432 verifacation(); 01433 positionControl_Mul_ensemble_complex(4,V_h,servo222, position222); 01434 verifacation(); 01435 positionControl_Mul_ensemble_complex(4,V_b,servo223, position223); 01436 verifacation(); 01437 wait(0.5); 01438 Pompe = 1; 01439 wait(0.5); 01440 positionControl_Mul_ensemble_complex(4,V_b,servo224, position224); 01441 verifacation(); 01442 01443 break; 01444 01445 case 42: 01446 uint8_t servo421[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id 01447 uint8_t servo422[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01448 uint8_t servo423[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; 01449 uint8_t servo424[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01450 01451 uint16_t position421[4] = {452, 801, 849, Pos_Engrenage_droit}; 01452 uint16_t position422[4] = {535, 768, 805, Pos_Engrenage_droit}; 01453 uint16_t position423[4] = {553, 819, 733, Pos_Engrenage_droit}; 01454 uint16_t position424[4] = {395, 864, 832, Pos_Engrenage_droit}; 01455 01456 positionControl_Mul_ensemble_complex(4,V_m,servo421, position421); 01457 verifacation(); 01458 positionControl_Mul_ensemble_complex(4,V_h,servo422, position422); 01459 verifacation(); 01460 positionControl_Mul_ensemble_complex(4,V_b,servo423, position423); 01461 verifacation(); 01462 wait(0.5); 01463 Pompe = 1; 01464 wait(0.5); 01465 positionControl_Mul_ensemble_complex(4,V_b,servo424, position424); 01466 verifacation(); 01467 01468 break;*/ 01469 } 01470 01471 switch(poi_fini) { 01472 /*case 11: 01473 uint8_t servo101[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id 01474 uint8_t servo102[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01475 uint8_t servo103[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; 01476 uint8_t servo104[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01477 01478 uint16_t position101[4] = {663, 615, 820, Pos_Engrenage_centre}; 01479 uint16_t position102[4] = {742, 542, 819, Pos_Engrenage_centre}; 01480 uint16_t position103[4] = {715, 660, 719, Pos_Engrenage_centre}; 01481 uint16_t position104[4] = {418, 885, 789, Pos_Engrenage_centre}; 01482 01483 positionControl_Mul_ensemble_complex(4,V_b,servo101, position101); 01484 verifacation(); 01485 positionControl_Mul_ensemble_complex(4,V_m,servo102, position102); 01486 verifacation(); 01487 positionControl_Mul_ensemble_complex(4,V_h,servo103, position103); 01488 verifacation(); 01489 wait(0.5); 01490 Pompe = 0; 01491 wait(0.5); 01492 positionControl_Mul_ensemble_complex(4,V_b,servo104, position104); 01493 verifacation(); 01494 01495 break;*/ 01496 01497 case 12: 01498 uint8_t servo121[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id 01499 uint8_t servo122[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01500 uint8_t servo123[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; 01501 uint8_t servo124[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01502 01503 uint16_t position121[4] = {660, 620, 828, Pos_Engrenage_centre}; 01504 uint16_t position122[4] = {659, 669, 763, Pos_Engrenage_centre}; 01505 uint16_t position123[4] = {532, 570, 835, Pos_Engrenage_centre}; 01506 uint16_t position124[4] = {418, 885, 789, Pos_Engrenage_centre}; 01507 01508 positionControl_Mul_ensemble_complex(4,V_m,servo121, position121); 01509 verifacation(); 01510 positionControl_Mul_ensemble_complex(4,V_h,servo122, position122); 01511 verifacation(); 01512 wait(0.1); 01513 Pompe = 0; 01514 wait(0.2); 01515 01516 positionControl_Mul_ensemble_complex(4,V_b,servo123, position123); 01517 verifacation(); 01518 positionControl_Mul_ensemble_complex(4,V_b,servo124, position124); 01519 verifacation(); 01520 01521 break; 01522 01523 case 13: 01524 uint8_t servo131[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id 01525 uint8_t servo132[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01526 uint8_t servo133[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; 01527 uint8_t servo134[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01528 01529 uint16_t position131[4] = {552, 649, 853, Pos_Engrenage_centre}; 01530 uint16_t position132[4] = {647, 589, 813, Pos_Engrenage_centre}; 01531 uint16_t position133[4] = {552, 649, 853, Pos_Engrenage_centre}; 01532 uint16_t position134[4] = {418, 885, 789, Pos_Engrenage_centre}; 01533 01534 positionControl_Mul_ensemble_complex(4,V_m,servo131, position131); 01535 verifacation(); 01536 positionControl_Mul_ensemble_complex(4,V_h,servo132, position132); 01537 verifacation(); 01538 wait(0.1); 01539 Pompe = 0; 01540 wait(0.2); 01541 01542 positionControl_Mul_ensemble_complex(4,V_b,servo133, position133); 01543 verifacation(); 01544 positionControl_Mul_ensemble_complex(4,V_b,servo134, position134); 01545 verifacation(); 01546 01547 break; 01548 01549 /*case 14: 01550 uint8_t servo141[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id 01551 uint8_t servo142[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01552 uint8_t servo143[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; 01553 uint8_t servo144[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01554 01555 uint16_t position141[4] = {504, 593, 845, Pos_Engrenage_centre}; 01556 uint16_t position142[4] = {609, 525, 816, Pos_Engrenage_centre}; 01557 uint16_t position143[4] = {420, 661, 845, Pos_Engrenage_centre}; 01558 uint16_t position144[4] = {418, 885, 789, Pos_Engrenage_centre}; 01559 01560 positionControl_Mul_ensemble_complex(4,V_m,servo141, position141); 01561 verifacation(); 01562 positionControl_Mul_ensemble_complex(4,V_h,servo142, position142); 01563 wait(0.5); 01564 Pompe = 0; 01565 wait(0.5); 01566 verifacation(); 01567 positionControl_Mul_ensemble_complex(4,V_b,servo143, position143); 01568 verifacation(); 01569 positionControl_Mul_ensemble_complex(4,V_b,servo144, position144); 01570 verifacation(); 01571 01572 break; 01573 01574 case 15: 01575 uint8_t servo151[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id 01576 uint8_t servo152[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01577 //uint8_t servo153[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; 01578 uint8_t servo154[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01579 uint8_t servo155[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01580 01581 uint16_t position151[4] = {439, 688, 723, Pos_Engrenage_centre}; 01582 uint16_t position152[4] = {406, 691, 789, Pos_Engrenage_centre}; 01583 //uint16_t position153[4] = {399, 667, 767, Pos_Engrenage_centre}; 01584 uint16_t position154[4] = {552, 539, 801, Pos_Engrenage_centre}; 01585 uint16_t position155[4] = {418, 885, 789, Pos_Engrenage_centre}; 01586 01587 //positionControl_Mul_ensemble_complex(4,V_m,servo151, position151); 01588 //verifacation(); 01589 positionControl_Mul_ensemble_complex(4,V_h,servo152, position152); 01590 verifacation(); 01591 //positionControl_Mul_ensemble_complex(4,V_b,servo153, position153); 01592 //verifacation(); 01593 positionControl_Mul_ensemble_complex(4,V_b,servo154, position154); 01594 verifacation(); 01595 wait(0.5); 01596 Pompe = 0; 01597 wait(0.5); 01598 positionControl_Mul_ensemble_complex(4,V_b,servo155, position155); 01599 verifacation(); 01600 01601 01602 break;*/ 01603 01604 case 22: 01605 uint8_t servo221[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id 01606 uint8_t servo222[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01607 uint8_t servo223[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; 01608 uint8_t servo224[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01609 01610 uint16_t position221[4] = {452, 801, 849, Pos_Engrenage_Vers_Gauche}; 01611 uint16_t position222[4] = {535, 768, 805, Pos_Engrenage_Vers_Gauche}; 01612 uint16_t position223[4] = {553, 819, 733, Pos_Engrenage_Vers_Gauche}; 01613 uint16_t position224[4] = {395, 864, 832, Pos_Engrenage_Vers_Gauche}; 01614 01615 positionControl_Mul_ensemble_complex(4,V_m,servo221, position221); 01616 verifacation(); 01617 positionControl_Mul_ensemble_complex(4,V_h,servo222, position222); 01618 verifacation(); 01619 positionControl_Mul_ensemble_complex(4,V_b,servo223, position223); 01620 verifacation(); 01621 wait(0.1); 01622 Pompe = 0; 01623 wait(0.2); 01624 positionControl_Mul_ensemble_complex(4,V_b,servo224, position224); 01625 verifacation(); 01626 01627 break; 01628 01629 case 23: 01630 uint8_t servo231[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id 01631 uint8_t servo232[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01632 uint8_t servo233[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; 01633 uint8_t servo234[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01634 01635 uint16_t position231[4] = {395, 885, 829, Pos_Engrenage_Vers_Gauche}; 01636 uint16_t position232[4] = {540, 733, 816, Pos_Engrenage_Vers_Gauche}; 01637 uint16_t position233[4] = {384, 772, 829, Pos_Engrenage_Vers_Gauche}; 01638 uint16_t position234[4] = {384, 887, 829, Pos_Engrenage_Vers_Gauche}; 01639 01640 positionControl_Mul_ensemble_complex(4,V_m,servo231, position231); 01641 verifacation(); 01642 positionControl_Mul_ensemble_complex(4,V_h,servo232, position232); 01643 verifacation(); 01644 wait(0.1); 01645 Pompe = 0; 01646 wait(0.2); 01647 positionControl_Mul_ensemble_complex(4,V_b,servo233, position233); 01648 verifacation(); 01649 positionControl_Mul_ensemble_complex(4,V_b,servo234, position234); 01650 verifacation(); 01651 01652 break; 01653 01654 case 32: 01655 uint8_t servo321[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id 01656 uint8_t servo322[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01657 uint8_t servo323[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; 01658 uint8_t servo324[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01659 01660 uint16_t position321[4] = {452, 801, 849, Pos_Engrenage_centre}; 01661 uint16_t position322[4] = {535, 768, 805, Pos_Engrenage_centre}; 01662 uint16_t position323[4] = {577, 819, 699, Pos_Engrenage_centre}; 01663 uint16_t position324[4] = {395, 864, 832, Pos_Engrenage_centre}; 01664 01665 positionControl_Mul_ensemble_complex(4,V_m,servo321, position321); 01666 verifacation(); 01667 positionControl_Mul_ensemble_complex(4,V_h,servo322, position322); 01668 verifacation(); 01669 positionControl_Mul_ensemble_complex(4,V_b,servo323, position323); 01670 verifacation(); 01671 wait(0.1); 01672 Pompe = 0; 01673 wait(0.2); 01674 positionControl_Mul_ensemble_complex(4,V_b,servo324, position324); 01675 verifacation(); 01676 01677 break; 01678 01679 case 33: 01680 uint8_t servo331[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id 01681 uint8_t servo332[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01682 uint8_t servo333[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; 01683 uint8_t servo334[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01684 01685 uint16_t position331[4] = {395, 885, 829, Pos_Engrenage_centre}; 01686 uint16_t position332[4] = {540, 733, 816, Pos_Engrenage_centre}; 01687 uint16_t position333[4] = {384, 772, 829, Pos_Engrenage_centre}; 01688 uint16_t position334[4] = {384, 887, 829, Pos_Engrenage_centre}; 01689 01690 positionControl_Mul_ensemble_complex(4,V_m,servo331, position331); 01691 verifacation(); 01692 positionControl_Mul_ensemble_complex(4,V_h,servo332, position332); 01693 verifacation(); 01694 wait(0.1); 01695 Pompe = 0; 01696 wait(0.2); 01697 positionControl_Mul_ensemble_complex(4,V_b,servo333, position333); 01698 verifacation(); 01699 positionControl_Mul_ensemble_complex(4,V_b,servo334, position334); 01700 verifacation(); 01701 01702 break; 01703 01704 case 42: 01705 uint8_t servo421[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id 01706 uint8_t servo422[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01707 uint8_t servo423[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; 01708 uint8_t servo424[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01709 01710 uint16_t position421[4] = {452, 801, 849, Pos_Engrenage_Vers_Droit}; 01711 uint16_t position422[4] = {535, 768, 805, Pos_Engrenage_Vers_Droit}; 01712 uint16_t position423[4] = {573, 818, 725, Pos_Engrenage_Vers_Droit}; 01713 uint16_t position424[4] = {395, 864, 832, Pos_Engrenage_Vers_Droit}; 01714 01715 positionControl_Mul_ensemble_complex(4,V_m,servo421, position421); 01716 verifacation(); 01717 positionControl_Mul_ensemble_complex(4,V_h,servo422, position422); 01718 verifacation(); 01719 positionControl_Mul_ensemble_complex(4,V_b,servo423, position423); 01720 verifacation(); 01721 wait(0.1); 01722 Pompe = 0; 01723 wait(0.2); 01724 positionControl_Mul_ensemble_complex(4,V_b,servo424, position424); 01725 verifacation(); 01726 01727 break; 01728 01729 case 43: 01730 uint8_t servo431[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id 01731 uint8_t servo432[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01732 uint8_t servo433[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; 01733 uint8_t servo434[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4}; 01734 01735 uint16_t position431[4] = {395, 885, 829, Pos_Engrenage_Vers_Droit}; 01736 uint16_t position432[4] = {540, 733, 816, Pos_Engrenage_Vers_Droit}; 01737 uint16_t position433[4] = {384, 772, 829, Pos_Engrenage_Vers_Droit}; 01738 uint16_t position434[4] = {384, 887, 829, Pos_Engrenage_Vers_Droit}; 01739 01740 positionControl_Mul_ensemble_complex(4,V_m,servo431, position431); 01741 verifacation(); 01742 positionControl_Mul_ensemble_complex(4,V_h,servo432, position432); 01743 verifacation(); 01744 wait(0.1); 01745 Pompe = 0; 01746 wait(0.2); 01747 01748 positionControl_Mul_ensemble_complex(4,V_b,servo433, position433); 01749 verifacation(); 01750 positionControl_Mul_ensemble_complex(4,V_b,servo434, position434); 01751 verifacation(); 01752 01753 break; 01754 01755 01756 } 01757 } 01758 01759 void GetPos_Engrenage(int8_t ID) 01760 { 01761 01762 Pos_Engrenage_centre = getPos(ID); 01763 Pos_Engrenage_gauche = Pos_Engrenage_centre + 256; 01764 Pos_Engrenage_droit = Pos_Engrenage_centre - 243; 01765 Pos_Engrenage_Vers_Gauche = Pos_Engrenage_centre + 256; 01766 Pos_Engrenage_Vers_Droit = Pos_Engrenage_centre - 289; 01767 pc.printf("Posi_central=%d Posi_gauche=%d Posi_droit=%d\n",Pos_Engrenage_centre,Pos_Engrenage_gauche,Pos_Engrenage_droit); 01768 01769 } 01770 01771 void Pompe_init(PinName Pompe_pin) 01772 { 01773 PwmOut Pompe1(Pompe_pin); 01774 Pompe1.period(1); 01775 Pompe1 = 0; 01776 wait(0.1); 01777 } 01778 01779 void Pompe_essai(PinName Pompe_pin){ 01780 PwmOut Pompe1(Pompe_pin); 01781 Pompe1 = 1; 01782 wait(1); 01783 Pompe1 = 0; 01784 wait(1); 01785 } 01786 01787 void Arreter_couple(int8_t ID){ 01788 setTorque(ID,TORQUE_FREE); 01789 } 01790 01791 void cubes_systeme(unsigned char Cb1,unsigned char Cb2,unsigned char Cb3, int8_t ID, int8_t ID2, int8_t ID3, int8_t ID4,PinName Pompe_pin){ 01792 //-----------------traitement des infos recues par CAN--------------------------------------- 01793 /*unsigned char C1 = combinaison_CAN/100; 01794 unsigned char C2 = (combinaison_CAN%100)/10; 01795 unsigned char C3 = combinaison_CAN%10; 01796 pc.printf("%d %d %d",C1,C2,C3);*/ 01797 unsigned short combinaison_CAN = Cb1*100+Cb2*10+Cb3; 01798 //------------------------------------------------------------------------------------------- 01799 if((combinaison_CAN == 123) || (combinaison_CAN == 321)) { //--- 01800 deplacement_cubes(5, 42,ID,ID2,ID3,ID4,Pompe_pin); 01801 deplacement_cubes(2, 43,ID,ID2,ID3,ID4,Pompe_pin); 01802 deplacement_cubes(3, 12,ID,ID2,ID3,ID4,Pompe_pin); 01803 } else if((combinaison_CAN == 154) || (combinaison_CAN == 451)) { //---- 01804 deplacement_cubes(1, 42,ID,ID2,ID3,ID4,Pompe_pin); 01805 deplacement_cubes(3, 43,ID,ID2,ID3,ID4,Pompe_pin); 01806 deplacement_cubes(5, 22,ID,ID2,ID3,ID4,Pompe_pin); 01807 } else if((combinaison_CAN == 241) || (combinaison_CAN == 142)) { //---- 01808 deplacement_cubes(3, 42,ID,ID2,ID3,ID4,Pompe_pin); 01809 deplacement_cubes(5, 43,ID,ID2,ID3,ID4,Pompe_pin); 01810 deplacement_cubes(2, 12,ID,ID2,ID3,ID4,Pompe_pin); 01811 } else if((combinaison_CAN == 253) || (combinaison_CAN == 352)) { //---- 01812 deplacement_cubes(1, 22,ID,ID2,ID3,ID4,Pompe_pin); 01813 deplacement_cubes(3, 42,ID,ID2,ID3,ID4,Pompe_pin); 01814 deplacement_cubes(5, 23,ID,ID2,ID3,ID4,Pompe_pin); 01815 } else if((combinaison_CAN == 345) || (combinaison_CAN == 543)) { //---- 01816 deplacement_cubes(3, 22,ID,ID2,ID3,ID4,Pompe_pin); 01817 deplacement_cubes(1, 23,ID,ID2,ID3,ID4,Pompe_pin); 01818 deplacement_cubes(5, 42,ID,ID2,ID3,ID4,Pompe_pin); 01819 } else if((combinaison_CAN == 314) || (combinaison_CAN == 413)) { //---- 01820 deplacement_cubes(4, 22,ID,ID2,ID3,ID4,Pompe_pin); 01821 deplacement_cubes(3, 23,ID,ID2,ID3,ID4,Pompe_pin); 01822 deplacement_cubes(5, 12,ID,ID2,ID3,ID4,Pompe_pin); 01823 } else if((combinaison_CAN == 425) || (combinaison_CAN == 524)) { //---- 01824 deplacement_cubes(5, 12,ID,ID2,ID3,ID4,Pompe_pin); 01825 deplacement_cubes(3, 13,ID,ID2,ID3,ID4,Pompe_pin); 01826 deplacement_cubes(2, 42,ID,ID2,ID3,ID4,Pompe_pin); 01827 } else if((combinaison_CAN == 432) || (combinaison_CAN == 234)) { //---- 01828 deplacement_cubes(1, 42,ID,ID2,ID3,ID4,Pompe_pin); 01829 deplacement_cubes(2, 32,ID,ID2,ID3,ID4,Pompe_pin); 01830 deplacement_cubes(5, 33,ID,ID2,ID3,ID4,Pompe_pin); 01831 } else if((combinaison_CAN == 531) || (combinaison_CAN == 135)) { //---- 01832 deplacement_cubes(2, 12,ID,ID2,ID3,ID4,Pompe_pin); 01833 deplacement_cubes(4, 13,ID,ID2,ID3,ID4,Pompe_pin); 01834 deplacement_cubes(5, 32,ID,ID2,ID3,ID4,Pompe_pin); 01835 } else if((combinaison_CAN == 512) || (combinaison_CAN == 215)) { //---- 01836 deplacement_cubes(4, 12,ID,ID2,ID3,ID4,Pompe_pin); 01837 deplacement_cubes(5, 13,ID,ID2,ID3,ID4,Pompe_pin); 01838 deplacement_cubes(2, 32,ID,ID2,ID3,ID4,Pompe_pin); 01839 } 01840 } 01841 01842 void monter_immeble(unsigned char nb_bras, unsigned char C1,unsigned char C2,unsigned char C3){ 01843 cubes_systeme(C1,C2,C3,ID_1,ID_2,ID_3,ID_4,Pompe_pin1); 01844 /*if(nb_bras == 2){ 01845 cubes_systeme(C1,C2,C3,ID,ID2,ID3,ID4,Pompe_pin1); //deuximeme bras 01846 }*/ 01847 } 01848 01849 void Systeme_deplacement_cube_init(void){ 01850 01851 Bras_Droite_init(); 01852 } 01853 01854 void Bras_Droite_init(void){ 01855 Interrupt4_en(); 01856 wait_ms(0.5); 01857 clear(ID_1); 01858 clear(ID_2); 01859 clear(ID_2); 01860 clear(ID_4); 01861 setTorque(ID_1,TORQUE_ON); 01862 setTorque(ID_2,TORQUE_ON); 01863 setTorque(ID_3,TORQUE_ON); 01864 setTorque(ID_4,TORQUE_ON); 01865 wait_ms(0.3); 01866 Pompe_init(Pompe_pin1); 01867 }
Generated on Wed Jul 20 2022 07:11:22 by
1.7.2