Diff: AX12-V2/AX12-V2.cpp

Revision:

0:ad97421fb1fb

Child:

1:116040d14164

diff -r 000000000000 -r ad97421fb1fb AX12-V2/AX12-V2.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AX12-V2/AX12-V2.cpp	Wed Apr 13 22:04:54 2016 +0000
@@ -0,0 +1,399 @@
+#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_Serial = SerialHalfDuplex(p9,p10);
+
+int lastAX12Use = 0;
+
+/****************************************************************************************/
+/* FUNCTION NAME: AX12_register                                                         */
+/* DESCRIPTION  : Indiquer qu'un AX12 est connecté à la carte                           */
+/****************************************************************************************/
+void AX12_register(unsigned char id, unsigned short speed)
+{
+    int localID = AX12_getLocalID(id);
+    char data[2];
+    
+    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[lastAX12Use].speed = speed;
+    
+    data[0] = speed & 0xff; // bottom 8 bits
+    data[1] = speed >> 8;   // top 8 bits
+    
+    AX12_Serial.baud(1000000);//On indique la vitesse de transmission des AX12
+    
+    AX12_write(id,AX12_REG_MOVING_SPEED,2,data);
+    
+    
+}
+
+/****************************************************************************************/
+/* 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
+    
+    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=3;
+        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)speed;
+        msgTx.data[4]=(unsigned char)(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<MAX_AX12;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_doLoop                                                           */
+/* DESCRIPTION  : Boucle de vérification de la position des AX12                        */
+/****************************************************************************************/
+void AX12_doLoop(void)
+{
+    //TODO
+}
+
+/****************************************************************************************/
+/* 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(void)
+{
+    int i=0;
+    int dataToSendLength = 0;
+    char dataToSend[50];
+
+    
+    for(i=0;i<MAX_AX12;i++)
+    {
+        if(AX12_data[i].needToUpdate == 1) //Il faut mettre à jour la position de l'AX12
+        {
+            if(AX12_data[i].isUsingCAN == 0)//Il faut envoyer la trame en local
+            {
+                if(dataToSendLength == 0)
+                    dataToSend[dataToSendLength++] = 2;//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                
+            }
+            AX12_data[i].needToUpdate = 0;//Remise à 0 de l'indicatif de mise à jour
+        }
+    } 
+    SendRawId(SERVO_AX12_PROCESS);//On indique par CAN qu'il faut bouger les AX12
+
+    if(dataToSendLength > 0)//Il y a des données à envoyer en local
+    {
+        AX12_syncWrite(AX12_REG_GOAL_POSITION, dataToSendLength, dataToSend);
+    }
+    
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: AX12_syncWrite                                                        */
+/* DESCRIPTION  : Fonction pour envoyer des trames aux AX12 en mode syncWrite           */
+/****************************************************************************************/
+int AX12_syncWrite(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(int ID, int start, int bytes, char* data, int flag)
+{
+// 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
+
+    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 )
+}