CRAC Team
code strat avant match 2, strat inversée OK normalement
Fork of
CRAC-Strat_2017_fin_premier_match
by 
CRAC Team
Diff: AX12/AX12.cpp
- Revision:
- 15:c2fc239e85df
- Child:
- 16:7321fb3bb396
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AX12/AX12.cpp Thu May 11 12:55:52 2017 +0000
@@ -0,0 +1,1828 @@
+/* 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 AX12_INITIALISATION 0
+#define AX12_PREPARATION_PRISE 1
+#define AX12_STOCKAGE_HAUT 2
+#define AX12_STOCKAGE_BAS 3
+#define AX12_DEPOSER 4
+#define AX12_PREPARATION_DEPOT_BAS 5
+#define AX12_PREPARATION_DEPOT_HAUT 6
+#define AX12_POUSSER_MODULE 7
+#define AX12_DEFAUT 20
+
+#define TIME 0.8
+#define SIZE_FIFO 25
+
+#define MAX_TIMEOUT 500
+
+extern "C" void mbed_reset();//Pour pouvoir reset la carte
+
+unsigned char action = 0, choix_bras = 0, etat_ax12 = 0, flag = 0, action_precedente = 0;
+short vitesse=700;
+float angle=0.0;
+float test_socle=0.0,test_bas=0.0,test_milieu=0.0,test_haut=0.0,test_ventouse=0.0, test_calcul=0.0, valeur_test=0.0;
+AX12 *un_myAX12, *sept_myAX12, *huit_myAX12, *seize_myAX12, *multiple_myAX12; //Pince centrale
+AX12 *un_myAX12_2, *deux_myAX12_2, *trois_myAX12_2, *multiple_myAX12_2; //Bras de gauche
+
+Timer t;
+Ticker flipper;
+
+////////////////////// TABLEAU PINCE CENTRALE ///////////////////////////
+static char TAB1[25]= {0x01,0x05, 0x02, 0xFF, 0x00, ///Position initiale
+ 0x10,0x00, 0x02, 0xFF, 0x00,
+ 0x07,0x00, 0x01, 0xFF, 0x00,
+ 0x08,0x00, 0x03, 0xFF, 0x00};
+
+static char TAB2[25]= {0x01,0x50, 0x00, 0xFF, 0x03, ///Preparation prise
+ 0x10,0x50, 0x01, 0xFF, 0x03,
+ 0x07,0xF4, 0x01, 0xFF, 0x03,
+ 0x08,0xF4, 0x01, 0xFF, 0x03};
+
+static char TAB3[25]= {0x01,0x50, 0x00, 0xFF, 0x03, ///Stockage haut (pince fermee)
+ 0x10,0x50, 0x01, 0xFF, 0x03,
+ 0x07,0xC5, 0x00, 0xFF, 0x03,
+ 0x08,0x4D, 0x03, 0xFF, 0x03};
+
+static char TAB4[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (pince en l'air)
+ 0x10,0x50, 0x01, 0xFF, 0x03,
+ 0x07,0xC5, 0x00, 0xFF, 0x03,
+ 0x08,0x4D, 0x03, 0xFF, 0x03};
+
+static char TAB5[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (module sur tige)
+ 0x10,0xF4, 0x01, 0xFF, 0x00,
+ 0x07,0xC5, 0x00, 0xFF, 0x03,
+ 0x08,0x4D, 0x03, 0xFF, 0x03};
+
+static char TAB6[25]= {0x01,0xB0, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte)
+ 0x10,0xF4, 0x01, 0xFF, 0x03,
+ 0x07,0x00, 0x01, 0xFF, 0x03,
+ 0x08,0x00, 0x03, 0xFF, 0x03};
+
+static char TAB7[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage bas (pince en l'air)
+ 0x10,0xB0, 0x00, 0xFF, 0x03,
+ 0x07,0xC5, 0x00, 0xFF, 0x00,
+ 0x08,0x4D, 0x03, 0xFF, 0x00};
+
+static char TAB8[25]= {0x01,0x40, 0x00, 0xFF, 0x03, ///Preparation_depot_bas
+ 0x10,0xF4, 0x01, 0xFF, 0x03,
+ 0x07,0xC5, 0x00, 0xFF, 0x00,
+ 0x08,0x4D, 0x03, 0xFF, 0x00};
+
+static char TAB9[25]= {0x01,0x40, 0x00, 0xFF, 0x03, ///Deposer
+ 0x10,0xF4, 0x01, 0xFF, 0x03,
+ 0x07,0xD0, 0x00, 0xFF, 0x00,
+ 0x08,0x35, 0x03, 0xFF, 0x00};
+
+static char TAB10[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (module sur tige)
+ 0x10,0x00, 0x01, 0xFF, 0x00,
+ 0x07,0xC5, 0x00, 0xFF, 0x03,
+ 0x08,0x4D, 0x03, 0xFF, 0x03};
+
+static char TAB11[25]= {0x01,0x60, 0x00, 0xFF, 0x03, ///Pousser_module
+ 0x10,0xF4, 0x01, 0xFF, 0x03,
+ 0x07,0xC5, 0x00, 0xFF, 0x00,
+ 0x08,0x4D, 0x03, 0xFF, 0x00};
+
+static char TAB12[25]= {0x01,0x05, 0x02, 0xFF, 0x03, ///Sortie position initiale
+ 0x10,0x00, 0x02, 0xFF, 0x03,
+ 0x07,0xF4, 0x01, 0xFF, 0x03,
+ 0x08,0xF4, 0x01, 0xFF, 0x03};
+
+static char TAB13[25]= {0x01,0xF4, 0x00, 0xFF, 0x03, ///Deposer
+ 0x10,0xA0, 0x02, 0xFF, 0x03,
+ 0x07,0xD0, 0x00, 0xFF, 0x00,
+ 0x08,0x35, 0x03, 0xFF, 0x00};
+
+////////////////////// TABLEAU BRAS GAUCHE ///////////////////////////
+static char TAB21[25]= {0x01,0x50, 0x03, 0xFF, 0x03, ///Position initiale
+ 0x02,0xF4, 0x01, 0xFF, 0x03};
+
+static char TAB22[25]= {0x01,0x20, 0x01, 0xFF, 0x03, ///Preparation_tourner
+ 0x02,0x40, 0x03, 0xFF, 0x03};
+
+static char TAB23[25]= {0x01,0x20, 0x01, 0xFF, 0x03, ///Tourner_module
+ 0x02,0xE5, 0x02, 0xFF, 0x03};
+
+
+
+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 )
+}
+
+
+
+
+//////////////////////////////////////////////////////////////////////////////
+
+/****************************************************************************************/
+/* FUNCTION NAME: Initialisation_position */
+/* DESCRIPTION : Fonction qui place les bras en position verticale */
+/****************************************************************************************/
+void Initialisation_position(void){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB12);
+ wait(TIME);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB1);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Preparation_prise */
+/* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */
+/****************************************************************************************/
+void Preparation_prise(void){
+ if (action_precedente == 0){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB12);
+ wait(TIME);
+ action_precedente = 1;
+ }
+ multiple_myAX12->multiple_goal_and_speed(4,TAB2);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Stockage_haut */
+/* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */
+/****************************************************************************************/
+void Stockage_haut(void){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB3);
+ wait(TIME);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB4);
+ wait(TIME);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB5);
+ wait(TIME);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB6);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Stockage_bas */
+/* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */
+/****************************************************************************************/
+void Stockage_bas(void){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB3);
+ wait(TIME);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB7);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Deposer */
+/* DESCRIPTION : Fonction qui permet de déposer le module */
+/****************************************************************************************/
+void Deposer(void){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB9);
+ wait(TIME/5);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB8);
+ wait(TIME/5);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB13);
+ wait(TIME/5);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Preparation_depot_bas */
+/* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */
+/****************************************************************************************/
+void Preparation_depot_bas(void){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB8);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Preparation_depot_haut */
+/* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */
+/****************************************************************************************/
+void Preparation_depot_haut(void){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB6);
+ wait(TIME);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB5);
+ wait(TIME);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB10);
+ wait(TIME);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB8);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Pousser_module */
+/* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */
+/****************************************************************************************/
+void Pousser_module(void){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB11);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Initialisation_gauche */
+/* DESCRIPTION : Fonction qui permet de placer le cote gauche en position initiale */
+/****************************************************************************************/
+void Initialisation_gauche(void){
+ trois_myAX12_2->Set_Secure_Goal(235);
+ multiple_myAX12_2->multiple_goal_and_speed(4,TAB22);
+ wait(TIME);
+ multiple_myAX12_2->multiple_goal_and_speed(4,TAB21);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Preparation_prise_gauche */
+/* DESCRIPTION : Fonction qui permet de preparer la recuperation d'un module */
+/****************************************************************************************/
+void Preparation_prise_gauche(void){
+ trois_myAX12_2->Set_Secure_Goal(120);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Prendre_module_gauche */
+/* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */
+/****************************************************************************************/
+void Prendre_module_gauche(void){
+ trois_myAX12_2->Set_Secure_Goal(160);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Preparation_module_gauche */
+/* DESCRIPTION : Fonction qui prepare le tournante */
+/****************************************************************************************/
+void Preparation_module_gauche(void){
+ multiple_myAX12_2->multiple_goal_and_speed(4,TAB22);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Tourner_module_gauche */
+/* DESCRIPTION : Fonction qui permet de tourner les modules a gauche */
+/****************************************************************************************/
+void Tourner_module_gauche(void){
+ multiple_myAX12_2->multiple_goal_and_speed(4,TAB23);
+ wait(TIME);
+}