Code d'homologation
This commit is contained in:
parent
09e0c50a9f
commit
cf1e4719d3
@ -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;
|
||||
|
@ -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){
|
||||
|
@ -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;
|
||||
}
|
@ -21,7 +21,7 @@ float angle_bras[6] =
|
||||
|
||||
float distance_bras_correction_mm[6] =
|
||||
{
|
||||
-5,
|
||||
-12,
|
||||
0,
|
||||
-5,
|
||||
-15,
|
||||
|
@ -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);
|
||||
|
Loading…
Reference in New Issue
Block a user