Debut de la gestion des objectifs

Strategie.c

@@ -16,6 +16,17 @@
 #define DISTANCE_OBSTACLE_CM 50
 #define DISTANCE_PAS_OBSTACLE_MM 2000
 
+#define NB_OBJECTIFS 4
+
+struct objectif_t{
+    int priorite;
+    enum {A_FAIRE, EN_COURS, BLOQUE, FAIT} etat;
+    enum {CERISE_BAS, CERISE_HAUT, CERISE_GAUCHE, CERISE_DROITE} cible;
+} objectifs[NB_OBJECTIFS];
+struct objectif_t * objectif_courant;
+
+
+
 // TODO: Peut-être à remetttre en variable locale après
 float distance_obstacle;
 
@@ -30,6 +41,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
     
     float angle_fin;
     float recal_pos_x, recal_pos_y;
+    float point_x, point_y;
     enum longer_direction_t longer_direction;
     enum etat_action_t etat_action;
     enum etat_trajet_t etat_trajet;
@@ -48,15 +60,33 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
         ATTRAPER_CERISE_DROITE,
         ALLER_PANIER,
         LANCER_PANIER,
+        DECISION_STRATEGIE,
     }etat_strategie;
 
     switch(etat_strategie){
         case STRATEGIE_INIT:
             if(couleur == COULEUR_BLEU){
                 Localisation_set(775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
+                struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_BAS};
+                struct objectif_t objectif_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_HAUT};
+                struct objectif_t objectif_3 = { .priorite = 3, .etat = A_FAIRE, .cible = CERISE_GAUCHE};
+                struct objectif_t objectif_4 = { .priorite = 4, .etat = A_FAIRE, .cible = CERISE_DROITE};
+                objectifs[0]= objectif_1;
+                objectifs[1]= objectif_2;
+                objectifs[2]= objectif_3;
+                objectifs[3]= objectif_4;
             }else{
                 Localisation_set(2000 - 775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
+                struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_BAS};
+                struct objectif_t objectif_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_HAUT};
+                struct objectif_t objectif_3 = { .priorite = 3, .etat = A_FAIRE, .cible = CERISE_DROITE};
+                struct objectif_t objectif_4 = { .priorite = 4, .etat = A_FAIRE, .cible = CERISE_GAUCHE};
+                objectifs[0]= objectif_1;
+                objectifs[1]= objectif_2;
+                objectifs[2]= objectif_3;
+                objectifs[3]= objectif_4;
             }
+            objectif_courant = &objectifs[0];
             etat_strategie = ALLER_CERISE_BAS;
             break;
 
@@ -64,12 +94,14 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
             
             if(couleur == COULEUR_BLEU){
                 angle_fin = 30. * DEGRE_EN_RADIAN;
+                point_x = 857;
             }else{
                 angle_fin = -150. * DEGRE_EN_RADIAN;
+                point_x = 2000 - 857;
             }
 
             Trajet_config(250, 500);            
-            Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 857, 156,
+            Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 156,
                      Localisation_get().angle_radian, angle_fin);
 
             if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){
@@ -92,6 +124,41 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
                 etat_strategie = ALLER_PANIER;
             }
             break;
+        
+        case ALLER_CERISE_HAUT:
+            
+            if(couleur == COULEUR_BLEU){
+                angle_fin = 30. * DEGRE_EN_RADIAN;
+                point_x = 857;
+            }else{
+                angle_fin = -150. * DEGRE_EN_RADIAN;
+                point_x = 2000 - 857;
+            }
+
+            Trajet_config(250, 500);            
+            Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 156,
+                     Localisation_get().angle_radian, angle_fin);
+
+            if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){
+                etat_strategie = ATTRAPER_CERISE_HAUT;
+            }
+            break;
+
+        case ATTRAPER_CERISE_HAUT:
+            recal_pos_y = 3000 - RAYON_ROBOT;
+            if(couleur == COULEUR_BLEU){
+                longer_direction = LONGER_VERS_C;
+                recal_pos_x = 1000 - 15 - PETIT_RAYON_ROBOT_MM;
+            }else{
+                longer_direction = LONGER_VERS_A;
+                recal_pos_x = 1000 + 15 + PETIT_RAYON_ROBOT_MM;
+            }
+
+            etat_action = cerise_attraper_bordure(longer_direction, step_ms, recal_pos_x, recal_pos_y);
+            if(etat_action == ACTION_TERMINEE){
+                etat_strategie = ALLER_PANIER;
+            }
+            break;
             
         case ALLER_PANIER:
             Trajet_config(500, 500);
@@ -116,10 +183,42 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
 
         case LANCER_PANIER:
             if(lance_balles_dans_panier(couleur, step_ms)== ACTION_TERMINEE){
-                etat_strategie = STRATEGIE_FIN;
+                objectif_courant->etat = FAIT;
+                etat_strategie = DECISION_STRATEGIE;
             }
             break;
 
+        case DECISION_STRATEGIE:
+            // Obtenir l'objectif suivant
+            for(uint m_objectif=0; m_objectif < NB_OBJECTIFS; m_objectif++){
+                if(objectif_courant->etat == FAIT && objectifs[m_objectif].etat == A_FAIRE){
+                    objectif_courant = &(objectifs[m_objectif]);
+                }else if(objectif_courant->priorite > objectifs[m_objectif].priorite){
+                    objectif_courant = &(objectifs[m_objectif]);
+                }
+            }
+
+            if(objectif_courant->etat == FAIT){
+                etat_strategie = STRATEGIE_FIN;
+            }else{
+                switch(objectif_courant->cible){
+                    case CERISE_BAS:
+                        etat_strategie = ALLER_CERISE_BAS;
+                        break;
+                    case CERISE_HAUT:
+                        etat_strategie = ALLER_CERISE_HAUT;
+                        break;
+                    case CERISE_GAUCHE:
+                        etat_strategie = STRATEGIE_FIN;
+                        break;
+                    case CERISE_DROITE:
+                        etat_strategie = STRATEGIE_FIN;
+                        break;
+                }
+            }
+
+            break;
+
         case STRATEGIE_FIN:
             i2c_annexe_desactive_propulseur();
             commande_vitesse_stop();
@@ -127,6 +226,71 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
     }
 }
 
+enum etat_action_t  Strategie_aller_panier(enum couleur_t couleur, uint32_t step_ms){
+    enum etat_action_t etat_action = ACTION_EN_COURS;
+    struct trajectoire_t trajectoire;
+    Trajet_config(500, 500);
+
+    // Definition des trajectoires
+    if(couleur == COULEUR_BLEU){
+        // Si le robot est déjà dans le quart haut gauche, 
+        // On va en ligne droite
+        if(Robot_est_dans_quart_haut_gauche()){
+            Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
+                465,2830,
+                +30. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN);
+        }else{
+        // Sinon, on a une courbe de bézier
+            Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
+                485, Localisation_get().y_mm,
+                465, 857,
+                465,2830,
+                +30. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN);
+        }
+    }else{
+        // Si le robot est déjà dans le quart haut droit, 
+        // On va en ligne droite
+        if(Robot_est_dans_quart_haut_droit()){
+            Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
+                2000-465, 2830,
+                -150. * DEGRE_EN_RADIAN, -240. * DEGRE_EN_RADIAN);
+        }else{
+        // Sinon, on a une courbe de bézier
+            Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
+                            485, Localisation_get().y_mm,
+                            2000-465, 857,
+                            2000-465, 2830,
+                            -150. * DEGRE_EN_RADIAN, -240. * DEGRE_EN_RADIAN);
+        }
+        
+    }
+
+    // parcours du trajet
+    if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
+        etat_action = ACTION_TERMINEE;
+    }
+    return etat_action;
+
+}
+
+/// @brief Renvoi 1 si le robot est dans le quart supérieur gauche du terrain
+/// @return 1 si oui, 0 si non.
+int Robot_est_dans_quart_haut_gauche(){
+    if(Localisation_get().x_mm < 1000 && Localisation_get().y_mm > 1500){
+        return 1;
+    }
+    return 0;
+}
+
+/// @brief Renvoi 1 si le robot est dans le quart supérieur droit du terrain
+/// @return 1 si oui, 0 si non.
+int Robot_est_dans_quart_haut_droit(){
+    if(Localisation_get().x_mm > 1000 && Localisation_get().y_mm > 1500){
+        return 1;
+    }
+    return 0;
+}
+
 
 void Homologation(uint32_t step_ms){
     

Strategie_prise_cerises.c

@@ -109,6 +109,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
             commande_vitesse_stop();
             i2c_annexe_desactive_turbine();
             etat_action = ACTION_TERMINEE;
+            etat_attrape = ATTRAPE_INIT;
             break;
     }
     return etat_action;