Réglage des actionneurs

Actionneurs/Actionneurs.ino

@@ -5,9 +5,9 @@
 #define SERVO_PINCE_GAUCHE 8
 #define SERVO_PINCE_DROITE 9
 
-#define FOURCHE_TRANSPORT 5, 287
+#define FOURCHE_TRANSPORT 5, 266
-#define FOURCHE_LEVEE     5, 217
+#define FOURCHE_LEVEE     5, 206
-#define FOURCHE_PRISE     5, 327
+#define FOURCHE_PRISE     5, 308
 
 #define DOIGT_PINCE_GAUCHE_FERME 3, 1530
 #define DOIGT_PINCE_GAUCHE_OUVRE 3, 1790
@@ -17,11 +17,11 @@
 #define DOIGT_PINCE_DROIT_OUVRE 4, 2365
 #define DOIGT_PINCE_DROIT_SEUIL 1575
 
-#define AIMANT_PINCE_DROIT_TIENT 8, 147
+#define AIMANT_PINCE_DROIT_TIENT 8, 176
-#define AIMANT_PINCE_DROIT_LACHE 8, 337
+#define AIMANT_PINCE_DROIT_LACHE 8, 366
 
-#define AIMANT_PINCE_GAUCHE_TIENT 9, 417
+#define AIMANT_PINCE_GAUCHE_TIENT 9, 366
-#define AIMANT_PINCE_GAUCHE_LACHE 9, 217
+#define AIMANT_PINCE_GAUCHE_LACHE 9, 236
 
 #define I2C_CDE_DEMANDE 0x00
 #define I2C_CDE_REALISE 0x01
@@ -490,6 +490,7 @@ enum etat_action_t Actionneur_empile(){
     case ACTIONNEUR_LEVE:
       Serial.println("ACTIONNEUR_LEVE");
       etat_actionneur = ACTIONNEUR_LEVE_1;
+      Servo_set(FOURCHE_PRISE);
       Ascenseur_monte();
       break;
 
@@ -527,6 +528,8 @@ enum etat_action_t Actionneur_empile(){
         etat_actionneur = ACTIONNEUR_DESCEND;
         Servo_set(DOIGT_PINCE_DROIT_FERME);
         Servo_set(DOIGT_PINCE_GAUCHE_FERME);
+        Servo_set(AIMANT_PINCE_DROIT_LACHE);
+        Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
         Ascenseur_descend();
       }
       break;

Actionneurs/Ascenseur.ino

@@ -86,7 +86,7 @@ enum etat_action_t Ascenseur_cherche_butees(void){
       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 ){
+      if(abs(my_load) > 200 ){
         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);
@@ -99,7 +99,7 @@ enum etat_action_t Ascenseur_cherche_butees(void){
       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 ){
+      if(abs(my_load) > 200 ){
         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);

Cerveau/Com_actionneur.h

@@ -0,0 +1,16 @@
+#ifndef COM_ACTIONNEUR_H
+#define COM_ACTIONNEUR_H
+
+#define I2C_SLAVE_actionneur 0x20
+
+#define ACTIONNEUR_NO_ACTION 0
+#define ACTIONNEUR_POS_INITIAL 1
+#define ACTIONNEUR_DEPLACEMENT 2
+#define ACTIONNEUR_PREPARE_PRISE 3
+#define ACTIONNEUR_PRISE 4
+
+struct com_actionneur_t{
+  char demande_action, action_terminee;
+};
+
+#endif //COM_ACTIONNEUR_H

Cerveau/Com_actionneur.ino

@@ -0,0 +1,40 @@
+#include "Com_actionneur.h"
+#include <Arduino.h>
+#include <HardwareSerial.h>
+
+
+
+int Actionneur_scan(struct com_actionneur_t * com_actionneur){
+  return Actionneur_scan(com_actionneur, true);
+}
+/// @brief Commande l'actionneur
+int Actionneur_scan(struct com_actionneur_t * com_actionneur, bool continuous_try){
+  unsigned char tampon[4];
+  //(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
+  error = I2C_lire_registre(I2C_SLAVE_actionneur, 0, tampon, 2);
+  while(error !=0 && continuous_try){
+    affiche_erreur("Actionneur_scan", "Erreur I2C");
+    error = I2C_lire_registre(I2C_SLAVE_actionneur, 0, tampon, 2);
+  }
+
+  com_actionneur->action_terminee = tampon[1];
+  //com_actionneur->demande_action = tampon[0];
+  Serial.println("I2C OK");
+  
+  return error;
+}
+
+int Actionneur_send(struct com_actionneur_t * com_actionneur){
+  
+  unsigned char tampon[1];
+  
+
+  tampon[0] = com_actionneur->demande_action;
+  
+  error = I2C_ecrire_registre(I2C_SLAVE_actionneur, 0, tampon, 1);
+  if (error !=0){
+    affiche_erreur("Actionneur_send", "Erreur I2C");
+    while(1);
+  }
+  return error;
+}