Début de la trajectoire 2025 pour la superstar

.vscode/settings.json

@@ -9,6 +9,9 @@
         "trajet.h": "c",
         "trajectoire.h": "c",
         "compare": "c",
-        "asser_position.h": "c"
+        "asser_position.h": "c",
+        "stdlib.h": "c",
+        "strategie.h": "c",
+        "strategie_deplacement.h": "c"
     }
 }

CMakeLists.txt

@@ -14,12 +14,15 @@ add_executable(Mon_Projet
   Asser_Position.c
   Asser_Moteurs.c
   Commande_vitesse.c
+  Evitement.c
   Geometrie.c
   i2c_maitre.c
   Moteurs.c
   Localisation.c
   main.c
   QEI.c
+  Strategie_deplacement.c
+  Strategie.c
   Temps.c
   Trajectoire_bezier.c
   Trajectoire_circulaire.c

Evitement.c

@@ -0,0 +1,38 @@
+#include "pico/stdlib.h"
+#include "Asser_Moteurs.h"
+#include "Evitement.h"
+#include "Trajet.h"
+
+// 250 ms
+#define TEMPS_VALIDE_OBSTACLE_US 250000
+
+enum evitement_statu_t evitement_statu=PAS_D_OBSTACLE;
+
+void Evitement_gestion(int step_ms){
+    static uint32_t temps_obstacle;
+    switch(evitement_statu){
+        case PAS_D_OBSTACLE:
+            if(Trajet_get_bloque() == 1 && AsserMoteur_RobotImmobile(step_ms)){
+                evitement_statu = OBSTACLE_NON_CONFIRME;
+                temps_obstacle = time_us_32();
+            }
+            break;
+        case OBSTACLE_NON_CONFIRME:
+            if(time_us_32() - temps_obstacle >  TEMPS_VALIDE_OBSTACLE_US){
+                evitement_statu = OBSTACLE_CONFIRME;
+            }
+            if(!Trajet_get_bloque()){
+                evitement_statu = PAS_D_OBSTACLE;
+            }
+            break;
+        case OBSTACLE_CONFIRME:
+            if(!Trajet_get_bloque()){
+                evitement_statu = PAS_D_OBSTACLE;
+            }
+            break;
+    }
+}
+
+enum evitement_statu_t Evitement_get_statu(){
+    return evitement_statu;
+}

Evitement.h

@@ -0,0 +1,8 @@
+enum evitement_statu_t{
+    PAS_D_OBSTACLE,
+    OBSTACLE_NON_CONFIRME,
+    OBSTACLE_CONFIRME,
+};
+
+enum evitement_statu_t Evitement_get_statu();
+void Evitement_gestion(int step_ms);

Strategie.c

@@ -0,0 +1,58 @@
+#include "Strategie.h"
+#include "Geometrie.h"
+
+
+
+enum etat_action_t Strategie_super_star(uint32_t step_ms){
+	static enum{
+        SSS_TEST_TOURNE,
+		SSS_INIT,
+		SSS_AVANCE_1,
+		SSS_TOURNE,
+		SSS_AVANCE_2,
+		SSS_DANCE
+	} etat_sss = SSS_TEST_TOURNE;
+    struct trajectoire_t trajectoire;
+
+	switch(etat_sss){
+        case SSS_TEST_TOURNE:
+            Localisation_set(1135, 1895, 0);
+            etat_sss = SSS_TOURNE;
+            break;
+		case SSS_INIT:
+			Localisation_set(45, 1895, 0);
+			etat_sss = SSS_AVANCE_1;
+			break;
+
+		case SSS_AVANCE_1:
+            Trajet_config(TRAJECT_CONFIG_STD);
+            Trajectoire_droite(&trajectoire, 45, 1895, 1135, 1895, 0, 0);
+            if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
+                //etat_sss = SSS_TOURNE;
+                etat_sss = SSS_DANCE;
+            }
+            break;
+
+        case SSS_TOURNE:
+            Trajet_config(100, 100);
+            Trajectoire_circulaire(&trajectoire, 1135, 1645, M_PI/2, 0, 250, 0, -M_PI/2);
+            if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
+                //etat_sss = SSS_AVANCE_2;
+                etat_sss = SSS_DANCE;
+            }
+            break;
+
+        case SSS_AVANCE_2:
+            Trajet_config(TRAJECT_CONFIG_STD);
+            Trajectoire_droite(&trajectoire, 1385, 1645, 1385, 1600, 0, 0);
+            if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
+                etat_sss = SSS_DANCE;
+            }
+            break;
+
+        case SSS_DANCE:
+            return ACTION_TERMINEE;
+	}
+    return ACTION_EN_COURS;
+
+}

Strategie.h

@@ -0,0 +1,53 @@
+#include "pico/stdlib.h"
+#include "Localisation.h"
+#include "Temps.h"
+#include "Trajectoire.h"
+#include "Trajet.h"
+
+
+#ifndef STRATEGIE_H
+#define STRATEGIE_H
+
+#define COULEUR 15
+#define TIRETTE 14
+//#define CORR_ANGLE_DEPART_DEGREE  (-1.145)
+#define CORR_ANGLE_DEPART_DEGREE  (0)
+
+enum etat_action_t{
+    ACTION_EN_COURS,
+    ACTION_TERMINEE,
+    ACTION_ECHEC
+};
+
+enum longer_direction_t{
+    LONGER_VERS_A,
+    LONGER_VERS_C
+};
+
+enum couleur_t{
+    COULEUR_BLEU=0,
+    COULEUR_JAUNE,
+    COULEUR_INCONNUE,
+};
+
+enum evitement_t{
+    EVITEMENT_SANS_EVITEMENT,
+    EVITEMENT_PAUSE_DEVANT_OBSTACLE,
+    EVITEMENT_ARRET_DEVANT_OBSTACLE,
+    EVITEMENT_RETOUR_SI_OBSTABLE,
+    EVITEMENT_CONTOURNEMENT
+};
+
+struct objectif_t{
+    int priorite;
+    enum {A_FAIRE, EN_COURS, BLOQUE, FAIT} etat;
+    enum {CERISE_BAS, CERISE_HAUT, CERISE_GAUCHE, CERISE_DROITE, 
+        ZONE_1, ZONE_2, ZONE_3, ZONE_4, ZONE_5} cible;
+};
+
+enum etat_action_t Strategie_super_star(uint32_t step_ms);
+
+enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement);
+
+// STRATEGIE_H
+#endif

Strategie_deplacement.c

@@ -0,0 +1,73 @@
+#include "Strategie.h"
+#include "Strategie_deplacement.h"
+#include "Trajet.h"
+#include "Evitement.h"
+#include "Geometrie.h"
+#include "VL53L8_2024.h"
+
+float distance_obstacle;
+
+enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement){
+    enum etat_action_t etat_action = ACTION_EN_COURS;
+    enum etat_trajet_t etat_trajet;
+    float angle_avancement;
+    static bool trajet_inverse = false;
+    
+    static enum {
+        PARCOURS_INIT,
+        PARCOURS_AVANCE,
+    } etat_parcourt=PARCOURS_INIT;
+
+    switch (etat_parcourt){
+        case PARCOURS_INIT:
+            Trajet_debut_trajectoire(trajectoire);
+            etat_parcourt = PARCOURS_AVANCE;
+            break;
+        
+        case PARCOURS_AVANCE:
+            if(evitement != EVITEMENT_SANS_EVITEMENT){
+                angle_avancement = Trajet_get_orientation_avance();
+                distance_obstacle = VL53L8_get_old_min_distance();
+                Trajet_set_obstacle_mm(distance_obstacle);
+            }
+
+            if(Evitement_get_statu() == OBSTACLE_CONFIRME || Evitement_get_statu() == OBSTACLE_NON_CONFIRME){
+                switch(evitement){
+                    case EVITEMENT_SANS_EVITEMENT:
+                        //printf("Evitement lors trajet EVITEMENT_SANS_EVITEMENT: ERREUR\n");
+                        break;
+                    case EVITEMENT_PAUSE_DEVANT_OBSTACLE:
+                        // Rien à faire ici
+                        break;
+                    
+                    case EVITEMENT_ARRET_DEVANT_OBSTACLE:
+                        etat_parcourt = PARCOURS_INIT;
+                        return ACTION_ECHEC;
+
+                    case EVITEMENT_RETOUR_SI_OBSTABLE:
+                        trajet_inverse = !trajet_inverse;
+                        Trajet_inverse();
+                        break;
+
+                    case EVITEMENT_CONTOURNEMENT: // TODO
+                        break;
+                        
+                }
+
+            }
+
+            etat_trajet = Trajet_avance(step_ms/1000.);
+            if(etat_trajet == TRAJET_TERMINE){
+                if(trajet_inverse){
+                    etat_action = ACTION_ECHEC;
+                }else{
+                    etat_action = ACTION_TERMINEE;
+                }
+                etat_parcourt = PARCOURS_INIT;
+            }
+            break;
+    }
+
+    return etat_action;
+
+}

Trajectoire.c

@@ -9,13 +9,13 @@
 #define PRECISION_ABSCISSE 0.001f
 
 
-void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_degre, float angle_fin_degre, float rayon,
+void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_rad, float angle_fin_rad, float rayon,
                         float orientation_debut_rad, float orientation_fin_rad){
     trajectoire->type = TRAJECTOIRE_CIRCULAIRE;
     trajectoire->p1.x = centre_x;
     trajectoire->p1.y = centre_y;
-    trajectoire->angle_debut_degre = angle_debut_degre;
+    trajectoire->angle_debut_rad = angle_debut_rad;
-    trajectoire->angle_fin_degre = angle_fin_degre;
+    trajectoire->angle_fin_rad = angle_fin_rad;
     trajectoire->rayon = rayon;
     trajectoire->longueur = -1;
     trajectoire->orientation_debut_rad = orientation_debut_rad;
@@ -58,8 +58,8 @@ void Trajectoire_inverse(struct trajectoire_t * trajectoire){
     trajectoire->orientation_fin_rad = old_trajectoire.orientation_debut_rad;
 
     if(trajectoire->type == TRAJECTOIRE_CIRCULAIRE){
-        trajectoire->angle_debut_degre = old_trajectoire.angle_fin_degre;
+        trajectoire->angle_debut_rad = old_trajectoire.angle_fin_rad;
-        trajectoire->angle_fin_degre = old_trajectoire.angle_debut_degre;
+        trajectoire->angle_fin_rad = old_trajectoire.angle_debut_rad;
         return;
     }
     if(trajectoire->type == TRAJECTOIRE_DROITE){

Trajectoire.h

@@ -20,7 +20,7 @@ struct trajectoire_t {
     enum trajectoire_type_t type;
     struct point_xy_t p1, p2, p3, p4;
     float orientation_debut_rad, orientation_fin_rad;
-    float rayon, angle_debut_degre, angle_fin_degre;
+    float rayon, angle_debut_rad, angle_fin_rad;
     float longueur;
 };
 
@@ -29,7 +29,7 @@ struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, dou
 float Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, float abscisse);
 float Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, double distance_mm);
 double distance_points(struct point_xy_t point, struct point_xy_t point_old);
-void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_degre, float angle_fin_degre,
+void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_rad, float angle_fin_rad,
                             float rayon, float orientation_debut_rad, float orientation_fin_rad);
 void Trajectoire_droite(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float orientation_debut_rad, float orientation_fin_rad);
 void Trajectoire_bezier(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float p3_x, float p3_y, float p4_x, float p4_y,

Trajectoire_circulaire.c

@@ -4,23 +4,23 @@
 
 void Trajectoire_circulaire_get_longueur(struct trajectoire_t * trajectoire){
     float distance_angulaire;
-    if(trajectoire->angle_debut_degre > trajectoire->angle_fin_degre){
+    if(trajectoire->angle_debut_rad > trajectoire->angle_fin_rad){
-        distance_angulaire = trajectoire->angle_debut_degre - trajectoire->angle_fin_degre;
+        distance_angulaire = trajectoire->angle_debut_rad - trajectoire->angle_fin_rad;
     }else{
-        distance_angulaire = trajectoire->angle_fin_degre - trajectoire->angle_debut_degre;
+        distance_angulaire = trajectoire->angle_fin_rad - trajectoire->angle_debut_rad;
     }
-    trajectoire->longueur = 2. * M_PI * trajectoire->rayon * distance_angulaire / 360.;
+    trajectoire->longueur = trajectoire->rayon * distance_angulaire;
 }
 
 /// @brief Retourne le point sur la trajectoire en fonction de l'abscisse
 /// @param abscisse : compris entre 0 et 1
 struct point_xy_t Trajectoire_circulaire_get_point(struct trajectoire_t * trajectoire, float abscisse){
     struct point_xy_t point;
-    float angle_degre;
+    float angle_rad;
 
-    angle_degre = (float) trajectoire->angle_debut_degre * (1-abscisse) + (float) trajectoire->angle_fin_degre * abscisse;
+    angle_rad = (float) trajectoire->angle_debut_rad * (1-abscisse) + (float) trajectoire->angle_fin_rad * abscisse;
-    point.x = trajectoire->p1.x + cos(angle_degre/180. * M_PI) * trajectoire->rayon;
+    point.x = trajectoire->p1.x + cos(angle_rad) * trajectoire->rayon;
-    point.y = trajectoire->p1.y + sin(angle_degre/180. * M_PI) * trajectoire->rayon;
+    point.y = trajectoire->p1.y + sin(angle_rad) * trajectoire->rayon;
 
     return point;
 }

main.c

@@ -13,6 +13,7 @@
 #include "i2c_maitre.h"
 #include "Localisation.h"
 #include "Moteurs.h"
+#include "Strategie.h"
 #include "Temps.h"
 #include "Trajectoire.h"
 #include "Trajet.h"
@@ -37,9 +38,8 @@ uint16_t tension_batterie_lire(void);
 void identifiant_init(void);
 uint identifiant_lire(void);
 
-int get_tirette(int);
+int get_tirette(void);
 int get_couleur(void);
-void configure_trajet(int identifiant, int couleur);
 
 void gestion_VL53L8CX(void);
 
@@ -54,7 +54,6 @@ VL53L8CX_Configuration Dev;
 
 void main(void)
 {
-	int ledpower = 500;
 	VL53L8CX_ResultsData Results;
 	bool fin_match = false;
 
@@ -62,7 +61,7 @@ void main(void)
 
     stdio_init_all();
 	Temps_init();
-	//tension_batterie_init();
+
 	identifiant_init();
 	Localisation_init(identifiant_lire());
 	Trajet_init(identifiant_lire());
@@ -81,22 +80,19 @@ void main(void)
 	gpio_put(LED1PIN, 1);
 
 	
-	//multicore_launch_core1(gestion_affichage);
+	multicore_launch_core1(gestion_affichage);
-	multicore_launch_core1(gestion_VL53L8CX);
+	// TODO: A remettre - quand on aura récupéré un capteur
+	//multicore_launch_core1(gestion_VL53L8CX);
 	sleep_ms(5000);
 	printf("Demarrage...\n");
 	
 	
-
-	configure_trajet(identifiant_lire(), get_couleur());
-
-	
 	float vitesse_init =300;
 	vitesse = vitesse_init;
 
 	enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;
 
-	while(get_tirette(identifiant_lire()));
+	while(get_tirette());
 	gpio_put(LED1PIN, 0);
 
 	// Seul le premier PAMI doit attendre 90s, les autres démarrent lorsque celui de devant part
@@ -119,9 +115,7 @@ void main(void)
 				QEI_update();
 				Localisation_gestion();
 				
-				if(etat_trajet != TRAJET_TERMINE){
+				if(Strategie_super_star(step_ms) == ACTION_TERMINEE){
-					etat_trajet = Trajet_avance((float)step_ms/1000.);
-				}else{
 					Asser_Position_maintien();
 					if(Asser_Position_panic_angle()){
 						fin_match=1;
@@ -168,18 +162,21 @@ void gestion_affichage(void){
 void affichage(void){
 /*printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) );
 	printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );*/
-	printf(">pos_x:%.1f\n>pos_y:%.1f\n>pos_angle:%.1f\n", Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian);
+
+	long temps_ms = (time_us_64()/1000);
+	printf(">temps_ms:%lu\n", temps_ms);
+	printf(">pos_angle:%.1f\n", Localisation_get().angle_radian);
 	printf(">distance_obstacle:%f\n",Trajet_get_obstacle_mm());
 
 	printf(">abscisse:%f\n",abscisse);
 	
 	struct position_t position_actuelle;
 	position_actuelle = Localisation_get();
-	printf(">delta_orientation_radian:%.2f\n>angle_delta:%.2f\n",delta_orientation_radian, atan2f(delta_y_mm, delta_x_mm));
+	printf(">delta_orientation_radian:%lu:%.2f\n>angle_delta:%lu:%.2f\n", temps_ms, delta_orientation_radian, temps_ms, atan2f(delta_y_mm, delta_x_mm));
-	printf(">pos_x:%.2f\n>pos_y:%.2f\n", position_actuelle.x_mm, position_actuelle.y_mm);
+	printf(">pos_x:%lu:%.2f\n>pos_y:%lu:%.2f\n", temps_ms, position_actuelle.x_mm, temps_ms, position_actuelle.y_mm);
-	printf(">con_x:%.2f\n>con_y:%.2f\n", point.point_xy.x, point.point_xy.y);
+	printf(">con_x:%lu:%.2f\n>con_y:%lu:%.2f\n", temps_ms, point.point_xy.x, temps_ms, point.point_xy.y);
 	
-	printf(">couleur:%d\n>id:%d\n>Tirette:%d\n", get_couleur(), identifiant_lire(), get_tirette(identifiant_lire()));
+	printf(">couleur:%d\n>id:%d\n>Tirette:%d\n", get_couleur(), identifiant_lire(), get_tirette());
 }
 
 void tension_batterie_init(void){
@@ -219,11 +216,8 @@ void identifiant_init(){
 	gpio_set_dir(COULEUR_PIN, GPIO_IN);
 }
 
-int get_tirette(int id){
+int get_tirette(){
-	if(id == 3){
+	return !gpio_get(TIRETTE_PIN);
-		return !gpio_get(TIRETTE_PIN);
-	}
-	return (VL53L8_get_old_min_distance() <50);
 	
 }
 
@@ -236,104 +230,3 @@ int get_couleur(void){
 uint identifiant_lire(){
 	return (gpio_get(21) << 2)+ (gpio_get(22) << 1) + gpio_get(26);
 }
-
-void configure_trajet(int identifiant, int couleur){
-	
-	struct trajectoire_t trajectoire;
-	Trajet_config(TRAJECT_CONFIG_RAPIDE);
-
-	switch(couleur){
-		case COULEUR_JAUNE:
-			switch (identifiant)
-			{
-				case 0:
-					Localisation_set(3000-1249, 2000-63, 0);
-					Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63, 
-					3000-750, 1400, 3000-750, 2100, 0, 0);
-					break;
-				case 1:
-					Localisation_set(3000-1249, 2000-63, 0);
-					Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63, 
-					3000-750, 1400, 3000-750, 2100, 0, 0);
-					break;
-				case 2:
-					Localisation_set(3000-1245, 2000-63, 0);
-					Trajectoire_bezier(&trajectoire, 3000-1244, 2000-63, 3000-950, 2000-63, 
-					3000-540, 1400, 3100, 1400, M_PI, M_PI);
-					break;
-				case 3:
-					Localisation_set(3000-1130, 2000-63, 0);
-					Trajectoire_bezier(&trajectoire, 3000-1122, 2000-63, 3000-905, 2000-63, 
-					3000-606, 2000-590, 3000-225, 2000-225, -M_PI, -M_PI);
-					break;
-				case 4:
-					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
-					Localisation_set(3000-1364, 2000-63, 0);
-					Trajectoire_bezier(&trajectoire, 3000-1363, 2000-63, 3000-550, 2000-63, 
-					2700-900, 600, 2700-0, 0, 0, 0);
-					break;
-				case 5:
-					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
-					Localisation_set(3000-1450, 2000-63, 0);
-					Trajectoire_bezier(&trajectoire, 3000-1449, 2000-63, 3000-675, 2000-63, 
-					3000-930, 970, 0, 1200, -M_PI / 2., M_PI);
-					break;
-				case 6:
-					break;
-				case 7:
-					break;
-				
-				default:
-					break;
-			}
-			break;
-
-		case COULEUR_BLEU:
-			switch (identifiant)
-			{
-				case 0:
-					Localisation_set(1249, 2000-63, M_PI);
-					Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63, 
-					750, 1400, 750, 2100, M_PI, M_PI);
-					break;
-				case 1:
-					Localisation_set(1249, 2000-63, M_PI);
-					Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63, 
-					750, 1400, 750, 2100, M_PI, M_PI);
-					break;
-				case 2:
-					Localisation_set(1245, 2000-63, M_PI);
-					Trajectoire_bezier(&trajectoire, 1244, 2000-63, 950, 2000-63, 
-					540, 1400, -100, 1400, M_PI, M_PI);
-					break;
-				case 3:
-					Localisation_set(1121, 2000-63, M_PI);
-					Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63, 
-					606, 2000-590, 225, 2000-225, M_PI, M_PI);
-					break;
-				case 4:
-					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
-					Localisation_set(1364, 2000-63, M_PI);
-					Trajectoire_bezier(&trajectoire, 1363, 2000-63, 550, 2000-63, 
-					900, 600, 0, 0, -M_PI / 2., M_PI);
-					break;
-				case 5:
-					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
-					Localisation_set(1450, 2000-63, M_PI);
-					Trajectoire_bezier(&trajectoire, 1449, 2000-63, 675, 2000-63, 
-					930, 970, 3000, 1200, -M_PI / 2., M_PI);
-					break;
-				case 6:
-					break;
-				case 7:
-					break;
-				
-				default:
-					break;
-			}
-			break;
-	}
-	
-	Trajet_debut_trajectoire(trajectoire);
-
-}