Accostage OK

CMakeLists.txt

@@ -22,15 +22,17 @@ i2c_annexe.c
 Localisation.c
 Moteurs.c
 Robot_config.c
+Servomoteur.c
+Strategie_prise_cerises.c
 Temps.c
 Test.c
+Test_strategie.c
 Trajet.c
 Trajectoire.c
 Trajectoire_bezier.c
 Trajectoire_circulaire.c
 Trajectoire_droite.c
 Trajectoire_rotation.c
-Servomoteur.c
 spi_nb.c)
 
 pico_generate_pio_header(test ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio)

Commande_vitesse.c

@@ -2,6 +2,8 @@
 #include "Geometrie_robot.h"
 #include "Commande_vitesse.h"
 
+
+
 /// @brief Commande une rotation autour d'un point dans le référentiel du robot.
 /// Cette fonction déplace le torseur cinétique du centre de rotation vers le centre du robot
 /// pour obtenir la vitesse de ce dernier.
@@ -34,3 +36,10 @@ void commande_vitesse(double vitesse_x_mm_s, double vitesse_y_mm_s, double orien
     AsserMoteur_setConsigne_mm_s(MOTEUR_C, vitesse_roue_c);
 
 }
+
+/// @brief Arrête le robot.
+void commande_vitesse_stop(){
+    AsserMoteur_setConsigne_mm_s(MOTEUR_A, 0);
+    AsserMoteur_setConsigne_mm_s(MOTEUR_B, 0);
+    AsserMoteur_setConsigne_mm_s(MOTEUR_C, 0);
+}

Commande_vitesse.h

@@ -1,2 +1,3 @@
 void commande_vitesse(double vitesse_x_mm_s, double vitesse_y_mm_s, double orientation_radian_s);
 void commande_rotation(double rotation_rad_s, double centre_x, double centre_y);
+void commande_vitesse_stop(void);

Strategie.h

@@ -2,3 +2,5 @@ enum etat_action_t{
     ACTION_EN_COURS,
     ACTION_TERMINEE
 };
+
+enum etat_action_t cerise_accostage(void);

Strategie_prise_cerises.c

@@ -3,22 +3,25 @@
 #include "Geometrie.h"
 #include "Geometrie_robot.h"
 #include "math.h"
+#include "i2c_annexe.h"
 
 // Rotation en rad/s pour accoster les cerises
 #define ROTATION_CERISE 0.5f
 
+void commande_rotation_contacteur_longer_A();
+void commande_rotation_contacteur_longer_C();
+
 
 double vitesse_accostage_mm_s=100;
 
 enum etat_action_t cerise_accostage(void){
     enum etat_action_t etat_action = ACTION_EN_COURS;
-    struct position_t contacteur_gauche, contacteur_droit;
     double rotation;
 
     static enum {
         CERISE_AVANCE_DROIT,
-        CERISE_TOURNE_CONTACTEUR_GAUCHE,
+        CERISE_TOURNE_CONTACTEUR_LONGER_A,
-        CERISE_TOURNE_CONTACTEUR_DROIT,
+        CERISE_TOURNE_CONTACTEUR_LONGER_C,
         CERISE_ACCOSTE
     } etat_accostage=CERISE_AVANCE_DROIT;
 
@@ -26,18 +29,48 @@ enum etat_action_t cerise_accostage(void){
     {
     case CERISE_AVANCE_DROIT:
         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;
+        }
+        if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_ACTIF){
+            etat_accostage=CERISE_TOURNE_CONTACTEUR_LONGER_C;
+        }
+        if (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF && i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_ACTIF){
+            etat_accostage=CERISE_ACCOSTE;
+        }
         break;
     
-    case CERISE_TOURNE_CONTACTEUR_GAUCHE:
+    case CERISE_TOURNE_CONTACTEUR_LONGER_A:
-        rotation = ROTATION_CERISE;
+        commande_rotation_contacteur_longer_A();
-        commande_rotation(ROTATION_CERISE, contacteur_gauche.x_mm, contacteur_gauche.y_mm);
+        if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF){
+            if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF){
+                etat_accostage = CERISE_AVANCE_DROIT;
+            }else{
+                etat_accostage = CERISE_TOURNE_CONTACTEUR_LONGER_A;
+            }
+        }else if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_ACTIF){
+            etat_accostage = CERISE_ACCOSTE;
+        }
         break;
     
-    case CERISE_TOURNE_CONTACTEUR_DROIT:
+    case CERISE_TOURNE_CONTACTEUR_LONGER_C:
-        rotation = ROTATION_CERISE;
+        commande_rotation_contacteur_longer_C();
-        commande_rotation(-ROTATION_CERISE, contacteur_droit.x_mm, contacteur_droit.y_mm);
+        if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF){
+            if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF){
+                etat_accostage = CERISE_AVANCE_DROIT;
+            }else{
+                etat_accostage = CERISE_TOURNE_CONTACTEUR_LONGER_C;
+            }
+        }else if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF){
+            etat_accostage = CERISE_ACCOSTE;
+        }
         break;
 
+    case CERISE_ACCOSTE:
+        commande_vitesse_stop();
+        //etat_accostage = CERISE_AVANCE_DROIT;
+        etat_action = ACTION_TERMINEE;
+    
     default:
         break;
     }
@@ -46,9 +79,9 @@ enum etat_action_t cerise_accostage(void){
 }
 
 void commande_rotation_contacteur_longer_A(){
-    commande_rotation(ROTATION_CERISE, RAYON_ROBOT, 0);
+    commande_rotation(-ROTATION_CERISE, RAYON_ROBOT, 0);
 }
 
 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, -RAYON_ROBOT* RACINE_DE_3/2);
 }

Test.c

@@ -26,6 +26,9 @@
 #include "Trajectoire.h"
 #include "Trajet.h"
 
+#include "Test_strategie.h"
+#include "Test.h"
+
 #define V_INIT -999.0
 #define TEST_TIMEOUT_US 10000000
 
@@ -65,6 +68,7 @@ int mode_test(){
     printf("C - pour les codeurs\n");
     printf("D - pour les codeurs (somme en mm)\n");
     printf("E - Commande en vitesse...\n");
+    printf("F - Accostage\n");
     printf("H - Asser Position - avance\n");
     printf("I - Asser Position - avance et tourne (gyro)\n");
     printf("J - Asser Position - avance et tourne (sans gyro)\n");
@@ -107,6 +111,11 @@ int mode_test(){
         while(test_cde_vitesse());
         break;
 
+    case 'F':
+    case 'f':
+        while(test_accostage());
+        break;
+
     case 'H':
     case 'h':
         while(test_asser_position_avance());
@@ -695,7 +704,7 @@ int test_aller_retour(){
         case 'e':
         case 'E':
             Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
-            Trajectoire_droite(&trajectoire, 0, 0, 0, 700, 0, M_PI);
+            Trajectoire_droite(&trajectoire, 0, 0, 0, 1500, 0, M_PI);
             printf("Trajectoire droite avec rotation (OK)\n");
             break;
 

Test.h

@@ -1 +1,3 @@
+
 int mode_test();
+void test_trajectoire_teleplot();

Test_strategie.c

@@ -0,0 +1,64 @@
+#include "pico/multicore.h"
+#include "stdio.h"
+#include "hardware/i2c.h"
+
+#include "Asser_Moteurs.h"
+#include "i2c_annexe.h"
+#include "i2c_maitre.h"
+#include "gyro.h"
+#include "Localisation.h"
+#include "QEI.h"
+#include "Robot_config.h"
+#include "Strategie.h"
+#include "Temps.h"
+#include "Trajet.h"
+#include "Trajectoire.h"
+#include "Test.h"
+
+int test_accostage(){
+    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_accostage() == ACTION_TERMINEE){
+                    printf("Accostage_terminee\n");
+                }
+            }
+
+            /*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);
+
+}

Test_strategie.h

@@ -0,0 +1 @@
+int test_accostage(void);

i2c_annexe.h

@@ -1,5 +1,8 @@
 #include "pico/stdlib.h"
 
+#define CONTACTEUR_ACTIF 0
+#define CONTACTEUR_INACTIF 1
+
 void i2c_annexe_gestion(void);
 void i2c_annexe_active_turbine(void);
 void i2c_annexe_desactive_turbine(void);