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

Ajout score dans la stratégie + modification cerises latérales

Browse Source
This commit is contained in:
Samuel 2023-05-11 21:04:02 +02:00
parent c7fb4b5ef8
commit 470b5cbc4a
8 changed files with 119 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -25,6 +25,7 @@ Localisation.c
Moteurs.c
Monitoring.c
Robot_config.c
Score.c
Servomoteur.c
Strategie.c
Strategie_prise_cerises.c

4
Holonome2023.c
View File

@ -82,7 +82,7 @@ int main() {
AsserMoteur_Init();
Localisation_init();
//while(mode_test());
while(mode_test());
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
@ -154,7 +154,7 @@ int main() {
printf("MATCH_ARRET_EN_COURS\n");
statu_match = MATCH_ARRET_EN_COURS;
}
Strategie(couleur, _step_ms);
Strategie(couleur, _step_ms, timer_match_ms);
break;
case MATCH_ARRET_EN_COURS:

40
Score.c Normal file
View File

@ -0,0 +1,40 @@
#include "pico/stdlib.h"
#include "i2c_annexe.h"
#include "Score.h"
#define SCORE_INITIAL 5
uint32_t Score_nb_cerises=0;
uint32_t Score_nb_points=0;
uint32_t Score_funny_action=0;
uint32_t Score_pieds_dans_plat=0;
void Score_recalcule(){
Score_nb_points = SCORE_INITIAL;
if(Score_nb_cerises > 0){
Score_nb_points += Score_nb_cerises + 5;
}
Score_nb_points += Score_funny_action;
Score_nb_points += Score_pieds_dans_plat;
}
void Score_actualise(){
Score_recalcule();
i2c_annexe_envoie_score(Score_nb_points);
}
void Score_set_funny_action(){
Score_funny_action = 5;
Score_actualise();
}
void Score_set_pieds_dans_plat(){
Score_pieds_dans_plat = 15;
Score_actualise();
}
void Score_ajout_cerise(uint32_t nb_cerises){
Score_nb_cerises += nb_cerises;
Score_actualise();
}

5
Score.h Normal file
View File

@ -0,0 +1,5 @@
#include "pico/stdlib.h"
void Score_set_funny_action();
void Score_set_pieds_dans_plat();
void Score_ajout_cerise(uint32_t nb_cerises);

49
Strategie.c
View File

@ -6,6 +6,7 @@
#include "Geometrie_robot.h"
#include "Localisation.h"
#include "Moteurs.h"
#include "Score.h"
#include "Strategie_prise_cerises.h"
#include "Strategie.h"
#include "Trajet.h"
@ -25,14 +26,13 @@ struct objectif_t{
} objectifs[NB_OBJECTIFS];
struct objectif_t * objectif_courant;
uint32_t Score_nb_cerises = 0;
uint32_t Score_cerises_dans_robot=0;
void Score_set_cerise_dans_robot(uint32_t nb_cerises){
void Strategie_set_cerise_dans_robot(uint32_t nb_cerises){
Score_cerises_dans_robot = nb_cerises;
}
uint32_t Score_get_cerise_dans_robot(void){
uint32_t Strategie_get_cerise_dans_robot(void){
return Score_cerises_dans_robot;
}
@ -48,11 +48,13 @@ enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float
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_homologation_t etat_homologation=STRATEGIE_INIT;
void Strategie(enum couleur_t couleur, uint32_t step_ms){
void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
float angle_fin;
float recal_pos_x, recal_pos_y;
@ -136,7 +138,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
etat_action = cerise_attraper_bordure(longer_direction, step_ms, recal_pos_x, recal_pos_y);
if(etat_action == ACTION_TERMINEE){
Score_set_cerise_dans_robot(6);
Strategie_set_cerise_dans_robot(6);
etat_strategie = ALLER_PANIER;
}
break;
@ -157,7 +159,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){
etat_strategie = ATTRAPER_CERISE_HAUT;
Score_set_cerise_dans_robot(6);
Strategie_set_cerise_dans_robot(6);
}
break;
@ -214,7 +216,9 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
break;
case LANCER_PANIER:
if(lance_balles_dans_panier(couleur, step_ms, Score_get_cerise_dans_robot())== ACTION_TERMINEE){
if(lance_balles_dans_panier(couleur, step_ms, Strategie_get_cerise_dans_robot())== ACTION_TERMINEE){
Score_ajout_cerise(Strategie_get_cerise_dans_robot());
Strategie_set_cerise_dans_robot(0);
objectif_courant->etat = FAIT;
etat_strategie = DECISION_STRATEGIE;
}
@ -265,13 +269,16 @@ enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step
// Definition des trajectoires
if(couleur == COULEUR_BLEU){
// Si le robot est déjà dans le quart haut gauche,
// Si le robot est déjà dans la zone cerise haut
// On va en ligne droite
if(Robot_est_dans_quart_haut_gauche()){
if(Robot_est_dans_zone_cerise_haut(couleur)){
Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
465,2830,
Localisation_get().angle_radian,
Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN));
}else if (Robot_est_dans_zone_cerise_gauche()){
/// !!! TODO !!!
}else{
// Sinon, on a une courbe de bézier
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
@ -307,6 +314,8 @@ enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step
}
/// Fonction de localisation aproximatives pour la stratégie.
/// @brief Renvoi 1 si le robot est dans le quart supérieur gauche du terrain
/// @return 1 si oui, 0 si non.
int Robot_est_dans_quart_haut_gauche(){
@ -325,6 +334,28 @@ int Robot_est_dans_quart_haut_droit(){
return 0;
}
int Robot_est_dans_zone_cerise_gauche(){
if(Localisation_get().x_mm < 630 &&
Localisation_get().y_mm >1000 && Localisation_get().y_mm < 2000){
return 1;
}
return 0;
}
int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur){
if(couleur == COULEUR_BLEU){
if(Localisation_get().y_mm > 2500 && Localisation_get().x_mm < 1000){
return 1;
}
}else{
if(Localisation_get().y_mm > 2500 && Localisation_get().x_mm > 1000){
return 1;
}
}
return 0;
}
enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms, uint32_t nb_cerises){
static enum {
CALAGE_PANIER_1,

2
Strategie.h
View File

@ -54,7 +54,7 @@ enum couleur_t lire_couleur(void);
uint attente_tirette(void);
enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises);
int test_panier(void);
void Strategie(enum couleur_t couleur, uint32_t step_ms);
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);

35
Strategie_prise_cerises.c
View File

@ -7,6 +7,7 @@
#include "Localisation.h"
#include "math.h"
#include "i2c_annexe.h"
#include "Trajet.h"
// Rotation en rad/s pour accoster les cerises
#define ROTATION_CERISE 0.5f
@ -18,9 +19,12 @@
// Translation en mm/s pour aspirer les cerises
#define TRANSLATION_CERISE 70
// Seuil angulaire pour la détection de la fin de la barrette de cerises
#define SEUIL_ANGLE_CERISE (2. * DEGRE_EN_RADIAN)
void commande_rotation_contacteur_longer_A();
void commande_rotation_contacteur_longer_C();
enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum longer_direction_t longer_direction);
enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum longer_direction_t longer_direction, float pos_recal_x_mm);
enum etat_action_t demarre_turbine(uint32_t step_ms);
enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction);
@ -39,10 +43,11 @@ enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms){
}etat_attrappe_cerises_gauche = 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_gauche){
case ATTRAPE_CERISE_DEMI_BAS:
if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_A) == ACTION_TERMINEE){
if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_A, pos_recal_x_mm) == ACTION_TERMINEE){
etat_attrappe_cerises_gauche = REVENIR_CENTRE;
}
break;
@ -59,7 +64,7 @@ enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms){
break;
case ATTRAPE_CERISE_DEMI_HAUT:
if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_C) == ACTION_TERMINEE){
if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_C, pos_recal_x_mm) == ACTION_TERMINEE){
etat_attrappe_cerises_gauche = ATTRAPE_CERISE_DEMI_BAS;
return ACTION_TERMINEE;
}
@ -70,7 +75,7 @@ enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms){
}
enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum longer_direction_t longer_direction){
enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum longer_direction_t longer_direction, float pos_recal_x_mm){
// 1 accoster
// Demarrer la turbine
// 2 Longer en aspirant
@ -79,12 +84,19 @@ enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum
ACCOSTAGE,
DEMARRE_TURBINE,
LONGE,
TOURNE,
AVANCE,
}etat_attrappe_demi_cerise=ACCOSTAGE;
const float continu_avance_mm = 60;
static float orientation_ref;
static float pos_y_ref;
struct trajectoire_t trajectoire;
switch(etat_attrappe_demi_cerise){
case ACCOSTAGE:
if(cerise_accostage() == ACTION_TERMINEE){
Localisation_set_x(pos_recal_x_mm);
orientation_ref = Localisation_get().angle_radian;
etat_attrappe_demi_cerise = DEMARRE_TURBINE;
}
break;
@ -101,7 +113,18 @@ enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum
// Le robot n'a les deux contacteurs sur le support que tant qu'il est à moins de 240mm (MAX_LONGE_MM) de la bordure
// ou 120 (MAX_LONGE_MM/2) du milieu de la bordure
// En fonction du demi-terrain sur lequel se trouve le robot, on surveille la position en Y pour respecter cette condition
if( (Localisation_get().y_mm > 1500 + MAX_LONGE_MM/2 ) || (Localisation_get().y_mm < 1500 - MAX_LONGE_MM/2 )){
/*if( (Localisation_get().y_mm > 1500 + MAX_LONGE_MM/2 ) || (Localisation_get().y_mm < 1500 - MAX_LONGE_MM/2 )){
etat_attrappe_demi_cerise = AVANCE;
}*/
if(fabs(orientation_ref - Localisation_get().angle_radian) > SEUIL_ANGLE_CERISE){
etat_attrappe_demi_cerise = TOURNE;
pos_y_ref = Localisation_get().y_mm;
}
break;
case TOURNE:
Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian, orientation_ref);
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
etat_attrappe_demi_cerise = AVANCE;
}
break;
@ -113,7 +136,7 @@ enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum
commande_translation_longer_vers_C();
}
if( (Localisation_get().y_mm > 1500 + MAX_ASPIRE_CERISE_MM/2 ) || (Localisation_get().y_mm < 1500 - MAX_ASPIRE_CERISE_MM/2 )){
if( (Localisation_get().y_mm > pos_y_ref + continu_avance_mm ) || (Localisation_get().y_mm < pos_y_ref - continu_avance_mm )){
etat_attrappe_demi_cerise = ACCOSTAGE;
i2c_annexe_desactive_turbine();
commande_vitesse_stop();

2
Test_strategie.c
View File

@ -139,7 +139,7 @@ int test_cerise_laterales(){
Trajet_init();
Balise_VL53L1X_init();
//printf("Init gyroscope\n");
set_position_avec_gyroscope(0);
set_position_avec_gyroscope(1);
if(get_position_avec_gyroscope()){
Gyro_Init();
}