CRAC Team / CRAC-Strat_2019

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   CRAC-Strat_2019 SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Dependents:   Codeprincipal_2019 CRAC-Strat_2019

AX12-V2/AX12-V2.cpp@0:ad97421fb1fb, 2016-04-13 (annotated)

Committer:
antbig
Date:
Wed Apr 13 22:04:54 2016 +0000
Revision:
0:ad97421fb1fb
Child:
1:116040d14164
Ajout interruption fin de match

Who changed what in which revision?

UserRevisionLine numberNew contents of line
antbig 0:ad97421fb1fb 1 #include "AX12-V2.h"
antbig 0:ad97421fb1fb 2
antbig 0:ad97421fb1fb 3 struct S_AX12 AX12_data[MAX_AX12];//La liste de tous les AX12 du robot possible, aussi bien par CAN que en local
antbig 0:ad97421fb1fb 4
antbig 0:ad97421fb1fb 5 SerialHalfDuplex AX12_Serial = SerialHalfDuplex(p9,p10);
antbig 0:ad97421fb1fb 6
antbig 0:ad97421fb1fb 7 int lastAX12Use = 0;
antbig 0:ad97421fb1fb 8
antbig 0:ad97421fb1fb 9 /****************************************************************************************/
antbig 0:ad97421fb1fb 10 /* FUNCTION NAME: AX12_register */
antbig 0:ad97421fb1fb 11 /* DESCRIPTION : Indiquer qu'un AX12 est connecté à la carte */
antbig 0:ad97421fb1fb 12 /****************************************************************************************/
antbig 0:ad97421fb1fb 13 void AX12_register(unsigned char id, unsigned short speed)
antbig 0:ad97421fb1fb 14 {
antbig 0:ad97421fb1fb 15 int localID = AX12_getLocalID(id);
antbig 0:ad97421fb1fb 16 char data[2];
antbig 0:ad97421fb1fb 17
antbig 0:ad97421fb1fb 18 AX12_data[localID].isUsingCAN = 0;//On indique que l'AX12 est connecté localement
antbig 0:ad97421fb1fb 19
antbig 0:ad97421fb1fb 20 if(speed > 0x3FF) speed = 0x3FF;//La vitesse ne doit pas depasser 1023
antbig 0:ad97421fb1fb 21
antbig 0:ad97421fb1fb 22 AX12_data[lastAX12Use].speed = speed;
antbig 0:ad97421fb1fb 23
antbig 0:ad97421fb1fb 24 data[0] = speed & 0xff; // bottom 8 bits
antbig 0:ad97421fb1fb 25 data[1] = speed >> 8; // top 8 bits
antbig 0:ad97421fb1fb 26
antbig 0:ad97421fb1fb 27 AX12_Serial.baud(1000000);//On indique la vitesse de transmission des AX12
antbig 0:ad97421fb1fb 28
antbig 0:ad97421fb1fb 29 AX12_write(id,AX12_REG_MOVING_SPEED,2,data);
antbig 0:ad97421fb1fb 30
antbig 0:ad97421fb1fb 31
antbig 0:ad97421fb1fb 32 }
antbig 0:ad97421fb1fb 33
antbig 0:ad97421fb1fb 34 /****************************************************************************************/
antbig 0:ad97421fb1fb 35 /* FUNCTION NAME: AX12_setGoal */
antbig 0:ad97421fb1fb 36 /* DESCRIPTION : Definir la position d'un ax12, !!Ne déclanche pas le mouvement */
antbig 0:ad97421fb1fb 37 /****************************************************************************************/
antbig 0:ad97421fb1fb 38 void AX12_setGoal(unsigned char id, unsigned short goal, unsigned short speed)
antbig 0:ad97421fb1fb 39 {
antbig 0:ad97421fb1fb 40 int localID = AX12_getLocalID(id);//On récupère les info sur l'AX12
antbig 0:ad97421fb1fb 41 CANMessage msgTx=CANMessage();
antbig 0:ad97421fb1fb 42
antbig 0:ad97421fb1fb 43 AX12_data[localID].needToUpdate = 1;
antbig 0:ad97421fb1fb 44 AX12_data[localID].goal = goal;
antbig 0:ad97421fb1fb 45 if(speed > 0x3FF) speed = 0x3FF;//La vitesse ne doit pas depasser 1023
antbig 0:ad97421fb1fb 46
antbig 0:ad97421fb1fb 47 AX12_data[localID].speed = speed;
antbig 0:ad97421fb1fb 48
antbig 0:ad97421fb1fb 49 if(AX12_data[localID].isUsingCAN != 0) {//Il faut envoyer la trame CAN car l'AX12 est sur une autre carte
antbig 0:ad97421fb1fb 50 msgTx.id=SERVO_AX12_SETGOAL;
antbig 0:ad97421fb1fb 51 msgTx.len=3;
antbig 0:ad97421fb1fb 52 msgTx.format=CANStandard;
antbig 0:ad97421fb1fb 53 msgTx.type=CANData;
antbig 0:ad97421fb1fb 54 // id de l'AX12 sur 1 octet
antbig 0:ad97421fb1fb 55 msgTx.data[0]=(unsigned char)id;
antbig 0:ad97421fb1fb 56 // Position de l'AX12 sur 2 octet
antbig 0:ad97421fb1fb 57 msgTx.data[1]=(unsigned char)goal;
antbig 0:ad97421fb1fb 58 msgTx.data[2]=(unsigned char)(goal>>8);
antbig 0:ad97421fb1fb 59 //Vitesse de l'AX12 sur 2 octet
antbig 0:ad97421fb1fb 60 msgTx.data[3]=(unsigned char)speed;
antbig 0:ad97421fb1fb 61 msgTx.data[4]=(unsigned char)(speed>>8);
antbig 0:ad97421fb1fb 62
antbig 0:ad97421fb1fb 63 can1.write(msgTx);
antbig 0:ad97421fb1fb 64 }
antbig 0:ad97421fb1fb 65
antbig 0:ad97421fb1fb 66 }
antbig 0:ad97421fb1fb 67
antbig 0:ad97421fb1fb 68 /****************************************************************************************/
antbig 0:ad97421fb1fb 69 /* FUNCTION NAME: AX12_isLocal */
antbig 0:ad97421fb1fb 70 /* DESCRIPTION : Savoir si un AX12 est enregistré sur la carte */
antbig 0:ad97421fb1fb 71 /****************************************************************************************/
antbig 0:ad97421fb1fb 72 unsigned char AX12_isLocal(unsigned char id)
antbig 0:ad97421fb1fb 73 {
antbig 0:ad97421fb1fb 74 int i=0;
antbig 0:ad97421fb1fb 75 for(i=0;i<MAX_AX12;i++)
antbig 0:ad97421fb1fb 76 {
antbig 0:ad97421fb1fb 77 if(AX12_data[i].id == id && AX12_data[i].isUsingCAN == 0) return 1;
antbig 0:ad97421fb1fb 78 }
antbig 0:ad97421fb1fb 79 return 0;
antbig 0:ad97421fb1fb 80 }
antbig 0:ad97421fb1fb 81
antbig 0:ad97421fb1fb 82
antbig 0:ad97421fb1fb 83 /****************************************************************************************/
antbig 0:ad97421fb1fb 84 /* FUNCTION NAME: AX12_getLocalID */
antbig 0:ad97421fb1fb 85 /* DESCRIPTION : Obtenir les info sur un AX12 ou l'initialiser si non présent */
antbig 0:ad97421fb1fb 86 /****************************************************************************************/
antbig 0:ad97421fb1fb 87 int AX12_getLocalID(unsigned char id)
antbig 0:ad97421fb1fb 88 {
antbig 0:ad97421fb1fb 89 int i=0;
antbig 0:ad97421fb1fb 90 for(i=0;i<MAX_AX12;i++)
antbig 0:ad97421fb1fb 91 {
antbig 0:ad97421fb1fb 92 if(AX12_data[i].id == id) return i;
antbig 0:ad97421fb1fb 93 }
antbig 0:ad97421fb1fb 94 //Si l'AX12 n'est pas déjà initialisé, on l'initialise
antbig 0:ad97421fb1fb 95 AX12_data[lastAX12Use].id = id;//
antbig 0:ad97421fb1fb 96 AX12_data[lastAX12Use].goal = 0;//
antbig 0:ad97421fb1fb 97 AX12_data[lastAX12Use].speed = 0x320;//
antbig 0:ad97421fb1fb 98 AX12_data[lastAX12Use].isUsingCAN = 1;//Indique qu'il faut envoyer le message via CAN
antbig 0:ad97421fb1fb 99 AX12_data[lastAX12Use].needToUpdate = 0;//
antbig 0:ad97421fb1fb 100 lastAX12Use++;
antbig 0:ad97421fb1fb 101 return lastAX12Use-1;
antbig 0:ad97421fb1fb 102 }
antbig 0:ad97421fb1fb 103
antbig 0:ad97421fb1fb 104 /****************************************************************************************/
antbig 0:ad97421fb1fb 105 /* FUNCTION NAME: AX12_doLoop */
antbig 0:ad97421fb1fb 106 /* DESCRIPTION : Boucle de vérification de la position des AX12 */
antbig 0:ad97421fb1fb 107 /****************************************************************************************/
antbig 0:ad97421fb1fb 108 void AX12_doLoop(void)
antbig 0:ad97421fb1fb 109 {
antbig 0:ad97421fb1fb 110 //TODO
antbig 0:ad97421fb1fb 111 }
antbig 0:ad97421fb1fb 112
antbig 0:ad97421fb1fb 113 /****************************************************************************************/
antbig 0:ad97421fb1fb 114 /* FUNCTION NAME: AX12_processChange */
antbig 0:ad97421fb1fb 115 /* DESCRIPTION : Permet de prendre en compte les changement d'instruction des AX12 */
antbig 0:ad97421fb1fb 116 /* Début du mouvement à partir de l'appel de cette fonction */
antbig 0:ad97421fb1fb 117 /****************************************************************************************/
antbig 0:ad97421fb1fb 118 void AX12_processChange(void)
antbig 0:ad97421fb1fb 119 {
antbig 0:ad97421fb1fb 120 int i=0;
antbig 0:ad97421fb1fb 121 int dataToSendLength = 0;
antbig 0:ad97421fb1fb 122 char dataToSend[50];
antbig 0:ad97421fb1fb 123
antbig 0:ad97421fb1fb 124
antbig 0:ad97421fb1fb 125 for(i=0;i<MAX_AX12;i++)
antbig 0:ad97421fb1fb 126 {
antbig 0:ad97421fb1fb 127 if(AX12_data[i].needToUpdate == 1) //Il faut mettre à jour la position de l'AX12
antbig 0:ad97421fb1fb 128 {
antbig 0:ad97421fb1fb 129 if(AX12_data[i].isUsingCAN == 0)//Il faut envoyer la trame en local
antbig 0:ad97421fb1fb 130 {
antbig 0:ad97421fb1fb 131 if(dataToSendLength == 0)
antbig 0:ad97421fb1fb 132 dataToSend[dataToSendLength++] = 2;//length data
antbig 0:ad97421fb1fb 133 dataToSend[dataToSendLength++] = AX12_data[i].id;//ID servo1
antbig 0:ad97421fb1fb 134 dataToSend[dataToSendLength++] = ((1023 * AX12_data[i].goal) / 300) & 0xff;// bottom 8 bits
antbig 0:ad97421fb1fb 135 dataToSend[dataToSendLength++] = ((1023 * AX12_data[i].goal) / 300) >> 8; // top 8 bits
antbig 0:ad97421fb1fb 136 }
antbig 0:ad97421fb1fb 137 AX12_data[i].needToUpdate = 0;//Remise à 0 de l'indicatif de mise à jour
antbig 0:ad97421fb1fb 138 }
antbig 0:ad97421fb1fb 139 }
antbig 0:ad97421fb1fb 140 SendRawId(SERVO_AX12_PROCESS);//On indique par CAN qu'il faut bouger les AX12
antbig 0:ad97421fb1fb 141
antbig 0:ad97421fb1fb 142 if(dataToSendLength > 0)//Il y a des données à envoyer en local
antbig 0:ad97421fb1fb 143 {
antbig 0:ad97421fb1fb 144 AX12_syncWrite(AX12_REG_GOAL_POSITION, dataToSendLength, dataToSend);
antbig 0:ad97421fb1fb 145 }
antbig 0:ad97421fb1fb 146
antbig 0:ad97421fb1fb 147 }
antbig 0:ad97421fb1fb 148
antbig 0:ad97421fb1fb 149 /****************************************************************************************/
antbig 0:ad97421fb1fb 150 /* FUNCTION NAME: AX12_syncWrite */
antbig 0:ad97421fb1fb 151 /* DESCRIPTION : Fonction pour envoyer des trames aux AX12 en mode syncWrite */
antbig 0:ad97421fb1fb 152 /****************************************************************************************/
antbig 0:ad97421fb1fb 153 int AX12_syncWrite(int start, int bytes, char* data)
antbig 0:ad97421fb1fb 154 {
antbig 0:ad97421fb1fb 155 //0 : 0xff
antbig 0:ad97421fb1fb 156 //1 : 0xff
antbig 0:ad97421fb1fb 157 //2 : ID de l'AX12 ou 0xFE pour le broadcast
antbig 0:ad97421fb1fb 158 //3 : Length => longueur de la trame
antbig 0:ad97421fb1fb 159 //4 : Intruction(write) => id de l'instruction 0x83 pour le syncwrite
antbig 0:ad97421fb1fb 160 //5 : Address => addresse du registre à modifier
antbig 0:ad97421fb1fb 161 //6+ : Data => les données à transmettre
antbig 0:ad97421fb1fb 162 //last : Checksum
antbig 0:ad97421fb1fb 163 int ID = 0xFE;//Toujours 0xFE dans le cas d'un broadcast
antbig 0:ad97421fb1fb 164
antbig 0:ad97421fb1fb 165 char TxBuf[60];
antbig 0:ad97421fb1fb 166 char sum = 0;
antbig 0:ad97421fb1fb 167
antbig 0:ad97421fb1fb 168 int timeout_transmit = 0;
antbig 0:ad97421fb1fb 169 int i = 0;
antbig 0:ad97421fb1fb 170 printf("Start sending moving trame\n");
antbig 0:ad97421fb1fb 171 // Build the TxPacket first in RAM, then we'll send in one go
antbig 0:ad97421fb1fb 172
antbig 0:ad97421fb1fb 173 TxBuf[0] = 0xff;
antbig 0:ad97421fb1fb 174 TxBuf[1] = 0xff;
antbig 0:ad97421fb1fb 175
antbig 0:ad97421fb1fb 176 // ID
antbig 0:ad97421fb1fb 177 TxBuf[2] = ID;
antbig 0:ad97421fb1fb 178 sum += TxBuf[2];
antbig 0:ad97421fb1fb 179
antbig 0:ad97421fb1fb 180 // packet Length
antbig 0:ad97421fb1fb 181 TxBuf[3] = 3+bytes;
antbig 0:ad97421fb1fb 182 sum += TxBuf[3];
antbig 0:ad97421fb1fb 183
antbig 0:ad97421fb1fb 184
antbig 0:ad97421fb1fb 185 // Instruction
antbig 0:ad97421fb1fb 186 TxBuf[4]=0x83;//toujours 0x83 dans le cas d'un syncwrite
antbig 0:ad97421fb1fb 187 sum += TxBuf[4];
antbig 0:ad97421fb1fb 188
antbig 0:ad97421fb1fb 189
antbig 0:ad97421fb1fb 190 // Start Address
antbig 0:ad97421fb1fb 191 TxBuf[5] = start;//addresse du registre à modifier
antbig 0:ad97421fb1fb 192 sum += TxBuf[5];
antbig 0:ad97421fb1fb 193
antbig 0:ad97421fb1fb 194 // data
antbig 0:ad97421fb1fb 195 for (char i=0; i<bytes ; i++) {
antbig 0:ad97421fb1fb 196 TxBuf[6+i] = data[i];
antbig 0:ad97421fb1fb 197 sum += TxBuf[6+i];
antbig 0:ad97421fb1fb 198 printf(" Data : 0x%x\n",TxBuf[6+i]);
antbig 0:ad97421fb1fb 199 }
antbig 0:ad97421fb1fb 200
antbig 0:ad97421fb1fb 201 // checksum
antbig 0:ad97421fb1fb 202 TxBuf[6+bytes] = 0xFF - sum;
antbig 0:ad97421fb1fb 203
antbig 0:ad97421fb1fb 204
antbig 0:ad97421fb1fb 205 /* Transmission de la trame construite precedemment dans le tableau TxBuf
antbig 0:ad97421fb1fb 206 */
antbig 0:ad97421fb1fb 207 while ((timeout_transmit<100) && (i < (7+bytes))) {
antbig 0:ad97421fb1fb 208 if (AX12_Serial.writeable()) {
antbig 0:ad97421fb1fb 209 AX12_Serial.putc(TxBuf[i]);
antbig 0:ad97421fb1fb 210 i++;
antbig 0:ad97421fb1fb 211 timeout_transmit = 0;
antbig 0:ad97421fb1fb 212 } else timeout_transmit++;
antbig 0:ad97421fb1fb 213 }
antbig 0:ad97421fb1fb 214
antbig 0:ad97421fb1fb 215 if (timeout_transmit == 100 ) { // dans le cas d'une sortie en timeout pour ne pas rester bloquer !
antbig 0:ad97421fb1fb 216 return(-1);
antbig 0:ad97421fb1fb 217 }
antbig 0:ad97421fb1fb 218
antbig 0:ad97421fb1fb 219 // Wait for data to transmit
antbig 0:ad97421fb1fb 220 wait (0.005);
antbig 0:ad97421fb1fb 221 return(0);//OK trame envoyé
antbig 0:ad97421fb1fb 222 }
antbig 0:ad97421fb1fb 223
antbig 0:ad97421fb1fb 224 /****************************************************************************************/
antbig 0:ad97421fb1fb 225 /* FUNCTION NAME: AX12_write */
antbig 0:ad97421fb1fb 226 /* DESCRIPTION : Fonction pour envoyer des trames aux AX12 */
antbig 0:ad97421fb1fb 227 /****************************************************************************************/
antbig 0:ad97421fb1fb 228 int AX12_write(int ID, int start, int bytes, char* data, int flag)
antbig 0:ad97421fb1fb 229 {
antbig 0:ad97421fb1fb 230 // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
antbig 0:ad97421fb1fb 231
antbig 0:ad97421fb1fb 232 char TxBuf[16];
antbig 0:ad97421fb1fb 233 char sum = 0;
antbig 0:ad97421fb1fb 234 char Status[6];
antbig 0:ad97421fb1fb 235
antbig 0:ad97421fb1fb 236 int timeout = 0;
antbig 0:ad97421fb1fb 237 int plen = 0;
antbig 0:ad97421fb1fb 238 int flag_out = 0;
antbig 0:ad97421fb1fb 239 int timeout_transmit = 0;
antbig 0:ad97421fb1fb 240 int i = 0;
antbig 0:ad97421fb1fb 241 /*int poubelle = 0;
antbig 0:ad97421fb1fb 242 int count = 0;
antbig 0:ad97421fb1fb 243 char vidage[50];*/
antbig 0:ad97421fb1fb 244
antbig 0:ad97421fb1fb 245 typedef enum {Header1, Header2, ident, length, erreur, checksum} type_etat;
antbig 0:ad97421fb1fb 246 type_etat etat = Header1;
antbig 0:ad97421fb1fb 247
antbig 0:ad97421fb1fb 248 // Build the TxPacket first in RAM, then we'll send in one go
antbig 0:ad97421fb1fb 249 TxBuf[0] = 0xff;
antbig 0:ad97421fb1fb 250 TxBuf[1] = 0xff;
antbig 0:ad97421fb1fb 251
antbig 0:ad97421fb1fb 252 // ID
antbig 0:ad97421fb1fb 253 TxBuf[2] = ID;
antbig 0:ad97421fb1fb 254 sum += TxBuf[2];
antbig 0:ad97421fb1fb 255
antbig 0:ad97421fb1fb 256 // packet Length
antbig 0:ad97421fb1fb 257 TxBuf[3] = 3+bytes;
antbig 0:ad97421fb1fb 258 sum += TxBuf[3];
antbig 0:ad97421fb1fb 259
antbig 0:ad97421fb1fb 260 // Instruction
antbig 0:ad97421fb1fb 261 if (flag == 1) {
antbig 0:ad97421fb1fb 262 TxBuf[4]=0x04;
antbig 0:ad97421fb1fb 263 sum += TxBuf[4];
antbig 0:ad97421fb1fb 264 } else {
antbig 0:ad97421fb1fb 265 TxBuf[4]=0x03;
antbig 0:ad97421fb1fb 266 sum += TxBuf[4];
antbig 0:ad97421fb1fb 267 }
antbig 0:ad97421fb1fb 268
antbig 0:ad97421fb1fb 269 // Start Address
antbig 0:ad97421fb1fb 270 TxBuf[5] = start;
antbig 0:ad97421fb1fb 271 sum += TxBuf[5];
antbig 0:ad97421fb1fb 272
antbig 0:ad97421fb1fb 273
antbig 0:ad97421fb1fb 274 // data
antbig 0:ad97421fb1fb 275 for (char i=0; i<bytes ; i++) {
antbig 0:ad97421fb1fb 276 TxBuf[6+i] = data[i];
antbig 0:ad97421fb1fb 277 sum += TxBuf[6+i];
antbig 0:ad97421fb1fb 278 }
antbig 0:ad97421fb1fb 279
antbig 0:ad97421fb1fb 280 // checksum
antbig 0:ad97421fb1fb 281 TxBuf[6+bytes] = 0xFF - sum;
antbig 0:ad97421fb1fb 282
antbig 0:ad97421fb1fb 283
antbig 0:ad97421fb1fb 284
antbig 0:ad97421fb1fb 285 /* Transmission de la trame construite precedemment dans le tableau TxBuf
antbig 0:ad97421fb1fb 286 */
antbig 0:ad97421fb1fb 287 while ((timeout_transmit<100) && (i < (7+bytes))) {
antbig 0:ad97421fb1fb 288 if (AX12_Serial.writeable()) {
antbig 0:ad97421fb1fb 289 AX12_Serial.putc(TxBuf[i]);
antbig 0:ad97421fb1fb 290 i++;
antbig 0:ad97421fb1fb 291 timeout_transmit = 0;
antbig 0:ad97421fb1fb 292 } else timeout_transmit++;
antbig 0:ad97421fb1fb 293 }
antbig 0:ad97421fb1fb 294
antbig 0:ad97421fb1fb 295 if (timeout_transmit == 100 ) { // dans le cas d'une sortie en timeout pour ne pas rester bloquer !
antbig 0:ad97421fb1fb 296 return(-1);
antbig 0:ad97421fb1fb 297 }
antbig 0:ad97421fb1fb 298 /* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le servomoteur
antbig 0:ad97421fb1fb 299 */
antbig 0:ad97421fb1fb 300
antbig 0:ad97421fb1fb 301
antbig 0:ad97421fb1fb 302 // Wait for data to transmit
antbig 0:ad97421fb1fb 303 wait (0.005);
antbig 0:ad97421fb1fb 304
antbig 0:ad97421fb1fb 305 // make sure we have a valid return
antbig 0:ad97421fb1fb 306 Status[4]=0x00;
antbig 0:ad97421fb1fb 307
antbig 0:ad97421fb1fb 308 // we'll only get a reply if it was not broadcast
antbig 0:ad97421fb1fb 309 if (ID!=0xFE) {
antbig 0:ad97421fb1fb 310
antbig 0:ad97421fb1fb 311
antbig 0:ad97421fb1fb 312 /* Partie de reception de la trame de retour
antbig 0:ad97421fb1fb 313 */
antbig 0:ad97421fb1fb 314 while ((flag_out != 1) && (timeout < MAX_TIMEOUT)) {
antbig 0:ad97421fb1fb 315 // Les differents etats de l'automate on été créés au debut de la fonction write !
antbig 0:ad97421fb1fb 316 switch (etat) {
antbig 0:ad97421fb1fb 317 case Header1:
antbig 0:ad97421fb1fb 318 if (AX12_Serial.readable()) { // reception du premier Header ( 0xFF )
antbig 0:ad97421fb1fb 319 Status[plen] = AX12_Serial.getc();
antbig 0:ad97421fb1fb 320 timeout = 0;
antbig 0:ad97421fb1fb 321 if (Status[plen] == 0xFF ) {
antbig 0:ad97421fb1fb 322 etat = Header2;
antbig 0:ad97421fb1fb 323 plen++;
antbig 0:ad97421fb1fb 324
antbig 0:ad97421fb1fb 325 } else etat = Header1;
antbig 0:ad97421fb1fb 326 } else timeout++;
antbig 0:ad97421fb1fb 327 break;
antbig 0:ad97421fb1fb 328
antbig 0:ad97421fb1fb 329
antbig 0:ad97421fb1fb 330 case Header2:
antbig 0:ad97421fb1fb 331 if (AX12_Serial.readable()) { // reception du second Header ( 0xFF )
antbig 0:ad97421fb1fb 332 Status[plen] = AX12_Serial.getc();
antbig 0:ad97421fb1fb 333 timeout = 0;
antbig 0:ad97421fb1fb 334 if (Status[plen] == 0xFF ) {
antbig 0:ad97421fb1fb 335 etat = ident;
antbig 0:ad97421fb1fb 336 plen++;
antbig 0:ad97421fb1fb 337 } else {
antbig 0:ad97421fb1fb 338 etat = Header1;
antbig 0:ad97421fb1fb 339 plen = 0;
antbig 0:ad97421fb1fb 340 }
antbig 0:ad97421fb1fb 341 } else timeout++;
antbig 0:ad97421fb1fb 342 break;
antbig 0:ad97421fb1fb 343
antbig 0:ad97421fb1fb 344 case ident:
antbig 0:ad97421fb1fb 345 if (AX12_Serial.readable()) { // reception de l'octet correspondant à l'ID du servomoteur
antbig 0:ad97421fb1fb 346 Status[plen] = AX12_Serial.getc();
antbig 0:ad97421fb1fb 347 timeout = 0;
antbig 0:ad97421fb1fb 348 if (Status[plen] == ID ) {
antbig 0:ad97421fb1fb 349 etat = length;
antbig 0:ad97421fb1fb 350 plen++;
antbig 0:ad97421fb1fb 351 } else {
antbig 0:ad97421fb1fb 352 etat = Header1;
antbig 0:ad97421fb1fb 353 plen = 0;
antbig 0:ad97421fb1fb 354 }
antbig 0:ad97421fb1fb 355 } else timeout++;
antbig 0:ad97421fb1fb 356 break;
antbig 0:ad97421fb1fb 357
antbig 0:ad97421fb1fb 358 case length:
antbig 0:ad97421fb1fb 359 if (AX12_Serial.readable()) { // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres )
antbig 0:ad97421fb1fb 360 Status[plen] = AX12_Serial.getc();
antbig 0:ad97421fb1fb 361 timeout = 0;
antbig 0:ad97421fb1fb 362 if (Status[plen] == 2 ) { // dans la trame de retour d'un write il n'y a pas de paramètre la taille vaudra donc 2!!
antbig 0:ad97421fb1fb 363 etat = erreur;
antbig 0:ad97421fb1fb 364 plen++;
antbig 0:ad97421fb1fb 365 } else {
antbig 0:ad97421fb1fb 366 etat = Header1;
antbig 0:ad97421fb1fb 367 plen = 0;
antbig 0:ad97421fb1fb 368 }
antbig 0:ad97421fb1fb 369 } else timeout++;
antbig 0:ad97421fb1fb 370 break;
antbig 0:ad97421fb1fb 371
antbig 0:ad97421fb1fb 372 case erreur:
antbig 0:ad97421fb1fb 373 if (AX12_Serial.readable()) { //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur )
antbig 0:ad97421fb1fb 374 Status[plen] = AX12_Serial.getc();
antbig 0:ad97421fb1fb 375 timeout = 0;
antbig 0:ad97421fb1fb 376 plen++;
antbig 0:ad97421fb1fb 377 etat = checksum;
antbig 0:ad97421fb1fb 378 } else timeout++;
antbig 0:ad97421fb1fb 379
antbig 0:ad97421fb1fb 380 case checksum:
antbig 0:ad97421fb1fb 381 if (AX12_Serial.readable()) { // recpetion du dernier octet ( Checksum ) >>> checksum = NOT ( ID + length ) >>>> dans le cas de la reception d'un write
antbig 0:ad97421fb1fb 382 Status[plen] = AX12_Serial.getc();
antbig 0:ad97421fb1fb 383 timeout = 0;
antbig 0:ad97421fb1fb 384 flag_out = 1;
antbig 0:ad97421fb1fb 385 etat = Header1;
antbig 0:ad97421fb1fb 386 } else timeout++;
antbig 0:ad97421fb1fb 387 break;
antbig 0:ad97421fb1fb 388 }
antbig 0:ad97421fb1fb 389 }
antbig 0:ad97421fb1fb 390
antbig 0:ad97421fb1fb 391 if (timeout == MAX_TIMEOUT ) { // permet d'afficher si il y a une erreur de timeout et de ne pas rester bloquer si il y a des erreurs de trames
antbig 0:ad97421fb1fb 392 return(-1);
antbig 0:ad97421fb1fb 393 }
antbig 0:ad97421fb1fb 394
antbig 0:ad97421fb1fb 395 // Build the TxPacket first in RAM, then we'll send in one go
antbig 0:ad97421fb1fb 396 }
antbig 0:ad97421fb1fb 397
antbig 0:ad97421fb1fb 398 return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour )
antbig 0:ad97421fb1fb 399 }