Keuronde/PAMI_2025
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Début de la trajectoire 2025 pour la superstar

Browse Source
This commit is contained in:
Samuel 2025-01-20 20:39:28 +01:00
parent 7538fdd311
commit 67e5c63f1c
12 changed files with 270 additions and 141 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
.vscode/settings.json vendored
View File

@ -9,6 +9,9 @@
"trajet.h": "c",
"trajectoire.h": "c",
"compare": "c",
"asser_position.h": "c"
"asser_position.h": "c",
"stdlib.h": "c",
"strategie.h": "c",
"strategie_deplacement.h": "c"
}
}

3
CMakeLists.txt
View File

@ -14,12 +14,15 @@ add_executable(Mon_Projet
Asser_Position.c
Asser_Moteurs.c
Commande_vitesse.c
Evitement.c
Geometrie.c
i2c_maitre.c
Moteurs.c
Localisation.c
main.c
QEI.c
Strategie_deplacement.c
Strategie.c
Temps.c
Trajectoire_bezier.c
Trajectoire_circulaire.c

38
Evitement.c Normal file
View File

@ -0,0 +1,38 @@
#include "pico/stdlib.h"
#include "Asser_Moteurs.h"
#include "Evitement.h"
#include "Trajet.h"
// 250 ms
#define TEMPS_VALIDE_OBSTACLE_US 250000
enum evitement_statu_t evitement_statu=PAS_D_OBSTACLE;
void Evitement_gestion(int step_ms){
static uint32_t temps_obstacle;
switch(evitement_statu){
case PAS_D_OBSTACLE:
if(Trajet_get_bloque() == 1 && AsserMoteur_RobotImmobile(step_ms)){
evitement_statu = OBSTACLE_NON_CONFIRME;
temps_obstacle = time_us_32();
}
break;
case OBSTACLE_NON_CONFIRME:
if(time_us_32() - temps_obstacle > TEMPS_VALIDE_OBSTACLE_US){
evitement_statu = OBSTACLE_CONFIRME;
}
if(!Trajet_get_bloque()){
evitement_statu = PAS_D_OBSTACLE;
}
break;
case OBSTACLE_CONFIRME:
if(!Trajet_get_bloque()){
evitement_statu = PAS_D_OBSTACLE;
}
break;
}
}
enum evitement_statu_t Evitement_get_statu(){
return evitement_statu;
}

8
Evitement.h Normal file
View File

@ -0,0 +1,8 @@
enum evitement_statu_t{
PAS_D_OBSTACLE,
OBSTACLE_NON_CONFIRME,
OBSTACLE_CONFIRME,
};
enum evitement_statu_t Evitement_get_statu();
void Evitement_gestion(int step_ms);

58
Strategie.c Normal file
View File

@ -0,0 +1,58 @@
#include "Strategie.h"
#include "Geometrie.h"
enum etat_action_t Strategie_super_star(uint32_t step_ms){
static enum{
SSS_TEST_TOURNE,
SSS_INIT,
SSS_AVANCE_1,
SSS_TOURNE,
SSS_AVANCE_2,
SSS_DANCE
} etat_sss = SSS_TEST_TOURNE;
struct trajectoire_t trajectoire;
switch(etat_sss){
case SSS_TEST_TOURNE:
Localisation_set(1135, 1895, 0);
etat_sss = SSS_TOURNE;
break;
case SSS_INIT:
Localisation_set(45, 1895, 0);
etat_sss = SSS_AVANCE_1;
break;
case SSS_AVANCE_1:
Trajet_config(TRAJECT_CONFIG_STD);
Trajectoire_droite(&trajectoire, 45, 1895, 1135, 1895, 0, 0);
if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
//etat_sss = SSS_TOURNE;
etat_sss = SSS_DANCE;
}
break;
case SSS_TOURNE:
Trajet_config(100, 100);
Trajectoire_circulaire(&trajectoire, 1135, 1645, M_PI/2, 0, 250, 0, -M_PI/2);
if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
//etat_sss = SSS_AVANCE_2;
etat_sss = SSS_DANCE;
}
break;
case SSS_AVANCE_2:
Trajet_config(TRAJECT_CONFIG_STD);
Trajectoire_droite(&trajectoire, 1385, 1645, 1385, 1600, 0, 0);
if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_sss = SSS_DANCE;
}
break;
case SSS_DANCE:
return ACTION_TERMINEE;
}
return ACTION_EN_COURS;
}

53
Strategie.h Normal file
View File

@ -0,0 +1,53 @@
#include "pico/stdlib.h"
#include "Localisation.h"
#include "Temps.h"
#include "Trajectoire.h"
#include "Trajet.h"
#ifndef STRATEGIE_H
#define STRATEGIE_H
#define COULEUR 15
#define TIRETTE 14
//#define CORR_ANGLE_DEPART_DEGREE (-1.145)
#define CORR_ANGLE_DEPART_DEGREE (0)
enum etat_action_t{
ACTION_EN_COURS,
ACTION_TERMINEE,
ACTION_ECHEC
};
enum longer_direction_t{
LONGER_VERS_A,
LONGER_VERS_C
};
enum couleur_t{
COULEUR_BLEU=0,
COULEUR_JAUNE,
COULEUR_INCONNUE,
};
enum evitement_t{
EVITEMENT_SANS_EVITEMENT,
EVITEMENT_PAUSE_DEVANT_OBSTACLE,
EVITEMENT_ARRET_DEVANT_OBSTACLE,
EVITEMENT_RETOUR_SI_OBSTABLE,
EVITEMENT_CONTOURNEMENT
};
struct objectif_t{
int priorite;
enum {A_FAIRE, EN_COURS, BLOQUE, FAIT} etat;
enum {CERISE_BAS, CERISE_HAUT, CERISE_GAUCHE, CERISE_DROITE,
ZONE_1, ZONE_2, ZONE_3, ZONE_4, ZONE_5} cible;
};
enum etat_action_t Strategie_super_star(uint32_t step_ms);
enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement);
// STRATEGIE_H
#endif

73
Strategie_deplacement.c Normal file
View File

@ -0,0 +1,73 @@
#include "Strategie.h"
#include "Strategie_deplacement.h"
#include "Trajet.h"
#include "Evitement.h"
#include "Geometrie.h"
#include "VL53L8_2024.h"
float distance_obstacle;
enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement){
enum etat_action_t etat_action = ACTION_EN_COURS;
enum etat_trajet_t etat_trajet;
float angle_avancement;
static bool trajet_inverse = false;
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:
if(evitement != EVITEMENT_SANS_EVITEMENT){
angle_avancement = Trajet_get_orientation_avance();
distance_obstacle = VL53L8_get_old_min_distance();
Trajet_set_obstacle_mm(distance_obstacle);
}
if(Evitement_get_statu() == OBSTACLE_CONFIRME || Evitement_get_statu() == OBSTACLE_NON_CONFIRME){
switch(evitement){
case EVITEMENT_SANS_EVITEMENT:
//printf("Evitement lors trajet EVITEMENT_SANS_EVITEMENT: ERREUR\n");
break;
case EVITEMENT_PAUSE_DEVANT_OBSTACLE:
// Rien à faire ici
break;
case EVITEMENT_ARRET_DEVANT_OBSTACLE:
etat_parcourt = PARCOURS_INIT;
return ACTION_ECHEC;
case EVITEMENT_RETOUR_SI_OBSTABLE:
trajet_inverse = !trajet_inverse;
Trajet_inverse();
break;
case EVITEMENT_CONTOURNEMENT: // TODO
break;
}
}
etat_trajet = Trajet_avance(step_ms/1000.);
if(etat_trajet == TRAJET_TERMINE){
if(trajet_inverse){
etat_action = ACTION_ECHEC;
}else{
etat_action = ACTION_TERMINEE;
}
etat_parcourt = PARCOURS_INIT;
}
break;
}
return etat_action;
}

0
Strategie_deplacement.h Normal file
View File

10
Trajectoire.c
View File

@ -9,13 +9,13 @@
#define PRECISION_ABSCISSE 0.001f
void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_degre, float angle_fin_degre, float rayon,
void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_rad, float angle_fin_rad, float rayon,
float orientation_debut_rad, float orientation_fin_rad){
trajectoire->type = TRAJECTOIRE_CIRCULAIRE;
trajectoire->p1.x = centre_x;
trajectoire->p1.y = centre_y;
trajectoire->angle_debut_degre = angle_debut_degre;
trajectoire->angle_fin_degre = angle_fin_degre;
trajectoire->angle_debut_rad = angle_debut_rad;
trajectoire->angle_fin_rad = angle_fin_rad;
trajectoire->rayon = rayon;
trajectoire->longueur = -1;
trajectoire->orientation_debut_rad = orientation_debut_rad;
@ -58,8 +58,8 @@ void Trajectoire_inverse(struct trajectoire_t * trajectoire){
trajectoire->orientation_fin_rad = old_trajectoire.orientation_debut_rad;
if(trajectoire->type == TRAJECTOIRE_CIRCULAIRE){
trajectoire->angle_debut_degre = old_trajectoire.angle_fin_degre;
trajectoire->angle_fin_degre = old_trajectoire.angle_debut_degre;
trajectoire->angle_debut_rad = old_trajectoire.angle_fin_rad;
trajectoire->angle_fin_rad = old_trajectoire.angle_debut_rad;
return;
}
if(trajectoire->type == TRAJECTOIRE_DROITE){

4
Trajectoire.h
View File

@ -20,7 +20,7 @@ struct trajectoire_t {
enum trajectoire_type_t type;
struct point_xy_t p1, p2, p3, p4;
float orientation_debut_rad, orientation_fin_rad;
float rayon, angle_debut_degre, angle_fin_degre;
float rayon, angle_debut_rad, angle_fin_rad;
float longueur;
};
@ -29,7 +29,7 @@ struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, dou
float Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, float abscisse);
float Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, double distance_mm);
double distance_points(struct point_xy_t point, struct point_xy_t point_old);
void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_degre, float angle_fin_degre,
void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_rad, float angle_fin_rad,
float rayon, float orientation_debut_rad, float orientation_fin_rad);
void Trajectoire_droite(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float orientation_debut_rad, float orientation_fin_rad);
void Trajectoire_bezier(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float p3_x, float p3_y, float p4_x, float p4_y,

16
Trajectoire_circulaire.c
View File

@ -4,23 +4,23 @@
void Trajectoire_circulaire_get_longueur(struct trajectoire_t * trajectoire){
float distance_angulaire;
if(trajectoire->angle_debut_degre > trajectoire->angle_fin_degre){
distance_angulaire = trajectoire->angle_debut_degre - trajectoire->angle_fin_degre;
if(trajectoire->angle_debut_rad > trajectoire->angle_fin_rad){
distance_angulaire = trajectoire->angle_debut_rad - trajectoire->angle_fin_rad;
}else{
distance_angulaire = trajectoire->angle_fin_degre - trajectoire->angle_debut_degre;
distance_angulaire = trajectoire->angle_fin_rad - trajectoire->angle_debut_rad;
}
trajectoire->longueur = 2. * M_PI * trajectoire->rayon * distance_angulaire / 360.;
trajectoire->longueur = trajectoire->rayon * distance_angulaire;
}
/// @brief Retourne le point sur la trajectoire en fonction de l'abscisse
/// @param abscisse : compris entre 0 et 1
struct point_xy_t Trajectoire_circulaire_get_point(struct trajectoire_t * trajectoire, float abscisse){
struct point_xy_t point;
float angle_degre;
float angle_rad;
angle_degre = (float) trajectoire->angle_debut_degre * (1-abscisse) + (float) trajectoire->angle_fin_degre * abscisse;
point.x = trajectoire->p1.x + cos(angle_degre/180. * M_PI) * trajectoire->rayon;
point.y = trajectoire->p1.y + sin(angle_degre/180. * M_PI) * trajectoire->rayon;
angle_rad = (float) trajectoire->angle_debut_rad * (1-abscisse) + (float) trajectoire->angle_fin_rad * abscisse;
point.x = trajectoire->p1.x + cos(angle_rad) * trajectoire->rayon;
point.y = trajectoire->p1.y + sin(angle_rad) * trajectoire->rayon;
return point;
}

143
main.c
View File

@ -13,6 +13,7 @@
#include "i2c_maitre.h"
#include "Localisation.h"
#include "Moteurs.h"
#include "Strategie.h"
#include "Temps.h"
#include "Trajectoire.h"
#include "Trajet.h"
@ -37,9 +38,8 @@ uint16_t tension_batterie_lire(void);
void identifiant_init(void);
uint identifiant_lire(void);
int get_tirette(int);
int get_tirette(void);
int get_couleur(void);
void configure_trajet(int identifiant, int couleur);
void gestion_VL53L8CX(void);
@ -54,7 +54,6 @@ VL53L8CX_Configuration Dev;
void main(void)
{
int ledpower = 500;
VL53L8CX_ResultsData Results;
bool fin_match = false;
@ -62,7 +61,7 @@ void main(void)
stdio_init_all();
Temps_init();
//tension_batterie_init();
identifiant_init();
Localisation_init(identifiant_lire());
Trajet_init(identifiant_lire());
@ -81,22 +80,19 @@ void main(void)
gpio_put(LED1PIN, 1);
//multicore_launch_core1(gestion_affichage);
multicore_launch_core1(gestion_VL53L8CX);
multicore_launch_core1(gestion_affichage);
// TODO: A remettre - quand on aura récupéré un capteur
//multicore_launch_core1(gestion_VL53L8CX);
sleep_ms(5000);
printf("Demarrage...\n");
configure_trajet(identifiant_lire(), get_couleur());
float vitesse_init =300;
vitesse = vitesse_init;
enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;
while(get_tirette(identifiant_lire()));
while(get_tirette());
gpio_put(LED1PIN, 0);
// Seul le premier PAMI doit attendre 90s, les autres démarrent lorsque celui de devant part
@ -119,9 +115,7 @@ void main(void)
QEI_update();
Localisation_gestion();
if(etat_trajet != TRAJET_TERMINE){
etat_trajet = Trajet_avance((float)step_ms/1000.);
}else{
if(Strategie_super_star(step_ms) == ACTION_TERMINEE){
Asser_Position_maintien();
if(Asser_Position_panic_angle()){
fin_match=1;
@ -168,18 +162,21 @@ void gestion_affichage(void){
void affichage(void){
/*printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) );
printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );*/
printf(">pos_x:%.1f\n>pos_y:%.1f\n>pos_angle:%.1f\n", Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian);
long temps_ms = (time_us_64()/1000);
printf(">temps_ms:%lu\n", temps_ms);
printf(">pos_angle:%.1f\n", Localisation_get().angle_radian);
printf(">distance_obstacle:%f\n",Trajet_get_obstacle_mm());
printf(">abscisse:%f\n",abscisse);
struct position_t position_actuelle;
position_actuelle = Localisation_get();
printf(">delta_orientation_radian:%.2f\n>angle_delta:%.2f\n",delta_orientation_radian, atan2f(delta_y_mm, delta_x_mm));
printf(">pos_x:%.2f\n>pos_y:%.2f\n", position_actuelle.x_mm, position_actuelle.y_mm);
printf(">con_x:%.2f\n>con_y:%.2f\n", point.point_xy.x, point.point_xy.y);
printf(">delta_orientation_radian:%lu:%.2f\n>angle_delta:%lu:%.2f\n", temps_ms, delta_orientation_radian, temps_ms, atan2f(delta_y_mm, delta_x_mm));
printf(">pos_x:%lu:%.2f\n>pos_y:%lu:%.2f\n", temps_ms, position_actuelle.x_mm, temps_ms, position_actuelle.y_mm);
printf(">con_x:%lu:%.2f\n>con_y:%lu:%.2f\n", temps_ms, point.point_xy.x, temps_ms, point.point_xy.y);
printf(">couleur:%d\n>id:%d\n>Tirette:%d\n", get_couleur(), identifiant_lire(), get_tirette(identifiant_lire()));
printf(">couleur:%d\n>id:%d\n>Tirette:%d\n", get_couleur(), identifiant_lire(), get_tirette());
}
void tension_batterie_init(void){
@ -219,11 +216,8 @@ void identifiant_init(){
gpio_set_dir(COULEUR_PIN, GPIO_IN);
}
int get_tirette(int id){
if(id == 3){
return !gpio_get(TIRETTE_PIN);
}
return (VL53L8_get_old_min_distance() <50);
int get_tirette(){
return !gpio_get(TIRETTE_PIN);
}
@ -236,104 +230,3 @@ int get_couleur(void){
uint identifiant_lire(){
return (gpio_get(21) << 2)+ (gpio_get(22) << 1) + gpio_get(26);
}
void configure_trajet(int identifiant, int couleur){
struct trajectoire_t trajectoire;
Trajet_config(TRAJECT_CONFIG_RAPIDE);
switch(couleur){
case COULEUR_JAUNE:
switch (identifiant)
{
case 0:
Localisation_set(3000-1249, 2000-63, 0);
Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63,
3000-750, 1400, 3000-750, 2100, 0, 0);
break;
case 1:
Localisation_set(3000-1249, 2000-63, 0);
Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63,
3000-750, 1400, 3000-750, 2100, 0, 0);
break;
case 2:
Localisation_set(3000-1245, 2000-63, 0);
Trajectoire_bezier(&trajectoire, 3000-1244, 2000-63, 3000-950, 2000-63,
3000-540, 1400, 3100, 1400, M_PI, M_PI);
break;
case 3:
Localisation_set(3000-1130, 2000-63, 0);
Trajectoire_bezier(&trajectoire, 3000-1122, 2000-63, 3000-905, 2000-63,
3000-606, 2000-590, 3000-225, 2000-225, -M_PI, -M_PI);
break;
case 4:
Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
Localisation_set(3000-1364, 2000-63, 0);
Trajectoire_bezier(&trajectoire, 3000-1363, 2000-63, 3000-550, 2000-63,
2700-900, 600, 2700-0, 0, 0, 0);
break;
case 5:
Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
Localisation_set(3000-1450, 2000-63, 0);
Trajectoire_bezier(&trajectoire, 3000-1449, 2000-63, 3000-675, 2000-63,
3000-930, 970, 0, 1200, -M_PI / 2., M_PI);
break;
case 6:
break;
case 7:
break;
default:
break;
}
break;
case COULEUR_BLEU:
switch (identifiant)
{
case 0:
Localisation_set(1249, 2000-63, M_PI);
Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63,
750, 1400, 750, 2100, M_PI, M_PI);
break;
case 1:
Localisation_set(1249, 2000-63, M_PI);
Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63,
750, 1400, 750, 2100, M_PI, M_PI);
break;
case 2:
Localisation_set(1245, 2000-63, M_PI);
Trajectoire_bezier(&trajectoire, 1244, 2000-63, 950, 2000-63,
540, 1400, -100, 1400, M_PI, M_PI);
break;
case 3:
Localisation_set(1121, 2000-63, M_PI);
Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63,
606, 2000-590, 225, 2000-225, M_PI, M_PI);
break;
case 4:
Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
Localisation_set(1364, 2000-63, M_PI);
Trajectoire_bezier(&trajectoire, 1363, 2000-63, 550, 2000-63,
900, 600, 0, 0, -M_PI / 2., M_PI);
break;
case 5:
Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
Localisation_set(1450, 2000-63, M_PI);
Trajectoire_bezier(&trajectoire, 1449, 2000-63, 675, 2000-63,
930, 970, 3000, 1200, -M_PI / 2., M_PI);
break;
case 6:
break;
case 7:
break;
default:
break;
}
break;
}
Trajet_debut_trajectoire(trajectoire);
}