File content as of revision 0:ad97421fb1fb:

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