From 68e16127bad3f22a1a55642eaf9388656425b34d Mon Sep 17 00:00:00 2001 From: Samuel Date: Sun, 31 Mar 2024 21:39:12 +0200 Subject: [PATCH] =?UTF-8?q?WIP=20-=20Calage=20initial=20du=20robot=20-=20P?= =?UTF-8?q?ourrait=20=C3=AAtre=20fonctionnel?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 2 - Demonstration.c | 5 +- Geometrie.c | 13 +- Geometrie_robot.h | 4 +- Holonome2023.c | 11 +- Strategie.c | 630 +++++--------------------------------- Strategie.h | 8 +- Strategie_2024_pots.c | 29 +- Strategie_2024_pots.h | 7 + Strategie_pousse_gateau.c | 84 ----- Strategie_pousse_gateau.h | 3 - Strategie_prise_cerises.c | 466 ---------------------------- Strategie_prise_cerises.h | 10 - Test.c | 37 +-- Test_strategie.c | 481 +---------------------------- Test_strategie_2024.c | 2 + Tests_unitaires.c | 8 - i2c_annexe.c | 81 ++--- i2c_annexe.h | 23 +- 19 files changed, 193 insertions(+), 1711 deletions(-) delete mode 100644 Strategie_pousse_gateau.c delete mode 100644 Strategie_pousse_gateau.h delete mode 100644 Strategie_prise_cerises.c delete mode 100644 Strategie_prise_cerises.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 184ae79..7ea2ec8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,8 +32,6 @@ Score.c Servomoteur.c Strategie.c Strategie_deplacement.c -Strategie_prise_cerises.c -Strategie_pousse_gateau.c Strategie_2024_pots.c Temps.c Test.c diff --git a/Demonstration.c b/Demonstration.c index de36862..3810752 100644 --- a/Demonstration.c +++ b/Demonstration.c @@ -200,9 +200,10 @@ enum etat_action_t Demonstration_calage(){ switch (etat_calage) { case CALAGE: - if(calage_angle(LONGER_VERS_C, RAYON_ROBOT, PETIT_RAYON_ROBOT_MM, -60 * DEGRE_EN_RADIAN) == ACTION_TERMINEE){ + // TODO: Appeler la nouvelle fonction de prise de référentiel + /*if(calage_angle(LONGER_VERS_C, RAYON_ROBOT, PETIT_RAYON_ROBOT_MM, -60 * DEGRE_EN_RADIAN) == ACTION_TERMINEE){ etat_calage = DECALAGE; - } + }*/ break; case DECALAGE: diff --git a/Geometrie.c b/Geometrie.c index 124a119..723abfb 100644 --- a/Geometrie.c +++ b/Geometrie.c @@ -1,4 +1,5 @@ #include "Geometrie.h" +#include "math.h" /// @brief Retourne l'angle entre -PI et +PI @@ -68,4 +69,14 @@ unsigned int Geometrie_intersecte_plage_angle(float angle1_min, float angle1_max return 1; } return 0; -} \ No newline at end of file +} + +/// @brief Déplace un point de la distance indiquée en se servant de l'angle de la position donnée. +struct position_t Geometrie_deplace(struct position_t position_depart, float distance_mm){ + struct position_t position_arrivée; + position_arrivée.angle_radian = position_depart.angle_radian; + position_arrivée.x_mm = cosf(position_depart.angle_radian) * distance_mm; + position_arrivée.y_mm = sinf(position_depart.angle_radian) * distance_mm; + + return position_arrivée; +} diff --git a/Geometrie_robot.h b/Geometrie_robot.h index ef75295..c65afbf 100644 --- a/Geometrie_robot.h +++ b/Geometrie_robot.h @@ -1,7 +1,9 @@ #define DISTANCE_ROUES_CENTRE_MM 112 // Distance entre le centre du robot et un angle du robot -#define RAYON_ROBOT 125 +#define RAYON_ROBOT 160. +#define DISTANCE_CENTRE_CAPTEUR 80. +#define ANGLE_PINCE (-150 * DEGRE_EN_RADIAN) // Distance entre le centre du robot et un bord du robot #define PETIT_RAYON_ROBOT_MM 108.2 diff --git a/Holonome2023.c b/Holonome2023.c index ae0c234..e5bee3e 100644 --- a/Holonome2023.c +++ b/Holonome2023.c @@ -86,10 +86,9 @@ int main() { switch(statu_match){ case MATCH_ATTENTE_TIRETTE: - Strategie_preparation(); - if(lire_couleur() == COULEUR_VERT){ - // Tout vert - i2c_annexe_couleur_balise(0b00011100, 0x0FFF); + if(lire_couleur() == COULEUR_JAUNE){ + // Tout jaune + i2c_annexe_couleur_balise(0b11111100, 0x0FFF); }else{ // Tout bleu i2c_annexe_couleur_balise(0b00000011, 0x0FFF); @@ -115,11 +114,9 @@ int main() { case MATCH_ARRET_EN_COURS: commande_vitesse_stop(); - i2c_annexe_active_deguisement(); - Score_set_funny_action(); if(Robot_est_dans_zone_depose(couleur)){ Score_set_pieds_dans_plat(); - } + } if (timer_match_ms > 100000){ // 100 secondes statu_match = MATCH_TERMINEE; diff --git a/Strategie.c b/Strategie.c index 68b5982..90c8a98 100644 --- a/Strategie.c +++ b/Strategie.c @@ -7,8 +7,6 @@ #include "Monitoring.h" #include "Moteurs.h" #include "Score.h" -#include "Strategie_prise_cerises.h" -#include "Strategie_pousse_gateau.h" #include "Strategie.h" #include "Trajet.h" #include "math.h" @@ -51,6 +49,8 @@ int Robot_est_dans_zone_cerise_droite(); enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t couleur, uint32_t step_ms); enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t couleur, uint32_t step_ms); +enum etat_action_t Strategie_calage(enum couleur_t couleur); + void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ const uint32_t temps_pre_fin_match = (97000 - 10000); @@ -94,336 +94,27 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ switch(etat_strategie){ case STRATEGIE_INIT: - if(couleur == COULEUR_BLEU){ - Localisation_set(225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN); - struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT}; - struct objectif_t objectif_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_GAUCHE}; - struct objectif_t objectif_3 = { .priorite = 3, .etat = A_FAIRE, .cible = CERISE_BAS}; - struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_DROITE}; - objectifs[0]= objectif_1; - objectifs[1]= objectif_2; - objectifs[2]= objectif_3; - objectifs[3]= objectif_4; - }else{ - Localisation_set(2000 - 225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN); - struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT}; - struct objectif_t objectif_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_DROITE}; - struct objectif_t objectif_3 = { .priorite = 3, .etat = A_FAIRE, .cible = CERISE_BAS}; - struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_GAUCHE}; - objectifs[0]= objectif_1; - objectifs[1]= objectif_2; - objectifs[2]= objectif_3; - objectifs[3]= objectif_4; - } - objectif_courant = &objectifs[4]; - Strategie_set_cerise_dans_robot(10); - etat_strategie = LANCER_PANIER; - break; - - case ALLER_CERISE_BAS: - - if(couleur == COULEUR_BLEU){ - angle_fin = 30. * DEGRE_EN_RADIAN; - angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin); - Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, - 650, Localisation_get().y_mm, - 490, 0, - 857, 0, - Localisation_get().angle_radian, angle_fin); - }else{ - angle_fin = -150. * DEGRE_EN_RADIAN; - angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin); - Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, - 2000 - 650, Localisation_get().y_mm, - 2000 - 490, 0, - 2000 - 857, 0, - Localisation_get().angle_radian, angle_fin); - } - Trajet_config(500,250); - - if(Strategie_parcourir_trajet(trajectoire, step_ms, RETOUR_SI_OBSTABLE) == TRAJET_TERMINE){ - etat_strategie = ATTRAPER_CERISE_BAS; - } - break; - - case ATTRAPER_CERISE_BAS: - recal_pos_y = 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){ - Strategie_set_cerise_dans_robot(6); - 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; - } - angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin); - - Trajet_config(500, 250); - Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 175, - Localisation_get().angle_radian, angle_fin); - - if(parcourt_trajet_simple(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_A; - recal_pos_x = 1000 - 15 - PETIT_RAYON_ROBOT_MM; - }else{ - longer_direction = LONGER_VERS_C; - 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){ - Strategie_set_cerise_dans_robot(6); - etat_strategie = ALLER_PANIER; - } - break; - - case ALLER_CERISE_GAUCHE: - Trajet_config(250, 250); - angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN); - if(couleur == COULEUR_BLEU){ - if(Strategie_aller_cerises_laterales_proches(couleur, step_ms)== ACTION_TERMINEE){ - etat_strategie = ATTRAPER_CERISE_GAUCHE; - Score_ajout_gateau(3); - } - }else{ - if(Strategie_aller_cerises_laterales_opposees(couleur, step_ms)== ACTION_TERMINEE){ - etat_strategie = ATTRAPER_CERISE_GAUCHE; - } - } - break; - - case ATTRAPER_CERISE_GAUCHE: - if(cerises_attraper_cerises_gauches(step_ms) == ACTION_TERMINEE){ - Strategie_set_cerise_dans_robot(10); - etat_strategie = ALLER_PANIER; - } - break; - - case ALLER_CERISE_DROITE: - Trajet_config(250, 250); - angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN); - if(couleur == COULEUR_BLEU){ - if(Strategie_aller_cerises_laterales_opposees(couleur, step_ms)== ACTION_TERMINEE){ - etat_strategie = ATTRAPER_CERISE_DROITE; - } - }else{ - if(Strategie_aller_cerises_laterales_proches(couleur, step_ms)== ACTION_TERMINEE){ - Score_ajout_gateau(3); - etat_strategie = ATTRAPER_CERISE_DROITE; - } - } - break; - - case ATTRAPER_CERISE_DROITE: - if(cerises_attraper_cerises_droite(step_ms) == ACTION_TERMINEE){ - Strategie_set_cerise_dans_robot(10); - etat_strategie = ALLER_PANIER; - } - break; - - case ALLER_PANIER: - if(Strategie_aller_panier(couleur, step_ms) == ACTION_TERMINEE){ - etat_strategie = LANCER_PANIER; - } - break; - - case LANCER_PANIER: - if(lance_balles_dans_panier(couleur, step_ms, Strategie_get_cerise_dans_robot())== ACTION_TERMINEE){ - Score_ajout_cerise(Strategie_get_cerise_dans_robot()); - Strategie_set_cerise_dans_robot(0); - 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->etat == A_FAIRE && objectifs[m_objectif].etat == A_FAIRE){ - if(objectif_courant->priorite > objectifs[m_objectif].priorite){ - objectif_courant = &(objectifs[m_objectif]); - } - } - } - - if(objectif_courant->etat == FAIT || pre_fin_match_active){ + if(Strategie_calage_debut(COULEUR_BLEU, step_ms) == ACTION_TERMINEE){ 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 = ALLER_CERISE_GAUCHE; - break; - case CERISE_DROITE: - etat_strategie = ALLER_CERISE_DROITE; - break; - } } - break; case RETOUR_MAISON: - i2c_annexe_plie_bras(); - i2c_annexe_desactive_turbine(); if(Strategie_pieds_dans_plat(couleur, step_ms) == ACTION_TERMINEE){ - // Si le robot est dans la zone du panier, jeter les cerises s'il en a - if(Strategie_get_cerise_dans_robot() > 0 && Robot_est_dans_zone_depose_panier(couleur)){ - //etat_strategie=LANCER_PANIER; // Il faut orienter le robot face au panier - etat_strategie=STRATEGIE_FIN; - }else{ - etat_strategie=STRATEGIE_FIN; - } + etat_strategie=STRATEGIE_FIN; } break; case STRATEGIE_FIN: pre_fin_match_active=true; - i2c_annexe_desactive_propulseur(); commande_vitesse_stop(); i2c_annexe_couleur_balise(0xE0, 0x0FFF); break; } } -enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t couleur, uint32_t step_ms){ - return Gateau_pousse_proche(couleur, step_ms); - /*if(couleur == COULEUR_BLEU){ - angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN); - Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, - 740, 3000 - 550, - 510, 3000 - 1580, - 500, 1400, - Localisation_get().angle_radian, angle_fin); - }else{ - angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN); - Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, - 2000 - 740, 3000 - 550, - 2000 - 510, 3000 - 1580, - 2000 - 500, 1400, - Localisation_get().angle_radian, angle_fin); - - } - return parcourt_trajet_simple(trajectoire, step_ms);*/ -} - -enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t couleur, uint32_t step_ms){ - struct trajectoire_t trajectoire; - float angle_fin; - if(couleur == COULEUR_BLEU){ - angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN); - Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, - 785, 3000 - 550, - 1550, 3000 - 785, - 1775, 1500, - Localisation_get().angle_radian, angle_fin); - }else{ - angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN); - Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, - 2000 - 785, 3000 - 550, - 2000 - 1550, 3000 - 785, - 2000 - 1775, 1500, - Localisation_get().angle_radian, angle_fin); - - } - return parcourt_trajet_simple(trajectoire, 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(TRAJECT_CONFIG_STD); - - // Definition des trajectoires - if(couleur == COULEUR_BLEU){ - // Si le robot est déjà dans la zone cerise haut - // On va en ligne droite - if(Robot_est_dans_zone_cerise_haut(couleur)){ - Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, - 180,2800, - Localisation_get().angle_radian, - Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN)); - }else if (Robot_est_dans_zone_cerise_gauche()){ - Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, - Localisation_get().x_mm + 330, Localisation_get().y_mm - 100, - 745, 3000 - 475, - 180, 3000 - 200, - Localisation_get().angle_radian, - Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN)); - }else{ - // Sinon, on a une courbe de bézier - Trajet_config(500,250); - Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, - 418, Localisation_get().y_mm, - 655, 2800, - 180, 2800, - Localisation_get().angle_radian, - Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN)); - } - }else{ // COULEUR_VERT - // 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-180, 2800, - Localisation_get().angle_radian ,Geometrie_get_angle_optimal(Localisation_get().angle_radian, -240. * DEGRE_EN_RADIAN)); - }else if (Robot_est_dans_zone_cerise_droite()){ - Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, - Localisation_get().x_mm + 330, Localisation_get().y_mm - 100, - 2000 - 745, 3000 - 475, - 2000 - 180, 3000 - 200, - Localisation_get().angle_radian, - Geometrie_get_angle_optimal(Localisation_get().angle_radian, -240. * DEGRE_EN_RADIAN)); - }else{ - // Sinon, on a une courbe de bézier - Trajet_config(500,250); - Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, - 2000 - 418, Localisation_get().y_mm, - 2000 - 655, 2800, - 2000 - 180, 2800, - Localisation_get().angle_radian, Geometrie_get_angle_optimal(Localisation_get().angle_radian, -240. * DEGRE_EN_RADIAN)); - } - } - - // parcours du trajet - if(Strategie_parcourir_trajet(trajectoire, step_ms, RETOUR_SI_OBSTABLE) == ACTION_TERMINEE){ - etat_action = ACTION_TERMINEE; - } - return etat_action; - -} /// Fonction de localisation approximatives pour la stratégie. @@ -551,194 +242,6 @@ int Robot_est_dans_zone_depose(enum couleur_t couleur){ } } -enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms, uint32_t nb_cerises){ - static enum { - CALAGE_PANIER_1, - RECULE_PANIER, - LANCE_DANS_PANIER, - }etat_lance_balles_dans_panier; - float recal_pos_x, recal_pos_y; - float angle_depart, angle_arrivee; - enum longer_direction_t longer_direction; - float point_x, point_y; - - enum etat_action_t etat_action = ACTION_EN_COURS; - - struct trajectoire_t trajectoire; - - switch(etat_lance_balles_dans_panier){ - case CALAGE_PANIER_1: - if(couleur == COULEUR_BLEU){ - recal_pos_x = RAYON_ROBOT; - longer_direction = LONGER_VERS_A; - }else{ - recal_pos_x = 2000- RAYON_ROBOT; - longer_direction = LONGER_VERS_C; - } - - if(calage_angle(longer_direction, recal_pos_x, 3000 - (PETIT_RAYON_ROBOT_MM), 120. *DEGRE_EN_RADIAN) == ACTION_TERMINEE){ - etat_lance_balles_dans_panier = RECULE_PANIER; - } - break; - - case RECULE_PANIER: - Trajet_config(120, 250); - if(couleur == COULEUR_BLEU){ - point_x = 180; - point_y = 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80; - }else{ - point_x = 1735; - point_y = 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80; - } - angle_depart = Localisation_get().angle_radian; - angle_arrivee = Geometrie_get_angle_optimal(angle_depart, 270. * DEGRE_EN_RADIAN); - Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, - point_x, point_y, - angle_depart, angle_arrivee); - - if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){ - etat_lance_balles_dans_panier = LANCE_DANS_PANIER; - Trajet_config(250, 250); - } - break; - - case LANCE_DANS_PANIER: - Asser_Position_maintien(); - if(lance_balles(step_ms, nb_cerises) == ACTION_TERMINEE){ - etat_action = ACTION_TERMINEE; - etat_lance_balles_dans_panier = CALAGE_PANIER_1; - } - break; - } - return etat_action; - -} - -/// @brief Active le propulseur, ouvre la porte, attend qql secondes. -/// @param step_ms : pas de temps. -/// @return ACTION_EN_COURS ou ACTION_TERMINEE -enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises){ - enum etat_action_t etat_action = ACTION_EN_COURS; - static uint32_t tempo_ms; - static uint32_t nb_iteration; - - static enum{ - LANCE_PROPULSEUR_ON, - LANCE_TEMPO_PROP_ON, - LANCE_PORTE_OUVERTE, - LANCE_OUVERTURE_INITIALE, - } etat_lance_balle = LANCE_PROPULSEUR_ON; - - switch(etat_lance_balle){ - case LANCE_PROPULSEUR_ON: - i2c_annexe_active_propulseur(); - tempo_ms = 1000; - nb_iteration=0; - etat_lance_balle = LANCE_OUVERTURE_INITIALE; - break; - - case LANCE_OUVERTURE_INITIALE: - if(temporisation_terminee(&tempo_ms, step_ms)){ - i2c_annexe_ouvre_porte(); - etat_lance_balle = LANCE_PORTE_OUVERTE; - tempo_ms = 250; - } - break; - - case LANCE_TEMPO_PROP_ON: - if(temporisation_terminee(&tempo_ms, step_ms)){ - i2c_annexe_ouvre_porte(); - nb_iteration++; - if(nb_iteration > nb_cerises){ - nb_iteration=0; - etat_action = ACTION_TERMINEE; - etat_lance_balle = LANCE_PROPULSEUR_ON; - i2c_annexe_desactive_propulseur(); - }else{ - etat_lance_balle = LANCE_PORTE_OUVERTE; - tempo_ms = 150; - } - } - break; - - case LANCE_PORTE_OUVERTE: - if(temporisation_terminee(&tempo_ms, step_ms)){ - i2c_annexe_mi_ferme_porte(); - etat_lance_balle = LANCE_TEMPO_PROP_ON; - tempo_ms = 500; - } - break; - - } - return etat_action; -} - -/// @brief Envoie le robot se caler dans l'angle en face de lui, recale la localisation -/// Ne fonctionne que contre les bordures haute et basse, pas contre les bordures gauche et droites -enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float x_mm, float y_mm, float angle_radian){ - enum etat_action_t etat_action = ACTION_EN_COURS; - struct position_t position; - struct trajectoire_t trajectoire; - static float y_pos_ref; - - static enum { - CONTACT_AVANT, - RECULE_BORDURE, - CONTACT_LATERAL, - RECALE_X - }etat_calage_angle; - - switch(etat_calage_angle){ - case CONTACT_AVANT: - if(cerise_accostage() == ACTION_TERMINEE){ - position = Localisation_get(); - //if(fabs(position.y_mm - y_mm) < SEUIL_RECAL_DIST_MM){ - Localisation_set_y(y_mm); - //} - //if(fabs(position.angle_radian - angle_radian) < SEUIL_RECAL_ANGLE_RADIAN){ - Localisation_set_angle(angle_radian); - //} - y_pos_ref = Localisation_get().y_mm; - etat_calage_angle = RECULE_BORDURE; - } - break; - - case RECULE_BORDURE: - commande_translation_recule_vers_trompe(); - position = Localisation_get(); - if(fabs(y_pos_ref - Localisation_get().y_mm) > 35){ - etat_calage_angle = CONTACT_LATERAL; - } - break; - - case CONTACT_LATERAL: - if(longer_direction == LONGER_VERS_A){ - commande_translation_avance_vers_A(); - if(i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF){ - etat_calage_angle = RECALE_X; - } - }else{ - commande_translation_avance_vers_C(); - if(i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF){ - etat_calage_angle = RECALE_X; - } - } - break; - - case RECALE_X: - etat_action = ACTION_TERMINEE; - commande_vitesse_stop(); - position = Localisation_get(); - //if(fabs(position.x_mm - x_mm) < SEUIL_RECAL_DIST_MM){ - Localisation_set_x(x_mm); - //} - etat_calage_angle = CONTACT_AVANT; - break; - - } - - return etat_action; -} /// @brief Renvoi 1 si on doit attendre le déclenchement de la tirette uint attente_tirette(void){ @@ -748,7 +251,7 @@ uint attente_tirette(void){ /// @brief Renvoi COULEUR_VERT ou COULEUR_BLEU enum couleur_t lire_couleur(void){ if (gpio_get(COULEUR)) - return COULEUR_VERT; + return COULEUR_JAUNE; return COULEUR_BLEU; } @@ -766,51 +269,6 @@ int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms){ } } -/// @brief Gère le chargement des balles au début du match -enum etat_action_t Strategie_preparation(){ - - static enum{ - PREP_INIT_V, - PREP_INIT_B, - PREP_ASPIRE, - PREP_ARRET_ASPIRATION, - PREP_TEMPO, - } etat_prep=PREP_INIT_V; - - switch(etat_prep){ - case PREP_INIT_V: - if(lire_couleur()== COULEUR_VERT){ - etat_prep = PREP_INIT_B; - } - break; - case PREP_INIT_B: - if(lire_couleur()== COULEUR_BLEU){ - etat_prep = PREP_ASPIRE; - } - break; - - case PREP_ASPIRE: - if(lire_couleur()== COULEUR_VERT){ - i2c_annexe_ferme_porte(); - i2c_annexe_active_turbine(); - etat_prep=PREP_ARRET_ASPIRATION; - } - break; - - case PREP_ARRET_ASPIRATION: - if(lire_couleur()== COULEUR_BLEU){ - i2c_annexe_desactive_turbine(); - etat_prep=PREP_TEMPO; - } - break; - - case PREP_TEMPO: - return ACTION_TERMINEE; - } - return ACTION_EN_COURS; - -} - enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, uint32_t step_ms){ struct trajectoire_t trajectoire; @@ -936,3 +394,81 @@ enum etat_action_t Strategie_pieds_dans_plat_trajet(struct objectif_t *objectif_ } } +enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_ms){ + // 1 Envoyer la commande pour détecter la bordure + // 2 Si la valeur de la bordure est valide, lire l'angle et la distance. + // Recaler la distance Y + // 3 Se tourner du bon angle (en fonction de la couleur et de l'angle lu) + // 4 Si la valeur de la bordure est valide, lire l'angle et la distance. + // 5 Se positionner à (250, 250), PINCE orientée à 45°. + static enum{ + CD_ENVOI_CDE_BORDURE, + CD_LECTURE_BORDURE_Y, + CD_ROTATION_VERS_X, + CD_LECTURE_BORDURE_X, + CD_ALLER_POSITION_INIT, + }etat_calage_debut=CD_ENVOI_CDE_BORDURE; + enum validite_vl53l8_t validite; + struct trajectoire_t trajectoire; + + float angle, distance; + + switch(etat_calage_debut){ + case CD_ENVOI_CDE_BORDURE: + i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE); + etat_calage_debut = CD_LECTURE_BORDURE_Y; + break; + + case CD_LECTURE_BORDURE_Y: + i2c_annexe_get_VL53L8(&validite, &angle, &distance); + if(validite){ + i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE); + commande_vitesse_stop(); + Localisation_set_y(distance + DISTANCE_CENTRE_CAPTEUR); + Localisation_set_angle((-90 * DEGRE_EN_RADIAN) - ANGLE_PINCE); + etat_calage_debut = CD_ROTATION_VERS_X; + } + break; + + case CD_ROTATION_VERS_X: + if(couleur == COULEUR_BLEU){ + Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian, + (-180 * DEGRE_EN_RADIAN) - ANGLE_PINCE); + }else{ + Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian, + (0 * DEGRE_EN_RADIAN) - ANGLE_PINCE); + } + if(Strategie_parcourir_trajet(trajectoire, step_ms, SANS_EVITEMENT) == ACTION_TERMINEE){ + i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE); + etat_calage_debut = CD_LECTURE_BORDURE_Y; + } + break; + + case CD_LECTURE_BORDURE_X: + i2c_annexe_get_VL53L8(&validite, &angle, &distance); + if(validite){ + i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE); + commande_vitesse_stop(); + if(couleur == COULEUR_BLEU){ + Localisation_set_x(distance + DISTANCE_CENTRE_CAPTEUR); + Localisation_set_angle((-180 * DEGRE_EN_RADIAN) - ANGLE_PINCE); + }else{ + Localisation_set_x(3000 - (distance + DISTANCE_CENTRE_CAPTEUR)); + Localisation_set_angle((0 * DEGRE_EN_RADIAN) - ANGLE_PINCE); + } + + etat_calage_debut = CD_ALLER_POSITION_INIT; + } + break; + + case CD_ALLER_POSITION_INIT: + if(couleur == COULEUR_BLEU){ + return Strategie_aller_a(250, 250, step_ms); + }else{ + return Strategie_aller_a(3000 - 250, 250, step_ms); + } + break; + } + + return ACTION_EN_COURS; +} \ No newline at end of file diff --git a/Strategie.h b/Strategie.h index b4cda19..8f10468 100644 --- a/Strategie.h +++ b/Strategie.h @@ -22,7 +22,7 @@ enum longer_direction_t{ enum couleur_t{ COULEUR_BLEU=0, - COULEUR_VERT, + COULEUR_JAUNE, COULEUR_INCONNUE, }; @@ -45,14 +45,10 @@ enum etat_action_t cerise_accostage(void); enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_direction); enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms); enum etat_action_t parcourt_trajet_simple_sans_evitement(struct trajectoire_t trajectoire, uint32_t step_ms); -enum etat_action_t depose_cerises(uint32_t step_ms); -enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float x_mm, float y_mm, float angle_radian); void Homologation(uint32_t step_ms); enum couleur_t lire_couleur(void); uint attente_tirette(void); -enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises); -int test_panier(void); void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms); int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms); @@ -66,6 +62,8 @@ enum etat_action_t Strategie_pieds_dans_plat_trajet(struct objectif_t *objectif_ enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t step_ms); enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, uint32_t step_ms); +enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_ms); + extern float distance_obstacle; // STRATEGIE_H diff --git a/Strategie_2024_pots.c b/Strategie_2024_pots.c index f9ad27c..856d3c4 100644 --- a/Strategie_2024_pots.c +++ b/Strategie_2024_pots.c @@ -1,6 +1,20 @@ #include "math.h" +#include "Strategie.h" +#include "Geometrie_robot.h" #include "Strategie_2024_pots.h" +#define DISTANCE_APPROCHE_POT + +float angle_bras[6] = +{ + 180 * DEGRE_EN_RADIAN, + 120 * DEGRE_EN_RADIAN, + 60 * DEGRE_EN_RADIAN, + 0, + -60 * DEGRE_EN_RADIAN, + -120 * DEGRE_EN_RADIAN +}; + struct position_t position_pots_dans_groupe_pot[5] = { {.x_mm = -40, .y_mm = 69.2, .angle_radian = 120 * DEGRE_EN_RADIAN}, @@ -42,4 +56,17 @@ struct position_t groupe_pot_get_pot(unsigned int groupe_pot, unsigned int num_p position_pot.angle_radian = my_position_groupe_pot.angle_radian + angle_groupe_pot; return position_pot; -} \ No newline at end of file +} + +/// @brief Fonction qui déplace le robot jusqu'à la zone pour attraper les pots et qui attrape les 5 pots +enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot){ + // Parcourir la trajectoire pour aller jusqu'au premier pot + + // Pour chaque pot + // Baisser le bras correspondant + // Aller jusqu'au point de prise de pot en s'orientant pour prendre le pot + // Avancer de X cm en direction du pot + // Reculer dans l'axe de prise et rejoindre le point de prise suivant + // Pendant le mouvement, apres 1 sec (à confirmer) Lever le bras + +} diff --git a/Strategie_2024_pots.h b/Strategie_2024_pots.h index 9867ccc..867991e 100644 --- a/Strategie_2024_pots.h +++ b/Strategie_2024_pots.h @@ -13,4 +13,11 @@ #define GROUPE_POT_R2 4 #define GROUPE_POT_R1 5 +#define BRAS_1 0 +#define BRAS_2 1 +#define BRAS_3 2 +#define BRAS_4 3 +#define BRAS_5 4 +#define BRAS_6 5 + struct position_t groupe_pot_get_pot(unsigned int groupe_pot, unsigned int num_pot); diff --git a/Strategie_pousse_gateau.c b/Strategie_pousse_gateau.c deleted file mode 100644 index 66caa11..0000000 --- a/Strategie_pousse_gateau.c +++ /dev/null @@ -1,84 +0,0 @@ -#include "i2c_annexe.h" -#include "Localisation.h" -#include "Monitoring.h" -#include "Strategie.h" -#include "Trajectoire.h" -#include "Trajet.h" -#include "Geometrie.h" - -enum etat_action_t Gateau_pousse_proche(enum couleur_t couleur, uint32_t step_ms){ - static enum { - ALLER_1, - ALLER_2, - ALLER_3 - } etat=ALLER_1; - float point_x, point_y; - float angle_fin; - struct trajectoire_t trajectoire; - - - switch(etat){ - case ALLER_1: - if(couleur == COULEUR_BLEU){ - angle_fin = 90. * DEGRE_EN_RADIAN; - point_x = 310; - }else{ - angle_fin = 205. * DEGRE_EN_RADIAN; - point_x = 1700; - } - angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin); - - Trajet_config(100, 250); - Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 380, - Localisation_get().angle_radian, angle_fin); - - if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ - etat=ALLER_2; - i2c_annexe_deplie_bras(); - } - break; - - case ALLER_2: - if(couleur == COULEUR_BLEU){ - angle_fin = 90. * DEGRE_EN_RADIAN; - point_x = 310; - }else{ - angle_fin = 205. * DEGRE_EN_RADIAN; - point_x = 1700; - } - angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin); - - Trajet_config(500,250); - Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 1700, - Localisation_get().angle_radian, angle_fin); - - if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ - i2c_annexe_plie_bras(); - etat=ALLER_3; - } - break; - - case ALLER_3: - if(couleur == COULEUR_BLEU){ - angle_fin = -150. * DEGRE_EN_RADIAN; - point_x = 225; - }else{ - angle_fin = 30. * DEGRE_EN_RADIAN; - point_x = 1775; - } - angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin); - - Trajet_config(250, 250); - Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 1600, - Localisation_get().angle_radian, angle_fin); - - if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ - etat=ALLER_1; - return ACTION_TERMINEE; - } - break; - - } - return ACTION_EN_COURS; - -} \ No newline at end of file diff --git a/Strategie_pousse_gateau.h b/Strategie_pousse_gateau.h deleted file mode 100644 index d2e7539..0000000 --- a/Strategie_pousse_gateau.h +++ /dev/null @@ -1,3 +0,0 @@ -#include "pico/stdlib.h" - -enum etat_action_t Gateau_pousse_proche(enum couleur_t couleur, uint32_t step_ms); \ No newline at end of file diff --git a/Strategie_prise_cerises.c b/Strategie_prise_cerises.c deleted file mode 100644 index e495ae9..0000000 --- a/Strategie_prise_cerises.c +++ /dev/null @@ -1,466 +0,0 @@ -#include "stdio.h" - -#include "Strategie_prise_cerises.h" -#include "Commande_vitesse.h" -#include "Geometrie.h" -#include "Geometrie_robot.h" -#include "Localisation.h" -#include "math.h" -#include "i2c_annexe.h" -#include "Trajet.h" - -// Rotation en rad/s pour accoster les cerises -#define ROTATION_CERISE 0.5f - -// Distance la plus éloignée du bord où le robot est capable d'aspirer les cerises en longeant la bodure. -#define MAX_LONGE_MM 240 -#define MAX_ASPIRE_CERISE_MM 320 - -// Translation en mm/s pour aspirer les cerises -#define TRANSLATION_CERISE 70 - -// Seuil angulaire pour la détection de la fin de la barrette de cerises -#define SEUIL_ANGLE_CERISE (2. * DEGRE_EN_RADIAN) - -void commande_rotation_contacteur_longer_A(); -void commande_rotation_contacteur_longer_C(); -enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum longer_direction_t longer_direction, float pos_recal_x_mm); -enum etat_action_t demarre_turbine(uint32_t step_ms); - -enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction); - - -float vitesse_accostage_mm_s=100; - - - - -enum etat_action_t cerises_attraper_cerises_droite(uint32_t step_ms){ - static enum { - ATTRAPE_CERISE_DEMI_BAS, - REVENIR_CENTRE, - ATTRAPE_CERISE_DEMI_HAUT, - }etat_attrappe_cerises_droite = ATTRAPE_CERISE_DEMI_BAS; - struct trajectoire_t trajectoire; - float angle_fin; - float pos_recal_x_mm = 2000 - (PETIT_RAYON_ROBOT_MM + 30); - - switch (etat_attrappe_cerises_droite){ - case ATTRAPE_CERISE_DEMI_BAS: - if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_C, pos_recal_x_mm) == ACTION_TERMINEE){ - etat_attrappe_cerises_droite = REVENIR_CENTRE; - } - break; - - case REVENIR_CENTRE: - angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 30 * DEGRE_EN_RADIAN); - - Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, - 2000 - 180, 3000 - 1600, Localisation_get().angle_radian, angle_fin); - - if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ - etat_attrappe_cerises_droite = ATTRAPE_CERISE_DEMI_HAUT; - } - break; - - case ATTRAPE_CERISE_DEMI_HAUT: - if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_A, pos_recal_x_mm) == ACTION_TERMINEE){ - etat_attrappe_cerises_droite = ATTRAPE_CERISE_DEMI_BAS; - return ACTION_TERMINEE; - } - break; - - } - return ACTION_EN_COURS; - -} - -enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms){ - static enum { - ATTRAPE_CERISE_DEMI_BAS, - REVENIR_CENTRE, - ATTRAPE_CERISE_DEMI_HAUT, - }etat_attrappe_cerises_gauche = ATTRAPE_CERISE_DEMI_BAS; - struct trajectoire_t trajectoire; - float angle_fin; - float pos_recal_x_mm = PETIT_RAYON_ROBOT_MM + 30; - - switch (etat_attrappe_cerises_gauche){ - case ATTRAPE_CERISE_DEMI_BAS: - if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_A, pos_recal_x_mm) == ACTION_TERMINEE){ - etat_attrappe_cerises_gauche = REVENIR_CENTRE; - } - break; - - case REVENIR_CENTRE: - angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150 * DEGRE_EN_RADIAN); - - Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, - 180, 1500 - 75, Localisation_get().angle_radian, angle_fin); - - if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ - etat_attrappe_cerises_gauche = ATTRAPE_CERISE_DEMI_HAUT; - } - break; - - case ATTRAPE_CERISE_DEMI_HAUT: - if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_C, pos_recal_x_mm) == ACTION_TERMINEE){ - etat_attrappe_cerises_gauche = ATTRAPE_CERISE_DEMI_BAS; - return ACTION_TERMINEE; - } - break; - - } - return ACTION_EN_COURS; - -} - -enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum longer_direction_t longer_direction, float pos_recal_x_mm){ - // 1 accoster - // Demarrer la turbine - // 2 Longer en aspirant - // 3 avancer en aspirant - static enum { - ACCOSTAGE, - DEMARRE_TURBINE, - LONGE, - TOURNE, - AVANCE, - }etat_attrappe_demi_cerise=ACCOSTAGE; - const float continu_avance_mm = 60; - static float orientation_ref; - static float pos_y_ref; - struct trajectoire_t trajectoire; - - switch(etat_attrappe_demi_cerise){ - case ACCOSTAGE: - if(cerise_accostage() == ACTION_TERMINEE){ - Localisation_set_x(pos_recal_x_mm); - orientation_ref = Localisation_get().angle_radian; - etat_attrappe_demi_cerise = DEMARRE_TURBINE; - } - break; - - case DEMARRE_TURBINE: - if(demarre_turbine(step_ms) == ACTION_TERMINEE){ - etat_attrappe_demi_cerise = LONGE; - } - break; - - case LONGE: - avance_puis_longe_bordure(longer_direction); - // La fonction cerise_longer_bord n'est efficace que tant que le robot a ses deux contacteur sur le support - // Le robot n'a les deux contacteurs sur le support que tant qu'il est à moins de 240mm (MAX_LONGE_MM) de la bordure - // ou 120 (MAX_LONGE_MM/2) du milieu de la bordure - // En fonction du demi-terrain sur lequel se trouve le robot, on surveille la position en Y pour respecter cette condition - /*if( (Localisation_get().y_mm > 1500 + MAX_LONGE_MM/2 ) || (Localisation_get().y_mm < 1500 - MAX_LONGE_MM/2 )){ - etat_attrappe_demi_cerise = AVANCE; - }*/ - if(fabs(orientation_ref - Localisation_get().angle_radian) > SEUIL_ANGLE_CERISE){ - etat_attrappe_demi_cerise = TOURNE; - pos_y_ref = Localisation_get().y_mm; - } - break; - - case TOURNE: - Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian, orientation_ref); - if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){ - etat_attrappe_demi_cerise = AVANCE; - } - break; - - case AVANCE: - if(longer_direction == LONGER_VERS_A){ - commande_translation_longer_vers_A(); - }else{ - commande_translation_longer_vers_C(); - } - - if( (Localisation_get().y_mm > pos_y_ref + continu_avance_mm ) || (Localisation_get().y_mm < pos_y_ref - continu_avance_mm )){ - etat_attrappe_demi_cerise = ACCOSTAGE; - i2c_annexe_desactive_turbine(); - commande_vitesse_stop(); - return ACTION_TERMINEE; - } - break; - } - return ACTION_EN_COURS; - -} - -/// @brief Fonction pour attraper les cerises sur les supports perpendiculaires à la bordure. -/// Le robot accoste, longe le support cerise vers la bordure, active la turbine, puis longe le support cerise jusqu'à son bout. -/// @param longer_direction : direction dans laquelle se trouve la bordure -/// @param step_ms : fréquence d'appel -/// @param pos_recalage_x_mm : Position de recalage contre le support de cerises -/// @param pos_recalage_y_mm : Position de recalage contre la bordure du terrain -/// @return ACTION_EN_COURS ou ACTION_TERMINEE -enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_recalage_x_mm, float pos_recalage_y_mm){ - enum etat_action_t etat_action = ACTION_EN_COURS; - enum longer_direction_t longer_direction_aspire; - static uint32_t tempo_ms = 0; - static enum { - ATTRAPE_INIT, - ATTRAPE_VERS_BORDURE, - TURBINE_DEMARRAGE, - TURBINE_DEMARRAGE_TEMPO, - ASPIRE_LONGE, - ASPIRE_LIBRE, - ASPIRE_FIN - } etat_attrape=ATTRAPE_INIT; - - switch(etat_attrape){ - case ATTRAPE_INIT: - etat_attrape=ATTRAPE_VERS_BORDURE; - break; - - case ATTRAPE_VERS_BORDURE: - avance_puis_longe_bordure(longer_direction); - if( (longer_direction == LONGER_VERS_A) && (i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF) || - (longer_direction == LONGER_VERS_C) && (i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF) ){ - Localisation_set_x(pos_recalage_x_mm); - Localisation_set_y(pos_recalage_y_mm); - etat_attrape = TURBINE_DEMARRAGE; - } - break; - - case TURBINE_DEMARRAGE: - i2c_annexe_ferme_porte(); - i2c_annexe_active_turbine(); - commande_vitesse_stop(); - tempo_ms = 1000; - etat_attrape = TURBINE_DEMARRAGE_TEMPO; - - break; - - case TURBINE_DEMARRAGE_TEMPO: - if(tempo_ms < step_ms){ - etat_attrape = ASPIRE_LONGE; - }else{ - tempo_ms -= step_ms; - } - break; - - case ASPIRE_LONGE: - longer_direction_aspire = inverser_longe_direction(longer_direction); - avance_puis_longe_bordure(longer_direction_aspire); - // La fonction cerise_longer_bord n'est efficace que tant que le robot a ses deux contacteur sur le support - // Le robot n'a les deux contacteurs sur le support que tant qu'il est à moins de 240mm (MAX_LONGE_MM) de la bordure - // En fonction du demi-terrain sur lequel se trouve le robot, on surveille la position en Y pour respecter cette condition - if( ((Localisation_get().y_mm > 1500) && (Localisation_get().y_mm < (3000 - MAX_LONGE_MM) )) || - ((Localisation_get().y_mm < 1500) && (Localisation_get().y_mm > (MAX_LONGE_MM))) ){ - etat_attrape = ASPIRE_LIBRE; - } - break; - - case ASPIRE_LIBRE: - longer_direction_aspire = inverser_longe_direction(longer_direction); - if(longer_direction_aspire == LONGER_VERS_A){ - commande_translation_longer_vers_A(); - }else{ - commande_translation_longer_vers_C(); - } - if( ((Localisation_get().y_mm > 1500) && (Localisation_get().y_mm < (3000 - MAX_ASPIRE_CERISE_MM) )) || - ((Localisation_get().y_mm < 1500) && (Localisation_get().y_mm > (MAX_ASPIRE_CERISE_MM))) ){ - etat_attrape = ASPIRE_FIN; - } - break; - - case ASPIRE_FIN: - commande_vitesse_stop(); - i2c_annexe_desactive_turbine(); - etat_action = ACTION_TERMINEE; - etat_attrape = ATTRAPE_INIT; - break; - } - return etat_action; -} - -/// @brief Envoie l'ordre de démarrer la turbine puis attends 1 seconde -/// @param step_ms -/// @return ACTION_EN_COURS ou ACTION_TERMINEE -enum etat_action_t demarre_turbine(uint32_t step_ms){ - static enum { - TURBINE_DEMARRAGE, - TURBINE_DEMARRAGE_TEMPO, - } etat_demarrage_turbine=TURBINE_DEMARRAGE; - static uint32_t tempo_ms; - - switch(etat_demarrage_turbine){ - case TURBINE_DEMARRAGE: - i2c_annexe_ferme_porte(); - i2c_annexe_active_turbine(); - commande_vitesse_stop(); - tempo_ms = 1000; - etat_demarrage_turbine = TURBINE_DEMARRAGE_TEMPO; - - break; - - case TURBINE_DEMARRAGE_TEMPO: - if(temporisation_terminee(&tempo_ms, step_ms)){ - etat_demarrage_turbine = TURBINE_DEMARRAGE; - return ACTION_TERMINEE; - } - - break; - } - return ACTION_EN_COURS; - -} - -/// @brief Fonction pour accoster et longer une bordure -/// @param longer_direction : direction dans laquelle le robot va aller une fois le long de la bordure -/// @return ACTION_EN_COURS -enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_direction){ - - static enum { - LONGE_INIT, - LONGE_NORMAL, - LONGE_COLLE - } etat_longer_bord=LONGE_INIT; - - - - switch (etat_longer_bord){ - case LONGE_INIT: - etat_longer_bord=LONGE_NORMAL; - break; - - case LONGE_NORMAL: - if(longer_direction==LONGER_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; - } - }else{ - 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; - } - - } - break; - - case LONGE_COLLE: - if(cerise_accostage() == ACTION_TERMINEE){ - etat_longer_bord = LONGE_NORMAL; - } - } - return ACTION_EN_COURS; - -} - -/// @brief Viens positionner le robot contre une bordure ou un support cerise devant lui. -enum etat_action_t cerise_accostage(void){ - enum etat_action_t etat_action = ACTION_EN_COURS; - float rotation; - - static enum { - CERISE_AVANCE_DROIT, - CERISE_TOURNE_CONTACTEUR_LONGER_A, - CERISE_TOURNE_CONTACTEUR_LONGER_C, - CERISE_ACCOSTE - } etat_accostage=CERISE_AVANCE_DROIT; - - switch (etat_accostage) - { - case CERISE_AVANCE_DROIT: - commande_translation_avance_vers_trompe(); - 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_LONGER_A: - commande_rotation_contacteur_longer_A(); - 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_LONGER_C: - commande_rotation_contacteur_longer_C(); - 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; - break; - - default: - break; - } - - return etat_action; -} - -void commande_rotation_contacteur_longer_A(){ - commande_rotation(ROTATION_CERISE, RAYON_ROBOT, 0); -} - -void commande_rotation_contacteur_longer_C(){ - 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); -} - -void commande_translation_avance_vers_trompe(){ - commande_vitesse(vitesse_accostage_mm_s * cos(-M_PI/6), vitesse_accostage_mm_s * sin(-M_PI/6), 0); -} - -void commande_translation_recule_vers_trompe(){ - commande_vitesse(-vitesse_accostage_mm_s * cos(-M_PI/6), -vitesse_accostage_mm_s * sin(-M_PI/6), 0); -} - -void commande_translation_avance_vers_A(){ - // V_x : V * cos (60°) = V / 2 - // V_y : V * sin (60°) = V * RACINE(3) / 2 - commande_vitesse(vitesse_accostage_mm_s/2., vitesse_accostage_mm_s / 2. * RACINE_DE_3, 0); -} - -void commande_translation_avance_vers_C(){ - // V_x : -V * cos (60°) = -V / 2 - // V_y : -V * sin (60°) = -V * RACINE(3) / 2 - commande_vitesse(-vitesse_accostage_mm_s/2., -vitesse_accostage_mm_s / 2. * RACINE_DE_3, 0); -} - -enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction){ - if(direction ==LONGER_VERS_A){ - return LONGER_VERS_C; - } - return LONGER_VERS_A; -} \ No newline at end of file diff --git a/Strategie_prise_cerises.h b/Strategie_prise_cerises.h deleted file mode 100644 index 6cf66d1..0000000 --- a/Strategie_prise_cerises.h +++ /dev/null @@ -1,10 +0,0 @@ -#include "Strategie.h" -enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_x_mm, float pos_y_mm); -enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms); -enum etat_action_t cerises_attraper_cerises_droite(uint32_t step_ms); -void commande_translation_longer_vers_A(); -void commande_translation_longer_vers_C(); -void commande_translation_avance_vers_trompe(); -void commande_translation_recule_vers_trompe(); -void commande_translation_avance_vers_A(); -void commande_translation_avance_vers_C(); \ No newline at end of file diff --git a/Test.c b/Test.c index 4a19d80..77b2c92 100644 --- a/Test.c +++ b/Test.c @@ -44,7 +44,7 @@ int test_APDS9960(void); int test_asser_position_avance(void); -int test_asser_position_avance_et_tourne(int, int); +int test_asser_position_avance_et_tourne(int); int test_transition_gyro_pas_gyro(void); void affiche_localisation(void); int test_capteurs_balise(void); @@ -66,7 +66,6 @@ int mode_test(){ printf("J - Asser Position - avance et tourne (sans gyro)\n"); printf("N - Fonctions geometrique\n"); printf("O - Analyse obstacle\n"); - printf("P - Asser Position - perturbation\n"); printf("Q - Asser Position - transition Gyro -> Pas gyro\n"); printf("R - Test des logs...\n"); printf("T - Trajectoire\n"); @@ -111,12 +110,12 @@ int mode_test(){ case 'I': case 'i': - while(test_asser_position_avance_et_tourne(1, 0)); + while(test_asser_position_avance_et_tourne(1)); break; case 'J': case 'j': - while(test_asser_position_avance_et_tourne(0, 0)); + while(test_asser_position_avance_et_tourne(0)); break; @@ -130,11 +129,6 @@ int mode_test(){ while(test_angle_balise()); break; - case 'P': - case 'p': - while(test_asser_position_avance_et_tourne(1, 1)); - break; - case 'Q': case 'q': while(test_transition_gyro_pas_gyro()); @@ -298,7 +292,7 @@ int test_transition_gyro_pas_gyro(void){ /// @param m_gyro : 1 pour utiliser le gyroscope, 0 sans /// @param propulseur : 1 pour activer le propulseur toutes les secondes /// @return -int test_asser_position_avance_et_tourne(int m_gyro, int propulseur){ +int test_asser_position_avance_et_tourne(int m_gyro){ int lettre, _step_ms = 1, _step_ms_gyro = 2, step_gyro=2, propulseur_on=0; uint32_t temps_ms = 0, temps_ms_init = 0, temps_ms_old, tempo_prop=0; struct position_t position_consigne; @@ -313,9 +307,6 @@ int test_asser_position_avance_et_tourne(int m_gyro, int propulseur){ Gyro_Init(); printf("C'est parti !\n"); } - if(propulseur){ - i2c_maitre_init(); - } stdio_flush(); set_position_avec_gyroscope(m_gyro); @@ -327,22 +318,7 @@ int test_asser_position_avance_et_tourne(int m_gyro, int propulseur){ do{ while(temps_ms == Temps_get_temps_ms()){ - if(propulseur){ - i2c_gestion(i2c0); - i2c_annexe_gestion(); - } - } - if(propulseur){ - tempo_prop++; - if(tempo_prop>1000){ - tempo_prop=0; - if(propulseur_on){ - i2c_annexe_active_propulseur(); - }else{ - i2c_annexe_desactive_propulseur(); - } - propulseur_on = !propulseur_on; - } + } temps_ms = Temps_get_temps_ms(); @@ -356,9 +332,6 @@ int test_asser_position_avance_et_tourne(int m_gyro, int propulseur){ AsserMoteur_Gestion(_step_ms); position_consigne.angle_radian = (float) (temps_ms - temps_ms_init) /1000.; - if(!propulseur){ - position_consigne.y_mm = (float) (temps_ms - temps_ms_init) * 100. / 1000.; - } Asser_Position(position_consigne); diff --git a/Test_strategie.c b/Test_strategie.c index e0ed276..0950be5 100644 --- a/Test_strategie.c +++ b/Test_strategie.c @@ -14,7 +14,6 @@ #include "QEI.h" #include "Robot_config.h" #include "Strategie.h" -#include "Strategie_prise_cerises.h" #include "Temps.h" #include "Trajet.h" #include "Trajectoire.h" @@ -73,59 +72,24 @@ void affichage_test_strategie(){ int test_strategie(){ - printf("A - Accoster.\n"); printf("C - Couleur et tirette.\n"); - printf("D - Attraper cerises laterales.\n"); printf("E - Evitement\n"); - printf("H - Homologation.\n"); - printf("L - Longer.\n"); - printf("P - Panier.\n"); int lettre; do{ lettre = getchar_timeout_us(0); }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0); switch(lettre){ - case 'a': - case 'A': - while(test_accostage()); - break; - case 'c': case 'C': while(test_tirette_et_couleur()); break; - case 'd': - case 'D': - while(test_cerise_laterales_droite()); - break; - case 'e': case 'E': while(test_evitement()); break; - case 'g': - case 'G': - while(test_cerise_laterales_gauche()); - break; - - case 'h': - case 'H': - //while(test_homologation()); - break; - - case 'l': - case 'L': - while(test_longe()); - break; - - case 'p': - case 'P': - while(test_panier()); - break; - case 'q': case 'Q': return 0; @@ -135,246 +99,6 @@ int test_strategie(){ } -int test_cerise_laterales_gauche(){ - int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init; - uint32_t temps_cycle_old; - enum etat_action_t etat_action; - printf("Attaper cerise latérales\n"); - - i2c_maitre_init(); - Trajet_init(); - Balise_VL53L1X_init(); - //printf("Init gyroscope\n"); - set_position_avec_gyroscope(1); - if(get_position_avec_gyroscope()){ - Gyro_Init(); - } - - stdio_flush(); - - multicore_launch_core1(affichage_test_strategie); - - temps_ms = Temps_get_temps_ms(); - temps_ms_init = temps_ms; - temps_cycle_old= time_us_32(); - - uint32_t tempo_ms=1000; - - Localisation_set(250, 1500, -150 * DEGRE_EN_RADIAN); - do{ - etat_action = ACTION_EN_COURS; - - temps_cycle_check(); - - i2c_gestion(i2c0); - i2c_annexe_gestion(); - Balise_VL53L1X_gestion(); - - // Routines à 1 ms - - if(temps_ms != Temps_get_temps_ms()){ - static enum { - TEMPO_AVANT, - TEST, - TEMPO_APRES - }etat_test; - temps_ms = Temps_get_temps_ms(); - QEI_update(); - Localisation_gestion(); - AsserMoteur_Gestion(_step_ms); - - - // Routine à 2 ms - if(temps_ms % _step_ms_gyro == 0){ - if(get_position_avec_gyroscope()){ - Gyro_Read(_step_ms_gyro); - } - } - - switch(etat_test){ - case TEMPO_AVANT: - if(temporisation_terminee(&tempo_ms, _step_ms)){ - etat_test = TEST; - } - break; - case TEST: - if(cerises_attraper_cerises_gauches(_step_ms) == ACTION_TERMINEE){ - tempo_ms = 1000; - etat_test = TEMPO_APRES; - } - break; - case TEMPO_APRES: - if(temporisation_terminee(&tempo_ms, _step_ms)){ - etat_test = TEMPO_AVANT; - etat_action = ACTION_TERMINEE; - } - break; - } - - - - } - //lettre = getchar_timeout_us(0); - //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0)); - }while(etat_action == ACTION_EN_COURS); - Moteur_Stop(); - return 0; - -} - -int test_cerise_laterales_droite(){ - int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init; - uint32_t temps_cycle_old; - enum etat_action_t etat_action; - printf("Attaper cerise latérales\n"); - - i2c_maitre_init(); - Trajet_init(); - Balise_VL53L1X_init(); - //printf("Init gyroscope\n"); - set_position_avec_gyroscope(1); - if(get_position_avec_gyroscope()){ - Gyro_Init(); - } - - stdio_flush(); - - multicore_launch_core1(affichage_test_strategie); - - temps_ms = Temps_get_temps_ms(); - temps_ms_init = temps_ms; - temps_cycle_old= time_us_32(); - - uint32_t tempo_ms=1000; - - Localisation_set(1775, 1500, 30 * DEGRE_EN_RADIAN); - do{ - etat_action = ACTION_EN_COURS; - - temps_cycle_check(); - - i2c_gestion(i2c0); - i2c_annexe_gestion(); - Balise_VL53L1X_gestion(); - - // Routines à 1 ms - - if(temps_ms != Temps_get_temps_ms()){ - static enum { - TEMPO_AVANT, - TEST, - TEMPO_APRES - }etat_test; - temps_ms = Temps_get_temps_ms(); - QEI_update(); - Localisation_gestion(); - AsserMoteur_Gestion(_step_ms); - - - // Routine à 2 ms - if(temps_ms % _step_ms_gyro == 0){ - if(get_position_avec_gyroscope()){ - Gyro_Read(_step_ms_gyro); - } - } - - switch(etat_test){ - case TEMPO_AVANT: - if(temporisation_terminee(&tempo_ms, _step_ms)){ - etat_test = TEST; - } - break; - case TEST: - if(cerises_attraper_cerises_droite(_step_ms) == ACTION_TERMINEE){ - tempo_ms = 1000; - etat_test = TEMPO_APRES; - } - break; - case TEMPO_APRES: - if(temporisation_terminee(&tempo_ms, _step_ms)){ - etat_test = TEMPO_AVANT; - etat_action = ACTION_TERMINEE; - } - break; - } - - } - //lettre = getchar_timeout_us(0); - //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0)); - }while(etat_action == ACTION_EN_COURS); - Moteur_Stop(); - return 0; - -} - -int test_homologation(){ - int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init; - uint32_t temps_cycle[10], temps_cycle_old, index_temps_cycle=0; - printf("Homologation\n"); - - i2c_maitre_init(); - Trajet_init(); - Balise_VL53L1X_init(); - //printf("Init gyroscope\n"); - set_position_avec_gyroscope(1); - if(get_position_avec_gyroscope()){ - Gyro_Init(); - } - - stdio_flush(); - - multicore_launch_core1(affichage_test_strategie); - - temps_ms = Temps_get_temps_ms(); - temps_ms_init = temps_ms; - temps_cycle_old= time_us_32(); - do{ - /*temps_cycle[index_temps_cycle++]= time_us_32() - temps_cycle_old; - if(index_temps_cycle >= 10){ - for(int i=0; i<10; i++){ - printf("t_cycle:%d\n", temps_cycle[i]); - } - index_temps_cycle=0; - } - temps_cycle_old=time_us_32();*/ - - temps_cycle_check(); - - i2c_gestion(i2c0); - i2c_annexe_gestion(); - Balise_VL53L1X_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){ - if(get_position_avec_gyroscope()){ - Gyro_Read(_step_ms_gyro); - } - } - - //Homologation(_step_ms); - - } - //lettre = getchar_timeout_us(0); - //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0)); - }while(1); - printf("STRATEGIE_LOOP_2\n"); - printf("Lettre : %d; %c\n", lettre, lettre); - - if(lettre == 'q' && lettre == 'Q'){ - return 0; - } - return 0; - -} void affichage_test_evitement(){ while(1){ @@ -471,201 +195,6 @@ int test_evitement(){ } -int test_panier(){ - int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init; - printf("Test panier\n"); - - static enum{ - TEST_PANIER_INIT, - TEST_PANIER_ASPIRE, - TEST_PANIER_TEMPO, - TEST_PANIER_LANCE_BALLES, - TEST_PANIER_PORTE_OUVERTE, - } etat_test_panier=TEST_PANIER_INIT; - static uint32_t tempo_ms; - - i2c_maitre_init(); - Trajet_init(); - //printf("Init gyroscope\n"); - set_position_avec_gyroscope(0); - if(get_position_avec_gyroscope()){ - Gyro_Init(); - } - - stdio_flush(); - - multicore_launch_core1(affichage_test_strategie); - - temps_ms = Temps_get_temps_ms(); - temps_ms_init = temps_ms; - do{ - i2c_gestion(i2c0); - i2c_annexe_gestion(); - Balise_VL53L1X_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){ - if(get_position_avec_gyroscope()){ - Gyro_Read(_step_ms_gyro); - } - } - - switch(etat_test_panier){ - case TEST_PANIER_INIT: - if(lire_couleur()== COULEUR_VERT){ - i2c_annexe_ferme_porte(); - i2c_annexe_active_turbine(); - etat_test_panier=TEST_PANIER_ASPIRE; - } - break; - - case TEST_PANIER_ASPIRE: - if(lire_couleur()== COULEUR_BLEU){ - i2c_annexe_desactive_turbine(); - tempo_ms = 3000; - etat_test_panier=TEST_PANIER_TEMPO; - } - break; - case TEST_PANIER_TEMPO: - if(temporisation_terminee(&tempo_ms, _step_ms)){ - etat_test_panier=TEST_PANIER_LANCE_BALLES; - } - break; - - case TEST_PANIER_LANCE_BALLES: - if(lance_balles(_step_ms, 10) == ACTION_TERMINEE){ - etat_test_panier=TEST_PANIER_PORTE_OUVERTE; - } - break; - - - case TEST_PANIER_PORTE_OUVERTE: - if(temporisation_terminee(&tempo_ms, _step_ms)){ - i2c_annexe_ouvre_porte(); - } - if(lire_couleur()== COULEUR_BLEU){ - etat_test_panier=TEST_PANIER_INIT; - } - break; - - - } - - } - lettre = getchar_timeout_us(0); - //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0)); - }while(1); - printf("STRATEGIE_LOOP_2\n"); - printf("Lettre : %d; %c\n", lettre, lettre); - - if(lettre == 'q' && lettre == 'Q'){ - return 0; - } - return 0; - -} - - -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(avance_puis_longe_bordure(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; - - 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"); - } - } - - } - 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_tirette_et_couleur(){ int lettre; @@ -683,8 +212,8 @@ int test_tirette_et_couleur(){ i2c_annexe_gestion(); printf(">tirette:%d\n", attente_tirette()); - if(lire_couleur() == COULEUR_VERT){ - printf(">couleur:Vert|t\n"); + if(lire_couleur() == COULEUR_JAUNE){ + printf(">couleur:Jaune|t\n"); }else{ printf(">couleur:Bleu|t\n"); } @@ -693,9 +222,9 @@ int test_tirette_et_couleur(){ if(couleur_old != lire_couleur() || tirette != attente_tirette()){ tirette = attente_tirette(); couleur_old = lire_couleur(); - if(couleur_old == COULEUR_VERT){ - // Tout vert - i2c_annexe_couleur_balise(0b00011100, 0x0FFF); + if(couleur_old == COULEUR_JAUNE){ + // Tout Jaune + i2c_annexe_couleur_balise(0b11111100, 0x0FFF); }else{ // Tout bleu i2c_annexe_couleur_balise(0b00000011, 0x0FFF); diff --git a/Test_strategie_2024.c b/Test_strategie_2024.c index b9d2459..81a88c1 100644 --- a/Test_strategie_2024.c +++ b/Test_strategie_2024.c @@ -1,4 +1,5 @@ #include +#include "pico/stdio.h" #include "pico/error.h" #include "Geometrie.h" #include "Strategie_2024_pots.h" @@ -17,6 +18,7 @@ int test_strategie_2024(){ case 'A': while(test_calcul_position_pot()); break; + } } diff --git a/Tests_unitaires.c b/Tests_unitaires.c index 97af4fd..596e10b 100644 --- a/Tests_unitaires.c +++ b/Tests_unitaires.c @@ -268,8 +268,6 @@ int test_localisation(){ uint32_t _step_ms_gyro = 2, _step_ms=1, tempo_moteur=0; uint32_t m_gyro = 0; int16_t vitesse; - int propulseur=0; - Localisation_init(); @@ -329,12 +327,6 @@ int test_localisation(){ Moteur_SetVitesse(MOTEUR_A ,vitesse); Moteur_SetVitesse(MOTEUR_B ,vitesse); Moteur_SetVitesse(MOTEUR_C ,vitesse); - if(propulseur){ - i2c_annexe_active_propulseur(); - }else{ - i2c_annexe_desactive_propulseur(); - } - propulseur = !propulseur; } } diff --git a/i2c_annexe.c b/i2c_annexe.c index 0920950..dcd1bbd 100644 --- a/i2c_annexe.c +++ b/i2c_annexe.c @@ -6,15 +6,18 @@ #define ADRESSE_DEBUT_W 0x05 #define ADRESSE_DEBUT_R 0x0A #define ADRESSE_SCORE 0x09 -#define ADRESSE_TURBINE_PORTE 0x0A +#define ADRESSE_VL53L8_MODE 0x0A #define ADRESSE_CONTACTEURS 0x0B #define ADRESSE_VL53L1X 0x0C #define ADRESSE_TENSION_BATTERIE 0x18 #define ADRESSE_COULEUR 0x05 #define ADRESSE_MASQUE_LED_1 0x06 #define ADRESSE_MASQUE_LED_2 0x07 +#define ADRESSE_VL53L8_VALIDITE 0x19 +#define ADRESSE_VL53L8_ANGLE 0x1A +#define ADRESSE_VL53L8_DISTANCE 0x1E #define TAILLE_DONNEES_EMISSION 6 -#define TAILLE_DONNEES_RECEPTION 15 +#define TAILLE_DONNEES_RECEPTION 24 #define ADRESSE_PIC18F4550 0x31 #define ADRESSE_PIC18F4550_DEBUT_W 0x00 @@ -26,6 +29,11 @@ uint8_t donnees_emission_pic18f4550[TAILLE_DONNEES_PIC18F4550_EMISSION]; uint8_t donnees_reception_pic18f4550[TAILLE_DONNEES_PIC18F4550_EMISSION]; uint8_t donnees_reception[TAILLE_DONNEES_RECEPTION]; +union float_or_octet_t +{ + float f_value; + unsigned char octet[4]; +}; uint donnees_a_envoyer=0; uint donnees_a_envoyer_pic=0; @@ -97,55 +105,8 @@ void i2c_annexe_couleur_balise(uint8_t couleur, uint16_t masque_led){ donnees_emission[ADRESSE_MASQUE_LED_2 - ADRESSE_DEBUT_W] = masque_led & 0xFF; donnees_a_envoyer=1; } - -void i2c_annexe_active_turbine(void){ - donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x01; - donnees_a_envoyer=1; -} -void i2c_annexe_desactive_turbine(void){ - donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xFE; - donnees_a_envoyer=1; -} - -void i2c_annexe_ouvre_porte(void){ - donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xF9; - donnees_a_envoyer=1; -} -void i2c_annexe_ferme_porte(void){ - donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xF9; - donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= (1 << 1); - donnees_a_envoyer=1; -} -void i2c_annexe_mi_ferme_porte(void){ - donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xF9; - donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= (2 << 1); - donnees_a_envoyer=1; -} - -void i2c_annexe_active_propulseur(void){ - donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x08; - donnees_a_envoyer=1; -} -void i2c_annexe_desactive_propulseur(void){ - donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xF7; - donnees_a_envoyer=1; -} - -void i2c_annexe_active_deguisement(void){ - donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x10; - donnees_a_envoyer=1; -} -void i2c_annexe_desactive_deguisement(void){ - donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xEF; - donnees_a_envoyer=1; -} - -void i2c_annexe_deplie_bras(void){ - donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x20; - donnees_a_envoyer=1; -} -void i2c_annexe_plie_bras(void){ - donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xDF; +void i2c_annexe_set_mode_VL53L8(enum validite_vl53l8_t mode){ + donnees_emission[ADRESSE_VL53L8_MODE - ADRESSE_DEBUT_W] = mode; donnees_a_envoyer=1; } @@ -173,6 +134,24 @@ uint8_t i2c_annexe_get_tension_batterie(void){ return donnees_reception[ADRESSE_TENSION_BATTERIE - ADRESSE_DEBUT_R]; } +void i2c_annexe_get_VL53L8(enum validite_vl53l8_t *validite, float * angle, float * distance){ + union float_or_octet_t donnee; + *validite = donnees_reception[ADRESSE_VL53L8_VALIDITE]; + + donnee.octet[0]= donnees_reception[ADRESSE_VL53L8_ANGLE]; + donnee.octet[1]= donnees_reception[ADRESSE_VL53L8_ANGLE+1]; + donnee.octet[2]= donnees_reception[ADRESSE_VL53L8_ANGLE+2]; + donnee.octet[3]= donnees_reception[ADRESSE_VL53L8_ANGLE+3]; + *angle = donnee.f_value; + + donnee.octet[0]= donnees_reception[ADRESSE_VL53L8_DISTANCE]; + donnee.octet[1]= donnees_reception[ADRESSE_VL53L8_DISTANCE+1]; + donnee.octet[2]= donnees_reception[ADRESSE_VL53L8_DISTANCE+2]; + donnee.octet[3]= donnees_reception[ADRESSE_VL53L8_DISTANCE+3]; + *distance = donnee.f_value; +} + + void i2c_annexe_get_distances(uint8_t *distance_capteur_cm){ for (uint8_t capteur = 0; capteur < 12; capteur++){ distance_capteur_cm[capteur] = donnees_reception[capteur + ADRESSE_VL53L1X - ADRESSE_DEBUT_R]; diff --git a/i2c_annexe.h b/i2c_annexe.h index 8288f46..a0373af 100644 --- a/i2c_annexe.h +++ b/i2c_annexe.h @@ -18,16 +18,13 @@ #define PLANTE_BRAS_1 '1' #define PLANTE_BRAS_6 '6' +enum validite_vl53l8_t{ + VL53L8_INVALIDE=0, + VL53L8_BORDURE=1, + VL53L8_PLANTE=2 +}; + void i2c_annexe_gestion(void); -void i2c_annexe_active_turbine(void); -void i2c_annexe_desactive_turbine(void); - -void i2c_annexe_ouvre_porte(void); -void i2c_annexe_ferme_porte(void); -void i2c_annexe_mi_ferme_porte(void); - -void i2c_annexe_active_propulseur(void); -void i2c_annexe_desactive_propulseur(void); void i2c_annexe_envoie_score(uint8_t score); @@ -40,13 +37,9 @@ uint8_t i2c_annexe_get_contacteur_longer_C(void); void i2c_annexe_get_distances(uint8_t *distance_capteur_cm); void i2c_annexe_couleur_balise(uint8_t couleur, uint16_t masque_led); -void i2c_annexe_active_deguisement(void); -void i2c_annexe_desactive_deguisement(void); - -void i2c_annexe_plie_bras(void); -void i2c_annexe_deplie_bras(void); - +void i2c_annexe_set_mode_VL53L8(enum validite_vl53l8_t mode); uint8_t i2c_annexe_get_tension_batterie(void); +void i2c_annexe_get_VL53L8(enum validite_vl53l8_t *validite, float * angle, float * distance); void i2c_annexe_actionneur_pot(int actionneur, uint8_t pos_bras, uint8_t pos_doigt); void i2c_annexe_attrape_plante(uint8_t action);