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@14:c8fc06c4887f, 2017-03-31 (annotated)

Committer:
ClementBreteau
Date:
Fri Mar 31 16:20:26 2017 +0000
Revision:
14:c8fc06c4887f
Parent:
12:14729d584500 
code strategie du robot, version 31 mars 2017

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