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

trajectoires multiples fonctionnelles, testées avec la super star

Browse Source
This commit is contained in:
Samuel 2025-01-22 20:29:32 +01:00
parent 5b34afcb27
commit 5fde54506c
7 changed files with 88 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.vscode/c_cpp_properties.json vendored
View File

@ -2,6 +2,7 @@
"env": {
"myDefaultIncludePath": [
"${workspaceFolder}",
"${workspaceFolder}/VL53L8CX_ULD_API/inc",
"${workspaceFolder}/build",
"${env:PICO_SDK_PATH}/src/**/include",
"${env:PICO_SDK_PATH}/src/common/pico_base/include",

1
CMakeLists.txt
View File

@ -26,6 +26,7 @@ add_executable(Mon_Projet
Temps.c
Trajectoire_bezier.c
Trajectoire_circulaire.c
Trajectoire_composees.c
Trajectoire_droite.c
Trajectoire.c
Trajet.c

47
Strategie.c
View File

@ -5,46 +5,31 @@
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_AVANCE,
SSS_DANCE
} etat_sss = SSS_INIT;
struct trajectoire_t trajectoire;
static struct trajectoire_t trajectoire_composee;
static struct trajectoire_t trajectoire1, trajectoire2, trajectoire3;
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;
Trajectoire_droite(&trajectoire1, 45, 1895, 1135, 1895, 0, 0);
Trajectoire_circulaire(&trajectoire2, 1135, 1645, M_PI/2, 0, 250, 0, -M_PI/2);
Trajectoire_droite(&trajectoire3, 1385, 1645, 1385, 1580, 0, 0);
Trajectoire_composee_init(&trajectoire_composee);
Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire1);
Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire2);
Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire3);
etat_sss = SSS_AVANCE;
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;
}
break;
case SSS_TOURNE:
//Trajet_config(100, 100);
Trajet_config(TRAJECT_CONFIG_STD);
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;
}
break;
case SSS_AVANCE_2:
Trajet_config(TRAJECT_CONFIG_STD);
Trajectoire_droite(&trajectoire, 1385, 1645, 1385, 1580, 0, 0);
if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
case SSS_AVANCE:
Trajet_config(TRAJECT_CONFIG_RAPIDE);
if(Strategie_parcourir_trajet(trajectoire_composee, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_sss = SSS_DANCE;
}
break;

39
Trajectoire.c
View File

@ -2,15 +2,15 @@
#include "Trajectoire_bezier.h"
#include "Trajectoire_circulaire.h"
#include "Trajectoire_droite.h"
#include "Trajectoire_composees.h"
#include "math.h"
#define NB_MAX_TRAJECTOIRES 5
#define PRECISION_ABSCISSE 0.001f
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_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;
@ -34,8 +34,8 @@ void Trajectoire_droite(struct trajectoire_t * trajectoire, float p1_x, float p1
trajectoire->orientation_fin_rad = 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,
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, float orientation_debut_rad, float orientation_fin_rad){
trajectoire->type = TRAJECTOIRE_BEZIER;
trajectoire->p1.x = p1_x;
trajectoire->p1.y = p1_y;
@ -50,6 +50,27 @@ void Trajectoire_bezier(struct trajectoire_t * trajectoire, float p1_x, float p1
trajectoire->orientation_fin_rad = orientation_fin_rad;
}
/// @brief Initialise une trajectoire composée
/// @param trajectoire : trajectoire à initialiser
void Trajectoire_composee_init(struct trajectoire_t * trajectoire){
trajectoire->type = TRAJECTOIRE_COMPOSEE;
trajectoire->longueur = -1;
trajectoire->nb_trajectoire = 0;
for (int index =0; index <NB_MAX_TRAJECTOIRE ; index++){
trajectoire->trajectoires[index] = NULL;
}
}
/// @brief Ajoute une trajectoire à une trajectoire composée déjà initialisée
/// @param trajectoire_composee
/// @param trajectoire
void Trajectoire_composee_ajout(struct trajectoire_t * trajectoire_composee, struct trajectoire_t * trajectoire){
if(trajectoire_composee->nb_trajectoire < NB_MAX_TRAJECTOIRE){
trajectoire_composee->trajectoires[trajectoire_composee->nb_trajectoire] = trajectoire;
trajectoire_composee->nb_trajectoire++;
}
}
void Trajectoire_inverse(struct trajectoire_t * trajectoire){
struct trajectoire_t old_trajectoire;
old_trajectoire = *trajectoire;
@ -97,6 +118,10 @@ float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire){
case TRAJECTOIRE_BEZIER:
Trajectoire_bezier_get_longueur(trajectoire);
break;
case TRAJECTOIRE_COMPOSEE:
Trajectoire_composee_get_longueur(trajectoire);
break;
}
}
return trajectoire->longueur;
@ -123,6 +148,10 @@ struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, dou
point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse);
break;
case TRAJECTOIRE_COMPOSEE:
point_xyo = Trajectoire_composee_get_point(trajectoire, abscisse);
break;
}
return point_xyo;
}

9
Trajectoire.h
View File

@ -1,10 +1,13 @@
#ifndef TRAJECTOIRE_H
#define TRAJECTOIRE_H
#define NB_MAX_TRAJECTOIRE 10
enum trajectoire_type_t{
TRAJECTOIRE_DROITE,
TRAJECTOIRE_CIRCULAIRE,
TRAJECTOIRE_BEZIER,
TRAJECTOIRE_COMPOSEE
};
struct point_xy_t{
@ -22,6 +25,9 @@ struct trajectoire_t {
float orientation_debut_rad, orientation_fin_rad;
float rayon, angle_debut_rad, angle_fin_rad;
float longueur;
// Pour les trajectoires composées
struct trajectoire_t * trajectoires[NB_MAX_TRAJECTOIRE];
int nb_trajectoire;
};
float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire);
@ -35,6 +41,9 @@ void Trajectoire_droite(struct trajectoire_t * trajectoire, float p1_x, float p1
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,
float orientation_debut_rad, float orientation_fin_rad);
void Trajectoire_rotation(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float orientation_debut_rad, float orientation_fin_rad);
void Trajectoire_composee_init(struct trajectoire_t * trajectoire);
void Trajectoire_composee_ajout(struct trajectoire_t * trajectoire_composee, struct trajectoire_t * trajectoire);
void Trajectoire_inverse(struct trajectoire_t * trajectoire);
#endif

24
Trajectoire_composees.c Normal file
View File

@ -0,0 +1,24 @@
#include <math.h>
#include "Trajectoire.h"
#include "Trajectoire_composees.h"
void Trajectoire_composee_get_longueur(struct trajectoire_t * trajectoire_composee){
trajectoire_composee->longueur = 0;
for(unsigned int i = 0 ; i<trajectoire_composee->nb_trajectoire ; i++) {
trajectoire_composee->longueur += Trajectoire_get_longueur_mm(trajectoire_composee->trajectoires[i]);
}
}
struct point_xyo_t Trajectoire_composee_get_point(struct trajectoire_t * trajectoire_composee, double abscisse){
// Obtenir la trajectoire sur laquelle nous sommes...
int index_trajectoire;
double abscisse_etendue, abscisse_locale;
abscisse_etendue = abscisse * trajectoire_composee->nb_trajectoire;
index_trajectoire = (int) floor(abscisse_etendue);
abscisse_locale = abscisse_etendue - floor(abscisse_etendue);
return Trajectoire_get_point(trajectoire_composee->trajectoires[index_trajectoire], abscisse_locale);
}

3
Trajectoire_composees.h Normal file
View File

@ -0,0 +1,3 @@
struct point_xyo_t Trajectoire_composee_get_point(struct trajectoire_t * trajectoire_composee, double abscisse);
void Trajectoire_composee_get_longueur(struct trajectoire_t * trajectoire_composee);