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

Chargement des cerises en début de match

Browse Source
This commit is contained in:
Samuel 2023-05-17 16:54:59 +02:00
parent a954a01912
commit 332cb10e3e
6 changed files with 268 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
Holonome2023.c
View File

@ -108,6 +108,7 @@ int main() {
uint16_t _step_ms_gyro = 2; uint16_t _step_ms_gyro = 2;
const uint16_t _step_ms = 1; const uint16_t _step_ms = 1;
static uint32_t timer_match_ms=0; static uint32_t timer_match_ms=0;
static uint32_t timer_match_init=0;
static enum couleur_t couleur; static enum couleur_t couleur;
// Surveillance du temps d'execution // Surveillance du temps d'execution
@ -119,7 +120,7 @@ int main() {
if(temps_ms != Temps_get_temps_ms()){ if(temps_ms != Temps_get_temps_ms()){
timer_match_ms = timer_match_ms + _step_ms; timer_match_ms = (time_us_32() - timer_match_init) / 1000;
temps_ms = Temps_get_temps_ms(); temps_ms = Temps_get_temps_ms();
QEI_update(); QEI_update();
Localisation_gestion(); Localisation_gestion();
@ -135,6 +136,7 @@ int main() {
switch(statu_match){ switch(statu_match){
case MATCH_ATTENTE_TIRETTE: case MATCH_ATTENTE_TIRETTE:
Strategie_preparation();
if(lire_couleur() == COULEUR_VERT){ if(lire_couleur() == COULEUR_VERT){
// Tout vert // Tout vert
i2c_annexe_couleur_balise(0b00011100, 0x0FFF); i2c_annexe_couleur_balise(0b00011100, 0x0FFF);
@ -144,6 +146,7 @@ int main() {
} }
if(attente_tirette() == 0){ if(attente_tirette() == 0){
statu_match = MATCH_EN_COURS; statu_match = MATCH_EN_COURS;
timer_match_init = time_us_32();
couleur = lire_couleur(); couleur = lire_couleur();
timer_match_ms = 0; timer_match_ms = 0;
i2c_annexe_couleur_balise(0, 0x00); i2c_annexe_couleur_balise(0, 0x00);
@ -164,7 +167,7 @@ int main() {
commande_vitesse_stop(); commande_vitesse_stop();
i2c_annexe_active_deguisement(); i2c_annexe_active_deguisement();
Score_set_funny_action(); Score_set_funny_action();
if(Robot_est_dans_zone_depose()){ if(Robot_est_dans_zone_depose(couleur)){
Score_set_pieds_dans_plat(); Score_set_pieds_dans_plat();
} }

138
Strategie.c
View File

@ -49,6 +49,8 @@ int Robot_est_dans_quart_haut_gauche();
int Robot_est_dans_quart_haut_droit(); int Robot_est_dans_quart_haut_droit();
int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur); int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur);
int Robot_est_dans_zone_cerise_gauche(); int Robot_est_dans_zone_cerise_gauche();
enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t couleur, uint32_t step_ms);
enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t couleur, uint32_t step_ms);
enum etat_homologation_t etat_homologation=STRATEGIE_INIT; enum etat_homologation_t etat_homologation=STRATEGIE_INIT;
@ -156,7 +158,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 175, Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 175,
Localisation_get().angle_radian, angle_fin); Localisation_get().angle_radian, angle_fin);
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){ if(parcourt_trajet_simple(trajectoire, step_ms) == TRAJET_TERMINE){
etat_strategie = ATTRAPER_CERISE_HAUT; etat_strategie = ATTRAPER_CERISE_HAUT;
} }
break; break;
@ -182,18 +184,13 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
Trajet_config(300, 250); Trajet_config(300, 250);
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN); angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
if(couleur == COULEUR_BLEU){ if(couleur == COULEUR_BLEU){
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, if(Strategie_aller_cerises_laterales_proches(couleur, step_ms)== ACTION_TERMINEE){
740, 3000 - 550, etat_strategie = ATTRAPER_CERISE_GAUCHE;
510, 3000 - 1580, }
180, 1500,
Localisation_get().angle_radian, angle_fin);
}else{ }else{
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, if(Strategie_aller_cerises_laterales_opposees(couleur, step_ms)== ACTION_TERMINEE){
1225, 3000 - 540, etat_strategie = ATTRAPER_CERISE_GAUCHE;
440, 3000 - 775, }
225, 3000 - (1500 - 45),
Localisation_get().angle_radian, angle_fin);
} }
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
@ -209,6 +206,32 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
} }
break; break;
case ALLER_CERISE_DROITE:
Trajet_config(300, 250);
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
if(couleur == COULEUR_BLEU){
if(Strategie_aller_cerises_laterales_proches(couleur, step_ms)== ACTION_TERMINEE){
etat_strategie = ATTRAPER_CERISE_DROITE;
}
}else{
if(Strategie_aller_cerises_laterales_opposees(couleur, step_ms)== ACTION_TERMINEE){
etat_strategie = ATTRAPER_CERISE_DROITE;
}
}
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = ATTRAPER_CERISE_GAUCHE;
Trajet_config(500, 500);
}
break;
case ATTRAPER_CERISE_DROITE:
if(cerises_attraper_cerises_droite(step_ms) == ACTION_TERMINEE){
Strategie_set_cerise_dans_robot(10);
etat_strategie = ALLER_PANIER;
}
break;
case ALLER_PANIER: case ALLER_PANIER:
if(Strategie_aller_panier(couleur, step_ms) == ACTION_TERMINEE){ if(Strategie_aller_panier(couleur, step_ms) == ACTION_TERMINEE){
etat_strategie = LANCER_PANIER; etat_strategie = LANCER_PANIER;
@ -263,6 +286,51 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
} }
} }
enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t couleur, uint32_t step_ms){
struct trajectoire_t trajectoire;
float angle_fin;
if(couleur == COULEUR_BLEU){
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
740, 3000 - 550,
510, 3000 - 1580,
180, 1500,
Localisation_get().angle_radian, angle_fin);
}else{
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN);
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
2000 - 740, 3000 - 550,
2000 - 510, 3000 - 1580,
2000 - 180, 1500,
Localisation_get().angle_radian, angle_fin);
}
return parcourt_trajet_simple(trajectoire, step_ms);
}
enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t couleur, uint32_t step_ms){
struct trajectoire_t trajectoire;
float angle_fin;
if(couleur == COULEUR_BLEU){
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN);
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
785, 3000 - 550,
1550, 3000 - 785,
1775, 1500,
Localisation_get().angle_radian, angle_fin);
}else{
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
2000 - 785, 3000 - 550,
2000 - 1550, 3000 - 785,
2000 - 1775, 1500,
Localisation_get().angle_radian, angle_fin);
}
return parcourt_trajet_simple(trajectoire, step_ms);
}
enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step_ms){ enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step_ms){
enum etat_action_t etat_action = ACTION_EN_COURS; enum etat_action_t etat_action = ACTION_EN_COURS;
struct trajectoire_t trajectoire; struct trajectoire_t trajectoire;
@ -451,7 +519,7 @@ enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t ste
longer_direction = LONGER_VERS_C; longer_direction = LONGER_VERS_C;
} }
if(calage_angle(longer_direction, recal_pos_x, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2.)), 120. *DEGRE_EN_RADIAN) == ACTION_TERMINEE){ if(calage_angle(longer_direction, recal_pos_x, 3000 - (PETIT_RAYON_ROBOT_MM), 120. *DEGRE_EN_RADIAN) == ACTION_TERMINEE){
etat_lance_balles_dans_panier = RECULE_PANIER; etat_lance_balles_dans_panier = RECULE_PANIER;
} }
break; break;
@ -640,3 +708,47 @@ int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms){
return 0; return 0;
} }
} }
/// @brief Gère le chargement des balles au début du match
void Strategie_preparation(){
static enum{
PREP_INIT_V,
PREP_INIT_B,
PREP_ASPIRE,
PREP_ARRET_ASPIRATION,
PREP_TEMPO,
} etat_prep=PREP_INIT_V;
switch(etat_prep){
case PREP_INIT_V:
if(lire_couleur()== COULEUR_VERT){
etat_prep = PREP_INIT_B;
}
break;
case PREP_INIT_B:
if(lire_couleur()== COULEUR_BLEU){
etat_prep = PREP_ASPIRE;
}
break;
case PREP_ASPIRE:
if(lire_couleur()== COULEUR_VERT){
i2c_annexe_ferme_porte();
i2c_annexe_active_turbine();
etat_prep=PREP_ARRET_ASPIRATION;
}
break;
case PREP_ARRET_ASPIRATION:
if(lire_couleur()== COULEUR_BLEU){
i2c_annexe_desactive_turbine();
etat_prep=PREP_TEMPO;
}
break;
case PREP_TEMPO:
break;
}
}

2
Strategie.h
View File

@ -67,6 +67,8 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms);
int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms); int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms);
enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step_ms); enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step_ms);
enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement); enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement);
int Robot_est_dans_zone_depose(enum couleur_t couleur);
void Strategie_preparation(void);
extern float distance_obstacle; extern float distance_obstacle;

40
Strategie_prise_cerises.c
View File

@ -35,6 +35,46 @@ float vitesse_accostage_mm_s=100;
enum etat_action_t cerises_attraper_cerises_droite(uint32_t step_ms){
static enum {
ATTRAPE_CERISE_DEMI_BAS,
REVENIR_CENTRE,
ATTRAPE_CERISE_DEMI_HAUT,
}etat_attrappe_cerises_droite = ATTRAPE_CERISE_DEMI_BAS;
struct trajectoire_t trajectoire;
float angle_fin;
float pos_recal_x_mm = PETIT_RAYON_ROBOT_MM + 30;
switch (etat_attrappe_cerises_droite){
case ATTRAPE_CERISE_DEMI_BAS:
if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_C, pos_recal_x_mm) == ACTION_TERMINEE){
etat_attrappe_cerises_droite = REVENIR_CENTRE;
}
break;
case REVENIR_CENTRE:
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 30 * DEGRE_EN_RADIAN);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
2000 - 180, 1500 - 75, Localisation_get().angle_radian, angle_fin);
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_attrappe_cerises_droite = ATTRAPE_CERISE_DEMI_HAUT;
}
break;
case ATTRAPE_CERISE_DEMI_HAUT:
if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_A, pos_recal_x_mm) == ACTION_TERMINEE){
etat_attrappe_cerises_droite = ATTRAPE_CERISE_DEMI_BAS;
return ACTION_TERMINEE;
}
break;
}
return ACTION_EN_COURS;
}
enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms){ enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms){
static enum { static enum {
ATTRAPE_CERISE_DEMI_BAS, ATTRAPE_CERISE_DEMI_BAS,

1
Strategie_prise_cerises.h
View File

@ -1,6 +1,7 @@
#include "Strategie.h" #include "Strategie.h"
enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_x_mm, float pos_y_mm); enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_x_mm, float pos_y_mm);
enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms); enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms);
enum etat_action_t cerises_attraper_cerises_droite(uint32_t step_ms);
void commande_translation_longer_vers_A(); void commande_translation_longer_vers_A();
void commande_translation_longer_vers_C(); void commande_translation_longer_vers_C();
void commande_translation_avance_vers_trompe(); void commande_translation_avance_vers_trompe();

97
Test_strategie.c
View File

@ -26,7 +26,8 @@ int test_panier(void);
int test_homologation(void); int test_homologation(void);
int test_evitement(void); int test_evitement(void);
int test_tirette_et_couleur(); int test_tirette_et_couleur();
int test_cerise_laterales(void); int test_cerise_laterales_droite(void);
int test_cerise_laterales_gauche(void);
void affichage_test_evitement(); void affichage_test_evitement();
void affichage_test_strategie(){ void affichage_test_strategie(){
@ -98,7 +99,7 @@ int test_strategie(){
case 'd': case 'd':
case 'D': case 'D':
while(test_cerise_laterales()); while(test_cerise_laterales_droite());
break; break;
case 'e': case 'e':
@ -106,6 +107,11 @@ int test_strategie(){
while(test_evitement()); while(test_evitement());
break; break;
case 'g':
case 'G':
while(test_cerise_laterales_gauche());
break;
case 'h': case 'h':
case 'H': case 'H':
//while(test_homologation()); //while(test_homologation());
@ -130,7 +136,7 @@ int test_strategie(){
} }
int test_cerise_laterales(){ int test_cerise_laterales_gauche(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init; int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
uint32_t temps_cycle_old; uint32_t temps_cycle_old;
enum etat_action_t etat_action; enum etat_action_t etat_action;
@ -217,6 +223,91 @@ int test_cerise_laterales(){
} }
int test_cerise_laterales_droite(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
uint32_t temps_cycle_old;
enum etat_action_t etat_action;
printf("Attaper cerise latérales\n");
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
//printf("Init gyroscope\n");
set_position_avec_gyroscope(1);
if(get_position_avec_gyroscope()){
Gyro_Init();
}
stdio_flush();
multicore_launch_core1(affichage_test_strategie);
temps_ms = Temps_get_temps_ms();
temps_ms_init = temps_ms;
temps_cycle_old= time_us_32();
uint32_t tempo_ms=1000;
Localisation_set(1775, 1500, 30 * DEGRE_EN_RADIAN);
do{
etat_action = ACTION_EN_COURS;
temps_cycle_check();
i2c_gestion(i2c0);
i2c_annexe_gestion();
Balise_VL53L1X_gestion();
// Routines à 1 ms
if(temps_ms != Temps_get_temps_ms()){
static enum {
TEMPO_AVANT,
TEST,
TEMPO_APRES
}etat_test;
temps_ms = Temps_get_temps_ms();
QEI_update();
Localisation_gestion();
AsserMoteur_Gestion(_step_ms);
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
if(get_position_avec_gyroscope()){
Gyro_Read(_step_ms_gyro);
}
}
switch(etat_test){
case TEMPO_AVANT:
if(temporisation_terminee(&tempo_ms, _step_ms)){
etat_test = TEST;
}
break;
case TEST:
if(cerises_attraper_cerises_droite(_step_ms) == ACTION_TERMINEE){
tempo_ms = 1000;
etat_test = TEMPO_APRES;
}
break;
case TEMPO_APRES:
if(temporisation_terminee(&tempo_ms, _step_ms)){
etat_test = TEMPO_AVANT;
etat_action = ACTION_TERMINEE;
}
break;
}
}
//lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(etat_action == ACTION_EN_COURS);
Moteur_Stop();
return 0;
}
int test_homologation(){ int test_homologation(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init; int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
uint32_t temps_cycle[10], temps_cycle_old, index_temps_cycle=0; uint32_t temps_cycle[10], temps_cycle_old, index_temps_cycle=0;