File content as of revision 14:c8fc06c4887f:

#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 )
}