diff --git a/Strategie.c b/Strategie.c index 2f7fe15..56feebb 100644 --- a/Strategie.c +++ b/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_angle((0. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle); + Localisation_set_x(3025 - (distance + DISTANCE_CENTRE_CAPTEUR)); + Localisation_set_angle((-3. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle); } etat_calage_debut = CD_ALLER_POSITION_INIT; diff --git a/Strategie_2024.c b/Strategie_2024.c index 32569ec..f12bb37 100644 --- a/Strategie_2024.c +++ b/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){ diff --git a/Strategie_2024_panneaux.c b/Strategie_2024_panneaux.c index 671c188..569ceb0 100644 --- a/Strategie_2024_panneaux.c +++ b/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; - return ACTION_TERMINEE; + etat_tourne_panneaux = TP_TOURNE_PANNEAU_4; }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; } \ No newline at end of file diff --git a/Strategie_2024_pots.c b/Strategie_2024_pots.c index 910ca70..7fd0ea9 100644 --- a/Strategie_2024_pots.c +++ b/Strategie_2024_pots.c @@ -21,7 +21,7 @@ float angle_bras[6] = float distance_bras_correction_mm[6] = { - -5, + -12, 0, -5, -15, diff --git a/gyro_ADXRS453.c b/gyro_ADXRS453.c index 4507ee8..53a9b01 100644 --- a/gyro_ADXRS453.c +++ b/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);