Test accostage + longer bordure

Strategie.h

@@ -3,4 +3,10 @@ enum etat_action_t{
     ACTION_TERMINEE
 };
 
+enum longer_direction_t{
+    LONGER_VERS_A,
+    LONGER_VERS_C
+};
+
 enum etat_action_t cerise_accostage(void);
+enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction);

Strategie_prise_cerises.c

@@ -1,3 +1,5 @@
+#include "stdio.h"
+
 #include "Strategie_prise_cerises.h"
 #include "Commande_vitesse.h"
 #include "Geometrie.h"
@@ -8,12 +10,64 @@
 // Rotation en rad/s pour accoster les cerises
 #define ROTATION_CERISE 0.5f
 
+// Translation en mm/s pour aspirer les cerises
+#define TRANSLATION_CERISE 15
+
 void commande_rotation_contacteur_longer_A();
 void commande_rotation_contacteur_longer_C();
+void commande_translation_longer_vers_A();
+void commande_translation_longer_vers_C();
 
 
 double vitesse_accostage_mm_s=100;
 
+enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction){
+    enum etat_action_t etat_action = ACTION_EN_COURS;
+    static enum {
+        LONGE_INIT,
+        LONGE_VERS_A,
+        LONGE_VERS_C,
+        LONGE_COLLE
+    } etat_longer_bord=LONGE_INIT;
+
+    switch  (etat_longer_bord){
+        case LONGE_INIT:
+            if(longer_direction==LONGER_VERS_A){
+                etat_longer_bord=LONGE_VERS_A;
+            }else{
+                // longer_direction==LONGER_VERS_C
+                etat_longer_bord=LONGE_VERS_C;
+            }
+            break;
+
+        case LONGE_VERS_A:
+            commande_translation_longer_vers_A();
+            if( (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF) || 
+                (i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF) ){
+                    etat_longer_bord = LONGE_COLLE;
+                    printf("Longer colle\n");
+            }
+            break;
+        
+        case LONGE_VERS_C:
+            commande_translation_longer_vers_C();
+            if( (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF) || 
+                (i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF) ){
+                    etat_longer_bord = LONGE_COLLE;
+                    printf("Longer colle\n");
+            }
+            break;
+
+        case LONGE_COLLE:
+            if(cerise_accostage() == ACTION_TERMINEE){
+                etat_longer_bord = LONGE_INIT;
+                printf("Longer INIT\n");
+            }
+    }
+    return etat_action;
+
+}
+
 enum etat_action_t cerise_accostage(void){
     enum etat_action_t etat_action = ACTION_EN_COURS;
     double rotation;
@@ -31,12 +85,15 @@ enum etat_action_t cerise_accostage(void){
         commande_vitesse(vitesse_accostage_mm_s * cos(-M_PI/6), vitesse_accostage_mm_s * sin(-M_PI/6), 0);
         if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF){
             etat_accostage=CERISE_TOURNE_CONTACTEUR_LONGER_A;
+            printf("D ACCOSTE_TOURNE_A\n");
         }
         if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_ACTIF){
             etat_accostage=CERISE_TOURNE_CONTACTEUR_LONGER_C;
+            printf("D ACCOSTE_TOURNE_C\n");
         }
         if (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF && i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_ACTIF){
             etat_accostage=CERISE_ACCOSTE;
+            printf("D ACCOSTE\n");
         }
         break;
     
@@ -45,11 +102,14 @@ enum etat_action_t cerise_accostage(void){
         if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF){
             if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF){
                 etat_accostage = CERISE_AVANCE_DROIT;
+                printf("A ACCOSTE_AVANCE\n");
             }else{
                 etat_accostage = CERISE_TOURNE_CONTACTEUR_LONGER_A;
+                printf("A ACCOSTE_TOURNE_A\n");
             }
         }else if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_ACTIF){
             etat_accostage = CERISE_ACCOSTE;
+            printf("A ACCOSTE\n");
         }
         break;
     
@@ -58,18 +118,22 @@ enum etat_action_t cerise_accostage(void){
         if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF){
             if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF){
                 etat_accostage = CERISE_AVANCE_DROIT;
+                printf("C ACCOSTE_AVANCE\n");
             }else{
                 etat_accostage = CERISE_TOURNE_CONTACTEUR_LONGER_C;
+                printf("C ACCOSTE_TOURNE_C\n");
             }
         }else if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF){
             etat_accostage = CERISE_ACCOSTE;
+            printf("C ACCOSTE\n");
         }
         break;
 
     case CERISE_ACCOSTE:
         commande_vitesse_stop();
-        //etat_accostage = CERISE_AVANCE_DROIT;
+        etat_accostage = CERISE_AVANCE_DROIT;
         etat_action = ACTION_TERMINEE;
+        break;
     
     default:
         break;
@@ -83,5 +147,17 @@ void commande_rotation_contacteur_longer_A(){
 }
 
 void commande_rotation_contacteur_longer_C(){
-    commande_rotation(ROTATION_CERISE, RAYON_ROBOT/2, -RAYON_ROBOT* RACINE_DE_3/2);
+    commande_rotation(ROTATION_CERISE, RAYON_ROBOT/2.0, -RAYON_ROBOT* RACINE_DE_3/2.0);
+}
+
+void commande_translation_longer_vers_A(){
+    // V_x : V * cos (60°) = V / 2
+    // V_y : V * sin (60°) = V * RACINE(3) / 2
+    commande_vitesse(TRANSLATION_CERISE/2., TRANSLATION_CERISE / 2. * RACINE_DE_3, 0);
+}
+
+void commande_translation_longer_vers_C(){
+    // V_x : -V * cos (60°) = -V / 2
+    // V_y : -V * sin (60°) = -V * RACINE(3) / 2
+    commande_vitesse(-TRANSLATION_CERISE/2., -TRANSLATION_CERISE / 2. * RACINE_DE_3, 0);
 }

Test.c

@@ -113,7 +113,7 @@ int mode_test(){
 
     case 'F':
     case 'f':
-        while(test_accostage());
+        while(test_strategie());
         break;
 
     case 'H':
@@ -690,7 +690,7 @@ int test_aller_retour(){
         case 'c':
         case 'C':
             Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
-            Trajectoire_circulaire(&trajectoire, 0, 700, -90, 90, 350, 0, 0);
+            Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250, 0, 0);
             printf("Trajectoire circulaire\n");
             break;
 

Test_strategie.c

@@ -15,6 +15,97 @@
 #include "Trajectoire.h"
 #include "Test.h"
 
+int test_accostage();
+int test_longe();
+
+void affichage_test_strategie(){
+    uint32_t temps;
+    temps = time_us_32()/1000;
+    printf(">contacteur_butee_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_A());
+    printf(">contacteur_butee_C:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_C());
+    printf(">contacteur_longer_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_longer_A());
+    printf(">contacteur_longer_C:%ld:%d\n", temps, i2c_annexe_get_contacteur_longer_C());
+    printf(">V_consigne_A:%ld:%f\n>V_consigne_B:%ld:%f\n>V_consigne_C:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C));
+
+    sleep_ms(10);
+}
+
+
+int test_strategie(){
+    printf("L - longer.\n");
+    printf("A - Accoster.\n");
+    int lettre;
+    do{
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
+    switch(lettre){
+        case 'l':
+        case 'L':
+            while(test_longe());
+            break;
+
+        case 'a':
+        case 'A':
+            while(test_accostage());
+            break;
+
+        case 'q':
+        case 'Q':
+            return 0;
+        default:
+            return 1;
+    }
+}
+
+
+int test_longe(){
+    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
+
+    i2c_maitre_init();
+    Trajet_init();
+    //printf("Init gyroscope\n");
+    //Gyro_Init();
+
+    stdio_flush();
+
+    //set_position_avec_gyroscope(1);
+
+    temps_ms = Temps_get_temps_ms();
+    temps_ms_init = temps_ms;
+    do{
+        i2c_gestion(i2c0);
+        i2c_annexe_gestion();
+        // Routines à 1 ms
+        if(temps_ms != Temps_get_temps_ms()){
+            temps_ms = Temps_get_temps_ms();
+            QEI_update();
+            Localisation_gestion();
+            AsserMoteur_Gestion(_step_ms);
+
+            // Routine à 2 ms
+            if(temps_ms % _step_ms_gyro == 0){
+            //    Gyro_Read(_step_ms_gyro);
+            }
+
+            if(temps_ms > temps_ms_init + 200){
+                if(cerise_longer_bord(LONGER_VERS_A) == ACTION_TERMINEE){
+                    printf("Accostage_terminee\n");
+                }
+            }
+
+        }
+        lettre = getchar_timeout_us(0);
+    }while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
+    printf("Lettre : %d; %c\n", lettre, lettre);
+    
+    if(lettre == 'q' && lettre == 'Q'){
+        return 0;
+    }
+    return 1;
+
+}
+
+
 int test_accostage(){
     int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
 
@@ -50,15 +141,13 @@ int test_accostage(){
                 }
             }
 
-            /*printf(">contacteur_butee_A:%d\n", i2c_annexe_get_contacteur_butee_A());
-            printf(">contacteur_butee_C:%d\n", i2c_annexe_get_contacteur_butee_C());
-            printf(">contacteur_longer_A:%d\n", i2c_annexe_get_contacteur_longer_A());
-            printf(">contacteur_longer_C:%d\n", i2c_annexe_get_contacteur_longer_C());*/
-
-
         }
         lettre = getchar_timeout_us(0);
     }while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
     printf("Lettre : %d; %c\n", lettre, lettre);
 
+    if(lettre == 'q' && lettre == 'Q'){
+        return 0;
+    }
+    return 1;
 }

Test_strategie.h

@@ -1 +1 @@
-int test_accostage(void);
+int test_strategie(void);