Code d'homologation

This commit is contained in:
Samuel 2024-05-08 16:42:19 +02:00
parent 09e0c50a9f
commit cf1e4719d3
5 changed files with 70 additions and 10 deletions

View File

@ -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;

View File

@ -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){

View File

@ -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;
}

View File

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

View File

@ -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);