pour trouver l'automate de reception can
Fork of CRAC-Strat_copy by
AX12-V2/AX12-V2.cpp
- Committer:
- antbig
- Date:
- 2016-04-27
- Revision:
- 8:0edc7dfb7f7e
- Parent:
- 5:dcd817534b57
- Child:
- 12:14729d584500
File content as of revision 8:0edc7dfb7f7e:
#include "AX12-V2.h" struct S_AX12 AX12_data[MAX_AX12];//La liste de tous les AX12 du robot possible, aussi bien par CAN que en local SerialHalfDuplex AX12_Serial1 = SerialHalfDuplex(p9,p10); SerialHalfDuplex AX12_Serial2 = SerialHalfDuplex(p28,p27); int lastAX12Use = 0; FunctionPointer AX12_CallbackEnd;//Fonction de callback lors de la fin d'un mouvement d'un AX12 /****************************************************************************************/ /* FUNCTION NAME: AX12_register */ /* DESCRIPTION : Indiquer qu'un AX12 est connecté à la carte */ /****************************************************************************************/ void AX12_register(unsigned char id, unsigned char serial, unsigned short speed) { int localID = AX12_getLocalID(id); AX12_data[localID].isUsingCAN = 0;//On indique que l'AX12 est connecté localement if(speed > 0x3FF) speed = 0x3FF;//La vitesse ne doit pas depasser 1023 AX12_data[localID].speed = speed; AX12_data[localID].serial = serial; //printf("registering AX12 id: %d local: %d\n",id,localID); AX12_Serial1.baud(1000000);//On indique la vitesse de transmission des AX12 AX12_Serial2.baud(1000000);//On indique la vitesse de transmission des AX12 } /****************************************************************************************/ /* FUNCTION NAME: AX12_setGoal */ /* DESCRIPTION : Definir la position d'un ax12, !!Ne déclanche pas le mouvement */ /****************************************************************************************/ void AX12_setGoal(unsigned char id, unsigned short goal, unsigned short speed) { int localID = AX12_getLocalID(id);//On récupère les info sur l'AX12 CANMessage msgTx=CANMessage(); AX12_data[localID].needToUpdate = 1; AX12_data[localID].goal = goal; if(speed > 0x3FF) speed = 0x3FF;//La vitesse ne doit pas depasser 1023 if(speed != 0x3FF) AX12_data[localID].speed = speed; if(AX12_data[localID].isUsingCAN != 0) {//Il faut envoyer la trame CAN car l'AX12 est sur une autre carte msgTx.id=SERVO_AX12_SETGOAL; msgTx.len=5; msgTx.format=CANStandard; msgTx.type=CANData; // id de l'AX12 sur 1 octet msgTx.data[0]=(unsigned char)id; // Position de l'AX12 sur 2 octet msgTx.data[1]=(unsigned char)goal; msgTx.data[2]=(unsigned char)(goal>>8); //Vitesse de l'AX12 sur 2 octet msgTx.data[3]=(unsigned char)AX12_data[localID].speed; msgTx.data[4]=(unsigned char)(AX12_data[localID].speed>>8); can1.write(msgTx); } } /****************************************************************************************/ /* FUNCTION NAME: AX12_isLocal */ /* DESCRIPTION : Savoir si un AX12 est enregistré sur la carte */ /****************************************************************************************/ unsigned char AX12_isLocal(unsigned char id) { int i=0; for(i=0;i<MAX_AX12;i++) { if(AX12_data[i].id == id && AX12_data[i].isUsingCAN == 0) return 1; } return 0; } /****************************************************************************************/ /* FUNCTION NAME: AX12_getLocalID */ /* DESCRIPTION : Obtenir les info sur un AX12 ou l'initialiser si non présent */ /****************************************************************************************/ int AX12_getLocalID(unsigned char id) { int i=0; for(i=0;i<lastAX12Use;i++) { if(AX12_data[i].id == id) return i; } //Si l'AX12 n'est pas déjà initialisé, on l'initialise AX12_data[lastAX12Use].id = id;// AX12_data[lastAX12Use].goal = 0;// AX12_data[lastAX12Use].speed = 0x320;// AX12_data[lastAX12Use].isUsingCAN = 1;//Indique qu'il faut envoyer le message via CAN AX12_data[lastAX12Use].needToUpdate = 0;// lastAX12Use++; return lastAX12Use-1; } /****************************************************************************************/ /* FUNCTION NAME: AX12_notifyCANEnd */ /* DESCRIPTION : indiquer qu'un mouvement d'AX12 CAN est terminé */ /****************************************************************************************/ void AX12_notifyCANEnd(unsigned char id) { if(waitingAckFrom == SERVO_AX12_DONE) { waitingAckFrom = 0; waitingAckID = 0; } } /****************************************************************************************/ /* FUNCTION NAME: AX12_doLoop */ /* DESCRIPTION : Boucle de vérification de la position des AX12 */ /****************************************************************************************/ void AX12_doLoop(void) { int i=0; CANMessage msgTx=CANMessage(); for(i=0;i<lastAX12Use;i++) { if(AX12_data[i].isUsingCAN == 0 && AX12_data[i].needCheckMoving == 1)//Il faut vérifier si l'AX12 a terminé de bouger { if(AX12_isMoving(i) == 0) {//L'AX12 a terminé de bouger AX12_data[i].needCheckMoving = 0; msgTx.id=SERVO_AX12_DONE; msgTx.len=1; msgTx.format=CANStandard; msgTx.type=CANData; // id de l'AX12 sur 1 octet msgTx.data[0]=(unsigned char)AX12_data[i].id; can1.write(msgTx); AX12_notifyCANEnd(AX12_data[i].id); } } } } /****************************************************************************************/ /* FUNCTION NAME: AX12_processChange */ /* DESCRIPTION : Permet de prendre en compte les changement d'instruction des AX12 */ /* Début du mouvement à partir de l'appel de cette fonction */ /****************************************************************************************/ void AX12_processChange(unsigned char fromCan) { int i=0; int dataToSendLength = 0; char dataToSend[100]; for(i=0;i<lastAX12Use;i++) { //printf("checking AX12 id: %d",AX12_data[i].id); if(AX12_data[i].needToUpdate == 1) //Il faut mettre à jour la position de l'AX12 { //printf(" => update"); if(AX12_data[i].isUsingCAN == 0)//Il faut envoyer la trame en local { //printf(" local"); if(dataToSendLength == 0) dataToSend[dataToSendLength++] = 4;//length data dataToSend[dataToSendLength++] = AX12_data[i].id;//ID servo1 dataToSend[dataToSendLength++] = ((1023 * AX12_data[i].goal) / 300) & 0xff;// bottom 8 bits dataToSend[dataToSendLength++] = ((1023 * AX12_data[i].goal) / 300) >> 8; // top 8 bits dataToSend[dataToSendLength++] = (AX12_data[i].speed) & 0xff;// bottom 8 bits dataToSend[dataToSendLength++] = (AX12_data[i].speed) >> 8; // top 8 bits AX12_data[i].needCheckMoving = 1; } AX12_data[i].needToUpdate = 0;//Remise à 0 de l'indicatif de mise à jour } //printf("\n"); } if(fromCan == 0) SendRawId(SERVO_AX12_PROCESS);//On indique par CAN qu'il faut bouger les AX12 //printf("need to send %d data\n",dataToSendLength); if(dataToSendLength > 0)//Il y a des données à envoyer en local { AX12_syncWrite(AX12_Serial1, AX12_REG_GOAL_POSITION, dataToSendLength, dataToSend); //wait_ms(10); AX12_syncWrite(AX12_Serial2, AX12_REG_GOAL_POSITION, dataToSendLength, dataToSend); } } /****************************************************************************************/ /* FUNCTION NAME: AX12_isMoving */ /* DESCRIPTION : Fonction pour savoir si un AX12 local est entrain de bouger */ /****************************************************************************************/ int AX12_isMoving(unsigned char id) { char data[1]; if(AX12_data[id].serial == 0) AX12_read(AX12_Serial1,AX12_data[id].id,AX12_REG_MOVING,1,data); else AX12_read(AX12_Serial2,AX12_data[id].id,AX12_REG_MOVING,1,data); return(data[0]); } /****************************************************************************************/ /* FUNCTION NAME: AX12_syncWrite */ /* DESCRIPTION : Fonction pour envoyer des trames aux AX12 en mode syncWrite */ /****************************************************************************************/ int AX12_syncWrite(SerialHalfDuplex& AX12_Serial, int start, int bytes, char* data) { //0 : 0xff //1 : 0xff //2 : ID de l'AX12 ou 0xFE pour le broadcast //3 : Length => longueur de la trame //4 : Intruction(write) => id de l'instruction 0x83 pour le syncwrite //5 : Address => addresse du registre à modifier //6+ : Data => les données à transmettre //last : Checksum int ID = 0xFE;//Toujours 0xFE dans le cas d'un broadcast char TxBuf[60]; char sum = 0; int timeout_transmit = 0; int i = 0; //printf("Start sending moving trame\n"); // Build the TxPacket first in RAM, then we'll send in one go TxBuf[0] = 0xff; TxBuf[1] = 0xff; // ID TxBuf[2] = ID; sum += TxBuf[2]; // packet Length TxBuf[3] = 3+bytes; sum += TxBuf[3]; // Instruction TxBuf[4]=0x83;//toujours 0x83 dans le cas d'un syncwrite sum += TxBuf[4]; // Start Address TxBuf[5] = start;//addresse du registre à modifier sum += TxBuf[5]; // data for (char i=0; i<bytes ; i++) { TxBuf[6+i] = data[i]; sum += TxBuf[6+i]; //printf(" Data : 0x%x\n",TxBuf[6+i]); } // checksum TxBuf[6+bytes] = 0xFF - sum; /* Transmission de la trame construite precedemment dans le tableau TxBuf */ while ((timeout_transmit<100) && (i < (7+bytes))) { if (AX12_Serial.writeable()) { AX12_Serial.putc(TxBuf[i]); i++; timeout_transmit = 0; } else timeout_transmit++; } if (timeout_transmit == 100 ) { // dans le cas d'une sortie en timeout pour ne pas rester bloquer ! return(-1); } // Wait for data to transmit wait (0.005); return(0);//OK trame envoyé } /****************************************************************************************/ /* FUNCTION NAME: AX12_write */ /* DESCRIPTION : Fonction pour envoyer des trames aux AX12 */ /****************************************************************************************/ int AX12_write(SerialHalfDuplex& AX12_Serial, int ID, int start, int bytes, char* data, int flag) { // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum // = AX12_Serial1; char TxBuf[16]; char sum = 0; char Status[6]; int timeout = 0; int plen = 0; int flag_out = 0; int timeout_transmit = 0; int i = 0; /*int poubelle = 0; int count = 0; char vidage[50];*/ typedef enum {Header1, Header2, ident, length, erreur, checksum} type_etat; type_etat etat = Header1; // Build the TxPacket first in RAM, then we'll send in one go TxBuf[0] = 0xff; TxBuf[1] = 0xff; // ID TxBuf[2] = ID; sum += TxBuf[2]; // packet Length TxBuf[3] = 3+bytes; sum += TxBuf[3]; // Instruction if (flag == 1) { TxBuf[4]=0x04; sum += TxBuf[4]; } else { TxBuf[4]=0x03; sum += TxBuf[4]; } // Start Address TxBuf[5] = start; sum += TxBuf[5]; // data for (char i=0; i<bytes ; i++) { TxBuf[6+i] = data[i]; sum += TxBuf[6+i]; } // checksum TxBuf[6+bytes] = 0xFF - sum; /* Transmission de la trame construite precedemment dans le tableau TxBuf */ while ((timeout_transmit<100) && (i < (7+bytes))) { if (AX12_Serial.writeable()) { AX12_Serial.putc(TxBuf[i]); i++; timeout_transmit = 0; } else timeout_transmit++; } if (timeout_transmit == 100 ) { // dans le cas d'une sortie en timeout pour ne pas rester bloquer ! return(-1); } /* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le servomoteur */ // Wait for data to transmit wait (0.005); // make sure we have a valid return Status[4]=0x00; // we'll only get a reply if it was not broadcast if (ID!=0xFE) { /* Partie de reception de la trame de retour */ while ((flag_out != 1) && (timeout < MAX_TIMEOUT)) { // Les differents etats de l'automate on été créés au debut de la fonction write ! switch (etat) { case Header1: if (AX12_Serial.readable()) { // reception du premier Header ( 0xFF ) Status[plen] = AX12_Serial.getc(); timeout = 0; if (Status[plen] == 0xFF ) { etat = Header2; plen++; } else etat = Header1; } else timeout++; break; case Header2: if (AX12_Serial.readable()) { // reception du second Header ( 0xFF ) Status[plen] = AX12_Serial.getc(); timeout = 0; if (Status[plen] == 0xFF ) { etat = ident; plen++; } else { etat = Header1; plen = 0; } } else timeout++; break; case ident: if (AX12_Serial.readable()) { // reception de l'octet correspondant à l'ID du servomoteur Status[plen] = AX12_Serial.getc(); timeout = 0; if (Status[plen] == ID ) { etat = length; plen++; } else { etat = Header1; plen = 0; } } else timeout++; break; case length: if (AX12_Serial.readable()) { // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres ) Status[plen] = AX12_Serial.getc(); timeout = 0; 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!! etat = erreur; plen++; } else { etat = Header1; plen = 0; } } else timeout++; break; case erreur: if (AX12_Serial.readable()) { //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur ) Status[plen] = AX12_Serial.getc(); timeout = 0; plen++; etat = checksum; } else timeout++; case checksum: if (AX12_Serial.readable()) { // recpetion du dernier octet ( Checksum ) >>> checksum = NOT ( ID + length ) >>>> dans le cas de la reception d'un write Status[plen] = AX12_Serial.getc(); timeout = 0; flag_out = 1; etat = Header1; } else timeout++; break; } } 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 return(-1); } // Build the TxPacket first in RAM, then we'll send in one go } return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour ) } /****************************************************************************************/ /* FUNCTION NAME: AX12_read */ /* DESCRIPTION : Lire des données dans un registre de l'AX12 */ /****************************************************************************************/ int AX12_read(SerialHalfDuplex& AX12_Serial, int ID, int start, int bytes, char* data) { //SerialHalfDuplex AX12_Serial = AX12_Serial1; char PacketLength = 0x3; char TxBuf[16]; char sum = 0; char Status[16]; int timeout = 0; int plen = 0; int flag_out = 0; int timeout_transmit = 0; int i = 0; int enable = 0; // int poubelle = 0; // int count = 0; // char vidage[50]; typedef enum {Header1, Header2, ident, length, erreur, reception, checksum} type_etat; type_etat etat = Header1; Status[4] = 0xFE; // return code /*********************************** CREATION DE LA TRAME A EVOYER *****************************************/ // Build the TxPacket first in RAM, then we'll send in one go TxBuf[0] = 0xff; TxBuf[1] = 0xff; // ID TxBuf[2] = ID; sum += TxBuf[2]; // Packet Length TxBuf[3] = PacketLength+bytes; // Length = 4 ; 2 + 1 (start) = 1 (bytes) sum += TxBuf[3]; // Accululate the packet sum // Instruction - Read TxBuf[4] = 0x2; sum += TxBuf[4]; // Start Address TxBuf[5] = start; sum += TxBuf[5]; // Bytes to read TxBuf[6] = bytes; sum += TxBuf[6]; // Checksum TxBuf[7] = 0xFF - sum; /********************************************TRAME CONSTRUITE DANS TxBuf***************************************/ /* Transmission de la trame construite precedemment dans le tableau TxBuf */ while ((timeout_transmit<1000) && (i < (7+bytes))) { if (AX12_Serial.writeable()) { AX12_Serial.putc(TxBuf[i]); i++; timeout_transmit = 0; } else timeout_transmit++; } if (timeout_transmit == 1000 ) { // dans le cas d'une sortie en timeout pour ne pas rester bloquer ! return(-1); } /* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le servomoteur */ // Wait for the bytes to be transmitted wait (0.001); // Skip if the read was to the broadcast address if (ID != 0xFE) { /* Partie de reception de la trame de retour */ while ((flag_out != 1) && (timeout < (1000*bytes))) { // Les differents etats de l'automate on été créés au debut de la fonction write ! switch (etat) { case Header1: if (AX12_Serial.readable()) { // reception du premier Header ( 0xFF ) Status[plen] = AX12_Serial.getc(); timeout = 0; if (Status[plen] == 0xFF ) { etat = Header2; plen++; } else etat = Header1; } else timeout++; break; case Header2: if (AX12_Serial.readable()) { // reception du second Header ( 0xFF ) Status[plen] = AX12_Serial.getc(); timeout = 0; if (Status[plen] == 0xFF ) { etat = ident; plen++; } else if (Status[plen] == ID ) { // PERMET D'EVITER CERTAINES ERREUR LORSQU'ON LIT PLUSIEURS REGISTRES !!!! Status[plen] = 0; plen++; Status[plen] = ID; etat = length; plen++; } else { etat = Header1; plen = 0; } } else timeout++; break; case ident: if (AX12_Serial.readable()) { // reception de l'octet correspondant à l'ID du servomoteur Status[plen] = AX12_Serial.getc(); timeout = 0; if (Status[plen] == ID ) { etat = length; plen++; } else { etat = Header1; plen = 0; } } else timeout++; break; case length: if (AX12_Serial.readable()) { // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres ) Status[plen] = AX12_Serial.getc(); timeout = 0; if (Status[plen] == (bytes+2) ) { etat = erreur; plen++; } else { etat = Header1; plen = 0; } } else timeout++; break; case erreur: if (AX12_Serial.readable()) { //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur ) Status[plen] = AX12_Serial.getc(); timeout = 0; plen++; etat = reception; } else timeout++; case reception: while ( enable < bytes ) { // reception du ou des octect(s) de donnés ( suivant la valeur de la variable length ) if (AX12_Serial.readable()) { Status[plen] = AX12_Serial.getc(); timeout = 0; plen++; enable++; } else timeout++; } etat = checksum; break; case checksum: 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!! Status[plen] = AX12_Serial.getc(); timeout = 0; flag_out = 1; etat = Header1; } else timeout++; break; default: break; } } 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 return(-1); } // copie des données dans le tableau data for (int i=0; i < Status[3]-2 ; i++) { data[i] = Status[5+i]; } } // toute la partie precedente ne s'effectue pas dans le cas d'un appel avec un broadcast ID (ID!=0xFE) return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour ) }