CRAC Team / CRAC-Strat_2017

pour trouver l'automate de reception can

Fork of CRAC-Strat_copy by Clement Breteau

AX12-V2/AX12-V2.cpp@5:dcd817534b57, 2016-04-23 (annotated)

Committer:
antbig
Date:
Sat Apr 23 09:16:14 2016 +0000
Revision:
5:dcd817534b57
Parent:
1:116040d14164
Child:
8:0edc7dfb7f7e
Ajout du choix de la couleur et de l'id de la strat?gie ? utiliser via CAN

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 5:dcd817534b57 5 SerialHalfDuplex AX12_Serial1 = SerialHalfDuplex(p9,p10);
antbig 5:dcd817534b57 6 SerialHalfDuplex AX12_Serial2 = SerialHalfDuplex(p28,p27);
antbig 0:ad97421fb1fb 7
antbig 0:ad97421fb1fb 8 int lastAX12Use = 0;
antbig 0:ad97421fb1fb 9
antbig 1:116040d14164 10 FunctionPointer AX12_CallbackEnd;//Fonction de callback lors de la fin d'un mouvement d'un AX12
antbig 1:116040d14164 11
antbig 0:ad97421fb1fb 12 /****************************************************************************************/
antbig 0:ad97421fb1fb 13 /* FUNCTION NAME: AX12_register */
antbig 0:ad97421fb1fb 14 /* DESCRIPTION : Indiquer qu'un AX12 est connecté à la carte */
antbig 0:ad97421fb1fb 15 /****************************************************************************************/
antbig 5:dcd817534b57 16 void AX12_register(unsigned char id, unsigned char serial, unsigned short speed)
antbig 0:ad97421fb1fb 17 {
antbig 0:ad97421fb1fb 18 int localID = AX12_getLocalID(id);
antbig 0:ad97421fb1fb 19 char data[2];
antbig 0:ad97421fb1fb 20
antbig 0:ad97421fb1fb 21 AX12_data[localID].isUsingCAN = 0;//On indique que l'AX12 est connecté localement
antbig 0:ad97421fb1fb 22
antbig 0:ad97421fb1fb 23 if(speed > 0x3FF) speed = 0x3FF;//La vitesse ne doit pas depasser 1023
antbig 0:ad97421fb1fb 24
antbig 0:ad97421fb1fb 25 AX12_data[lastAX12Use].speed = speed;
antbig 5:dcd817534b57 26 AX12_data[lastAX12Use].serial = serial;
antbig 5:dcd817534b57 27
antbig 5:dcd817534b57 28 AX12_Serial1.baud(1000000);//On indique la vitesse de transmission des AX12
antbig 5:dcd817534b57 29 AX12_Serial2.baud(1000000);//On indique la vitesse de transmission des AX12
antbig 0:ad97421fb1fb 30
antbig 0:ad97421fb1fb 31 }
antbig 0:ad97421fb1fb 32
antbig 0:ad97421fb1fb 33 /****************************************************************************************/
antbig 0:ad97421fb1fb 34 /* FUNCTION NAME: AX12_setGoal */
antbig 0:ad97421fb1fb 35 /* DESCRIPTION : Definir la position d'un ax12, !!Ne déclanche pas le mouvement */
antbig 0:ad97421fb1fb 36 /****************************************************************************************/
antbig 0:ad97421fb1fb 37 void AX12_setGoal(unsigned char id, unsigned short goal, unsigned short speed)
antbig 0:ad97421fb1fb 38 {
antbig 0:ad97421fb1fb 39 int localID = AX12_getLocalID(id);//On récupère les info sur l'AX12
antbig 0:ad97421fb1fb 40 CANMessage msgTx=CANMessage();
antbig 0:ad97421fb1fb 41
antbig 0:ad97421fb1fb 42 AX12_data[localID].needToUpdate = 1;
antbig 0:ad97421fb1fb 43 AX12_data[localID].goal = goal;
antbig 0:ad97421fb1fb 44 if(speed > 0x3FF) speed = 0x3FF;//La vitesse ne doit pas depasser 1023
antbig 1:116040d14164 45 if(speed != 0x3FF)
antbig 1:116040d14164 46 AX12_data[localID].speed = speed;
antbig 0:ad97421fb1fb 47
antbig 0:ad97421fb1fb 48 if(AX12_data[localID].isUsingCAN != 0) {//Il faut envoyer la trame CAN car l'AX12 est sur une autre carte
antbig 0:ad97421fb1fb 49 msgTx.id=SERVO_AX12_SETGOAL;
antbig 1:116040d14164 50 msgTx.len=5;
antbig 0:ad97421fb1fb 51 msgTx.format=CANStandard;
antbig 0:ad97421fb1fb 52 msgTx.type=CANData;
antbig 0:ad97421fb1fb 53 // id de l'AX12 sur 1 octet
antbig 0:ad97421fb1fb 54 msgTx.data[0]=(unsigned char)id;
antbig 0:ad97421fb1fb 55 // Position de l'AX12 sur 2 octet
antbig 0:ad97421fb1fb 56 msgTx.data[1]=(unsigned char)goal;
antbig 0:ad97421fb1fb 57 msgTx.data[2]=(unsigned char)(goal>>8);
antbig 0:ad97421fb1fb 58 //Vitesse de l'AX12 sur 2 octet
antbig 1:116040d14164 59 msgTx.data[3]=(unsigned char)AX12_data[localID].speed;
antbig 1:116040d14164 60 msgTx.data[4]=(unsigned char)(AX12_data[localID].speed>>8);
antbig 0:ad97421fb1fb 61
antbig 0:ad97421fb1fb 62 can1.write(msgTx);
antbig 0:ad97421fb1fb 63 }
antbig 0:ad97421fb1fb 64
antbig 0:ad97421fb1fb 65 }
antbig 0:ad97421fb1fb 66
antbig 0:ad97421fb1fb 67 /****************************************************************************************/
antbig 0:ad97421fb1fb 68 /* FUNCTION NAME: AX12_isLocal */
antbig 0:ad97421fb1fb 69 /* DESCRIPTION : Savoir si un AX12 est enregistré sur la carte */
antbig 0:ad97421fb1fb 70 /****************************************************************************************/
antbig 0:ad97421fb1fb 71 unsigned char AX12_isLocal(unsigned char id)
antbig 0:ad97421fb1fb 72 {
antbig 0:ad97421fb1fb 73 int i=0;
antbig 0:ad97421fb1fb 74 for(i=0;i<MAX_AX12;i++)
antbig 0:ad97421fb1fb 75 {
antbig 0:ad97421fb1fb 76 if(AX12_data[i].id == id && AX12_data[i].isUsingCAN == 0) return 1;
antbig 0:ad97421fb1fb 77 }
antbig 0:ad97421fb1fb 78 return 0;
antbig 0:ad97421fb1fb 79 }
antbig 0:ad97421fb1fb 80
antbig 0:ad97421fb1fb 81
antbig 0:ad97421fb1fb 82 /****************************************************************************************/
antbig 0:ad97421fb1fb 83 /* FUNCTION NAME: AX12_getLocalID */
antbig 0:ad97421fb1fb 84 /* DESCRIPTION : Obtenir les info sur un AX12 ou l'initialiser si non présent */
antbig 0:ad97421fb1fb 85 /****************************************************************************************/
antbig 0:ad97421fb1fb 86 int AX12_getLocalID(unsigned char id)
antbig 0:ad97421fb1fb 87 {
antbig 0:ad97421fb1fb 88 int i=0;
antbig 0:ad97421fb1fb 89 for(i=0;i<MAX_AX12;i++)
antbig 0:ad97421fb1fb 90 {
antbig 0:ad97421fb1fb 91 if(AX12_data[i].id == id) return i;
antbig 0:ad97421fb1fb 92 }
antbig 0:ad97421fb1fb 93 //Si l'AX12 n'est pas déjà initialisé, on l'initialise
antbig 0:ad97421fb1fb 94 AX12_data[lastAX12Use].id = id;//
antbig 0:ad97421fb1fb 95 AX12_data[lastAX12Use].goal = 0;//
antbig 0:ad97421fb1fb 96 AX12_data[lastAX12Use].speed = 0x320;//
antbig 0:ad97421fb1fb 97 AX12_data[lastAX12Use].isUsingCAN = 1;//Indique qu'il faut envoyer le message via CAN
antbig 0:ad97421fb1fb 98 AX12_data[lastAX12Use].needToUpdate = 0;//
antbig 0:ad97421fb1fb 99 lastAX12Use++;
antbig 0:ad97421fb1fb 100 return lastAX12Use-1;
antbig 0:ad97421fb1fb 101 }
antbig 0:ad97421fb1fb 102
antbig 0:ad97421fb1fb 103 /****************************************************************************************/
antbig 1:116040d14164 104 /* FUNCTION NAME: AX12_notifyCANEnd */
antbig 1:116040d14164 105 /* DESCRIPTION : indiquer qu'un mouvement d'AX12 CAN est terminé */
antbig 1:116040d14164 106 /****************************************************************************************/
antbig 1:116040d14164 107 void AX12_notifyCANEnd(unsigned char id)
antbig 1:116040d14164 108 {
antbig 1:116040d14164 109 if(waitingAckFrom == SERVO_AX12_DONE) {
antbig 1:116040d14164 110 waitingAckFrom = 0;
antbig 1:116040d14164 111 waitingAckID = 0;
antbig 1:116040d14164 112 }
antbig 1:116040d14164 113 }
antbig 1:116040d14164 114
antbig 1:116040d14164 115 /****************************************************************************************/
antbig 0:ad97421fb1fb 116 /* FUNCTION NAME: AX12_doLoop */
antbig 0:ad97421fb1fb 117 /* DESCRIPTION : Boucle de vérification de la position des AX12 */
antbig 0:ad97421fb1fb 118 /****************************************************************************************/
antbig 0:ad97421fb1fb 119 void AX12_doLoop(void)
antbig 0:ad97421fb1fb 120 {
antbig 1:116040d14164 121 int i=0;
antbig 1:116040d14164 122 CANMessage msgTx=CANMessage();
antbig 1:116040d14164 123
antbig 1:116040d14164 124 for(i=0;i<MAX_AX12;i++)
antbig 1:116040d14164 125 {
antbig 1:116040d14164 126 if(AX12_data[i].isUsingCAN == 0 && AX12_data[i].needCheckMoving == 1)//Il faut vérifier si l'AX12 a terminé de bouger
antbig 1:116040d14164 127 {
antbig 5:dcd817534b57 128 if(AX12_isMoving(i) == 0) {//L'AX12 a terminé de bouger
antbig 1:116040d14164 129 AX12_data[i].needCheckMoving = 0;
antbig 1:116040d14164 130
antbig 1:116040d14164 131 msgTx.id=SERVO_AX12_DONE;
antbig 1:116040d14164 132 msgTx.len=1;
antbig 1:116040d14164 133 msgTx.format=CANStandard;
antbig 1:116040d14164 134 msgTx.type=CANData;
antbig 1:116040d14164 135 // id de l'AX12 sur 1 octet
antbig 1:116040d14164 136 msgTx.data[0]=(unsigned char)AX12_data[i].id;
antbig 1:116040d14164 137 can1.write(msgTx);
antbig 1:116040d14164 138 AX12_notifyCANEnd(AX12_data[i].id);
antbig 1:116040d14164 139 }
antbig 1:116040d14164 140 }
antbig 1:116040d14164 141 }
antbig 0:ad97421fb1fb 142 }
antbig 0:ad97421fb1fb 143
antbig 0:ad97421fb1fb 144 /****************************************************************************************/
antbig 0:ad97421fb1fb 145 /* FUNCTION NAME: AX12_processChange */
antbig 0:ad97421fb1fb 146 /* DESCRIPTION : Permet de prendre en compte les changement d'instruction des AX12 */
antbig 0:ad97421fb1fb 147 /* Début du mouvement à partir de l'appel de cette fonction */
antbig 0:ad97421fb1fb 148 /****************************************************************************************/
antbig 5:dcd817534b57 149 void AX12_processChange(unsigned char fromCan)
antbig 0:ad97421fb1fb 150 {
antbig 0:ad97421fb1fb 151 int i=0;
antbig 0:ad97421fb1fb 152 int dataToSendLength = 0;
antbig 1:116040d14164 153 char dataToSend[100];
antbig 0:ad97421fb1fb 154
antbig 0:ad97421fb1fb 155
antbig 0:ad97421fb1fb 156 for(i=0;i<MAX_AX12;i++)
antbig 0:ad97421fb1fb 157 {
antbig 0:ad97421fb1fb 158 if(AX12_data[i].needToUpdate == 1) //Il faut mettre à jour la position de l'AX12
antbig 0:ad97421fb1fb 159 {
antbig 0:ad97421fb1fb 160 if(AX12_data[i].isUsingCAN == 0)//Il faut envoyer la trame en local
antbig 0:ad97421fb1fb 161 {
antbig 0:ad97421fb1fb 162 if(dataToSendLength == 0)
antbig 1:116040d14164 163 dataToSend[dataToSendLength++] = 4;//length data
antbig 0:ad97421fb1fb 164 dataToSend[dataToSendLength++] = AX12_data[i].id;//ID servo1
antbig 0:ad97421fb1fb 165 dataToSend[dataToSendLength++] = ((1023 * AX12_data[i].goal) / 300) & 0xff;// bottom 8 bits
antbig 1:116040d14164 166 dataToSend[dataToSendLength++] = ((1023 * AX12_data[i].goal) / 300) >> 8; // top 8 bits
antbig 1:116040d14164 167 dataToSend[dataToSendLength++] = (AX12_data[i].speed) & 0xff;// bottom 8 bits
antbig 1:116040d14164 168 dataToSend[dataToSendLength++] = (AX12_data[i].speed) >> 8; // top 8 bits
antbig 1:116040d14164 169 AX12_data[i].needCheckMoving = 1;
antbig 0:ad97421fb1fb 170 }
antbig 0:ad97421fb1fb 171 AX12_data[i].needToUpdate = 0;//Remise à 0 de l'indicatif de mise à jour
antbig 0:ad97421fb1fb 172 }
antbig 0:ad97421fb1fb 173 }
antbig 5:dcd817534b57 174 if(fromCan == 0)
antbig 5:dcd817534b57 175 SendRawId(SERVO_AX12_PROCESS);//On indique par CAN qu'il faut bouger les AX12
antbig 0:ad97421fb1fb 176
antbig 0:ad97421fb1fb 177 if(dataToSendLength > 0)//Il y a des données à envoyer en local
antbig 0:ad97421fb1fb 178 {
antbig 5:dcd817534b57 179 AX12_syncWrite(AX12_Serial1, AX12_REG_GOAL_POSITION, dataToSendLength, dataToSend);
antbig 5:dcd817534b57 180 AX12_syncWrite(AX12_Serial2, AX12_REG_GOAL_POSITION, dataToSendLength, dataToSend);
antbig 0:ad97421fb1fb 181 }
antbig 0:ad97421fb1fb 182
antbig 0:ad97421fb1fb 183 }
antbig 0:ad97421fb1fb 184
antbig 0:ad97421fb1fb 185 /****************************************************************************************/
antbig 1:116040d14164 186 /* FUNCTION NAME: AX12_isMoving */
antbig 1:116040d14164 187 /* DESCRIPTION : Fonction pour savoir si un AX12 local est entrain de bouger */
antbig 1:116040d14164 188 /****************************************************************************************/
antbig 1:116040d14164 189 int AX12_isMoving(unsigned char id)
antbig 1:116040d14164 190 {
antbig 1:116040d14164 191 char data[1];
antbig 5:dcd817534b57 192 if(AX12_data[id].serial == 0)
antbig 5:dcd817534b57 193 AX12_read(AX12_Serial1,AX12_data[id].id,AX12_REG_MOVING,1,data);
antbig 5:dcd817534b57 194 else
antbig 5:dcd817534b57 195 AX12_read(AX12_Serial2,AX12_data[id].id,AX12_REG_MOVING,1,data);
antbig 1:116040d14164 196 return(data[0]);
antbig 1:116040d14164 197 }
antbig 1:116040d14164 198
antbig 1:116040d14164 199 /****************************************************************************************/
antbig 0:ad97421fb1fb 200 /* FUNCTION NAME: AX12_syncWrite */
antbig 0:ad97421fb1fb 201 /* DESCRIPTION : Fonction pour envoyer des trames aux AX12 en mode syncWrite */
antbig 0:ad97421fb1fb 202 /****************************************************************************************/
antbig 5:dcd817534b57 203 int AX12_syncWrite(SerialHalfDuplex& AX12_Serial, int start, int bytes, char* data)
antbig 0:ad97421fb1fb 204 {
antbig 0:ad97421fb1fb 205 //0 : 0xff
antbig 0:ad97421fb1fb 206 //1 : 0xff
antbig 0:ad97421fb1fb 207 //2 : ID de l'AX12 ou 0xFE pour le broadcast
antbig 0:ad97421fb1fb 208 //3 : Length => longueur de la trame
antbig 0:ad97421fb1fb 209 //4 : Intruction(write) => id de l'instruction 0x83 pour le syncwrite
antbig 0:ad97421fb1fb 210 //5 : Address => addresse du registre à modifier
antbig 0:ad97421fb1fb 211 //6+ : Data => les données à transmettre
antbig 0:ad97421fb1fb 212 //last : Checksum
antbig 0:ad97421fb1fb 213 int ID = 0xFE;//Toujours 0xFE dans le cas d'un broadcast
antbig 0:ad97421fb1fb 214
antbig 0:ad97421fb1fb 215 char TxBuf[60];
antbig 0:ad97421fb1fb 216 char sum = 0;
antbig 0:ad97421fb1fb 217
antbig 0:ad97421fb1fb 218 int timeout_transmit = 0;
antbig 0:ad97421fb1fb 219 int i = 0;
antbig 0:ad97421fb1fb 220 printf("Start sending moving trame\n");
antbig 0:ad97421fb1fb 221 // Build the TxPacket first in RAM, then we'll send in one go
antbig 0:ad97421fb1fb 222
antbig 0:ad97421fb1fb 223 TxBuf[0] = 0xff;
antbig 0:ad97421fb1fb 224 TxBuf[1] = 0xff;
antbig 0:ad97421fb1fb 225
antbig 0:ad97421fb1fb 226 // ID
antbig 0:ad97421fb1fb 227 TxBuf[2] = ID;
antbig 0:ad97421fb1fb 228 sum += TxBuf[2];
antbig 0:ad97421fb1fb 229
antbig 0:ad97421fb1fb 230 // packet Length
antbig 0:ad97421fb1fb 231 TxBuf[3] = 3+bytes;
antbig 0:ad97421fb1fb 232 sum += TxBuf[3];
antbig 0:ad97421fb1fb 233
antbig 0:ad97421fb1fb 234
antbig 0:ad97421fb1fb 235 // Instruction
antbig 0:ad97421fb1fb 236 TxBuf[4]=0x83;//toujours 0x83 dans le cas d'un syncwrite
antbig 0:ad97421fb1fb 237 sum += TxBuf[4];
antbig 0:ad97421fb1fb 238
antbig 0:ad97421fb1fb 239
antbig 0:ad97421fb1fb 240 // Start Address
antbig 0:ad97421fb1fb 241 TxBuf[5] = start;//addresse du registre à modifier
antbig 0:ad97421fb1fb 242 sum += TxBuf[5];
antbig 0:ad97421fb1fb 243
antbig 0:ad97421fb1fb 244 // data
antbig 0:ad97421fb1fb 245 for (char i=0; i<bytes ; i++) {
antbig 0:ad97421fb1fb 246 TxBuf[6+i] = data[i];
antbig 0:ad97421fb1fb 247 sum += TxBuf[6+i];
antbig 0:ad97421fb1fb 248 printf(" Data : 0x%x\n",TxBuf[6+i]);
antbig 0:ad97421fb1fb 249 }
antbig 0:ad97421fb1fb 250
antbig 0:ad97421fb1fb 251 // checksum
antbig 0:ad97421fb1fb 252 TxBuf[6+bytes] = 0xFF - sum;
antbig 0:ad97421fb1fb 253
antbig 0:ad97421fb1fb 254
antbig 0:ad97421fb1fb 255 /* Transmission de la trame construite precedemment dans le tableau TxBuf
antbig 0:ad97421fb1fb 256 */
antbig 0:ad97421fb1fb 257 while ((timeout_transmit<100) && (i < (7+bytes))) {
antbig 0:ad97421fb1fb 258 if (AX12_Serial.writeable()) {
antbig 0:ad97421fb1fb 259 AX12_Serial.putc(TxBuf[i]);
antbig 0:ad97421fb1fb 260 i++;
antbig 0:ad97421fb1fb 261 timeout_transmit = 0;
antbig 0:ad97421fb1fb 262 } else timeout_transmit++;
antbig 0:ad97421fb1fb 263 }
antbig 0:ad97421fb1fb 264
antbig 0:ad97421fb1fb 265 if (timeout_transmit == 100 ) { // dans le cas d'une sortie en timeout pour ne pas rester bloquer !
antbig 0:ad97421fb1fb 266 return(-1);
antbig 0:ad97421fb1fb 267 }
antbig 0:ad97421fb1fb 268
antbig 0:ad97421fb1fb 269 // Wait for data to transmit
antbig 0:ad97421fb1fb 270 wait (0.005);
antbig 0:ad97421fb1fb 271 return(0);//OK trame envoyé
antbig 0:ad97421fb1fb 272 }
antbig 0:ad97421fb1fb 273
antbig 0:ad97421fb1fb 274 /****************************************************************************************/
antbig 0:ad97421fb1fb 275 /* FUNCTION NAME: AX12_write */
antbig 0:ad97421fb1fb 276 /* DESCRIPTION : Fonction pour envoyer des trames aux AX12 */
antbig 0:ad97421fb1fb 277 /****************************************************************************************/
antbig 5:dcd817534b57 278 int AX12_write(SerialHalfDuplex& AX12_Serial, int ID, int start, int bytes, char* data, int flag)
antbig 0:ad97421fb1fb 279 {
antbig 0:ad97421fb1fb 280 // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
antbig 5:dcd817534b57 281 // = AX12_Serial1;
antbig 0:ad97421fb1fb 282 char TxBuf[16];
antbig 0:ad97421fb1fb 283 char sum = 0;
antbig 0:ad97421fb1fb 284 char Status[6];
antbig 0:ad97421fb1fb 285
antbig 0:ad97421fb1fb 286 int timeout = 0;
antbig 0:ad97421fb1fb 287 int plen = 0;
antbig 0:ad97421fb1fb 288 int flag_out = 0;
antbig 0:ad97421fb1fb 289 int timeout_transmit = 0;
antbig 0:ad97421fb1fb 290 int i = 0;
antbig 0:ad97421fb1fb 291 /*int poubelle = 0;
antbig 0:ad97421fb1fb 292 int count = 0;
antbig 0:ad97421fb1fb 293 char vidage[50];*/
antbig 0:ad97421fb1fb 294
antbig 0:ad97421fb1fb 295 typedef enum {Header1, Header2, ident, length, erreur, checksum} type_etat;
antbig 0:ad97421fb1fb 296 type_etat etat = Header1;
antbig 0:ad97421fb1fb 297
antbig 0:ad97421fb1fb 298 // Build the TxPacket first in RAM, then we'll send in one go
antbig 0:ad97421fb1fb 299 TxBuf[0] = 0xff;
antbig 0:ad97421fb1fb 300 TxBuf[1] = 0xff;
antbig 0:ad97421fb1fb 301
antbig 0:ad97421fb1fb 302 // ID
antbig 0:ad97421fb1fb 303 TxBuf[2] = ID;
antbig 0:ad97421fb1fb 304 sum += TxBuf[2];
antbig 0:ad97421fb1fb 305
antbig 0:ad97421fb1fb 306 // packet Length
antbig 0:ad97421fb1fb 307 TxBuf[3] = 3+bytes;
antbig 0:ad97421fb1fb 308 sum += TxBuf[3];
antbig 0:ad97421fb1fb 309
antbig 0:ad97421fb1fb 310 // Instruction
antbig 0:ad97421fb1fb 311 if (flag == 1) {
antbig 0:ad97421fb1fb 312 TxBuf[4]=0x04;
antbig 0:ad97421fb1fb 313 sum += TxBuf[4];
antbig 0:ad97421fb1fb 314 } else {
antbig 0:ad97421fb1fb 315 TxBuf[4]=0x03;
antbig 0:ad97421fb1fb 316 sum += TxBuf[4];
antbig 0:ad97421fb1fb 317 }
antbig 0:ad97421fb1fb 318
antbig 0:ad97421fb1fb 319 // Start Address
antbig 0:ad97421fb1fb 320 TxBuf[5] = start;
antbig 0:ad97421fb1fb 321 sum += TxBuf[5];
antbig 0:ad97421fb1fb 322
antbig 0:ad97421fb1fb 323
antbig 0:ad97421fb1fb 324 // data
antbig 0:ad97421fb1fb 325 for (char i=0; i<bytes ; i++) {
antbig 0:ad97421fb1fb 326 TxBuf[6+i] = data[i];
antbig 0:ad97421fb1fb 327 sum += TxBuf[6+i];
antbig 0:ad97421fb1fb 328 }
antbig 0:ad97421fb1fb 329
antbig 0:ad97421fb1fb 330 // checksum
antbig 0:ad97421fb1fb 331 TxBuf[6+bytes] = 0xFF - sum;
antbig 0:ad97421fb1fb 332
antbig 0:ad97421fb1fb 333
antbig 0:ad97421fb1fb 334
antbig 0:ad97421fb1fb 335 /* Transmission de la trame construite precedemment dans le tableau TxBuf
antbig 0:ad97421fb1fb 336 */
antbig 0:ad97421fb1fb 337 while ((timeout_transmit<100) && (i < (7+bytes))) {
antbig 0:ad97421fb1fb 338 if (AX12_Serial.writeable()) {
antbig 0:ad97421fb1fb 339 AX12_Serial.putc(TxBuf[i]);
antbig 0:ad97421fb1fb 340 i++;
antbig 0:ad97421fb1fb 341 timeout_transmit = 0;
antbig 0:ad97421fb1fb 342 } else timeout_transmit++;
antbig 0:ad97421fb1fb 343 }
antbig 0:ad97421fb1fb 344
antbig 0:ad97421fb1fb 345 if (timeout_transmit == 100 ) { // dans le cas d'une sortie en timeout pour ne pas rester bloquer !
antbig 0:ad97421fb1fb 346 return(-1);
antbig 0:ad97421fb1fb 347 }
antbig 0:ad97421fb1fb 348 /* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le servomoteur
antbig 0:ad97421fb1fb 349 */
antbig 0:ad97421fb1fb 350
antbig 0:ad97421fb1fb 351
antbig 0:ad97421fb1fb 352 // Wait for data to transmit
antbig 0:ad97421fb1fb 353 wait (0.005);
antbig 0:ad97421fb1fb 354
antbig 0:ad97421fb1fb 355 // make sure we have a valid return
antbig 0:ad97421fb1fb 356 Status[4]=0x00;
antbig 0:ad97421fb1fb 357
antbig 0:ad97421fb1fb 358 // we'll only get a reply if it was not broadcast
antbig 0:ad97421fb1fb 359 if (ID!=0xFE) {
antbig 0:ad97421fb1fb 360
antbig 0:ad97421fb1fb 361
antbig 0:ad97421fb1fb 362 /* Partie de reception de la trame de retour
antbig 0:ad97421fb1fb 363 */
antbig 0:ad97421fb1fb 364 while ((flag_out != 1) && (timeout < MAX_TIMEOUT)) {
antbig 0:ad97421fb1fb 365 // Les differents etats de l'automate on été créés au debut de la fonction write !
antbig 0:ad97421fb1fb 366 switch (etat) {
antbig 0:ad97421fb1fb 367 case Header1:
antbig 0:ad97421fb1fb 368 if (AX12_Serial.readable()) { // reception du premier Header ( 0xFF )
antbig 0:ad97421fb1fb 369 Status[plen] = AX12_Serial.getc();
antbig 0:ad97421fb1fb 370 timeout = 0;
antbig 0:ad97421fb1fb 371 if (Status[plen] == 0xFF ) {
antbig 0:ad97421fb1fb 372 etat = Header2;
antbig 0:ad97421fb1fb 373 plen++;
antbig 0:ad97421fb1fb 374
antbig 0:ad97421fb1fb 375 } else etat = Header1;
antbig 0:ad97421fb1fb 376 } else timeout++;
antbig 0:ad97421fb1fb 377 break;
antbig 0:ad97421fb1fb 378
antbig 0:ad97421fb1fb 379
antbig 0:ad97421fb1fb 380 case Header2:
antbig 0:ad97421fb1fb 381 if (AX12_Serial.readable()) { // reception du second Header ( 0xFF )
antbig 0:ad97421fb1fb 382 Status[plen] = AX12_Serial.getc();
antbig 0:ad97421fb1fb 383 timeout = 0;
antbig 0:ad97421fb1fb 384 if (Status[plen] == 0xFF ) {
antbig 0:ad97421fb1fb 385 etat = ident;
antbig 0:ad97421fb1fb 386 plen++;
antbig 0:ad97421fb1fb 387 } else {
antbig 0:ad97421fb1fb 388 etat = Header1;
antbig 0:ad97421fb1fb 389 plen = 0;
antbig 0:ad97421fb1fb 390 }
antbig 0:ad97421fb1fb 391 } else timeout++;
antbig 0:ad97421fb1fb 392 break;
antbig 0:ad97421fb1fb 393
antbig 0:ad97421fb1fb 394 case ident:
antbig 0:ad97421fb1fb 395 if (AX12_Serial.readable()) { // reception de l'octet correspondant à l'ID du servomoteur
antbig 0:ad97421fb1fb 396 Status[plen] = AX12_Serial.getc();
antbig 0:ad97421fb1fb 397 timeout = 0;
antbig 0:ad97421fb1fb 398 if (Status[plen] == ID ) {
antbig 0:ad97421fb1fb 399 etat = length;
antbig 0:ad97421fb1fb 400 plen++;
antbig 0:ad97421fb1fb 401 } else {
antbig 0:ad97421fb1fb 402 etat = Header1;
antbig 0:ad97421fb1fb 403 plen = 0;
antbig 0:ad97421fb1fb 404 }
antbig 0:ad97421fb1fb 405 } else timeout++;
antbig 0:ad97421fb1fb 406 break;
antbig 0:ad97421fb1fb 407
antbig 0:ad97421fb1fb 408 case length:
antbig 0:ad97421fb1fb 409 if (AX12_Serial.readable()) { // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres )
antbig 0:ad97421fb1fb 410 Status[plen] = AX12_Serial.getc();
antbig 0:ad97421fb1fb 411 timeout = 0;
antbig 0:ad97421fb1fb 412 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 413 etat = erreur;
antbig 0:ad97421fb1fb 414 plen++;
antbig 0:ad97421fb1fb 415 } else {
antbig 0:ad97421fb1fb 416 etat = Header1;
antbig 0:ad97421fb1fb 417 plen = 0;
antbig 0:ad97421fb1fb 418 }
antbig 0:ad97421fb1fb 419 } else timeout++;
antbig 0:ad97421fb1fb 420 break;
antbig 0:ad97421fb1fb 421
antbig 0:ad97421fb1fb 422 case erreur:
antbig 0:ad97421fb1fb 423 if (AX12_Serial.readable()) { //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur )
antbig 0:ad97421fb1fb 424 Status[plen] = AX12_Serial.getc();
antbig 0:ad97421fb1fb 425 timeout = 0;
antbig 0:ad97421fb1fb 426 plen++;
antbig 0:ad97421fb1fb 427 etat = checksum;
antbig 0:ad97421fb1fb 428 } else timeout++;
antbig 0:ad97421fb1fb 429
antbig 0:ad97421fb1fb 430 case checksum:
antbig 0:ad97421fb1fb 431 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 432 Status[plen] = AX12_Serial.getc();
antbig 0:ad97421fb1fb 433 timeout = 0;
antbig 0:ad97421fb1fb 434 flag_out = 1;
antbig 0:ad97421fb1fb 435 etat = Header1;
antbig 0:ad97421fb1fb 436 } else timeout++;
antbig 0:ad97421fb1fb 437 break;
antbig 0:ad97421fb1fb 438 }
antbig 0:ad97421fb1fb 439 }
antbig 0:ad97421fb1fb 440
antbig 0:ad97421fb1fb 441 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 442 return(-1);
antbig 0:ad97421fb1fb 443 }
antbig 0:ad97421fb1fb 444
antbig 0:ad97421fb1fb 445 // Build the TxPacket first in RAM, then we'll send in one go
antbig 0:ad97421fb1fb 446 }
antbig 0:ad97421fb1fb 447
antbig 0:ad97421fb1fb 448 return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour )
antbig 0:ad97421fb1fb 449 }
antbig 1:116040d14164 450
antbig 1:116040d14164 451
antbig 1:116040d14164 452 /****************************************************************************************/
antbig 1:116040d14164 453 /* FUNCTION NAME: AX12_read */
antbig 1:116040d14164 454 /* DESCRIPTION : Lire des données dans un registre de l'AX12 */
antbig 1:116040d14164 455 /****************************************************************************************/
antbig 5:dcd817534b57 456 int AX12_read(SerialHalfDuplex& AX12_Serial, int ID, int start, int bytes, char* data)
antbig 1:116040d14164 457 {
antbig 1:116040d14164 458
antbig 5:dcd817534b57 459 //SerialHalfDuplex AX12_Serial = AX12_Serial1;
antbig 1:116040d14164 460 char PacketLength = 0x3;
antbig 1:116040d14164 461 char TxBuf[16];
antbig 1:116040d14164 462 char sum = 0;
antbig 1:116040d14164 463 char Status[16];
antbig 1:116040d14164 464
antbig 1:116040d14164 465 int timeout = 0;
antbig 1:116040d14164 466 int plen = 0;
antbig 1:116040d14164 467 int flag_out = 0;
antbig 1:116040d14164 468 int timeout_transmit = 0;
antbig 1:116040d14164 469 int i = 0;
antbig 1:116040d14164 470 int enable = 0;
antbig 1:116040d14164 471 // int poubelle = 0;
antbig 1:116040d14164 472 // int count = 0;
antbig 1:116040d14164 473 // char vidage[50];
antbig 1:116040d14164 474
antbig 1:116040d14164 475 typedef enum {Header1, Header2, ident, length, erreur, reception, checksum} type_etat;
antbig 1:116040d14164 476 type_etat etat = Header1;
antbig 1:116040d14164 477
antbig 1:116040d14164 478 Status[4] = 0xFE; // return code
antbig 1:116040d14164 479
antbig 1:116040d14164 480
antbig 1:116040d14164 481
antbig 1:116040d14164 482
antbig 1:116040d14164 483
antbig 1:116040d14164 484 /*********************************** CREATION DE LA TRAME A EVOYER *****************************************/
antbig 1:116040d14164 485
antbig 1:116040d14164 486
antbig 1:116040d14164 487 // Build the TxPacket first in RAM, then we'll send in one go
antbig 1:116040d14164 488
antbig 1:116040d14164 489 TxBuf[0] = 0xff;
antbig 1:116040d14164 490 TxBuf[1] = 0xff;
antbig 1:116040d14164 491
antbig 1:116040d14164 492 // ID
antbig 1:116040d14164 493 TxBuf[2] = ID;
antbig 1:116040d14164 494 sum += TxBuf[2];
antbig 1:116040d14164 495
antbig 1:116040d14164 496 // Packet Length
antbig 1:116040d14164 497 TxBuf[3] = PacketLength+bytes; // Length = 4 ; 2 + 1 (start) = 1 (bytes)
antbig 1:116040d14164 498 sum += TxBuf[3]; // Accululate the packet sum
antbig 1:116040d14164 499
antbig 1:116040d14164 500
antbig 1:116040d14164 501 // Instruction - Read
antbig 1:116040d14164 502 TxBuf[4] = 0x2;
antbig 1:116040d14164 503 sum += TxBuf[4];
antbig 1:116040d14164 504
antbig 1:116040d14164 505 // Start Address
antbig 1:116040d14164 506 TxBuf[5] = start;
antbig 1:116040d14164 507 sum += TxBuf[5];
antbig 1:116040d14164 508
antbig 1:116040d14164 509 // Bytes to read
antbig 1:116040d14164 510 TxBuf[6] = bytes;
antbig 1:116040d14164 511 sum += TxBuf[6];
antbig 1:116040d14164 512
antbig 1:116040d14164 513 // Checksum
antbig 1:116040d14164 514 TxBuf[7] = 0xFF - sum;
antbig 1:116040d14164 515
antbig 1:116040d14164 516 /********************************************TRAME CONSTRUITE DANS TxBuf***************************************/
antbig 1:116040d14164 517
antbig 1:116040d14164 518
antbig 1:116040d14164 519
antbig 1:116040d14164 520
antbig 1:116040d14164 521 /* Transmission de la trame construite precedemment dans le tableau TxBuf
antbig 1:116040d14164 522 */
antbig 1:116040d14164 523 while ((timeout_transmit<1000) && (i < (7+bytes))) {
antbig 1:116040d14164 524 if (AX12_Serial.writeable()) {
antbig 1:116040d14164 525 AX12_Serial.putc(TxBuf[i]);
antbig 1:116040d14164 526 i++;
antbig 1:116040d14164 527 timeout_transmit = 0;
antbig 1:116040d14164 528 } else timeout_transmit++;
antbig 1:116040d14164 529 }
antbig 1:116040d14164 530
antbig 1:116040d14164 531 if (timeout_transmit == 1000 ) { // dans le cas d'une sortie en timeout pour ne pas rester bloquer !
antbig 1:116040d14164 532 return(-1);
antbig 1:116040d14164 533 }
antbig 1:116040d14164 534 /* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le servomoteur
antbig 1:116040d14164 535 */
antbig 1:116040d14164 536 // Wait for the bytes to be transmitted
antbig 1:116040d14164 537 wait (0.001);
antbig 1:116040d14164 538
antbig 1:116040d14164 539 // Skip if the read was to the broadcast address
antbig 1:116040d14164 540 if (ID != 0xFE) {
antbig 1:116040d14164 541
antbig 1:116040d14164 542 /* Partie de reception de la trame de retour
antbig 1:116040d14164 543 */
antbig 1:116040d14164 544 while ((flag_out != 1) && (timeout < (1000*bytes))) {
antbig 1:116040d14164 545 // Les differents etats de l'automate on été créés au debut de la fonction write !
antbig 1:116040d14164 546 switch (etat) {
antbig 1:116040d14164 547 case Header1:
antbig 1:116040d14164 548 if (AX12_Serial.readable()) { // reception du premier Header ( 0xFF )
antbig 1:116040d14164 549 Status[plen] = AX12_Serial.getc();
antbig 1:116040d14164 550 timeout = 0;
antbig 1:116040d14164 551 if (Status[plen] == 0xFF ) {
antbig 1:116040d14164 552 etat = Header2;
antbig 1:116040d14164 553 plen++;
antbig 1:116040d14164 554
antbig 1:116040d14164 555 } else etat = Header1;
antbig 1:116040d14164 556 } else timeout++;
antbig 1:116040d14164 557 break;
antbig 1:116040d14164 558
antbig 1:116040d14164 559
antbig 1:116040d14164 560 case Header2:
antbig 1:116040d14164 561 if (AX12_Serial.readable()) { // reception du second Header ( 0xFF )
antbig 1:116040d14164 562 Status[plen] = AX12_Serial.getc();
antbig 1:116040d14164 563 timeout = 0;
antbig 1:116040d14164 564 if (Status[plen] == 0xFF ) {
antbig 1:116040d14164 565 etat = ident;
antbig 1:116040d14164 566 plen++;
antbig 1:116040d14164 567
antbig 1:116040d14164 568 } else if (Status[plen] == ID ) { // PERMET D'EVITER CERTAINES ERREUR LORSQU'ON LIT PLUSIEURS REGISTRES !!!!
antbig 1:116040d14164 569 Status[plen] = 0;
antbig 1:116040d14164 570 plen++;
antbig 1:116040d14164 571 Status[plen] = ID;
antbig 1:116040d14164 572 etat = length;
antbig 1:116040d14164 573 plen++;
antbig 1:116040d14164 574
antbig 1:116040d14164 575 } else {
antbig 1:116040d14164 576
antbig 1:116040d14164 577 etat = Header1;
antbig 1:116040d14164 578 plen = 0;
antbig 1:116040d14164 579 }
antbig 1:116040d14164 580 } else timeout++;
antbig 1:116040d14164 581 break;
antbig 1:116040d14164 582
antbig 1:116040d14164 583 case ident:
antbig 1:116040d14164 584 if (AX12_Serial.readable()) { // reception de l'octet correspondant à l'ID du servomoteur
antbig 1:116040d14164 585 Status[plen] = AX12_Serial.getc();
antbig 1:116040d14164 586 timeout = 0;
antbig 1:116040d14164 587 if (Status[plen] == ID ) {
antbig 1:116040d14164 588 etat = length;
antbig 1:116040d14164 589 plen++;
antbig 1:116040d14164 590
antbig 1:116040d14164 591 } else {
antbig 1:116040d14164 592 etat = Header1;
antbig 1:116040d14164 593 plen = 0;
antbig 1:116040d14164 594 }
antbig 1:116040d14164 595 } else timeout++;
antbig 1:116040d14164 596 break;
antbig 1:116040d14164 597
antbig 1:116040d14164 598 case length:
antbig 1:116040d14164 599 if (AX12_Serial.readable()) { // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres )
antbig 1:116040d14164 600 Status[plen] = AX12_Serial.getc();
antbig 1:116040d14164 601 timeout = 0;
antbig 1:116040d14164 602 if (Status[plen] == (bytes+2) ) {
antbig 1:116040d14164 603 etat = erreur;
antbig 1:116040d14164 604 plen++;
antbig 1:116040d14164 605
antbig 1:116040d14164 606 } else {
antbig 1:116040d14164 607 etat = Header1;
antbig 1:116040d14164 608 plen = 0;
antbig 1:116040d14164 609 }
antbig 1:116040d14164 610 } else timeout++;
antbig 1:116040d14164 611 break;
antbig 1:116040d14164 612
antbig 1:116040d14164 613 case erreur:
antbig 1:116040d14164 614 if (AX12_Serial.readable()) { //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur )
antbig 1:116040d14164 615 Status[plen] = AX12_Serial.getc();
antbig 1:116040d14164 616 timeout = 0;
antbig 1:116040d14164 617 plen++;
antbig 1:116040d14164 618
antbig 1:116040d14164 619 etat = reception;
antbig 1:116040d14164 620 } else timeout++;
antbig 1:116040d14164 621
antbig 1:116040d14164 622 case reception:
antbig 1:116040d14164 623 while ( enable < bytes ) { // reception du ou des octect(s) de donnés ( suivant la valeur de la variable length )
antbig 1:116040d14164 624 if (AX12_Serial.readable()) {
antbig 1:116040d14164 625 Status[plen] = AX12_Serial.getc();
antbig 1:116040d14164 626 timeout = 0;
antbig 1:116040d14164 627 plen++;
antbig 1:116040d14164 628 enable++;
antbig 1:116040d14164 629
antbig 1:116040d14164 630 } else timeout++;
antbig 1:116040d14164 631 }
antbig 1:116040d14164 632 etat = checksum;
antbig 1:116040d14164 633 break;
antbig 1:116040d14164 634
antbig 1:116040d14164 635 case checksum:
antbig 1:116040d14164 636 if (AX12_Serial.readable()) { // reception du dernier octet ( Checksum ) >>> checksum = NOT ( ID + length + somme des données ) >>>> dans le cas d'un retour d'un read!!
antbig 1:116040d14164 637 Status[plen] = AX12_Serial.getc();
antbig 1:116040d14164 638 timeout = 0;
antbig 1:116040d14164 639 flag_out = 1;
antbig 1:116040d14164 640 etat = Header1;
antbig 1:116040d14164 641 } else timeout++;
antbig 1:116040d14164 642 break;
antbig 1:116040d14164 643
antbig 1:116040d14164 644 default:
antbig 1:116040d14164 645 break;
antbig 1:116040d14164 646 }
antbig 1:116040d14164 647 }
antbig 1:116040d14164 648
antbig 1:116040d14164 649
antbig 1:116040d14164 650 if (timeout == (1000*bytes) ) { // 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 1:116040d14164 651 return(-1);
antbig 1:116040d14164 652 }
antbig 1:116040d14164 653
antbig 1:116040d14164 654
antbig 1:116040d14164 655 // copie des données dans le tableau data
antbig 1:116040d14164 656 for (int i=0; i < Status[3]-2 ; i++) {
antbig 1:116040d14164 657 data[i] = Status[5+i];
antbig 1:116040d14164 658 }
antbig 1:116040d14164 659
antbig 1:116040d14164 660 } // toute la partie precedente ne s'effectue pas dans le cas d'un appel avec un broadcast ID (ID!=0xFE)
antbig 1:116040d14164 661
antbig 1:116040d14164 662 return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour )
antbig 1:116040d14164 663 }
antbig 1:116040d14164 664
antbig 1:116040d14164 665