Keuronde/RPiPico-Holonome2023
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Arrêt propre devant les cerises

Browse Source
This commit is contained in:
Samuel 2023-04-01 17:55:24 +02:00
parent 0d1761e94e
commit c0a18b343c
1 changed files with 52 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
Strategie.c
View File

@ -2,6 +2,7 @@
#include "i2c_annexe.h"
#include "Asser_Position.h"
#include "Balise_VL53L1X.h"
#include "Commande_vitesse.h"
#include "Geometrie_robot.h"
#include "Localisation.h"
#include "Moteurs.h"
@ -42,8 +43,8 @@ void Homologation(uint32_t step_ms){
case APPROCHE_CERISE_1_A:
Trajet_config(250, 500);
Trajectoire_droite(&trajectoire,775, 109, 857, 156, -60. * DEGREE_EN_RADIAN, +30. * DEGREE_EN_RADIAN);
if(parcourt_trajet_simple(trajectoire, step_ms) == TRAJET_TERMINE){
Trajectoire_droite(&trajectoire,775, 109, 857, 156, -60. * DEGREE_EN_RADIAN, 30. * DEGREE_EN_RADIAN);
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){
etat_strategie = ATTRAPE_CERISE_1;
}
break;
@ -91,7 +92,7 @@ void Homologation(uint32_t step_ms){
180, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80,
120. * DEGREE_EN_RADIAN, +270. * DEGREE_EN_RADIAN);
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = LANCE_DANS_PANIER;
}
break;
@ -99,14 +100,32 @@ void Homologation(uint32_t step_ms){
case LANCE_DANS_PANIER:
Asser_Position_maintien();
if(lance_balles(step_ms) == ACTION_TERMINEE){
etat_strategie = APPROCHE_CERISE_2;
}
break;
case APPROCHE_CERISE_2:
Trajet_config(250, 500);
Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
830, 3000 - 156,
Localisation_get().angle_radian, 30. * DEGREE_EN_RADIAN);
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = STRATEGIE_FIN;
}
break;
case ATTRAPPE_CERISE_2:
etat_action = cerise_attraper_bordure(LONGER_VERS_A, step_ms);
if(etat_action == ACTION_TERMINEE){
etat_strategie = APPROCHE_PANIER_2;
}
break;
case STRATEGIE_FIN:
Moteur_Stop();
i2c_annexe_desactive_propulseur();
commande_vitesse_stop();
break;
}
}
@ -117,7 +136,7 @@ void Homologation(uint32_t step_ms){
/// @return ACTION_EN_COURS ou ACTION_TERMINEE
enum etat_action_t lance_balles(uint32_t step_ms){
enum etat_action_t etat_action = ACTION_EN_COURS;
uint32_t tempo_ms;
static uint32_t tempo_ms;
static enum{
LANCE_PROPULSEUR_ON,
@ -128,14 +147,13 @@ enum etat_action_t lance_balles(uint32_t step_ms){
switch(etat_lance_balle){
case LANCE_PROPULSEUR_ON:
i2c_annexe_active_propulseur();
tempo_ms = 2000;
tempo_ms = 1000;
etat_lance_balle = LANCE_TEMPO_PROP_ON;
break;
case LANCE_TEMPO_PROP_ON:
if (tempo_ms < step_ms){
//etat_lance_balle = LANCE_PORTE_OUVERTE;
etat_lance_balle = LANCE_TEMPO_PROP_ON;
etat_lance_balle = LANCE_PORTE_OUVERTE;
i2c_annexe_ouvre_porte();
tempo_ms = 6000;
}else{
@ -152,8 +170,6 @@ enum etat_action_t lance_balles(uint32_t step_ms){
tempo_ms -= step_ms;
}
break;
}
return etat_action;
}
@ -212,6 +228,32 @@ enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint
return etat_action;
}
enum etat_action_t parcourt_trajet_simple_sans_evitement(struct trajectoire_t trajectoire, uint32_t step_ms){
enum etat_action_t etat_action = ACTION_EN_COURS;
enum etat_trajet_t etat_trajet;
static enum {
PARCOURS_INIT,
PARCOURS_AVANCE,
} etat_parcourt=PARCOURS_INIT;
switch (etat_parcourt){
case PARCOURS_INIT:
Trajet_debut_trajectoire(trajectoire);
etat_parcourt = PARCOURS_AVANCE;
break;
case PARCOURS_AVANCE:
etat_trajet = Trajet_avance(step_ms/1000.);
if(etat_trajet == TRAJET_TERMINE){
etat_action = ACTION_TERMINEE;
etat_parcourt = PARCOURS_INIT;
}
break;
}
return etat_action;
}
/// @brief Renvoi 1 si on doit attendre le déclenchement de la tirette
uint attente_tirette(void){
return !gpio_get(TIRETTE);