code de la carte à tout faire / carte strat

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
ClementBreteau
Date:
Wed May 10 09:10:26 2017 +0000
Commit message:
pour gatien le 10 5 2017

Changed in this revision

AX12/AX12.cpp Show annotated file Show diff for this revision Revisions of this file
AX12/AX12.h Show annotated file Show diff for this revision Revisions of this file
AX12_V2/AX12_V2.cpp Show annotated file Show diff for this revision Revisions of this file
AX12_V2/AX12_V2.h Show annotated file Show diff for this revision Revisions of this file
Actionneurs/Actionneurs.cpp Show annotated file Show diff for this revision Revisions of this file
Actionneurs/Actionneurs.h Show annotated file Show diff for this revision Revisions of this file
Globals/constantes.h Show annotated file Show diff for this revision Revisions of this file
Globals/global.h Show annotated file Show diff for this revision Revisions of this file
Globals/ident_crac.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 76bc3ed27822 AX12/AX12.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AX12/AX12.cpp	Wed May 10 09:10:26 2017 +0000
@@ -0,0 +1,1577 @@
+/* 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 ) 
+}
diff -r 000000000000 -r 76bc3ed27822 AX12/AX12.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AX12/AX12.h	Wed May 10 09:10:26 2017 +0000
@@ -0,0 +1,429 @@
+/* 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.
+ */
+
+#ifndef MBED_AX12_H
+#define MBED_AX12_H
+
+
+#include "global.h"
+
+//#define AX12_WRITE_DEBUG 0
+#define AX12_READ_DEBUG   
+//#define AX12_TRIGGER_DEBUG 0
+#define AX12_DEBUG 0
+
+/****** à utiliser pour le debug !!  ******/
+
+//#define AX12_DEBUG_WRITE 0
+//#define AX12_DEBUG_READ 0
+
+/******************************************/
+
+
+#define AX12_REG_ID 0x03
+#define AX12_REG_BAUD 0x04
+#define AX12_REG_DELAY_TIME 0x05
+#define AX12_REG_CW_LIMIT 0x06
+#define AX12_REG_CCW_LIMIT 0x08
+#define AX12_REG_TEMP_MAX 0x0B
+#define AX12_REG_LOWEST_VOLTAGE 0x0C
+#define AX12_REG_HIGHEST_VOLTAGE 0x0D
+#define AX12_REG_MAX_TORQUE 0x0E
+#define AX12_REG_SATUS_RETURN 0x10
+#define AX12_REG_ALARM_LED 0x11
+#define AX12_REG_ALARM_SHUTDOWN 0x12
+#define AX12_REG_DOWN_CALIBRATION 0x14
+#define AX12_REG_UP_CALIBRATION 0x16
+#define AX12_REG_TORQUE_ENABLE 0x18
+#define AX12_REG_LED 0x19
+#define AX12_REG_CW_MARGIN 0x1A
+#define AX12_REG_CCW_MARGIN 0x1B
+#define AX12_REG_CW_SLOPE 0x1C
+#define AX12_REG_CCW_SLOPE 0x1D
+#define AX12_REG_GOAL_POSITION 0x1E
+#define AX12_REG_MOVING_SPEED 0x20
+#define AX12_REG_TORQUE_LIMIT 0x22
+#define AX12_REG_POSITION 0x24
+#define AX12_REG_PRESENT_SPEED 0x26
+#define AX12_REG_PRESENT_LOAD 0x28
+#define AX12_REG_VOLTS 0x2A
+#define AX12_REG_TEMP 0x2B
+#define AX12_REG_INSTRUCTION 0x2C
+#define AX12_REG_MOVING 0x2E
+#define AX12_REG_LOCK 0x2F
+#define AX12_REG_PUNCH 0x30
+
+
+#define AX12_MODE_POSITION  0
+#define AX12_MODE_ROTATION  1
+
+#define AX12_CW 1
+#define AX12_CCW 0
+
+//--- Instruction ---
+#define INST_PING 0x01
+#define INST_READ 0x02
+#define INST_WRITE 0x03
+#define INST_REG_WRITE 0x04
+#define INST_ACTION 0x05
+#define INST_RESET 0x06
+#define INST_DIGITAL_RESET 0x07
+#define INST_SYSTEM_READ 0x0C
+#define INST_SYSTEM_WRITE 0x0D
+#define INST_SYNC_WRITE 0x83
+#define INST_SYNC_REG_WRITE 0x84
+
+#define DEFAULT_RETURN_PACKET_SIZE 6
+
+#define BROADCASTING_ID 0xfe
+
+/** Servo control class, based on a PwmOut
+ *
+ * Example:
+ * @code
+ * #include "mbed.h"
+ * #include "AX12.h"
+ *
+ * int main() {
+ *
+ *   AX12 myax12 (p9, p10, 1);
+ *
+ *   while (1) {
+ *       myax12.SetGoal(0);    // go to 0 degrees
+ *       wait (2.0);
+ *       myax12.SetGoal(300);  // go to 300 degrees
+ *       wait (2.0);
+ *   }
+ * }
+ * @endcode
+ */
+class AX12
+{
+
+public:
+    
+    /** Create an AX12 servo object connected to the specified serial port, with the specified ID
+     *
+     * @param pin tx pin
+     * @param pin rx pin
+     * @param int ID, the Bus ID of the servo 1-255
+     */
+    AX12(PinName tx, PinName rx, int ID, int baud=1000000);
+    
+    /** Nouvelle fonction de commande de position du servomoteur avec sécurité d'arriver à la bonne position 
+        Si une erreur se produit et que le servo ne recoit vraiment rien cette fonction dispose d'une sortie en timeout error;
+    */
+    int Set_Secure_Goal(int degres);
+    
+    /** Retourne le temps d'attente avant l'envoi de la trame de retour par l'actionneur ( en micro seconde )
+    */
+    int Get_Return_Delay_Time(void);
+    
+    
+    /** Retourne la vitesse de communication de l'actionneur ( en Bits/seconde )
+    */
+    int Get_Baud_Rate(void);
+    
+    
+    /** Reglage du courant minimum necessaire au bon fonctionnement de l'actionneur 
+    //  minimum >>  Ox000    >>  decimal  0
+    //  maximum >>  0x3FF    >>  decimal  1023
+    //  deflaut >>  0x20     >>  decimal  32
+    */
+    int Set_Punch(int punch);
+    
+    
+    /** Retourne le courant minimum au fonctionnement de l'actionneur
+    */
+    int Get_Punch (void);
+    
+    
+    /** Retourne l'ampleur de la charge sur l'actionneur
+    */
+    int Get_Load_Value (void);
+    
+    void read_all_info(unsigned char, unsigned char);
+    
+    /** Reset
+    */
+    int Reset(int);
+    
+    /** Retourne la direction de la charge sur l'actionneur
+    */
+    int Get_Load_Direction (void);
+    
+    
+    /** Retourne la vitesse angulaire actuelle de l'actionneur
+    */
+    int Get_Present_Speed (void);
+    
+    
+    /** Retourne la valeur en degres du registre CCW angle limit qui est l'angle limite maximum de l'actionneur
+    */
+    int Get_CCW_Angle_Limit (void);
+    
+    
+    /** Retourne la valeur en degres du registre CW angle limit qui est l'angle limite minimum de l'actionneur
+    */
+    int Get_CW_Angle_Limit (void);
+    
+    
+    /** Retourne la valeur du registre Torque Enable 
+    */
+    int Get_Torque_Enable(void);
+    
+    
+    /** 
+    1  >>>  
+    0  >>>  
+    */
+    int Set_Torque_Enable(int etat); 
+    
+    
+    /** Retourne les données de compensation des différences entre les potentiomètres 
+    utilisés dans l'AX12    (Up)     ????????
+    */
+    int Get_Up_Calibration (void);
+    
+    
+    /** Retourne les données de compensation des différences entre les potentiomètres 
+    utilisés dans l'AX12    (Dowm)     ????????
+    */
+    int Get_Down_Calibration(void);
+    
+    
+    /** Retourne l'ID de l'AX_12 avec lequel on dialogue 
+    utile seulement dans le cas d'un broadcast ID 
+    */
+    int Get_ID(void);
+    
+    
+    /** Reglage du couple maximum de l'actionneur 
+    //  minimum >>  Ox000    >>  decimal  0
+    //  maximum >>  0x3FF    >>  decimal  1023
+    //  deflaut >>           >>  decimal  
+    */
+    int Set_Max_Torque(int torque);
+    
+    
+    /** 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 Set_Alarm_Shutdown(int valeur); 
+    
+    
+    /** 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 Set_Alarm_LED(int valeur);
+    
+    
+    /** Reglage de la réponse à une instruction
+    * @param mode  
+    *  0   >>  ne repond a aucune instructions
+    *  1   >>  repond seulement aux instructions READ_DATA 
+    *  2   >>  repond à toutes les instructions 
+    */
+    int Set_Status_Return_Level(int etat); 
+    
+    
+    /**  Reglage de la tension minimale
+    * @param mode
+    *  minimum >>  0x32    >>  decimal  50
+    *  maximum >>  0xFA    >>  decimal  250
+    *  deflaut >>  0x3C    >>  decimal  60
+    */  
+    int Set_Lowest_Voltage(int val_lowest_voltage); 
+    
+    
+     /**  Reglage de la tension maximale
+    * @param mode
+    *  minimum >>  Ox32    >>  decimal  50
+    *  maximum >>  0xFA    >>  decimal  250
+    *  deflaut >>  0xBE    >>  decimal  190
+    */  
+    int Set_Highest_Voltage(int val_highest_voltage); 
+    
+    
+    // Reglage du return time delay  EN MICRO SECONDE              2uSec * val_delay_time
+    //  minimum >>  0       us 
+    //  maximum >>  508     us
+    //  deflaut >>  125     us
+    int Set_Delay_Time (int val_delay_time );
+    
+    
+    /** Set Highest Limit Temperature
+    * @param mode
+    *    minimum >>  Ox00    >>  decimal  0
+    *    maximum >>  0x96    >>  decimal  150
+    */
+    int Set_Temperature_Max (int val_temperature );
+    
+    
+    
+    /** Set the state of LED 
+    * @param mode
+    *    0 = off, default
+    *    1 = on 
+    */  
+    int Set_Etat_LED(int etat);
+    
+    
+
+    /** Set the mode of the servo
+     * @param mode
+     *    0 = Positional, default
+     *    1 = Continuous rotation
+     */
+    int Set_Mode(int mode);
+
+    /** Set baud rate of all attached servos
+     * @param mode
+     *    0x01 = 1,000,000 bps
+     *    0x03 =   500,000 bps
+     *    0x04 =   400,000 bps
+     *    0x07 =   250,000 bps
+     *    0x09 =   200,000 bps
+     *    0x10 =   115,200 bps
+     *    0x22 =    57,600 bps
+     *    0x67 =    19,200 bps
+     *    0xCF =     9,600 bp
+     */
+    int Set_Baud(int baud);
+
+
+    /** Set goal angle in integer degrees, in positional mode
+     *
+     * @param degrees 0-300
+     * @param flags, defaults to 0
+     *    flags[0] = blocking, return when goal position reached
+     *    flags[1] = register, activate with a broadcast trigger
+     *
+     */
+    int Set_Goal(int degrees, int flags = 0);
+
+    int Set_Goal_speed(int degrees, int flags = 0);
+
+    /** Set the speed of the servo in continuous rotation mode
+     *
+     * @param speed, -1.0 to 1.0
+     *   -1.0 = full speed counter clock wise
+     *    1.0 = full speed clock wise
+     */
+    int Set_CR_Speed(float speed);
+
+
+    /** Permet de définir l'angle limite minimum de l'actionneur ( prend une valeur d'entrée en degres )
+    //  minimum >>  0°   
+    //  maximum >>  300°  
+    //  deflaut >>  0°        
+    */
+    int Set_CW_Angle_Limit(int degrees);
+
+    /** Permet de définir l'angle limite maximum de l'actionneur ( prend une valeur d'entrée en degres )
+    //  minimum >>  0°   
+    //  maximum >>  300°  
+    //  deflaut >>  300°        
+    */
+    int Set_CCW_Angle_Limit(int degrees);
+
+    // Change the ID
+
+    /** Change the ID of a servo
+     *
+     * @param CurentID 1-255
+     * @param NewID 1-255
+     *
+     * If a servo ID is not know, the broadcast address of 0 can be used for CurrentID.
+     * In this situation, only one servo should be connected to the bus
+     */
+    int Set_ID(int CurrentID, int NewID);
+
+
+    /** Poll to see if the servo is moving
+     *
+     * @returns true is the servo is moving
+     */
+    int isMoving(void);
+
+    /** Send the broadcast "trigger" command, to activate any outstanding registered commands
+     */
+    void trigger(void);
+
+    /** Send the "reset" command, to activate any outstanding registered commands
+     */
+    void reset();
+
+    /** Read the current angle of the servo
+     *
+     * @returns float in the range 0.0-300.0
+     */
+    float Get_Position();
+
+    /** Read the temperature of the servo
+     *
+     * @returns float temperature
+     */
+    float Get_Temp(void);
+
+    /** Read the supply voltage of the servo
+     *
+     * @returns float voltage
+     */
+    float Get_Volts(void);
+    
+    // Lecture du couple maximum ( retourne la valeur du registre Max Torque de l'AX12 ) 
+    int Get_Max_Torque (void);
+    
+
+    int read(int ID, int start, int length, char* data);
+    int write(int ID, int start, int length, char* data, int flag=0);
+    void multiple_goal_and_speed(int number_ax12,char* tab);
+    float read_and_test(float angle,char* Tab);
+    
+private :
+
+    SerialHalfDuplex _ax12;
+    int _ID;
+    int _baud;
+
+
+};
+
+#endif
diff -r 000000000000 -r 76bc3ed27822 AX12_V2/AX12_V2.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AX12_V2/AX12_V2.cpp	Wed May 10 09:10:26 2017 +0000
@@ -0,0 +1,24 @@
+#include "AX12_V2.h"
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: AX12BaisserMainGauche                                                 */
+/* DESCRIPTION  : envoie valeurActuelle+1 à l'AX12 des mains GAUCHE                     */
+/****************************************************************************************/
+char AX12BaisserMainGauche(char valeurActuelle){
+    //envoie valeurActuelle+1 à l'AX12 des mains GAUCHE
+    valeurActuelle += 10;
+    myAX12_2->Set_Secure_Goal(valeurActuelle);
+    return valeurActuelle;
+    }
+
+/****************************************************************************************/
+/* FUNCTION NAME: AX12BaisserMainDroite                                                 */
+/* DESCRIPTION  : envoie valeurActuelle+1 à l'AX12 des mains DROITE                     */
+/****************************************************************************************/
+char AX12BaisserMainDroite(char valeurActuelle){
+    //envoie valeurActuelle+1 à l'AX12 des mains DROITE
+    valeurActuelle += 10;
+    myAX12_2->Set_Secure_Goal(valeurActuelle);
+    return valeurActuelle;
+    }
diff -r 000000000000 -r 76bc3ed27822 AX12_V2/AX12_V2.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AX12_V2/AX12_V2.h	Wed May 10 09:10:26 2017 +0000
@@ -0,0 +1,18 @@
+#ifndef CRAC_AX12_V2
+#define CRAC_AX12_V2
+
+#include "global.h"
+
+/****************************************************************************************/
+/* FUNCTION NAME: AX12BaisserMainGauche                                                 */
+/* DESCRIPTION  : envoie valeurActuelle+1 à l'AX12 des mains GAUCHE                     */
+/****************************************************************************************/
+char AX12BaisserMainGauche(char valeurActuelle);
+/****************************************************************************************/
+/* FUNCTION NAME: AX12BaisserMainDroite                                                 */
+/* DESCRIPTION  : envoie valeurActuelle+1 à l'AX12 des mains DROITE                     */
+/****************************************************************************************/
+char AX12BaisserMainDroite(char valeurActuelle);
+
+
+#endif
\ No newline at end of file
diff -r 000000000000 -r 76bc3ed27822 Actionneurs/Actionneurs.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Actionneurs/Actionneurs.cpp	Wed May 10 09:10:26 2017 +0000
@@ -0,0 +1,28 @@
+#include "Actionneurs.h"
+
+DigitalOut electroVanne(PA_6);
+
+/****************************************************************************************/
+/* FUNCTION NAME: turbineWritePWM                                                       */
+/* DESCRIPTION  : envoie une pwm de valeurPWM à la turbine                              */
+/****************************************************************************************/
+void turbineWritePWM(int valeurPWM){
+    // envoie une pwm de valeurPWM à la turbine
+    }
+    
+/****************************************************************************************/
+/* FUNCTION NAME: pompeWritePWM                                                         */
+/* DESCRIPTION  : envoie une pwm de valeurPWM à la pompe                                */
+/****************************************************************************************/
+void pompeWritePWM(int valeurPWM){
+    // envoie une pwm de valeurPWM à la pompe
+    }
+    
+    
+/****************************************************************************************/
+/* FUNCTION NAME: controlElectrovanne                                                   */
+/* DESCRIPTION  : permet de controller l'electrovanne                                   */
+/****************************************************************************************/
+void controlElectrovanne(int etat){
+    electroVanne = 1;
+    }
\ No newline at end of file
diff -r 000000000000 -r 76bc3ed27822 Actionneurs/Actionneurs.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Actionneurs/Actionneurs.h	Wed May 10 09:10:26 2017 +0000
@@ -0,0 +1,47 @@
+#ifndef CRAC_ACTIONNEUR
+#define CRAC_ACTIONNEUR
+
+#include "global.h"
+
+typedef enum
+{
+    ETAT_ATTENTE,
+    
+    ETAT_TRAITEMENT_ACTION_AX12,
+    
+    ETAT_BAISSER_MAIN_GAUCHE,
+    ETAT_BAISSER_MAIN_DROITE,
+    ETAT_CHECK_COULEUR,
+    
+    ETAT_TURBINE,
+    ETAT_POMPES
+    
+} E_GameEtat;
+
+
+typedef enum{
+    BLEU,
+    JAUNE
+}E_Couleur;
+
+/****************************************************************************************/
+/* FUNCTION NAME: turbineWritePWM                                                       */
+/* DESCRIPTION  : envoie une pwm de valeurPWM à la turbine                              */
+/****************************************************************************************/
+void turbineWritePWM(int valeurPWM);
+/****************************************************************************************/
+/* FUNCTION NAME: pompeWritePWM                                                         */
+/* DESCRIPTION  : envoie une pwm de valeurPWM à la pompe                                */
+/****************************************************************************************/
+void pompeWritePWM(int valeurPWM);
+/****************************************************************************************/
+/* FUNCTION NAME: pompeWritePWM                                                         */
+/* DESCRIPTION  : ouvre ou ferme l'electrovannes                                        */
+/****************************************************************************************/
+void electrovanneControle(int etat);
+/****************************************************************************************/
+/* FUNCTION NAME: pompeWritePWM                                                         */
+/* DESCRIPTION  : ouvre ou ferme l'electrovannes                                        */
+/****************************************************************************************/
+void electrovanneControle(int etat);
+#endif
\ No newline at end of file
diff -r 000000000000 -r 76bc3ed27822 Globals/constantes.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Globals/constantes.h	Wed May 10 09:10:26 2017 +0000
@@ -0,0 +1,8 @@
+#ifndef CRAC_CONSTANTES
+#define CRAC_CONSTANTES
+
+#define SIZE_FIFO 20
+#define ENFONCER 0
+#define AX12_POSIOTION_BASSE 0x100
+
+#endif
\ No newline at end of file
diff -r 000000000000 -r 76bc3ed27822 Globals/global.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Globals/global.h	Wed May 10 09:10:26 2017 +0000
@@ -0,0 +1,14 @@
+#include "mbed.h"
+
+#include "SerialHalfDuplex.h"
+
+#include "constantes.h"
+#include "ident_crac.h"
+#include "Actionneurs.h"
+#include "AX12.h"
+#include "AX12_V2.h"
+
+
+extern CAN can1;
+extern CANMessage msgRxBuffer[SIZE_FIFO];
+extern unsigned char FIFO_ecriture;
\ No newline at end of file
diff -r 000000000000 -r 76bc3ed27822 Globals/ident_crac.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Globals/ident_crac.h	Wed May 10 09:10:26 2017 +0000
@@ -0,0 +1,138 @@
+#ifndef CRAC_IDENTH
+#define CRAC_IDENTH
+
+
+
+#define GLOBAL_GAME_END 0x004  // Stop fin du match
+#define GLOBAL_START 0x002  // Start
+#define GLOBAL_END_INIT_POSITION 0x005  // Fin positionnement robot avant depart
+#define GLOBAL_FUNNY_ACTION 0x007  // Funny action start  (0: start, 1: stop)
+
+#define BALISE_STOP 0x003  // Trame stop
+
+#define BALISE_DANGER 0xA  // Trame danger
+
+#define BALISE_END_DANGER 0xB  // Trame fin de danger
+
+
+#define ASSERVISSEMENT_STOP 0x001  // Stop moteur
+#define ASSERVISSEMENT_SPEED_DANGER 0x006  // Vitesse de danger
+#define ASSERVISSEMENT_XYT 0x020  // Asservissement (x,y,theta)  (0 : au choix 1 : avant -1 : arrière)
+#define ASSERVISSEMENT_COURBURE 0x021  // Asservissement rayon de courbure  (+ gauche, - droite , sens : 1avt , -1arr; enchainement => 1 oui, 0 => non, 2=>derniére instruction de l'enchainement)
+#define ASSERVISSEMENT_CONFIG 0x022  // Asservissement paramètre  (définir les valeurs de vitesse max et d'eccélération max)
+#define ASSERVISSEMENT_ROTATION 0x023  // Asservissement rotation
+#define ASSERVISSEMENT_RECALAGE 0x024  // Moteur tout droit  (recalage : 0 mouvement seul, 1 x, 2y valeur : coordonnée à laquelle est recalé x/y; enchainement => 1 oui, 0 => non)
+
+#define ODOMETRIE_BIG_POSITION 0x026  // Odométrie position robot  (Position actuel du robot)
+#define ODOMETRIE_BIG_VITESSE 0x027  // Odométrie vitesse  (Indication sur l'état actuel)
+#define ODOMETRIE_SMALL_POSITION 0x028  // Odométrie position robot  (Position actuel du robot)
+#define ODOMETRIE_SMALL_VITESSE 0x029  // Odométrie vitesse  (Indication sur l'état actuel)
+#define ACTION_BIG_DEMARRAGE 0x025  // Action de départ du GR  (Lancement de la trajectoire de départ du GR)
+
+#define ASSERVISSEMENT_INFO_CONSIGNE 0x1F0  // Info Consigne et Commande moteur
+#define ASSERVISSEMENT_CONFIG_KPP_DROITE 0x1F1  // Config coef KPP_Droit
+#define ASSERVISSEMENT_CONFIG_KPI_DROITE 0x1F2  // Config coef KPI_Droit
+#define ASSERVISSEMENT_CONFIG_KPD_DROITE 0x1F3  // Config coef KPD_Droit
+#define ASSERVISSEMENT_CONFIG_KPP_GAUCHE 0x1F4  // Config coef KPP_Gauche
+#define ASSERVISSEMENT_CONFIG_KPI_GAUCHE 0x1F5  // Config coef KPI_Gauche
+#define ASSERVISSEMENT_CONFIG_KPD_GAUCHE 0x1F6  // Config coef KPD_Gauche
+#define ASSERVISSEMENT_ENABLE 0x1F7  // Activation asservissement  (0 : désactivation, 1 : activation)
+
+
+#define RESET_BALISE 0x030  // Reset balise
+#define RESET_MOTEUR 0x031  // Reset moteur
+#define RESET_IHM 0x032  // Reset écran tactile
+#define RESET_ACTIONNEURS 0x033  // Reset actionneurs
+#define RESET_POMPES 0x034  // Reset pompes
+#define RESET_AX12 0x035  // Reset AX12
+#define RESET_TELEMETRE 0x036 // Reset telemetre
+
+
+
+#define RESET_STRAT 0x3A  // Reset stratégie
+
+
+#define CHECK_BALISE 0x060  // Check balise
+#define CHECK_MOTEUR 0x061  // Check moteur
+#define CHECK_IHM 0x062  // Check écran tactile
+#define CHECK_ACTIONNEURS 0x063  // Check actionneurs
+#define CHECK_POMPES 0x064  // Check pompes
+#define CHECK_AX12 0x065  // Check AX12
+#define CHECK_OK_TELEMETRE 0x066 // Check telemetre
+
+
+
+
+#define ALIVE_BALISE 0x070  // Alive balise
+#define ALIVE_MOTEUR 0x071  // Alive moteur
+#define ALIVE_IHM 0x072  // Alive écran tactile
+#define ALIVE_ACTIONNEURS 0x073  // Alive actionneurs
+#define ALIVE_POMPES 0x074  // Alive pompes
+#define ALIVE_AX12 0x075  // Alive AX12
+#define ALIVE_TELEMETRE 0x076 // Alive telemetre
+
+
+
+
+#define ACKNOWLEDGE_BALISE 0x100  // Acknowledge balise
+#define ACKNOWLEDGE_MOTEUR 0x101  // Acknowledge moteur
+#define ACKNOWLEDGE_IHM 0x102  // Acknowledge ecran tactile
+#define ACKNOWLEDGE_ACTIONNEURS 0x103  // Acknowledge actionneurs
+#define ACKNOWLEDGE_POMPES 0x104  // Acknowledge pompes
+#define ACKNOWLEDGE_TELEMETRE 0x105 // Acknowledge telemetre
+#define ACKNOWLEDGE_AX12 0x106 // Ack ax12
+#define ACKNOWLEDGE_STRAT 0x10A  // Acknowledge pompes
+
+
+#define INSTRUCTION_END_BALISE 0x110  // Fin instruction balise  (Indique que l'instruction est terminée)
+#define INSTRUCTION_END_MOTEUR 0x111  // Fin instruction moteur  (Indique que l'instruction est terminée)
+#define INSTRUCTION_END_IHM 0x112  // Fin instruction ecran tactile  (Indique que l'instruction est terminée)
+#define INSTRUCTION_END_ACTIONNEURS 0x113  // Fin instruction actionneurs  (Indique que l'instruction est terminée)
+#define INSTRUCTION_END_AX12 0x116
+
+#define ECRAN_CHOICE_STRAT 0x601  // Choix d'une stratégie  (n° strat (1-4))
+#define ECRAN_CHOICE_COLOR 0x602  // Couleur  (0->Blue;1->Yellow)
+#define ECRAN_START_MATCH 0x603  // Match  (Indique que l'on souhaite commencer le match)
+#define ECRAN_DEMO_BEGIN 0x604 // Demo (Indique que l'on souhaite faire une demo)
+#define ECRAN_ACK_STRAT 0x611  // Acknowledge stratégie  (si 0 erreur, sinon n°strat)
+#define ECRAN_ACK_COLOR 0x612  // Acknowledge couleur  (0->Blue;1->Yellow)
+#define ECRAN_ACK_START_MATCH 0x613  // Acknowledge Match  (Indique que l'on a bien reçu le debut du match)
+#define ECRAN_ACK_DEMO 0x614
+#define ECRAN_ALL_CHECK 0x620  // Carte all check  (Si provient de carte strat => toutes les cartes sont en ligne, Si provient IHM => forcer le lancement)
+#define ECRAN_TIME 0x621  // Time match  (Indication de moment cle du temps (10,30,60,70,80,85,90))
+#define ECRAN_PRINTF_1 0x6C0  // Tactile printf  (Afficher les 8 permier caractères)
+#define ECRAN_PRINTF_2 0x6C1  // Tactile printf  (Afficher les 8 second caractères)
+#define ECRAN_PRINTF_3 0x6C2  // Tactile printf  (Afficher les 8 troisième caractères)
+#define ECRAN_PRINTF_4 0x6C3  // Tactile printf  (Afficher les 8 quatrième caractères)
+#define ECRAN_PRINTF_CLEAR 0x6CF  // Tactile printf clear  (Permet d'effacer l'ecran)
+#define ECRAN_CHOICE_START_ACTION 0x604  // Tactile printf clear  (Choisir si il faut lancer le test actionneur)
+#define ECRAN_ACK_CHOICE_START_ACTION 0x605  // Tactile printf clear  (Ack du test actionneur)
+
+#define ERROR_OVERFLOW_BALISE 0x040  // Overflow odométrie
+#define ERROR_OVERFLOW_MOTEUR 0x041  // Overflow asservissement
+#define ERROR_OVERFLOW_IHM 0x042  // Overflow balise
+#define ERROR_OVERFLOW_STRAT 0x043  // Overflow stratégie
+#define ERROR_BALISE 0x785  // Bug balise
+#define ERROR_RTC 0x786  // Bug RTC
+#define ERROR_MOTEUR 0x787  // Bug moteur
+#define ERROR_TELEMETRIE 0x788  // Bug télémètre
+#define ERROR_STRATEGIE 0x789  // Bug stratégie
+
+#define DEBUG_STRATEGIE_AUTOMATE 0x760  // Etat automate stratégie  (Permet de savoir l'etat de l'automate)
+#define DEBUG_FAKE_JAKE 0x761  // Fake jack  (Permet d'outre passerr le JACk du robot)
+#define DEBUG_ASSERV 0x762  // Info debug carte moteur
+
+#define SERVO_AX12_SETGOAL 0x090  // AX12 setGoal  (Indiquer la nouvelle position de l'AX12 !! Ne bouge pas)
+#define SERVO_AX12_PROCESS 0x091  // AX12 processChange  (Lancer le déplacement des AX12)
+#define SERVO_AX12_DONE 0x092  // AX12 done  (Indique q'un AX12 a terminé son déplacement)
+#define SERVO_XL320 0x093  // XL320
+#define SERVO_AX12_ACTION 0x96
+
+
+#define TELEMETRE_RECHERCHE_COIN 0x301
+#define TELEMETRE_OBJET 0x302
+#define OBJET_SUR_TABLE 0x304
+
+#define POMPE_PWM 0x9A  // pwm des pompes  (pwm entre 0 et 100)
+    
+#endif
diff -r 000000000000 -r 76bc3ed27822 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed May 10 09:10:26 2017 +0000
@@ -0,0 +1,237 @@
+#include "global.h"
+
+/****************************************************************************************/
+/* FUNCTION NAME: automate_process                                                      */
+/* DESCRIPTION  : Automate de gestion de la stratégie du robot                          */
+/****************************************************************************************/
+void automate_process(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: canProcessRx                                                          */
+/* DESCRIPTION  : Fonction de traitement des messages CAN                               */
+/****************************************************************************************/
+void canProcessRx(void);
+
+/*********************************************************************************************************/
+/* FUNCTION NAME: SendRawId                                                                              */
+/* DESCRIPTION  : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */
+/*********************************************************************************************************/
+void SendRawId (unsigned short id);
+
+DigitalOut led(LED1);
+Serial pc(SERIAL_TX, SERIAL_RX);
+/*DigitalIn IO1(PB_7);
+DigitalIn IO2(PB_6);
+DigitalIn IO3(PF_1);
+DigitalIn IO4(PA_8);
+
+DigitalIn A_in1(PB_7);
+DigitalIn A_in2(PA_6);
+DigitalIn A_in3(PA_4);
+DigitalIn A_in4(PA_1);
+DigitalIn A_in5(PA_0); 
+
+DigitalOut AX12(PA_9);
+*/
+
+AnalogIn cptGauche(PA_1);
+AnalogIn cptDroit(PA_0);
+DigitalIn pressionGauche(PA_8);
+DigitalIn pressionDroit(PB_7);
+    
+
+
+
+CAN can1(PA_11,PA_12); // Rx&Tx pour le CAN
+CANMessage msgRxBuffer[SIZE_FIFO]; // buffer en réception pour le CAN
+unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
+
+signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
+
+extern "C" void mbed_reset();//Pour pouvoir reset la carte
+
+E_GameEtat carte_etat = ETAT_ATTENTE;
+E_Couleur couleurSelect = BLEU;
+
+AX12 *myAX12;
+
+
+
+
+void canRx_ISR (void)
+{
+    if (can1.read(msgRxBuffer[FIFO_ecriture])) {
+        /*if(msgRxBuffer[FIFO_ecriture].id==RESET_ACTIONNEURS) mbed_reset();
+        else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;*/
+        FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
+    }
+}
+int main() {
+    char truc = 0;
+    
+    can1.frequency(1000000); // fréquence de travail 1Mbit/s
+    can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
+    
+    CANMessage msgTx=CANMessage();
+    
+    myAX12-> Set_Goal_speed(vitesse);
+    myAX12-> Set_Mode(0);        
+    
+    pc.printf("Hello World\n"); 
+    while(1) {
+        
+        led = ((led == 1)?0:1);
+        
+        automate_process();
+        canProcessRx();
+        //SendRawId(cpt); 
+        wait(0.5);   
+    }
+}
+
+
+
+
+int action;
+char localAX12 = AX12_POSIOTION_BASSE;
+/****************************************************************************************/
+/* FUNCTION NAME: automate_process                                                      */
+/* DESCRIPTION  : Automate de gestion de la stratégie du robot                          */
+/****************************************************************************************/
+void automate_process(void){
+    SendRawId(carte_etat);
+    switch(carte_etat){
+        case ETAT_ATTENTE:
+        break;
+        
+        case ETAT_TRAITEMENT_ACTION_AX12:
+            switch(action){
+                #define ACTION_TOURNER_MODULE_GAUCHE 0x10
+                #define ACTION_TOURNER_MODULE_DROIT  0x11
+                case ACTION_TOURNER_MODULE_GAUCHE:
+                        carte_etat = ETAT_BAISSER_MAIN_GAUCHE;
+                break;
+                case ACTION_TOURNER_MODULE_DROIT:
+                        carte_etat = ETAT_BAISSER_MAIN_DROITE;
+                break;
+                default:
+                    carte_etat = ETAT_ATTENTE;
+                }
+        break;
+    
+        case ETAT_BAISSER_MAIN_GAUCHE:
+            if(pressionGauche.read() == ENFONCER){
+                carte_etat = ETAT_CHECK_COULEUR;
+            } else{
+                // 
+                localAX12 = AX12BaisserMainGauche(localAX12);
+            }             
+        break;
+        
+        case ETAT_BAISSER_MAIN_DROITE:
+            if(pressionDroit.read() == ENFONCER){
+                carte_etat = ETAT_CHECK_COULEUR;
+            }else{
+                // 
+                localAX12 = AX12BaisserMainDroite(localAX12);
+            }
+        break;
+        
+        case ETAT_CHECK_COULEUR:
+            //if (((unsigned short)cptGauche.read()&(unsigned short)cptDroit.read()) == 0){
+            if ((unsigned short)cptDroit.read() == 0){
+                carte_etat = ETAT_ATTENTE;
+            }else{
+                // on active les moteurs
+                localAX12 = AX12BaisserMainGauche(localAX12);    
+            }
+        break;
+        
+        case ETAT_TURBINE:
+            carte_etat = ETAT_ATTENTE;
+            turbineWritePWM(50);
+        break;
+        
+        case ETAT_POMPES:
+            carte_etat = ETAT_ATTENTE;
+            pompeWritePWM(50);
+        break;
+        
+        
+        
+        default:
+            pc.printf("etat non identifie");
+        }
+    }
+    
+    
+/*********************************************************************************************************/
+/* FUNCTION NAME: SendRawId                                                                              */
+/* DESCRIPTION  : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */
+/*********************************************************************************************************/
+void SendRawId (unsigned short id)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=id;
+    msgTx.len=0;
+    can1.write(msgTx);
+    wait_us(200);
+}
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: canProcessRx                                                          */
+/* DESCRIPTION  : Fonction de traitement des messages CAN                               */
+/****************************************************************************************/
+void canProcessRx(void){
+    static signed char FIFO_occupation=0,FIFO_max_occupation=0;
+    CANMessage msgTx=CANMessage();
+    FIFO_occupation=FIFO_ecriture-FIFO_lecture;
+    if(FIFO_occupation<0)
+        FIFO_occupation=FIFO_occupation+SIZE_FIFO;
+    if(FIFO_max_occupation<FIFO_occupation)
+        FIFO_max_occupation=FIFO_occupation;
+    if(FIFO_occupation!=0) {
+        
+        switch(msgRxBuffer[FIFO_lecture].id) {
+            case CHECK_ACTIONNEURS:
+                SendRawId(ALIVE_ACTIONNEURS);
+            break;
+            
+            case SERVO_AX12_ACTION : 
+                carte_etat = ETAT_TRAITEMENT_ACTION_AX12;
+                
+                action = msgRxBuffer[FIFO_lecture].data[0];
+                couleurSelect = (E_Couleur)msgRxBuffer[FIFO_lecture].data[1];
+                //ACK de reception des actions a effectuer
+                msgTx.id = ACKNOWLEDGE_AX12;
+                msgTx.len = 1;
+                msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0];
+                can1.write(msgTx);
+            break;
+            
+            case POMPE_PWM:
+                carte_etat = ETAT_POMPES;
+            
+                msgTx.id = ACKNOWLEDGE_POMPES;
+                msgTx.len = 1;
+                msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0];
+                can1.write(msgTx);
+            break;
+            
+            #define TURBINE_PWM 0xa1
+            #define ACKNOWLEDGE_TURBINE 0x107
+            case TURBINE_PWM:
+                carte_etat = ETAT_TURBINE;
+            
+                msgTx.id = ACKNOWLEDGE_TURBINE;
+                msgTx.len = 1;
+                msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0];
+                can1.write(msgTx);
+            break;  
+            
+        }
+        FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
+    }
+    }
+
diff -r 000000000000 -r 76bc3ed27822 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed May 10 09:10:26 2017 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/mbed_official/code/mbed/builds/794e51388b66
\ No newline at end of file