Code d'homologation

Strategie.c

@@ -586,8 +586,8 @@ enum etat_action_t Strategie_calage_debut_manuel(enum couleur_t couleur, uint32_
                     Localisation_set_angle((-180. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
                     Localisation_set_angle((-60. * DEGRE_EN_RADIAN));
                 }else{
-                    Localisation_set_x(3000 - (distance + DISTANCE_CENTRE_CAPTEUR));
+                    Localisation_set_x(3025 - (distance + DISTANCE_CENTRE_CAPTEUR));
-                    Localisation_set_angle((0. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
+                    Localisation_set_angle((-3. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
                 }
                 
                 etat_calage_debut = CD_ALLER_POSITION_INIT;

Strategie_2024.c

@@ -109,6 +109,7 @@ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms)
             if(Strategie_aller_a(940, 300, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms) == ACTION_TERMINEE){
                 etat_test=TAP_PLANTE_ORIENTATION;
             }
+            break;
 
         case TAP_PLANTE_ORIENTATION:
             if(Strat_2024_aller_zone_plante(get_zone_plante(couleur), _step_ms) == ACTION_TERMINEE){

Strategie_2024_panneaux.c

@@ -21,6 +21,8 @@ enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t
         TP_TOURNE_PANNEAU_4,
         TP_TOURNE_PANNEAU_5,
         TP_TOURNE_PANNEAU_6,
+        TP_DEGAGEMENT,
+        TP_DEGAGEMENT_ACTIF
     } etat_tourne_panneaux = TP_APPROCHE_PANNEAU_1;
 
 
@@ -34,7 +36,7 @@ enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t
                     EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
             }else{
                 etat_action = Strategie_tourner_et_aller_a(
-                    323, 300, (150. *DEGRE_EN_RADIAN),
+                    3000 - 323, 300, (150. *DEGRE_EN_RADIAN),
                     EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
             }
             if (etat_action == ACTION_TERMINEE){
@@ -50,7 +52,7 @@ enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t
                     323, 200, (150. *DEGRE_EN_RADIAN), EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
             }else{
                 etat_action = Strategie_tourner_et_aller_a(
-                    323, 300, (150. *DEGRE_EN_RADIAN), EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
+                    3000 - 323, 300, (150. *DEGRE_EN_RADIAN), EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
             }
             if (etat_action == ACTION_TERMINEE){
                 Score_ajout_panneau(1);
@@ -67,9 +69,9 @@ enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t
             etat_action =Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
             if (etat_action == ACTION_TERMINEE){
                 Score_ajout_panneau(1);
-                etat_tourne_panneaux = TP_TOURNE_PANNEAU_3;
+                etat_tourne_panneaux = TP_DEGAGEMENT;
             }else if(etat_action == ACTION_ECHEC){
-                return ACTION_TERMINEE;
+                etat_tourne_panneaux = TP_DEGAGEMENT;
             }
             break;
 
@@ -82,12 +84,69 @@ enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t
             etat_action =Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
             if (etat_action == ACTION_TERMINEE){
                 Score_ajout_panneau(1);
-                //etat_tourne_panneaux = TP_TOURNE_PANNEAU_3;
+                etat_tourne_panneaux = TP_TOURNE_PANNEAU_4;
-                return ACTION_TERMINEE;
             }else if(etat_action == ACTION_ECHEC){
+                etat_tourne_panneaux = TP_DEGAGEMENT;
+            }
+            break;
+
+        case TP_TOURNE_PANNEAU_4:
+            if(couleur == COULEUR_BLEU){
+                Trajectoire_bezier(&trajectoire, 773, 200, 855, 310, 1323, 400, 1323, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
+            }else{
+                Trajectoire_bezier(&trajectoire, 3000-773, 200, 3000-855, 310, 3000-1323, 400, 3000-1323, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
+            }
+            etat_action =Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
+            if (etat_action == ACTION_TERMINEE){
+                Score_ajout_panneau(1);
+                etat_tourne_panneaux = TP_TOURNE_PANNEAU_5;
+            }else if(etat_action == ACTION_ECHEC){
+                etat_tourne_panneaux = TP_DEGAGEMENT;
+            }
+            break;
+
+        case TP_TOURNE_PANNEAU_5:
+            if(couleur == COULEUR_BLEU){
+                Trajectoire_bezier(&trajectoire, 1323, 200, 1410, 310, 1547, 400, 1547, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
+            }else{
+                Trajectoire_bezier(&trajectoire, 3000-1323, 200, 3000-1410, 310, 3000-1547, 400, 3000-1547, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
+            }
+            etat_action =Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
+            if (etat_action == ACTION_TERMINEE){
+                Score_ajout_panneau(1);
+                etat_tourne_panneaux = TP_TOURNE_PANNEAU_6;
+            }else if(etat_action == ACTION_ECHEC){
+                etat_tourne_panneaux = TP_DEGAGEMENT;
+            }
+            break;
+        
+        case TP_TOURNE_PANNEAU_6:
+            if(couleur == COULEUR_BLEU){
+                Trajectoire_bezier(&trajectoire, 1547, 200, 1630, 310, 1771, 400, 1771, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
+            }else{
+                Trajectoire_bezier(&trajectoire, 3000-1547, 200, 3000-1630, 310, 3000-1771, 400, 3000-1771, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
+            }
+            etat_action =Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
+            if (etat_action == ACTION_TERMINEE){
+                Score_ajout_panneau(1);
+                etat_tourne_panneaux = TP_TOURNE_PANNEAU_6;
+            }else if(etat_action == ACTION_ECHEC){
+                etat_tourne_panneaux = TP_DEGAGEMENT;
+            }
+            break;
+
+        case TP_DEGAGEMENT:
+            if(Localisation_get().y_mm < 250){
+                etat_tourne_panneaux = TP_DEGAGEMENT_ACTIF;
+            }else{
+                commande_vitesse_stop();
                 return ACTION_TERMINEE;
             }
             break;
+
+        case TP_DEGAGEMENT_ACTIF:
+                return Strategie_aller_a(Localisation_get().x_mm, 250, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
+            break;
     }
     return ACTION_EN_COURS;
 }

Strategie_2024_pots.c

@@ -21,7 +21,7 @@ float angle_bras[6] =
 
 float distance_bras_correction_mm[6] =
 {
-    -5,
+    -12,
     0,
     -5,
     -15,

gyro_ADXRS453.c

@@ -69,7 +69,7 @@ int gyro_get_sensor_data(uint16_t tampon_envoi[], uint8_t tampon_reception[]){
             return 1;
         }else{
             set_position_avec_gyroscope_error(1);
-            Monitoring_set_erreur_critique();
+            //Monitoring_set_erreur_critique();
             //while(1){
                 affiche_tampon_32bits(tampon_reception);
                 printf("Gyro Failed - SQ bits (%#3x)!= 0x4\n", Gyro_SensorData.SQ);