Revision 0:0cb11adecfc6, committed 2020-06-02

Comitter:

maximilienlv

Date:

Tue Jun 02 06:44:31 2020 +0000

Commit message:

programme de la carte esclave des bras, debugeur

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Actionneurs/herkulex_rob.cpp	Tue Jun 02 06:44:31 2020 +0000
@@ -0,0 +1,11 @@
+#include "herkulex_rob.h"
+
+//---------------------------------------------------------------------------------------------
+void deverouillage_torque(void)  //débloquer les servomoteurs
+{
+            setTorque(1 ,TORQUE_ON ,1);
+            setTorque(1 ,TORQUE_ON ,2);
+            setTorque(1 ,TORQUE_ON ,3);
+            setTorque(1 ,TORQUE_ON ,4);
+            setTorque(1 ,TORQUE_ON ,5);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Actionneurs/herkulex_rob.h	Tue Jun 02 06:44:31 2020 +0000
@@ -0,0 +1,7 @@
+#ifndef HERKULEX_H
+#define HERKULEX_H
+#include "main.h"
+
+void deverouillage_torque(void);
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Asservissement/Asservissement.cpp	Tue Jun 02 06:44:31 2020 +0000
@@ -0,0 +1,286 @@
+#include "Asservissement.h"
+
+/*********************************************************************************************************/
+/* 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;
+    can.write(msgTx);
+    wait_us(200);
+}
+
+/*********************************************************************************************/
+/* FUNCTION NAME: SendAck                                                                    */
+/* DESCRIPTION  : Envoyer un acknowledge                                                     */
+/*********************************************************************************************/
+void SendAck(unsigned short id, unsigned short from)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=id;
+    msgTx.len=2;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    // from sur 2 octets
+    msgTx.data[0]=(unsigned char)from;
+    msgTx.data[1]=(unsigned char)(from>>8);
+
+    can.write(msgTx);
+}
+
+
+void Send2Short(unsigned short id, unsigned short d1, unsigned short d2)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=id;
+    msgTx.len=4;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    // from sur 2 octets
+    msgTx.data[0]=(unsigned char)d1;
+    msgTx.data[1]=(unsigned char)(d1>>8);
+    msgTx.data[2]=(unsigned char)d2;
+    msgTx.data[3]=(unsigned char)(d2>>8);
+
+    can.write(msgTx);
+}
+
+void Send2Char(unsigned short id, unsigned char d1, unsigned char d2)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=id;
+    msgTx.len=2;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    // from sur 2 octets
+    msgTx.data[0]=(unsigned char)d1;
+    msgTx.data[1]=(unsigned char)d2;
+
+    can.write(msgTx);
+}
+
+void SendMsgCan(unsigned short id, unsigned char* data, int len)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=id;
+    msgTx.len=len;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    // from sur 2 octets
+    for(int i = 0; i<len; i++)
+    {
+        msgTx.data[i]=data[i];
+    }
+
+    can.write(msgTx);
+}
+void SendCharCan(unsigned short id, unsigned char data)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=id;
+    msgTx.len=1;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    msgTx.data[0]=data;
+    
+    can.write(msgTx);
+}
+
+
+
+/*********************************************************************************************/
+/* FUNCTION NAME: GoToPosition                                                               */
+/* DESCRIPTION  : Transmission CAN correspondant à un asservissement en position (x,y,theta) */
+/*********************************************************************************************/
+void GoToPosition (unsigned short x,unsigned short y,signed short theta,signed char sens)
+{
+    //id_to_expect=ACK_CONSIGNE;
+
+    CANMessage msgTx=CANMessage();
+    msgTx.id=ASSERVISSEMENT_XYT; // tx nouvelle position en (x,y,theta)
+    msgTx.len=7;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    // x sur 2 octets
+    msgTx.data[0]=(unsigned char)x;
+    msgTx.data[1]=(unsigned char)(x>>8);
+    // y sur 2 octets
+    msgTx.data[2]=(unsigned char)y;
+    msgTx.data[3]=(unsigned char)(y>>8);
+    // theta signé sur 2 octets
+    msgTx.data[4]=(unsigned char)theta;
+    msgTx.data[5]=(unsigned char)(theta>>8);
+    msgTx.data[6]=sens;
+
+    can.write(msgTx);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Rotate                                                                */
+/* DESCRIPTION  : Transmission CAN correspondant à une rotation                         */
+/****************************************************************************************/
+void Rotate (signed short angle)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=ASSERVISSEMENT_ROTATION;  // Tx rotation autour du centre du robot
+    msgTx.len=2;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    //  Angle signé sur 2 octets
+    msgTx.data[0]=(unsigned char)angle;
+    msgTx.data[1]=(unsigned char)(angle>>8);
+
+    can.write(msgTx);
+}
+
+
+/*********************************************************************************************/
+/* FUNCTION NAME: GoStraight                                                                 */
+/* DESCRIPTION  : Transmission CAN correspondant à une ligne droite, avec ou sans recalage   */
+/*  recalage : 0 => pas de recalage                                                          */
+/*             1 => recalage en X                                                            */
+/*             2 => Recalage en Y                                                            */
+/*  newValue : Uniquement en cas de recalage, indique la nouvelle valeur de l'odo            */
+/*  isEnchainement : Indique si il faut executer l'instruction en enchainement               */
+/*                   0 => non                                                                */
+/*                   1 => oui                                                                */
+/*                   2 => dernière instruction de l'enchainement                             */
+/*********************************************************************************************/
+void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=ASSERVISSEMENT_RECALAGE;
+    msgTx.len=6;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    // x sur 2 octets
+    msgTx.data[0]=(unsigned char)distance;
+    msgTx.data[1]=(unsigned char)(distance>>8);
+    //Recalage sur 1 octet
+    msgTx.data[2]=recalage;
+    //Valeur du recalage sur 2 octets
+    msgTx.data[3]=(unsigned char)newValue;
+    msgTx.data[4]=(unsigned char)(newValue>>8);
+    //Enchainement sur 1 octet
+    msgTx.data[5]=isEnchainement;
+
+    can.write(msgTx);
+    //wait_ms(500);
+}
+
+/********************************************************************************************/
+/* FUNCTION NAME: BendRadius                                                                */
+/* DESCRIPTION  : Transmission CAN correspondant à un rayon de courbure                     */
+/********************************************************************************************/
+void BendRadius (unsigned short rayon,signed short angle,signed char sens, unsigned char enchainement)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=ASSERVISSEMENT_COURBURE; // tx asservissement rayon de courbure
+    msgTx.len=6;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    // Rayon sur 2 octets
+    msgTx.data[0]=(unsigned char)rayon;
+    msgTx.data[1]=(unsigned char)(rayon>>8);
+    // Angle signé sur 2 octets
+    msgTx.data[2]=(unsigned char)angle;
+    msgTx.data[3]=(unsigned char)(angle>>8);
+    // Sens signé sur 1 octet
+    msgTx.data[4]=sens;
+    // Enchainement sur 1 octet
+    msgTx.data[5]=enchainement;
+
+    can.write(msgTx);
+}
+
+void SetOdometrie (unsigned short canId, unsigned short x,unsigned short y,signed short theta)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=canId;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    msgTx.len=6;
+
+        // x sur 2 octets
+    msgTx.data[0]=(unsigned char)x;
+    msgTx.data[1]=(unsigned char)(x>>8);
+    // y sur 2 octets
+    msgTx.data[2]=(unsigned char)y;
+    msgTx.data[3]=(unsigned char)(y>>8);
+    // theta signé sur 2 octets
+    msgTx.data[4]=(unsigned char)theta;
+    msgTx.data[5]=(unsigned char)(theta>>8);
+
+    can.write(msgTx);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: setAsservissementEtat                                                 */
+/* DESCRIPTION  : Activer ou désactiver l'asservissement                                */
+/****************************************************************************************/
+void setAsservissementEtat(unsigned char enable)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=ASSERVISSEMENT_ENABLE;  // Tx rotation autour du centre du robot
+    msgTx.len=1;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    //  Angle signé sur 2 octets
+    msgTx.data[0]=(unsigned char)((enable==0)?0:1);
+
+    can.write(msgTx);
+}
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: SendSpeed                                                             */
+/* DESCRIPTION  : Envoie un asservissement paramètre retournant à une vitesse           */
+/****************************************************************************************/
+void SendSpeed (unsigned short vitesse, unsigned short acceleration,unsigned short deceleration)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=ASSERVISSEMENT_CONFIG;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    msgTx.len=8;
+    msgTx.data[0]=(unsigned char)(vitesse&0x00FF);
+    msgTx.data[1]=(unsigned char)((vitesse&0xFF00)>>8);
+    
+    msgTx.data[2]=(unsigned char)(acceleration&0x00FF);
+    msgTx.data[3]=(unsigned char)((acceleration&0xFF00)>>8);
+    
+    msgTx.data[4]=(unsigned char)(deceleration&0x00FF);
+    msgTx.data[5]=(unsigned char)((deceleration&0xFF00)>>8);
+    
+    msgTx.data[6]=(unsigned char)(acceleration&0x00FF);//cloto
+    msgTx.data[7]=(unsigned char)((acceleration&0xFF00)>>8);//cloto
+
+    can.write(msgTx);
+        
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: SendSpeedDecel                                                        */
+/* DESCRIPTION  : Envoie un asservissement paramètre retournant à une vitesse           */
+/****************************************************************************************/
+ /*
+void SendSpeedDecel (unsigned short vitesse, unsigned short deceleration)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=ASSERVISSEMENT_CONFIG_DECEL;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    msgTx.len=4;
+    msgTx.data[0]=(unsigned char)(vitesse&0x00FF);
+    msgTx.data[1]=(unsigned char)((vitesse&0xFF00)>>8);
+    msgTx.data[2]=(unsigned char)(deceleration&0x00FF);
+    msgTx.data[3]=(unsigned char)((deceleration&0xFF00)>>8);
+ 
+    can.write(msgTx);
+        
+}*/
+   
+   
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Asservissement/Asservissement.h	Tue Jun 02 06:44:31 2020 +0000
@@ -0,0 +1,74 @@
+#ifndef CRAC_ASSERVISSEMENT
+#define CRAC_ASSERVISSEMENT
+#include "main.h"
+
+void Send2Char(unsigned short id, unsigned char d1, unsigned char d2);
+void Send2Short(unsigned short id, unsigned short d1, unsigned short d2);
+void SendMsgCan(unsigned short id, unsigned char* data, int len);
+void SendCharCan(unsigned short id, unsigned char data);
+/*********************************************************************************************************/
+/* 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);
+
+/*********************************************************************************************/
+/* FUNCTION NAME: SendAck                                                                    */
+/* DESCRIPTION  : Envoyer un acknowledge                                                     */
+/*********************************************************************************************/
+void SendAck(unsigned short id, unsigned short from);
+
+/*********************************************************************************************/
+/* FUNCTION NAME: GoToPosition                                                               */
+/* DESCRIPTION  : Transmission CAN correspondant à un asservissement en position (x,y,theta) */
+/*********************************************************************************************/
+
+void GoToPosition (unsigned short x,unsigned short y,signed short theta,signed char sens);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Rotate                                                                */
+/* DESCRIPTION  : Transmission CAN correspondant à une rotation                         */
+/****************************************************************************************/
+
+void Rotate (signed short angle);
+
+/*********************************************************************************************/
+/* FUNCTION NAME: GoStraight                                                                 */
+/* DESCRIPTION  : Transmission CAN correspondant à une ligne droite, avec ou sans recalage   */
+/*  recalage : 0 => pas de recalage                                                          */
+/*             1 => recalage en X                                                            */
+/*             2 => Recalage en Y                                                            */
+/*  newValue : Uniquement en cas de recalage, indique la nouvelle valeur de l'odo            */
+/*  isEnchainement : Indique si il faut executer l'instruction en enchainement               */
+/*                   0 => non                                                                */
+/*                   1 => oui                                                                */
+/*                   2 => dernière instruction de l'enchainement                             */
+/*********************************************************************************************/
+void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement);
+
+/********************************************************************************************/
+/* FUNCTION NAME: BendRadius                                                                */
+/* DESCRIPTION  : Transmission CAN correspondant à un rayon de courbure                     */
+/********************************************************************************************/
+void BendRadius (unsigned short rayon,signed short angle,signed char sens, unsigned char enchainement);
+
+void SetOdometrie (unsigned short canId, unsigned short x,unsigned short y,signed short theta);
+
+/****************************************************************************************/
+/* FUNCTION NAME: setAsservissementEtat                                                 */
+/* DESCRIPTION  : Activer ou désactiver l'asservissement                                */
+/****************************************************************************************/
+void setAsservissementEtat(unsigned char enable);
+
+/****************************************************************************************/
+/* FUNCTION NAME: SendSpeed                                                             */
+/* DESCRIPTION  : Envoie un asservissement paramètre retournant à une vitesse           */
+/****************************************************************************************/
+void SendSpeed (unsigned short vitesse, unsigned short acceleration,unsigned short deceleration);
+/****************************************************************************************/
+/* FUNCTION NAME: SendSpeedDecel                                                        */
+/* DESCRIPTION  : Envoie un asservissement paramètre retournant à une vitesse           */
+/****************************************************************************************/
+void SendSpeedDecel (unsigned short vitesse, unsigned short deceleration);
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Capteurs/Capteur.cpp	Tue Jun 02 06:44:31 2020 +0000
@@ -0,0 +1,33 @@
+#include "Capteur.h"
+
+/////////////////////////////////////Lecture Batterie////////////////////////////
+AnalogIn Val_batterie(PC_2);
+/////////////////////////////////////Capteurs monocouleur////////////////////////
+DigitalIn monocouleur1(PC_15);
+DigitalIn monocouleur2(PC_14);
+DigitalIn monocouleur3(PB_10);
+DigitalIn monocouleur4(PC_3);
+
+short distance_moyenne;
+
+short lecture_telemetre(char numero_telemetre)  // DEGUEUX MAIS FONCTIONNEL :')
+{
+    switch(numero_telemetre) {
+        case 1:
+            distance_moyenne=(short)DT1_trait_Ex;
+            break;
+
+        case 2:
+            distance_moyenne=(short)DT2_trait_Ex;
+            break;
+
+        case 3:
+            distance_moyenne=(short)DT3_trait_Ex;
+            break;
+
+        case 4:
+            distance_moyenne=(short)DT4_trait_Ex;
+            break;
+    }
+    return distance_moyenne;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Capteurs/Capteur.h	Tue Jun 02 06:44:31 2020 +0000
@@ -0,0 +1,7 @@
+#ifndef CAPTEUR_H
+#define CAPTEUR_H
+#include "main.h"
+
+short lecture_telemetre(char numero_telemetre);
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Capteurs/dt.cpp	Tue Jun 02 06:44:31 2020 +0000
@@ -0,0 +1,106 @@
+#include "dt.h"
+//RawSerial pc(USBTX, USBRX,9600);//serie pour debug
+
+AnalogIn DT1(PB_0);
+AnalogIn DT2(PC_5);
+AnalogIn DT3(PC_4);
+AnalogIn DT4(PA_5);
+
+InterruptIn DT1_isr(PB_1);
+InterruptIn DT2_isr(PA_7);
+InterruptIn DT3_isr(PA_6);
+InterruptIn DT4_isr(PA_4);
+
+Timer t;
+
+double DT1_mes[256]= {0};
+double DT1_trait[256]= {0};
+double DT1_trait_Ex;
+
+double DT2_mes[256]= {0};
+double DT2_trait[256]= {0};
+double DT2_trait_Ex;
+
+double DT3_mes[256]= {0};
+double DT3_trait[256]= {0};
+double DT3_trait_Ex;
+
+double DT4_mes[256]= {0};
+double DT4_trait[256]= {0};
+double DT4_trait_Ex;
+
+unsigned char DT1_interrupt_Ex;
+unsigned char DT2_interrupt_Ex;
+unsigned char DT3_interrupt_Ex;
+unsigned char DT4_interrupt_Ex;
+
+unsigned char n = 0;
+bool flag_t=0;
+
+void f_mesure()
+{
+    if(!flag_t) {
+        t.start();
+        flag_t=1;
+    }
+
+    if (t.read_ms() >= 4) {
+        n++;
+        DT1.read();//lecture dans le vide le temp que l'adc switch
+        wait_us(100);//attente du switch de l'adc
+        DT1_mes[n]= DT1.read() * A + B;
+        DT1_trait[n] = mediane(DT1_mes, 10);
+        DT1_trait_Ex = DT1_trait[n];
+
+        DT2.read();//lecture dans le vide le temp que l'adc switch
+        wait_us(100);//attente du switch de l'adc
+        DT2_mes[n]= DT2.read() * A + B;
+        DT2_trait[n] = mediane(DT2_mes, 10);
+        DT2_trait_Ex = DT2_trait[n];
+
+        DT3.read();//lecture dans le vide le temp que l'adc switch
+        wait_us(100);//attente du switch de l'adc
+        DT3_mes[n]= DT3.read() * A + B;
+        DT3_trait[n] = mediane(DT3_mes, 10);
+        DT3_trait_Ex = DT3_trait[n];
+
+        DT4.read();//lecture dans le vide le temp que l'adc switch
+        wait_us(100);//attente du switch de l'adc
+        DT4_mes[n]= DT4.read() * A + B;
+        DT4_trait[n] = mediane(DT4_mes, 10);
+        DT4_trait_Ex = DT4_trait[n];
+
+        t.reset();
+    }
+
+}
+
+double mediane(double* buff_med, int size_med)
+{
+    double DT_med[30]= {0};
+    for(unsigned char i =0; i< size_med; i++)DT_med[i] = buff_med[(unsigned char)(n-i)];
+
+    tri(DT_med, size_med);
+    return DT_med[(int)(size_med/2)];
+}
+
+void tri(double* tab, int size)
+{
+    for (int i=0; i<size; i++) {
+        for(int j=i; j<size; j++) {
+            if(tab[j]<tab[i]) {
+                double temp = tab[i];
+                tab[i] = tab[j];
+                tab[j] = temp;
+            }
+        }
+    }
+}
+
+void interrupt()
+{
+    DT1_interrupt_Ex = DT1_isr.read();
+    DT2_interrupt_Ex = DT2_isr.read();
+    DT3_interrupt_Ex = DT3_isr.read();
+    DT4_interrupt_Ex = DT4_isr.read();
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Capteurs/dt.h	Tue Jun 02 06:44:31 2020 +0000
@@ -0,0 +1,37 @@
+#ifndef DT_H
+#define DT_H
+#include "main.h"
+
+#define V_min 0.561  //Tension (V) minimale prélevée sur la résistance
+#define V_max 3.282   //Tension (V) maximale prélevée sur la résistance
+
+#define D_min 50     //Distance (mm) minimale mesurée par le capteur
+#define D_max 1500   //Distance (mm) maximale mesurée par le capteur
+
+#define Nb_echantillon 5 //Nombre de valeurs utilisées pour le moyennage 
+
+#define Correction 0 //Correction apportée sur la formule
+
+#define D_ROULEAU_AXE 200
+
+#define Conv (V_max - V_min)/((V_max/3.3)-(V_min/3.3))
+
+#define A (D_max - D_min)/(V_max - V_min)*Conv
+#define B (D_max - ((D_max - D_min)/(V_max - V_min))*V_max) + Correction
+
+
+void f_mesure(void);
+double mediane(double* buff_med, int size_med);
+void tri(double* tab, int size);
+void interrupt(void);
+
+extern double DT1_trait_Ex;
+extern double DT2_trait_Ex;
+extern double DT3_trait_Ex;
+extern double DT4_trait_Ex;
+
+extern unsigned char DT1_interrupt_Ex;
+extern unsigned char DT2_interrupt_Ex;
+extern unsigned char DT3_interrupt_Ex;
+extern unsigned char DT4_interrupt_Ex;
+#endif 
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Herkulex_Library_2019.lib	Tue Jun 02 06:44:31 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/CRAC-Team/code/Herkulex_Library_2019/#a6e5d1ce2133

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/actions/actions_Pr.cpp	Tue Jun 02 06:44:31 2020 +0000
@@ -0,0 +1,30 @@
+#include "actions_Pr.h"
+
+uint8_t servos_bras_testeur[2] = {RLED_ON, 1}; 
+uint16_t pos_testeur[1] = {400};
+    
+
+void test_BRAS_1(void)
+{
+        positionControl_Mul_ensemble_complex(1, 100, servos_bras_testeur, pos_testeur, 1);
+}
+
+void test_BRAS_2(void)
+{
+        positionControl_Mul_ensemble_complex(1, 100, servos_bras_testeur, pos_testeur, 2);
+}
+
+void test_BRAS_3(void)
+{
+        positionControl_Mul_ensemble_complex(1, 100, servos_bras_testeur, pos_testeur, 3);
+}
+
+void test_BRAS_4(void)
+{
+        positionControl_Mul_ensemble_complex(1, 100, servos_bras_testeur, pos_testeur, 4);
+}
+
+void test_BRAS_5(void)
+{
+        positionControl_Mul_ensemble_complex(1, 100, servos_bras_testeur, pos_testeur, 5);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/actions/actions_Pr.h	Tue Jun 02 06:44:31 2020 +0000
@@ -0,0 +1,10 @@
+#ifndef ACTIONS_PR_H
+#define ACTIONS_PR_H
+#include "main.h"
+void test_BRAS_1(void);
+void test_BRAS_2(void);
+void test_BRAS_3(void);
+void test_BRAS_4(void);
+void test_BRAS_5(void);
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ident_crac_testeur/ident_crac.h	Tue Jun 02 06:44:31 2020 +0000
@@ -0,0 +1,260 @@
+#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_JACK 0x008
+#define ACKNOWLEDGE_JACK 0X009
+#define RECALAGE_START 0x010//on a commencé le recalage de début
+ 
+#define BALISE_STOP 0x003  // Trame stop
+ 
+#define BALISE_DANGER 0xA  // Trame danger
+ 
+#define BALISE_END_DANGER 0xB  // Trame fin de danger
+ 
+ 
+#define ASSERVISSEMENT_ERROR_MOTEUR 0x025  // robot s'arrete car percuté quelque chose
+ 
+//------------------------------------Asservissement -------------------------------------------------------------
+ 
+#define ASSERVISSEMENT_ENABLE 0x1F7  // Activation asservissement  (0 : désactivation, 1 : activation)
+ 
+ 
+#define ASSERVISSEMENT_STOP 0x001  // Stop moteur
+#define ASSERVISSEMENT_XYT 0x020  // Asservissement (x,y,theta)  (0 : au choix 1 : avant -1 : arrière)
+#define ASSERVISSEMENT_XYT_ROTATE 0x030//premiere rotation durant xy theta
+#define ASSERVISSEMENT_XYT_LINE 0x040//ligne droite durant xy theta
+#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_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 ASSERVISSEMENT_BEZIER 0x02A
+   
+   
+#define ODOMETRIE_BIG_VITESSE 0x027  // Odométrie vitesse  (Indication sur l'état actuel)
+#define ODOMETRIE_SMALL_VITESSE 0x027  // Odométrie vitesse  (Indication sur l'état actuel)
+ 
+#define ODOMETRIE_BIG_POSITION 0x028  // Odométrie position robot  (Position actuel du robot)
+#define ODOMETRIE_SMALL_POSITION 0x026  // Odométrie position robot  (Position actuel du robot)
+ 
+//#define ODOMETRIE 0x026  // Odométrie position robot  (Position actuel du robot)
+//#define ODOMETRIE_DBUG 0x028  // Odométrie position robot  (Position actuel du robot) 
+ 
+ 
+ 
+ 
+#define ASSERVISSEMENT_CONFIG 0x022  // Asservissement paramètre  (définir les valeurs de vitesse max et d'eccélération max)
+ 
+#define ASSERVISSEMENT_CONFIG_VIT 0x031 
+#define ASSERVISSEMENT_CONFIG_ACCEL 0x032 
+ 
+#define ASSERVISSEMENT_SPEED_DANGER 0x006  // Vitesse de danger
+ 
+ 
+#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_CONFIG_KPP 0x710  // Config coef KPP
+#define ASSERVISSEMENT_CONFIG_KPI 0x711  // Config coef KPI
+#define ASSERVISSEMENT_CONFIG_KPD 0x712  // Config coef KPD
+#define ASSERVISSEMENT_CONFIG_LARG 0x713
+#define ASSERVISSEMENT_CONFIG_PERIM 0x714
+#define ASSERVISSEMENT_CONFIG_COEF_G 0x715
+#define ASSERVISSEMENT_CONFIG_COEF_D 0x716
+ 
+                //--------------Débug asserv --------
+       
+#define ASSERVISSEMENT_ERREUR 0x025    
+    
+#define ID_FIN_CLOTHO 0x501
+ 
+ 
+    
+#define ID_ENTRAXE 0x510
+#define ID_RAYON 0x511
+#define ID_ALPHA 0x512
+#define ID_VITESSE 0x513
+#define ID_ACCELERATION 0x514
+#define ID_TCLOTHO 0x515
+#define ID_TARC 0x516
+#define ID_TEMPS 0x517
+#define ID_VIT 0x518
+#define ID_VIT1 0x519
+#define ID_POS 0x520
+#define ID_POS1 0x521
+#define ID_T_CALCUL 0x522
+ 
+    
+#define ERREUR_TEMP_CALCUL 0x5A0
+ 
+#define ID_DBUG_ETAT 0x5A1
+#define ID_DBUG_ETAT_DPL 0x5A2
+    
+    
+#define ID_DBUG_LIGNE_TPS 0x5A3
+#define ID_DBUG_LIGNE_PCONS 0x5A4
+#define ID_DBUG_LIGNE_VIT 0x5A5
+#define ID_DIST_TIC_GENE 0x5A6  
+#define ID_TEMPS_CALCUL_CLOTHO 0x5A7
+#define ID_DBUG_LIGNE_GENE_VIT 0x5A8
+#define ID_CLOTHO_IMPOSSIBLE 0x5A9
+    
+#define ID_TRAIT_LIGNE_GENE 0x5C0
+#define ID_TRAIT_CLOTHO 0x5C1
+#define ID_TRAIT 0x5C2
+    
+#define ID_TEMPS_LONG_1 0x5C3
+#define ID_TEMPS_LONG_2 0x5C4
+    
+#define ID_TEST_VITESSE 0x5C5
+    
+#define ID_REACLLAGE_AVANT 0x5C6
+ 
+//////////////////////////////////////////////////////////RESETS///////////////////////////////////////////////////
+#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
+ 
+//////////////////////////////////////////////////////////CHECK CARTES/////////////////////////////////////////////////
+#define CHECK_BALISE 0x060  // Check balise
+#define CHECK_MOTEUR 0x061  // Check moteur
+#define CHECK_IHM 0x062  // Check écran tactile
+#define CHECK_ACTIONNEURS_AVANT 0x063  // Check actionneurs
+#define CHECK_ACTIONNEURS_ARRIERE 0x064  // Check pompes
+#define CHECK_AX12 0x065  // Check AX12
+#define CHECK_OK_TELEMETRE 0x066 // Check telemetre
+ 
+//////////////////////////////////////////////////////////ACK CARTES///////////////////////////////////////////////////
+#define ALIVE_BALISE 0x070  // Alive balise
+#define ALIVE_MOTEUR 0x071  // Alive moteur
+#define ALIVE_IHM 0x072  // Alive écran tactile
+#define ALIVE_ACTIONNEURS_AVANT 0x073  // Alive actionneurs
+#define ALIVE_ACTIONNEURS_ARRIERE 0x074  // Alive pompes
+#define ALIVE_AX12 0x075  // Alive AX12
+#define ALIVE_TELEMETRE 0x076 // Alive telemetre
+ 
+ 
+/////////////////////////////////////////////////////ACTIONS COMPLEXES/////////////////////////////////////////////////
+#define MONTER_IMMEUBLE_DOUBLE 0x090  // Monte deux immeubles selon un code couleur
+#define MONTER_IMMEUBLE 0x091
+#define ACK_ACTION 0x99 //autre action possible via les herkulex, ne peut pas passer en sendrawid
+ 
+/////////////////////////////////////////////////////////ACKS////////////////////////////////////////////////////////////
+#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_TELEMETRE 0x105 // Acknowledge telemetre
+#define ACKNOWLEDGE_HERKULEX 0x106 // Ack ax12
+#define ACKNOWLEDGE_STRAT 0x10A  // Acknowledge pompes
+#define ACKNOWLEDGE_CAMERA 0x108 //Acknowledge couleur caméra
+ 
+ 
+#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 ACK_FIN_ACTION 0x116
+ 
+ 
+ 
+/////////////////////////////////////////////////////////ERREURS////////////////////////////////////////////////////////
+#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
+ 
+  
+/////////////////////////////////////////////////ACTIONS SIMPLES DU ROBOT/////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+#define ACCELERATEUR_INSERTION_AVANT_GAUCHE 0X218
+#define ACCELERATEUR_INSERTION_ARRIERE_GAUCHE 0X219
+ 
+ 
+///////////////////////////////////////////CAPTEURS///////////////////////////////////////////////////////////////////
+#define DATA_TELEMETRE 0x3B0        // Demande sa valeur à un télémètre parmis les 
+#define RECEPTION_DATA 0x3B1        // envoi de la valeur d'un des télémètres
+#define TELEMETRE_OBJET 0x3B2
+#define OBJET_SUR_TABLE 0x3B3
+#define RECEPTION_RECALAGE 0x3B5    //Valeur des télémètres 
+#define DATA_RECALAGE 0x3B6         //Demande de la valeur de tous les télémètres afin de procèder au récalage
+#define LIRE_PANNEAU 0x3B7
+#define VIBRO 0x3B8
+ 
+#define DATA_TELEMETRE_LOGIQUE 0x3B9
+#define RECEPTION_TELEMETRE_LOGIQUE 0x3B0
+
+///////////////////////////////////////////////////ENVOI DE PARAMETRES//////////////////////////////////////////////////
+#define CHOICE_COLOR 0x602  // Couleur  (0->VERT;1->ORANGE)
+#define RECEPTION_COULEUR 0x603 //Code Couleur
+#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)
+ 
+///////////////////////////////////////////////////////////DEBUGS///////////////////////////////////////////////////////
+#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
+
+ 
+//////////////////////////////////////////////////////////ACK CARTES///////////////////////////////////////////////////
+#define ALIVE_BALISE 0x070  // Alive balise
+#define ALIVE_MOTEUR 0x071  // Alive moteur
+#define ALIVE_IHM 0x072  // Alive écran tactile
+#define ALIVE_ACTIONNEURS_AVANT 0x073  // Alive actionneurs
+#define ALIVE_ACTIONNEURS_ARRIERE 0x074  // Alive pompes
+#define ALIVE_HERKULEX 0x075  // Alive AX12
+#define ALIVE_TELEMETRE 0x076 // Alive telemetre
+ 
+//////////////////////////////////////bras//////////////////////////////////////
+#define TEST_TELEMETRE_1 0x280
+#define TEST_TELEMETRE_2 0x281
+#define TEST_TELEMETRE_3 0x282
+#define TEST_TELEMETRE_4 0x283
+
+#define TELEMETRE_1_REP 0x284
+#define TELEMETRE_2_REP 0x285
+#define TELEMETRE_3_REP 0x286
+#define TELEMETRE_4_REP 0x287
+//ces tests permettent de tester les bras pour attraper puis relacher directement
+#define TEST_BRAS_1 0x254
+#define TEST_BRAS_2 0x255
+#define TEST_BRAS_3 0x256
+#define TEST_BRAS_4 0x257
+#define TEST_BRAS_5 0x258
+#define TEST_BRAS_6 0x259
+
+///////////////////////define des bras stratégie////////////////////////////////
+#define BRAS_AT 0x260
+#define BRAS_RE 0x261
+
+////////////////////////////////////// ventouse/électrovanne //////////////////////////////////
+//les ventouses fonctionnent avec les électrovannes
+#define VENT_AT 0x266
+#define VENT_RE 0x267
+//-----------------------------------define bras/ventouse valeur--------------------------//
+#define AV_DROIT    0
+#define AV_CENTRE   1
+#define AV_GAUCHE   2
+ 
+#define AR_DROIT    3
+#define AR_CENTRE   4
+#define AR_GAUCHE   5
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Jun 02 06:44:31 2020 +0000
@@ -0,0 +1,110 @@
+#include "main.h"
+
+#define SIZE_FIFO 50
+
+CAN can(PB_8,PB_9,1000000); // Rx&Tx pour le CAN
+
+Serial pc(USBTX,USBRX);
+
+CANMessage msgRxBuffer[SIZE_FIFO];
+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
+unsigned short ackFinAction = 0;
+
+void canProcessRx(void);
+
+
+/*********************************************************************************************/
+/* FUNCTION NAME: canRx_ISR                                                                  */
+/* DESCRIPTION  : lit les messages sur le can et les stocke dans la FIFO                     */
+/*********************************************************************************************/
+void canRx_ISR (void)
+{
+    if (can.read(msgRxBuffer[FIFO_ecriture])) 
+    {  
+        FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;        
+    }
+}
+
+int main()
+{
+    can.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
+    servo_interrupt_en(); //permettre les interuptions
+    wait_ms(200);
+    deverouillage_torque();
+    pc.printf("\nLAUNCHED\n\r");
+    while(1) 
+    {                    
+        canProcessRx();
+        f_mesure();
+    }
+}
+
+/****************************************************************************************/
+/* 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) {
+        int identifiant=msgRxBuffer[FIFO_lecture].id;
+
+        switch(identifiant) 
+        {         
+            case TEST_BRAS_1:
+                test_BRAS_1();
+                break;
+                 
+            case TEST_BRAS_2: 
+                test_BRAS_2();
+                break;
+                
+            case TEST_BRAS_3: 
+                test_BRAS_3();
+                break;
+                
+            case TEST_BRAS_4: 
+                test_BRAS_4();
+                break;
+                
+            case TEST_BRAS_5: 
+                test_BRAS_5();
+                break;
+                
+            case TEST_TELEMETRE_1:
+                short distance1=lecture_telemetre(1);
+                unsigned char distance_telemetre_1 = distance1;
+                SendMsgCan(TELEMETRE_1_REP, &distance_telemetre_1,1);
+                break;
+            case TEST_TELEMETRE_2:
+                short distance2=lecture_telemetre(2);
+                unsigned char distance_telemetre_2 = distance2;
+                SendMsgCan(TELEMETRE_2_REP, &distance_telemetre_2,1);
+                break;
+            case TEST_TELEMETRE_3:
+                short distance3=lecture_telemetre(3);
+                unsigned char distance_telemetre_3 = distance3;
+                SendMsgCan(TELEMETRE_3_REP, &distance_telemetre_3,1);
+                break;
+            case TEST_TELEMETRE_4:
+                short distance4=lecture_telemetre(4);
+                unsigned char distance_telemetre_4 = distance4;
+                SendMsgCan(TELEMETRE_4_REP, &distance_telemetre_4,1);
+                break;
+                
+            default:
+                break;
+        }
+        FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Tue Jun 02 06:44:31 2020 +0000
@@ -0,0 +1,18 @@
+#ifndef MAIN_H
+#define MAIN_H
+
+#include "mbed.h"
+#include "Asservissement.h"
+#include "ident_crac.h"
+#include "Capteur.h"
+#include "fonctions_herkulex.h"
+#include "herkulex_rob.h"
+#include "dt.h"
+#include "actions_Pr.h"
+
+
+extern CAN can;
+extern Serial pc;
+
+extern unsigned short ackFinAction;
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Jun 02 06:44:31 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file