I2C Cerveau: diagnostic du réseau, Actionneur: logique d'empilement

Actionneurs/Actionneurs.ino

@@ -1,19 +1,34 @@
 #include <SCServo.h>
+#include "Ascenseur.h"
 
 #define SERVO_FOUCHE 5
 #define SERVO_PINCE_GAUCHE 8
 #define SERVO_PINCE_DROITE 9
 
-#define FOURCHE_TRANSPORT 5, 307
-#define FOURCHE_LEVEE     5, 227
-#define FOURCHE_PRISE     5, 345
+#define FOURCHE_TRANSPORT 5, 287
+#define FOURCHE_LEVEE     5, 217
+#define FOURCHE_PRISE     5, 327
 
-#define DOIGT_PINCE_GAUCHE_FERME 2080
-#define DOIGT_PINCE_GAUCHE_OUVRE 2367
-#define DOIGT_PINCE_GAUCHE_PARKING 1550
+#define DOIGT_PINCE_GAUCHE_FERME 3, 1500
+#define DOIGT_PINCE_GAUCHE_OUVRE 3, 1842
+#define DOIGT_PINCE_GAUCHE_SEUIL 1515
 
-#define ID_FEETECH_ASC_G 11
-#define ID_FEETECH_ASC_D 10
+/// TODO renseigner de vraies valeurs
+#define DOIGT_PINCE_DROIT_FERME 4, 1000
+#define DOIGT_PINCE_DROIT_OUVRE 4, 800
+#define DOIGT_PINCE_DROIT_SEUIL 815
+/// FIN TODO
+
+#define AIMANT_PINCE_DROIT_TIENT 8, 147
+#define AIMANT_PINCE_DROIT_LACHE 8, 337
+
+#define AIMANT_PINCE_GAUCHE_TIENT 9, 500
+#define AIMANT_PINCE_GAUCHE_LACHE 9, 400
+
+#define I2C_CDE_DEMANDE 0x00
+#define I2C_DCE_REALISE 0x01
+
+volatile byte * I2C_memory;
 
 SMS_STS sms_sts;
 
@@ -24,42 +39,16 @@ struct position_t{
   uint position;
 } cible_haute, cible_basse, actuelle;
 
-
-/// @brief pilote la vitesse des moteurs
-/// @param vitesse vitesse signée, sera saturée à 255
-void Moteur_set(int vitesse){
-  ledcWrite(3, 0);
-  ledcWrite(4, 0);
-  if(vitesse < 0){
-    vitesse = -vitesse;
-    if(vitesse > 255){
-      vitesse = 255;
-    }
-    ledcWrite(3, vitesse);
-  }else{
-    if(vitesse > 255){
-      vitesse = 255;
-    }
-    ledcWrite(4, vitesse);
-  }
-}
-
-void Translateur_avance(){
-  Moteur_set(128);
-}
-
-void Translateur_recule(){
-  Moteur_set(-128);
-}
+enum etat_action_t Actionneur_empile(void);
 
 void setup()
 {
+  int liste_servo_id[4]= {3, 4, 10, 11};
   Serial.begin(115200);//sms舵机波特率115200
   Serial1.begin(1000000,SERIAL_8N1, RX, TX);//sts舵机波特率1000000
   sms_sts.pSerial = &Serial1;
-  delay(1000);
-  sms_sts.WheelMode(ID_Feetech);//舵机ID1切换至电机恒速模式
-  //sms_sts.WriteSpe(ID_Feetech, 2400, 100);
+  delay(5000);
+  
   cible_haute.nb_tour = 0;
   cible_haute.position = 500;
 
@@ -75,6 +64,9 @@ void setup()
   //ledcWrite(5, 227); // Position levée (butée haute)
   //ledcWrite(5, 345); // Position de prise
 
+  I2C_Slave_init(0x20);
+  I2C_memory = get_i2c_data();
+
 /*
   while(1){
     ledcWrite(FOURCHE_PRISE);
@@ -87,6 +79,18 @@ void setup()
     delay(5000);
 
   }*/
+  int error = 0;
+  do{
+    error = 0;
+    for (int i =0; i<4; i++){
+      int ID = sms_sts.Ping(liste_servo_id[i]);
+      printf("Servo %d: %d\n", liste_servo_id[i], ID );
+      if( ID != liste_servo_id[i]){
+        error = 1;
+        delay(100);
+      }
+    }
+  }while(error);
 
 
   // Servo pinces à aimant
@@ -95,12 +99,10 @@ void setup()
 
   ledcAttach(9, 50, 12);
   ledcWrite(9, 307);
+  
 
-  // Moteur
-  ledcAttach(3, 500, 8);
-  ledcWrite(3, 0);
-  ledcAttach(4, 500, 8);
-  ledcWrite(4, 0);
+  Translateur_init();
+  
 
   /*while(1){
 
@@ -123,6 +125,15 @@ void setup()
     delay(5000);
 
   }*/
+  /*
+  sms_sts.WheelMode(10);
+  sms_sts.WriteSpe(10, 2000, 50);
+  while(1){
+    printf(">load_d:%d\n", sms_sts.ReadLoad(10));
+    delay(25);
+  }*/
+
+
 
 
   if(sms_sts.FeedBack(ID_Feetech)!=-1){
@@ -135,97 +146,66 @@ void setup()
     actuelle.position = sms_sts.ReadPos(-1);
   }
 
-  
+  Ascenseur_init();  
 }
 
-void goto_position(int servo_id, position_t position){
-  // Actualise la position
-  struct position_t precedente;
-  static int loin = 1;
-  precedente = actuelle;
-  if(sms_sts.FeedBack(servo_id)!=-1){
-    actuelle.position = sms_sts.ReadPos(-1);
-  }
-  if(abs((int)actuelle.position - (int)precedente.position) > 2000){
-    // Rebouclage compteur
-    if(actuelle.position < precedente.position){
-      // Sens positif
-      actuelle.nb_tour++;
-    }else{
-      actuelle.nb_tour--;
-    }
-  }
-
-  // Sens de déplacement
-  if(actuelle.nb_tour != position.nb_tour){
-    if(loin != 1){
-      loin = 1;
-      sms_sts.WheelMode(servo_id);
-      if(actuelle.nb_tour < position.nb_tour){
-        sms_sts.WriteSpe(servo_id, -2400, 50);
-      }else{
-        sms_sts.WriteSpe(servo_id, 2400, 50);
+void reglage_servo(int pin_servo){
+  static int valeur=307; // Neutre
+  printf("reglage_servo %d\n", pin_servo);
+  while(1){
+    if (Serial.available() > 0) {
+      // get incoming byte:
+      int inByte = 0;
+      inByte = Serial.read();
+      if(inByte == 'e'){
+        valeur++;
       }
+      if(inByte == 'z'){
+        valeur+=10;
+      }
+      if(inByte == 'a'){
+        valeur+=100;
+      }
+      if(inByte == 'd'){
+        valeur--;
+      }
+      if(inByte == 's'){
+        valeur-=10;
+      }
+      if(inByte == 'f'){
+        valeur-=100;
+      }
+      if(inByte == 'q'){
+        break;
+      }
+      printf("Valeur %d\n", valeur);
+      ledcWrite(pin_servo, valeur);
     }
-  }else{
-    if(loin != 0){
-      loin = 0;
-      sms_sts.ServoMode(servo_id);
-      sms_sts.WritePosEx(ID_Feetech, position.position, 2400, 50);
-    }
+    Serial.printf(">Servo3:%d\n", sms_sts.ReadPos(3));
+    Serial.printf(">Servo4:%d\n", sms_sts.ReadPos(4));
+    delay(50);
   }
-  Serial.printf(">cible_pos:%d\n",position.position);
-  Serial.printf(">cible_nb_tour:%d\n",position.nb_tour);
-  Serial.printf(">act_pos:%d\n",actuelle.position);
-  Serial.printf(">act_nb_tour:%d\n",actuelle.nb_tour);
-
-
 
 }
 
+
+
 void loop()
 {
   static int m_pos=0;
   static unsigned long myTime=0;
   static position_t cible;
 
-  /*
-  //舵机(ID1)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，运行至P1=4095位置
-  sms_sts.WritePosEx(1, 4095, 2400, 50);
-  delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
-
-  //舵机(ID1)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，运行至P0=0位置
-  sms_sts.WritePosEx(1, 0, 2400, 50);
-  delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
-  */
-  Serial.print(">millis:");
-  Serial.println(millis());
-  if(millis() > myTime + 4000){
+  
+  
+  
+  if(millis() > myTime + 100){
     myTime = millis();
-    if(m_pos){
-      //sms_sts.WritePosEx(ID_Feetech, 4095, 2400, 50);
-      //sms_sts.WriteSpe(ID_Feetech, 2400, 50);
-      cible = cible_haute;
-      m_pos = 0;
-    }else{
-      m_pos ++;
-      //sms_sts.WriteSpe(ID_Feetech, -2400, 50);
-      //sms_sts.WritePosEx(ID_Feetech, 0, 2400, 50);
-      //sms_sts.WriteSpe(ID_Feetech, -80, 100);
-      cible = cible_basse;
-    }
-  }
-  goto_position(ID_Feetech, cible);
-  int ID = sms_sts.Ping(ID_Feetech);
-  if(ID!=-1){
-    Serial.print("Servo ID:");
-    Serial.println(ID, DEC);
-    delay(100);
-  }else{
-    Serial.println("Ping servo ID error!");
-    delay(2000);
+    Serial.print(">millis:");
+    Serial.println(millis());
   }
 
+
     int Pos;
   int Speed;
   int Load;
@@ -233,6 +213,131 @@ void loop()
   int Temper;
   int Move;
   int Current;
+
+  if(I2C_memory[I2C_CDE_DEMANDE] != I2C_memory[I2C_DCE_REALISE]){
+    switch (I2C_memory[I2C_CDE_DEMANDE]){
+      case 0:
+        // Aucune commande
+        I2C_memory[I2C_DCE_REALISE] = 0;
+        break;
+
+      case 1:
+        // Position de départ
+        break;
+
+      case 2: 
+        // Position de déplacement
+        // Fourche levée pour ne pas géner les capteurs
+        Servo_set(FOURCHE_LEVEE);
+        // Ascenseur en bas
+        Ascenseur_descend();
+        // Pinces internes fermées
+        Servo_set(DOIGT_PINCE_DROIT_FERME);
+        Servo_set(DOIGT_PINCE_GAUCHE_FERME);
+        // Aimant rentrés
+        Servo_set(AIMANT_PINCE_DROIT_LACHE);
+        Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
+        // Translateur à l'arrière
+        Translateur_recule();
+        I2C_memory[I2C_DCE_REALISE] = 2;
+        break;
+        
+      case 3:
+        // Prise initiale
+        // Fourche en position de prise
+        Servo_set(FOURCHE_PRISE);
+        // Ascenseur en bas
+        Ascenseur_descend();
+        // Pinces internes fermées
+        Servo_set(DOIGT_PINCE_DROIT_FERME);
+        Servo_set(DOIGT_PINCE_GAUCHE_FERME);
+        // Aimant rentrés
+        Servo_set(AIMANT_PINCE_DROIT_TIENT);
+        Servo_set(AIMANT_PINCE_GAUCHE_TIENT);
+        // Translateur à l'arrière
+        Translateur_recule();
+        I2C_memory[I2C_DCE_REALISE] = 3;
+
+      case 4:
+        // Position de transport
+    }
+
+  }
+
+
+  if (Serial.available() > 0) {
+    // get incoming byte:
+    int inByte = 0;
+    inByte = Serial.read();
+    if(inByte == 'd'){
+      Ascenseur_step_down();
+    }
+    if(inByte == 'u'){
+      Ascenseur_step_up();
+    }
+    if(inByte == 'm'){
+      Ascenseur_monte();
+    }
+    if(inByte == 'l'){
+      Ascenseur_descend();
+    }
+    if(inByte == 'a'){
+      Translateur_avance();
+    }
+    if(inByte == 'w'){
+      Servo_set(DOIGT_PINCE_GAUCHE_OUVRE);
+    }
+    if(inByte == 'x'){
+      Servo_set(DOIGT_PINCE_GAUCHE_FERME);
+    }
+    if(inByte == 'z'){
+      Translateur_recule();
+    }
+    if(inByte == '1'){
+      reglage_servo(8);
+    }
+    if(inByte == '2'){
+      reglage_servo(9);
+    }
+    if(inByte == '3'){
+      reglage_servo(5);
+    }
+    if(inByte == 'u'){
+      while(Actionneur_empile() == ACTION_EN_COURS);
+    }
+    if(inByte == 'v'){
+      Ascenseur_cycle();
+    }
+    if(inByte == 'c'){
+      Serial.println("Cycle translateur");
+      while(Serial.available() > 0){
+      inByte = Serial.read();
+      }
+      while(1){
+        Translateur_cycle();
+        if(Serial.available() > 0){
+          break;
+        }
+        Serial.printf(">GPIO2:%d\n",digitalRead(2));
+        Serial.printf(">GPIO10:%d\n",digitalRead(10));
+        delay(25);
+      }
+    }
+
+
+    while(Serial.available() > 0){
+      inByte = Serial.read();
+    }
+  }
+  Serial.printf(">Servo3:%d\n", sms_sts.ReadPos(3));
+  Serial.printf(">Servo4:%d\n", sms_sts.ReadPos(4));
+  Serial.printf(">GPIO2:%d\n",digitalRead(2));
+  Serial.printf(">GPIO10:%d\n",digitalRead(10));
+  Serial.printf(">OUT3:%d\n",digitalRead(3));
+  Serial.printf(">OUT4:%d\n",digitalRead(4));
+
+  Ascenseur_gestion();
+  /*
   if(sms_sts.FeedBack(ID)!=-1){
     Pos = sms_sts.ReadPos(-1);
     Speed = sms_sts.ReadSpeed(-1);
@@ -259,6 +364,151 @@ void loop()
   }else{
     Serial.println("FeedBack err");
     delay(500);
+  }*/
+
+  delay(25);
+}
+
+void Servo_set(int servo, int position){
+  switch(servo){
+    case 5:
+    case 8:
+    case 9:
+      ledcWrite(servo, position);
+      break;
+
+    case 3:
+    case 4:
+      sms_sts.WritePosEx(servo, position, 2000);
+      break;
+
   }
 }
 
+
+enum etat_action_t Actionneur_empile(){
+  static enum etat_actionneur_t{
+    ACTIONNEUR_DEPLACEMENT,
+    ACTIONNEUR_PRISE_EXTERNE,
+    ACTIONNEUR_DEPILE,
+    ACTIONNEUR_PRISE_INTERNE_1,
+    ACTIONNEUR_PRISE_INTERNE_2,
+    ACTIONNEUR_RECULE_PRISE_INTERNE_1,
+    ACTIONNEUR_RECULE_PRISE_INTERNE_2,
+    ACTIONNEUR_LEVE,
+    ACTIONNEUR_AVANCE_PRISE_INTERNE,
+    ACTIONNEUR_DEPOSE_PRISE_INTERNE,
+    ACTIONNEUR_RANGE_TRANSLATEUR,
+    ACTIONNEUR_DESCEND
+  } etat_actionneur=ACTIONNEUR_DEPLACEMENT;
+
+  switch(etat_actionneur){
+    case ACTIONNEUR_DEPLACEMENT:
+      Servo_set(FOURCHE_TRANSPORT);
+      Translateur_recule();
+      Servo_set(DOIGT_PINCE_GAUCHE_FERME);
+      Servo_set(DOIGT_PINCE_DROIT_FERME);
+      Servo_set(AIMANT_PINCE_DROIT_LACHE);
+      Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
+      delay(5000);
+      etat_actionneur = ACTIONNEUR_PRISE_EXTERNE;
+      break;
+
+    case ACTIONNEUR_PRISE_EXTERNE:
+      Servo_set(FOURCHE_PRISE);
+      Servo_set(AIMANT_PINCE_DROIT_LACHE);
+      Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
+      // Attente avance
+      delay(2000);
+      etat_actionneur = ACTIONNEUR_DEPILE;
+      break;
+
+    case ACTIONNEUR_DEPILE:
+      Servo_set(FOURCHE_LEVEE);
+      etat_actionneur = ACTIONNEUR_PRISE_INTERNE_1;
+      break;
+
+    case ACTIONNEUR_PRISE_INTERNE_1:
+      Serial.println("ACTIONNEUR_PRISE_INTERNE_1");
+      Translateur_avance();
+      
+      Servo_set(DOIGT_PINCE_GAUCHE_OUVRE);
+      Servo_set(DOIGT_PINCE_DROIT_OUVRE);
+      etat_actionneur = ACTIONNEUR_PRISE_INTERNE_2;
+      break;
+
+    case ACTIONNEUR_PRISE_INTERNE_2:
+      Serial.println("ACTIONNEUR_PRISE_INTERNE_2");
+      if(Translateur_etat() == ACTION_TERMINEE){
+        delay(500);
+        Servo_set(DOIGT_PINCE_GAUCHE_FERME);
+        Servo_set(DOIGT_PINCE_DROIT_FERME);
+        delay(500);
+        etat_actionneur = ACTIONNEUR_RECULE_PRISE_INTERNE_1;
+      }
+      break;
+
+    case ACTIONNEUR_RECULE_PRISE_INTERNE_1:
+      Serial.println("ACTIONNEUR_RECULE_PRISE_INTERNE_1");
+      Translateur_recule();
+      etat_actionneur = ACTIONNEUR_RECULE_PRISE_INTERNE_2;
+      break;
+
+    case ACTIONNEUR_RECULE_PRISE_INTERNE_2:
+      Serial.println("ACTIONNEUR_RECULE_PRISE_INTERNE_2");
+      if(Translateur_etat() == ACTION_TERMINEE){
+        etat_actionneur = ACTIONNEUR_LEVE;
+        Ascenseur_monte();
+      }
+      break;
+
+    case ACTIONNEUR_LEVE:
+      Serial.println("ACTIONNEUR_LEVE");
+      if(Ascenseur_get_etat() == ACTION_TERMINEE){
+        etat_actionneur = ACTIONNEUR_AVANCE_PRISE_INTERNE;
+        Translateur_avance();
+      }
+      break;
+
+    case ACTIONNEUR_AVANCE_PRISE_INTERNE:
+      Serial.println("ACTIONNEUR_AVANCE_PRISE_INTERNE");
+      if(Translateur_etat() == ACTION_TERMINEE){
+        delay(500);
+        Ascenseur_depose();
+        Servo_set(DOIGT_PINCE_DROIT_OUVRE);
+        Servo_set(DOIGT_PINCE_GAUCHE_OUVRE);
+        etat_actionneur = ACTIONNEUR_DEPOSE_PRISE_INTERNE;
+      }
+      break;
+
+    case ACTIONNEUR_DEPOSE_PRISE_INTERNE:
+      Serial.println("ACTIONNEUR_DEPOSE_PRISE_INTERNE");
+      if(Ascenseur_get_etat() == ACTION_TERMINEE){        
+        delay(500);
+        Translateur_recule();
+        etat_actionneur = ACTIONNEUR_RANGE_TRANSLATEUR;
+      }
+      break;
+
+    case ACTIONNEUR_RANGE_TRANSLATEUR:
+      Serial.println("ACTIONNEUR_RANGE_TRANSLATEUR");
+      if(Translateur_etat() == ACTION_TERMINEE){
+        etat_actionneur = ACTIONNEUR_DESCEND;
+        Servo_set(DOIGT_PINCE_DROIT_FERME);
+        Servo_set(DOIGT_PINCE_GAUCHE_FERME);
+        Ascenseur_descend();
+      }
+      break;
+
+    case ACTIONNEUR_DESCEND:
+      Serial.println("ACTIONNEUR_DESCEND");
+      if(Ascenseur_get_etat() == ACTION_TERMINEE){
+        etat_actionneur = ACTIONNEUR_DEPLACEMENT;
+        Servo_set(FOURCHE_TRANSPORT);
+        return ACTION_TERMINEE;
+      }
+      break;
+  }
+  Ascenseur_gestion(); 
+  return ACTION_EN_COURS;
+}

Actionneurs/Ascenseur.h

@@ -0,0 +1,15 @@
+enum etat_action_t{
+  ACTION_EN_COURS,
+  ACTION_TERMINEE,
+  ACTION_ECHEC
+};
+
+void Ascenseur_init(void);
+int Ascenseur_monte(void);
+int Ascenseur_descend(void);
+void Ascenseur_step_up(void);
+void Ascenseur_step_down(void);
+void Ascenseur_gestion(void);
+void Ascenseur_cycle(void);
+int Ascenseur_depose(void);
+enum etat_action_t Ascenseur_get_etat(void);

Actionneurs/Ascenseur.ino

@@ -0,0 +1,293 @@
+#define ID_FEETECH_ASC_G 11
+#define ID_FEETECH_ASC_D 10
+
+#define ASC_G_SIGNE 1
+#define ASC_D_SIGNE -1
+
+enum etat_ascenseur_t{
+  ASCENSEUR_INIT,
+  ASCENSEUR_ACTIF,
+  ASCENSEUR_MONTE,
+  ASCENSEUR_DESCENT,
+  ASCENSEUR_DEPOSE,
+  ASCENSEUR_BUSY,
+}etat_ascenseur=ASCENSEUR_INIT;
+
+// Fonction privée
+void Ascenseur_update_step(void);
+
+int step=0;
+
+u8 asc_ID[2] = {ID_FEETECH_ASC_D, ID_FEETECH_ASC_G};
+s16 tab_vitesses[2], tab_position[2];
+u16 tab_vitesses_u[2];
+u8 tab_acc[2];
+
+int position_basse_gauche, position_haute_gauche;
+int position_basse_droit, position_haute_droit;
+
+
+void Ascenseur_init(void){
+  tab_acc[0] = 50;
+  tab_acc[1] = 50;
+  Ascenseur_config_servo(ID_FEETECH_ASC_D);
+  Ascenseur_config_servo(ID_FEETECH_ASC_G);
+
+  while(Ascenseur_cherche_butees() != ACTION_TERMINEE);
+
+}
+
+void Ascenseur_config_servo(int id){
+  sms_sts.unLockEprom(id); // unlock EPROM-SAFE
+  sms_sts.writeByte(id, 30, 1); // Resolution à 1
+  sms_sts.writeByte(id, SMS_STS_MAX_ANGLE_LIMIT_L, 0); // Multitour 
+  sms_sts.writeByte(id, SMS_STS_MAX_ANGLE_LIMIT_H, 0); // Multitour 
+
+  sms_sts.LockEprom(id); // unlock EPROM-SAFE
+  Serial.printf("Servo ID : %d\n",id);
+  for(int i=0; i < 40; i++){
+    Serial.printf("Memoire %d : %d\n", i, sms_sts.readByte(ID_Feetech, i));
+  }
+
+}
+
+enum etat_action_t Ascenseur_cherche_butees(void){
+  static bool butee_g = 0, butee_d = 0;
+  static int old_pos_d, old_pos_g;
+  int pos_d, pos_g, my_load;
+
+  static enum {
+    CHERCHE_BUTEE_INIT,
+    CHERCHE_BUTEE,
+    EA_POS_INIT,
+  }etat_cherche_butees=CHERCHE_BUTEE_INIT;
+
+  switch(etat_cherche_butees){
+    case CHERCHE_BUTEE_INIT:
+      butee_g = 0;
+      butee_d = 0;
+      tab_vitesses[0] = 500 * ASC_D_SIGNE;
+      tab_vitesses[1] = 500 * ASC_G_SIGNE;
+      sms_sts.WheelMode(ID_FEETECH_ASC_D);
+      sms_sts.WheelMode(ID_FEETECH_ASC_G);
+      old_pos_d = sms_sts.ReadPos(ID_FEETECH_ASC_D); 
+      old_pos_g = sms_sts.ReadPos(ID_FEETECH_ASC_G);
+      sms_sts.SyncWriteSpe(asc_ID, 2, tab_vitesses, tab_acc);
+      delay(200);
+      etat_cherche_butees = CHERCHE_BUTEE;
+      Serial.printf("CHERCHE_BUTEE_INIT\n");
+      break;
+
+    case CHERCHE_BUTEE:
+      {
+      
+      /*pos_d = sms_sts.ReadPos(ID_FEETECH_ASC_D);
+      printf(">delta_pos_d:%d\n",pos_d - old_pos_d);
+      if(abs(pos_d - old_pos_d) < 10 ){*/
+      my_load = sms_sts.ReadLoad(ID_FEETECH_ASC_D);
+      printf(">load_d:%d\n", my_load);
+      if(abs(my_load) > 250 ){
+        sms_sts.EnableTorque(ID_FEETECH_ASC_D, 0);
+        position_basse_droit = sms_sts.ReadPos(ID_FEETECH_ASC_D);
+        Serial.printf("position_basse_droit:%d\n", position_basse_droit);
+        butee_d = true;
+      }
+      //old_pos_d = pos_d;
+      
+      /*
+      pos_g = sms_sts.ReadPos(ID_FEETECH_ASC_G);
+      printf(">delta_pos_g:%d\n",pos_g - old_pos_g);*/
+      my_load = sms_sts.ReadLoad(ID_FEETECH_ASC_G);
+      printf(">load_g:%d\n", my_load);
+      if(abs(my_load) > 250 ){
+        sms_sts.EnableTorque(ID_FEETECH_ASC_G, 0);
+        position_basse_gauche = sms_sts.ReadPos(ID_FEETECH_ASC_G);
+        Serial.printf("position_basse_gauche:%d\n", position_basse_gauche);
+        butee_g = true;
+      }
+      //old_pos_g = pos_g;
+
+      if(butee_d && butee_g){
+        
+        etat_cherche_butees = EA_POS_INIT;
+      }
+      Serial.printf("CHERCHE_BUTEE\n");
+      delay(100);
+      break;}
+
+    case EA_POS_INIT:
+      sms_sts.ServoMode(ID_FEETECH_ASC_D);
+      sms_sts.ServoMode(ID_FEETECH_ASC_G);
+      tab_position[0] = position_basse_droit + (-250 * ASC_D_SIGNE); // Droit
+      tab_position[1] = position_basse_gauche + (-250 * ASC_G_SIGNE); // Gauche
+      position_haute_droit = position_basse_droit - (8100 * ASC_D_SIGNE);
+      position_haute_gauche = position_basse_gauche - (8100 * ASC_G_SIGNE);
+      tab_vitesses_u[0] = 2000;
+      tab_vitesses_u[1] = 2000;
+      sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc);
+      etat_cherche_butees = CHERCHE_BUTEE_INIT;
+      Serial.printf("EA_POS_INIT\n");
+      return ACTION_TERMINEE;
+      break;
+  }
+
+  return ACTION_EN_COURS;
+
+}
+
+
+/// @brief Commande l'ascenseur en position haute
+/// @return 1 si l'ascenseur n'est pas prêt
+int Ascenseur_monte(void){
+  if(etat_ascenseur == ASCENSEUR_ACTIF){
+    tab_position[0] = position_haute_droit;
+    tab_position[1] = position_haute_gauche;
+    tab_vitesses_u[0] = 2000;
+    tab_vitesses_u[1] = 2000;
+    sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc);
+    etat_ascenseur = ASCENSEUR_MONTE;
+    return 0;
+  }
+  return 1;
+}
+
+/// @brief Commande l'ascenseur en position basse
+/// @return 1 si l'ascenseur n'est pas prêt
+int Ascenseur_descend(void){
+  if(etat_ascenseur == ASCENSEUR_ACTIF){
+    tab_position[0] = position_basse_droit;
+    tab_position[1] = position_basse_gauche;
+    tab_vitesses_u[0] = 2000;
+    tab_vitesses_u[1] = 2000;
+    sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc);
+    etat_ascenseur = ASCENSEUR_DESCENT;
+    return 0;
+  }
+  return 1;
+}
+/// @brief Commande l'ascenseur en position de dépose
+/// @return 1 si l'ascenseur n'est pas prêt
+int Ascenseur_depose(void){
+  if(etat_ascenseur == ASCENSEUR_ACTIF){
+    tab_position[0] = position_haute_droit*0.9 + position_basse_droit * 0.1;
+    tab_position[1] = position_haute_gauche * 0.9 + position_basse_gauche * 0.1;
+    tab_vitesses_u[0] = 2000;
+    tab_vitesses_u[1] = 2000;
+    sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc);
+    etat_ascenseur = ASCENSEUR_DEPOSE;
+    return 0;
+  }
+  return 1;
+}
+
+enum etat_action_t Ascenseur_get_etat(void){
+  if(etat_ascenseur == ASCENSEUR_ACTIF){
+    return ACTION_TERMINEE;
+  }
+  return ACTION_EN_COURS;
+}
+
+void Ascenseur_step_up(void){
+  step--;
+  Ascenseur_update_step();
+}
+
+void Ascenseur_step_down(void){
+  step++;
+  Ascenseur_update_step();
+}
+
+void Ascenseur_update_step(void){
+  tab_position[0] = position_basse_droit + (250 + step * 500)* (ASC_D_SIGNE); // Droit
+  tab_position[1] = position_basse_gauche + (250 + step * 500)* (ASC_G_SIGNE); // Gauche
+  Serial.printf("position_droit:%d\n", tab_position[0]);
+  Serial.printf("position_gauche:%d\n", tab_position[1]);
+  if((float)(tab_position[0] - position_basse_droit) /(float)(position_haute_droit - position_basse_droit) > 1){
+    tab_position[0] = position_haute_droit;
+  }
+  if((float)(tab_position[0] - position_basse_droit) /(float)(position_haute_droit - position_basse_droit) < 0){
+    tab_position[0] = position_basse_droit;
+  }
+  if((float)(tab_position[1] - position_basse_gauche) /(float)(position_haute_gauche - position_basse_gauche) > 1){
+    tab_position[1] = position_haute_gauche;
+  }
+  if((float)(tab_position[1] - position_basse_gauche) /(float)(position_haute_gauche - position_basse_gauche) < 0){
+    tab_position[1] = position_basse_gauche;
+  }
+  tab_vitesses_u[0] = 2000;
+  tab_vitesses_u[1] = 2000;
+  sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc);
+
+}
+
+void Ascenseur_cycle(){
+  while(!Serial.available()){
+    Serial.println("Ascenseur_monte");
+    Ascenseur_monte();
+    while(Ascenseur_get_etat() != ACTION_TERMINEE){
+      Ascenseur_gestion();
+    }
+    Serial.println("Ascenseur_descend");
+    Ascenseur_descend();
+    while(Ascenseur_get_etat() != ACTION_TERMINEE){
+      Ascenseur_gestion();
+    }
+  }
+  while(Serial.available() > 0){
+    Serial.read();
+  }
+}
+
+void Ascenseur_gestion(void){
+
+  bool erreur;
+  int mov_g, mov_d;
+
+  switch(etat_ascenseur){
+    case ASCENSEUR_INIT:
+      // On vérifie que les deux servomoteurs répondent
+      do{
+        erreur = 0;
+        int ID = sms_sts.Ping(asc_ID[0]);
+        erreur = sms_sts.getLastError();
+        ID = sms_sts.Ping(asc_ID[1]);
+        erreur +=  sms_sts.getLastError();
+        Serial.printf("ASCENSEUR_INIT erreur:%d\n",erreur);
+      }while(erreur != 0);
+      etat_ascenseur = ASCENSEUR_ACTIF;
+      break;
+
+    case ASCENSEUR_ACTIF:
+      break;
+
+    case ASCENSEUR_MONTE:
+      delay(200);
+      etat_ascenseur = ASCENSEUR_BUSY;
+      break;
+
+    case ASCENSEUR_DESCENT:
+      delay(200);
+      etat_ascenseur = ASCENSEUR_BUSY;
+      break;
+
+    case ASCENSEUR_DEPOSE:
+      delay(200);
+      etat_ascenseur = ASCENSEUR_BUSY;
+      break;
+
+    case ASCENSEUR_BUSY:
+      mov_g = sms_sts.ReadMove(ID_FEETECH_ASC_G);
+      mov_d = sms_sts.ReadMove(ID_FEETECH_ASC_D);
+      if(!mov_g && !mov_d) {
+        // Fin du mouvement
+        etat_ascenseur = ASCENSEUR_ACTIF;
+
+      }
+      
+
+      break;
+
+    
+
+  }
+}

Actionneurs/I2C_Slave_lib.ino

@@ -0,0 +1,61 @@
+#define TAILLE_MEMOIRE_I2C 256
+#define TAILLE_MESSAGE_ENVOI_MAX 32
+byte memoire_I2C[TAILLE_MEMOIRE_I2C];
+byte memoire_I2C_index=0;
+
+bool nouveau_message=false;
+
+uint8_t * get_i2c_data(){
+  return memoire_I2C;
+}
+void onRequest(){
+  uint32_t taille_envoi;
+  taille_envoi = min (TAILLE_MEMOIRE_I2C-memoire_I2C_index, TAILLE_MESSAGE_ENVOI_MAX);
+  
+  Wire.write(&memoire_I2C[memoire_I2C_index], taille_envoi);
+  memoire_I2C_index += taille_envoi;
+  if(memoire_I2C_index>=TAILLE_MEMOIRE_I2C){
+    Serial.printf("memoire_I2C_index>=TAILLE_MEMOIRE_I2C\n");
+  }
+}
+
+void onReceive(int len){
+  memoire_I2C_index = Wire.read();
+  while(Wire.available()){
+    nouveau_message=true;
+    memoire_I2C[memoire_I2C_index] = Wire.read();
+    memoire_I2C_index++;
+  }
+  //Serial.printf("I2C reg: %d, val %d\n", memoire_I2C_index, memoire_I2C[memoire_I2C_index]);
+}
+
+void I2C_Slave_init(int addr){
+  Wire.onReceive(onReceive);
+  Wire.onRequest(onRequest);
+  Wire.begin(addr);
+}
+
+bool I2C_Slave_nouveau_message(){
+  if(nouveau_message){
+    nouveau_message=false;
+    return true;
+  }
+  return false;
+}
+
+void I2C_envoi_8bits(byte value, char adresse){
+  //printf("I2C_envoi_8bits a:%d v:%d %x %b\n",adresse, value, value, value);
+  memoire_I2C[adresse] = value;
+}
+
+void I2C_envoi_16bits(int16_t value, char adresse){
+  memoire_I2C[adresse] = (value >> 8) & 0xFF;
+  memoire_I2C[adresse+1] = value & 0xFF;
+}
+
+void I2C_envoi_32bits(int32_t value, char adresse){
+  memoire_I2C[adresse] = value >> 24;
+  memoire_I2C[adresse+1] = (value >> 16) & 0xFF;
+  memoire_I2C[adresse+2] = (value >> 8) & 0xFF;
+  memoire_I2C[adresse+3] = value & 0xFF;
+}

Actionneurs/Translateur.ino

@@ -0,0 +1,92 @@
+#define DUREE_MVT_TRANSLATEUR_MS 1000
+enum translateur_action_t{
+  TRANSLATEUR_INIT,
+  TRANSLATEUR_AVANCE,
+  TRANSLATEUR_RECULE,
+}translateur_action = TRANSLATEUR_INIT;
+
+unsigned long temps_debut_action = 0;
+
+void Translateur_init(void){
+// Moteur
+  /*ledcAttach(3, 500, 8);
+  ledcWrite(3, 0);
+  ledcAttach(4, 500, 8);
+  ledcWrite(4, 0);*/
+  pinMode(3, OUTPUT);
+  pinMode(4, OUTPUT);
+  // Etat des contacteurs de fin de course
+  // relecture de 3 sur 2 (Avant)
+  // relecture de 4 sur 10 (Arrière)
+  pinMode(2, INPUT);
+  pinMode(10, INPUT);
+}
+
+
+enum etat_action_t Translateur_etat(void){
+  if(temps_debut_action + DUREE_MVT_TRANSLATEUR_MS < millis()){
+    if(translateur_action == TRANSLATEUR_AVANCE){
+      if(digitalRead(2) == 0){
+          return ACTION_TERMINEE;
+      }
+    }else  if(translateur_action == TRANSLATEUR_RECULE){
+      if(digitalRead(10) == 0){
+        return ACTION_TERMINEE;
+      }
+    }else{
+        return ACTION_TERMINEE;
+    }
+  }
+  return ACTION_EN_COURS;
+}
+
+
+void Translateur_cycle(void){
+  if(Translateur_etat() == ACTION_TERMINEE){
+    if(translateur_action == TRANSLATEUR_RECULE){
+      Translateur_avance();
+    }else{
+      Translateur_recule();
+    }
+  }
+}
+
+
+void Translateur_avance(){
+  Moteur_set(-255);
+  temps_debut_action = millis();
+  translateur_action = TRANSLATEUR_AVANCE;
+}
+
+void Translateur_recule(){
+  Moteur_set(255);
+  temps_debut_action = millis();
+  translateur_action = TRANSLATEUR_RECULE;
+}
+
+/// @brief pilote la vitesse des moteurs
+/// @param vitesse vitesse signée, sera saturée à 255
+void Moteur_set(int vitesse){
+  if(vitesse < 0){
+    digitalWrite(3, 1);
+    digitalWrite(4, 0);
+  }else{
+    digitalWrite(3, 0);
+    digitalWrite(4, 1);
+  }
+  /*ledcWrite(3, 0);
+  ledcWrite(4, 0);
+  if(vitesse < 0){
+    vitesse = -vitesse;
+    if(vitesse > 255){
+      vitesse = 255;
+    }
+    ledcWrite(3, vitesse);
+  }else{
+    if(vitesse > 255){
+      vitesse = 255;
+    }
+    ledcWrite(4, vitesse);
+  }*/
+}
+

Cerveau/Com_chassis.ino

@@ -3,15 +3,21 @@
 #include <HardwareSerial.h>
 
 
-/// @brief Lit l'état du chassis en I2C
 int Scan_chassis(struct chassis_reception_t * chassis_reception){
+  return Scan_chassis(chassis_reception, true);
+}
+
+/// @brief Lit l'état du chassis en I2C
+int Scan_chassis(struct chassis_reception_t * chassis_reception, bool blocking){
   unsigned char tampon2[14];
   //(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
   error = I2C_lire_registre(I2C_SLAVE_chassi, 0, tampon2, 12);
   if (error !=0){
     Err_Chassi_com =1;IndexErr = 2;
     affiche_erreur("Scan_Chassi", "Erreur I2C");
-    while(1);
+    if(blocking){
+      while(1);
+    }
   }else{
     Err_Chassi_com =0;
     IndexErr = 0;

Cerveau/Com_detection_adversaire.h

@@ -7,6 +7,7 @@ struct detect_adv_reception_t {
   unsigned char distance_cm[12];
 };
 
-void Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception);
+int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception, bool);
+int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception);
 
 #endif

Cerveau/Com_detection_adversaire.ino

@@ -2,17 +2,21 @@
 #include <Arduino.h>
 #include <HardwareSerial.h>
 
-
+int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception){
+  return Detect_adv_lire(detect_adv_reception, true);
+}
 /// @brief Lit les capteurs VL53L1X
-void Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception){
+int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception, bool blocking){
   unsigned char tampon2[14];
   //(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
   error = I2C_lire_registre(I2C_SLAVE_detect_adv, 0, detect_adv_reception->distance_cm, 12);
   if (error !=0){
     affiche_erreur("Scan_Detect_adversaire", "Erreur I2C");
-    while(1);
+    if(blocking){
+      while(1);
+    }
   }else{
     Serial.println("I2C OK");
-    
   }
+  return error;
 }

Cerveau/Com_gradins.ino

@@ -4,8 +4,12 @@
 #include "Com_gradins.h"
 
 
+int Detect_gradin(struct detect_gradin_t * detect_gradin){
+  return Detect_gradin(detect_gradin, true);
+}
+
 /// @brief Lit les capteurs VL53L1X
-void Detect_gradin(struct detect_gradin_t * detect_gradin){
+int Detect_gradin(struct detect_gradin_t * detect_gradin, bool blocking){
   unsigned char tampon[14];
   char chaine[200];
   int angle_mrad;
@@ -13,7 +17,9 @@ void Detect_gradin(struct detect_gradin_t * detect_gradin){
   error = I2C_lire_registre(I2C_SLAVE_detect_gradin, 0, tampon, 13);
   if (error !=0){
     affiche_erreur("Detect_gradin", "Erreur I2C");
-    while(1);
+    if (blocking){
+      while(1);
+    }
   }else{
     
     detect_gradin->status = tampon[0];
@@ -21,8 +27,6 @@ void Detect_gradin(struct detect_gradin_t * detect_gradin){
     detect_gradin->centre_y_mm = tampon[5] << 24 | tampon[6] << 16 | tampon[7] << 8 | tampon[8];
     angle_mrad = tampon[9] << 24 | tampon[10] << 16 | tampon[11] << 8 | tampon[12];
     detect_gradin->angle_rad = angle_mrad / 1000.;
-    
-
-    
   }
+  return error;
 }

Cerveau/Com_triangulation.ino

@@ -1,5 +1,10 @@
+int Scan_Triangulation(struct triangulation_reception_t * triangulation_reception){
+  return Scan_Triangulation(triangulation_reception, true);
+}
+
+
 /// @brief Récupère position (X, Y) et l'orientation du robot
-void Scan_Triangulation(struct triangulation_reception_t * triangulation_reception){
+int Scan_Triangulation(struct triangulation_reception_t * triangulation_reception, bool blocking){
   unsigned char tampon2[14];
   lec_Balise_1, lec_Balise_2, lec_Balise_3 = 0, 0, 0;
   //(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
@@ -8,7 +13,10 @@ void Scan_Triangulation(struct triangulation_reception_t * triangulation_recepti
   if (error !=0){
     Err_Tri_com =1;IndexErr = 1;lec_Balise_1=0;lec_Balise_2=0;lec_Balise_3=0;
     affiche_erreur("Scan_Triangulation", "Erreur I2C");
-    while(1);
+    if(blocking){
+      while(1);
+    }
+    return error;
   }
   else{Err_Tri_com =0;IndexErr = 0;}
   if (error ==0){
@@ -35,6 +43,7 @@ void Scan_Triangulation(struct triangulation_reception_t * triangulation_recepti
   if(lec_Balise_1 == 1 && lec_Balise_2 == 1 && lec_Balise_3 == 1 && lec_Calcul_ok == 1 && error ==0){
     triangulation_reception->validite = true;
   }
+  return error;
 }
 
 void Triangulation_send_immobile(int immobile){

Cerveau/ServerWeb.ino

@@ -105,12 +105,19 @@ void showStatus() {
   char message_statu[500];
   const char message_statu_tmplt[] = "<!DOCTYPE html><html><head><meta charset=\"UTF-8\"><title>A simple form</title></head>\
   <body>\n<h1>Statu du robot</h1>\n\
-  <p>Chassis: %s</p><p>Gradin: %d</p><p>Triangulation: %d</p><p>Detection_adversaire: %d</p>";
+  <p>Chassis: %s</p><p>Gradin: %s</p><p>Triangulation: %s</p><p>Detection_adversaire: %s</p>";
   struct chassis_reception_t chassis_reception;
+  struct detect_gradin_t detect_gradin;
+  struct triangulation_reception_t triangulation_reception;
+  struct detect_adv_reception_t detect_adv_reception;
 
-  statu_to_string(chassis, !Scan_chassis(&chassis_reception) );
+  statu_to_string(chassis, !Scan_chassis(&chassis_reception, false) );
+  statu_to_string(gradin, !Detect_gradin(&detect_gradin, false) );
+  statu_to_string(triangulation, !Scan_Triangulation(&triangulation_reception, false) );
+  statu_to_string(detection_adversaire, !Detect_adv_lire(&detect_adv_reception, false) );
 
-  sprintf(message_statu, message_statu_tmplt, chassis, 0, 0, 0);
+
+  sprintf(message_statu, message_statu_tmplt, chassis, gradin, triangulation, detection_adversaire);
   server.send(200, "text/html", message_statu);
   
 }