CRAC Team
pour trouver l'automate de reception can
Fork of
CRAC-Strat_copy
by 
Clement Breteau
Diff: AX12-V2/AX12-V2.cpp
- Revision:
- 15:c2fc239e85df
- Parent:
- 14:c8fc06c4887f
--- a/AX12-V2/AX12-V2.cpp Fri Mar 31 16:20:26 2017 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,672 +0,0 @@
-#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 && waitingAckID == id) {
- 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];
- int sendTwice = 0;
-
- 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
- {
- for(sendTwice=0;sendTwice<2;sendTwice++)
- {
- AX12_syncWrite(AX12_Serial1, AX12_REG_GOAL_POSITION, dataToSendLength, dataToSend);
- //wait_ms(10);
- AX12_syncWrite(AX12_Serial2, AX12_REG_GOAL_POSITION, dataToSendLength, dataToSend);
- }
- }
- SendRawId(0x456);
-}
-
-/****************************************************************************************/
-/* 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 )
-}
-
-