CRAC Team
CAN_Servomoteur_V1_Haoxuan avec lib de servo
Dependencies: mbed
Revision 0:3b683da943e6, committed 2018-04-12
- Comitter:
- Shanglin
- Date:
- Thu Apr 12 08:54:49 2018 +0000
- Commit message:
- CAN_Servo_V1_Haoxuan_avec_lib_servo
Changed in this revision
diff -r 000000000000 -r 3b683da943e6 ident_crac.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ident_crac.h Thu Apr 12 08:54:49 2018 +0000 @@ -0,0 +1,143 @@ +#ifndef CRAC_IDENTH +#define CRAC_IDENTH + + + +#define GLOBAL_GAME_END 0x004 // Stop fin du match +#define GLOBAL_START 0x002 // Start +#define GLOBAL_END_INIT_POSITION 0x005 // Fin positionnement robot avant depart +#define GLOBAL_FUNNY_ACTION 0x007 // Funny action start (0: start, 1: stop) +#define GLOBAL_JACK 0x008 +#define ACKNOWLEDGE_JACK 0X009 + +#define BALISE_STOP 0x003 // Trame stop + +#define BALISE_DANGER 0xA // Trame danger + +#define BALISE_END_DANGER 0xB // Trame fin de danger + + +#define ASSERVISSEMENT_STOP 0x001 // Stop moteur +#define ASSERVISSEMENT_SPEED_DANGER 0x006 // Vitesse de danger +#define ASSERVISSEMENT_XYT 0x020 // Asservissement (x,y,theta) (0 : au choix 1 : avant -1 : arrière) +#define ASSERVISSEMENT_COURBURE 0x021 // Asservissement rayon de courbure (+ gauche, - droite , sens : 1avt , -1arr; enchainement => 1 oui, 0 => non, 2=>derniére instruction de l'enchainement) +#define ASSERVISSEMENT_CONFIG 0x022 // Asservissement paramètre (définir les valeurs de vitesse max et d'eccélération max) +#define ASSERVISSEMENT_ROTATION 0x023 // Asservissement rotation +#define ASSERVISSEMENT_RECALAGE 0x024 // Moteur tout droit (recalage : 0 mouvement seul, 1 x, 2y valeur : coordonnée à laquelle est recalé x/y; enchainement => 1 oui, 0 => non) + +#define ODOMETRIE_BIG_POSITION 0x026 // Odométrie position robot (Position actuel du robot) +#define ODOMETRIE_BIG_VITESSE 0x027 // Odométrie vitesse (Indication sur l'état actuel) +#define ODOMETRIE_SMALL_POSITION 0x028 // Odométrie position robot (Position actuel du robot) +#define ODOMETRIE_SMALL_VITESSE 0x029 // Odométrie vitesse (Indication sur l'état actuel) +#define ACTION_BIG_DEMARRAGE 0x025 // Action de départ du GR (Lancement de la trajectoire de départ du GR) + +#define ASSERVISSEMENT_INFO_CONSIGNE 0x1F0 // Info Consigne et Commande moteur +#define ASSERVISSEMENT_CONFIG_KPP_DROITE 0x1F1 // Config coef KPP_Droit +#define ASSERVISSEMENT_CONFIG_KPI_DROITE 0x1F2 // Config coef KPI_Droit +#define ASSERVISSEMENT_CONFIG_KPD_DROITE 0x1F3 // Config coef KPD_Droit +#define ASSERVISSEMENT_CONFIG_KPP_GAUCHE 0x1F4 // Config coef KPP_Gauche +#define ASSERVISSEMENT_CONFIG_KPI_GAUCHE 0x1F5 // Config coef KPI_Gauche +#define ASSERVISSEMENT_CONFIG_KPD_GAUCHE 0x1F6 // Config coef KPD_Gauche +#define ASSERVISSEMENT_ENABLE 0x1F7 // Activation asservissement (0 : désactivation, 1 : activation) +#define ASSERVISSEMENT_CONFIG_DECEL 0x019 // Asservissement paramètre (définir les valeurs de vitesse max et de decélération max) + +#define RESET_BALISE 0x030 // Reset balise +#define RESET_MOTEUR 0x031 // Reset moteur +#define RESET_IHM 0x032 // Reset écran tactile +#define RESET_ACTIONNEURS 0x033 // Reset actionneurs +#define RESET_POMPES 0x034 // Reset pompes +#define RESET_AX12 0x035 // Reset AX12 +#define RESET_TELEMETRE 0x036 // Reset telemetre + + + +#define RESET_STRAT 0x3A // Reset stratégie + + +#define CHECK_BALISE 0x060 // Check balise +#define CHECK_MOTEUR 0x061 // Check moteur +#define CHECK_IHM 0x062 // Check écran tactile +#define CHECK_ACTIONNEURS_AVANT 0x063 // Check actionneurs +#define CHECK_ACTIONNEURS_ARRIERE 0x064 // Check pompes +#define CHECK_AX12 0x065 // Check AX12 +#define CHECK_OK_TELEMETRE 0x066 // Check telemetre + + +#define ALIVE_BALISE 0x070 // Alive balise +#define ALIVE_MOTEUR 0x071 // Alive moteur +#define ALIVE_IHM 0x072 // Alive écran tactile +#define ALIVE_ACTIONNEURS_AVANT 0x073 // Alive actionneurs +#define ALIVE_ACTIONNEURS_ARRIERE 0x074 // Alive pompes +#define ALIVE_AX12 0x075 // Alive AX12 +#define ALIVE_TELEMETRE 0x076 // Alive telemetre + + + + +#define ACKNOWLEDGE_BALISE 0x100 // Acknowledge balise +#define ACKNOWLEDGE_MOTEUR 0x101 // Acknowledge moteur +#define ACKNOWLEDGE_IHM 0x102 // Acknowledge ecran tactile +#define ACKNOWLEDGE_ACTIONNEURS 0x103 // Acknowledge actionneurs +#define ACKNOWLEDGE_POMPES 0x104 // Acknowledge pompes +#define ACKNOWLEDGE_TELEMETRE 0x105 // Acknowledge telemetre +#define ACKNOWLEDGE_AX12 0x106 // Ack ax12 +#define ACKNOWLEDGE_STRAT 0x10A // Acknowledge pompes +#define ACKNOWLEDGE_CAMERA 0x108 //Acknowledge couleur caméra + + +#define INSTRUCTION_END_BALISE 0x110 // Fin instruction balise (Indique que l'instruction est terminée) +#define INSTRUCTION_END_MOTEUR 0x111 // Fin instruction moteur (Indique que l'instruction est terminée) +#define INSTRUCTION_END_IHM 0x112 // Fin instruction ecran tactile (Indique que l'instruction est terminée) +#define INSTRUCTION_END_ACTIONNEURS 0x113 // Fin instruction actionneurs (Indique que l'instruction est terminée) +#define INSTRUCTION_END_AX12 0x116 + +#define ECRAN_CHOICE_STRAT 0x601 // Choix d'une stratégie (n° strat (1-4)) +#define ECRAN_CHOICE_COLOR 0x602 // Couleur (0->Blue;1->Yellow) +#define ECRAN_START_MATCH 0x603 // Match (Indique que l'on souhaite commencer le match) +#define ECRAN_DEMO_BEGIN 0x604 // Demo (Indique que l'on souhaite faire une demo) +#define ECRAN_ACK_STRAT 0x611 // Acknowledge stratégie (si 0 erreur, sinon n°strat) +#define ECRAN_ACK_COLOR 0x612 // Acknowledge couleur (0->Blue;1->Yellow) +#define ECRAN_ACK_START_MATCH 0x613 // Acknowledge Match (Indique que l'on a bien reçu le debut du match) +#define ECRAN_ACK_DEMO 0x614 +#define ECRAN_ALL_CHECK 0x620 // Carte all check (Si provient de carte strat => toutes les cartes sont en ligne, Si provient IHM => forcer le lancement) +#define ECRAN_TIME 0x621 // Time match (Indication de moment cle du temps (10,30,60,70,80,85,90)) +#define ECRAN_PRINTF_1 0x6C0 // Tactile printf (Afficher les 8 permier caractères) +#define ECRAN_PRINTF_2 0x6C1 // Tactile printf (Afficher les 8 second caractères) +#define ECRAN_PRINTF_3 0x6C2 // Tactile printf (Afficher les 8 troisième caractères) +#define ECRAN_PRINTF_4 0x6C3 // Tactile printf (Afficher les 8 quatrième caractères) +#define ECRAN_PRINTF_CLEAR 0x6CF // Tactile printf clear (Permet d'effacer l'ecran) +#define ECRAN_CHOICE_START_ACTION 0x604 // Tactile printf clear (Choisir si il faut lancer le test actionneur) +#define ECRAN_ACK_CHOICE_START_ACTION 0x605 // Tactile printf clear (Ack du test actionneur) + +#define ERROR_OVERFLOW_BALISE 0x040 // Overflow odométrie +#define ERROR_OVERFLOW_MOTEUR 0x041 // Overflow asservissement +#define ERROR_OVERFLOW_IHM 0x042 // Overflow balise +#define ERROR_OVERFLOW_STRAT 0x043 // Overflow stratégie +#define ERROR_BALISE 0x785 // Bug balise +#define ERROR_RTC 0x786 // Bug RTC +#define ERROR_MOTEUR 0x787 // Bug moteur +#define ERROR_TELEMETRIE 0x788 // Bug télémètre +#define ERROR_STRATEGIE 0x789 // Bug stratégie + +#define DEBUG_STRATEGIE_AUTOMATE 0x760 // Etat automate stratégie (Permet de savoir l'etat de l'automate) +#define DEBUG_FAKE_JAKE 0x761 // Fake jack (Permet d'outre passerr le JACk du robot) +#define DEBUG_ASSERV 0x762 // Info debug carte moteur + +//--------------------------------------------------------------------------------------------- +#define Monter_immeuble 0x090 // AX12 setGoal (Indiquer la nouvelle position de l'AX12 !! Ne bouge pas) //------------ +#define SERVO_AX12_PROCESS 0x091 // AX12 processChange (Lancer le déplacement des AX12) +#define SERVO_AX12_DONE 0x092 // AX12 done (Indique q'un AX12 a terminé son déplacement) +#define SERVO_XL320 0x093 // XL320 +#define SERVO_AX12_ACTION 0x96 +//--------------------------------------------------------------------------------------------- + +#define TELEMETRE_RECHERCHE_COIN 0x301 +#define TELEMETRE_OBJET 0x302 +#define OBJET_SUR_TABLE 0x304 + +#define POMPE_PWM 0x9A // pwm des pompes (pwm entre 0 et 100) + + +//////0x400 Plage de la caméra///// +#define RECEPTION_COULEUR 0x401 +#endif
diff -r 000000000000 -r 3b683da943e6 lib.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/lib.cpp Thu Apr 12 08:54:49 2018 +0000
@@ -0,0 +1,1867 @@
+#include "mbed.h"
+#include "lib.h"
+
+//RawSerial pc(SERIAL_TX, SERIAL_RX, 57600);
+//RawSerial serial4(PB_6,PB_7,57600);
+
+
+#define tolerance_en_position 16 //1 degre=(1002-21)/320=3.066position
+#define tolerance_en_position_negatif -16
+#define V_b 45
+#define V_m 45
+#define V_h 45
+#define TEMPO_R 30
+
+//------------------------------------------a choisir---------------------------
+#define nombre_servomoteur 3 //
+ //
+#define ID_1 0xFD //=253 premiere //
+#define ID_3 0x0D //=13 troixieme //
+#define ID_2 0x0C //=12 deuxieme //
+#define ID_4 0x09 //Avec engrenage
+#define Pompe_pin1 PA_15 //Pin de Pompe
+//------------------------------------------------------------------------------
+
+uint16_t Pos_Engrenage_centre = 351;
+uint16_t Pos_Engrenage_gauche = 607;
+uint16_t Pos_Engrenage_droit = 108;
+uint16_t Pos_Engrenage_Vers_Droit = 62;
+uint16_t Pos_Engrenage_Vers_Gauche = 630;
+
+//----------------------------variables de reception----------------------------
+uint8_t rx[300];
+uint8_t rx2[256];
+unsigned char size_reponse=100;
+unsigned char recevoir = 0;
+unsigned char i2 = 0;
+unsigned char flag_serial4_receive2 = 0;
+//--------------------variables et fonction de verification---------------------
+int16_t pos_position = 0, get_pos = 0, pos_ID = 0;
+uint8_t pos_led = 0, Status = 0,iID = 0;
+uint8_t nombre_servo = 0;
+uint8_t pos_time = 0;
+uint16_t position_servo_mul[20];
+uint8_t data_servo_mul[40];
+uint8_t flag_correction = 0;
+float new_tempo=0;
+float tab_tempo[20];
+uint16_t position_servo_mul_different[20];
+uint8_t data_servo_mul_different[60];
+int8_t my_Tor = 0;
+int8_t Tension_inter = 0;
+float Tension = 0;
+uint8_t coeffient_time = 1;
+uint8_t veri = 0;
+typedef enum {pos,vitesse,pos_mul_complex,pos_mul_complex_different} type_etat ;
+static type_etat etat=pos;
+void verifacation()
+{
+ uint8_t i = 0;
+ switch(etat) {
+ case pos:
+ //------------------------Status--------------------
+ Status = getStatus(pos_ID);
+ wait_ms(3);
+ pc.printf("status = %d",Status);
+ switch(Status) {
+ case 0:
+ break;
+
+ case 2: //Exceed allowed POT limit
+ pc.printf("ERR-Depasse la limite de position\n");
+ //clean_ERR(pos_ID);
+ //wait_ms(500);
+ clear(pos_ID);
+ //positionControl(pos_ID, 1000, 3, GLED_ON);
+ wait_ms(3);
+ Status = getStatus(pos_ID);
+ wait_ms(3);
+ pc.printf("status = %d",Status);
+ break;
+ }
+ //------------------Torque et position------------------------------
+ my_Tor = Get_Torque(pos_ID);
+ wait_ms(5);
+ //pc.printf("my_Tor = %x\n",my_Tor);
+ while(my_Tor != 0x60) {
+ setTorque(pos_ID,TORQUE_ON);
+ my_Tor = Get_Torque(pos_ID);
+ wait_ms(5);
+ }
+ Tension_inter = Get_Tension_actuelle(pos_ID);
+ Tension = Tension_inter*0.074;
+ if(Tension <=6.60) {
+ coeffient_time = 6;
+ } else if(Tension <= 6.90) {
+ coeffient_time = 4;
+ } else if(Tension <= 7.10) {
+ coeffient_time = 2;
+ } else if(Tension > 7.10) {
+ coeffient_time = 1;
+ }
+ get_pos = getPos(pos_ID);
+ pc.printf("P4=%d ",get_pos);
+ if(((get_pos - pos_position)>tolerance_en_position)||((get_pos - pos_position)<tolerance_en_position_negatif)) {
+ if((get_pos - pos_position)>tolerance_en_position) {
+ new_tempo=(get_pos - pos_position)*0.084*coeffient_time + 1;
+ if (new_tempo > 254) new_tempo = 254;
+ } else if((get_pos - pos_position)<tolerance_en_position_negatif) {
+ new_tempo=(get_pos - pos_position)*0.084*coeffient_time +1;
+ if (new_tempo > 254) new_tempo = 254;
+ }
+ positionControl(pos_ID, pos_position, new_tempo, pos_led);
+ pc.printf("Correction!\n");
+ }
+ break;
+ case pos_mul_complex:
+ //---------------------------Status---------------------------
+ for(i=0; i<nombre_servo; i++) {
+ Status = getStatus(data_servo_mul[1+2*i]);
+ pc.printf("status = %d",Status);
+ switch(Status) {
+ case 0:
+ break;
+
+ case 2: //Exceed allowed POT limit
+ //pc.printf("ERR-Depasse la limite de position\n");
+ //clean_ERR(id);
+ //wait_ms(500);
+ clear(data_servo_mul[1+2*i]);
+ //positionControl(id, 1000, 3, GLED_ON);
+ //wait_ms(3);
+ //Status = getStatus(data_servo_mul[1+2*i]);
+ //wait_ms(3);
+ //pc.printf("status = %d",Status);
+ break;
+ }
+ }
+ //----------------------Torque et position--------------------------
+ for(i=0; i<nombre_servo; i++) {
+ my_Tor = Get_Torque(data_servo_mul[1+2*i]);
+ while(my_Tor != 0x60) {
+ setTorque(data_servo_mul[1+2*i],TORQUE_ON);
+ my_Tor = Get_Torque(data_servo_mul[1+2*i]);
+ //pc.printf(" SET_TORQUE ");
+
+ Status = getStatus(data_servo_mul[1+2*i]);
+ clear(data_servo_mul[1+2*i]);
+ Status = getStatus(data_servo_mul[1+2*i]);
+ }
+ }
+ veri = 0;
+ while(veri < nombre_servo){
+ for(i=0; i<nombre_servo; i++) {
+ my_Tor = Get_Torque(data_servo_mul[1+2*i]);
+ while(my_Tor != 0x60) {
+ setTorque(data_servo_mul[1+2*i],TORQUE_ON);
+ my_Tor = Get_Torque(data_servo_mul[1+2*i]);
+ //pc.printf(" SET_TORQUE ");
+
+ Status = getStatus(data_servo_mul[1+2*i]);
+ clear(data_servo_mul[1+2*i]);
+ Status = getStatus(data_servo_mul[1+2*i]);
+ }
+ }
+ for(i=0; i<nombre_servo; i++) {
+ Tension_inter = Get_Tension_actuelle(data_servo_mul[1+2*i]);
+ Tension = Tension_inter*0.074;
+ if(Tension <=6.60) {
+ coeffient_time = 6;
+ } else if(Tension <= 6.90) {
+ coeffient_time = 4;
+ } else if(Tension <= 7.10) {
+ coeffient_time = 2;
+ } else if(Tension > 7.10) {
+ coeffient_time = 1;
+ }
+ get_pos = getPos(data_servo_mul[1+2*i]);
+ pc.printf("PosiM=%d ",get_pos);
+ if((get_pos - position_servo_mul[i])>tolerance_en_position) {
+ 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
+ if (tab_tempo[i] > 254) tab_tempo[i] = 254;
+ flag_correction = 1;
+ } else if((get_pos - position_servo_mul[i])<tolerance_en_position_negatif) {
+ tab_tempo[i]=(position_servo_mul[i] - get_pos)*0.084*coeffient_time+1;
+ if (tab_tempo[i] > 254) tab_tempo[i] = 254;
+ flag_correction = 1;
+ }
+ }
+ if(flag_correction == 1) {
+ new_tempo = 0;
+ for(i=0; i<nombre_servo; i++) {
+ if(tab_tempo[i]>new_tempo) {
+ new_tempo = tab_tempo[i];
+ }
+ }
+ flag_correction = 0;
+ positionControl_Mul_ensemble_complex(nombre_servo,new_tempo,data_servo_mul, position_servo_mul);
+ pc.printf("Correction!\n");
+ }
+ veri = 0;
+ for(i=0; i<nombre_servo; i++) {
+ get_pos = getPos(data_servo_mul[1+2*i]);
+ pc.printf("PosiM=%d ",get_pos);
+ if((get_pos - position_servo_mul[i])>tolerance_en_position) {
+ 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
+ if (tab_tempo[i] > 254) tab_tempo[i] = 254;
+ flag_correction = 1;
+ } else if((get_pos - position_servo_mul[i])<tolerance_en_position_negatif) {
+ tab_tempo[i]=(position_servo_mul[i] - get_pos)*0.084*coeffient_time+1;
+ if (tab_tempo[i] > 254) tab_tempo[i] = 254;
+ flag_correction = 1;
+ }else //if(((get_pos - position_servo_mul[i])<tolerance_en_position)&&((get_pos - position_servo_mul[i])>tolerance_en_position_negatif))
+ {
+ veri++;
+ }
+ }
+ }
+ break;
+ case pos_mul_complex_different:
+ //---------------------------Status---------------------------
+ for(i=0; i<nombre_servo; i++) {
+ Status = getStatus(data_servo_mul_different[1+3*i]);
+ //pc.printf("status = %d",Status);
+ switch(Status) {
+ case 0:
+ break;
+
+ case 2: //Exceed allowed POT limit
+ //pc.printf("ERR-Depasse la limite de position\n");
+ //clean_ERR(id);
+ //wait_ms(500);
+ clear(data_servo_mul_different[1+3*i]);
+ //positionControl(id, 1000, 3, GLED_ON);
+ //wait_ms(3);
+ //Status = getStatus(data_servo_mul_different[1+2*i]);
+ //wait_ms(3);
+ //pc.printf("status = %d",Status);
+ break;
+ }
+ }
+ //-------------------Torque et position-----------------------------
+ for(i=0; i<nombre_servo; i++) {
+ my_Tor = Get_Torque(data_servo_mul_different[1+3*i]);
+ while(my_Tor != 0x60) {
+ setTorque(data_servo_mul_different[1+3*i],TORQUE_ON);
+ my_Tor = Get_Torque(data_servo_mul_different[1+3*i]);
+ //wait_ms(5);
+ //pc.printf(" SET_TORQUE ");
+ }
+ }
+ for(i=0; i<nombre_servo; i++) {
+ Tension_inter = Get_Tension_actuelle(data_servo_mul_different[1+3*i]);
+ Tension = Tension_inter*0.074;
+ if(Tension <=6.60) {
+ coeffient_time = 6;
+ } else if(Tension <= 6.90) {
+ coeffient_time = 4;
+ } else if(Tension <= 7.10) {
+ coeffient_time = 2;
+ } else if(Tension > 7.10) {
+ coeffient_time = 1;
+ }
+ get_pos = getPos(data_servo_mul_different[1+3*i]);
+ pc.printf("PosiM=%d ",get_pos);
+ if((get_pos - position_servo_mul_different[i])>tolerance_en_position) {
+ 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
+ if (tab_tempo[i] > 254) tab_tempo[i] = 254;
+ data_servo_mul_different[2+3*i] = tab_tempo[i];
+ flag_correction = 1;
+ } else if((get_pos - position_servo_mul_different[i])<tolerance_en_position_negatif) {
+ tab_tempo[i]=(position_servo_mul_different[i] - get_pos)*0.084*coeffient_time+1;
+ if (tab_tempo[i] > 254) tab_tempo[i] = 254;
+ data_servo_mul_different[2+3*i] = tab_tempo[i];
+ flag_correction = 1;
+ }
+ }
+ if(flag_correction == 1) {
+ flag_correction = 0;
+ positionControl_Mul_ensemble_different_complex(nombre_servo,data_servo_mul_different, position_servo_mul_different);
+ pc.printf("Correction!\n");
+ }
+ break;
+ }
+}
+//---------------------fonction d'interruption de reception---------------------
+unsigned char flag_perdu_info = 0, indicateur = 0, Size_trame = 0, old_valueserial4 = 0;
+unsigned char char_receive_pc[100];
+unsigned char char_receive_serial4[100];
+unsigned char valueserial4=0;
+unsigned char valuepc=0,flag_seconde=0,flag_pc_receive=0,flag_serial4_receive=0;
+void receive_serial4()
+{
+ char_receive_serial4[valueserial4]=serial4.getc();
+ automate();
+}
+
+void Interrupt4_en(void){
+ serial4.attach(&receive_serial4,Serial::RxIrq);
+ }
+
+void automate()
+{
+ typedef enum {Attente,FF,Size,Data} type_etat;
+ static type_etat etat=Attente;
+ //pc.printf("coucou");
+
+//pc.printf("%d\r\n", char_receive_serial4[valueserial4]);
+
+ switch (etat) {
+ case Attente:
+ if(char_receive_serial4[0] == 0xFF)
+ {
+ etat = FF;
+ valueserial4 = 1;
+ }
+ break;
+ case FF:
+ if(char_receive_serial4[1] == 0xFF)
+ {
+ etat = Size;
+ valueserial4 = 2;
+ }
+ else
+ {
+ etat = Attente;
+ valueserial4 = 0;
+ flag_perdu_info = 1; //flag_perdu_info
+ }
+ break;
+ case Size:
+ if(char_receive_serial4[2] < 7)
+ {
+ etat = Attente;
+ valueserial4 = 0;
+ flag_perdu_info = 1; //flag_perdu_info
+ } else {
+ etat = Data;
+ old_valueserial4 = 2;
+ valueserial4 = 3;
+ }
+ Size_trame = char_receive_serial4[2];
+ break;
+
+ case Data:
+ if((valueserial4-2)==(Size_trame-3))
+ {
+ flag_serial4_receive = 1;
+ etat = Attente;
+ valueserial4 = 0;
+ } else {
+ valueserial4++;
+ }
+ break;
+
+ default:break;
+ }
+}
+
+//----------------xxxxx----fonction de fermture de serial-----------------------
+/*void N_Herkulex()
+{
+
+if(Sv != NULL)
+delete Sv;
+if(recevoir==2) {
+size_reponse = rx2[recevoir];
+}
+}*/
+//--------------2e fonction d'effacer les erreurs (pas utilisee)----------------
+int8_t clean_ERR(uint8_t id)
+{
+ uint8_t Cx[11]; //{0xFF,0xFF,0x0B,0xFD,0x03,0xC6,0x38,0x30,0x02,0x00,0x00};
+ uint8_t Cr[9]; //{0xFF,0xFF,0x09,0xFD,0x04,0xC2,0x3C,0x30,0x02};
+
+ Cx[0] = HEADER; // Packet Header (0xFF)
+ Cx[1] = HEADER; // Packet Header (0xFF)
+ Cx[2] = MIN_PACKET_SIZE + 4; // Packet Size
+ Cx[3] = id; // Servo ID
+ Cx[4] = CMD_RAM_WRITE; // Command Ram Write (0x03)
+ Cx[5] = 0xC6; // Checksum1
+ Cx[6] = 0x38; // Checksum2
+ Cx[7] = RAM_POSITION_FEEDFORWARD_1ST_GAIN; // Address
+ Cx[8] = BYTE2; // Length
+ Cx[9] = 0;
+ Cx[10]= 0;
+
+ Cr[0] = HEADER; // Packet Header (0xFF)
+ Cr[1] = HEADER; // Packet Header (0xFF)
+ Cr[2] = MIN_PACKET_SIZE + 2; // Packet Size
+ Cr[3] = id; // Servo ID
+ Cr[4] = CMD_RAM_READ; // Command Ram Read
+ Cr[5] = 0xC2; // Checksum1
+ Cr[6] = 0x3C; // Checksum2
+ Cr[7] = RAM_POSITION_FEEDFORWARD_1ST_GAIN; // Address
+ Cr[8] = BYTE2; // Length
+
+ size_reponse = 13;
+ for(uint8_t i = 0; i < 11 ; i++) {
+ while(serial4.writeable() == 0);
+ serial4.putc(Cx[i]);
+ }
+ wait_ms(0.3);
+ for(uint8_t i = 0; i < 9 ; i++) {
+ while(serial4.writeable() == 0);
+ serial4.putc(Cr[i]);
+ }
+ wait_ms(0.5);
+
+ uint8_t rxBuf[13];
+ if(flag_serial4_receive2) {
+ for(i2 = 0; i2<13; i2++) {
+ rxBuf[i2] = rx2[i2];
+ pc.printf(" %x",rx2[i2]);
+ }
+ flag_serial4_receive2=0;
+ }
+// Checksum1
+ uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE;
+ if (chksum1 != rxBuf[5]) {
+ return -1;
+ }
+// Checksum2
+ uint8_t chksum2 = (~rxBuf[5]&0xFE);
+ if (chksum2 != rxBuf[6]) {
+ return -1;
+ }
+ return 0;
+}
+//-------------------------fonction de transmission-----------------------------
+//comme j'ecrit cette partie dans chaque conction, cette fonction n'est pas souvent utilisee mais on l'utilise
+void txPacket(uint8_t packetSize, uint8_t* data)
+{
+ /*#ifdef HERKULEX_DEBUG
+ pc->printf("[TX]");
+ #endif
+ for(uint8_t i = 0; i < packetSize ; i++)
+ {
+ #ifdef HERKULEX_DEBUG
+ pc->printf("%02X ",data[i]);
+ #endif
+ txd->putc(data[i]);
+ }
+ #ifdef HERKULEX_DEBUG
+ pc->printf("\n");
+ #endif*/
+
+ for(uint8_t i = 0; i < packetSize ; i++)
+ {
+ while(serial4.writeable() == 0);
+ serial4.putc(data[i]);
+ wait_us(100);
+ }
+ wait_ms(TEMPO_R);
+}
+//----------------------------fonction de reception-----------------------------
+//comme j'ecrit cette partie dans chaque conction, cette fonction n'est pas souvent utilisee mais on l'utilise
+void rxPacket(uint8_t packetSize, uint8_t* data)
+{
+ /*#ifdef HERKULEX_DEBUG
+ pc->printf("[RX]");
+ #endif
+ for (uint8_t i=0; i < packetSize; i++)
+ {
+ data[i] = rxd->getc();
+ #ifdef HERKULEX_DEBUG
+ pc->printf("%02X ",data[i]);
+ #endif
+ }
+ #ifdef HERKULEX_DEBUG
+ pc->printf("\n");
+ #endif*/
+
+ if(flag_serial4_receive)
+ {
+ for(unsigned char i4=0;i4<Size_trame; i4++)
+ {
+ data[i4] = char_receive_serial4[i4];
+ pc.printf("%d ",(int)char_receive_serial4[i4]);
+ }
+ flag_serial4_receive=0;
+ valueserial4=0;
+ }
+}
+//----------------------fonction d'effacer les erreurs--------------------------
+void clear(uint8_t id)
+{
+ uint8_t txBuf[11];
+ txBuf[0] = HEADER; // Packet Header (0xFF)
+ txBuf[1] = HEADER; // Packet Header (0xFF)
+ txBuf[2] = MIN_PACKET_SIZE + 4; // Packet Size
+ txBuf[3] = id; // Servo ID
+ txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03)
+ txBuf[5] = 0; // Checksum1
+ txBuf[6] = 0; // Checksum2
+ txBuf[7] = RAM_STATUS_ERROR; // Address
+ txBuf[8] = BYTE2; // Length
+ txBuf[9] = 0; // Clear RAM_STATUS_ERROR
+ txBuf[10]= 0; // Clear RAM_STATUS_DETAIL
+ // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
+ // Checksum2 = (~Checksum1)&0xFE
+ txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]) & 0xFE;
+ txBuf[6] = (~txBuf[5])&0xFE;
+ // send packet (mbed -> herkulex)
+ for(uint8_t i = 0; i < 11 ; i++)
+ {
+ while(serial4.writeable() == 0);
+ serial4.putc(txBuf[i]);
+ wait_us(100);
+ }
+ wait_ms(TEMPO_R);
+}
+//----------------fonction de mis a jour le couple de servo---------------------
+void setTorque(uint8_t id, uint8_t cmdTorue)
+{
+ uint8_t txBuf[10];
+ txBuf[0] = HEADER; // Packet Header (0xFF)
+ txBuf[1] = HEADER; // Packet Header (0xFF)
+ txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size
+ txBuf[3] = id; // Servo ID
+ txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03)
+ txBuf[5] = 0; // Checksum1
+ txBuf[6] = 0; // Checksum2
+ txBuf[7] = RAM_TORQUE_CONTROL; // Address
+ txBuf[8] = BYTE1; // Length
+ txBuf[9] = cmdTorue; // Torque ON
+// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
+// Checksum2 = (~Checksum1)&0xFE
+ txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE;
+ txBuf[6] = (~txBuf[5])&0xFE;
+// send packet (mbed -> herkulex)
+
+ for(uint8_t i = 0; i < 10 ; i++)
+ {
+ while(serial4.writeable() == 0);
+ serial4.putc(txBuf[i]);
+ wait_us(100);
+ }
+ wait_ms(TEMPO_R);
+}
+//-------------fonction de controle de position pour un seul servo--------------
+void positionControl(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED)
+{
+ float tempo=0;
+ //if (position > 1023) return; //1002-21
+ if (playtime > 254) playtime = 254; //return; //1-254 == 11.2ms-2.844sec.
+ tempo=playtime*0.012;
+ pos_ID = id;
+ uint8_t txBuf[12];
+ etat = pos;
+ pos_position = position;
+ pos_time = playtime;
+ pos_led = setLED;
+ txBuf[0] = HEADER; // Packet Header (0xFF)
+ txBuf[1] = HEADER; // Packet Header (0xFF)
+ txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size
+ //txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
+ txBuf[3] = id; // pID is total number of servos in the network (0 ~ 253)
+ txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
+ txBuf[5] = 0; // Checksum1
+ txBuf[6] = 0; // Checksum2
+ txBuf[7] = playtime; // Playtime
+ txBuf[8] = position & 0x00FF; // Position (LSB, Least Significant Bit)
+ txBuf[9] =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
+ txBuf[10] = POS_MODE | setLED; // Pos Mode and LED on/off
+ txBuf[11] = id; // Servo ID
+// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
+// Checksum2 = (~Checksum1)&0xFE
+ txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
+ txBuf[6] = (~txBuf[5])&0xFE;
+// send packet (mbed -> herkulex)
+//txPacket(12, txBuf);
+ for(uint8_t i = 0; i < 12 ; i++)
+ {
+ while(serial4.writeable() == 0);
+ serial4.putc(txBuf[i]);
+ wait_us(100);
+ }
+ wait(tempo);
+ wait_ms(TEMPO_R);
+}
+//-------------fonction de controle de vitesse pour un seul servo---------------
+//Comme je n'ai pas utilise cette fonction, je ne l'ai pas encore change
+void velocityControl(uint8_t id, int16_t speed, uint8_t setLED)
+{
+ if (speed > 1023 || speed < -1023) return;
+ uint8_t txBuf[12];
+ txBuf[0] = HEADER; // Packet Header (0xFF)
+ txBuf[1] = HEADER; // Packet Header (0xFF)
+ txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size
+ txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
+ txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
+ txBuf[5] = 0; // Checksum1
+ txBuf[6] = 0; // Checksum2
+ txBuf[7] = 0; // Playtime, unmeaningful in turn mode
+ txBuf[8] = speed & 0x00FF; // Speed (LSB, Least Significant Bit)
+ txBuf[9] =(speed & 0xFF00) >> 8; // Speed (MSB, Most Significanct Bit)
+ txBuf[10] = TURN_MODE | setLED; // Turn Mode and LED on/off
+ txBuf[11] = id; // Servo ID
+// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
+// Checksum2 = (~Checksum1)&0xFE
+ txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
+ txBuf[6] = (~txBuf[5])&0xFE;
+// send packet (mbed -> herkulex)
+ txPacket(12, txBuf);
+ wait_ms(TEMPO_R);
+}
+//--------------------fonction d'acquis d'etat d'un servo-----------------------
+int8_t getStatus(uint8_t id)
+{
+ uint8_t status;
+ uint8_t txBuf[7];
+ size_reponse = 9;
+
+ txBuf[0] = HEADER; // Packet Header (0xFF)
+ txBuf[1] = HEADER; // Packet Header (0xFF)
+ txBuf[2] = MIN_PACKET_SIZE; // Packet Size
+ txBuf[3] = id; // Servo ID
+ txBuf[4] = CMD_STAT; // Status Error, Status Detail request
+// Check Sum1 and Check Sum2
+ txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]) & 0xFE;
+ txBuf[6] = (~txBuf[5])&0xFE;
+
+// send packet (mbed -> herkulex)
+ for(uint8_t i = 0; i < 7 ; i++)
+ {
+ while(serial4.writeable() == 0);
+ serial4.putc(txBuf[i]);
+ wait_us(100);
+ }
+
+// send packet (mbed -> herkulex)
+ uint8_t rxBuf[9];
+ wait_ms(TEMPO_R);
+ if(flag_serial4_receive)
+ {
+ for(unsigned char i4=0;i4<Size_trame; i4++)
+ {
+ rxBuf[i4] = char_receive_serial4[i4];
+ pc.printf("%d ",(int)char_receive_serial4[i4]);
+ }
+ flag_serial4_receive=0;
+ valueserial4=0;
+ }
+
+
+// Checksum1
+ uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]) & 0xFE;
+ if (chksum1 != rxBuf[5]) {
+
+ return -1;
+ }
+// Checksum2
+ uint8_t chksum2 = (~rxBuf[5]&0xFE);
+ if (chksum2 != rxBuf[6]) {
+
+ return -1;
+ }
+ status = rxBuf[7]; // Status Error
+//status = rxBuf[8]; // Status Detail
+
+
+ return status;
+}
+//------------------fonction de lire la position actuelle-----------------------
+int16_t getPos(uint8_t id)
+{
+ uint16_t position = 0;
+ uint8_t txBuf[9];
+ size_reponse = 13;
+
+ txBuf[0] = HEADER; // Packet Header (0xFF)
+ txBuf[1] = HEADER; // Packet Header (0xFF)
+ txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
+ txBuf[3] = id; // Servo ID
+ txBuf[4] = CMD_RAM_READ; // Command Ram Read
+ txBuf[5] = 0; // Checksum1
+ txBuf[6] = 0; // Checksum2
+ txBuf[7] = RAM_CALIBRATED_POSITION; // Address
+ txBuf[8] = BYTE2;
+// Check Sum1 and Check Sum2
+ txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
+ txBuf[6] = (~txBuf[5])&0xFE;
+
+ for(uint8_t i = 0; i < 9 ; i++)
+ {
+ while(serial4.writeable() == 0);
+ serial4.putc(txBuf[i]);
+ wait_us(100);
+ }
+
+// send packet (mbed -> herkulex)
+ uint8_t rxBuf[13];
+ wait_ms(TEMPO_R);
+ if(flag_serial4_receive)
+ {
+ for(unsigned char i4=0;i4<Size_trame; i4++)
+ {
+ rxBuf[i4] = char_receive_serial4[i4];
+ pc.printf("%d ",(int)char_receive_serial4[i4]);
+ }
+ flag_serial4_receive=0;
+ valueserial4=0;
+ }
+
+// Checksum1
+ uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE;
+ if (chksum1 != rxBuf[5]) {
+ /*#ifdef HERKULEX_DEBUG
+ pc->printf("Checksum1 fault\n");
+ #endif*/
+ return -1;
+ }
+// Checksum2
+ uint8_t chksum2 = (~rxBuf[5]&0xFE);
+ if (chksum2 != rxBuf[6]) {
+ /* #ifdef HERKULEX_DEBUG
+ pc->printf("Checksum2 fault\n");
+ #endif*/
+ return -1;
+ }
+ position = ((rxBuf[10]&0x03)<<8) | rxBuf[9];
+ /* #ifdef HERKULEX_DEBUG
+ pc->printf("position = %04X(%d)\n", position, position);
+ #endif*/
+//}
+ return position;
+}
+//---------------fonction d'acquis d'etat de couple d'un servo------------------
+int8_t Get_Torque(int8_t id)
+{
+ uint8_t txBuf[9];
+ int8_t Tor = 0;
+
+ uint8_t iv=0;
+ for(iv=0; iv<20; iv++) {
+ rx2[iv] = 0;
+ }
+
+ txBuf[0] = HEADER; // Packet Header (0xFF)
+ txBuf[1] = HEADER; // Packet Header (0xFF)
+ txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
+ txBuf[3] = id; // Servo ID
+ txBuf[4] = CMD_RAM_READ; // Command Ram Read
+ txBuf[5] = 0; // Checksum1
+ txBuf[6] = 0; // Checksum2
+ txBuf[7] = RAM_TORQUE_CONTROL;
+ txBuf[8] = BYTE1; // Length
+ // Check Sum1 and Check Sum2
+ txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
+ txBuf[6] = (~txBuf[5])&0xFE;
+
+ pc.printf(" Torque ");
+
+ for(uint8_t i = 0; i < 9 ; i++)
+ {
+ while(serial4.writeable() == 0);
+ serial4.putc(txBuf[i]);
+ wait_us(100);
+ }
+
+// send packet (mbed -> herkulex)
+ uint8_t rxBuf[12];
+ //wait_ms(3);
+ wait_ms(TEMPO_R);
+ if(flag_serial4_receive)
+ {
+ for(unsigned char i4=0;i4<Size_trame; i4++)
+ {
+ rxBuf[i4] = char_receive_serial4[i4];
+ pc.printf("%d ",(int)char_receive_serial4[i4]);
+ }
+ flag_serial4_receive=0;
+ valueserial4=0;
+ }
+
+ uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]) & 0xFE;
+ if (chksum1 != rxBuf[5]) {
+ /*#ifdef HERKULEX_DEBUG
+ pc->printf("Checksum1 fault\n");
+ #endif*/
+ recevoir=0;
+ return -1;
+ }
+// Checksum2
+ uint8_t chksum2 = (~rxBuf[5]&0xFE);
+ if (chksum2 != rxBuf[6]) {
+ /* #ifdef HERKULEX_DEBUG
+ pc->printf("Checksum2 fault\n");
+ #endif*/
+ recevoir=0;
+ return -1;
+ }
+ Tor = rxBuf[9];
+ /* #ifdef HERKULEX_DEBUG
+ pc->printf("position = %04X(%d)\n", position, position);
+ #endif*/
+//}
+ return Tor;
+}
+//---------------fonction pour lire le temperature max pour un servo------------
+int8_t Get_Temperature_MAX(int8_t id)
+{
+
+ uint8_t txBuf[9];
+ int8_t tempeMAX = 0;
+
+ txBuf[0] = HEADER; // Packet Header (0xFF)
+ txBuf[1] = HEADER; // Packet Header (0xFF)
+ txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
+ txBuf[3] = id; // Servo ID
+ txBuf[4] = CMD_RAM_READ; // Command Ram Read
+ txBuf[5] = 0; // Checksum1
+ txBuf[6] = 0; // Checksum2
+ txBuf[7] = RAM_MAX_TEMPERATURE;
+ txBuf[8] = BYTE1; // Length
+ // Check Sum1 and Check Sum2
+ txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
+ txBuf[6] = (~txBuf[5])&0xFE;
+
+ pc.printf(" tempeMAX ");
+
+ for(uint8_t i = 0; i < 9 ; i++)
+ {
+ while(serial4.writeable() == 0);
+ serial4.putc(txBuf[i]);
+ wait_us(100);
+ }
+
+// send packet (mbed -> herkulex)
+ uint8_t rxBuf[12];
+ //wait_ms(3);
+ wait_ms(TEMPO_R);
+ if(flag_serial4_receive)
+ {
+ for(unsigned char i4=0;i4<Size_trame; i4++)
+ {
+ rxBuf[i4] = char_receive_serial4[i4];
+ pc.printf("%d ",(int)char_receive_serial4[i4]);
+ }
+ flag_serial4_receive=0;
+ valueserial4=0;
+ }
+
+
+ uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]) & 0xFE;
+ if (chksum1 != rxBuf[5]) {
+ /*#ifdef HERKULEX_DEBUG
+ pc->printf("Checksum1 fault\n");
+ #endif*/
+ recevoir=0;
+ return -1;
+ }
+// Checksum2
+ uint8_t chksum2 = (~rxBuf[5]&0xFE);
+ if (chksum2 != rxBuf[6]) {
+ /* #ifdef HERKULEX_DEBUG
+ pc->printf("Checksum2 fault\n");
+ #endif*/
+ recevoir=0;
+ return -1;
+ }
+ tempeMAX = rxBuf[9];
+ /* #ifdef HERKULEX_DEBUG
+ pc->printf("position = %04X(%d)\n", position, position);
+ #endif*/
+//}
+ return tempeMAX;
+}
+//--------fonction de controle de position pour deux servo(same playtime)-------
+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)
+{
+ float tempo=0;
+//if (position > 1023) return; //1002-21
+ if (playtime > 254) return; //1-254 == 11.2ms-2.844sec.
+ tempo=playtime*0.012;
+ uint8_t txBuf[16];
+ etat = pos;
+ pos_position = position;
+ pos_time = playtime;
+ pos_led = setLED;
+ txBuf[0] = HEADER; // Packet Header (0xFF)
+ txBuf[1] = HEADER; // Packet Header (0xFF)
+ txBuf[2] = MIN_PACKET_SIZE + 9; // Packet Size
+ //txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
+ txBuf[3] = 254; // broadcast ID
+ txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
+ txBuf[5] = 0; // Checksum1
+ txBuf[6] = 0; // Checksum2
+ txBuf[7] = playtime; // Playtime
+ txBuf[8] = position & 0x00FF; // Position (LSB, Least Significant Bit)
+ txBuf[9] =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
+ txBuf[10] = POS_MODE | setLED; // Pos Mode and LED on/off
+ txBuf[11] = id; // Servo ID
+ txBuf[12] = position2 & 0x00FF; // Position (LSB, Least Significant Bit)
+ txBuf[13] =(position2 & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
+ txBuf[14] = POS_MODE | setLED2; // Pos Mode and LED on/off
+ txBuf[15] = id2; // Servo ID
+// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
+// Checksum2 = (~Checksum1)&0xFE
+ 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;
+ txBuf[6] = (~txBuf[5])&0xFE;
+// send packet (mbed -> herkulex)
+
+ for(uint8_t i = 0; i < 16 ; i++)
+ {
+ while(serial4.writeable() == 0);
+ serial4.putc(txBuf[i]);
+ wait_us(100);
+ }
+ wait(tempo);
+ wait_ms(TEMPO_R);
+}
+//-----fonction de controle de position pour deux servo(different playtime)----- //a changer...
+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)
+{
+ float tempo=0;
+//if (position > 1023) return; //1002-21
+ if (playtime > 254) playtime = 254; //return; //1-254 == 11.2ms-2.844sec.
+ if(playtime>playtime2) {
+ tempo=playtime*0.012;
+ } else if(playtime<playtime2) {
+ tempo=playtime2*0.012;
+ }
+ uint8_t txBuf[17];
+ etat = pos;
+ pos_position = position;
+ pos_time = playtime;
+ pos_led = setLED;
+ txBuf[0] = HEADER; // Packet Header (0xFF)
+ txBuf[1] = HEADER; // Packet Header (0xFF)
+ txBuf[2] = MIN_PACKET_SIZE + 9; // Packet Size
+ //txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
+ txBuf[3] = 254; // broadcast ID
+ txBuf[4] = CMD_I_JOG; // Command I JOG
+ txBuf[5] = 0; // Checksum1
+ txBuf[6] = 0; // Checksum2
+ txBuf[7] = position & 0x00FF; // Position (LSB, Least Significant Bit)
+ txBuf[8] =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
+ txBuf[9] = POS_MODE | setLED; // Pos Mode and LED on/off
+ txBuf[10] = id; // Servo ID
+ txBuf[11] = playtime; // Playtime
+ txBuf[12] = position2 & 0x00FF; // Position (LSB, Least Significant Bit)
+ txBuf[13] =(position2 & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
+ txBuf[14] = POS_MODE | setLED2; // Pos Mode and LED on/off
+ txBuf[15] = id2; // Servo ID
+ txBuf[16] = playtime2; // Playtime
+// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
+// Checksum2 = (~Checksum1)&0xFE
+ 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;
+ txBuf[6] = (~txBuf[5])&0xFE;
+// send packet (mbed -> herkulex)
+//txPacket(12, txBuf);
+
+ for(uint8_t i = 0; i < 17 ; i++)
+ {
+ while(serial4.writeable() == 0);
+ serial4.putc(txBuf[i]);
+ wait_us(100);
+ }
+ wait(tempo);
+ wait_ms(TEMPO_R);
+}
+//-----fonction de controle de position pour plusieurs servo(same playtime)-----
+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
+{
+ float tempo=0;
+ uint8_t taille = 0,i = 0,idata = 0, ipos = 0;
+ //if (position > 1023) return; //1002-21
+ if (playtime > 254) return; //1-254 == 11.2ms-2.844sec.
+ tempo=playtime*0.012;
+ taille = 7 + 1 + 4 * nb_servo;
+ nombre_servo = nb_servo;
+ pos_time = playtime;
+ uint8_t txBuf[taille];
+ etat = pos_mul_complex;
+
+ txBuf[0] = HEADER; // Packet Header (0xFF)
+ txBuf[1] = HEADER; // Packet Header (0xFF)
+ txBuf[2] = MIN_PACKET_SIZE + 1 + 4 * nb_servo; // Packet Size
+ //txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
+ txBuf[3] = 254; // broadcast ID
+ txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
+ txBuf[5] = 0; // Checksum1
+ txBuf[6] = 0; // Checksum2
+ txBuf[7] = playtime; // Playtime
+
+ for(i=0; i<nb_servo; i++) {
+ txBuf[8+i*4] = pos[ipos] & 0x00FF; // Position (LSB, Least Significant Bit)
+ txBuf[9+i*4] =(pos[ipos] & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
+ position_servo_mul[ipos] = pos[ipos];
+ ipos++;
+ txBuf[10+i*4] = POS_MODE | data[idata]; // Pos Mode and LED on/off
+ data_servo_mul[idata] = data[idata];
+ idata++;
+ txBuf[11+i*4] = data[idata]; // Servo ID
+ data_servo_mul[idata] = data[idata];
+ idata++;
+ }
+
+// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
+// Checksum2 = (~Checksum1)&0xFE
+ txBuf[5] = txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7];
+
+ for(i=1; i<(taille-7); i++) {
+ txBuf[5]=txBuf[5]^txBuf[7+i];
+ }
+ txBuf[5] = txBuf[5]& 0xFE;
+ txBuf[6] = (~txBuf[5])&0xFE;
+
+// send packet (mbed -> herkulex)
+
+ for(uint8_t i = 0; i < taille ; i++)
+ {
+ while(serial4.writeable() == 0);
+ serial4.putc(txBuf[i]);
+ wait_us(100);
+ }
+ //wait(tempo);
+ //wait_ms(TEMPO_R);
+}
+//--fonction de controle de position pour plusieurs servo(different playtime)---
+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
+{
+ float tempo=0;
+ uint8_t Max_playtime = 0;
+ uint8_t taille = 0,i = 0,idata = 0, ipos = 0,iplay_time = 0;
+ //if (position > 1023) return; //1002-21
+ //if (playtime > 254) return; //1-254 == 11.2ms-2.844sec.
+
+ for(iplay_time=0; iplay_time<nb_servo; iplay_time++) {
+ if(Max_playtime<data[2+3*iplay_time]) {
+ Max_playtime=data[2+3*iplay_time];
+ }
+ }
+ tempo=Max_playtime*0.012;
+ taille = 7 + 5 * nb_servo;
+ nombre_servo = nb_servo;
+ uint8_t txBuf[taille];
+ etat = pos_mul_complex_different;
+
+ txBuf[0] = HEADER; // Packet Header (0xFF)
+ txBuf[1] = HEADER; // Packet Header (0xFF)
+ txBuf[2] = MIN_PACKET_SIZE + 5 * nb_servo; // Packet Size
+ //txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
+ txBuf[3] = 254; // broadcast ID
+ txBuf[4] = CMD_I_JOG; // Command I JOG (0x06)
+ txBuf[5] = 0; // Checksum1
+ txBuf[6] = 0; // Checksum2
+
+ for(i=0; i<nb_servo; i++) {
+ txBuf[7+i*5] = pos[ipos] & 0x00FF; // Position (LSB, Least Significant Bit)
+ txBuf[8+i*5] =(pos[ipos] & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
+ position_servo_mul_different[ipos] = pos[ipos];
+ ipos++;
+ txBuf[9+i*5] = POS_MODE | data[idata]; // Pos Mode and LED on/off
+ data_servo_mul_different[idata] = data[idata];
+ idata++;
+ txBuf[10+i*5] = data[idata]; // Servo ID
+ data_servo_mul_different[idata] = data[idata];
+ idata++;
+ txBuf[11+i*5] = data[idata]; // Playtime
+ data_servo_mul_different[idata] = data[idata];
+ idata++;
+ }
+
+// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
+// Checksum2 = (~Checksum1)&0xFE
+ txBuf[5] = txBuf[2]^txBuf[3]^txBuf[4];
+
+ for(i=1; i<(taille-6); i++) {
+ txBuf[5]=txBuf[5]^txBuf[6+i];
+ }
+ txBuf[5] = txBuf[5]& 0xFE;
+ txBuf[6] = (~txBuf[5])&0xFE;
+
+// send packet (mbed -> herkulex)
+
+ for(uint8_t i = 0; i < taille ; i++)
+ {
+ while(serial4.writeable() == 0);
+ serial4.putc(txBuf[i]);
+ wait_us(100);
+ }
+ wait(tempo);
+ wait_ms(TEMPO_R);
+}
+//---------------fonction pour lire la tension min pour un servo----------------
+int8_t Get_Tension_MIN(int8_t id)
+{
+
+ uint8_t txBuf[9];
+ int8_t tensionMIN = 0;
+
+ txBuf[0] = HEADER; // Packet Header (0xFF)
+ txBuf[1] = HEADER; // Packet Header (0xFF)
+ txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
+ txBuf[3] = id; // Servo ID
+ txBuf[4] = CMD_RAM_READ; // Command Ram Read
+ txBuf[5] = 0; // Checksum1
+ txBuf[6] = 0; // Checksum2
+ txBuf[7] = RAM_MIN_VOLTAGE;
+ txBuf[8] = BYTE1; // Length
+ // Check Sum1 and Check Sum2
+ txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
+ txBuf[6] = (~txBuf[5])&0xFE;
+
+ pc.printf(" tensionMIN ");
+
+ for(uint8_t i = 0; i < 9 ; i++)
+ {
+ while(serial4.writeable() == 0);
+ serial4.putc(txBuf[i]);
+ wait_us(100);
+ }
+
+// send packet (mbed -> herkulex)
+ uint8_t rxBuf[12];
+ //wait_ms(3);
+ wait_ms(TEMPO_R);
+ if(flag_serial4_receive)
+ {
+ for(unsigned char i4=0;i4<Size_trame; i4++)
+ {
+ rxBuf[i4] = char_receive_serial4[i4];
+ pc.printf("%d ",(int)char_receive_serial4[i4]);
+ }
+ flag_serial4_receive=0;
+ valueserial4=0;
+ }
+
+ uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]) & 0xFE;
+ if (chksum1 != rxBuf[5]) {
+ /*#ifdef HERKULEX_DEBUG
+ pc->printf("Checksum1 fault\n");
+ #endif*/
+ recevoir=0;
+ return -1;
+ }
+// Checksum2
+ uint8_t chksum2 = (~rxBuf[5]&0xFE);
+ if (chksum2 != rxBuf[6]) {
+ /* #ifdef HERKULEX_DEBUG
+ pc->printf("Checksum2 fault\n");
+ #endif*/
+ recevoir=0;
+ return -1;
+ }
+ tensionMIN = rxBuf[9];
+ /* #ifdef HERKULEX_DEBUG
+ pc->printf("position = %04X(%d)\n", position, position);
+ #endif*/
+//}
+ return tensionMIN;
+}
+//-------------fonction pour controle la tension min pour un servo--------------
+void Set_Tension_MIN(int8_t id,uint8_t Tension_Min)
+{
+ uint8_t txBuf[10];
+
+ txBuf[0] = HEADER; // Packet Header (0xFF)
+ txBuf[1] = HEADER; // Packet Header (0xFF)
+ txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size
+ txBuf[3] = id; // Servo ID
+ txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03)
+ txBuf[5] = 0; // Checksum1
+ txBuf[6] = 0; // Checksum2
+ txBuf[7] = RAM_MIN_VOLTAGE;
+ txBuf[8] = BYTE1; // Length
+ txBuf[9] = Tension_Min;
+ // Check Sum1 and Check Sum2
+ txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE;
+ txBuf[6] = (~txBuf[5])&0xFE;
+
+ pc.printf(" tensionMIN ");
+ for(uint8_t i = 0; i < 10 ; i++)
+ {
+ while(serial4.writeable() == 0);
+ serial4.putc(txBuf[i]);
+ wait_us(100);
+ }
+ //wait_ms(3);
+ wait_ms(TEMPO_R);
+}
+//------------fonction pour lire la tension acttuelle pour un servo-------------
+int8_t Get_Tension_actuelle(int8_t id)
+{
+
+ uint8_t txBuf[9];
+ int8_t tension = 0;
+
+ txBuf[0] = HEADER; // Packet Header (0xFF)
+ txBuf[1] = HEADER; // Packet Header (0xFF)
+ txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
+ txBuf[3] = id; // Servo ID
+ txBuf[4] = CMD_RAM_READ; // Command Ram Read (0x03)
+ txBuf[5] = 0; // Checksum1
+ txBuf[6] = 0; // Checksum2
+ txBuf[7] = RAM_VOLTAGE;
+ txBuf[8] = BYTE2; // Length
+ // Check Sum1 and Check Sum2
+ txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
+ txBuf[6] = (~txBuf[5])&0xFE;
+
+ pc.printf(" tension ");
+
+ for(uint8_t i = 0; i < 9 ; i++)
+ {
+ while(serial4.writeable() == 0);
+ serial4.putc(txBuf[i]);
+ wait_us(100);
+ }
+
+// send packet (mbed -> herkulex)
+ uint8_t rxBuf[13];
+ //wait_ms(3);
+ wait_ms(TEMPO_R);
+ if(flag_serial4_receive)
+ {
+ for(unsigned char i4=0;i4<Size_trame; i4++)
+ {
+ rxBuf[i4] = char_receive_serial4[i4];
+ pc.printf("%d ",(int)char_receive_serial4[i4]);
+ }
+ flag_serial4_receive=0;
+ valueserial4=0;
+ }
+
+
+ uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE;
+ if (chksum1 != rxBuf[5]) {
+ /*#ifdef HERKULEX_DEBUG
+ pc->printf("Checksum1 fault\n");
+ #endif*/
+ recevoir=0;
+ return -1;
+ }
+// Checksum2
+ uint8_t chksum2 = (~rxBuf[5]&0xFE);
+ if (chksum2 != rxBuf[6]) {
+ /* #ifdef HERKULEX_DEBUG
+ pc->printf("Checksum2 fault\n");
+ #endif*/
+ recevoir=0;
+ return -1;
+ }
+ tension = rxBuf[9];
+ /* #ifdef HERKULEX_DEBUG
+ pc->printf("position = %04X(%d)\n", position, position);
+ #endif*/
+//}
+ return tension;
+}
+
+//-----------------deplacement des cubes---------------------
+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)
+{
+ PwmOut Pompe(Pompe_pin);
+ switch(poi_init) {
+ case 5: //verifie X
+ //uint8_t servo1[6] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3}; // uint8_t setLED, uint8_t id
+ //uint8_t servo2[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3};
+ uint8_t servo3[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4};
+ uint8_t servo4[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo5[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+ //uint16_t position1[3] = {512, 236, 236};
+ //uint16_t position2[3] = {512, 512, 788};
+ uint16_t position3[4] = {374, 926, 788, Pos_Engrenage_centre};
+ uint16_t position6[4] = {481, 950, 714, Pos_Engrenage_centre};
+ uint16_t position4[4] = {650, 952, 511, Pos_Engrenage_centre};
+ uint16_t position5[4] = {451, 944, 720, Pos_Engrenage_centre};
+
+ /*positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo1, position1);
+ verifacation();
+ positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo2, position2);
+ verifacation();*/
+ positionControl_Mul_ensemble_complex(4,V_b,servo3, position3);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_b,servo3, position6);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_h,servo4, position4);
+ verifacation();
+ wait(0.3);
+ Pompe = 1;
+ wait(0.1);
+ positionControl_Mul_ensemble_complex(4,V_m,servo5, position5);
+ verifacation();
+
+ break;
+ case 3: //verifie X
+ //uint8_t servo31[6] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3}; // uint8_t setLED, uint8_t id
+ //uint8_t servo32[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3};
+ uint8_t servo33[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4};
+ uint8_t servo34[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, BLED_ON, ID4};
+ uint8_t servo35[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, BLED_ON, ID4};
+ //uint16_t position31[3] = {512, 236, 236};
+ //uint16_t position32[3] = {512, 512, 788};
+ uint16_t position33[4] = {374, 926, 788, Pos_Engrenage_centre};
+ uint16_t position34[4] = {673, 837, 595, Pos_Engrenage_centre}; //647, 842, 604
+ uint16_t position36[4] = {547, 845, 702, Pos_Engrenage_centre};
+ uint16_t position35[4] = {451, 944, 720, Pos_Engrenage_centre};
+
+ //positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo31, position31);
+ //verifacation();
+ //positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo32, position32);
+ //verifacation();
+ positionControl_Mul_ensemble_complex(4,V_b,servo33, position33);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_h,servo34, position34);
+ verifacation();
+ wait(0.3);
+ Pompe = 1;
+ wait(0.1);
+ positionControl_Mul_ensemble_complex(4,V_m,servo33, position36);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_m,servo35, position35);
+ verifacation();
+
+ break;
+ case 2:
+ //uint8_t servo20[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3};
+ //uint8_t servo21[6] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3}; // uint8_t setLED, uint8_t id
+ //uint8_t servo22[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3};
+ uint8_t servo23[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4};
+ uint8_t servo24[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo25[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+ //uint16_t position20[3] = {512, 236, 236};
+ //uint16_t position21[3] = {512, 236, 236};
+ //uint16_t position22[3] = {512, 512, 788};
+ uint16_t position23[4] = {374, 926, 788, Pos_Engrenage_gauche};
+ uint16_t position24[4] = {673, 837, 595, Pos_Engrenage_gauche};
+ uint16_t position25[4] = {451, 944, 720, Pos_Engrenage_gauche};
+
+ /*positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo20, position20);
+ verifacation();
+ wait(0.2);
+ positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo21, position21);
+ verifacation();
+ positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo22, position22);
+ verifacation();*/
+ positionControl_Mul_ensemble_complex(4,V_b,servo23, position23);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_h,servo24, position24);
+ verifacation();
+ wait(0.3);
+ Pompe = 1;
+ wait(0.1);
+ positionControl_Mul_ensemble_complex(4,V_m,servo25, position25);
+ verifacation();
+
+ break;
+ case 4:
+ //uint8_t servo40[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3};
+ //uint8_t servo41[6] = {GLED_ON, ID4, BLED_ON, ID2, GLED_ON, ID3}; // uint8_t setLED, uint8_t id
+ //uint8_t servo42[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3};
+ uint8_t servo43[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4};
+ uint8_t servo44[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo45[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+ //uint16_t position40[3] = {512, 236, 236};
+ //uint16_t position41[3] = {512, 236, 236};
+ //uint16_t position42[3] = {512, 512, 788};
+ uint16_t position43[4] = {374, 926, 788, Pos_Engrenage_droit};
+ uint16_t position44[4] = {673, 837, 595, Pos_Engrenage_droit};
+ uint16_t position45[4] = {451, 944, 720, Pos_Engrenage_droit};
+
+ /*positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo40, position40);
+ verifacation();
+ wait(0.2);
+ positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo41, position41);
+ verifacation();
+ positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo42, position42);
+ verifacation();*/
+ positionControl_Mul_ensemble_complex(4,V_b,servo43, position43);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_h,servo44, position44);
+ verifacation();
+ wait(0.3);
+ Pompe = 1;
+ wait(0.1);
+ positionControl_Mul_ensemble_complex(4,V_m,servo45, position45);
+ verifacation();
+
+ break;
+ case 1: //verifie X
+ //uint8_t servo11[6] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3}; // uint8_t setLED, uint8_t id
+ //uint8_t servo12[6] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3};
+ uint8_t servo13[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4};
+ uint8_t servo14[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, BLED_ON, ID4};
+ uint8_t servo15[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, BLED_ON, ID4};
+ uint8_t servo16[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, BLED_ON, ID4};
+ //uint16_t position11[3] = {512, 236, 236};
+ //uint16_t position12[3] = {512, 512, 788};
+ uint16_t position13[4] = {374, 926, 788, Pos_Engrenage_centre};
+ uint16_t position17[4] = {658, 657, 790, Pos_Engrenage_centre};
+ uint16_t position14[4] = {731, 675, 695, Pos_Engrenage_centre};
+ uint16_t position15[4] = {658, 657, 790, Pos_Engrenage_centre};
+ uint16_t position16[4] = {417, 885, 796, Pos_Engrenage_centre};
+
+
+ //positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo11, position11);
+ //verifacation();
+ //positionControl_Mul_ensemble_complex(nombre_servomoteur,60,servo12, position12);
+ //verifacation();
+ positionControl_Mul_ensemble_complex(4,V_b,servo13, position13);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_m,servo13, position17);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_h,servo14, position14);
+ verifacation();
+ wait(0.3);
+ Pompe = 1;
+ wait(0.1);
+ positionControl_Mul_ensemble_complex(4,V_h,servo15, position15);
+ verifacation();
+ //wait(2);
+ positionControl_Mul_ensemble_complex(4,V_h,servo16, position16);
+ verifacation();
+
+ break;
+
+ /*case 22:
+ uint8_t servo221[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
+ uint8_t servo222[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo223[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo224[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+
+ uint16_t position221[4] = {452, 801, 849, Pos_Engrenage_gauche};
+ uint16_t position222[4] = {535, 768, 805, Pos_Engrenage_gauche};
+ uint16_t position223[4] = {553, 819, 733, Pos_Engrenage_gauche};
+ uint16_t position224[4] = {395, 864, 832, Pos_Engrenage_gauche};
+
+ positionControl_Mul_ensemble_complex(4,V_m,servo221, position221);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_h,servo222, position222);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_b,servo223, position223);
+ verifacation();
+ wait(0.5);
+ Pompe = 1;
+ wait(0.5);
+ positionControl_Mul_ensemble_complex(4,V_b,servo224, position224);
+ verifacation();
+
+ break;
+
+ case 42:
+ uint8_t servo421[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
+ uint8_t servo422[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo423[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo424[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+
+ uint16_t position421[4] = {452, 801, 849, Pos_Engrenage_droit};
+ uint16_t position422[4] = {535, 768, 805, Pos_Engrenage_droit};
+ uint16_t position423[4] = {553, 819, 733, Pos_Engrenage_droit};
+ uint16_t position424[4] = {395, 864, 832, Pos_Engrenage_droit};
+
+ positionControl_Mul_ensemble_complex(4,V_m,servo421, position421);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_h,servo422, position422);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_b,servo423, position423);
+ verifacation();
+ wait(0.5);
+ Pompe = 1;
+ wait(0.5);
+ positionControl_Mul_ensemble_complex(4,V_b,servo424, position424);
+ verifacation();
+
+ break;*/
+ }
+
+ switch(poi_fini) {
+ /*case 11:
+ uint8_t servo101[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
+ uint8_t servo102[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo103[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo104[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+
+ uint16_t position101[4] = {663, 615, 820, Pos_Engrenage_centre};
+ uint16_t position102[4] = {742, 542, 819, Pos_Engrenage_centre};
+ uint16_t position103[4] = {715, 660, 719, Pos_Engrenage_centre};
+ uint16_t position104[4] = {418, 885, 789, Pos_Engrenage_centre};
+
+ positionControl_Mul_ensemble_complex(4,V_b,servo101, position101);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_m,servo102, position102);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_h,servo103, position103);
+ verifacation();
+ wait(0.5);
+ Pompe = 0;
+ wait(0.5);
+ positionControl_Mul_ensemble_complex(4,V_b,servo104, position104);
+ verifacation();
+
+ break;*/
+
+ case 12:
+ uint8_t servo121[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
+ uint8_t servo122[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo123[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo124[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+
+ uint16_t position121[4] = {660, 620, 828, Pos_Engrenage_centre};
+ uint16_t position122[4] = {659, 669, 763, Pos_Engrenage_centre};
+ uint16_t position123[4] = {532, 570, 835, Pos_Engrenage_centre};
+ uint16_t position124[4] = {418, 885, 789, Pos_Engrenage_centre};
+
+ positionControl_Mul_ensemble_complex(4,V_m,servo121, position121);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_h,servo122, position122);
+ verifacation();
+ wait(0.1);
+ Pompe = 0;
+ wait(0.2);
+
+ positionControl_Mul_ensemble_complex(4,V_b,servo123, position123);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_b,servo124, position124);
+ verifacation();
+
+ break;
+
+ case 13:
+ uint8_t servo131[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
+ uint8_t servo132[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo133[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo134[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+
+ uint16_t position131[4] = {552, 649, 853, Pos_Engrenage_centre};
+ uint16_t position132[4] = {647, 589, 813, Pos_Engrenage_centre};
+ uint16_t position133[4] = {552, 649, 853, Pos_Engrenage_centre};
+ uint16_t position134[4] = {418, 885, 789, Pos_Engrenage_centre};
+
+ positionControl_Mul_ensemble_complex(4,V_m,servo131, position131);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_h,servo132, position132);
+ verifacation();
+ wait(0.1);
+ Pompe = 0;
+ wait(0.2);
+
+ positionControl_Mul_ensemble_complex(4,V_b,servo133, position133);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_b,servo134, position134);
+ verifacation();
+
+ break;
+
+ /*case 14:
+ uint8_t servo141[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
+ uint8_t servo142[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo143[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo144[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+
+ uint16_t position141[4] = {504, 593, 845, Pos_Engrenage_centre};
+ uint16_t position142[4] = {609, 525, 816, Pos_Engrenage_centre};
+ uint16_t position143[4] = {420, 661, 845, Pos_Engrenage_centre};
+ uint16_t position144[4] = {418, 885, 789, Pos_Engrenage_centre};
+
+ positionControl_Mul_ensemble_complex(4,V_m,servo141, position141);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_h,servo142, position142);
+ wait(0.5);
+ Pompe = 0;
+ wait(0.5);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_b,servo143, position143);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_b,servo144, position144);
+ verifacation();
+
+ break;
+
+ case 15:
+ uint8_t servo151[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
+ uint8_t servo152[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+ //uint8_t servo153[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo154[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo155[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+
+ uint16_t position151[4] = {439, 688, 723, Pos_Engrenage_centre};
+ uint16_t position152[4] = {406, 691, 789, Pos_Engrenage_centre};
+ //uint16_t position153[4] = {399, 667, 767, Pos_Engrenage_centre};
+ uint16_t position154[4] = {552, 539, 801, Pos_Engrenage_centre};
+ uint16_t position155[4] = {418, 885, 789, Pos_Engrenage_centre};
+
+ //positionControl_Mul_ensemble_complex(4,V_m,servo151, position151);
+ //verifacation();
+ positionControl_Mul_ensemble_complex(4,V_h,servo152, position152);
+ verifacation();
+ //positionControl_Mul_ensemble_complex(4,V_b,servo153, position153);
+ //verifacation();
+ positionControl_Mul_ensemble_complex(4,V_b,servo154, position154);
+ verifacation();
+ wait(0.5);
+ Pompe = 0;
+ wait(0.5);
+ positionControl_Mul_ensemble_complex(4,V_b,servo155, position155);
+ verifacation();
+
+
+ break;*/
+
+ case 22:
+ uint8_t servo221[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
+ uint8_t servo222[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo223[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo224[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+
+ uint16_t position221[4] = {452, 801, 849, Pos_Engrenage_Vers_Gauche};
+ uint16_t position222[4] = {535, 768, 805, Pos_Engrenage_Vers_Gauche};
+ uint16_t position223[4] = {553, 819, 733, Pos_Engrenage_Vers_Gauche};
+ uint16_t position224[4] = {395, 864, 832, Pos_Engrenage_Vers_Gauche};
+
+ positionControl_Mul_ensemble_complex(4,V_m,servo221, position221);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_h,servo222, position222);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_b,servo223, position223);
+ verifacation();
+ wait(0.1);
+ Pompe = 0;
+ wait(0.2);
+ positionControl_Mul_ensemble_complex(4,V_b,servo224, position224);
+ verifacation();
+
+ break;
+
+ case 23:
+ uint8_t servo231[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
+ uint8_t servo232[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo233[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo234[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+
+ uint16_t position231[4] = {395, 885, 829, Pos_Engrenage_Vers_Gauche};
+ uint16_t position232[4] = {540, 733, 816, Pos_Engrenage_Vers_Gauche};
+ uint16_t position233[4] = {384, 772, 829, Pos_Engrenage_Vers_Gauche};
+ uint16_t position234[4] = {384, 887, 829, Pos_Engrenage_Vers_Gauche};
+
+ positionControl_Mul_ensemble_complex(4,V_m,servo231, position231);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_h,servo232, position232);
+ verifacation();
+ wait(0.1);
+ Pompe = 0;
+ wait(0.2);
+ positionControl_Mul_ensemble_complex(4,V_b,servo233, position233);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_b,servo234, position234);
+ verifacation();
+
+ break;
+
+ case 32:
+ uint8_t servo321[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
+ uint8_t servo322[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo323[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo324[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+
+ uint16_t position321[4] = {452, 801, 849, Pos_Engrenage_centre};
+ uint16_t position322[4] = {535, 768, 805, Pos_Engrenage_centre};
+ uint16_t position323[4] = {577, 819, 699, Pos_Engrenage_centre};
+ uint16_t position324[4] = {395, 864, 832, Pos_Engrenage_centre};
+
+ positionControl_Mul_ensemble_complex(4,V_m,servo321, position321);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_h,servo322, position322);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_b,servo323, position323);
+ verifacation();
+ wait(0.1);
+ Pompe = 0;
+ wait(0.2);
+ positionControl_Mul_ensemble_complex(4,V_b,servo324, position324);
+ verifacation();
+
+ break;
+
+ case 33:
+ uint8_t servo331[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
+ uint8_t servo332[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo333[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo334[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+
+ uint16_t position331[4] = {395, 885, 829, Pos_Engrenage_centre};
+ uint16_t position332[4] = {540, 733, 816, Pos_Engrenage_centre};
+ uint16_t position333[4] = {384, 772, 829, Pos_Engrenage_centre};
+ uint16_t position334[4] = {384, 887, 829, Pos_Engrenage_centre};
+
+ positionControl_Mul_ensemble_complex(4,V_m,servo331, position331);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_h,servo332, position332);
+ verifacation();
+ wait(0.1);
+ Pompe = 0;
+ wait(0.2);
+ positionControl_Mul_ensemble_complex(4,V_b,servo333, position333);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_b,servo334, position334);
+ verifacation();
+
+ break;
+
+ case 42:
+ uint8_t servo421[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
+ uint8_t servo422[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo423[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo424[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+
+ uint16_t position421[4] = {452, 801, 849, Pos_Engrenage_Vers_Droit};
+ uint16_t position422[4] = {535, 768, 805, Pos_Engrenage_Vers_Droit};
+ uint16_t position423[4] = {573, 818, 725, Pos_Engrenage_Vers_Droit};
+ uint16_t position424[4] = {395, 864, 832, Pos_Engrenage_Vers_Droit};
+
+ positionControl_Mul_ensemble_complex(4,V_m,servo421, position421);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_h,servo422, position422);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_b,servo423, position423);
+ verifacation();
+ wait(0.1);
+ Pompe = 0;
+ wait(0.2);
+ positionControl_Mul_ensemble_complex(4,V_b,servo424, position424);
+ verifacation();
+
+ break;
+
+ case 43:
+ uint8_t servo431[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4}; // uint8_t setLED, uint8_t id
+ uint8_t servo432[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo433[8] = {GLED_ON, ID, BLED_ON, ID2, GLED_ON, ID3, GLED_ON, ID4};
+ uint8_t servo434[8] = {BLED_ON, ID, GLED_ON, ID2, BLED_ON, ID3, GLED_ON, ID4};
+
+ uint16_t position431[4] = {395, 885, 829, Pos_Engrenage_Vers_Droit};
+ uint16_t position432[4] = {540, 733, 816, Pos_Engrenage_Vers_Droit};
+ uint16_t position433[4] = {384, 772, 829, Pos_Engrenage_Vers_Droit};
+ uint16_t position434[4] = {384, 887, 829, Pos_Engrenage_Vers_Droit};
+
+ positionControl_Mul_ensemble_complex(4,V_m,servo431, position431);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_h,servo432, position432);
+ verifacation();
+ wait(0.1);
+ Pompe = 0;
+ wait(0.2);
+
+ positionControl_Mul_ensemble_complex(4,V_b,servo433, position433);
+ verifacation();
+ positionControl_Mul_ensemble_complex(4,V_b,servo434, position434);
+ verifacation();
+
+ break;
+
+
+ }
+}
+
+void GetPos_Engrenage(int8_t ID)
+{
+
+ Pos_Engrenage_centre = getPos(ID);
+ Pos_Engrenage_gauche = Pos_Engrenage_centre + 256;
+ Pos_Engrenage_droit = Pos_Engrenage_centre - 243;
+ Pos_Engrenage_Vers_Gauche = Pos_Engrenage_centre + 256;
+ Pos_Engrenage_Vers_Droit = Pos_Engrenage_centre - 289;
+ pc.printf("Posi_central=%d Posi_gauche=%d Posi_droit=%d\n",Pos_Engrenage_centre,Pos_Engrenage_gauche,Pos_Engrenage_droit);
+
+}
+
+void Pompe_init(PinName Pompe_pin)
+{
+ PwmOut Pompe1(Pompe_pin);
+ Pompe1.period(1);
+ Pompe1 = 0;
+ wait(0.1);
+}
+
+void Pompe_essai(PinName Pompe_pin){
+ PwmOut Pompe1(Pompe_pin);
+ Pompe1 = 1;
+ wait(1);
+ Pompe1 = 0;
+ wait(1);
+ }
+
+void Arreter_couple(int8_t ID){
+ setTorque(ID,TORQUE_FREE);
+ }
+
+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){
+ //-----------------traitement des infos recues par CAN---------------------------------------
+ /*unsigned char C1 = combinaison_CAN/100;
+ unsigned char C2 = (combinaison_CAN%100)/10;
+ unsigned char C3 = combinaison_CAN%10;
+ pc.printf("%d %d %d",C1,C2,C3);*/
+ unsigned short combinaison_CAN = Cb1*100+Cb2*10+Cb3;
+ //-------------------------------------------------------------------------------------------
+ if((combinaison_CAN == 123) || (combinaison_CAN == 321)) { //---
+ deplacement_cubes(5, 42,ID,ID2,ID3,ID4,Pompe_pin);
+ deplacement_cubes(2, 43,ID,ID2,ID3,ID4,Pompe_pin);
+ deplacement_cubes(3, 12,ID,ID2,ID3,ID4,Pompe_pin);
+ } else if((combinaison_CAN == 154) || (combinaison_CAN == 451)) { //----
+ deplacement_cubes(1, 42,ID,ID2,ID3,ID4,Pompe_pin);
+ deplacement_cubes(3, 43,ID,ID2,ID3,ID4,Pompe_pin);
+ deplacement_cubes(5, 22,ID,ID2,ID3,ID4,Pompe_pin);
+ } else if((combinaison_CAN == 241) || (combinaison_CAN == 142)) { //----
+ deplacement_cubes(3, 42,ID,ID2,ID3,ID4,Pompe_pin);
+ deplacement_cubes(5, 43,ID,ID2,ID3,ID4,Pompe_pin);
+ deplacement_cubes(2, 12,ID,ID2,ID3,ID4,Pompe_pin);
+ } else if((combinaison_CAN == 253) || (combinaison_CAN == 352)) { //----
+ deplacement_cubes(1, 22,ID,ID2,ID3,ID4,Pompe_pin);
+ deplacement_cubes(3, 42,ID,ID2,ID3,ID4,Pompe_pin);
+ deplacement_cubes(5, 23,ID,ID2,ID3,ID4,Pompe_pin);
+ } else if((combinaison_CAN == 345) || (combinaison_CAN == 543)) { //----
+ deplacement_cubes(3, 22,ID,ID2,ID3,ID4,Pompe_pin);
+ deplacement_cubes(1, 23,ID,ID2,ID3,ID4,Pompe_pin);
+ deplacement_cubes(5, 42,ID,ID2,ID3,ID4,Pompe_pin);
+ } else if((combinaison_CAN == 314) || (combinaison_CAN == 413)) { //----
+ deplacement_cubes(4, 22,ID,ID2,ID3,ID4,Pompe_pin);
+ deplacement_cubes(3, 23,ID,ID2,ID3,ID4,Pompe_pin);
+ deplacement_cubes(5, 12,ID,ID2,ID3,ID4,Pompe_pin);
+ } else if((combinaison_CAN == 425) || (combinaison_CAN == 524)) { //----
+ deplacement_cubes(5, 12,ID,ID2,ID3,ID4,Pompe_pin);
+ deplacement_cubes(3, 13,ID,ID2,ID3,ID4,Pompe_pin);
+ deplacement_cubes(2, 42,ID,ID2,ID3,ID4,Pompe_pin);
+ } else if((combinaison_CAN == 432) || (combinaison_CAN == 234)) { //----
+ deplacement_cubes(1, 42,ID,ID2,ID3,ID4,Pompe_pin);
+ deplacement_cubes(2, 32,ID,ID2,ID3,ID4,Pompe_pin);
+ deplacement_cubes(5, 33,ID,ID2,ID3,ID4,Pompe_pin);
+ } else if((combinaison_CAN == 531) || (combinaison_CAN == 135)) { //----
+ deplacement_cubes(2, 12,ID,ID2,ID3,ID4,Pompe_pin);
+ deplacement_cubes(4, 13,ID,ID2,ID3,ID4,Pompe_pin);
+ deplacement_cubes(5, 32,ID,ID2,ID3,ID4,Pompe_pin);
+ } else if((combinaison_CAN == 512) || (combinaison_CAN == 215)) { //----
+ deplacement_cubes(4, 12,ID,ID2,ID3,ID4,Pompe_pin);
+ deplacement_cubes(5, 13,ID,ID2,ID3,ID4,Pompe_pin);
+ deplacement_cubes(2, 32,ID,ID2,ID3,ID4,Pompe_pin);
+ }
+}
+
+void monter_immeble(unsigned char nb_bras, unsigned char C1,unsigned char C2,unsigned char C3){
+ cubes_systeme(C1,C2,C3,ID_1,ID_2,ID_3,ID_4,Pompe_pin1);
+ /*if(nb_bras == 2){
+ cubes_systeme(C1,C2,C3,ID,ID2,ID3,ID4,Pompe_pin1); //deuximeme bras
+ }*/
+ }
+
+void Systeme_deplacement_cube_init(void){
+
+ Bras_Droite_init();
+}
+
+void Bras_Droite_init(void){
+ Interrupt4_en();
+ wait_ms(0.5);
+ clear(ID_1);
+ clear(ID_2);
+ clear(ID_2);
+ clear(ID_4);
+ setTorque(ID_1,TORQUE_ON);
+ setTorque(ID_2,TORQUE_ON);
+ setTorque(ID_3,TORQUE_ON);
+ setTorque(ID_4,TORQUE_ON);
+ wait_ms(0.3);
+ Pompe_init(Pompe_pin1);
+ }
\ No newline at end of file
diff -r 000000000000 -r 3b683da943e6 lib.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/lib.h Thu Apr 12 08:54:49 2018 +0000 @@ -0,0 +1,314 @@ +#include "mbed.h" + +//#define ID 0xFD //=253 + +// Herkulex ROM Register +#define ROM_MODEL_NO1 0 +#define ROM_MODEL_NO2 1 +#define ROM_VERSION1 2 +#define ROM_VERSION2 3 +#define ROM_BAUD_RATE 4 +#define ROM_RESERVED5 5 +#define ROM_ID 6 +#define ROM_ACK_POLICY 7 +#define ROM_ALARM_LED_POLICY 8 +#define ROM_TORQUE_POLICY 9 +#define ROM_RESERVED10 10 +#define ROM_MAX_TEMPERATURE 11 +#define ROM_MIN_VOLTAGE 12 +#define ROM_MAX_VOLTAGE 13 +#define ROM_ACCELERATION_RATIO 14 +#define ROM_MAX_ACCELERATION_TIME 15 +#define ROM_DEAD_ZONE 16 +#define ROM_SATURATOR_OFFSET 17 +#define ROM_SATURATOR_SLOPE 18 // 2Byte +#define ROM_PWM_OFFSET 20 +#define ROM_MIN_PWM 21 +#define ROM_MAX_PWM 22 // 2Byte +#define ROM_OVERLOAD_PWM_THRESHOLD 24 // 2Byte +#define ROM_MIN_POSITION 26 // 2Byte +#define ROM_MAX_POSITION 28 // 2Byte +#define ROM_POSITION_KP 30 // 2Byte +#define ROM_POSITION_KD 32 // 2Byte +#define ROM_POSITION_KI 34 // 2Byte +#define ROM_POSITION_FEEDFORWARD_1ST_GAIN 36 // 2Byte +#define ROM_POSITION FEEDFORWARD_2ND_GAIN 38 // 2Byte +#define ROM_RESERVED40 40 // 2Byte +#define ROM_RESERVED42 42 // 2Byte +#define ROM_LED_BLINK_PERIOD 44 +#define ROM_ADC_FAULT_CHECK_PERIOD 45 +#define ROM_PACKET_GARBAGE_CHECK_PERIOD 46 +#define ROM_STOP_DETECTION_PERIOD 47 +#define ROM_OVERLOAD_DETECTION_PERIOD 48 +#define ROM_STOP_THRESHOLD 49 +#define ROM_INPOSITION_MARGIN 50 +#define ROM_RESERVED51 51 +#define ROM_RESERVED52 52 +#define ROM_CALIBRATION_DIFFERENCE 53 + +//------------------------------------------------------------------------------ +// Herkulex RAM Register +#define RAM_ID 0 +#define RAM_ACK_POLICY 1 +#define RAM_ALARM_LED_POLICY 2 +#define RAM_TORQUE_POLICY 3 +#define RAM_RESERVED4 4 +#define RAM_MAX_TEMPERATURE 5 +#define RAM_MIN_VOLTAGE 6 +#define RAM_MAX_VOLTAGE 7 +#define RAM_ACCELERATION_RATIO 8 +#define RAM_MAX_ACCELERATION 9 +#define RAM_DEAD_ZONE 10 +#define RAM_SATURATOR_OFFSET 11 +#define RAM_SATURATOR_SLOPE 12 // 2Byte +#define RAM_PWM_OFFSET 14 +#define RAM_MIN_PWM 15 +#define RAM_MAX_PWM 16 // 2Byte +#define RAM_OVERLOAD_PWM_THRESHOLD 18 // 2Byte +#define RAM_MIN_POSITION 20 // 2Byte +#define RAM_MAX_POSITION 22 // 2Byte +#define RAM_POSITION_KP 24 // 2Byte +#define RAM_POSITION_KD 26 // 2Byte +#define RAM_POSITION_KI 28 // 2Byte +#define RAM_POSITION_FEEDFORWARD_1ST_GAIN 30 // 2Byte +#define RAM_POSITION_FEEDFORWARD 2ND GAIN 32 // 2Byte +#define RAM_RESERVED34 34 // 2Byte +#define RAM_RESERVED36 36 // 2Byte +#define RAM_LED_BLINK_PERIOD 38 +#define RAM_ADC_FAULT_DETECTION_PERIOD 39 +#define RAM_PACKET_GARBAGE_DETECTION_PERIOD 40 +#define RAM_STOP_DETECTION_PERIOD 41 +#define RAM_OVERLOAD_DETECTION_PERIOD 42 +#define RAM_STOP_THRESHOLD 43 +#define RAM_INPOSITION_MARGIN 44 +#define RAM_RESERVED45 45 +#define RAM_RESERVED46 46 +#define RAM_CALIBRATION_DIFFERENCE 47 +#define RAM_STATUS_ERROR 48 +#define RAM_STATUS_DETAIL 49 +#define RAM_RESERVED50 50 +#define RAM_RESERVED51 51 +#define RAM_TORQUE_CONTROL 52 +#define RAM_LED_CONTROL 53 +#define RAM_VOLTAGE 54 +#define RAM_TEMPERATURE 55 +#define RAM_CURRENT_CONTROL_MODE 56 +#define RAM_TICK 57 +#define RAM_CALIBRATED_POSITION 58 // 2Byte +#define RAM_ABSOLUTE_POSITION 60 // 2Byte +#define RAM_DIFFERENTIAL_POSITION 62 // 2Byte +#define RAM_PWM 64 // 2Byte +#define RAM_RESERVED66 66 // 2Byte +#define RAM_ABSOLUTE_GOAL_POSITION 68 // 2Byte +#define RAM_ABSOLUTE_DESIRED_TRAJECTORY_POSITION 70 // 2Byte +#define RAM_DESIRED_VELOCITY 72 // 2Byte + +//------------------------------------------------------------------------------ +// Request Packet [To Servo Module] +#define CMD_ROM_WRITE 0x01 // Write Length number of values to EEP Register Address +#define CMD_ROM_READ 0x02 // Request Length number of values from EEP Register Address +#define CMD_RAM_WRITE 0x03 // Write Length number of values to RAM Register Address +#define CMD_RAM_READ 0x04 // Request Lenght number of values from RAM Register Address +#define CMD_I_JOG 0x05 // Able to send JOG command to maximum 43 servos (operate timing of individual Servo) +#define CMD_S_JOG 0x06 // Able to send JOG command to maximum 53 servos (operate simultaneously at same time) +#define CMD_STAT 0x07 // Status Error, Status Detail request +#define CMD_ROLLBACK 0x08 // Change all EEP Regsters to Factory Default value +#define CMD_REBOOT 0x09 // Request Reboot + +//------------------------------------------------------------------------------ +// ACK Packet [To Controller(ACK)] +#define CMD_ACK_MASK 0x40 // ACK Packet CMD is Request Packet CMD + 0x40 +#define CMD_EEP_WRITE_ACK (CMD_EEP_WRITE|CMD_ACK_MASK) +#define CMD_EEP_READ_ACK (CMD_EEP_READ|CMD_ACK_MASK) +#define CMD_RAM_WRITE_ACK (CMD_RAM_WRITE|CMD_ACK_MASK) +#define CMD_RAM_READ_ACK (CMD_RAM_READ|CMD_ACK_MASK) +#define CMD_I_JOG_ACK (CMD_I_JOG|CMD_ACK_MASK) +#define CMD_S_JOG_ACK (CMD_S_JOG|CMD_ACK_MASK) +#define CMD_STAT_ACK (CMD_STAT|CMD_ACK_MASK) +#define CMD_ROLLBACK_ACK (CMD_ROLLBACK|CMD_ACK_MASK) +#define CMD_REBOOT_ACK (CMD_REBOOT|CMD_ACK_MASK) + +//------------------------------------------------------------------------------ +// Status Error +#define STATUS_OK = 0x00; +#define ERROR_EXCEED_INPUT_VOLTAGE = 0x01; +#define ERROR_EXCEED_POT_LIMIT = 0x02; +#define ERROR_EXCEED_TEMPERATURE_LIMIT = 0x04; +#define ERROR_INVALID_PACKET = 0x08; +#define ERROR_OVERLOAD = 0x10; +#define ERROR_DRIVER_FAULT = 0x20; +#define ERROR_EEP_REG_DISTORT = 0x40; + +//------------------------------------------------------------------------------ +// Status Detail +#define MOVING_FLAG = 0x01; +#define INPOSITION_FLAG = 0x02; +#define CHECKSUM_ERROR = 0x04; // Invalid packet`s detailed information +#define UNKNOWN_COMMAND = 0x08; // Invalid packet`s detailed information +#define EXCEED_REG_RANGE = 0x10; // Invalid packet`s detailed information +#define GARBAGE_DETECTED = 0x20; // Invalid packet`s detailed information +#define MOTOR_ON_FLAG = 0x40; + +//------------------------------------------------------------------------------ +// Header +#define HEADER 0xFF + +// Size +#define MIN_PACKET_SIZE 7 +#define MIN_ACK_PACKET_SIZE 9 +#define WRITE_PACKET_SIZE 13 +#define MAX_PACKET_SIZE 223 +#define MAX_DATA_SIZE (MAX_PACKET_SIZE-MIN_PACKET_SIZE) + +// ID +#define MAX_PID 0xFD +#define DEFAULT_ID 0xFD +#define MAX_ID 0xFD +#define BROADCAST_ID 0xFE + +// Checksum +#define CHKSUM_MASK 0xFE + +// Torque CMD +#define TORQUE_FREE 0x00 +#define BREAK_ON 0x40 +#define TORQUE_ON 0x60 + +// Register Size +#define BYTE1 1 +#define BYTE2 2 + +// Jog Set CMD +#define STOP 0x01 +#define POS_MODE 0x00 +#define TURN_MODE 0x02 +#define GLED_ON 0x04 +#define BLED_ON 0x08 +#define RLED_ON 0x10 + +//------------------------------------------------------------------------------ + +//------------------------------------------------------------------------------ + +/** Create an Herkulex servo object connected to the serial pins and baudrate + * + * @param tx Transmit pin. + * @param rx Receive pin. + * @param baudRate The serial tx/rx speed. + */ +// void Herkulex(PinName tx, PinName rx, uint32_t baudRate); + +/** Destroy an Herkulex servo object + */ +//void N_Herkulex(); + +/** Transmit packet datas + * + * @param packetSize The packet size. + * @param data The transmit packet data array. + */ +void txPacket(uint8_t packetSize, uint8_t* data); + +/** Receive packet datas + * + * @param packetSize The packet size. + * @param data The receive packet data array. + */ +void rxPacket(uint8_t packetSize, uint8_t* data); + +/** Clear error status + * + * @param id The herkulex servo ID. + */ +void clear(uint8_t id); + +/** Set torque setting + * + * @param id The herkulex servo ID. + * @param cmdTorue The Command for setting of torque (TORQUE_FREE 0x00, BREAK_ON 0x40, TORQUE_ON 0x60) + */ +void setTorque(uint8_t id, uint8_t cmdTorue); + +/** Position Control + * + * @param id The herkulex servo ID. + * @param position The goal position of herkulex servo. + * @param playtime Time to target position. + * @param setLED Select LED and on/off controll (GLED_ON 0x00,BLED_ON 0x08, RLED_ON 0x10) + */ +void positionControl(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED); + +/** Velocity Control + * + * @param id The herkulex servo ID. + * @param speed The goal position of herkulex servo. + * @param setLED Select LED and on/off controll (GLED_ON 0x00,BLED_ON 0x08, RLED_ON 0x10) + */ +void velocityControl(uint8_t id, int16_t speed,uint8_t setLED); + +/** Get Status + * + * @param id The herkulex servo ID. + * @return -1 is getStatus failed. other is servo`s status error value. + */ +int8_t getStatus(uint8_t id); + +/** Get Position + * + * @param id The herkulex servo ID. + * @return -1 is getPos failed. other is servo's current position. + */ +int16_t getPos(uint8_t id); + +//void recevoir_irq(void); + +void receive_Sv(void); + +void verifacation(); + +int8_t clean_ERR(uint8_t id); + +int8_t Get_Torque(int8_t id); + +int8_t Get_Temperature_MAX(int8_t id); + +int8_t Get_Tension_MIN(int8_t id); + +void Set_Tension_MIN(int8_t id,uint8_t Tension_Min); + +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); + +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); + +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 + +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 + +int8_t Get_Tension_actuelle(int8_t id); + +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); + +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); + +void Interrupt4_en(void); + +void automate(void); + +void GetPos_Engrenage(int8_t ID); + +void Pompe_init(PinName Pompe_pin); + +void Pompe_essai(PinName Pompe_pin); + +void Arreter_couple(int8_t ID); + +void monter_immeble(unsigned char nb_bras, unsigned char C1,unsigned char C2,unsigned char C3); + +void Systeme_deplacement_cube_init(void); + +void Bras_Droite_init(void); + +extern RawSerial pc; +extern RawSerial serial4; +//extern RawSerial Serial1; +//#define Serial1 serial4
diff -r 000000000000 -r 3b683da943e6 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Apr 12 08:54:49 2018 +0000
@@ -0,0 +1,104 @@
+#include "mbed.h"
+#include "ident_crac.h"
+#include "lib.h"
+
+#define SIZE_FIFO 50 //Taille du buffer pour le bus CAN
+
+AnalogIn analog_value(A0);
+
+DigitalOut led(LED1);
+
+void canProcessRx(void);
+unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
+signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
+
+CAN can1(PB_8,PB_9); // Rx&Tx pour le CAN
+CANMessage msgRxBuffer[SIZE_FIFO]; // buffer en r�ception pour le CAN
+
+void canRx_ISR (void)
+{
+ if (can1.read(msgRxBuffer[FIFO_ecriture])) {
+ //if(msgRxBuffer[FIFO_ecriture].id==RESET_STRAT) mbed_reset();
+ /*else*/ FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
+ }
+}
+
+int Combinaison_cube = 0;
+
+int main() {
+
+ can1.frequency(1000000); // fr�quence de travail 1Mbit/s
+ can1.attach(&canRx_ISR); // cr�ation de l'interrupt attach�e � la r�ception sur le CAN
+
+ wait_ms(500);
+
+ while(1) {
+ canProcessRx();
+ }
+}
+
+void canProcessRx(void)
+{
+ static signed char FIFO_occupation=0,FIFO_max_occupation=0;
+ CANMessage msgTx=CANMessage();
+ FIFO_occupation=FIFO_ecriture-FIFO_lecture;
+ if(FIFO_occupation<0)
+ FIFO_occupation=FIFO_occupation+SIZE_FIFO;
+ if(FIFO_max_occupation<FIFO_occupation)
+ FIFO_max_occupation=FIFO_occupation;
+ if(FIFO_occupation!=0) {
+
+ switch(msgRxBuffer[FIFO_lecture].id) {
+ case DEBUG_FAKE_JAKE://Permet de lancer le match � distance
+ case GLOBAL_JACK:
+ break;
+ case ALIVE_BALISE:
+ case ALIVE_MOTEUR:
+ case ALIVE_IHM:
+ case ALIVE_ACTIONNEURS_AVANT:
+ case ALIVE_ACTIONNEURS_ARRIERE:
+ case ALIVE_AX12:
+ case ECRAN_ALL_CHECK:
+ break;
+ case ACKNOWLEDGE_BALISE:
+ case ACKNOWLEDGE_MOTEUR:
+ case ACKNOWLEDGE_IHM:
+ case ACKNOWLEDGE_TELEMETRE:
+ case ACKNOWLEDGE_AX12:
+ case INSTRUCTION_END_BALISE:
+ case INSTRUCTION_END_MOTEUR:
+ case INSTRUCTION_END_IHM:
+ case INSTRUCTION_END_AX12:
+ break;
+ case ECRAN_START_MATCH:
+ break;
+ case SERVO_AX12_PROCESS:
+ break;
+ case SERVO_AX12_DONE:
+ break;
+ case ECRAN_CHOICE_COLOR:
+ break;
+ case ECRAN_CHOICE_STRAT:
+ break;
+ case BALISE_DANGER :
+ break;
+ case BALISE_STOP:
+ break;
+ case BALISE_END_DANGER:
+ break;
+
+ case ECRAN_CHOICE_START_ACTION:
+ break;
+ case OBJET_SUR_TABLE:
+ break;
+ case Monter_immeuble:
+ unsigned char Combinaison_cube_1 = msgRxBuffer[FIFO_lecture].data[0];
+ unsigned char Combinaison_cube_2 = msgRxBuffer[FIFO_lecture].data[1];
+ unsigned char Combinaison_cube_3 = msgRxBuffer[FIFO_lecture].data[2];
+ monter_immeble(1,Combinaison_cube_1,Combinaison_cube_2,Combinaison_cube_3);
+ break;
+ }
+
+ FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
+ }
+}
\ No newline at end of file
diff -r 000000000000 -r 3b683da943e6 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Apr 12 08:54:49 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/994bdf8177cb \ No newline at end of file