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

9
Holonome2023.c
View File

@ -108,6 +108,7 @@ int main() {
uint16_t _step_ms_gyro = 2;
const uint16_t _step_ms = 1;
static uint32_t timer_match_ms=0;
static uint32_t timer_match_init=0;
static enum couleur_t couleur;
// Surveillance du temps d'execution
@ -119,7 +120,7 @@ int main() {
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();
QEI_update();
Localisation_gestion();
@ -135,6 +136,7 @@ int main() {
switch(statu_match){
case MATCH_ATTENTE_TIRETTE:
Strategie_preparation();
if(lire_couleur() == COULEUR_VERT){
// Tout vert
i2c_annexe_couleur_balise(0b00011100, 0x0FFF);
@ -144,8 +146,9 @@ int main() {
}
if(attente_tirette() == 0){
statu_match = MATCH_EN_COURS;
timer_match_init = time_us_32();
couleur = lire_couleur();
timer_match_ms=0;
timer_match_ms = 0;
i2c_annexe_couleur_balise(0, 0x00);
score=5;
i2c_annexe_envoie_score(score);
@ -164,7 +167,7 @@ int main() {
commande_vitesse_stop();
i2c_annexe_active_deguisement();
Score_set_funny_action();
if(Robot_est_dans_zone_depose()){
if(Robot_est_dans_zone_depose(couleur)){
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_zone_cerise_haut(enum couleur_t couleur);
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;
@ -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,
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;
}
break;
@ -182,18 +184,13 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
Trajet_config(300, 250);
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
if(couleur == COULEUR_BLEU){
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);
if(Strategie_aller_cerises_laterales_proches(couleur, step_ms)== ACTION_TERMINEE){
etat_strategie = ATTRAPER_CERISE_GAUCHE;
}
}else{
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
1225, 3000 - 540,
440, 3000 - 775,
225, 3000 - (1500 - 45),
Localisation_get().angle_radian, angle_fin);
if(Strategie_aller_cerises_laterales_opposees(couleur, step_ms)== ACTION_TERMINEE){
etat_strategie = ATTRAPER_CERISE_GAUCHE;
}
}
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
@ -208,6 +205,32 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
etat_strategie = ALLER_PANIER;
}
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:
if(Strategie_aller_panier(couleur, step_ms) == ACTION_TERMINEE){
@ -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 etat_action = ACTION_EN_COURS;
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;
}
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;
}
break;
@ -639,4 +707,48 @@ int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms){
*tempo_ms -= step_ms;
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);
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);
int Robot_est_dans_zone_depose(enum couleur_t couleur);
void Strategie_preparation(void);
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){
static enum {
ATTRAPE_CERISE_DEMI_BAS,

1
Strategie_prise_cerises.h
View File

@ -1,6 +1,7 @@
#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 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_C();
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_evitement(void);
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_strategie(){
@ -98,7 +99,7 @@ int test_strategie(){
case 'd':
case 'D':
while(test_cerise_laterales());
while(test_cerise_laterales_droite());
break;
case 'e':
@ -106,6 +107,11 @@ int test_strategie(){
while(test_evitement());
break;
case 'g':
case 'G':
while(test_cerise_laterales_gauche());
break;
case 'h':
case 'H':
//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;
uint32_t temps_cycle_old;
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 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;