File content as of revision 1:ac14e1422ab3:

/* mbed AX-12+ Servo Library
 *
 * Copyright (c) 2010, cstyles (http://mbed.org)
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include "mbed.h"
#include "AX12.h"

#define MAX_TIMEOUT 500

extern Timer t;

typedef struct 
{
    unsigned short Model_Number;
    unsigned char Firmware;
    unsigned char ID;
    unsigned char Baud_Rate;
    unsigned char Return_Delay_Time;
    unsigned short CW_Angle_Limit;
    unsigned short CCW_Angle_Limit;
    unsigned char Reserved1;
    unsigned char Highest_Limit_Temperature;
    unsigned char Lowest_Limit_voltage;
    unsigned char Highest_Limit_voltage;
    unsigned short Max_Torque;
    unsigned char Status_Return_Level;
    unsigned char Alarm_LED;
    unsigned char Alarm_Shutdown;
    unsigned char Reserved2;
    unsigned short Down_Calibration;
    unsigned short Up_Calibration;
    unsigned char Torque_Enable;
    unsigned char LED;
    unsigned char CW_Compliance_Margin;
    unsigned char CCW_Compliance_Margin;
    unsigned char CW_Compliance_Slope;
    unsigned char CCW_Compliance_Slope;
    unsigned short Goal_Position;
    unsigned short Moving_Speed;
    unsigned short Torque_Limit;
    unsigned short Present_Position;
    unsigned short Present_Speed;
    unsigned short Present_Load;
    unsigned char Present_Voltage;
    unsigned char Present_Temperature;
    unsigned char Registered_Instruction;
    unsigned char Reserved3;
    unsigned char Moving;    
    unsigned char Lock;
    unsigned short Punch;
} T_AX12;


AX12::AX12(PinName tx, PinName rx, int ID, int baud)
    : _ax12(tx,rx)
{
    _baud = baud;
    _ID = ID;
    _ax12.baud(_baud);
    

}

int AX12::Set_Secure_Goal(int degres)
{
    int error = 0;
    int position = 0;
    int difference = 0;
    int timeout_secure = 0;
    int autorisation = 0;
    
    position = Get_Position();
    error = Set_Goal(degres);
    
    while ((autorisation == 0) && (timeout_secure < 100) ) 
    {
        position = Get_Position();
        //printf("position : %d", position );
        error = Set_Goal(degres);
        //printf("degres : %d", degres);
        difference = degres - position;
        //printf ("difference : %d", difference );
        if (((difference < 2) && (difference > (-2) )))
        autorisation = 1;
        
        timeout_secure++;
    }
    
    if ( timeout_secure == 100)
    {
        #ifdef AX12_DEBUG
        printf (" timeout secure error ");
        #endif
        return(-1);    
    }
    return(error);
}


int AX12::Get_Return_Delay_Time(void)
{
    char data[1];
    int ErrorCode = read(_ID, AX12_REG_DELAY_TIME, 1, data);
    int time = data[0];
    time = 2.0 * time;
    return(time);
}


int AX12::Get_Baud_Rate(void)
{
    char data[1];
    int ErrorCode = read(_ID, AX12_REG_BAUD, 1, data);
    int baud = data[0];
    baud = 2000000 / ( baud + 1 );
    return(baud);
}


/** Reglage du courant minimum necessaire au bon fonctionnement de l'actionneur 
//  minimum >>  Ox000    >>  decimal  0
//  maximum >>  0x3FF    >>  decimal  1023
//  deflaut >>  0x20     >>  decimal  32
*/
int AX12::Set_Punch(int punch)
{
    char data[2];

    data[0] = punch & 0xff; // bottom 8 bits
    data[1] = punch >> 8;   // top 8 bits

    // write the packet, return the error code
    return (write(_ID, AX12_REG_PUNCH, 2, data));

}

/** Reset
*/
int AX12::Reset(int punch)
{
    char data[2];

    data[0] = punch & 0xff; // bottom 8 bits
    data[1] = punch >> 8;   // top 8 bits

    // write the packet, return the error code
    return (write(_ID, 0x06, 1,data));

}

int AX12::Get_Punch (void)
{
    char data[2];
    int ErrorCode = read(_ID, AX12_REG_PUNCH, 2, data);
    int punch = data[0] | (data[1]<<8);
    return(punch);
}


int AX12::Get_Load_Direction (void)
{
    char data[2];
    int ErrorCode = read(_ID, AX12_REG_PRESENT_LOAD, 2, data);
    int direction = (data[1]>>2) & 0x01;
    return(direction);
}


int AX12::Get_Load_Value (void)
{
    char data[2];
    int ErrorCode = read(_ID, AX12_REG_PRESENT_LOAD, 2, data);
    int Load = (data[0] | (data[1]<<8)) & 0x3FF;
    return(Load);
}


int AX12::Get_Present_Speed (void)
{
    char data[2];
    int ErrorCode = read(_ID, AX12_REG_PRESENT_SPEED, 2, data);
    int speed = data[0] | (data[1]<<8);
    return(speed);
}


int AX12::Get_CCW_Angle_Limit (void)
{
    char data[2];
    int ErrorCode = read(_ID, AX12_REG_CCW_LIMIT, 2, data);
    int angle = data[0] | (data[1]<<8);
    angle = (angle * 300) / 1023;
    return(angle);
}


int AX12::Get_CW_Angle_Limit (void)
{
    char data[2];
    int ErrorCode = read(_ID, AX12_REG_CW_LIMIT, 2, data);
    int angle = data[0] | (data[1]<<8);
    angle = (angle * 300) / 1023;
    return(angle);
}



int AX12::Get_Torque_Enable(void)
{
    char data[1];
    int ErrorCode = read(_ID, AX12_REG_TORQUE_ENABLE, 1, data);
    int enable = data[0];
    return(enable);
}


int AX12::Set_Torque_Enable(int etat) 
{
    char data[1];
    data [0] = etat;
    
    int error = write(_ID, AX12_REG_TORQUE_ENABLE, 1, data);
    return (error);
}



int AX12::Get_Up_Calibration (void)
{
    char data[1];
    int ErrorCode = read(_ID, AX12_REG_UP_CALIBRATION, 2, data);
    int Up_calibration = data[0] | (data[1]<<8);
    return(Up_calibration);
}



int AX12::Get_Down_Calibration (void)
{
    char data[1];
    int ErrorCode = read(_ID, AX12_REG_DOWN_CALIBRATION, 2, data);
    int Dowm_calibration = data[0] | (data[1]<<8);
    return(Dowm_calibration);
}



int AX12::Get_ID(void)
{

    char data[1]={-1};
    int ErrorCode = read(_ID, AX12_REG_ID, 1, data);
    int id = data[0];
    return(id);
}


// Lecture du couple maximum ( retourne la valeur du registre Max Torque de l'AX12 ) 
int AX12::Get_Max_Torque (void)
{
    char data[2];
    int ErrorCode = read(_ID, AX12_REG_MAX_TORQUE, 2, data);
    int torque = data[0] | (data[1]<<8);
    return(torque);
}



/** Reglage du couple maximum de l'actionneur 
//  minimum >>  Ox000    >>  decimal  0
//  maximum >>  0x3FF    >>  decimal  1023
//  deflaut >>           >>  decimal  
*/
int AX12::Set_Max_Torque(int torque)
{
    char data[2];

    data[0] = torque & 0xff; // bottom 8 bits
    data[1] = torque >> 8;   // top 8 bits

    // write the packet, return the error code
    return (write(_ID, AX12_REG_MAX_TORQUE, 2, data));

}




/** Reglage de la desactivation des actionneurs si une erreur le concernant se produit 
Bit                 Function
Bit 7               0
Bit 6               If set to 1, torque off when an Instruction Error occurs 
Bit 5               If set to 1, torque off when an Overload Error occurs 
Bit 4               If set to 1, torque off when a Checksum Error occurs 
Bit 3               If set to 1, torque off when a Range Error occurs 
Bit 2               If set to 1, torque off when an Overheating Error occurs 
Bit 1               If set to 1, torque off when an Angle Limit Error occurs 
Bit 0               If set to 1, torque off when an Input Voltage Error occurs
*/
int AX12::Set_Alarm_Shutdown(int valeur) 
{
    char data[1];
    data [0] = valeur;
    
    int val_alarm_shutdown = write(_ID, AX12_REG_ALARM_SHUTDOWN, 1, data);
    return (val_alarm_shutdown);
}



/** Reglage de l'activation de l'alarme
Bit                 Function
Bit 7               0
Bit 6               If set to 1, the LED blinks when an Instruction Error occurs
Bit 5               If set to 1, the LED blinks when an Overload Error occurs
Bit 4               If set to 1, the LED blinks when a Checksum Error occurs
Bit 3               If set to 1, the LED blinks when a Range Error occurs
Bit 2               If set to 1, the LED blinks when an Overheating Error occurs
Bit 1               If set to 1, the LED blinks when an Angle Limit Error occurs
Bit 0               If set to 1, the LED blinks when an Input Voltage Error occurs
*/
int AX12::Set_Alarm_LED(int valeur) 
{
    char data[1];
    data [0] = valeur;
    
    int val_alarmLED = write(_ID, AX12_REG_ALARM_LED, 1, data);
    return (val_alarmLED);
}




// Reglage de la réponse à une instruction  
//  0   >>  ne repond a aucune instructions
//  1   >>  repond seulement aux instructions READ_DATA 
//  2   >>  repond à toutes les instructions 
int AX12::Set_Status_Return_Level(int etat) 
{
    char data[1];
    data [0] = etat;
    
    int val_return_lvl = write(_ID, AX12_REG_SATUS_RETURN, 1, data);
    return (val_return_lvl);
}


// Reglage de la tension minimale 
//  minimum >>  Ox32    >>  decimal  50
//  maximum >>  0xFA    >>  decimal  250
//  deflaut >>  0x3C    >>  decimal  60
int AX12::Set_Lowest_Voltage(int val_lowest_voltage) 
{
    char data[1];
    data [0] = val_lowest_voltage;
    
    int val_lowvolt = write(_ID, AX12_REG_LOWEST_VOLTAGE, 1, data);
    return (val_lowvolt);
}


// Reglage de la tension maximale
//  minimum >>  Ox32    >>  decimal  50
//  maximum >>  0xFA    >>  decimal  250
//  deflaut >>  0xBE    >>  decimal  190
int AX12::Set_Highest_Voltage(int val_highest_voltage) 
{
    char data[1];
    data [0] = val_highest_voltage;
    
    int val_highvolt = write(_ID, AX12_REG_HIGHEST_VOLTAGE, 1, data);
    return (val_highvolt);
}


// Reglage du return time delay  EN MICRO SECONDE              2uSec * val_delay_time
//  minimum >>  0       us 
//  maximum >>  508     us
//  deflaut >>  125     us
int AX12::Set_Delay_Time (int val_delay_time )
 {
    char data[1];
    data [0] = val_delay_time/2.0;
    
    int valdelay_time = write(_ID, AX12_REG_DELAY_TIME, 1, data);          
    return (valdelay_time );
 }


// Reglage de la température max du cervo 
//  minimum >>  Ox00    >>  decimal  0
//  maximum >>  0x96    >>  decimal  150
int AX12::Set_Temperature_Max (int val_temperature )
 {
    char data[1];
    data [0] = val_temperature;
    
    int valtemp_max = write(_ID, AX12_REG_TEMP_MAX, 1, data);          
    return (valtemp_max );
 }

// Etat LED 
//  0 = off
//  1 = on 
int AX12::Set_Etat_LED(int etat) 
{
    char data[1];
    data [0] = etat;
    
    int valLED = write(_ID, AX12_REG_LED, 1, data);
    return (valLED);
}

// Set the mode of the servo
//  0 = Positional (0-300 degrees)
//  1 = Rotational -1 to 1 speed
int AX12::Set_Mode(int mode)
{

    if (mode == 1) { // set CR
        //wait(0.001);
        Set_CW_Angle_Limit(0);
        //wait(0.001);
        Set_CCW_Angle_Limit(0);
        //wait(0.001);
        Set_CR_Speed(0.0);
        //wait(0.001);
    } else {
        //wait(0.001);
        Set_CW_Angle_Limit(0);
        //wait(0.001);
        Set_CCW_Angle_Limit(300);
        //wait(0.001);
        Set_CR_Speed(0.0);
        //wait(0.001);
    }
    return(0);
}

int AX12::Set_Goal_speed(int speed, int flags)
{

     char reg_flag = 0;
    char data[2];

    // set the flag is only the register bit is set in the flag
    if (flags == 0x2) {
        reg_flag = 1;
    }

    data[0] = speed & 0xff; // bottom 8 bits
    data[1] = speed >> 8;   // top 8 bits

    // write the packet, return the error code
    int rVal = write(_ID, AX12_REG_MOVING_SPEED, 2, data, reg_flag);

    /*if (flags == 1) {
        // block until it comes to a halt
        while (isMoving()) 
            {
            }
    
    }*/
    return(rVal);
}


// if flag[0] is set, were blocking
// if flag[1] is set, we're registering
// they are mutually exclusive operations
int AX12::Set_Goal(int degrees, int flags)
{

    char reg_flag = 0;
    char data[2];

    // set the flag is only the register bit is set in the flag
    if (flags == 0x2) {
        reg_flag = 1;
    }

    // 1023 / 300 * degrees
    short goal = (1023 * degrees) / 300;
#ifdef AX12_DEBUG
    printf("SetGoal to 0x%x\n",goal);
#endif

    data[0] = goal & 0xff; // bottom 8 bits
    data[1] = goal >> 8;   // top 8 bits

    // write the packet, return the error code
    int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data, reg_flag);

    /*if (flags == 1) {
        // block until it comes to a halt
        while (isMoving()) 
            {
            }
    
    }*/
    return(rVal);
}


// Set continuous rotation speed from -1 to 1
int AX12::Set_CR_Speed(float speed)
{

    // bit 10     = direction, 0 = CCW, 1=CW
    // bits 9-0   = Speed
    char data[2];

    int goal = (0x3ff * abs(speed));

    // Set direction CW if we have a negative speed
    if (speed < 0) {
        goal |= (0x1 << 10);
    }

    data[0] = goal & 0xff; // bottom 8 bits
    data[1] = goal >> 8;   // top 8 bits

    // write the packet, return the error code
    int rVal = write(_ID, 0x20, 2, data);

    return(rVal);
}


int AX12::Set_CW_Angle_Limit (int degrees)
{

    char data[2];

    // 1023 / 300 * degrees
    short limit = (1023 * degrees) / 300;

#ifdef AX12_DEBUG
    printf("SetCWLimit to 0x%x\n",limit);
#endif

    data[0] = limit & 0xff; // bottom 8 bits
    data[1] = limit >> 8;   // top 8 bits

    // write the packet, return the error code
    return (write(_ID, AX12_REG_CW_LIMIT, 2, data));

}

int AX12::Set_CCW_Angle_Limit (int degrees)
{

    char data[2];

    // 1023 / 300 * degrees
    short limit = (1023 * degrees) / 300;

#ifdef AX12_DEBUG
    printf("SetCCWLimit to 0x%x\n",limit);
#endif

    data[0] = limit & 0xff; // bottom 8 bits
    data[1] = limit >> 8;   // top 8 bits

    // write the packet, return the error code
    return (write(_ID, AX12_REG_CCW_LIMIT, 2, data));
}


int AX12::Set_ID (int CurrentID, int NewID)
{

    char data[1];
    data[0] = NewID;

#ifdef AX12_DEBUG
    printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID);
#endif

    return (write(CurrentID, AX12_REG_ID, 1, data));

}


int AX12::Set_Baud (int baud)
{

    char data[1];
    data[0] = baud;

#ifdef AX12_DEBUG
    printf("Setting Baud rate to %d\n",baud);
#endif

    return (write(_ID, AX12_REG_BAUD, 1, data));

}



// return 1 is the servo is still in flight
int AX12::isMoving(void)
{

    char data[1];
    read(_ID,AX12_REG_MOVING,1,data);
    return(data[0]);
}

void AX12::reset()
{

    unsigned char TxBuf[16];
    unsigned char sum = 0;
    unsigned long debut=0;

#ifdef AX12_TRIGGER_DEBUG
    // Build the TxPacket first in RAM, then we'll send in one go
    printf("\nreset\n");
    printf("\nreset Packet\n  Header : 0xFF, 0xFF\n");
#endif

    TxBuf[0] = 0xFF;
    TxBuf[1] = 0xFF;

    // ID - Broadcast
    TxBuf[2] =_ID;
    sum += TxBuf[2];

#ifdef AX12_TRIGGER_DEBUG
    printf("  ID : %d\n",TxBuf[2]);
#endif

    // Length
    TxBuf[3] = 0x02;
    sum += TxBuf[3];

#ifdef AX12_TRIGGER_DEBUG
    printf("  Length %d\n",TxBuf[3]);
#endif

    // Instruction - ACTION
    TxBuf[4] = 0x06;  //reset
    sum += TxBuf[4];

#ifdef AX12_TRIGGER_DEBUG
    printf("  Instruction 0x%X\n",TxBuf[5]);
#endif

    // Checksum
    TxBuf[5] = 0xFF - sum;
//#ifdef AX12_TRIGGER_DEBUG
    printf("  Checksum 0x%X\n",TxBuf[5]);
//#endif

    // Transmit the packet in one burst with no pausing
    for (int i = 0; i < 6 ; i++) 
    {
        while(_ax12.writeable()==0); 
            _ax12.putc(TxBuf[i]);
            
    }
    wait(0.001);
    debut=t.read_ms();
    
        do
        {
            if (_ax12.readable()==-1)       // reception du premier Header ( 0xFF )
                printf("%02x",_ax12.getc());
        }
        while((t.read_ms()-debut)<500);
    
    printf("\n");
    return;
}

void AX12::read_all_info(unsigned char start, unsigned char longueur)
{

    unsigned char TxBuf[16];
    unsigned char sum = 0;
    unsigned long debut=0;

#ifdef AX12_TRIGGER_DEBUG
    // Build the TxPacket first in RAM, then we'll send in one go
    printf("\nreset\n");
    printf("\nreset Packet\n  Header : 0xFF, 0xFF\n");
#endif

    TxBuf[0] = 0xFF;
    TxBuf[1] = 0xFF;

    // ID - Broadcast
    TxBuf[2] =_ID;
    sum += TxBuf[2];

#ifdef AX12_TRIGGER_DEBUG
    printf("  ID : %d\n",TxBuf[2]);
#endif

    // Length
    TxBuf[3] = 0x04;
    sum += TxBuf[3];

#ifdef AX12_TRIGGER_DEBUG
    printf("  Length %d\n",TxBuf[3]);
#endif

    // Instruction - ACTION
    TxBuf[4] = INST_READ;  //reset
    sum += TxBuf[4];

#ifdef AX12_TRIGGER_DEBUG
    printf("  Instruction 0x%X\n",TxBuf[4]);
#endif

    TxBuf[5] = start;  //reset
    sum += TxBuf[5];

    TxBuf[6] = longueur;  //reset
    sum += TxBuf[6];


    // Checksum
    TxBuf[7] = 0xFF - sum;
//#ifdef AX12_TRIGGER_DEBUG
    //printf("  Checksum 0x%X\n\r",TxBuf[7]);
//#endif

    // Transmit the packet in one burst with no pausing
    for (int i = 0; i < 8 ; i++) 
    {
        while(_ax12.writeable()==0); 
            _ax12.putc(TxBuf[i]);
            
    }
    
    debut=t.read_ms();
    int i=0;
        do
        {
            if (_ax12.readable())
            {       // reception du premier Header ( 0xFF )
                printf("%02d:%02x ",start+i,_ax12.getc());
                i++;
            }
        }
        while((t.read_ms()-debut)<5000);
    
    printf("\n");
    return;
}


void AX12::trigger(void)
{

    char TxBuf[16];
    char sum = 0;

#ifdef AX12_TRIGGER_DEBUG
    // Build the TxPacket first in RAM, then we'll send in one go
    printf("\nTriggered\n");
    printf("\nTrigger Packet\n  Header : 0xFF, 0xFF\n");
#endif

    TxBuf[0] = 0xFF;
    TxBuf[1] = 0xFF;

    // ID - Broadcast
    TxBuf[2] = 0xFE;
    sum += TxBuf[2];

#ifdef AX12_TRIGGER_DEBUG
    printf("  ID : %d\n",TxBuf[2]);
#endif

    // Length
    TxBuf[3] = 0x02;
    sum += TxBuf[3];

#ifdef AX12_TRIGGER_DEBUG
    printf("  Length %d\n",TxBuf[3]);
#endif

    // Instruction - ACTION
    TxBuf[4] = 0x04;
    sum += TxBuf[4];

#ifdef AX12_TRIGGER_DEBUG
    printf("  Instruction 0x%X\n",TxBuf[5]);
#endif

    // Checksum
    TxBuf[5] = 0xFF - sum;
#ifdef AX12_TRIGGER_DEBUG
    printf("  Checksum 0x%X\n",TxBuf[5]);
#endif

    // Transmit the packet in one burst with no pausing
    for (int i = 0; i < 6 ; i++) {
        _ax12.putc(TxBuf[i]);
    }

    // This is a broadcast packet, so there will be no reply
    return;
}


float AX12::Get_Position(void)
{

    #ifdef AX12_DEBUG
    printf("\nGetPositionID(%d)",_ID);
    #endif

    char data[2];

    int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data);
    int position = data[0] | (data[1] << 8);
    float angle = ((float)position * 300.0)/1023.0;

    return (angle);
}


float AX12::Get_Temp ()
{

#ifdef AX12_DEBUG
    printf("\nGetTemp(%d)",_ID);
#endif

    char data[1];
    int ErrorCode = read(_ID, AX12_REG_TEMP, 1, data);
    float temp = data[0];
    return(temp);
}


float AX12::Get_Volts (void)
{

#ifdef AX12_DEBUG
    printf("\nGetVolts(%d)",_ID);
#endif

    char data[1];
    int ErrorCode = read(_ID, AX12_REG_VOLTS, 1, data);
    float volts = data[0]/10.0;
    return(volts);
}


int AX12::read(int ID, int start, int bytes, char* data) {


    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 *****************************************/
 
#ifdef AX12_READ_DEBUG
    printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
#endif
 
    // Build the TxPacket first in RAM, then we'll send in one go
#ifdef AX12_READ_DEBUG
    printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
#endif
 
    TxBuf[0] = 0xff;
    TxBuf[1] = 0xff;
 
    // ID
    TxBuf[2] = ID;
    sum += TxBuf[2];
 
#ifdef AX12_READ_DEBUG
    printf("  ID : %d\n",TxBuf[2]);
#endif
 
    // Packet Length
    TxBuf[3] = 4;//PacketLength+bytes;    // Length = 4 ; 2 + 1 (start) = 1 (bytes)
    sum += TxBuf[3];            // Accululate the packet sum
 
#ifdef AX12_READ_DEBUG
    printf("  Length : 0x%x\n",TxBuf[3]);
#endif
 
    // Instruction - Read
    TxBuf[4] = 0x2;
    sum += TxBuf[4];
 
#ifdef AX12_READ_DEBUG
    printf("  Instruction : 0x%x\n",TxBuf[4]);
#endif
 
    // Start Address
    TxBuf[5] = start;
    sum += TxBuf[5];
 
#ifdef AX12_READ_DEBUG
    printf("  Start Address : 0x%x\n",TxBuf[5]);
#endif
 
    // Bytes to read
    TxBuf[6] = bytes;
    sum += TxBuf[6];
 
#ifdef AX12_READ_DEBUG
    printf("  No bytes : 0x%x\n",TxBuf[6]);
#endif
 
    // Checksum
    TxBuf[7] = 0xFF - sum;
#ifdef AX12_READ_DEBUG
    printf("  Checksum : 0x%x\n",TxBuf[7]);
#endif
/********************************************TRAME CONSTRUITE DANS TxBuf***************************************/



    
    /* Transmission de la trame construite precedemment dans le tableau TxBuf 
    */
    while ((timeout_transmit<5000) && (i < (7+bytes)))
    {
        if (_ax12.writeable())  
        {
            _ax12.putc(TxBuf[i]);
            i++;
            timeout_transmit = 0;
        }
        else timeout_transmit++;
    }
    
    if (timeout_transmit == 5000 )   // dans le cas d'une sortie en timeout pour ne pas rester bloquer !
        {
            #ifdef AX12_DEBUG
            printf ("timeout transmit erreur\r\n");
            #endif
            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.readable())       // reception du premier Header ( 0xFF )
                        { 
                        Status[plen] = _ax12.getc();
                        timeout = 0;
                            if (Status[plen] == 0xFF )
                            {
                                etat = Header2;
                                #ifdef AX12_DEBUG_READ
                                    printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
                                #endif
                                plen++; 
                                
                            }
                            else etat = Header1;
                        }
                        else timeout++;
                        break;
                    
     
     case Header2:      if (_ax12.readable())       // reception du second Header ( 0xFF )
                        { 
                            Status[plen] = _ax12.getc();
                            timeout = 0;
                            if (Status[plen] == 0xFF )
                            {
                                etat = ident;
                                #ifdef AX12_DEBUG_READ
                                    printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
                                #endif
                                plen++; 
                                
                            }
                            else if (Status[plen] == ID )  // PERMET D'EVITER CERTAINES ERREUR LORSQU'ON LIT PLUSIEURS REGISTRES !!!!
                            {   
                                Status[plen] = 0;
                                #ifdef AX12_DEBUG_READ
                                    printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
                                #endif
                                plen++;
                                Status[plen] = ID;
                                etat = length;
                                #ifdef AX12_DEBUG_READ
                                    printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
                                #endif
                                plen++; 
                                
                            }
                            else 
                            {
                                
                                etat = Header1;
                                plen = 0;
                            }
                        }
                        else timeout++;
                        break;
     
     case ident:        if (_ax12.readable())       // reception de l'octet correspondant à l'ID du servomoteur
                        {
                            Status[plen] = _ax12.getc();
                            timeout = 0;
                            if (Status[plen] == ID )
                            {
                                etat = length;
                                #ifdef AX12_DEBUG_READ
                                    printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
                                #endif
                                plen++; 
                                
                            }
                            else 
                            {
                                etat = Header1;
                                plen = 0;
                            }
                        }
                        else timeout++;
                        break;
     
     case length:       if (_ax12.readable())          // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres )
                        {
                            Status[plen] = _ax12.getc();
                            timeout = 0;
                            if (Status[plen] == (bytes+2) )
                            {
                                etat = erreur;
                                #ifdef AX12_DEBUG_READ
                                    printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
                                #endif
                                plen++; 
                                
                            }
                            else 
                            {
                                etat = Header1;
                                plen = 0;
                            }
                        }
                        else timeout++;
                        break;
    
     case erreur:       if (_ax12.readable())       //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur ) 
                        {
                            Status[plen] = _ax12.getc();
                            timeout = 0; 
                            #ifdef AX12_DEBUG_READ
                                printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
                            #endif
                            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.readable())  
                            {
                                Status[plen] = _ax12.getc();
                                timeout = 0; 
                                #ifdef AX12_DEBUG_READ
                                    printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
                                #endif
                                plen++;
                                enable++;
                                
                            }
                            else timeout++;
                        }
                        etat = checksum;
                        break;
                        
     case checksum:     if (_ax12.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.getc();
                                timeout = 0;
                                flag_out = 1;
                                etat = Header1;
                                
                                #ifdef AX12_DEBUG_READ
                                    printf("data[%d]  : %d\r\n\n", plen, (int)Status[plen]);
                                #endif
                            } 
                        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
        {
            #ifdef AX12_DEBUG
            printf ("timeout erreur\n");
            #endif
            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 )
}

void AX12::multiple_goal_and_speed(int number_ax12,char* tab)
{
    char TxBuf[50];
    char sum = 0;      
    int timeout_transmit =0;
    int i=0, k=0, j=0;
    int L=4;                                                                // nombre instructions par paquets
    int bytes= ((L+1)*number_ax12)+4;
    
    typedef enum {Header1, Header2, ident, length, erreur, checksum} type_etat;
    type_etat etat= Header1;
    
    for(j=0; j<50; j++)
    {
      TxBuf[i]=0;  
    }
    
   #ifdef AX12_WRITE_DEBUG
    //printf(" MULTIPLE_GOAL_AND_SPEED \n ");
    #endif  

// Build the TxPacket first in RAM, then we'll send in one go
    #ifdef AX12_WRITE_DEBUG
    //printf("\nInstruction Packet\n  Header :%d, %d\n",TxBuf[0], TxBuf[1]);
    #endif
    
    TxBuf[0]=0xFF;                                                          // bit de start
    TxBuf[1]=0xFF;                                                          // bit de start
    
    TxBuf[2] = 0xFE;                                                        //ID broadcast
    sum += TxBuf[2];
    
    #ifdef AX12_WRITE_DEBUG
    printf("  adresse de difusion : %d\n",TxBuf[2]);
    #endif
    
    TxBuf[3] =bytes;                                                        // longueur
    sum += TxBuf[3];
    
    #ifdef AX12_WRITE_DEBUG
    printf("  Longueur : %d\n",TxBuf[3]);
    #endif
    
    TxBuf[4]=0x83;                                                          //SYNC_WRITE
    sum += TxBuf[4];
    
    #ifdef AX12_WRITE_DEBUG
    printf("  Instruction : 0x%x\n",TxBuf[4]);
    #endif
    
    TxBuf[5] = 0x1E;                                                        // addresse "GOAL_POSITION"
    sum += TxBuf[5];
    
    #ifdef AX12_WRITE_DEBUG
    printf(" Adresse de debut : 0x%x\n",TxBuf[5]);
    #endif
    
    TxBuf[6]=L;                                                             // Nombre instruction par paquets
    sum += TxBuf[6];
    
     #ifdef AX12_WRITE_DEBUG
    printf("  nombre instruction/paquet : 0x%x\n",TxBuf[6]);
    #endif
    
    for(i=0; i<(number_ax12*5); i++)                                            // Copie des data de TAB sur TxBuf
    {         
       
         TxBuf[i+7]=tab[i];
         sum += TxBuf[i+7];            
        
    }
    
    #ifdef AX12_WRITE_DEBUG
    for(i=0; i<(number_ax12*5); i++)
    {
        
        printf("  Data : 0x%x\n",TxBuf[i+7]);
        
    }  
    #endif
    
   TxBuf[(number_ax12*5)+7] = 0xFF - sum ;                                             // CHECKSUM
   
   #ifdef AX12_WRITE_DEBUG
   printf("  Checksum : 0x%x\n",TxBuf[(number_ax12*5)+9]);
   #endif
     
   for(k=0;  k<((number_ax12*5)+8); k++)                                               // TRANSMISSION DE LA TRAME
    {
            _ax12.putc(TxBuf[k]);          
            #ifdef AX12_WRITE_DEBUG
                 printf("  transmission : 0x%x\n",TxBuf[k]);
             #endif         
    }
    

}

float AX12::read_and_test(float angle,char* Tab)
{
    int k=0;
    unsigned short val_angle=0, val_reche=0;
     
     #ifdef AX12_DEBUG
      printf("\nread_and_test");
      #endif
      
    if( _ID==0x12)                                                              
    { k=1;}
    else if( _ID==0x04)                                                      
    { k=6;}
    else if( _ID==0x07)                                                       
    { k=11;}
    else if( _ID==0x0F)                                                     
    { k=16;}
    
    val_angle = (unsigned short) (angle/0.3);
    val_reche = (unsigned short) Tab[k] + ((unsigned short)Tab[k+1]<<8);
     
    if((val_angle < (val_reche+(28))) && (val_angle > (val_reche-(28))))
    {
      #ifdef AX12_DEBUG
      printf("\nreturn1");
      #endif
      return 1;  
    }
    else 
    {
      #ifdef AX12_DEBUG
      printf("\nreturn0");
      #endif
      return 0;
    }
    
}

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;

#ifdef AX12_WRITE_DEBUG
    printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag);
#endif

    // Build the TxPacket first in RAM, then we'll send in one go
#ifdef AX12_WRITE_DEBUG
    printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
#endif

    TxBuf[0] = 0xff;
    TxBuf[1] = 0xff;

    // ID
    TxBuf[2] = ID;
    sum += TxBuf[2];

#ifdef AX12_WRITE_DEBUG
    printf("  ID : %d\n",TxBuf[2]);
#endif

    // packet Length
    TxBuf[3] = 3+bytes;
    sum += TxBuf[3];

#ifdef AX12_WRITE_DEBUG
    printf("  Length : %d\n",TxBuf[3]);
#endif

    // Instruction
    if (flag == 1) {
        TxBuf[4]=0x04;
        sum += TxBuf[4];  
    } else {
        TxBuf[4]=0x03;
        sum += TxBuf[4];
    }

#ifdef AX12_WRITE_DEBUG
    printf("  Instruction : 0x%x\n",TxBuf[4]);
#endif

    // Start Address
    TxBuf[5] = start;
    sum += TxBuf[5];

#ifdef AX12_WRITE_DEBUG
    printf("  Start : 0x%x\n",TxBuf[5]);
#endif

    // data
    for (char i=0; i<bytes ; i++) {
        TxBuf[6+i] = data[i];
        sum += TxBuf[6+i];

#ifdef AX12_WRITE_DEBUG
        printf("  Data : 0x%x\n",TxBuf[6+i]);
#endif

    }

    // checksum
    TxBuf[6+bytes] = 0xFF - sum;

#ifdef AX12_WRITE_DEBUG
    printf("  Checksum : 0x%x\n",TxBuf[6+bytes]);
#endif
    
    
    /* Transmission de la trame construite precedemment dans le tableau TxBuf 
    */
    while ((timeout_transmit<100) && (i < (7+bytes)))
    {
        if (_ax12.writeable())  
        {
            _ax12.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 !
        {
            #ifdef AX12_DEBUG
            printf ("TIMEOUT TRANSMIT ERROR\r\n");
            #endif
            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.readable())       // reception du premier Header ( 0xFF )
                        { 
                        Status[plen] = _ax12.getc();
                        timeout = 0;
                            if (Status[plen] == 0xFF )
                            {
                                etat = Header2;
                                #ifdef AX12_DEBUG_WRITE
                                    printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
                                #endif
                                plen++; 
                                
                            }
                            else etat = Header1;
                        }
                        else timeout++;
                        break;
                    
     
     case Header2:      if (_ax12.readable())        // reception du second Header ( 0xFF )                               
                        { 
                            Status[plen] = _ax12.getc();
                            timeout = 0;
                            if (Status[plen] == 0xFF )
                            {
                                etat = ident;
                                #ifdef AX12_DEBUG_WRITE
                                    printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
                                #endif
                                plen++;             
                            }   
                            else 
                            {
                                etat = Header1;
                                plen = 0;
                            }
                        }
                        else timeout++;
                        break;
     
     case ident:        if (_ax12.readable())                   // reception de l'octet correspondant à l'ID du servomoteur
                        {
                            Status[plen] = _ax12.getc();
                            timeout = 0;
                            if (Status[plen] == ID )
                            {
                                etat = length;
                                #ifdef AX12_DEBUG_WRITE
                                    printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
                                #endif
                                plen++; 
                            }
                            else 
                            {
                                etat = Header1;
                                plen = 0;
                            }
                        }
                        else timeout++;
                        break;
     
     case length:       if (_ax12.readable())       // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres )
                        {
                            Status[plen] = _ax12.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;
                                #ifdef AX12_DEBUG_WRITE
                                    printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
                                #endif
                                plen++; 
                            }
                            else 
                            {
                                etat = Header1;
                                plen = 0;
                            }
                        }
                        else timeout++;
                        break;
    
     case erreur:       if (_ax12.readable())           //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur ) 
                        {
                            Status[plen] = _ax12.getc();
                            timeout = 0;
                            #ifdef AX12_DEBUG_WRITE
                                    printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
                            #endif
                            plen++;
                            etat = checksum;
                        } 
                        else timeout++;
                        
     case checksum:     if (_ax12.readable())              // recpetion du dernier octet ( Checksum ) >>> checksum = NOT ( ID + length ) >>>> dans le cas de la reception d'un write 
                        {
                            Status[plen] = _ax12.getc();
                            timeout = 0;
                            flag_out = 1;
                            etat = Header1;
                            #ifdef AX12_DEBUG_WRITE
                                    printf("data[%d]  : %d\r\n\n", plen, (int)Status[plen]);
                            #endif
                        } 
                        else timeout++;
                        break;    
        }
    }
  
    
    if ( Status[4] != 0 )
    {
        #ifdef AX12_DEBUG
        printf ("erreur ! \r\n");
        #endif
        for (int i = 0; i<5; i++)
        {
            #ifdef AX12_DEBUG
            printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
            #endif
        }   
    }
    
    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
        {
            #ifdef AX12_DEBUG
            printf ("timeout erreur\n\r");
            #endif
            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 ) 
}