Sébastien Posada Ramirez / Herkulex_Library_2021_phare

Lib herkulex utilisée pour la carte du phare

fonctions_herkulex.cpp@0:32fd5a08c430, 2019-04-23 (annotated)

Committer:
AdrienSalle
Date:
Tue Apr 23 15:41:44 2019 +0000
Revision:
0:32fd5a08c430
Child:
1:94a29f20ca18
Biblio 2019

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AdrienSalle 0:32fd5a08c430 1 #include "mbed.h"
AdrienSalle 0:32fd5a08c430 2 #include "fonctions_herkulex.h"
AdrienSalle 0:32fd5a08c430 3 //#include "ident_crac.h"
AdrienSalle 0:32fd5a08c430 4
AdrienSalle 0:32fd5a08c430 5 //#ifdef ROBOT_BIG
AdrienSalle 0:32fd5a08c430 6
AdrienSalle 0:32fd5a08c430 7 //-------------------------Définition des ports série---------------------------
AdrienSalle 0:32fd5a08c430 8 RawSerial pc(USBTX, USBRX,115200);
AdrienSalle 0:32fd5a08c430 9
AdrienSalle 0:32fd5a08c430 10 RawSerial serial1(PB_6,PB_7,57600); // P : 41 et 42
AdrienSalle 0:32fd5a08c430 11 RawSerial serial2(PC_12,PD_2,115200); // P : 43 et 47
AdrienSalle 0:32fd5a08c430 12 RawSerial serial3(PC_10,PC_11,57600); // P : 44 et 48
AdrienSalle 0:32fd5a08c430 13 RawSerial serial4(PC_6,PC_7,57600); // P : 45
AdrienSalle 0:32fd5a08c430 14 RawSerial serial5(PA_0,PA_1,57600); // P : 46
AdrienSalle 0:32fd5a08c430 15
AdrienSalle 0:32fd5a08c430 16
AdrienSalle 0:32fd5a08c430 17 //----------------------------variables de reception----------------------------
AdrienSalle 0:32fd5a08c430 18 uint8_t rx[300];
AdrienSalle 0:32fd5a08c430 19 uint8_t rx2[256];
AdrienSalle 0:32fd5a08c430 20 unsigned char size_reponse=100;
AdrienSalle 0:32fd5a08c430 21 unsigned char recevoir = 0;
AdrienSalle 0:32fd5a08c430 22 unsigned char i2 = 0;
AdrienSalle 0:32fd5a08c430 23 unsigned char flag_serial1_receive2 = 0;
AdrienSalle 0:32fd5a08c430 24 //--------------------variables et fonction de verification---------------------
AdrienSalle 0:32fd5a08c430 25 #define tolerance_en_position 16 //1 degre=(1002-21)/320=3.066position
AdrienSalle 0:32fd5a08c430 26 #define tolerance_en_position_negatif -16
AdrienSalle 0:32fd5a08c430 27 #define B_tolerance_en_position 16 //1 degre=(1002-21)/320=3.066position
AdrienSalle 0:32fd5a08c430 28 #define B_tolerance_en_position_negatif -16
AdrienSalle 0:32fd5a08c430 29 #define V_b 45 //temps d'attente de bras
AdrienSalle 0:32fd5a08c430 30 #define V_m 45 //temps d'attente de bras
AdrienSalle 0:32fd5a08c430 31 #define V_h 45 //temps d'attente de bras
AdrienSalle 0:32fd5a08c430 32 #define TEMPO_R 16 //temps d'attente de reception
AdrienSalle 0:32fd5a08c430 33 #define PWM_recl 0.6
AdrienSalle 0:32fd5a08c430 34 #define temps_servo_G 65 //temps d'attente des servos sauf 2 bras
AdrienSalle 0:32fd5a08c430 35 #define new_tempoX 45 //temps d'attente de correction double
AdrienSalle 0:32fd5a08c430 36
AdrienSalle 0:32fd5a08c430 37
AdrienSalle 0:32fd5a08c430 38 int16_t pos_position = 0, get_pos = 0, pos_ID = 0;
AdrienSalle 0:32fd5a08c430 39 uint8_t pos_led = 0, Status = 0,iID = 0;
AdrienSalle 0:32fd5a08c430 40 uint8_t nombre_servo = 0;
AdrienSalle 0:32fd5a08c430 41 uint8_t pos_time = 0;
AdrienSalle 0:32fd5a08c430 42 uint16_t position_servo_mul[20];
AdrienSalle 0:32fd5a08c430 43 uint8_t data_servo_mul[40];
AdrienSalle 0:32fd5a08c430 44 uint8_t flag_correction = 0;
AdrienSalle 0:32fd5a08c430 45 float new_tempo=0;
AdrienSalle 0:32fd5a08c430 46 float tab_tempo[20];
AdrienSalle 0:32fd5a08c430 47 uint16_t position_servo_mul_different[20];
AdrienSalle 0:32fd5a08c430 48 uint8_t data_servo_mul_different[60];
AdrienSalle 0:32fd5a08c430 49 int8_t my_Tor = 0;
AdrienSalle 0:32fd5a08c430 50 int8_t Tension_inter = 0;
AdrienSalle 0:32fd5a08c430 51 double Tension = 0;
AdrienSalle 0:32fd5a08c430 52 uint8_t coeffient_time = 1;
AdrienSalle 0:32fd5a08c430 53 uint8_t veri = 0;
AdrienSalle 0:32fd5a08c430 54 typedef enum {pos,vitesse,pos_mul_complex,pos_mul_complex_different} type_etat ;
AdrienSalle 0:32fd5a08c430 55 unsigned char serial_numero = 0;
AdrienSalle 0:32fd5a08c430 56 static type_etat etat=pos;
AdrienSalle 0:32fd5a08c430 57 int nb_erreur_pas_de_couple = 0;
AdrienSalle 0:32fd5a08c430 58 int nb_erreur_pos = 0;
AdrienSalle 0:32fd5a08c430 59
AdrienSalle 0:32fd5a08c430 60
AdrienSalle 0:32fd5a08c430 61
AdrienSalle 0:32fd5a08c430 62 //---------------------fonction d'interruption de reception de serial1---------------------
AdrienSalle 0:32fd5a08c430 63 /*
AdrienSalle 0:32fd5a08c430 64 *Ici on crée une interruption afin de rendre prioritaire la réception de données
AdrienSalle 0:32fd5a08c430 65 *!!! Pour utiliser les fonctions utilisant les interruptions, il faut 'activer' ces dernières
AdrienSalle 0:32fd5a08c430 66 *!!!avec la fonction servo_interrupt_en();
AdrienSalle 0:32fd5a08c430 67 */
AdrienSalle 0:32fd5a08c430 68 unsigned char flag_perdu_info_serial1 = 0, indicateur_serial1 = 0, Size_trame_serial1 = 0, old_valueserial1 = 0;
AdrienSalle 0:32fd5a08c430 69 unsigned char char_receive_pc[100];
AdrienSalle 0:32fd5a08c430 70 unsigned char char_receive_serial1[100];
AdrienSalle 0:32fd5a08c430 71 unsigned char valueserial1=0;
AdrienSalle 0:32fd5a08c430 72 unsigned char valuepc=0,flag_seconde=0,flag_pc_receive=0,flag_serial1_receive=0;
AdrienSalle 0:32fd5a08c430 73 int pospos;
AdrienSalle 0:32fd5a08c430 74 //
AdrienSalle 0:32fd5a08c430 75 //La fonction receive_serial1() est appelée par la fonction d'interruption
AdrienSalle 0:32fd5a08c430 76 //elle appelle la fonction automate_serial() et range les données reçues dans un tableau
AdrienSalle 0:32fd5a08c430 77 //
AdrienSalle 0:32fd5a08c430 78 void receive_serial1()
AdrienSalle 0:32fd5a08c430 79 {
AdrienSalle 0:32fd5a08c430 80 char_receive_serial1[valueserial1]=serial1.getc();
AdrienSalle 0:32fd5a08c430 81 automate_serial1();
AdrienSalle 0:32fd5a08c430 82 }
AdrienSalle 0:32fd5a08c430 83 //
AdrienSalle 0:32fd5a08c430 84 //fonction d'interruption
AdrienSalle 0:32fd5a08c430 85 // elle se déclenche dès que des données se trouvent sur le port de réception
AdrienSalle 0:32fd5a08c430 86 // elle appelle alors immédiatement la fonction receive_serial1();
AdrienSalle 0:32fd5a08c430 87 //
AdrienSalle 0:32fd5a08c430 88 void Interrupt1_en(void)
AdrienSalle 0:32fd5a08c430 89 {
AdrienSalle 0:32fd5a08c430 90 serial1.attach(&receive_serial1,Serial::RxIrq);
AdrienSalle 0:32fd5a08c430 91 }
AdrienSalle 0:32fd5a08c430 92 //
AdrienSalle 0:32fd5a08c430 93 //La fonction automate_serial1() sert à vérifier la bonne réception des données
AdrienSalle 0:32fd5a08c430 94 //elle est automatiquement appelée par la fonction receive_serial1()
AdrienSalle 0:32fd5a08c430 95 //
AdrienSalle 0:32fd5a08c430 96 void automate_serial1()
AdrienSalle 0:32fd5a08c430 97 {
AdrienSalle 0:32fd5a08c430 98 typedef enum {Attente,FF,Size,Data} type_etat1;
AdrienSalle 0:32fd5a08c430 99 static type_etat1 etat1=Attente;
AdrienSalle 0:32fd5a08c430 100 /////pc.printf("coucou");
AdrienSalle 0:32fd5a08c430 101
AdrienSalle 0:32fd5a08c430 102 ////pc.printf("%d\r\n", char_receive_serial1[valueserial1]);
AdrienSalle 0:32fd5a08c430 103
AdrienSalle 0:32fd5a08c430 104 switch (etat1) {
AdrienSalle 0:32fd5a08c430 105 // état Attente
AdrienSalle 0:32fd5a08c430 106 //on attend la réception des données
AdrienSalle 0:32fd5a08c430 107 //si on reçois l'octet 0xFF, il s'agit d'un début de trame
AdrienSalle 0:32fd5a08c430 108 //on passe à l'état suivant
AdrienSalle 0:32fd5a08c430 109 //
AdrienSalle 0:32fd5a08c430 110 case Attente:
AdrienSalle 0:32fd5a08c430 111 if(char_receive_serial1[0] == 0xFF) {
AdrienSalle 0:32fd5a08c430 112 etat1 = FF;
AdrienSalle 0:32fd5a08c430 113 valueserial1 = 1;
AdrienSalle 0:32fd5a08c430 114 }
AdrienSalle 0:32fd5a08c430 115 break;
AdrienSalle 0:32fd5a08c430 116 // état FF
AdrienSalle 0:32fd5a08c430 117 //on attend le second octet 0xFF pour confirmer qu'il s'agit d'une trame
AdrienSalle 0:32fd5a08c430 118 //si on reçoit l'octet 0xFF, il s'agit bien d'une trame Herkulex
AdrienSalle 0:32fd5a08c430 119 //on passe à l'état suivant
AdrienSalle 0:32fd5a08c430 120 //Sinon on retourne à l'état précédent
AdrienSalle 0:32fd5a08c430 121 //
AdrienSalle 0:32fd5a08c430 122 case FF:
AdrienSalle 0:32fd5a08c430 123 if(char_receive_serial1[1] == 0xFF) {
AdrienSalle 0:32fd5a08c430 124 etat1 = Size;
AdrienSalle 0:32fd5a08c430 125 valueserial1 = 2;
AdrienSalle 0:32fd5a08c430 126 } else {
AdrienSalle 0:32fd5a08c430 127 etat1 = Attente;
AdrienSalle 0:32fd5a08c430 128 valueserial1 = 0;
AdrienSalle 0:32fd5a08c430 129 flag_perdu_info_serial1 = 1; //flag_perdu_info_serial1
AdrienSalle 0:32fd5a08c430 130 }
AdrienSalle 0:32fd5a08c430 131 break;
AdrienSalle 0:32fd5a08c430 132 // état size
AdrienSalle 0:32fd5a08c430 133 //On vérifie si l'octet size est supérieur à la taille minimale d'une trame Herkulex,
AdrienSalle 0:32fd5a08c430 134 //Si oui on passe à l'état suivant
AdrienSalle 0:32fd5a08c430 135 //Sinon on passe à l'état attente et flag_perdu_info_serial1 passe à 1 pour signaler la perte d'information
AdrienSalle 0:32fd5a08c430 136 case Size:
AdrienSalle 0:32fd5a08c430 137 if(char_receive_serial1[2] < 7) {
AdrienSalle 0:32fd5a08c430 138 etat1 = Attente;
AdrienSalle 0:32fd5a08c430 139 valueserial1 = 0;
AdrienSalle 0:32fd5a08c430 140 flag_perdu_info_serial1 = 1; //flag_perdu_info_serial1
AdrienSalle 0:32fd5a08c430 141 } else {
AdrienSalle 0:32fd5a08c430 142 etat1 = Data;
AdrienSalle 0:32fd5a08c430 143 old_valueserial1 = 2;
AdrienSalle 0:32fd5a08c430 144 valueserial1 = 3;
AdrienSalle 0:32fd5a08c430 145 }
AdrienSalle 0:32fd5a08c430 146 Size_trame_serial1 = char_receive_serial1[2];
AdrienSalle 0:32fd5a08c430 147 break;
AdrienSalle 0:32fd5a08c430 148 //état data
AdrienSalle 0:32fd5a08c430 149 //on verifie que la taille de la trame reçue correspond à celle indiquée dans l'octet 'size'
AdrienSalle 0:32fd5a08c430 150 //si oui
AdrienSalle 0:32fd5a08c430 151 //flag_serial1_receive passe à 1 pour indiquer que la trame à bien été transmise
AdrienSalle 0:32fd5a08c430 152 case Data:
AdrienSalle 0:32fd5a08c430 153 if((valueserial1-2)==(Size_trame_serial1-3)) {
AdrienSalle 0:32fd5a08c430 154 flag_serial1_receive = 1;
AdrienSalle 0:32fd5a08c430 155 etat1 = Attente;
AdrienSalle 0:32fd5a08c430 156 valueserial1 = 0;
AdrienSalle 0:32fd5a08c430 157 } else {
AdrienSalle 0:32fd5a08c430 158 valueserial1++;
AdrienSalle 0:32fd5a08c430 159 }
AdrienSalle 0:32fd5a08c430 160 break;
AdrienSalle 0:32fd5a08c430 161
AdrienSalle 0:32fd5a08c430 162 default:
AdrienSalle 0:32fd5a08c430 163 break;
AdrienSalle 0:32fd5a08c430 164 }
AdrienSalle 0:32fd5a08c430 165 }
AdrienSalle 0:32fd5a08c430 166 //---------------------fonction d'interruption de reception de serial2---------------------
AdrienSalle 0:32fd5a08c430 167 //même principe que la fonction d'interrutpion de serial1
AdrienSalle 0:32fd5a08c430 168 unsigned char flag_perdu_info_serial2 = 0, indicateur_serial2 = 0, Size_trame_serial2 = 0, old_valueserial2 = 0;
AdrienSalle 0:32fd5a08c430 169 unsigned char char_receive_serial2[100];
AdrienSalle 0:32fd5a08c430 170 unsigned char valueserial2=0;
AdrienSalle 0:32fd5a08c430 171 unsigned char flag_serial2_receive=0;
AdrienSalle 0:32fd5a08c430 172 void receive_serial2()
AdrienSalle 0:32fd5a08c430 173 {
AdrienSalle 0:32fd5a08c430 174 char_receive_serial2[valueserial2]=serial2.getc();
AdrienSalle 0:32fd5a08c430 175 automate_serial2();
AdrienSalle 0:32fd5a08c430 176 }
AdrienSalle 0:32fd5a08c430 177
AdrienSalle 0:32fd5a08c430 178 void Interrupt2_en(void)
AdrienSalle 0:32fd5a08c430 179 {
AdrienSalle 0:32fd5a08c430 180 serial2.attach(&receive_serial2,Serial::RxIrq);
AdrienSalle 0:32fd5a08c430 181 }
AdrienSalle 0:32fd5a08c430 182
AdrienSalle 0:32fd5a08c430 183 void automate_serial2()
AdrienSalle 0:32fd5a08c430 184 {
AdrienSalle 0:32fd5a08c430 185 typedef enum {Attente,FF,Size,Data} type_etat2;
AdrienSalle 0:32fd5a08c430 186 static type_etat2 etat2=Attente;
AdrienSalle 0:32fd5a08c430 187 ////////pc.printf("coucou");
AdrienSalle 0:32fd5a08c430 188
AdrienSalle 0:32fd5a08c430 189 ////pc.printf("%d\r\n", char_receive_serial2[valueserial2]);
AdrienSalle 0:32fd5a08c430 190
AdrienSalle 0:32fd5a08c430 191 switch (etat2) {
AdrienSalle 0:32fd5a08c430 192 case Attente:
AdrienSalle 0:32fd5a08c430 193 if(char_receive_serial2[0] == 0xFF) {
AdrienSalle 0:32fd5a08c430 194 etat2 = FF;
AdrienSalle 0:32fd5a08c430 195 valueserial2 = 1;
AdrienSalle 0:32fd5a08c430 196 }
AdrienSalle 0:32fd5a08c430 197 break;
AdrienSalle 0:32fd5a08c430 198 case FF:
AdrienSalle 0:32fd5a08c430 199 if(char_receive_serial2[1] == 0xFF) {
AdrienSalle 0:32fd5a08c430 200 etat2 = Size;
AdrienSalle 0:32fd5a08c430 201 valueserial2 = 2;
AdrienSalle 0:32fd5a08c430 202 } else {
AdrienSalle 0:32fd5a08c430 203 etat2 = Attente;
AdrienSalle 0:32fd5a08c430 204 valueserial2 = 0;
AdrienSalle 0:32fd5a08c430 205 flag_perdu_info_serial2 = 1; //flag_perdu_info_serial1
AdrienSalle 0:32fd5a08c430 206 }
AdrienSalle 0:32fd5a08c430 207 break;
AdrienSalle 0:32fd5a08c430 208 case Size:
AdrienSalle 0:32fd5a08c430 209 if(char_receive_serial2[2] < 7) {
AdrienSalle 0:32fd5a08c430 210 etat2 = Attente;
AdrienSalle 0:32fd5a08c430 211 valueserial2 = 0;
AdrienSalle 0:32fd5a08c430 212 flag_perdu_info_serial2 = 1; //flag_perdu_info_serial1
AdrienSalle 0:32fd5a08c430 213 } else {
AdrienSalle 0:32fd5a08c430 214 etat2 = Data;
AdrienSalle 0:32fd5a08c430 215 old_valueserial2 = 2;
AdrienSalle 0:32fd5a08c430 216 valueserial2 = 3;
AdrienSalle 0:32fd5a08c430 217 }
AdrienSalle 0:32fd5a08c430 218 Size_trame_serial2 = char_receive_serial2[2];
AdrienSalle 0:32fd5a08c430 219 break;
AdrienSalle 0:32fd5a08c430 220
AdrienSalle 0:32fd5a08c430 221 case Data:
AdrienSalle 0:32fd5a08c430 222 if((valueserial2-2)==(Size_trame_serial2-3)) {
AdrienSalle 0:32fd5a08c430 223 flag_serial2_receive = 1;
AdrienSalle 0:32fd5a08c430 224 etat2 = Attente;
AdrienSalle 0:32fd5a08c430 225 valueserial2 = 0;
AdrienSalle 0:32fd5a08c430 226 } else {
AdrienSalle 0:32fd5a08c430 227 valueserial2++;
AdrienSalle 0:32fd5a08c430 228 }
AdrienSalle 0:32fd5a08c430 229 break;
AdrienSalle 0:32fd5a08c430 230
AdrienSalle 0:32fd5a08c430 231 default:
AdrienSalle 0:32fd5a08c430 232 break;
AdrienSalle 0:32fd5a08c430 233 }
AdrienSalle 0:32fd5a08c430 234 }
AdrienSalle 0:32fd5a08c430 235 //---------------------fonction d'interruption de reception de serial3---------------------
AdrienSalle 0:32fd5a08c430 236 //même principe que la fonction d'interrutpion de serial1
AdrienSalle 0:32fd5a08c430 237 unsigned char flag_perdu_info_serial3 = 0, indicateur_serial3 = 0, Size_trame_serial3 = 0, old_valueserial3 = 0;
AdrienSalle 0:32fd5a08c430 238 unsigned char char_receive_serial3[100];
AdrienSalle 0:32fd5a08c430 239 unsigned char valueserial3=0;
AdrienSalle 0:32fd5a08c430 240 unsigned char flag_serial3_receive=0;
AdrienSalle 0:32fd5a08c430 241 void receive_serial3()
AdrienSalle 0:32fd5a08c430 242 {
AdrienSalle 0:32fd5a08c430 243 char_receive_serial3[valueserial3]=serial3.getc();
AdrienSalle 0:32fd5a08c430 244 automate_serial3();
AdrienSalle 0:32fd5a08c430 245 }
AdrienSalle 0:32fd5a08c430 246
AdrienSalle 0:32fd5a08c430 247 void Interrupt3_en(void)
AdrienSalle 0:32fd5a08c430 248 {
AdrienSalle 0:32fd5a08c430 249 serial3.attach(&receive_serial3,Serial::RxIrq);
AdrienSalle 0:32fd5a08c430 250 }
AdrienSalle 0:32fd5a08c430 251
AdrienSalle 0:32fd5a08c430 252 void automate_serial3()
AdrienSalle 0:32fd5a08c430 253 {
AdrienSalle 0:32fd5a08c430 254 typedef enum {Attente,FF,Size,Data} type_etat3;
AdrienSalle 0:32fd5a08c430 255 static type_etat3 etat3=Attente;
AdrienSalle 0:32fd5a08c430 256 ////////pc.printf("coucou");
AdrienSalle 0:32fd5a08c430 257
AdrienSalle 0:32fd5a08c430 258 ////pc.printf("%d\r\n", char_receive_serial3[valueserial3]);
AdrienSalle 0:32fd5a08c430 259
AdrienSalle 0:32fd5a08c430 260 switch (etat3) {
AdrienSalle 0:32fd5a08c430 261 case Attente:
AdrienSalle 0:32fd5a08c430 262 if(char_receive_serial3[0] == 0xFF) {
AdrienSalle 0:32fd5a08c430 263 etat3 = FF;
AdrienSalle 0:32fd5a08c430 264 valueserial3 = 1;
AdrienSalle 0:32fd5a08c430 265 }
AdrienSalle 0:32fd5a08c430 266 break;
AdrienSalle 0:32fd5a08c430 267 case FF:
AdrienSalle 0:32fd5a08c430 268 if(char_receive_serial3[1] == 0xFF) {
AdrienSalle 0:32fd5a08c430 269 etat3 = Size;
AdrienSalle 0:32fd5a08c430 270 valueserial3 = 2;
AdrienSalle 0:32fd5a08c430 271 } else {
AdrienSalle 0:32fd5a08c430 272 etat3 = Attente;
AdrienSalle 0:32fd5a08c430 273 valueserial3 = 0;
AdrienSalle 0:32fd5a08c430 274 flag_perdu_info_serial3 = 1; //flag_perdu_info_serial1
AdrienSalle 0:32fd5a08c430 275 }
AdrienSalle 0:32fd5a08c430 276 break;
AdrienSalle 0:32fd5a08c430 277 case Size:
AdrienSalle 0:32fd5a08c430 278 if(char_receive_serial3[2] < 7) {
AdrienSalle 0:32fd5a08c430 279 etat3 = Attente;
AdrienSalle 0:32fd5a08c430 280 valueserial3 = 0;
AdrienSalle 0:32fd5a08c430 281 flag_perdu_info_serial3 = 1; //flag_perdu_info_serial1
AdrienSalle 0:32fd5a08c430 282 } else {
AdrienSalle 0:32fd5a08c430 283 etat3 = Data;
AdrienSalle 0:32fd5a08c430 284 old_valueserial3 = 2;
AdrienSalle 0:32fd5a08c430 285 valueserial3 = 3;
AdrienSalle 0:32fd5a08c430 286 }
AdrienSalle 0:32fd5a08c430 287 Size_trame_serial3 = char_receive_serial3[2];
AdrienSalle 0:32fd5a08c430 288 break;
AdrienSalle 0:32fd5a08c430 289
AdrienSalle 0:32fd5a08c430 290 case Data:
AdrienSalle 0:32fd5a08c430 291 if((valueserial3-2)==(Size_trame_serial3-3)) {
AdrienSalle 0:32fd5a08c430 292 flag_serial3_receive = 1;
AdrienSalle 0:32fd5a08c430 293 etat3 = Attente;
AdrienSalle 0:32fd5a08c430 294 valueserial3 = 0;
AdrienSalle 0:32fd5a08c430 295 } else {
AdrienSalle 0:32fd5a08c430 296 valueserial3++;
AdrienSalle 0:32fd5a08c430 297 }
AdrienSalle 0:32fd5a08c430 298 break;
AdrienSalle 0:32fd5a08c430 299
AdrienSalle 0:32fd5a08c430 300 default:
AdrienSalle 0:32fd5a08c430 301 break;
AdrienSalle 0:32fd5a08c430 302 }
AdrienSalle 0:32fd5a08c430 303 }
AdrienSalle 0:32fd5a08c430 304 //---------------------fonction d'interruption de reception de serial4---------------------
AdrienSalle 0:32fd5a08c430 305 //même principe que la fonction d'interrutpion de serial1
AdrienSalle 0:32fd5a08c430 306 unsigned char flag_perdu_info_serial4 = 0, indicateur_serial4 = 0, Size_trame_serial4 = 0, old_valueserial4 = 0;
AdrienSalle 0:32fd5a08c430 307 unsigned char char_receive_serial4[100];
AdrienSalle 0:32fd5a08c430 308 unsigned char valueserial4=0;
AdrienSalle 0:32fd5a08c430 309 unsigned char flag_serial4_receive=0;
AdrienSalle 0:32fd5a08c430 310 void receive_serial4()
AdrienSalle 0:32fd5a08c430 311 {
AdrienSalle 0:32fd5a08c430 312 char_receive_serial4[valueserial4]=serial4.getc();
AdrienSalle 0:32fd5a08c430 313 automate_serial4();
AdrienSalle 0:32fd5a08c430 314 }
AdrienSalle 0:32fd5a08c430 315
AdrienSalle 0:32fd5a08c430 316 void Interrupt4_en(void)
AdrienSalle 0:32fd5a08c430 317 {
AdrienSalle 0:32fd5a08c430 318 serial4.attach(&receive_serial4,Serial::RxIrq);
AdrienSalle 0:32fd5a08c430 319 }
AdrienSalle 0:32fd5a08c430 320
AdrienSalle 0:32fd5a08c430 321 void automate_serial4()
AdrienSalle 0:32fd5a08c430 322 {
AdrienSalle 0:32fd5a08c430 323 typedef enum {Attente,FF,Size,Data} type_etat4;
AdrienSalle 0:32fd5a08c430 324 static type_etat4 etat4=Attente;
AdrienSalle 0:32fd5a08c430 325 ////////pc.printf("coucou");
AdrienSalle 0:32fd5a08c430 326
AdrienSalle 0:32fd5a08c430 327 ////pc.printf("%d\r\n", char_receive_serial4[valueserial4]);
AdrienSalle 0:32fd5a08c430 328
AdrienSalle 0:32fd5a08c430 329 switch (etat4) {
AdrienSalle 0:32fd5a08c430 330 case Attente:
AdrienSalle 0:32fd5a08c430 331 if(char_receive_serial4[0] == 0xFF) {
AdrienSalle 0:32fd5a08c430 332 etat4 = FF;
AdrienSalle 0:32fd5a08c430 333 valueserial4 = 1;
AdrienSalle 0:32fd5a08c430 334 }
AdrienSalle 0:32fd5a08c430 335 break;
AdrienSalle 0:32fd5a08c430 336 case FF:
AdrienSalle 0:32fd5a08c430 337 if(char_receive_serial4[1] == 0xFF) {
AdrienSalle 0:32fd5a08c430 338 etat4 = Size;
AdrienSalle 0:32fd5a08c430 339 valueserial4 = 2;
AdrienSalle 0:32fd5a08c430 340 } else {
AdrienSalle 0:32fd5a08c430 341 etat4 = Attente;
AdrienSalle 0:32fd5a08c430 342 valueserial4 = 0;
AdrienSalle 0:32fd5a08c430 343 flag_perdu_info_serial4 = 1; //flag_perdu_info_serial1
AdrienSalle 0:32fd5a08c430 344 }
AdrienSalle 0:32fd5a08c430 345 break;
AdrienSalle 0:32fd5a08c430 346 case Size:
AdrienSalle 0:32fd5a08c430 347 if(char_receive_serial4[2] < 7) {
AdrienSalle 0:32fd5a08c430 348 etat4 = Attente;
AdrienSalle 0:32fd5a08c430 349 valueserial4 = 0;
AdrienSalle 0:32fd5a08c430 350 flag_perdu_info_serial4 = 1; //flag_perdu_info_serial1
AdrienSalle 0:32fd5a08c430 351 } else {
AdrienSalle 0:32fd5a08c430 352 etat4 = Data;
AdrienSalle 0:32fd5a08c430 353 old_valueserial4 = 2;
AdrienSalle 0:32fd5a08c430 354 valueserial4 = 3;
AdrienSalle 0:32fd5a08c430 355 }
AdrienSalle 0:32fd5a08c430 356 Size_trame_serial4 = char_receive_serial4[2];
AdrienSalle 0:32fd5a08c430 357 break;
AdrienSalle 0:32fd5a08c430 358
AdrienSalle 0:32fd5a08c430 359 case Data:
AdrienSalle 0:32fd5a08c430 360 if((valueserial4-2)==(Size_trame_serial4-3)) {
AdrienSalle 0:32fd5a08c430 361 flag_serial4_receive = 1;
AdrienSalle 0:32fd5a08c430 362 etat4 = Attente;
AdrienSalle 0:32fd5a08c430 363 valueserial4 = 0;
AdrienSalle 0:32fd5a08c430 364 } else {
AdrienSalle 0:32fd5a08c430 365 valueserial4++;
AdrienSalle 0:32fd5a08c430 366 }
AdrienSalle 0:32fd5a08c430 367 break;
AdrienSalle 0:32fd5a08c430 368
AdrienSalle 0:32fd5a08c430 369 default:
AdrienSalle 0:32fd5a08c430 370 break;
AdrienSalle 0:32fd5a08c430 371 }
AdrienSalle 0:32fd5a08c430 372 }
AdrienSalle 0:32fd5a08c430 373 //---------------------fonction d'interruption de reception de serial5---------------------
AdrienSalle 0:32fd5a08c430 374 //même principe que la fonction d'interrutpion de serial1
AdrienSalle 0:32fd5a08c430 375 unsigned char flag_perdu_info_serial5 = 0, indicateur_serial5 = 0, Size_trame_serial5 = 0, old_valueserial5 = 0;
AdrienSalle 0:32fd5a08c430 376 unsigned char char_receive_serial5[100];
AdrienSalle 0:32fd5a08c430 377 unsigned char valueserial5=0;
AdrienSalle 0:32fd5a08c430 378 unsigned char flag_serial5_receive=0;
AdrienSalle 0:32fd5a08c430 379 void receive_serial5()
AdrienSalle 0:32fd5a08c430 380 {
AdrienSalle 0:32fd5a08c430 381 char_receive_serial5[valueserial5]=serial5.getc();
AdrienSalle 0:32fd5a08c430 382 automate_serial5();
AdrienSalle 0:32fd5a08c430 383 }
AdrienSalle 0:32fd5a08c430 384
AdrienSalle 0:32fd5a08c430 385 void Interrupt5_en(void)
AdrienSalle 0:32fd5a08c430 386 {
AdrienSalle 0:32fd5a08c430 387 serial5.attach(&receive_serial5,Serial::RxIrq);
AdrienSalle 0:32fd5a08c430 388 }
AdrienSalle 0:32fd5a08c430 389
AdrienSalle 0:32fd5a08c430 390 void automate_serial5()
AdrienSalle 0:32fd5a08c430 391 {
AdrienSalle 0:32fd5a08c430 392 typedef enum {Attente,FF,Size,Data} type_etat5;
AdrienSalle 0:32fd5a08c430 393 static type_etat5 etat5=Attente;
AdrienSalle 0:32fd5a08c430 394 ////////pc.printf("coucou");
AdrienSalle 0:32fd5a08c430 395
AdrienSalle 0:32fd5a08c430 396 ////pc.printf("%d\r\n", char_receive_serial5[valueserial5]);
AdrienSalle 0:32fd5a08c430 397
AdrienSalle 0:32fd5a08c430 398 switch (etat5) {
AdrienSalle 0:32fd5a08c430 399 case Attente:
AdrienSalle 0:32fd5a08c430 400 if(char_receive_serial5[0] == 0xFF) {
AdrienSalle 0:32fd5a08c430 401 etat5 = FF;
AdrienSalle 0:32fd5a08c430 402 valueserial5 = 1;
AdrienSalle 0:32fd5a08c430 403 }
AdrienSalle 0:32fd5a08c430 404 break;
AdrienSalle 0:32fd5a08c430 405 case FF:
AdrienSalle 0:32fd5a08c430 406 if(char_receive_serial5[1] == 0xFF) {
AdrienSalle 0:32fd5a08c430 407 etat5 = Size;
AdrienSalle 0:32fd5a08c430 408 valueserial5 = 2;
AdrienSalle 0:32fd5a08c430 409 } else {
AdrienSalle 0:32fd5a08c430 410 etat5 = Attente;
AdrienSalle 0:32fd5a08c430 411 valueserial5 = 0;
AdrienSalle 0:32fd5a08c430 412 flag_perdu_info_serial5 = 1; //flag_perdu_info_serial1
AdrienSalle 0:32fd5a08c430 413 }
AdrienSalle 0:32fd5a08c430 414 break;
AdrienSalle 0:32fd5a08c430 415 case Size:
AdrienSalle 0:32fd5a08c430 416 if(char_receive_serial5[2] < 7) {
AdrienSalle 0:32fd5a08c430 417 etat5 = Attente;
AdrienSalle 0:32fd5a08c430 418 valueserial5 = 0;
AdrienSalle 0:32fd5a08c430 419 flag_perdu_info_serial5 = 1; //flag_perdu_info_serial1
AdrienSalle 0:32fd5a08c430 420 } else {
AdrienSalle 0:32fd5a08c430 421 etat5 = Data;
AdrienSalle 0:32fd5a08c430 422 old_valueserial5 = 2;
AdrienSalle 0:32fd5a08c430 423 valueserial5 = 3;
AdrienSalle 0:32fd5a08c430 424 }
AdrienSalle 0:32fd5a08c430 425 Size_trame_serial5 = char_receive_serial5[2];
AdrienSalle 0:32fd5a08c430 426 break;
AdrienSalle 0:32fd5a08c430 427
AdrienSalle 0:32fd5a08c430 428 case Data:
AdrienSalle 0:32fd5a08c430 429 if((valueserial5-2)==(Size_trame_serial5-3)) {
AdrienSalle 0:32fd5a08c430 430 flag_serial5_receive = 1;
AdrienSalle 0:32fd5a08c430 431 etat5 = Attente;
AdrienSalle 0:32fd5a08c430 432 valueserial5 = 0;
AdrienSalle 0:32fd5a08c430 433 } else {
AdrienSalle 0:32fd5a08c430 434 valueserial5++;
AdrienSalle 0:32fd5a08c430 435 }
AdrienSalle 0:32fd5a08c430 436 break;
AdrienSalle 0:32fd5a08c430 437
AdrienSalle 0:32fd5a08c430 438 default:
AdrienSalle 0:32fd5a08c430 439 break;
AdrienSalle 0:32fd5a08c430 440 }
AdrienSalle 0:32fd5a08c430 441 }
AdrienSalle 0:32fd5a08c430 442 //----------------xxxxx----fonction de fermture de serial-----------------------
AdrienSalle 0:32fd5a08c430 443 /*void N_Herkulex()
AdrienSalle 0:32fd5a08c430 444 {
AdrienSalle 0:32fd5a08c430 445
AdrienSalle 0:32fd5a08c430 446 if(Sv != NULL)
AdrienSalle 0:32fd5a08c430 447 delete Sv;
AdrienSalle 0:32fd5a08c430 448 if(recevoir==2) {
AdrienSalle 0:32fd5a08c430 449 size_reponse = rx2[recevoir];
AdrienSalle 0:32fd5a08c430 450 }
AdrienSalle 0:32fd5a08c430 451 }*/
AdrienSalle 0:32fd5a08c430 452 //-------------------------fonction de transmission-----------------------------
AdrienSalle 0:32fd5a08c430 453 //
AdrienSalle 0:32fd5a08c430 454 //Permet de transmettre une trame manuellement sur une liaison série choisie
AdrienSalle 0:32fd5a08c430 455 //
AdrienSalle 0:32fd5a08c430 456 //packetSize ==> Taille totale de la trame en octets
AdrienSalle 0:32fd5a08c430 457 // en-têtes (HEADER) et données (data) inclus
AdrienSalle 0:32fd5a08c430 458 //
AdrienSalle 0:32fd5a08c430 459 //data ==> Données ( Ici il d'agit de la trame en entier) à rentrer sous forme de tableau (1 octet par case!)
AdrienSalle 0:32fd5a08c430 460 //
AdrienSalle 0:32fd5a08c430 461 //numero_serial ==> Numéro de la liaison série sur laquelle on
AdrienSalle 0:32fd5a08c430 462 // envoie la trame
AdrienSalle 0:32fd5a08c430 463 void txPacket(uint8_t packetSize, uint8_t* data, uint8_t numero_serial)
AdrienSalle 0:32fd5a08c430 464
AdrienSalle 0:32fd5a08c430 465
AdrienSalle 0:32fd5a08c430 466 /*#ifdef HERKULEX_DEBUG
AdrienSalle 0:32fd5a08c430 467 pc->printf("[TX]");
AdrienSalle 0:32fd5a08c430 468 #endif
AdrienSalle 0:32fd5a08c430 469 for(uint8_t i = 0; i < packetSize ; i++)
AdrienSalle 0:32fd5a08c430 470 {
AdrienSalle 0:32fd5a08c430 471 #ifdef HERKULEX_DEBUG
AdrienSalle 0:32fd5a08c430 472 pc->printf("%02X ",data[i]);
AdrienSalle 0:32fd5a08c430 473 #endif
AdrienSalle 0:32fd5a08c430 474 txd->putc(data[i]);
AdrienSalle 0:32fd5a08c430 475 }
AdrienSalle 0:32fd5a08c430 476 #ifdef HERKULEX_DEBUG
AdrienSalle 0:32fd5a08c430 477 pc->printf("\n");
AdrienSalle 0:32fd5a08c430 478 #endif*/
AdrienSalle 0:32fd5a08c430 479 {
AdrienSalle 0:32fd5a08c430 480 serial_numero = numero_serial;
AdrienSalle 0:32fd5a08c430 481 if(numero_serial == 1) { //Envoi sur la liaison série 1
AdrienSalle 0:32fd5a08c430 482 for(uint8_t i = 0; i < packetSize ; i++) { //
AdrienSalle 0:32fd5a08c430 483 while(serial1.writeable() == 0); //On envoie 1 octet toute les 100 us
AdrienSalle 0:32fd5a08c430 484 serial1.putc(data[i]); //
AdrienSalle 0:32fd5a08c430 485 wait_us(100); //
AdrienSalle 0:32fd5a08c430 486 }
AdrienSalle 0:32fd5a08c430 487 } else if(numero_serial == 2) { //Envoi sur la liaison série 2
AdrienSalle 0:32fd5a08c430 488 for(uint8_t i = 0; i < packetSize ; i++) {
AdrienSalle 0:32fd5a08c430 489 while(serial2.writeable() == 0);
AdrienSalle 0:32fd5a08c430 490 serial2.putc(data[i]);
AdrienSalle 0:32fd5a08c430 491 wait_us(100);
AdrienSalle 0:32fd5a08c430 492 }
AdrienSalle 0:32fd5a08c430 493 } else if(numero_serial == 3) { //Envoi sur la liaison série 3
AdrienSalle 0:32fd5a08c430 494 for(uint8_t i = 0; i < packetSize ; i++) {
AdrienSalle 0:32fd5a08c430 495 while(serial3.writeable() == 0);
AdrienSalle 0:32fd5a08c430 496 serial3.putc(data[i]);
AdrienSalle 0:32fd5a08c430 497 wait_us(100);
AdrienSalle 0:32fd5a08c430 498 }
AdrienSalle 0:32fd5a08c430 499 } else if(numero_serial == 4) { //Envoi sur la liaison série 4
AdrienSalle 0:32fd5a08c430 500 for(uint8_t i = 0; i < packetSize ; i++) {
AdrienSalle 0:32fd5a08c430 501 while(serial4.writeable() == 0);
AdrienSalle 0:32fd5a08c430 502 serial4.putc(data[i]);
AdrienSalle 0:32fd5a08c430 503 wait_us(100);
AdrienSalle 0:32fd5a08c430 504 }
AdrienSalle 0:32fd5a08c430 505 } else if(numero_serial == 5) { //Envoi sur la liaison série 5
AdrienSalle 0:32fd5a08c430 506 for(uint8_t i = 0; i < packetSize ; i++) {
AdrienSalle 0:32fd5a08c430 507 while(serial5.writeable() == 0);
AdrienSalle 0:32fd5a08c430 508 serial5.putc(data[i]);
AdrienSalle 0:32fd5a08c430 509 wait_us(100);
AdrienSalle 0:32fd5a08c430 510 }
AdrienSalle 0:32fd5a08c430 511 }
AdrienSalle 0:32fd5a08c430 512 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 513 }
AdrienSalle 0:32fd5a08c430 514 //----------------------------fonction de reception-----------------------------
AdrienSalle 0:32fd5a08c430 515 //Permet de recevoir une trame
AdrienSalle 0:32fd5a08c430 516 void rxPacket(uint8_t packetSize, uint8_t* data, uint8_t numero_serial)
AdrienSalle 0:32fd5a08c430 517 //
AdrienSalle 0:32fd5a08c430 518 //packetSize ==> taille de la trame à recevoir
AdrienSalle 0:32fd5a08c430 519 //data ==> Données
AdrienSalle 0:32fd5a08c430 520 //
AdrienSalle 0:32fd5a08c430 521 {
AdrienSalle 0:32fd5a08c430 522
AdrienSalle 0:32fd5a08c430 523 /*#ifdef HERKULEX_DEBUG
AdrienSalle 0:32fd5a08c430 524 pc->printf("[RX]");
AdrienSalle 0:32fd5a08c430 525 #endif
AdrienSalle 0:32fd5a08c430 526 for (uint8_t i=0; i < packetSize; i++)
AdrienSalle 0:32fd5a08c430 527 {
AdrienSalle 0:32fd5a08c430 528 data[i] = rxd->getc();
AdrienSalle 0:32fd5a08c430 529 #ifdef HERKULEX_DEBUG
AdrienSalle 0:32fd5a08c430 530 pc->printf("%02X ",data[i]);
AdrienSalle 0:32fd5a08c430 531 #endif
AdrienSalle 0:32fd5a08c430 532 }
AdrienSalle 0:32fd5a08c430 533 #ifdef HERKULEX_DEBUG
AdrienSalle 0:32fd5a08c430 534 pc->printf("\n");
AdrienSalle 0:32fd5a08c430 535 #endif*/
AdrienSalle 0:32fd5a08c430 536 serial_numero = numero_serial;
AdrienSalle 0:32fd5a08c430 537 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 538 if(flag_serial1_receive) {
AdrienSalle 0:32fd5a08c430 539 for(unsigned char i4=0; i4<Size_trame_serial1; i4++) {
AdrienSalle 0:32fd5a08c430 540 data[i4] = char_receive_serial1[i4];
AdrienSalle 0:32fd5a08c430 541 //////pc.printf("%d ",(int)char_receive_serial1[i4]);
AdrienSalle 0:32fd5a08c430 542 }
AdrienSalle 0:32fd5a08c430 543 flag_serial1_receive=0;
AdrienSalle 0:32fd5a08c430 544 valueserial1=0;
AdrienSalle 0:32fd5a08c430 545 }
AdrienSalle 0:32fd5a08c430 546 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 547 if(flag_serial2_receive) {
AdrienSalle 0:32fd5a08c430 548 for(unsigned char i4=0; i4<Size_trame_serial2; i4++) {
AdrienSalle 0:32fd5a08c430 549 data[i4] = char_receive_serial2[i4];
AdrienSalle 0:32fd5a08c430 550 //////pc.printf("%d ",(int)char_receive_serial2[i4]);
AdrienSalle 0:32fd5a08c430 551 }
AdrienSalle 0:32fd5a08c430 552 flag_serial2_receive=0;
AdrienSalle 0:32fd5a08c430 553 valueserial2=0;
AdrienSalle 0:32fd5a08c430 554 }
AdrienSalle 0:32fd5a08c430 555 } else if(numero_serial == 3) {
AdrienSalle 0:32fd5a08c430 556 if(flag_serial3_receive) {
AdrienSalle 0:32fd5a08c430 557 for(unsigned char i4=0; i4<Size_trame_serial3; i4++) {
AdrienSalle 0:32fd5a08c430 558 data[i4] = char_receive_serial3[i4];
AdrienSalle 0:32fd5a08c430 559 //////pc.printf("%d ",(int)char_receive_serial3[i4]);
AdrienSalle 0:32fd5a08c430 560 }
AdrienSalle 0:32fd5a08c430 561 flag_serial3_receive=0;
AdrienSalle 0:32fd5a08c430 562 valueserial3=0;
AdrienSalle 0:32fd5a08c430 563 }
AdrienSalle 0:32fd5a08c430 564 } else if(numero_serial == 4) {
AdrienSalle 0:32fd5a08c430 565 if(flag_serial4_receive) {
AdrienSalle 0:32fd5a08c430 566 for(unsigned char i4=0; i4<Size_trame_serial4; i4++) {
AdrienSalle 0:32fd5a08c430 567 data[i4] = char_receive_serial4[i4];
AdrienSalle 0:32fd5a08c430 568 //////pc.printf("%d ",(int)char_receive_serial4[i4]);
AdrienSalle 0:32fd5a08c430 569 }
AdrienSalle 0:32fd5a08c430 570 flag_serial4_receive=0;
AdrienSalle 0:32fd5a08c430 571 valueserial4=0;
AdrienSalle 0:32fd5a08c430 572 }
AdrienSalle 0:32fd5a08c430 573 } else if(numero_serial == 5) {
AdrienSalle 0:32fd5a08c430 574 if(flag_serial5_receive) {
AdrienSalle 0:32fd5a08c430 575 for(unsigned char i4=0; i4<Size_trame_serial5; i4++) {
AdrienSalle 0:32fd5a08c430 576 data[i4] = char_receive_serial5[i4];
AdrienSalle 0:32fd5a08c430 577 //////pc.printf("%d ",(int)char_receive_serial5[i4]);
AdrienSalle 0:32fd5a08c430 578 }
AdrienSalle 0:32fd5a08c430 579 flag_serial5_receive=0;
AdrienSalle 0:32fd5a08c430 580 valueserial5=0;
AdrienSalle 0:32fd5a08c430 581 }
AdrienSalle 0:32fd5a08c430 582 }
AdrienSalle 0:32fd5a08c430 583 }
AdrienSalle 0:32fd5a08c430 584 //----------------------fonction pour sortir de l'état d'erreur-------------------------
AdrienSalle 0:32fd5a08c430 585 //
AdrienSalle 0:32fd5a08c430 586 //Permet de "sortir" de la mise en erreur d'un servomoteur
AdrienSalle 0:32fd5a08c430 587 //
AdrienSalle 0:32fd5a08c430 588 void clear(uint8_t id, uint8_t numero_serial)
AdrienSalle 0:32fd5a08c430 589 //
AdrienSalle 0:32fd5a08c430 590 // id ==> On entre l'ID du servomoteur que l'on souhaite sortir de l'état d'erreur
AdrienSalle 0:32fd5a08c430 591 // numero serial ==> On entre le numéro de la liaison série sur laquelle se trouve le servomoteur concerné
AdrienSalle 0:32fd5a08c430 592 {
AdrienSalle 0:32fd5a08c430 593 uint8_t txBuf[11];
AdrienSalle 0:32fd5a08c430 594 serial_numero = numero_serial;
AdrienSalle 0:32fd5a08c430 595 txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 596 txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 597 txBuf[2] = MIN_PACKET_SIZE + 4; // Packet Size
AdrienSalle 0:32fd5a08c430 598 txBuf[3] = id; // Servo ID
AdrienSalle 0:32fd5a08c430 599 txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03) *On choisi le CMD pour écrire dans un registre
AdrienSalle 0:32fd5a08c430 600 txBuf[5] = 0; // Checksum1
AdrienSalle 0:32fd5a08c430 601 txBuf[6] = 0; // Checksum2
AdrienSalle 0:32fd5a08c430 602 txBuf[7] = RAM_STATUS_ERROR; // Address *On écrit dans le registre RAM_STATUS_ERROR
AdrienSalle 0:32fd5a08c430 603 txBuf[8] = BYTE2; // Length *
AdrienSalle 0:32fd5a08c430 604 txBuf[9] = 0; // Clear RAM_STATUS_ERROR
AdrienSalle 0:32fd5a08c430 605 txBuf[10]= 0; // Clear RAM_STATUS_DETAIL
AdrienSalle 0:32fd5a08c430 606 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
AdrienSalle 0:32fd5a08c430 607 // Checksum2 = (~Checksum1)&0xFE
AdrienSalle 0:32fd5a08c430 608 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]) & 0xFE; //calcul de checksum1
AdrienSalle 0:32fd5a08c430 609 txBuf[6] = (~txBuf[5])&0xFE; //calcul de checksum2
AdrienSalle 0:32fd5a08c430 610 // send packet (mbed -> herkulex)
AdrienSalle 0:32fd5a08c430 611 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 612 for(uint8_t i = 0; i < 11 ; i++) {
AdrienSalle 0:32fd5a08c430 613 while(serial1.writeable() == 0);
AdrienSalle 0:32fd5a08c430 614 serial1.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 615 wait_us(100);
AdrienSalle 0:32fd5a08c430 616 }
AdrienSalle 0:32fd5a08c430 617 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 618 for(uint8_t i = 0; i < 11 ; i++) {
AdrienSalle 0:32fd5a08c430 619 while(serial2.writeable() == 0);
AdrienSalle 0:32fd5a08c430 620 serial2.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 621 wait_us(100);
AdrienSalle 0:32fd5a08c430 622 }
AdrienSalle 0:32fd5a08c430 623 } else if(numero_serial == 3) {
AdrienSalle 0:32fd5a08c430 624 for(uint8_t i = 0; i < 11 ; i++) {
AdrienSalle 0:32fd5a08c430 625 while(serial3.writeable() == 0);
AdrienSalle 0:32fd5a08c430 626 serial3.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 627 wait_us(100);
AdrienSalle 0:32fd5a08c430 628 }
AdrienSalle 0:32fd5a08c430 629 } else if(numero_serial == 4) {
AdrienSalle 0:32fd5a08c430 630 for(uint8_t i = 0; i < 11 ; i++) {
AdrienSalle 0:32fd5a08c430 631 while(serial4.writeable() == 0);
AdrienSalle 0:32fd5a08c430 632 serial4.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 633 wait_us(100);
AdrienSalle 0:32fd5a08c430 634 }
AdrienSalle 0:32fd5a08c430 635 } else if(numero_serial == 5) {
AdrienSalle 0:32fd5a08c430 636 for(uint8_t i = 0; i < 11 ; i++) {
AdrienSalle 0:32fd5a08c430 637 while(serial5.writeable() == 0);
AdrienSalle 0:32fd5a08c430 638 serial5.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 639 wait_us(100);
AdrienSalle 0:32fd5a08c430 640 }
AdrienSalle 0:32fd5a08c430 641 }
AdrienSalle 0:32fd5a08c430 642 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 643 }
AdrienSalle 0:32fd5a08c430 644 //----------------fonction de mis a jour le couple de servo---------------------
AdrienSalle 0:32fd5a08c430 645 void setTorque(uint8_t id, uint8_t cmdTorque, uint8_t numero_serial)
AdrienSalle 0:32fd5a08c430 646 // Permet de modifier l'état du couple d'un servo------------
AdrienSalle 0:32fd5a08c430 647 // id ==> ID du servomoteur
AdrienSalle 0:32fd5a08c430 648 //cmdTorque ==> état souhaité pour le couple
AdrienSalle 0:32fd5a08c430 649 //numero_serial ==> Numéro de la liaison série sur laquelle se trouve le servo
AdrienSalle 0:32fd5a08c430 650
AdrienSalle 0:32fd5a08c430 651 // valeurs posssibles pour cmdTorque
AdrienSalle 0:32fd5a08c430 652 // 0x40 Break On Opérations commandes impossibles
AdrienSalle 0:32fd5a08c430 653 // 0x60 Torque On Fontionnement normal
AdrienSalle 0:32fd5a08c430 654 // 0x00 Torque Free Opérations commandes impossibles + possibilité de déplacer le servo manuellement
AdrienSalle 0:32fd5a08c430 655 {
AdrienSalle 0:32fd5a08c430 656 uint8_t txBuf[10];
AdrienSalle 0:32fd5a08c430 657 serial_numero = numero_serial;
AdrienSalle 0:32fd5a08c430 658 txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 659 txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 660 txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size
AdrienSalle 0:32fd5a08c430 661 txBuf[3] = id; // Servo ID
AdrienSalle 0:32fd5a08c430 662 txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03)
AdrienSalle 0:32fd5a08c430 663 txBuf[5] = 0; // Checksum1
AdrienSalle 0:32fd5a08c430 664 txBuf[6] = 0; // Checksum2
AdrienSalle 0:32fd5a08c430 665 txBuf[7] = RAM_TORQUE_CONTROL; // Address
AdrienSalle 0:32fd5a08c430 666 txBuf[8] = BYTE1; // Length
AdrienSalle 0:32fd5a08c430 667 txBuf[9] = cmdTorque; // Torque ON
AdrienSalle 0:32fd5a08c430 668 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
AdrienSalle 0:32fd5a08c430 669 // Checksum2 = (~Checksum1)&0xFE
AdrienSalle 0:32fd5a08c430 670 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE;
AdrienSalle 0:32fd5a08c430 671 txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle 0:32fd5a08c430 672 // send packet (mbed -> herkulex)
AdrienSalle 0:32fd5a08c430 673 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 674 for(uint8_t i = 0; i < 10 ; i++) {
AdrienSalle 0:32fd5a08c430 675 while(serial1.writeable() == 0);
AdrienSalle 0:32fd5a08c430 676 serial1.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 677 wait_us(100);
AdrienSalle 0:32fd5a08c430 678 }
AdrienSalle 0:32fd5a08c430 679 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 680 for(uint8_t i = 0; i < 10 ; i++) {
AdrienSalle 0:32fd5a08c430 681 while(serial2.writeable() == 0);
AdrienSalle 0:32fd5a08c430 682 serial2.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 683 wait_us(100);
AdrienSalle 0:32fd5a08c430 684 }
AdrienSalle 0:32fd5a08c430 685 } else if(numero_serial == 3) {
AdrienSalle 0:32fd5a08c430 686 for(uint8_t i = 0; i < 10 ; i++) {
AdrienSalle 0:32fd5a08c430 687 while(serial3.writeable() == 0);
AdrienSalle 0:32fd5a08c430 688 serial3.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 689 wait_us(100);
AdrienSalle 0:32fd5a08c430 690 }
AdrienSalle 0:32fd5a08c430 691 } else if(numero_serial == 4) {
AdrienSalle 0:32fd5a08c430 692 for(uint8_t i = 0; i < 10 ; i++) {
AdrienSalle 0:32fd5a08c430 693 while(serial4.writeable() == 0);
AdrienSalle 0:32fd5a08c430 694 serial4.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 695 wait_us(100);
AdrienSalle 0:32fd5a08c430 696 }
AdrienSalle 0:32fd5a08c430 697 } else if(numero_serial == 5) {
AdrienSalle 0:32fd5a08c430 698 for(uint8_t i = 0; i < 10 ; i++) {
AdrienSalle 0:32fd5a08c430 699 while(serial5.writeable() == 0);
AdrienSalle 0:32fd5a08c430 700 serial5.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 701 wait_us(100);
AdrienSalle 0:32fd5a08c430 702 }
AdrienSalle 0:32fd5a08c430 703 }
AdrienSalle 0:32fd5a08c430 704 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 705 }
AdrienSalle 0:32fd5a08c430 706 //-------------fonction de contrôle de position pour un seul servo--------------
AdrienSalle 0:32fd5a08c430 707 //Permet de controler un servomoteur en position
AdrienSalle 0:32fd5a08c430 708 void positionControl(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED, uint8_t numero_serial)
AdrienSalle 0:32fd5a08c430 709 //
AdrienSalle 0:32fd5a08c430 710 //id ==> id du servo à déplacer
AdrienSalle 0:32fd5a08c430 711 //position ==> position à atteindre
AdrienSalle 0:32fd5a08c430 712 //playtime ==> temps à mettre pour effectuer le déplacement
AdrienSalle 0:32fd5a08c430 713 //setLED ==> LED à allumer
AdrienSalle 0:32fd5a08c430 714 //numero-serial ==> numéro de la liaison série
AdrienSalle 0:32fd5a08c430 715 {
AdrienSalle 0:32fd5a08c430 716 float tempo=0;
AdrienSalle 0:32fd5a08c430 717 serial_numero = numero_serial;
AdrienSalle 0:32fd5a08c430 718 //if (position > 1023) return; //1002-21
AdrienSalle 0:32fd5a08c430 719 if (playtime > 254) playtime = 254; //return; //1-254 == 11.2ms-2.844sec.
AdrienSalle 0:32fd5a08c430 720 tempo=playtime*0.012;
AdrienSalle 0:32fd5a08c430 721 pos_ID = id;
AdrienSalle 0:32fd5a08c430 722 uint8_t txBuf[12];
AdrienSalle 0:32fd5a08c430 723 etat = pos;
AdrienSalle 0:32fd5a08c430 724 pos_position = position;
AdrienSalle 0:32fd5a08c430 725 pos_time = playtime;
AdrienSalle 0:32fd5a08c430 726 pos_led = setLED;
AdrienSalle 0:32fd5a08c430 727 txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 728 txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 729 txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size
AdrienSalle 0:32fd5a08c430 730 //txBuf[3] = MAX_PID;
AdrienSalle 0:32fd5a08c430 731 txBuf[3] = id; // pID is total number of servos in the network (0 ~ 253)
AdrienSalle 0:32fd5a08c430 732 txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
AdrienSalle 0:32fd5a08c430 733 txBuf[5] = 0; // Checksum1
AdrienSalle 0:32fd5a08c430 734 txBuf[6] = 0; // Checksum2
AdrienSalle 0:32fd5a08c430 735 txBuf[7] = playtime; // Playtime
AdrienSalle 0:32fd5a08c430 736 txBuf[8] = position & 0x00FF; // Position (LSB, Least Significant Bit)
AdrienSalle 0:32fd5a08c430 737 txBuf[9] =(position & 0xFF00) >> 8; // position (MSB, Most Significanct Bit)
AdrienSalle 0:32fd5a08c430 738 txBuf[10] = POS_MODE | setLED; // Pos Mode and LED on/off
AdrienSalle 0:32fd5a08c430 739 txBuf[11] = id; // Servo ID
AdrienSalle 0:32fd5a08c430 740 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
AdrienSalle 0:32fd5a08c430 741 // Checksum2 = (~Checksum1)&0xFE
AdrienSalle 0:32fd5a08c430 742 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
AdrienSalle 0:32fd5a08c430 743 txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle 0:32fd5a08c430 744 // send packet (mbed -> herkulex)
AdrienSalle 0:32fd5a08c430 745 //txPacket(12, txBuf);
AdrienSalle 0:32fd5a08c430 746 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 747 for(uint8_t i = 0; i < 12 ; i++) {
AdrienSalle 0:32fd5a08c430 748 while(serial1.writeable() == 0);
AdrienSalle 0:32fd5a08c430 749 serial1.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 750 pc.printf("%d/",txBuf[i]);
AdrienSalle 0:32fd5a08c430 751 wait_us(100);
AdrienSalle 0:32fd5a08c430 752 }
AdrienSalle 0:32fd5a08c430 753 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 754 for(uint8_t i = 0; i < 12 ; i++) {
AdrienSalle 0:32fd5a08c430 755 while(serial2.writeable() == 0);
AdrienSalle 0:32fd5a08c430 756 serial2.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 757
AdrienSalle 0:32fd5a08c430 758 wait_us(100);
AdrienSalle 0:32fd5a08c430 759 }
AdrienSalle 0:32fd5a08c430 760 } else if(numero_serial == 3) {
AdrienSalle 0:32fd5a08c430 761 for(uint8_t i = 0; i < 12 ; i++) {
AdrienSalle 0:32fd5a08c430 762 while(serial3.writeable() == 0);
AdrienSalle 0:32fd5a08c430 763 serial3.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 764 wait_us(100);
AdrienSalle 0:32fd5a08c430 765 }
AdrienSalle 0:32fd5a08c430 766 } else if(numero_serial == 4) {
AdrienSalle 0:32fd5a08c430 767 for(uint8_t i = 0; i < 12 ; i++) {
AdrienSalle 0:32fd5a08c430 768 while(serial4.writeable() == 0);
AdrienSalle 0:32fd5a08c430 769 serial4.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 770 wait_us(100);
AdrienSalle 0:32fd5a08c430 771 }
AdrienSalle 0:32fd5a08c430 772 } else if(numero_serial == 5) {
AdrienSalle 0:32fd5a08c430 773 for(uint8_t i = 0; i < 12 ; i++) {
AdrienSalle 0:32fd5a08c430 774 while(serial5.writeable() == 0);
AdrienSalle 0:32fd5a08c430 775 serial5.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 776 wait_us(100);
AdrienSalle 0:32fd5a08c430 777 }
AdrienSalle 0:32fd5a08c430 778 }
AdrienSalle 0:32fd5a08c430 779 wait(tempo);
AdrienSalle 0:32fd5a08c430 780 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 781 }
AdrienSalle 0:32fd5a08c430 782 //-------------fonction de controle de vitesse pour un seul servo---------------
AdrienSalle 0:32fd5a08c430 783
AdrienSalle 0:32fd5a08c430 784 void velocityControl(uint8_t id, int16_t speed, uint8_t setLED, uint8_t numero_serial)
AdrienSalle 0:32fd5a08c430 785 //
AdrienSalle 0:32fd5a08c430 786 //id ==> id du servo à déplacer
AdrienSalle 0:32fd5a08c430 787 //speed ==> vitesse (sans dec)
AdrienSalle 0:32fd5a08c430 788 //setLED ==> LED à allumer
AdrienSalle 0:32fd5a08c430 789 //numero_serial ==> numéro de la liaison série
AdrienSalle 0:32fd5a08c430 790 //
AdrienSalle 0:32fd5a08c430 791 {
AdrienSalle 0:32fd5a08c430 792 serial_numero = numero_serial;
AdrienSalle 0:32fd5a08c430 793 if (speed > 1023 || speed < -1023) return;
AdrienSalle 0:32fd5a08c430 794 uint8_t txBuf[12];
AdrienSalle 0:32fd5a08c430 795 txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 796 txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 797 txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size
AdrienSalle 0:32fd5a08c430 798 txBuf[3] = id; // pID is total number of servos in the network (0 ~ 253)
AdrienSalle 0:32fd5a08c430 799 txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
AdrienSalle 0:32fd5a08c430 800 txBuf[5] = 0; // Checksum1
AdrienSalle 0:32fd5a08c430 801 txBuf[6] = 0; // Checksum2
AdrienSalle 0:32fd5a08c430 802 txBuf[7] = 0; // Playtime, unmeaningful in turn mode
AdrienSalle 0:32fd5a08c430 803 if (speed >= 0) { //On gère la vitesse positive
AdrienSalle 0:32fd5a08c430 804 txBuf[8] = speed & 0x00FF; // Speed (LSB, Least Significant Bit)
AdrienSalle 0:32fd5a08c430 805 txBuf[9] =(speed & 0xFF00) >> 8; // Speed (MSB, Most Significanct Bit)
AdrienSalle 0:32fd5a08c430 806 } else if(speed < 0) { //On gère la vitesse négative (voir pg.48 de la documentation herkulex)
AdrienSalle 0:32fd5a08c430 807 speed= abs(speed);
AdrienSalle 0:32fd5a08c430 808 txBuf[8] = speed & 0x00FF; // Speed (LSB, Least Significant Bit)
AdrienSalle 0:32fd5a08c430 809 txBuf[9] =((speed|0x4000) & 0xFF00) >> 8; // Speed (MSB, Most Significanct Bit)
AdrienSalle 0:32fd5a08c430 810 }
AdrienSalle 0:32fd5a08c430 811
AdrienSalle 0:32fd5a08c430 812 txBuf[10] = TURN_MODE | setLED; // Turn Mode and LED on/off
AdrienSalle 0:32fd5a08c430 813 txBuf[11] = id; // Servo ID
AdrienSalle 0:32fd5a08c430 814 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
AdrienSalle 0:32fd5a08c430 815 // Checksum2 = (~Checksum1)&0xFE
AdrienSalle 0:32fd5a08c430 816 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
AdrienSalle 0:32fd5a08c430 817 txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle 0:32fd5a08c430 818 // send packet (mbed -> herkulex)
AdrienSalle 0:32fd5a08c430 819 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 820 txPacket(12, txBuf,1);
AdrienSalle 0:32fd5a08c430 821 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 822 txPacket(12, txBuf,2);
AdrienSalle 0:32fd5a08c430 823 } else if(numero_serial == 3) {
AdrienSalle 0:32fd5a08c430 824 txPacket(12, txBuf,3);
AdrienSalle 0:32fd5a08c430 825 } else if(numero_serial == 4) {
AdrienSalle 0:32fd5a08c430 826 txPacket(12, txBuf,4);
AdrienSalle 0:32fd5a08c430 827 } else if(numero_serial == 5) {
AdrienSalle 0:32fd5a08c430 828 txPacket(12, txBuf,5);
AdrienSalle 0:32fd5a08c430 829 }
AdrienSalle 0:32fd5a08c430 830 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 831 }
AdrienSalle 0:32fd5a08c430 832 //--------------------Compteur de tour------------------------------------------------------------------------
AdrienSalle 0:32fd5a08c430 833 //c'est un compteur de tour (CQFD)
AdrienSalle 0:32fd5a08c430 834 void compteTour(int ID,int16_t speed,uint8_t tour, uint16_t position,uint8_t setLED,uint8_t serial)
AdrienSalle 0:32fd5a08c430 835 //
AdrienSalle 0:32fd5a08c430 836 //id ==> id du servo à déplacer
AdrienSalle 0:32fd5a08c430 837 //speed ==> vitesse
AdrienSalle 0:32fd5a08c430 838 //tour ==> nombre de tour à effectuer
AdrienSalle 0:32fd5a08c430 839 //position ==> position finale
AdrienSalle 0:32fd5a08c430 840 //setLED ==> LED à allumer
AdrienSalle 0:32fd5a08c430 841 //numero_serial ==> numéro de la liaison série
AdrienSalle 0:32fd5a08c430 842 //
AdrienSalle 0:32fd5a08c430 843 //
AdrienSalle 0:32fd5a08c430 844 {
AdrienSalle 0:32fd5a08c430 845 int etat =0;
AdrienSalle 0:32fd5a08c430 846 int flagTour=0;
AdrienSalle 0:32fd5a08c430 847 int end = 0;
AdrienSalle 0:32fd5a08c430 848 int posAct, posCible;
AdrienSalle 0:32fd5a08c430 849 servo_interrupt_en();
AdrienSalle 0:32fd5a08c430 850 clear(ID,serial);
AdrienSalle 0:32fd5a08c430 851 setTorque(ID, TORQUE_ON,serial);
AdrienSalle 0:32fd5a08c430 852
AdrienSalle 0:32fd5a08c430 853 posCible=position;
AdrienSalle 0:32fd5a08c430 854 velocityControl(ID,speed,setLED,serial);
AdrienSalle 0:32fd5a08c430 855 wait_ms(100);
AdrienSalle 0:32fd5a08c430 856
AdrienSalle 0:32fd5a08c430 857 if(speed > 0) {
AdrienSalle 0:32fd5a08c430 858 while(end != 1) {
AdrienSalle 0:32fd5a08c430 859 switch (etat) {
AdrienSalle 0:32fd5a08c430 860 case 0 :
AdrienSalle 0:32fd5a08c430 861
AdrienSalle 0:32fd5a08c430 862 posAct = getPos(ID,serial);
AdrienSalle 0:32fd5a08c430 863 posAct = posAct-posCible;
AdrienSalle 0:32fd5a08c430 864 pc.printf("%d",posAct);
AdrienSalle 0:32fd5a08c430 865 if (posAct < 0) {
AdrienSalle 0:32fd5a08c430 866 clear(ID,serial);
AdrienSalle 0:32fd5a08c430 867 setTorque(ID, TORQUE_ON,serial);
AdrienSalle 0:32fd5a08c430 868 etat=1;
AdrienSalle 0:32fd5a08c430 869 }
AdrienSalle 0:32fd5a08c430 870 break;
AdrienSalle 0:32fd5a08c430 871
AdrienSalle 0:32fd5a08c430 872 case 1 :
AdrienSalle 0:32fd5a08c430 873
AdrienSalle 0:32fd5a08c430 874 velocityControl(ID,speed,RLED_ON,serial);
AdrienSalle 0:32fd5a08c430 875 posAct = getPos(ID,serial);
AdrienSalle 0:32fd5a08c430 876 posAct = posCible-posAct;
AdrienSalle 0:32fd5a08c430 877 if (posAct < 0) etat = 2;
AdrienSalle 0:32fd5a08c430 878 break;
AdrienSalle 0:32fd5a08c430 879
AdrienSalle 0:32fd5a08c430 880 case 2 :
AdrienSalle 0:32fd5a08c430 881
AdrienSalle 0:32fd5a08c430 882 clear(ID,serial);
AdrienSalle 0:32fd5a08c430 883 setTorque(ID, TORQUE_ON,serial);
AdrienSalle 0:32fd5a08c430 884 if (flagTour == tour-1) {
AdrienSalle 0:32fd5a08c430 885 velocityControl(ID,0,setLED,serial);
AdrienSalle 0:32fd5a08c430 886 positionControl(ID,posCible,1,setLED,serial);
AdrienSalle 0:32fd5a08c430 887 end = 1;
AdrienSalle 0:32fd5a08c430 888
AdrienSalle 0:32fd5a08c430 889 } else {
AdrienSalle 0:32fd5a08c430 890 flagTour=flagTour+1;
AdrienSalle 0:32fd5a08c430 891 etat = 0;
AdrienSalle 0:32fd5a08c430 892 }
AdrienSalle 0:32fd5a08c430 893 break;
AdrienSalle 0:32fd5a08c430 894 }
AdrienSalle 0:32fd5a08c430 895 }
AdrienSalle 0:32fd5a08c430 896 } else if(speed < 0) {
AdrienSalle 0:32fd5a08c430 897 while(end != 1) {
AdrienSalle 0:32fd5a08c430 898 switch (etat) {
AdrienSalle 0:32fd5a08c430 899 case 0 :
AdrienSalle 0:32fd5a08c430 900
AdrienSalle 0:32fd5a08c430 901 posAct = getPos(ID,serial);
AdrienSalle 0:32fd5a08c430 902 posAct = posCible-posAct;
AdrienSalle 0:32fd5a08c430 903 pc.printf("%d",posAct);
AdrienSalle 0:32fd5a08c430 904 if (posAct < 0) {
AdrienSalle 0:32fd5a08c430 905 clear(ID,serial);
AdrienSalle 0:32fd5a08c430 906 setTorque(ID, TORQUE_ON,serial);
AdrienSalle 0:32fd5a08c430 907 etat=1;
AdrienSalle 0:32fd5a08c430 908 }
AdrienSalle 0:32fd5a08c430 909 break;
AdrienSalle 0:32fd5a08c430 910
AdrienSalle 0:32fd5a08c430 911 case 1 :
AdrienSalle 0:32fd5a08c430 912
AdrienSalle 0:32fd5a08c430 913 velocityControl(ID,speed,RLED_ON,serial);
AdrienSalle 0:32fd5a08c430 914 posAct = getPos(ID,serial);
AdrienSalle 0:32fd5a08c430 915 posAct = posAct-posCible;
AdrienSalle 0:32fd5a08c430 916 if (posAct < 0) etat = 2;
AdrienSalle 0:32fd5a08c430 917 break;
AdrienSalle 0:32fd5a08c430 918
AdrienSalle 0:32fd5a08c430 919 case 2 :
AdrienSalle 0:32fd5a08c430 920
AdrienSalle 0:32fd5a08c430 921 clear(ID,serial);
AdrienSalle 0:32fd5a08c430 922 setTorque(ID, TORQUE_ON,serial);
AdrienSalle 0:32fd5a08c430 923 if (flagTour == tour-1) {
AdrienSalle 0:32fd5a08c430 924 velocityControl(ID,0,setLED,serial);
AdrienSalle 0:32fd5a08c430 925 positionControl(ID,posCible,1,setLED,serial);
AdrienSalle 0:32fd5a08c430 926 end =1;
AdrienSalle 0:32fd5a08c430 927 } else {
AdrienSalle 0:32fd5a08c430 928 flagTour=flagTour+1;
AdrienSalle 0:32fd5a08c430 929 etat = 0;
AdrienSalle 0:32fd5a08c430 930 }
AdrienSalle 0:32fd5a08c430 931 break;
AdrienSalle 0:32fd5a08c430 932 }
AdrienSalle 0:32fd5a08c430 933 }
AdrienSalle 0:32fd5a08c430 934 }
AdrienSalle 0:32fd5a08c430 935 }
AdrienSalle 0:32fd5a08c430 936
AdrienSalle 0:32fd5a08c430 937 //--------------------fonction d'acquis d'etat d'un servo-----------------------
AdrienSalle 0:32fd5a08c430 938 int8_t getStatus(uint8_t id, uint8_t numero_serial)
AdrienSalle 0:32fd5a08c430 939 //
AdrienSalle 0:32fd5a08c430 940 // renvoi l'état du servomoteur (doc pg 39)
AdrienSalle 0:32fd5a08c430 941 //
AdrienSalle 0:32fd5a08c430 942 //id ==> Id du servo concerné
AdrienSalle 0:32fd5a08c430 943 //numero-serial ==> numéro de la liaison série du servo
AdrienSalle 0:32fd5a08c430 944 //
AdrienSalle 0:32fd5a08c430 945 {
AdrienSalle 0:32fd5a08c430 946 serial_numero = numero_serial;
AdrienSalle 0:32fd5a08c430 947 uint8_t status;
AdrienSalle 0:32fd5a08c430 948 uint8_t txBuf[7];
AdrienSalle 0:32fd5a08c430 949 size_reponse = 9;
AdrienSalle 0:32fd5a08c430 950
AdrienSalle 0:32fd5a08c430 951 txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 952 txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 953 txBuf[2] = MIN_PACKET_SIZE; // Packet Size
AdrienSalle 0:32fd5a08c430 954 txBuf[3] = id; // Servo ID
AdrienSalle 0:32fd5a08c430 955 txBuf[4] = CMD_STAT; // Status Error, Status Detail request
AdrienSalle 0:32fd5a08c430 956 // Check Sum1 and Check Sum2
AdrienSalle 0:32fd5a08c430 957 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]) & 0xFE;
AdrienSalle 0:32fd5a08c430 958 txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle 0:32fd5a08c430 959
AdrienSalle 0:32fd5a08c430 960 uint8_t rxBuf[9];
AdrienSalle 0:32fd5a08c430 961
AdrienSalle 0:32fd5a08c430 962 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 963 for(uint8_t i = 0; i < 7 ; i++) {
AdrienSalle 0:32fd5a08c430 964 while(serial1.writeable() == 0);
AdrienSalle 0:32fd5a08c430 965 serial1.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 966 wait_us(100);
AdrienSalle 0:32fd5a08c430 967 }
AdrienSalle 0:32fd5a08c430 968 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 969 if(flag_serial1_receive) {
AdrienSalle 0:32fd5a08c430 970 for(unsigned char i4=0; i4<Size_trame_serial1; i4++) {
AdrienSalle 0:32fd5a08c430 971 rxBuf[i4] = char_receive_serial1[i4];
AdrienSalle 0:32fd5a08c430 972 ////////////pc.printf("%d ",(int)char_receive_serial1[i4]);
AdrienSalle 0:32fd5a08c430 973 }
AdrienSalle 0:32fd5a08c430 974 flag_serial1_receive=0;
AdrienSalle 0:32fd5a08c430 975 valueserial1=0;
AdrienSalle 0:32fd5a08c430 976 }
AdrienSalle 0:32fd5a08c430 977 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 978 for(uint8_t i = 0; i < 7 ; i++) {
AdrienSalle 0:32fd5a08c430 979 while(serial2.writeable() == 0);
AdrienSalle 0:32fd5a08c430 980 serial2.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 981 wait_us(100);
AdrienSalle 0:32fd5a08c430 982 }
AdrienSalle 0:32fd5a08c430 983 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 984 if(flag_serial2_receive) {
AdrienSalle 0:32fd5a08c430 985 for(unsigned char i4=0; i4<Size_trame_serial2; i4++) {
AdrienSalle 0:32fd5a08c430 986 rxBuf[i4] = char_receive_serial2[i4];
AdrienSalle 0:32fd5a08c430 987 //////pc.printf("%d ",(int)char_receive_serial2[i4]);
AdrienSalle 0:32fd5a08c430 988 }
AdrienSalle 0:32fd5a08c430 989 flag_serial2_receive=0;
AdrienSalle 0:32fd5a08c430 990 valueserial2=0;
AdrienSalle 0:32fd5a08c430 991 }
AdrienSalle 0:32fd5a08c430 992 } else if(numero_serial == 3) {
AdrienSalle 0:32fd5a08c430 993 for(uint8_t i = 0; i < 7 ; i++) {
AdrienSalle 0:32fd5a08c430 994 while(serial3.writeable() == 0);
AdrienSalle 0:32fd5a08c430 995 serial3.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 996 wait_us(100);
AdrienSalle 0:32fd5a08c430 997 }
AdrienSalle 0:32fd5a08c430 998 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 999 if(flag_serial3_receive) {
AdrienSalle 0:32fd5a08c430 1000 for(unsigned char i4=0; i4<Size_trame_serial3; i4++) {
AdrienSalle 0:32fd5a08c430 1001 rxBuf[i4] = char_receive_serial3[i4];
AdrienSalle 0:32fd5a08c430 1002 //////pc.printf("%d ",(int)char_receive_serial3[i4]);
AdrienSalle 0:32fd5a08c430 1003 }
AdrienSalle 0:32fd5a08c430 1004 flag_serial3_receive=0;
AdrienSalle 0:32fd5a08c430 1005 valueserial3=0;
AdrienSalle 0:32fd5a08c430 1006 }
AdrienSalle 0:32fd5a08c430 1007 } else if(numero_serial == 4) {
AdrienSalle 0:32fd5a08c430 1008 for(uint8_t i = 0; i < 7 ; i++) {
AdrienSalle 0:32fd5a08c430 1009 while(serial4.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1010 serial4.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1011 wait_us(100);
AdrienSalle 0:32fd5a08c430 1012 }
AdrienSalle 0:32fd5a08c430 1013 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1014 if(flag_serial4_receive) {
AdrienSalle 0:32fd5a08c430 1015 for(unsigned char i4=0; i4<Size_trame_serial4; i4++) {
AdrienSalle 0:32fd5a08c430 1016 rxBuf[i4] = char_receive_serial4[i4];
AdrienSalle 0:32fd5a08c430 1017 //////pc.printf("%d ",(int)char_receive_serial4[i4]);
AdrienSalle 0:32fd5a08c430 1018 }
AdrienSalle 0:32fd5a08c430 1019 flag_serial4_receive=0;
AdrienSalle 0:32fd5a08c430 1020 valueserial4=0;
AdrienSalle 0:32fd5a08c430 1021 }
AdrienSalle 0:32fd5a08c430 1022 } else if(numero_serial == 5) {
AdrienSalle 0:32fd5a08c430 1023 for(uint8_t i = 0; i < 7 ; i++) {
AdrienSalle 0:32fd5a08c430 1024 while(serial5.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1025 serial5.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1026 wait_us(100);
AdrienSalle 0:32fd5a08c430 1027 }
AdrienSalle 0:32fd5a08c430 1028 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1029 if(flag_serial5_receive) {
AdrienSalle 0:32fd5a08c430 1030 for(unsigned char i4=0; i4<Size_trame_serial5; i4++) {
AdrienSalle 0:32fd5a08c430 1031 rxBuf[i4] = char_receive_serial5[i4];
AdrienSalle 0:32fd5a08c430 1032 //////pc.printf("%d ",(int)char_receive_serial5[i4]);
AdrienSalle 0:32fd5a08c430 1033 }
AdrienSalle 0:32fd5a08c430 1034 flag_serial5_receive=0;
AdrienSalle 0:32fd5a08c430 1035 valueserial5=0;
AdrienSalle 0:32fd5a08c430 1036 }
AdrienSalle 0:32fd5a08c430 1037 }
AdrienSalle 0:32fd5a08c430 1038
AdrienSalle 0:32fd5a08c430 1039 // Checksum1
AdrienSalle 0:32fd5a08c430 1040 uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]) & 0xFE;
AdrienSalle 0:32fd5a08c430 1041 if (chksum1 != rxBuf[5]) {
AdrienSalle 0:32fd5a08c430 1042 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 1043 flag_serial1_receive=0;
AdrienSalle 0:32fd5a08c430 1044 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 1045 flag_serial2_receive=0;
AdrienSalle 0:32fd5a08c430 1046 }
AdrienSalle 0:32fd5a08c430 1047 return -1;
AdrienSalle 0:32fd5a08c430 1048 }
AdrienSalle 0:32fd5a08c430 1049 // Checksum2
AdrienSalle 0:32fd5a08c430 1050 uint8_t chksum2 = (~rxBuf[5]&0xFE);
AdrienSalle 0:32fd5a08c430 1051 if (chksum2 != rxBuf[6]) {
AdrienSalle 0:32fd5a08c430 1052 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 1053 flag_serial1_receive=0;
AdrienSalle 0:32fd5a08c430 1054 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 1055 flag_serial2_receive=0;
AdrienSalle 0:32fd5a08c430 1056 }
AdrienSalle 0:32fd5a08c430 1057 return -1;
AdrienSalle 0:32fd5a08c430 1058 }
AdrienSalle 0:32fd5a08c430 1059 status = rxBuf[7]; // Status Error
AdrienSalle 0:32fd5a08c430 1060 //status = rxBuf[8]; // Status Detail
AdrienSalle 0:32fd5a08c430 1061
AdrienSalle 0:32fd5a08c430 1062
AdrienSalle 0:32fd5a08c430 1063 return status;
AdrienSalle 0:32fd5a08c430 1064 }
AdrienSalle 0:32fd5a08c430 1065 //------------------fonction pour lire la position actuelle-----------------------
AdrienSalle 0:32fd5a08c430 1066 int16_t getPos(uint8_t id, uint8_t numero_serial)
AdrienSalle 0:32fd5a08c430 1067 //
AdrienSalle 0:32fd5a08c430 1068 //renvoie la position d'un servo
AdrienSalle 0:32fd5a08c430 1069 //
AdrienSalle 0:32fd5a08c430 1070 //!!!ne pas oublier d'utiliser servo_interrupt_en();!!!
AdrienSalle 0:32fd5a08c430 1071 //
AdrienSalle 0:32fd5a08c430 1072 //id ==> id d'un servomoteur
AdrienSalle 0:32fd5a08c430 1073 //numero_serial==> numéro de la liaison série du servo
AdrienSalle 0:32fd5a08c430 1074 //
AdrienSalle 0:32fd5a08c430 1075 {
AdrienSalle 0:32fd5a08c430 1076 serial_numero = numero_serial;
AdrienSalle 0:32fd5a08c430 1077 uint16_t position = 0;
AdrienSalle 0:32fd5a08c430 1078 uint8_t txBuf[9];
AdrienSalle 0:32fd5a08c430 1079 size_reponse = 13;
AdrienSalle 0:32fd5a08c430 1080
AdrienSalle 0:32fd5a08c430 1081 txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 1082 txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 1083 txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
AdrienSalle 0:32fd5a08c430 1084 txBuf[3] = id; // Servo ID
AdrienSalle 0:32fd5a08c430 1085 txBuf[4] = CMD_RAM_READ; // Command Ram Read
AdrienSalle 0:32fd5a08c430 1086 txBuf[5] = 0; // Checksum1
AdrienSalle 0:32fd5a08c430 1087 txBuf[6] = 0; // Checksum2
AdrienSalle 0:32fd5a08c430 1088 txBuf[7] = RAM_CALIBRATED_POSITION; // Address
AdrienSalle 0:32fd5a08c430 1089 txBuf[8] = BYTE2;
AdrienSalle 0:32fd5a08c430 1090 // Check Sum1 and Check Sum2
AdrienSalle 0:32fd5a08c430 1091 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
AdrienSalle 0:32fd5a08c430 1092 txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle 0:32fd5a08c430 1093
AdrienSalle 0:32fd5a08c430 1094 uint8_t rxBuf[13];
AdrienSalle 0:32fd5a08c430 1095
AdrienSalle 0:32fd5a08c430 1096 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 1097 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 1098 while(serial1.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1099 serial1.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1100 wait_us(100);
AdrienSalle 0:32fd5a08c430 1101 }
AdrienSalle 0:32fd5a08c430 1102 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1103 if(flag_serial1_receive) {
AdrienSalle 0:32fd5a08c430 1104 for(unsigned char i4=0; i4<Size_trame_serial1; i4++) {
AdrienSalle 0:32fd5a08c430 1105 rxBuf[i4] = char_receive_serial1[i4];
AdrienSalle 0:32fd5a08c430 1106 //pc.printf("%d ",(int)char_receive_serial1[i4]);
AdrienSalle 0:32fd5a08c430 1107 }
AdrienSalle 0:32fd5a08c430 1108 flag_serial1_receive=0;
AdrienSalle 0:32fd5a08c430 1109 valueserial1=0;
AdrienSalle 0:32fd5a08c430 1110 }
AdrienSalle 0:32fd5a08c430 1111 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 1112 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 1113 while(serial2.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1114 serial2.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1115 wait_us(100);
AdrienSalle 0:32fd5a08c430 1116 }
AdrienSalle 0:32fd5a08c430 1117 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1118 if(flag_serial2_receive) {
AdrienSalle 0:32fd5a08c430 1119 for(unsigned char i4=0; i4<Size_trame_serial2; i4++) {
AdrienSalle 0:32fd5a08c430 1120 rxBuf[i4] = char_receive_serial2[i4];
AdrienSalle 0:32fd5a08c430 1121 //////pc.printf("%d ",(int)char_receive_serial2[i4]);
AdrienSalle 0:32fd5a08c430 1122 }
AdrienSalle 0:32fd5a08c430 1123 flag_serial2_receive=0;
AdrienSalle 0:32fd5a08c430 1124 valueserial2=0;
AdrienSalle 0:32fd5a08c430 1125 }
AdrienSalle 0:32fd5a08c430 1126 } else if(numero_serial == 3) {
AdrienSalle 0:32fd5a08c430 1127 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 1128 while(serial3.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1129 serial3.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1130 wait_us(100);
AdrienSalle 0:32fd5a08c430 1131 }
AdrienSalle 0:32fd5a08c430 1132 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1133 if(flag_serial3_receive) {
AdrienSalle 0:32fd5a08c430 1134 for(unsigned char i4=0; i4<Size_trame_serial3; i4++) {
AdrienSalle 0:32fd5a08c430 1135 rxBuf[i4] = char_receive_serial3[i4];
AdrienSalle 0:32fd5a08c430 1136 //////pc.printf("%d ",(int)char_receive_serial3[i4]);
AdrienSalle 0:32fd5a08c430 1137 }
AdrienSalle 0:32fd5a08c430 1138 flag_serial3_receive=0;
AdrienSalle 0:32fd5a08c430 1139 valueserial3=0;
AdrienSalle 0:32fd5a08c430 1140 }
AdrienSalle 0:32fd5a08c430 1141 } else if(numero_serial == 4) {
AdrienSalle 0:32fd5a08c430 1142 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 1143 while(serial4.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1144 serial4.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1145 wait_us(100);
AdrienSalle 0:32fd5a08c430 1146 }
AdrienSalle 0:32fd5a08c430 1147 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1148 if(flag_serial4_receive) {
AdrienSalle 0:32fd5a08c430 1149 for(unsigned char i4=0; i4<Size_trame_serial4; i4++) {
AdrienSalle 0:32fd5a08c430 1150 rxBuf[i4] = char_receive_serial4[i4];
AdrienSalle 0:32fd5a08c430 1151 //////pc.printf("%d ",(int)char_receive_serial4[i4]);
AdrienSalle 0:32fd5a08c430 1152 }
AdrienSalle 0:32fd5a08c430 1153 flag_serial4_receive=0;
AdrienSalle 0:32fd5a08c430 1154 valueserial4=0;
AdrienSalle 0:32fd5a08c430 1155 }
AdrienSalle 0:32fd5a08c430 1156 } else if(numero_serial == 5) {
AdrienSalle 0:32fd5a08c430 1157 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 1158 while(serial5.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1159 serial5.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1160 wait_us(100);
AdrienSalle 0:32fd5a08c430 1161 }
AdrienSalle 0:32fd5a08c430 1162 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1163 if(flag_serial5_receive) {
AdrienSalle 0:32fd5a08c430 1164 for(unsigned char i4=0; i4<Size_trame_serial5; i4++) {
AdrienSalle 0:32fd5a08c430 1165 rxBuf[i4] = char_receive_serial5[i4];
AdrienSalle 0:32fd5a08c430 1166 //////pc.printf("%d ",(int)char_receive_serial5[i4]);
AdrienSalle 0:32fd5a08c430 1167 }
AdrienSalle 0:32fd5a08c430 1168 flag_serial5_receive=0;
AdrienSalle 0:32fd5a08c430 1169 valueserial5=0;
AdrienSalle 0:32fd5a08c430 1170 }
AdrienSalle 0:32fd5a08c430 1171 }
AdrienSalle 0:32fd5a08c430 1172
AdrienSalle 0:32fd5a08c430 1173 // Checksum1
AdrienSalle 0:32fd5a08c430 1174 uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE;
AdrienSalle 0:32fd5a08c430 1175 if (chksum1 != rxBuf[5]) {
AdrienSalle 0:32fd5a08c430 1176 /*#ifdef HERKULEX_DEBUG
AdrienSalle 0:32fd5a08c430 1177 pc->printf("Checksum1 fault\n");
AdrienSalle 0:32fd5a08c430 1178 #endif*/
AdrienSalle 0:32fd5a08c430 1179 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 1180 flag_serial1_receive=0;
AdrienSalle 0:32fd5a08c430 1181 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 1182 flag_serial2_receive=0;
AdrienSalle 0:32fd5a08c430 1183 }
AdrienSalle 0:32fd5a08c430 1184 return -1;
AdrienSalle 0:32fd5a08c430 1185 }
AdrienSalle 0:32fd5a08c430 1186 // Checksum2
AdrienSalle 0:32fd5a08c430 1187 uint8_t chksum2 = (~rxBuf[5]&0xFE);
AdrienSalle 0:32fd5a08c430 1188 if (chksum2 != rxBuf[6]) {
AdrienSalle 0:32fd5a08c430 1189 /* #ifdef HERKULEX_DEBUG
AdrienSalle 0:32fd5a08c430 1190 pc->printf("Checksum2 fault\n");
AdrienSalle 0:32fd5a08c430 1191 #endif*/
AdrienSalle 0:32fd5a08c430 1192 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 1193 flag_serial1_receive=0;
AdrienSalle 0:32fd5a08c430 1194 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 1195 flag_serial2_receive=0;
AdrienSalle 0:32fd5a08c430 1196 }
AdrienSalle 0:32fd5a08c430 1197 return -1;
AdrienSalle 0:32fd5a08c430 1198 }
AdrienSalle 0:32fd5a08c430 1199 position = ((rxBuf[10]&0x03)<<8) | rxBuf[9];
AdrienSalle 0:32fd5a08c430 1200
AdrienSalle 0:32fd5a08c430 1201
AdrienSalle 0:32fd5a08c430 1202 //}
AdrienSalle 0:32fd5a08c430 1203 return position;
AdrienSalle 0:32fd5a08c430 1204 }
AdrienSalle 0:32fd5a08c430 1205 //---------------fonction d'acquis d'etat de couple d'un servo------------------
AdrienSalle 0:32fd5a08c430 1206 //Obtenir la valeur du couple d'un servo
AdrienSalle 0:32fd5a08c430 1207 int8_t Get_Torque(int8_t id, uint8_t numero_serial)
AdrienSalle 0:32fd5a08c430 1208 //
AdrienSalle 0:32fd5a08c430 1209 //id ==> id du servomoteur sur la liaison série
AdrienSalle 0:32fd5a08c430 1210 //numero_serial ==> numéro de la liaison série sur laquelle se trouve le servomoteur
AdrienSalle 0:32fd5a08c430 1211 //
AdrienSalle 0:32fd5a08c430 1212 {
AdrienSalle 0:32fd5a08c430 1213 serial_numero = numero_serial;
AdrienSalle 0:32fd5a08c430 1214 uint8_t txBuf[9];
AdrienSalle 0:32fd5a08c430 1215 int8_t Tor = 0;
AdrienSalle 0:32fd5a08c430 1216
AdrienSalle 0:32fd5a08c430 1217 uint8_t iv=0;
AdrienSalle 0:32fd5a08c430 1218 for(iv=0; iv<20; iv++) {
AdrienSalle 0:32fd5a08c430 1219 rx2[iv] = 0;
AdrienSalle 0:32fd5a08c430 1220 }
AdrienSalle 0:32fd5a08c430 1221
AdrienSalle 0:32fd5a08c430 1222 txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 1223 txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 1224 txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
AdrienSalle 0:32fd5a08c430 1225 txBuf[3] = id; // Servo ID
AdrienSalle 0:32fd5a08c430 1226 txBuf[4] = CMD_RAM_READ; // Command Ram Read
AdrienSalle 0:32fd5a08c430 1227 txBuf[5] = 0; // Checksum1
AdrienSalle 0:32fd5a08c430 1228 txBuf[6] = 0; // Checksum2
AdrienSalle 0:32fd5a08c430 1229 txBuf[7] = RAM_TORQUE_CONTROL;
AdrienSalle 0:32fd5a08c430 1230 txBuf[8] = BYTE1; // Length
AdrienSalle 0:32fd5a08c430 1231 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;//Checksum2
AdrienSalle 0:32fd5a08c430 1232 txBuf[6] = (~txBuf[5])&0xFE; // CheckSum2
AdrienSalle 0:32fd5a08c430 1233
AdrienSalle 0:32fd5a08c430 1234 //pc.printf(" Torque ");
AdrienSalle 0:32fd5a08c430 1235
AdrienSalle 0:32fd5a08c430 1236 uint8_t rxBuf[12];
AdrienSalle 0:32fd5a08c430 1237
AdrienSalle 0:32fd5a08c430 1238 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 1239 //send packet (mbed -> herkulex)
AdrienSalle 0:32fd5a08c430 1240 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 1241 while(serial1.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1242 serial1.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1243 wait_us(100);
AdrienSalle 0:32fd5a08c430 1244 }
AdrienSalle 0:32fd5a08c430 1245
AdrienSalle 0:32fd5a08c430 1246 //send packet (mbed -> herkulex)
AdrienSalle 0:32fd5a08c430 1247 //uint8_t rxBuf[12];
AdrienSalle 0:32fd5a08c430 1248 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1249 if(flag_serial1_receive) {
AdrienSalle 0:32fd5a08c430 1250 for(unsigned char i4=0; i4<Size_trame_serial1; i4++) {
AdrienSalle 0:32fd5a08c430 1251 rxBuf[i4] = char_receive_serial1[i4];
AdrienSalle 0:32fd5a08c430 1252 //////pc.printf("%d ",(int)char_receive_serial1[i4]);
AdrienSalle 0:32fd5a08c430 1253 }
AdrienSalle 0:32fd5a08c430 1254 flag_serial1_receive=0;
AdrienSalle 0:32fd5a08c430 1255 valueserial1=0;
AdrienSalle 0:32fd5a08c430 1256 }
AdrienSalle 0:32fd5a08c430 1257 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 1258 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 1259 while(serial2.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1260 serial2.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1261 wait_us(100);
AdrienSalle 0:32fd5a08c430 1262 }
AdrienSalle 0:32fd5a08c430 1263 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1264 if(flag_serial2_receive) {
AdrienSalle 0:32fd5a08c430 1265 for(unsigned char i4=0; i4<Size_trame_serial2; i4++) {
AdrienSalle 0:32fd5a08c430 1266 rxBuf[i4] = char_receive_serial2[i4];
AdrienSalle 0:32fd5a08c430 1267 //////pc.printf("%d ",(int)char_receive_serial2[i4]);
AdrienSalle 0:32fd5a08c430 1268 }
AdrienSalle 0:32fd5a08c430 1269 flag_serial2_receive=0;
AdrienSalle 0:32fd5a08c430 1270 valueserial2=0;
AdrienSalle 0:32fd5a08c430 1271 }
AdrienSalle 0:32fd5a08c430 1272 } else if(numero_serial == 3) {
AdrienSalle 0:32fd5a08c430 1273 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 1274 while(serial3.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1275 serial3.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1276 wait_us(100);
AdrienSalle 0:32fd5a08c430 1277 }
AdrienSalle 0:32fd5a08c430 1278 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1279 if(flag_serial3_receive) {
AdrienSalle 0:32fd5a08c430 1280 for(unsigned char i4=0; i4<Size_trame_serial3; i4++) {
AdrienSalle 0:32fd5a08c430 1281 rxBuf[i4] = char_receive_serial3[i4];
AdrienSalle 0:32fd5a08c430 1282 //////pc.printf("%d ",(int)char_receive_serial3[i4]);
AdrienSalle 0:32fd5a08c430 1283 }
AdrienSalle 0:32fd5a08c430 1284 flag_serial3_receive=0;
AdrienSalle 0:32fd5a08c430 1285 valueserial3=0;
AdrienSalle 0:32fd5a08c430 1286 }
AdrienSalle 0:32fd5a08c430 1287 } else if(numero_serial == 4) {
AdrienSalle 0:32fd5a08c430 1288 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 1289 while(serial4.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1290 serial4.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1291 wait_us(100);
AdrienSalle 0:32fd5a08c430 1292 }
AdrienSalle 0:32fd5a08c430 1293 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1294 if(flag_serial4_receive) {
AdrienSalle 0:32fd5a08c430 1295 for(unsigned char i4=0; i4<Size_trame_serial4; i4++) {
AdrienSalle 0:32fd5a08c430 1296 rxBuf[i4] = char_receive_serial4[i4];
AdrienSalle 0:32fd5a08c430 1297 //////pc.printf("%d ",(int)char_receive_serial4[i4]);
AdrienSalle 0:32fd5a08c430 1298 }
AdrienSalle 0:32fd5a08c430 1299 flag_serial4_receive=0;
AdrienSalle 0:32fd5a08c430 1300 valueserial4=0;
AdrienSalle 0:32fd5a08c430 1301 }
AdrienSalle 0:32fd5a08c430 1302 } else if(numero_serial == 5) {
AdrienSalle 0:32fd5a08c430 1303 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 1304 while(serial5.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1305 serial5.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1306 wait_us(100);
AdrienSalle 0:32fd5a08c430 1307 }
AdrienSalle 0:32fd5a08c430 1308 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1309 if(flag_serial5_receive) {
AdrienSalle 0:32fd5a08c430 1310 for(unsigned char i4=0; i4<Size_trame_serial5; i4++) {
AdrienSalle 0:32fd5a08c430 1311 rxBuf[i4] = char_receive_serial5[i4];
AdrienSalle 0:32fd5a08c430 1312 //////pc.printf("%d ",(int)char_receive_serial5[i4]);
AdrienSalle 0:32fd5a08c430 1313 }
AdrienSalle 0:32fd5a08c430 1314 flag_serial5_receive=0;
AdrienSalle 0:32fd5a08c430 1315 valueserial5=0;
AdrienSalle 0:32fd5a08c430 1316 }
AdrienSalle 0:32fd5a08c430 1317 }
AdrienSalle 0:32fd5a08c430 1318
AdrienSalle 0:32fd5a08c430 1319 uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]) & 0xFE;
AdrienSalle 0:32fd5a08c430 1320 if (chksum1 != rxBuf[5]) {
AdrienSalle 0:32fd5a08c430 1321 /*#ifdef HERKULEX_DEBUG
AdrienSalle 0:32fd5a08c430 1322 pc->printf("Checksum1 fault\n");
AdrienSalle 0:32fd5a08c430 1323 #endif*/
AdrienSalle 0:32fd5a08c430 1324 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 1325 flag_serial1_receive=0;
AdrienSalle 0:32fd5a08c430 1326 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 1327 flag_serial2_receive=0;
AdrienSalle 0:32fd5a08c430 1328 }
AdrienSalle 0:32fd5a08c430 1329 return -1;
AdrienSalle 0:32fd5a08c430 1330 }
AdrienSalle 0:32fd5a08c430 1331 // Checksum2
AdrienSalle 0:32fd5a08c430 1332 uint8_t chksum2 = (~rxBuf[5]&0xFE);
AdrienSalle 0:32fd5a08c430 1333 if (chksum2 != rxBuf[6]) {
AdrienSalle 0:32fd5a08c430 1334 /* #ifdef HERKULEX_DEBUG
AdrienSalle 0:32fd5a08c430 1335 pc->printf("Checksum2 fault\n");
AdrienSalle 0:32fd5a08c430 1336 #endif*/
AdrienSalle 0:32fd5a08c430 1337 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 1338 flag_serial1_receive=0;
AdrienSalle 0:32fd5a08c430 1339 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 1340 flag_serial2_receive=0;
AdrienSalle 0:32fd5a08c430 1341 }
AdrienSalle 0:32fd5a08c430 1342 return -1;
AdrienSalle 0:32fd5a08c430 1343 }
AdrienSalle 0:32fd5a08c430 1344 Tor = rxBuf[9];
AdrienSalle 0:32fd5a08c430 1345 /* #ifdef HERKULEX_DEBUG
AdrienSalle 0:32fd5a08c430 1346 pc->printf("position = %04X(%d)\n", position, position);
AdrienSalle 0:32fd5a08c430 1347 #endif*/
AdrienSalle 0:32fd5a08c430 1348 //}
AdrienSalle 0:32fd5a08c430 1349 return Tor;
AdrienSalle 0:32fd5a08c430 1350 }
AdrienSalle 0:32fd5a08c430 1351 //---------------fonction pour lire le temperature max pour un servo------------
AdrienSalle 0:32fd5a08c430 1352 //obtenir la valeur de température maximum tolérée par le servomoteur
AdrienSalle 0:32fd5a08c430 1353 int8_t Get_Temperature_MAX(int8_t id, uint8_t numero_serial)
AdrienSalle 0:32fd5a08c430 1354 //
AdrienSalle 0:32fd5a08c430 1355 //id ==> id du servomoteur sur la liaison série
AdrienSalle 0:32fd5a08c430 1356 //numero_serial ==> numéro de la liaison série sur laquelle se trouve le servomoteur
AdrienSalle 0:32fd5a08c430 1357 //
AdrienSalle 0:32fd5a08c430 1358 {
AdrienSalle 0:32fd5a08c430 1359 serial_numero = numero_serial;
AdrienSalle 0:32fd5a08c430 1360 uint8_t txBuf[9];
AdrienSalle 0:32fd5a08c430 1361 int8_t tempeMAX = 0;
AdrienSalle 0:32fd5a08c430 1362
AdrienSalle 0:32fd5a08c430 1363 txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 1364 txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 1365 txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
AdrienSalle 0:32fd5a08c430 1366 txBuf[3] = id; // Servo ID
AdrienSalle 0:32fd5a08c430 1367 txBuf[4] = CMD_RAM_READ; // Command Ram Read
AdrienSalle 0:32fd5a08c430 1368 txBuf[5] = 0; // Checksum1
AdrienSalle 0:32fd5a08c430 1369 txBuf[6] = 0; // Checksum2
AdrienSalle 0:32fd5a08c430 1370 txBuf[7] = RAM_MAX_TEMPERATURE;
AdrienSalle 0:32fd5a08c430 1371 txBuf[8] = BYTE1; // Length
AdrienSalle 0:32fd5a08c430 1372 // Check Sum1 and Check Sum2
AdrienSalle 0:32fd5a08c430 1373 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
AdrienSalle 0:32fd5a08c430 1374 txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle 0:32fd5a08c430 1375
AdrienSalle 0:32fd5a08c430 1376 //pc.printf(" tempeMAX ");
AdrienSalle 0:32fd5a08c430 1377
AdrienSalle 0:32fd5a08c430 1378 uint8_t rxBuf[12];
AdrienSalle 0:32fd5a08c430 1379
AdrienSalle 0:32fd5a08c430 1380 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 1381 //send packet (mbed -> herkulex)
AdrienSalle 0:32fd5a08c430 1382 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 1383 while(serial1.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1384 serial1.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1385 wait_us(100);
AdrienSalle 0:32fd5a08c430 1386 }
AdrienSalle 0:32fd5a08c430 1387
AdrienSalle 0:32fd5a08c430 1388 //send packet (mbed -> herkulex)
AdrienSalle 0:32fd5a08c430 1389 //uint8_t rxBuf[12];
AdrienSalle 0:32fd5a08c430 1390 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1391 if(flag_serial1_receive) {
AdrienSalle 0:32fd5a08c430 1392 for(unsigned char i4=0; i4<Size_trame_serial1; i4++) {
AdrienSalle 0:32fd5a08c430 1393 rxBuf[i4] = char_receive_serial1[i4];
AdrienSalle 0:32fd5a08c430 1394 //////pc.printf("%d ",(int)char_receive_serial1[i4]);
AdrienSalle 0:32fd5a08c430 1395 }
AdrienSalle 0:32fd5a08c430 1396 flag_serial1_receive=0;
AdrienSalle 0:32fd5a08c430 1397 valueserial1=0;
AdrienSalle 0:32fd5a08c430 1398 }
AdrienSalle 0:32fd5a08c430 1399 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 1400 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 1401 while(serial2.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1402 serial2.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1403 wait_us(100);
AdrienSalle 0:32fd5a08c430 1404 }
AdrienSalle 0:32fd5a08c430 1405 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1406 if(flag_serial2_receive) {
AdrienSalle 0:32fd5a08c430 1407 for(unsigned char i4=0; i4<Size_trame_serial2; i4++) {
AdrienSalle 0:32fd5a08c430 1408 rxBuf[i4] = char_receive_serial2[i4];
AdrienSalle 0:32fd5a08c430 1409 //////pc.printf("%d ",(int)char_receive_serial2[i4]);
AdrienSalle 0:32fd5a08c430 1410 }
AdrienSalle 0:32fd5a08c430 1411 flag_serial2_receive=0;
AdrienSalle 0:32fd5a08c430 1412 valueserial2=0;
AdrienSalle 0:32fd5a08c430 1413 }
AdrienSalle 0:32fd5a08c430 1414 } else if(numero_serial == 3) {
AdrienSalle 0:32fd5a08c430 1415 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 1416 while(serial3.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1417 serial3.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1418 wait_us(100);
AdrienSalle 0:32fd5a08c430 1419 }
AdrienSalle 0:32fd5a08c430 1420 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1421 if(flag_serial3_receive) {
AdrienSalle 0:32fd5a08c430 1422 for(unsigned char i4=0; i4<Size_trame_serial3; i4++) {
AdrienSalle 0:32fd5a08c430 1423 rxBuf[i4] = char_receive_serial3[i4];
AdrienSalle 0:32fd5a08c430 1424 //////pc.printf("%d ",(int)char_receive_serial3[i4]);
AdrienSalle 0:32fd5a08c430 1425 }
AdrienSalle 0:32fd5a08c430 1426 flag_serial3_receive=0;
AdrienSalle 0:32fd5a08c430 1427 valueserial3=0;
AdrienSalle 0:32fd5a08c430 1428 }
AdrienSalle 0:32fd5a08c430 1429 } else if(numero_serial == 4) {
AdrienSalle 0:32fd5a08c430 1430 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 1431 while(serial4.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1432 serial4.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1433 wait_us(100);
AdrienSalle 0:32fd5a08c430 1434 }
AdrienSalle 0:32fd5a08c430 1435 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1436 if(flag_serial4_receive) {
AdrienSalle 0:32fd5a08c430 1437 for(unsigned char i4=0; i4<Size_trame_serial4; i4++) {
AdrienSalle 0:32fd5a08c430 1438 rxBuf[i4] = char_receive_serial4[i4];
AdrienSalle 0:32fd5a08c430 1439 //////pc.printf("%d ",(int)char_receive_serial4[i4]);
AdrienSalle 0:32fd5a08c430 1440 }
AdrienSalle 0:32fd5a08c430 1441 flag_serial4_receive=0;
AdrienSalle 0:32fd5a08c430 1442 valueserial4=0;
AdrienSalle 0:32fd5a08c430 1443 }
AdrienSalle 0:32fd5a08c430 1444 } else if(numero_serial == 5) {
AdrienSalle 0:32fd5a08c430 1445 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 1446 while(serial5.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1447 serial5.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1448 wait_us(100);
AdrienSalle 0:32fd5a08c430 1449 }
AdrienSalle 0:32fd5a08c430 1450 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1451 if(flag_serial5_receive) {
AdrienSalle 0:32fd5a08c430 1452 for(unsigned char i4=0; i4<Size_trame_serial5; i4++) {
AdrienSalle 0:32fd5a08c430 1453 rxBuf[i4] = char_receive_serial5[i4];
AdrienSalle 0:32fd5a08c430 1454 //////pc.printf("%d ",(int)char_receive_serial5[i4]);
AdrienSalle 0:32fd5a08c430 1455 }
AdrienSalle 0:32fd5a08c430 1456 flag_serial5_receive=0;
AdrienSalle 0:32fd5a08c430 1457 valueserial5=0;
AdrienSalle 0:32fd5a08c430 1458 }
AdrienSalle 0:32fd5a08c430 1459 }
AdrienSalle 0:32fd5a08c430 1460
AdrienSalle 0:32fd5a08c430 1461 uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]) & 0xFE;
AdrienSalle 0:32fd5a08c430 1462 if (chksum1 != rxBuf[5]) {
AdrienSalle 0:32fd5a08c430 1463 /*#ifdef HERKULEX_DEBUG
AdrienSalle 0:32fd5a08c430 1464 pc->printf("Checksum1 fault\n");
AdrienSalle 0:32fd5a08c430 1465 #endif*/
AdrienSalle 0:32fd5a08c430 1466 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 1467 flag_serial1_receive=0;
AdrienSalle 0:32fd5a08c430 1468 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 1469 flag_serial2_receive=0;
AdrienSalle 0:32fd5a08c430 1470 }
AdrienSalle 0:32fd5a08c430 1471 return -1;
AdrienSalle 0:32fd5a08c430 1472 }
AdrienSalle 0:32fd5a08c430 1473 // Checksum2
AdrienSalle 0:32fd5a08c430 1474 uint8_t chksum2 = (~rxBuf[5]&0xFE);
AdrienSalle 0:32fd5a08c430 1475 if (chksum2 != rxBuf[6]) {
AdrienSalle 0:32fd5a08c430 1476 /* #ifdef HERKULEX_DEBUG
AdrienSalle 0:32fd5a08c430 1477 pc->printf("Checksum2 fault\n");
AdrienSalle 0:32fd5a08c430 1478 #endif*/
AdrienSalle 0:32fd5a08c430 1479 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 1480 flag_serial1_receive=0;
AdrienSalle 0:32fd5a08c430 1481 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 1482 flag_serial2_receive=0;
AdrienSalle 0:32fd5a08c430 1483 }
AdrienSalle 0:32fd5a08c430 1484 return -1;
AdrienSalle 0:32fd5a08c430 1485 }
AdrienSalle 0:32fd5a08c430 1486 tempeMAX = rxBuf[9];
AdrienSalle 0:32fd5a08c430 1487 /* #ifdef HERKULEX_DEBUG
AdrienSalle 0:32fd5a08c430 1488 pc->printf("position = %04X(%d)\n", position, position);
AdrienSalle 0:32fd5a08c430 1489 #endif*/
AdrienSalle 0:32fd5a08c430 1490 //}
AdrienSalle 0:32fd5a08c430 1491 return tempeMAX;
AdrienSalle 0:32fd5a08c430 1492 }
AdrienSalle 0:32fd5a08c430 1493 //--------fonction de controle de position pour deux servo(same playtime)-------
AdrienSalle 0:32fd5a08c430 1494 //permet de déplacer deux servomoteurs sur la même liaison série avec le même temps d'execution
AdrienSalle 0:32fd5a08c430 1495 void positionControl_Mul_ensemble(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED,uint8_t id2, uint16_t position2, uint8_t setLED2, uint8_t numero_serial)
AdrienSalle 0:32fd5a08c430 1496 //
AdrienSalle 0:32fd5a08c430 1497 //id
AdrienSalle 0:32fd5a08c430 1498 //
AdrienSalle 0:32fd5a08c430 1499
AdrienSalle 0:32fd5a08c430 1500 {
AdrienSalle 0:32fd5a08c430 1501 serial_numero = numero_serial;
AdrienSalle 0:32fd5a08c430 1502 float tempo=0;
AdrienSalle 0:32fd5a08c430 1503 //if (position > 1023) return; //1002-21
AdrienSalle 0:32fd5a08c430 1504 if (playtime > 254) return; //1-254 == 11.2ms-2.844sec.
AdrienSalle 0:32fd5a08c430 1505 tempo=playtime*0.012;
AdrienSalle 0:32fd5a08c430 1506 uint8_t txBuf[16];
AdrienSalle 0:32fd5a08c430 1507 etat = pos;
AdrienSalle 0:32fd5a08c430 1508 pos_position = position;
AdrienSalle 0:32fd5a08c430 1509 pos_time = playtime;
AdrienSalle 0:32fd5a08c430 1510 pos_led = setLED;
AdrienSalle 0:32fd5a08c430 1511 txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 1512 txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 1513 txBuf[2] = MIN_PACKET_SIZE + 9; // Packet Size
AdrienSalle 0:32fd5a08c430 1514 //txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
AdrienSalle 0:32fd5a08c430 1515 txBuf[3] = 254; // broadcast ID
AdrienSalle 0:32fd5a08c430 1516 txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
AdrienSalle 0:32fd5a08c430 1517 txBuf[5] = 0; // Checksum1
AdrienSalle 0:32fd5a08c430 1518 txBuf[6] = 0; // Checksum2
AdrienSalle 0:32fd5a08c430 1519 txBuf[7] = playtime; // Playtime
AdrienSalle 0:32fd5a08c430 1520 txBuf[8] = position & 0x00FF; // Position (LSB, Least Significant Bit)
AdrienSalle 0:32fd5a08c430 1521 txBuf[9] =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
AdrienSalle 0:32fd5a08c430 1522 txBuf[10] = POS_MODE | setLED; // Pos Mode and LED on/off
AdrienSalle 0:32fd5a08c430 1523 txBuf[11] = id; // Servo ID
AdrienSalle 0:32fd5a08c430 1524 txBuf[12] = position2 & 0x00FF; // Position (LSB, Least Significant Bit)
AdrienSalle 0:32fd5a08c430 1525 txBuf[13] =(position2 & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
AdrienSalle 0:32fd5a08c430 1526 txBuf[14] = POS_MODE | setLED2; // Pos Mode and LED on/off
AdrienSalle 0:32fd5a08c430 1527 txBuf[15] = id2; // Servo ID
AdrienSalle 0:32fd5a08c430 1528 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
AdrienSalle 0:32fd5a08c430 1529 // Checksum2 = (~Checksum1)&0xFE
AdrienSalle 0:32fd5a08c430 1530 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;
AdrienSalle 0:32fd5a08c430 1531 txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle 0:32fd5a08c430 1532 // send packet (mbed -> herkulex)
AdrienSalle 0:32fd5a08c430 1533
AdrienSalle 0:32fd5a08c430 1534 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 1535 for(uint8_t i = 0; i < 16 ; i++) {
AdrienSalle 0:32fd5a08c430 1536 while(serial1.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1537 serial1.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1538 wait_us(100);
AdrienSalle 0:32fd5a08c430 1539 }
AdrienSalle 0:32fd5a08c430 1540 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 1541 for(uint8_t i = 0; i < 16 ; i++) {
AdrienSalle 0:32fd5a08c430 1542 while(serial2.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1543 serial2.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1544 wait_us(100);
AdrienSalle 0:32fd5a08c430 1545 }
AdrienSalle 0:32fd5a08c430 1546 } else if(numero_serial == 3) {
AdrienSalle 0:32fd5a08c430 1547 for(uint8_t i = 0; i < 16 ; i++) {
AdrienSalle 0:32fd5a08c430 1548 while(serial3.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1549 serial3.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1550 wait_us(100);
AdrienSalle 0:32fd5a08c430 1551 }
AdrienSalle 0:32fd5a08c430 1552 } else if(numero_serial == 4) {
AdrienSalle 0:32fd5a08c430 1553 for(uint8_t i = 0; i < 16 ; i++) {
AdrienSalle 0:32fd5a08c430 1554 while(serial4.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1555 serial4.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1556 wait_us(100);
AdrienSalle 0:32fd5a08c430 1557 }
AdrienSalle 0:32fd5a08c430 1558 } else if(numero_serial == 5) {
AdrienSalle 0:32fd5a08c430 1559 for(uint8_t i = 0; i < 16 ; i++) {
AdrienSalle 0:32fd5a08c430 1560 while(serial5.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1561 serial5.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1562 wait_us(100);
AdrienSalle 0:32fd5a08c430 1563 }
AdrienSalle 0:32fd5a08c430 1564 }
AdrienSalle 0:32fd5a08c430 1565 wait(tempo);
AdrienSalle 0:32fd5a08c430 1566 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1567 }
AdrienSalle 0:32fd5a08c430 1568 //-----fonction de controle de position pour deux servo(different playtime)----- //a changer...
AdrienSalle 0:32fd5a08c430 1569 void positionControl_Mul_playtime_different(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED,uint8_t id2, uint16_t position2, uint8_t playtime2, uint8_t setLED2, uint8_t numero_serial)
AdrienSalle 0:32fd5a08c430 1570 //
AdrienSalle 0:32fd5a08c430 1571 //permet de controler deux servomoteurs avec des temps d'execution différents
AdrienSalle 0:32fd5a08c430 1572 //
AdrienSalle 0:32fd5a08c430 1573 {
AdrienSalle 0:32fd5a08c430 1574 serial_numero = numero_serial;
AdrienSalle 0:32fd5a08c430 1575 float tempo=0;
AdrienSalle 0:32fd5a08c430 1576 //if (position > 1023) return; //1002-21
AdrienSalle 0:32fd5a08c430 1577 if (playtime > 254) playtime = 254; //return; //1-254 == 11.2ms-2.844sec.
AdrienSalle 0:32fd5a08c430 1578 if(playtime>playtime2) {
AdrienSalle 0:32fd5a08c430 1579 tempo=playtime*0.012;
AdrienSalle 0:32fd5a08c430 1580 } else if(playtime<playtime2) {
AdrienSalle 0:32fd5a08c430 1581 tempo=playtime2*0.012;
AdrienSalle 0:32fd5a08c430 1582 }
AdrienSalle 0:32fd5a08c430 1583 uint8_t txBuf[17];
AdrienSalle 0:32fd5a08c430 1584 etat = pos;
AdrienSalle 0:32fd5a08c430 1585 pos_position = position;
AdrienSalle 0:32fd5a08c430 1586 pos_time = playtime;
AdrienSalle 0:32fd5a08c430 1587 pos_led = setLED;
AdrienSalle 0:32fd5a08c430 1588 txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 1589 txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 1590 txBuf[2] = MIN_PACKET_SIZE + 9; // Packet Size
AdrienSalle 0:32fd5a08c430 1591 //txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
AdrienSalle 0:32fd5a08c430 1592 txBuf[3] = 254; // broadcast ID
AdrienSalle 0:32fd5a08c430 1593 txBuf[4] = CMD_I_JOG; // Command I JOG
AdrienSalle 0:32fd5a08c430 1594 txBuf[5] = 0; // Checksum1
AdrienSalle 0:32fd5a08c430 1595 txBuf[6] = 0; // Checksum2
AdrienSalle 0:32fd5a08c430 1596 txBuf[7] = position & 0x00FF; // Position (LSB, Least Significant Bit)
AdrienSalle 0:32fd5a08c430 1597 txBuf[8] =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
AdrienSalle 0:32fd5a08c430 1598 txBuf[9] = POS_MODE | setLED; // Pos Mode and LED on/off
AdrienSalle 0:32fd5a08c430 1599 txBuf[10] = id; // Servo ID
AdrienSalle 0:32fd5a08c430 1600 txBuf[11] = playtime; // Playtime
AdrienSalle 0:32fd5a08c430 1601 txBuf[12] = position2 & 0x00FF; // Position (LSB, Least Significant Bit)
AdrienSalle 0:32fd5a08c430 1602 txBuf[13] =(position2 & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
AdrienSalle 0:32fd5a08c430 1603 txBuf[14] = POS_MODE | setLED2; // Pos Mode and LED on/off
AdrienSalle 0:32fd5a08c430 1604 txBuf[15] = id2; // Servo ID
AdrienSalle 0:32fd5a08c430 1605 txBuf[16] = playtime2; // Playtime
AdrienSalle 0:32fd5a08c430 1606 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
AdrienSalle 0:32fd5a08c430 1607 // Checksum2 = (~Checksum1)&0xFE
AdrienSalle 0:32fd5a08c430 1608 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;
AdrienSalle 0:32fd5a08c430 1609 txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle 0:32fd5a08c430 1610 // send packet (mbed -> herkulex)
AdrienSalle 0:32fd5a08c430 1611 //txPacket(12, txBuf);
AdrienSalle 0:32fd5a08c430 1612
AdrienSalle 0:32fd5a08c430 1613 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 1614 for(uint8_t i = 0; i < 17 ; i++) {
AdrienSalle 0:32fd5a08c430 1615 while(serial1.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1616 serial1.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1617 wait_us(100);
AdrienSalle 0:32fd5a08c430 1618 }
AdrienSalle 0:32fd5a08c430 1619 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 1620 for(uint8_t i = 0; i < 17 ; i++) {
AdrienSalle 0:32fd5a08c430 1621 while(serial2.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1622 serial2.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1623 wait_us(100);
AdrienSalle 0:32fd5a08c430 1624 }
AdrienSalle 0:32fd5a08c430 1625 } else if(numero_serial == 3) {
AdrienSalle 0:32fd5a08c430 1626 for(uint8_t i = 0; i < 17 ; i++) {
AdrienSalle 0:32fd5a08c430 1627 while(serial3.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1628 serial3.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1629 wait_us(100);
AdrienSalle 0:32fd5a08c430 1630 }
AdrienSalle 0:32fd5a08c430 1631 } else if(numero_serial == 4) {
AdrienSalle 0:32fd5a08c430 1632 for(uint8_t i = 0; i < 17 ; i++) {
AdrienSalle 0:32fd5a08c430 1633 while(serial4.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1634 serial4.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1635 wait_us(100);
AdrienSalle 0:32fd5a08c430 1636 }
AdrienSalle 0:32fd5a08c430 1637 } else if(numero_serial == 5) {
AdrienSalle 0:32fd5a08c430 1638 for(uint8_t i = 0; i < 17 ; i++) {
AdrienSalle 0:32fd5a08c430 1639 while(serial5.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1640 serial5.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1641 wait_us(100);
AdrienSalle 0:32fd5a08c430 1642 }
AdrienSalle 0:32fd5a08c430 1643 }
AdrienSalle 0:32fd5a08c430 1644
AdrienSalle 0:32fd5a08c430 1645 wait(tempo);
AdrienSalle 0:32fd5a08c430 1646 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1647 }
AdrienSalle 0:32fd5a08c430 1648 //-----fonction de controle de position pour plusieurs servo(same playtime)-----
AdrienSalle 0:32fd5a08c430 1649 void positionControl_Mul_ensemble_complex(uint8_t nb_servo, uint8_t playtime, uint8_t* data, uint16_t* pos, uint8_t numero_serial) // uint16_t position, uint8_t setLED, uint8_t id
AdrienSalle 0:32fd5a08c430 1650 //
AdrienSalle 0:32fd5a08c430 1651 //Permet de controler tout les servos de la même liaison série avec le même temps d'execution
AdrienSalle 0:32fd5a08c430 1652 //
AdrienSalle 0:32fd5a08c430 1653 //
AdrienSalle 0:32fd5a08c430 1654 {
AdrienSalle 0:32fd5a08c430 1655 serial_numero = numero_serial;
AdrienSalle 0:32fd5a08c430 1656 //float tempo=0;
AdrienSalle 0:32fd5a08c430 1657 uint8_t taille = 0,i = 0,idata = 0, ipos = 0;
AdrienSalle 0:32fd5a08c430 1658 //if (position > 1023) return; //1002-21
AdrienSalle 0:32fd5a08c430 1659 if (playtime > 254) return; //1-254 == 11.2ms-2.844sec.
AdrienSalle 0:32fd5a08c430 1660 //tempo=playtime*0.012;
AdrienSalle 0:32fd5a08c430 1661 taille = 7 + 1 + 4 * nb_servo;
AdrienSalle 0:32fd5a08c430 1662 nombre_servo = nb_servo;
AdrienSalle 0:32fd5a08c430 1663 pos_time = playtime;
AdrienSalle 0:32fd5a08c430 1664 uint8_t txBuf[taille];
AdrienSalle 0:32fd5a08c430 1665 etat = pos_mul_complex;
AdrienSalle 0:32fd5a08c430 1666
AdrienSalle 0:32fd5a08c430 1667 txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 1668 txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 1669 txBuf[2] = MIN_PACKET_SIZE + 1 + 4 * nb_servo; // Packet Size
AdrienSalle 0:32fd5a08c430 1670 //txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
AdrienSalle 0:32fd5a08c430 1671 txBuf[3] = 254; // broadcast ID
AdrienSalle 0:32fd5a08c430 1672 txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
AdrienSalle 0:32fd5a08c430 1673 txBuf[5] = 0; // Checksum1
AdrienSalle 0:32fd5a08c430 1674 txBuf[6] = 0; // Checksum2
AdrienSalle 0:32fd5a08c430 1675 txBuf[7] = playtime; // Playtime
AdrienSalle 0:32fd5a08c430 1676
AdrienSalle 0:32fd5a08c430 1677 for(i=0; i<nb_servo; i++) {
AdrienSalle 0:32fd5a08c430 1678 txBuf[8+i*4] = pos[ipos] & 0x00FF; // Position (LSB, Least Significant Bit)
AdrienSalle 0:32fd5a08c430 1679 txBuf[9+i*4] =(pos[ipos] & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
AdrienSalle 0:32fd5a08c430 1680 position_servo_mul[ipos] = pos[ipos];
AdrienSalle 0:32fd5a08c430 1681 ipos++;
AdrienSalle 0:32fd5a08c430 1682 txBuf[10+i*4] = POS_MODE | data[idata]; // Pos Mode and LED on/off
AdrienSalle 0:32fd5a08c430 1683 data_servo_mul[idata] = data[idata];
AdrienSalle 0:32fd5a08c430 1684 idata++;
AdrienSalle 0:32fd5a08c430 1685 txBuf[11+i*4] = data[idata]; // Servo ID
AdrienSalle 0:32fd5a08c430 1686 data_servo_mul[idata] = data[idata];
AdrienSalle 0:32fd5a08c430 1687 idata++;
AdrienSalle 0:32fd5a08c430 1688 }
AdrienSalle 0:32fd5a08c430 1689
AdrienSalle 0:32fd5a08c430 1690 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
AdrienSalle 0:32fd5a08c430 1691 // Checksum2 = (~Checksum1)&0xFE
AdrienSalle 0:32fd5a08c430 1692 txBuf[5] = txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7];
AdrienSalle 0:32fd5a08c430 1693
AdrienSalle 0:32fd5a08c430 1694 for(i=1; i<(taille-7); i++) {
AdrienSalle 0:32fd5a08c430 1695 txBuf[5]=txBuf[5]^txBuf[7+i];
AdrienSalle 0:32fd5a08c430 1696 }
AdrienSalle 0:32fd5a08c430 1697 txBuf[5] = txBuf[5]& 0xFE;
AdrienSalle 0:32fd5a08c430 1698 txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle 0:32fd5a08c430 1699
AdrienSalle 0:32fd5a08c430 1700 // send packet (mbed -> herkulex)
AdrienSalle 0:32fd5a08c430 1701 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 1702 for(uint8_t i = 0; i < taille ; i++) {
AdrienSalle 0:32fd5a08c430 1703 while(serial1.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1704 serial1.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1705 wait_us(100);
AdrienSalle 0:32fd5a08c430 1706 }
AdrienSalle 0:32fd5a08c430 1707 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 1708 for(uint8_t i = 0; i < taille ; i++) {
AdrienSalle 0:32fd5a08c430 1709 while(serial2.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1710 serial2.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1711 wait_us(100);
AdrienSalle 0:32fd5a08c430 1712 }
AdrienSalle 0:32fd5a08c430 1713 } else if(numero_serial == 3) {
AdrienSalle 0:32fd5a08c430 1714 for(uint8_t i = 0; i < taille ; i++) {
AdrienSalle 0:32fd5a08c430 1715 while(serial3.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1716 serial3.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1717 wait_us(100);
AdrienSalle 0:32fd5a08c430 1718 }
AdrienSalle 0:32fd5a08c430 1719 } else if(numero_serial == 4) {
AdrienSalle 0:32fd5a08c430 1720 for(uint8_t i = 0; i < taille ; i++) {
AdrienSalle 0:32fd5a08c430 1721 while(serial4.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1722 serial4.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1723 wait_us(100);
AdrienSalle 0:32fd5a08c430 1724 }
AdrienSalle 0:32fd5a08c430 1725 } else if(numero_serial == 5) {
AdrienSalle 0:32fd5a08c430 1726 for(uint8_t i = 0; i < taille ; i++) {
AdrienSalle 0:32fd5a08c430 1727 while(serial5.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1728 serial5.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1729 wait_us(100);
AdrienSalle 0:32fd5a08c430 1730 }
AdrienSalle 0:32fd5a08c430 1731 }
AdrienSalle 0:32fd5a08c430 1732
AdrienSalle 0:32fd5a08c430 1733 /*for(uint8_t i = 0; i < taille ; i++)
AdrienSalle 0:32fd5a08c430 1734 {
AdrienSalle 0:32fd5a08c430 1735 while(serial1.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1736 serial1.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1737 wait_us(100);
AdrienSalle 0:32fd5a08c430 1738 }*/
AdrienSalle 0:32fd5a08c430 1739 //wait(tempo);
AdrienSalle 0:32fd5a08c430 1740 //wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1741 }
AdrienSalle 0:32fd5a08c430 1742 //--fonction de controle de position pour plusieurs servo(different playtime)---
AdrienSalle 0:32fd5a08c430 1743 void positionControl_Mul_ensemble_different_complex(uint8_t nb_servo, uint8_t* data, uint16_t* pos, uint8_t numero_serial) // uint16_t position, uint8_t setLED, uint8_t id, uint8_t playtime
AdrienSalle 0:32fd5a08c430 1744 //
AdrienSalle 0:32fd5a08c430 1745 //Permet de controler tout les servos de la même liaison série avec un temps d'execution différent
AdrienSalle 0:32fd5a08c430 1746 //
AdrienSalle 0:32fd5a08c430 1747 {
AdrienSalle 0:32fd5a08c430 1748 serial_numero = numero_serial;
AdrienSalle 0:32fd5a08c430 1749 float tempo=0;
AdrienSalle 0:32fd5a08c430 1750 uint8_t Max_playtime = 0;
AdrienSalle 0:32fd5a08c430 1751 uint8_t taille = 0,i = 0,idata = 0, ipos = 0,iplay_time = 0;
AdrienSalle 0:32fd5a08c430 1752 //if (position > 1023) return; //1002-21
AdrienSalle 0:32fd5a08c430 1753 //if (playtime > 254) return; //1-254 == 11.2ms-2.844sec.
AdrienSalle 0:32fd5a08c430 1754
AdrienSalle 0:32fd5a08c430 1755 for(iplay_time=0; iplay_time<nb_servo; iplay_time++) {
AdrienSalle 0:32fd5a08c430 1756 if(Max_playtime<data[2+3*iplay_time]) {
AdrienSalle 0:32fd5a08c430 1757 Max_playtime=data[2+3*iplay_time];
AdrienSalle 0:32fd5a08c430 1758 }
AdrienSalle 0:32fd5a08c430 1759 }
AdrienSalle 0:32fd5a08c430 1760 tempo=Max_playtime*0.012;
AdrienSalle 0:32fd5a08c430 1761 taille = 7 + 5 * nb_servo;
AdrienSalle 0:32fd5a08c430 1762 nombre_servo = nb_servo;
AdrienSalle 0:32fd5a08c430 1763 uint8_t txBuf[taille];
AdrienSalle 0:32fd5a08c430 1764 etat = pos_mul_complex_different;
AdrienSalle 0:32fd5a08c430 1765
AdrienSalle 0:32fd5a08c430 1766 txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 1767 txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 1768 txBuf[2] = MIN_PACKET_SIZE + 5 * nb_servo; // Packet Size
AdrienSalle 0:32fd5a08c430 1769 //txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
AdrienSalle 0:32fd5a08c430 1770 txBuf[3] = 254; // broadcast ID
AdrienSalle 0:32fd5a08c430 1771 txBuf[4] = CMD_I_JOG; // Command I JOG (0x06)
AdrienSalle 0:32fd5a08c430 1772 txBuf[5] = 0; // Checksum1
AdrienSalle 0:32fd5a08c430 1773 txBuf[6] = 0; // Checksum2
AdrienSalle 0:32fd5a08c430 1774
AdrienSalle 0:32fd5a08c430 1775 for(i=0; i<nb_servo; i++) {
AdrienSalle 0:32fd5a08c430 1776 txBuf[7+i*5] = pos[ipos] & 0x00FF; // Position (LSB, Least Significant Bit)
AdrienSalle 0:32fd5a08c430 1777 txBuf[8+i*5] =(pos[ipos] & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
AdrienSalle 0:32fd5a08c430 1778 position_servo_mul_different[ipos] = pos[ipos];
AdrienSalle 0:32fd5a08c430 1779 ipos++;
AdrienSalle 0:32fd5a08c430 1780 txBuf[9+i*5] = POS_MODE | data[idata]; // Pos Mode and LED on/off
AdrienSalle 0:32fd5a08c430 1781 data_servo_mul_different[idata] = data[idata];
AdrienSalle 0:32fd5a08c430 1782 idata++;
AdrienSalle 0:32fd5a08c430 1783 txBuf[10+i*5] = data[idata]; // Servo ID
AdrienSalle 0:32fd5a08c430 1784 data_servo_mul_different[idata] = data[idata];
AdrienSalle 0:32fd5a08c430 1785 idata++;
AdrienSalle 0:32fd5a08c430 1786 txBuf[11+i*5] = data[idata]; // Playtime
AdrienSalle 0:32fd5a08c430 1787 data_servo_mul_different[idata] = data[idata];
AdrienSalle 0:32fd5a08c430 1788 idata++;
AdrienSalle 0:32fd5a08c430 1789 }
AdrienSalle 0:32fd5a08c430 1790
AdrienSalle 0:32fd5a08c430 1791 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
AdrienSalle 0:32fd5a08c430 1792 // Checksum2 = (~Checksum1)&0xFE
AdrienSalle 0:32fd5a08c430 1793 txBuf[5] = txBuf[2]^txBuf[3]^txBuf[4];
AdrienSalle 0:32fd5a08c430 1794
AdrienSalle 0:32fd5a08c430 1795 for(i=1; i<(taille-6); i++) {
AdrienSalle 0:32fd5a08c430 1796 txBuf[5]=txBuf[5]^txBuf[6+i];
AdrienSalle 0:32fd5a08c430 1797 }
AdrienSalle 0:32fd5a08c430 1798 txBuf[5] = txBuf[5]& 0xFE;
AdrienSalle 0:32fd5a08c430 1799 txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle 0:32fd5a08c430 1800
AdrienSalle 0:32fd5a08c430 1801 // send packet (mbed -> herkulex)
AdrienSalle 0:32fd5a08c430 1802 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 1803 for(uint8_t i = 0; i < taille ; i++) {
AdrienSalle 0:32fd5a08c430 1804 while(serial1.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1805 serial1.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1806 wait_us(100);
AdrienSalle 0:32fd5a08c430 1807 }
AdrienSalle 0:32fd5a08c430 1808 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 1809 for(uint8_t i = 0; i < taille ; i++) {
AdrienSalle 0:32fd5a08c430 1810 while(serial2.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1811 serial2.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1812 wait_us(100);
AdrienSalle 0:32fd5a08c430 1813 }
AdrienSalle 0:32fd5a08c430 1814 } else if(numero_serial == 3) {
AdrienSalle 0:32fd5a08c430 1815 for(uint8_t i = 0; i < taille ; i++) {
AdrienSalle 0:32fd5a08c430 1816 while(serial3.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1817 serial3.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1818 wait_us(100);
AdrienSalle 0:32fd5a08c430 1819 }
AdrienSalle 0:32fd5a08c430 1820 } else if(numero_serial == 4) {
AdrienSalle 0:32fd5a08c430 1821 for(uint8_t i = 0; i < taille ; i++) {
AdrienSalle 0:32fd5a08c430 1822 while(serial4.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1823 serial4.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1824 wait_us(100);
AdrienSalle 0:32fd5a08c430 1825 }
AdrienSalle 0:32fd5a08c430 1826 } else if(numero_serial == 5) {
AdrienSalle 0:32fd5a08c430 1827 for(uint8_t i = 0; i < taille ; i++) {
AdrienSalle 0:32fd5a08c430 1828 while(serial5.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1829 serial5.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1830 wait_us(100);
AdrienSalle 0:32fd5a08c430 1831 }
AdrienSalle 0:32fd5a08c430 1832 }
AdrienSalle 0:32fd5a08c430 1833
AdrienSalle 0:32fd5a08c430 1834 /*for(uint8_t i = 0; i < taille ; i++)
AdrienSalle 0:32fd5a08c430 1835 {
AdrienSalle 0:32fd5a08c430 1836 while(serial1.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1837 serial1.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1838 wait_us(100);
AdrienSalle 0:32fd5a08c430 1839 }*/
AdrienSalle 0:32fd5a08c430 1840 wait(tempo);
AdrienSalle 0:32fd5a08c430 1841 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1842 }
AdrienSalle 0:32fd5a08c430 1843 //---------------fonction pour lire la tension minimale pour un servo----------------
AdrienSalle 0:32fd5a08c430 1844 int8_t Get_Tension_MIN(int8_t id, uint8_t numero_serial)
AdrienSalle 0:32fd5a08c430 1845 {
AdrienSalle 0:32fd5a08c430 1846 serial_numero = numero_serial;
AdrienSalle 0:32fd5a08c430 1847 uint8_t txBuf[9];
AdrienSalle 0:32fd5a08c430 1848 int8_t tensionMIN = 0;
AdrienSalle 0:32fd5a08c430 1849
AdrienSalle 0:32fd5a08c430 1850 txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 1851 txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 1852 txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
AdrienSalle 0:32fd5a08c430 1853 txBuf[3] = id; // Servo ID
AdrienSalle 0:32fd5a08c430 1854 txBuf[4] = CMD_RAM_READ; // Command Ram Read
AdrienSalle 0:32fd5a08c430 1855 txBuf[5] = 0; // Checksum1
AdrienSalle 0:32fd5a08c430 1856 txBuf[6] = 0; // Checksum2
AdrienSalle 0:32fd5a08c430 1857 txBuf[7] = RAM_MIN_VOLTAGE;
AdrienSalle 0:32fd5a08c430 1858 txBuf[8] = BYTE1; // Length
AdrienSalle 0:32fd5a08c430 1859 // Check Sum1 and Check Sum2
AdrienSalle 0:32fd5a08c430 1860 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
AdrienSalle 0:32fd5a08c430 1861 txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle 0:32fd5a08c430 1862
AdrienSalle 0:32fd5a08c430 1863 //pc.printf(" tensionMIN ");
AdrienSalle 0:32fd5a08c430 1864
AdrienSalle 0:32fd5a08c430 1865 uint8_t rxBuf[12];
AdrienSalle 0:32fd5a08c430 1866
AdrienSalle 0:32fd5a08c430 1867 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 1868 //send packet (mbed -> herkulex)
AdrienSalle 0:32fd5a08c430 1869 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 1870 while(serial1.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1871 serial1.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1872 wait_us(100);
AdrienSalle 0:32fd5a08c430 1873 }
AdrienSalle 0:32fd5a08c430 1874
AdrienSalle 0:32fd5a08c430 1875 //send packet (mbed -> herkulex)
AdrienSalle 0:32fd5a08c430 1876 //uint8_t rxBuf[12];
AdrienSalle 0:32fd5a08c430 1877 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1878 if(flag_serial1_receive) {
AdrienSalle 0:32fd5a08c430 1879 for(unsigned char i4=0; i4<Size_trame_serial1; i4++) {
AdrienSalle 0:32fd5a08c430 1880 rxBuf[i4] = char_receive_serial1[i4];
AdrienSalle 0:32fd5a08c430 1881 //////pc.printf("%d ",(int)char_receive_serial1[i4]);
AdrienSalle 0:32fd5a08c430 1882 }
AdrienSalle 0:32fd5a08c430 1883 flag_serial1_receive=0;
AdrienSalle 0:32fd5a08c430 1884 valueserial1=0;
AdrienSalle 0:32fd5a08c430 1885 }
AdrienSalle 0:32fd5a08c430 1886 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 1887 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 1888 while(serial2.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1889 serial2.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1890 wait_us(100);
AdrienSalle 0:32fd5a08c430 1891 }
AdrienSalle 0:32fd5a08c430 1892 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1893 if(flag_serial2_receive) {
AdrienSalle 0:32fd5a08c430 1894 for(unsigned char i4=0; i4<Size_trame_serial2; i4++) {
AdrienSalle 0:32fd5a08c430 1895 rxBuf[i4] = char_receive_serial2[i4];
AdrienSalle 0:32fd5a08c430 1896 //////pc.printf("%d ",(int)char_receive_serial2[i4]);
AdrienSalle 0:32fd5a08c430 1897 }
AdrienSalle 0:32fd5a08c430 1898 flag_serial2_receive=0;
AdrienSalle 0:32fd5a08c430 1899 valueserial2=0;
AdrienSalle 0:32fd5a08c430 1900 }
AdrienSalle 0:32fd5a08c430 1901 } else if(numero_serial == 3) {
AdrienSalle 0:32fd5a08c430 1902 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 1903 while(serial3.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1904 serial3.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1905 wait_us(100);
AdrienSalle 0:32fd5a08c430 1906 }
AdrienSalle 0:32fd5a08c430 1907 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1908 if(flag_serial3_receive) {
AdrienSalle 0:32fd5a08c430 1909 for(unsigned char i4=0; i4<Size_trame_serial3; i4++) {
AdrienSalle 0:32fd5a08c430 1910 rxBuf[i4] = char_receive_serial3[i4];
AdrienSalle 0:32fd5a08c430 1911 //////pc.printf("%d ",(int)char_receive_serial3[i4]);
AdrienSalle 0:32fd5a08c430 1912 }
AdrienSalle 0:32fd5a08c430 1913 flag_serial3_receive=0;
AdrienSalle 0:32fd5a08c430 1914 valueserial3=0;
AdrienSalle 0:32fd5a08c430 1915 }
AdrienSalle 0:32fd5a08c430 1916 } else if(numero_serial == 4) {
AdrienSalle 0:32fd5a08c430 1917 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 1918 while(serial4.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1919 serial4.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1920 wait_us(100);
AdrienSalle 0:32fd5a08c430 1921 }
AdrienSalle 0:32fd5a08c430 1922 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1923 if(flag_serial4_receive) {
AdrienSalle 0:32fd5a08c430 1924 for(unsigned char i4=0; i4<Size_trame_serial4; i4++) {
AdrienSalle 0:32fd5a08c430 1925 rxBuf[i4] = char_receive_serial4[i4];
AdrienSalle 0:32fd5a08c430 1926 //////pc.printf("%d ",(int)char_receive_serial4[i4]);
AdrienSalle 0:32fd5a08c430 1927 }
AdrienSalle 0:32fd5a08c430 1928 flag_serial4_receive=0;
AdrienSalle 0:32fd5a08c430 1929 valueserial4=0;
AdrienSalle 0:32fd5a08c430 1930 }
AdrienSalle 0:32fd5a08c430 1931 } else if(numero_serial == 5) {
AdrienSalle 0:32fd5a08c430 1932 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 1933 while(serial5.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1934 serial5.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1935 wait_us(100);
AdrienSalle 0:32fd5a08c430 1936 }
AdrienSalle 0:32fd5a08c430 1937 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1938 if(flag_serial5_receive) {
AdrienSalle 0:32fd5a08c430 1939 for(unsigned char i4=0; i4<Size_trame_serial5; i4++) {
AdrienSalle 0:32fd5a08c430 1940 rxBuf[i4] = char_receive_serial5[i4];
AdrienSalle 0:32fd5a08c430 1941 //////pc.printf("%d ",(int)char_receive_serial5[i4]);
AdrienSalle 0:32fd5a08c430 1942 }
AdrienSalle 0:32fd5a08c430 1943 flag_serial5_receive=0;
AdrienSalle 0:32fd5a08c430 1944 valueserial5=0;
AdrienSalle 0:32fd5a08c430 1945 }
AdrienSalle 0:32fd5a08c430 1946 }
AdrienSalle 0:32fd5a08c430 1947
AdrienSalle 0:32fd5a08c430 1948 /*for(uint8_t i = 0; i < 9 ; i++)
AdrienSalle 0:32fd5a08c430 1949 {
AdrienSalle 0:32fd5a08c430 1950 while(serial1.writeable() == 0);
AdrienSalle 0:32fd5a08c430 1951 serial1.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 1952 wait_us(100);
AdrienSalle 0:32fd5a08c430 1953 }
AdrienSalle 0:32fd5a08c430 1954
AdrienSalle 0:32fd5a08c430 1955 // send packet (mbed -> herkulex)
AdrienSalle 0:32fd5a08c430 1956 uint8_t rxBuf[12];
AdrienSalle 0:32fd5a08c430 1957 //wait_ms(3);
AdrienSalle 0:32fd5a08c430 1958 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 1959 if(flag_serial1_receive)
AdrienSalle 0:32fd5a08c430 1960 {
AdrienSalle 0:32fd5a08c430 1961 for(unsigned char i4=0;i4<Size_trame_serial1; i4++)
AdrienSalle 0:32fd5a08c430 1962 {
AdrienSalle 0:32fd5a08c430 1963 rxBuf[i4] = char_receive_serial1[i4];
AdrienSalle 0:32fd5a08c430 1964 ////pc.printf("%d ",(int)char_receive_serial1[i4]);
AdrienSalle 0:32fd5a08c430 1965 }
AdrienSalle 0:32fd5a08c430 1966 flag_serial1_receive=0;
AdrienSalle 0:32fd5a08c430 1967 valueserial1=0;
AdrienSalle 0:32fd5a08c430 1968 }*/
AdrienSalle 0:32fd5a08c430 1969
AdrienSalle 0:32fd5a08c430 1970 uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]) & 0xFE;
AdrienSalle 0:32fd5a08c430 1971 if (chksum1 != rxBuf[5]) {
AdrienSalle 0:32fd5a08c430 1972 /*#ifdef HERKULEX_DEBUG
AdrienSalle 0:32fd5a08c430 1973 pc->printf("Checksum1 fault\n");
AdrienSalle 0:32fd5a08c430 1974 #endif*/
AdrienSalle 0:32fd5a08c430 1975 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 1976 flag_serial1_receive=0;
AdrienSalle 0:32fd5a08c430 1977 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 1978 flag_serial2_receive=0;
AdrienSalle 0:32fd5a08c430 1979 }
AdrienSalle 0:32fd5a08c430 1980 return -1;
AdrienSalle 0:32fd5a08c430 1981 }
AdrienSalle 0:32fd5a08c430 1982 // Checksum2
AdrienSalle 0:32fd5a08c430 1983 uint8_t chksum2 = (~rxBuf[5]&0xFE);
AdrienSalle 0:32fd5a08c430 1984 if (chksum2 != rxBuf[6]) {
AdrienSalle 0:32fd5a08c430 1985 /* #ifdef HERKULEX_DEBUG
AdrienSalle 0:32fd5a08c430 1986 pc->printf("Checksum2 fault\n");
AdrienSalle 0:32fd5a08c430 1987 #endif*/
AdrienSalle 0:32fd5a08c430 1988 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 1989 flag_serial1_receive=0;
AdrienSalle 0:32fd5a08c430 1990 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 1991 flag_serial2_receive=0;
AdrienSalle 0:32fd5a08c430 1992 }
AdrienSalle 0:32fd5a08c430 1993 return -1;
AdrienSalle 0:32fd5a08c430 1994 }
AdrienSalle 0:32fd5a08c430 1995 tensionMIN = rxBuf[9];
AdrienSalle 0:32fd5a08c430 1996 /* #ifdef HERKULEX_DEBUG
AdrienSalle 0:32fd5a08c430 1997 pc->printf("position = %04X(%d)\n", position, position);
AdrienSalle 0:32fd5a08c430 1998 #endif*/
AdrienSalle 0:32fd5a08c430 1999 //}
AdrienSalle 0:32fd5a08c430 2000 return tensionMIN;
AdrienSalle 0:32fd5a08c430 2001 }
AdrienSalle 0:32fd5a08c430 2002 //-------------fonction pour controle la tension min pour un servo--------------
AdrienSalle 0:32fd5a08c430 2003 void Set_Tension_MIN(int8_t id,uint8_t Tension_Min, uint8_t numero_serial)
AdrienSalle 0:32fd5a08c430 2004 {
AdrienSalle 0:32fd5a08c430 2005 serial_numero = numero_serial;
AdrienSalle 0:32fd5a08c430 2006 uint8_t txBuf[10];
AdrienSalle 0:32fd5a08c430 2007
AdrienSalle 0:32fd5a08c430 2008 txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 2009 txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 2010 txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size
AdrienSalle 0:32fd5a08c430 2011 txBuf[3] = id; // Servo ID
AdrienSalle 0:32fd5a08c430 2012 txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03)
AdrienSalle 0:32fd5a08c430 2013 txBuf[5] = 0; // Checksum1
AdrienSalle 0:32fd5a08c430 2014 txBuf[6] = 0; // Checksum2
AdrienSalle 0:32fd5a08c430 2015 txBuf[7] = RAM_MIN_VOLTAGE;
AdrienSalle 0:32fd5a08c430 2016 txBuf[8] = BYTE1; // Length
AdrienSalle 0:32fd5a08c430 2017 txBuf[9] = Tension_Min;
AdrienSalle 0:32fd5a08c430 2018 // Check Sum1 and Check Sum2
AdrienSalle 0:32fd5a08c430 2019 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE;
AdrienSalle 0:32fd5a08c430 2020 txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle 0:32fd5a08c430 2021
AdrienSalle 0:32fd5a08c430 2022 //pc.printf(" tensionMIN ");
AdrienSalle 0:32fd5a08c430 2023 /*for(uint8_t i = 0; i < 10 ; i++)
AdrienSalle 0:32fd5a08c430 2024 {
AdrienSalle 0:32fd5a08c430 2025 while(serial1.writeable() == 0);
AdrienSalle 0:32fd5a08c430 2026 serial1.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 2027 wait_us(100);
AdrienSalle 0:32fd5a08c430 2028 }*/
AdrienSalle 0:32fd5a08c430 2029 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 2030 for(uint8_t i = 0; i < 10 ; i++) {
AdrienSalle 0:32fd5a08c430 2031 while(serial1.writeable() == 0);
AdrienSalle 0:32fd5a08c430 2032 serial1.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 2033 wait_us(100);
AdrienSalle 0:32fd5a08c430 2034 }
AdrienSalle 0:32fd5a08c430 2035 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 2036 for(uint8_t i = 0; i < 10 ; i++) {
AdrienSalle 0:32fd5a08c430 2037 while(serial2.writeable() == 0);
AdrienSalle 0:32fd5a08c430 2038 serial2.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 2039 wait_us(100);
AdrienSalle 0:32fd5a08c430 2040 }
AdrienSalle 0:32fd5a08c430 2041 } else if(numero_serial == 3) {
AdrienSalle 0:32fd5a08c430 2042 for(uint8_t i = 0; i < 10 ; i++) {
AdrienSalle 0:32fd5a08c430 2043 while(serial3.writeable() == 0);
AdrienSalle 0:32fd5a08c430 2044 serial3.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 2045 wait_us(100);
AdrienSalle 0:32fd5a08c430 2046 }
AdrienSalle 0:32fd5a08c430 2047 } else if(numero_serial == 4) {
AdrienSalle 0:32fd5a08c430 2048 for(uint8_t i = 0; i < 10 ; i++) {
AdrienSalle 0:32fd5a08c430 2049 while(serial4.writeable() == 0);
AdrienSalle 0:32fd5a08c430 2050 serial4.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 2051 wait_us(100);
AdrienSalle 0:32fd5a08c430 2052 }
AdrienSalle 0:32fd5a08c430 2053 } else if(numero_serial == 5) {
AdrienSalle 0:32fd5a08c430 2054 for(uint8_t i = 0; i < 10 ; i++) {
AdrienSalle 0:32fd5a08c430 2055 while(serial5.writeable() == 0);
AdrienSalle 0:32fd5a08c430 2056 serial5.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 2057 wait_us(100);
AdrienSalle 0:32fd5a08c430 2058 }
AdrienSalle 0:32fd5a08c430 2059 }
AdrienSalle 0:32fd5a08c430 2060 //wait_ms(3);
AdrienSalle 0:32fd5a08c430 2061 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 2062 }
AdrienSalle 0:32fd5a08c430 2063 //------------fonction pour lire la tension d'un servo-------------
AdrienSalle 0:32fd5a08c430 2064 int8_t Get_Tension_actuelle(int8_t id, uint8_t numero_serial)
AdrienSalle 0:32fd5a08c430 2065 {
AdrienSalle 0:32fd5a08c430 2066 serial_numero = numero_serial;
AdrienSalle 0:32fd5a08c430 2067 uint8_t txBuf[9];
AdrienSalle 0:32fd5a08c430 2068 int8_t tension = 0;
AdrienSalle 0:32fd5a08c430 2069
AdrienSalle 0:32fd5a08c430 2070 txBuf[0] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 2071 txBuf[1] = HEADER; // Packet Header (0xFF)
AdrienSalle 0:32fd5a08c430 2072 txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
AdrienSalle 0:32fd5a08c430 2073 txBuf[3] = id; // Servo ID
AdrienSalle 0:32fd5a08c430 2074 txBuf[4] = CMD_RAM_READ; // Command Ram Read (0x03)
AdrienSalle 0:32fd5a08c430 2075 txBuf[5] = 0; // Checksum1
AdrienSalle 0:32fd5a08c430 2076 txBuf[6] = 0; // Checksum2
AdrienSalle 0:32fd5a08c430 2077 txBuf[7] = RAM_VOLTAGE;
AdrienSalle 0:32fd5a08c430 2078 txBuf[8] = BYTE2; // Length
AdrienSalle 0:32fd5a08c430 2079 // Check Sum1 and Check Sum2
AdrienSalle 0:32fd5a08c430 2080 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
AdrienSalle 0:32fd5a08c430 2081 txBuf[6] = (~txBuf[5])&0xFE;
AdrienSalle 0:32fd5a08c430 2082
AdrienSalle 0:32fd5a08c430 2083 //pc.printf(" tension ");
AdrienSalle 0:32fd5a08c430 2084
AdrienSalle 0:32fd5a08c430 2085 uint8_t rxBuf[13];
AdrienSalle 0:32fd5a08c430 2086
AdrienSalle 0:32fd5a08c430 2087 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 2088 //send packet (mbed -> herkulex)
AdrienSalle 0:32fd5a08c430 2089 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 2090 while(serial1.writeable() == 0);
AdrienSalle 0:32fd5a08c430 2091 serial1.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 2092 wait_us(100);
AdrienSalle 0:32fd5a08c430 2093 }
AdrienSalle 0:32fd5a08c430 2094
AdrienSalle 0:32fd5a08c430 2095 //send packet (mbed -> herkulex)
AdrienSalle 0:32fd5a08c430 2096 //uint8_t rxBuf[13];
AdrienSalle 0:32fd5a08c430 2097 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 2098 if(flag_serial1_receive) {
AdrienSalle 0:32fd5a08c430 2099 for(unsigned char i4=0; i4<Size_trame_serial1; i4++) {
AdrienSalle 0:32fd5a08c430 2100 rxBuf[i4] = char_receive_serial1[i4];
AdrienSalle 0:32fd5a08c430 2101 //////pc.printf("%d ",(int)char_receive_serial1[i4]);
AdrienSalle 0:32fd5a08c430 2102 }
AdrienSalle 0:32fd5a08c430 2103 flag_serial1_receive=0;
AdrienSalle 0:32fd5a08c430 2104 valueserial1=0;
AdrienSalle 0:32fd5a08c430 2105 }
AdrienSalle 0:32fd5a08c430 2106 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 2107 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 2108 while(serial2.writeable() == 0);
AdrienSalle 0:32fd5a08c430 2109 serial2.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 2110 wait_us(100);
AdrienSalle 0:32fd5a08c430 2111 }
AdrienSalle 0:32fd5a08c430 2112 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 2113 if(flag_serial2_receive) {
AdrienSalle 0:32fd5a08c430 2114 for(unsigned char i4=0; i4<Size_trame_serial2; i4++) {
AdrienSalle 0:32fd5a08c430 2115 rxBuf[i4] = char_receive_serial2[i4];
AdrienSalle 0:32fd5a08c430 2116 //////pc.printf("%d ",(int)char_receive_serial2[i4]);
AdrienSalle 0:32fd5a08c430 2117 }
AdrienSalle 0:32fd5a08c430 2118 flag_serial2_receive=0;
AdrienSalle 0:32fd5a08c430 2119 valueserial2=0;
AdrienSalle 0:32fd5a08c430 2120 }
AdrienSalle 0:32fd5a08c430 2121 } else if(numero_serial == 3) {
AdrienSalle 0:32fd5a08c430 2122 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 2123 while(serial3.writeable() == 0);
AdrienSalle 0:32fd5a08c430 2124 serial3.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 2125 wait_us(100);
AdrienSalle 0:32fd5a08c430 2126 }
AdrienSalle 0:32fd5a08c430 2127 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 2128 if(flag_serial3_receive) {
AdrienSalle 0:32fd5a08c430 2129 for(unsigned char i4=0; i4<Size_trame_serial3; i4++) {
AdrienSalle 0:32fd5a08c430 2130 rxBuf[i4] = char_receive_serial3[i4];
AdrienSalle 0:32fd5a08c430 2131 //////pc.printf("%d ",(int)char_receive_serial3[i4]);
AdrienSalle 0:32fd5a08c430 2132 }
AdrienSalle 0:32fd5a08c430 2133 flag_serial3_receive=0;
AdrienSalle 0:32fd5a08c430 2134 valueserial3=0;
AdrienSalle 0:32fd5a08c430 2135 }
AdrienSalle 0:32fd5a08c430 2136 } else if(numero_serial == 4) {
AdrienSalle 0:32fd5a08c430 2137 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 2138 while(serial4.writeable() == 0);
AdrienSalle 0:32fd5a08c430 2139 serial4.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 2140 wait_us(100);
AdrienSalle 0:32fd5a08c430 2141 }
AdrienSalle 0:32fd5a08c430 2142 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 2143 if(flag_serial4_receive) {
AdrienSalle 0:32fd5a08c430 2144 for(unsigned char i4=0; i4<Size_trame_serial4; i4++) {
AdrienSalle 0:32fd5a08c430 2145 rxBuf[i4] = char_receive_serial4[i4];
AdrienSalle 0:32fd5a08c430 2146 //////pc.printf("%d ",(int)char_receive_serial4[i4]);
AdrienSalle 0:32fd5a08c430 2147 }
AdrienSalle 0:32fd5a08c430 2148 flag_serial4_receive=0;
AdrienSalle 0:32fd5a08c430 2149 valueserial4=0;
AdrienSalle 0:32fd5a08c430 2150 }
AdrienSalle 0:32fd5a08c430 2151 } else if(numero_serial == 5) {
AdrienSalle 0:32fd5a08c430 2152 for(uint8_t i = 0; i < 9 ; i++) {
AdrienSalle 0:32fd5a08c430 2153 while(serial5.writeable() == 0);
AdrienSalle 0:32fd5a08c430 2154 serial5.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 2155 wait_us(100);
AdrienSalle 0:32fd5a08c430 2156 }
AdrienSalle 0:32fd5a08c430 2157 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 2158 if(flag_serial5_receive) {
AdrienSalle 0:32fd5a08c430 2159 for(unsigned char i4=0; i4<Size_trame_serial5; i4++) {
AdrienSalle 0:32fd5a08c430 2160 rxBuf[i4] = char_receive_serial5[i4];
AdrienSalle 0:32fd5a08c430 2161 //////pc.printf("%d ",(int)char_receive_serial5[i4]);
AdrienSalle 0:32fd5a08c430 2162 }
AdrienSalle 0:32fd5a08c430 2163 flag_serial5_receive=0;
AdrienSalle 0:32fd5a08c430 2164 valueserial5=0;
AdrienSalle 0:32fd5a08c430 2165 }
AdrienSalle 0:32fd5a08c430 2166 }
AdrienSalle 0:32fd5a08c430 2167 /*
AdrienSalle 0:32fd5a08c430 2168 for(uint8_t i = 0; i < 9 ; i++)
AdrienSalle 0:32fd5a08c430 2169 {
AdrienSalle 0:32fd5a08c430 2170 while(serial1.writeable() == 0);
AdrienSalle 0:32fd5a08c430 2171 serial1.putc(txBuf[i]);
AdrienSalle 0:32fd5a08c430 2172 wait_us(100);
AdrienSalle 0:32fd5a08c430 2173 }
AdrienSalle 0:32fd5a08c430 2174
AdrienSalle 0:32fd5a08c430 2175 // send packet (mbed -> herkulex)
AdrienSalle 0:32fd5a08c430 2176 uint8_t rxBuf[13];
AdrienSalle 0:32fd5a08c430 2177 //wait_ms(3);
AdrienSalle 0:32fd5a08c430 2178 wait_ms(TEMPO_R);
AdrienSalle 0:32fd5a08c430 2179 if(flag_serial1_receive)
AdrienSalle 0:32fd5a08c430 2180 {
AdrienSalle 0:32fd5a08c430 2181 for(unsigned char i4=0;i4<Size_trame_serial1; i4++)
AdrienSalle 0:32fd5a08c430 2182 {
AdrienSalle 0:32fd5a08c430 2183 rxBuf[i4] = char_receive_serial1[i4];
AdrienSalle 0:32fd5a08c430 2184 ////pc.printf("%d ",(int)char_receive_serial1[i4]);
AdrienSalle 0:32fd5a08c430 2185 }
AdrienSalle 0:32fd5a08c430 2186 flag_serial1_receive=0;
AdrienSalle 0:32fd5a08c430 2187 valueserial1=0;
AdrienSalle 0:32fd5a08c430 2188 }
AdrienSalle 0:32fd5a08c430 2189 */
AdrienSalle 0:32fd5a08c430 2190
AdrienSalle 0:32fd5a08c430 2191 uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE;
AdrienSalle 0:32fd5a08c430 2192 if (chksum1 != rxBuf[5]) {
AdrienSalle 0:32fd5a08c430 2193 /*#ifdef HERKULEX_DEBUG
AdrienSalle 0:32fd5a08c430 2194 pc->printf("Checksum1 fault\n");
AdrienSalle 0:32fd5a08c430 2195 #endif*/
AdrienSalle 0:32fd5a08c430 2196 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 2197 flag_serial1_receive=0;
AdrienSalle 0:32fd5a08c430 2198 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 2199 flag_serial2_receive=0;
AdrienSalle 0:32fd5a08c430 2200 }
AdrienSalle 0:32fd5a08c430 2201 return -1;
AdrienSalle 0:32fd5a08c430 2202 }
AdrienSalle 0:32fd5a08c430 2203 // Checksum2
AdrienSalle 0:32fd5a08c430 2204 uint8_t chksum2 = (~rxBuf[5]&0xFE);
AdrienSalle 0:32fd5a08c430 2205 if (chksum2 != rxBuf[6]) {
AdrienSalle 0:32fd5a08c430 2206 /* #ifdef HERKULEX_DEBUG
AdrienSalle 0:32fd5a08c430 2207 pc->printf("Checksum2 fault\n");
AdrienSalle 0:32fd5a08c430 2208 #endif*/
AdrienSalle 0:32fd5a08c430 2209 if(numero_serial == 1) {
AdrienSalle 0:32fd5a08c430 2210 flag_serial1_receive=0;
AdrienSalle 0:32fd5a08c430 2211 } else if(numero_serial == 2) {
AdrienSalle 0:32fd5a08c430 2212 flag_serial2_receive=0;
AdrienSalle 0:32fd5a08c430 2213 }
AdrienSalle 0:32fd5a08c430 2214 return -1;
AdrienSalle 0:32fd5a08c430 2215 }
AdrienSalle 0:32fd5a08c430 2216 tension = rxBuf[9];
AdrienSalle 0:32fd5a08c430 2217 /* #ifdef HERKULEX_DEBUG
AdrienSalle 0:32fd5a08c430 2218 pc->printf("position = %04X(%d)\n", position, position);
AdrienSalle 0:32fd5a08c430 2219 #endif*/
AdrienSalle 0:32fd5a08c430 2220 //}
AdrienSalle 0:32fd5a08c430 2221 return tension;
AdrienSalle 0:32fd5a08c430 2222 }
AdrienSalle 0:32fd5a08c430 2223
AdrienSalle 0:32fd5a08c430 2224 //---------------------------------------Petit robot---------------------------------------------------------
AdrienSalle 0:32fd5a08c430 2225 /*void gros_robot_init(void)
AdrienSalle 0:32fd5a08c430 2226 {
AdrienSalle 0:32fd5a08c430 2227 void palet_accelerateur(){
AdrienSalle 0:32fd5a08c430 2228 compteTour(ID,1000,1,1,PLED_ON,serialbarillet);
AdrienSalle 0:32fd5a08c430 2229 }
AdrienSalle 0:32fd5a08c430 2230 void palet_balance(){
AdrienSalle 0:32fd5a08c430 2231 compteTour(ID,-1000,1,1,PLED_ON,serialbarillet);
AdrienSalle 0:32fd5a08c430 2232 }
AdrienSalle 0:32fd5a08c430 2233 void sortie_balance()
AdrienSalle 0:32fd5a08c430 2234 {
AdrienSalle 0:32fd5a08c430 2235
AdrienSalle 0:32fd5a08c430 2236 }
AdrienSalle 0:32fd5a08c430 2237
AdrienSalle 0:32fd5a08c430 2238 */
AdrienSalle 0:32fd5a08c430 2239
AdrienSalle 0:32fd5a08c430 2240
AdrienSalle 0:32fd5a08c430 2241 //-----------------------------------------------------------------------------------------
AdrienSalle 0:32fd5a08c430 2242 void servo_interrupt_en(void)
AdrienSalle 0:32fd5a08c430 2243 {
AdrienSalle 0:32fd5a08c430 2244 Interrupt1_en();
AdrienSalle 0:32fd5a08c430 2245 Interrupt2_en();
AdrienSalle 0:32fd5a08c430 2246 Interrupt3_en();
AdrienSalle 0:32fd5a08c430 2247 Interrupt4_en();
AdrienSalle 0:32fd5a08c430 2248 Interrupt5_en();
AdrienSalle 0:32fd5a08c430 2249 }
AdrienSalle 0:32fd5a08c430 2250
AdrienSalle 0:32fd5a08c430 2251 //---------------------------------------------------------------------------------------------