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

evitement presque ok

Browse Source
This commit is contained in:
Samuel 2023-04-21 22:33:29 +02:00
parent 3794fbf3c8
commit 51bba9dc23
8 changed files with 60 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
Balise_VL53L1X.c
View File

@ -124,7 +124,7 @@ double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant){
double angle;
} cone[NB_CONE];
cone[0].angle = 22 * DEGRE_EN_RADIAN;
cone[0].distance_mm = 800;
cone[0].distance_mm = 1200;
cone[1].angle = 50 * DEGRE_EN_RADIAN;
cone[1].distance_mm = 580;
@ -152,12 +152,13 @@ double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant){
if(capteurs_VL53L1X[capteur].donnee_valide){
// Si la distance est plus petite que la distance minimale actuelle
// Si la distance est plus petite que le cône en question...
if(capteurs_VL53L1X[capteur].distance_obstacle_robot_mm < distance_minimale /*&&
capteurs_VL53L1X[capteur].distance_obstacle_robot_mm < cone[cone_index].distance_mm*/){
if(capteurs_VL53L1X[capteur].distance_obstacle_robot_mm < distance_minimale){
if(capteurs_VL53L1X[capteur].distance_obstacle_robot_mm < cone[cone_index].distance_mm){
// On retient cette distance comme étant notre distance minimale
distance_minimale = capteurs_VL53L1X[capteur].distance_obstacle_robot_mm;
}
}
}
// Le capteur est pris en compte au moins dans un cône
capteurs_VL53L1X[capteur].donnee_ignore = 0;
}

1
Geometrie.h
View File

@ -6,6 +6,7 @@
#endif
#define DEGRE_EN_RADIAN (M_PI / 180.)
#define DISTANCE_INVALIDE (-1.)
struct position_t{
double x_mm, y_mm;

15
Strategie.c
View File

@ -16,6 +16,9 @@
#define DISTANCE_OBSTACLE_CM 50
#define DISTANCE_PAS_OBSTACLE_MM 2000
// TODO: Peut-être à remetttre en variable locale après
double distance_obstacle;
enum etat_action_t calage_angle(enum longer_direction_t longer_direction, double x_mm, double y_mm, double angle_radian);
enum etat_action_t lance_balles(uint32_t step_ms);
@ -201,6 +204,8 @@ enum etat_action_t calage_angle(enum longer_direction_t longer_direction, double
enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms){
enum etat_action_t etat_action = ACTION_EN_COURS;
enum etat_trajet_t etat_trajet;
double angle_avancement;
static enum {
PARCOURS_INIT,
PARCOURS_AVANCE,
@ -213,12 +218,13 @@ enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint
break;
case PARCOURS_AVANCE:
if(Balise_VL53L1X_get_min_distance() <DISTANCE_OBSTACLE_CM){
Trajet_stop(step_ms);
break;
}
angle_avancement = Trajet_get_orientation_avance();
distance_obstacle = Balise_VL53L1X_get_distance_obstacle_mm(angle_avancement);
Trajet_set_obstacle_mm(distance_obstacle);
etat_trajet = Trajet_avance(step_ms/1000.);
if(etat_trajet == TRAJET_TERMINE){
Trajet_set_obstacle_mm(DISTANCE_INVALIDE);
etat_action = ACTION_TERMINEE;
etat_parcourt = PARCOURS_INIT;
}
@ -239,6 +245,7 @@ enum etat_action_t parcourt_trajet_simple_sans_evitement(struct trajectoire_t tr
switch (etat_parcourt){
case PARCOURS_INIT:
Trajet_debut_trajectoire(trajectoire);
Trajet_set_obstacle_mm(distance_pas_obstacle);
etat_parcourt = PARCOURS_AVANCE;
break;

2
Strategie.h
View File

@ -56,7 +56,7 @@ int test_panier(void);
int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms);
extern double distance_obstacle;
// STRATEGIE_H
#endif

2
Test.c
View File

@ -1393,6 +1393,8 @@ int test_angle_balise(void){
distance = Balise_VL53L1X_get_distance_obstacle_mm(angle);
printf(">distance_obstacle:%3.0f\n", distance);
sleep_ms(100);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);

6
Test_strategie.c
View File

@ -67,6 +67,7 @@ void affichage_test_strategie(){
int test_strategie(){
printf("A - Accoster.\n");
printf("C - Couleur et tirette.\n");
printf("E - Evitement\n");
printf("H - Homologation.\n");
printf("L - Longer.\n");
printf("P - Panier.\n");
@ -169,7 +170,6 @@ int test_homologation(){
void affichage_test_evitement(){
while(1){
printf(">min_dist:%d\n",Balise_VL53L1X_get_min_distance());
for(uint8_t capteur=0; capteur<12; capteur++){
printf(">c%d:%d\n",capteur, Balise_VL53L1X_get_capteur_cm(capteur));
}
@ -218,11 +218,11 @@ int test_evitement(){
}
Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
1000,0,
1000,200,
0, 0); // Angles
if(parcourt_trajet_simple(trajectoire, _step_ms) == ACTION_TERMINEE){
etat_strategie = CALAGE_PANIER_1;
}
}

41
Trajet.c
View File

@ -12,10 +12,12 @@ double position_mm; // Position en mm sur la trajectoire
double vitesse_mm_s;
double vitesse_max_trajet_mm_s=500;
double acceleration_mm_ss;
const double acceleration_mm_ss_obstacle = 1500;
struct trajectoire_t trajet_trajectoire;
struct position_t position_consigne;
double distance_obstacle_mm;
const double distance_pas_obstacle = 2000;
/// @brief Initialise le module Trajet. A appeler en phase d'initilisation
void Trajet_init(){
@ -112,6 +114,7 @@ struct position_t Trajet_get_consigne(){
/// @return vitesse déterminée en m/s
double Trajet_calcul_vitesse(double pas_de_temps_s){
double vitesse_max_contrainte;
double vitesse_max_contrainte_obstacle;
double distance_contrainte,distance_contrainte_obstacle;
double vitesse;
// Calcul de la vitesse avec acceleration
@ -120,13 +123,6 @@ double Trajet_calcul_vitesse(double pas_de_temps_s){
// Calcul de la vitesse maximale due à la contrainte en fin de trajectoire (0 mm/s)
// https://poivron-robotique.fr/Consigne-de-vitesse.html
distance_contrainte = Trajectoire_get_longueur_mm(&trajet_trajectoire) - position_mm;
/*
distance_contrainte_obstacle = Trajet_get_obstacle_mm();
if(distance_contrainte > distance_contrainte_obstacle){
distance_contrainte = distance_contrainte_obstacle;
}*/
// En cas de dépassement, on veut garder la contrainte, pour l'instant
if(distance_contrainte > 0){
vitesse_max_contrainte = sqrt(2 * acceleration_mm_ss * distance_contrainte);
@ -134,6 +130,15 @@ double Trajet_calcul_vitesse(double pas_de_temps_s){
vitesse_max_contrainte = 0;
}
distance_contrainte_obstacle = Trajet_get_obstacle_mm();
if(distance_contrainte_obstacle != DISTANCE_INVALIDE){
vitesse_max_contrainte_obstacle = sqrt(2 * acceleration_mm_ss_obstacle * distance_contrainte_obstacle);
if(vitesse_max_contrainte_obstacle < vitesse_max_contrainte){
vitesse_max_contrainte = vitesse_max_contrainte_obstacle;
}
}
// Selection de la vitesse la plus faible
if(vitesse > vitesse_max_contrainte){
vitesse = vitesse_max_contrainte;
@ -152,3 +157,25 @@ double Trajet_get_obstacle_mm(void){
void Trajet_set_obstacle_mm(double distance_mm){
distance_obstacle_mm = distance_mm;
}
/// @brief Renvoi l'angle d'avancement du robot dans le référentiel du terrain
/// @return angle en radian.
double Trajet_get_orientation_avance(){
struct point_xyo_t point, point_suivant;
double avance_abscisse = 0.01;
double angle;
if(abscisse >= 1){
return 0;
}
if(abscisse + avance_abscisse >= 1){
avance_abscisse = 1 - abscisse;
}
point = Trajectoire_get_point(&trajet_trajectoire, abscisse);
point_suivant = Trajectoire_get_point(&trajet_trajectoire, abscisse + avance_abscisse);
angle = atan2(point_suivant.point_xy.y - point.point_xy.y, point_suivant.point_xy.x - point.point_xy.x);
return angle;
}

2
Trajet.h
View File

@ -12,6 +12,7 @@ enum etat_trajet_t{
// Vitesse et acceleration pour une rotation (rad/s et rad/s²)
#define TRAJECT_CONFIG_ROTATION_PURE 2, 2
extern const double distance_pas_obstacle;
void Trajet_init();
void Trajet_config(double _vitesse_max_trajet_mm_s, double _acceleration_mm_ss);
@ -21,3 +22,4 @@ struct position_t Trajet_get_consigne(void);
double Trajet_get_obstacle_mm(void);
void Trajet_set_obstacle_mm(double distance_mm);
void Trajet_stop(double);
double Trajet_get_orientation_avance(void);