diff --git a/Holonome2023.c b/Holonome2023.c index e5bbd37..f9fdfdf 100644 --- a/Holonome2023.c +++ b/Holonome2023.c @@ -110,7 +110,7 @@ int main() { break; case MATCH_EN_COURS: - if (timer_match_ms > 98000){ // 98 secondes + if (timer_match_ms > 90000){ // 98 secondes printf("MATCH_ARRET_EN_COURS\n"); statu_match = MATCH_ARRET_EN_COURS; } @@ -119,9 +119,6 @@ int main() { case MATCH_ARRET_EN_COURS: commande_vitesse_stop(); - 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 56feebb..a76a239 100644 --- a/Strategie.c +++ b/Strategie.c @@ -473,17 +473,19 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_ enum validite_vl53l8_t validite; struct trajectoire_t trajectoire; enum etat_action_t etat_action; + int tempo_ms; float angle, distance; switch(etat_calage_debut){ case CD_ENVOI_CDE_BORDURE: i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE); + tempo_ms = 2000; etat_calage_debut = CD_LECTURE_BORDURE_Y; break; case CD_LECTURE_BORDURE_Y: - + tempo_ms--; i2c_annexe_get_VL53L8(&validite, &angle, &distance); if(validite == VL53L8_BORDURE){ i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE); @@ -492,6 +494,10 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_ Localisation_set_angle((-90. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle); etat_calage_debut = CD_ROTATION_VERS_X; } + if(tempo_ms <= 0){ + etat_calage_debut=CD_ENVOI_CDE_BORDURE; + return ACTION_ECHEC; + } break; case CD_ROTATION_VERS_X: @@ -506,10 +512,12 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_ if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE); etat_calage_debut = CD_LECTURE_BORDURE_X; + tempo_ms = 2000; } break; case CD_LECTURE_BORDURE_X: + tempo_ms--; i2c_annexe_get_VL53L8(&validite, &angle, &distance); if(validite == VL53L8_BORDURE){ i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE); @@ -524,6 +532,10 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_ etat_calage_debut = CD_ALLER_POSITION_INIT; } + if(tempo_ms <= 0){ + etat_calage_debut=CD_ENVOI_CDE_BORDURE; + return ACTION_ECHEC; + } break; case CD_ALLER_POSITION_INIT: @@ -597,4 +609,5 @@ enum etat_action_t Strategie_calage_debut_manuel(enum couleur_t couleur, uint32_ } return ACTION_EN_COURS; -} \ No newline at end of file +} + diff --git a/Strategie_2024.c b/Strategie_2024.c index f12bb37..76a7968 100644 --- a/Strategie_2024.c +++ b/Strategie_2024.c @@ -62,7 +62,7 @@ enum zone_plante_t get_zone_plante(enum couleur_t couleur){ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ int lettre, _step_ms = 1, _step_ms_gyro=2,temps_ms_init; - float angle_destination; + float angle_destination, pos_x; struct trajectoire_t trajectoire; enum evitement_t evitement; enum etat_action_t etat_action=ACTION_EN_COURS; @@ -80,11 +80,14 @@ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms) TAP_PLANTE_ATTRAPE_3, TAP_PLANTE_ATTRAPE_4, TAP_RENTRE, + TAP_RENTRE_RECALE, + TAP_DEPOSE_0, TAP_DEPOSE_1, TAP_DEPOSE_2, TAP_DEPOSE_3, + TAP_RECHARGE, TAP_FINI - } etat_test = TAP_CALAGE; + } etat_test = TAP_CALAGE, next_etat_test; switch(etat_test){ @@ -106,7 +109,12 @@ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms) break; case TAP_ALLER_PLANTE: - if(Strategie_aller_a(940, 300, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms) == ACTION_TERMINEE){ + if(couleur == COULEUR_BLEU){ + pos_x = 940; + }else{ + pos_x = 3000 - 940; + } + if(Strategie_aller_a(pos_x, 300, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms) == ACTION_TERMINEE){ etat_test=TAP_PLANTE_ORIENTATION; } break; @@ -169,13 +177,37 @@ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms) case TAP_RENTRE: if(couleur == COULEUR_BLEU){ + angle_destination = 60 * DEGRE_EN_RADIAN; + pos_x = 350; + }else{ + angle_destination = 60 * DEGRE_EN_RADIAN; + pos_x = 3000-350; + } + + Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); + if(Strategie_tourner_et_aller_a(pos_x, 300, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){ + etat_test=TAP_RENTRE_RECALE; + } + break; + + case TAP_RENTRE_RECALE: + etat_action = Strategie_calage_debut(couleur, step_ms); + if(etat_action == ACTION_TERMINEE || etat_action == ACTION_ECHEC){ + etat_test=TAP_DEPOSE_0; + } + break; + + case TAP_DEPOSE_0: + if(couleur == COULEUR_BLEU){ + pos_x = 300; angle_destination = 15 * DEGRE_EN_RADIAN; }else{ + pos_x = 3000-300; angle_destination = (90-15) * DEGRE_EN_RADIAN; } Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); - if(Strategie_tourner_et_aller_a(300, 300, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){ + if(Strategie_aller_a_puis_tourner(pos_x, 300, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){ etat_test=TAP_DEPOSE_1; } break; @@ -190,25 +222,76 @@ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms) case TAP_DEPOSE_2: if(couleur == COULEUR_BLEU){ angle_destination = (180+15) * DEGRE_EN_RADIAN; + pos_x = 450; }else{ angle_destination = (90-(180+15)) * DEGRE_EN_RADIAN; + pos_x = 3000-450; } Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); - if(Strategie_aller_a_puis_tourner(450, 450, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){ + if(Strategie_aller_a_puis_tourner(pos_x, 450, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){ + etat_test=TAP_DEPOSE_3; } break; case TAP_DEPOSE_3: - commande_vitesse_stop(); if(Strat_2024_depose_pot(MASQUE_POT_3 | MASQUE_POT_4, _step_ms)== ACTION_TERMINEE){ + etat_test=TAP_RECHARGE; + } + break; + + case TAP_RECHARGE: + if(rentre_recharge(couleur, step_ms) == ACTION_TERMINEE){ etat_test=TAP_FINI; } break; case TAP_FINI: + commande_vitesse_stop(); break; } +} + + +enum etat_action_t rentre_recharge(enum couleur_t couleur, int step_ms){ + struct trajectoire_t trajectoire; + enum etat_action_t etat_action; + + static enum { + ZONE_HAUTE, + ZONE_OPPOSEE, + } etat_rentre_charge = ZONE_OPPOSEE; + + Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); + + switch (etat_rentre_charge){ + case ZONE_OPPOSEE: + if(couleur == COULEUR_BLEU){ + etat_action = Strategie_aller_a(2700, 1000, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms); + }else{ + etat_action = Strategie_aller_a(300, 1000, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms); + } + if(etat_action == ACTION_ECHEC){ + etat_rentre_charge = ZONE_HAUTE; + }else if(etat_action == ACTION_TERMINEE){ + return ACTION_TERMINEE; + } + break; + + case ZONE_HAUTE: + if(couleur == COULEUR_BLEU){ + etat_action = Strategie_aller_a(300, 1700, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms); + }else{ + etat_action = Strategie_aller_a(2700, 1700, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms); + } + if(etat_action == ACTION_ECHEC){ + etat_rentre_charge = ZONE_OPPOSEE; + }else if(etat_action == ACTION_TERMINEE){ + return ACTION_TERMINEE; + } + break; + } + return ACTION_EN_COURS; } \ No newline at end of file diff --git a/Strategie_2024.h b/Strategie_2024.h index b9df56f..c8b6f20 100644 --- a/Strategie_2024.h +++ b/Strategie_2024.h @@ -13,6 +13,7 @@ #define STRATEGIE_2024_H void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms); +enum etat_action_t rentre_recharge(enum couleur_t couleur, int step_ms); // STRATEGIE_H #endif diff --git a/Strategie_2024_panneaux.c b/Strategie_2024_panneaux.c index 569ceb0..662a43b 100644 --- a/Strategie_2024_panneaux.c +++ b/Strategie_2024_panneaux.c @@ -7,6 +7,8 @@ #include "Localisation.h" #include "Score.h" +enum etat_action_t active_panneau_solaire(float pos_x, uint32_t step_ms); + /// @brief Fonction demande au robot d'aller activer les panneaux solaires enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t step_ms){ enum etat_action_t etat_action; @@ -36,7 +38,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( - 3000 - 323, 300, (150. *DEGRE_EN_RADIAN), + 3000 - 173, 300, (150. *DEGRE_EN_RADIAN), EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms); } if (etat_action == ACTION_TERMINEE){ @@ -49,10 +51,10 @@ enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t case TP_TOURNE_PANNEAU_1: if(couleur == COULEUR_BLEU){ etat_action = Strategie_tourner_et_aller_a( - 323, 200, (150. *DEGRE_EN_RADIAN), EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms); + 323, 180, (150. *DEGRE_EN_RADIAN), EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms); }else{ etat_action = Strategie_tourner_et_aller_a( - 3000 - 323, 300, (150. *DEGRE_EN_RADIAN), EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms); + 3000 - 173, 180, (150. *DEGRE_EN_RADIAN), EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms); } if (etat_action == ACTION_TERMINEE){ Score_ajout_panneau(1); @@ -62,14 +64,13 @@ enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t case TP_TOURNE_PANNEAU_2: if(couleur == COULEUR_BLEU){ - Trajectoire_bezier(&trajectoire, 323, 200, 405, 310, 548, 400, 548, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN)); + etat_action = active_panneau_solaire(558, step_ms); }else{ - Trajectoire_bezier(&trajectoire, 3000-323, 200, 3000-405, 310, 3000-548, 400, 3000-548, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN)); + etat_action = active_panneau_solaire(3000 - 408, step_ms); } - 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_DEGAGEMENT; + etat_tourne_panneaux = TP_TOURNE_PANNEAU_3; }else if(etat_action == ACTION_ECHEC){ etat_tourne_panneaux = TP_DEGAGEMENT; } @@ -77,14 +78,13 @@ enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t case TP_TOURNE_PANNEAU_3: if(couleur == COULEUR_BLEU){ - Trajectoire_bezier(&trajectoire, 548, 200, 630, 310, 773, 400, 773, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN)); + etat_action = active_panneau_solaire(803, step_ms); }else{ - Trajectoire_bezier(&trajectoire, 3000-548, 200, 3000-630, 310, 3000-773, 400, 3000-773, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN)); + etat_action = active_panneau_solaire(3000 - 653, step_ms); } - 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_4; + etat_tourne_panneaux = TP_DEGAGEMENT; }else if(etat_action == ACTION_ECHEC){ etat_tourne_panneaux = TP_DEGAGEMENT; } @@ -149,4 +149,17 @@ enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t break; } return ACTION_EN_COURS; +} + + +enum etat_action_t active_panneau_solaire(float pos_x, uint32_t step_ms){ + struct trajectoire_t trajectoire; + enum etat_action_t etat_action; + Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, + Localisation_get().x_mm, 400, pos_x, 400, pos_x, 180, + Geometrie_get_angle_optimal(Localisation_get().angle_radian, 150. *DEGRE_EN_RADIAN), + Geometrie_get_angle_optimal(Localisation_get().angle_radian, 150. *DEGRE_EN_RADIAN)); + + etat_action = Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE); + return etat_action; } \ No newline at end of file diff --git a/Strategie_2024_pots.c b/Strategie_2024_pots.c index 7fd0ea9..84966ae 100644 --- a/Strategie_2024_pots.c +++ b/Strategie_2024_pots.c @@ -22,11 +22,11 @@ float angle_bras[6] = float distance_bras_correction_mm[6] = { -12, - 0, + -7, -5, -15, 0, - 0 + -10 }; enum etat_bras_t{ @@ -48,7 +48,7 @@ struct position_t position_groupe_pot[6] = {.x_mm = 36.1, .y_mm = 1386.8, .angle_radian = -90 * DEGRE_EN_RADIAN}, {.x_mm = 36.1, .y_mm = 616.2, .angle_radian = -90 * DEGRE_EN_RADIAN}, {.x_mm = 1020, .y_mm = 36.4, .angle_radian = 0 * DEGRE_EN_RADIAN}, - {.x_mm = 2000, .y_mm = 36.4, .angle_radian = 0 * DEGRE_EN_RADIAN}, + {.x_mm = 1980, .y_mm = 41.4, .angle_radian = 0 * DEGRE_EN_RADIAN}, // Attention bidouille !!! {.x_mm = 2963.9, .y_mm = 616.2, .angle_radian = 90 * DEGRE_EN_RADIAN}, {.x_mm = 2963.9, .y_mm = 1386.8, .angle_radian = 90 * DEGRE_EN_RADIAN} }; @@ -146,7 +146,6 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms); if (etat_action == ACTION_TERMINEE){ etat_attrape_pot = AP_ORIENTE; - i2c_annexe_set_mode_VL53L8(VL53L8_DISTANCE_LOIN); } break;