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

Fonction d'évitement en aller-retour testé

Browse Source
This commit is contained in:
Samuel 2023-05-13 23:03:23 +02:00
parent 374da66a5e
commit 10a0013a77
9 changed files with 121 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
Asser_Moteurs.c
View File

@ -60,6 +60,19 @@ float AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms){
return distance / temps;
}
/// @brief Indique si le robot est à l'arrêt
/// @param step_ms : pas de temps (utilisé pour déterminer les vitesses)
/// @return 1 si le robot est immobile, 0 s'il est en mouvement.
uint32_t AsserMoteur_RobotImmobile(int step_ms){
const float seuil_vitesse_immobile_mm_s = 0.1;
if(AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms) < seuil_vitesse_immobile_mm_s &&
AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) < seuil_vitesse_immobile_mm_s &&
AsserMoteur_getVitesse_mm_s(MOTEUR_C, step_ms) < seuil_vitesse_immobile_mm_s ){
return 1;
}
return 0;
}
/// @brief Fonction d'asservissement des moteurs, à appeler périodiquement
/// @param step_ms
void AsserMoteur_Gestion(int step_ms){

1
Asser_Moteurs.h
View File

@ -1,5 +1,6 @@
#include "Moteurs.h"
uint32_t AsserMoteur_RobotImmobile(int step_ms);
void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, float consigne_mm_s);
float AsserMoteur_getConsigne_mm_s(enum t_moteur moteur);
float AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms);

13
Evitement.c
View File

@ -1,4 +1,5 @@
#include "pico/stdlib.h"
#include "Asser_Moteurs.h"
#include "Evitement.h"
#include "Trajet.h"
@ -7,25 +8,25 @@
enum evitement_statu_t evitement_statu=PAS_D_OBSTACLE;
void Evitement_gestion(){
void Evitement_gestion(int step_ms){
static uint32_t temps_obstacle;
switch(evitement_statu){
case PAS_D_OBSTACLE:
if(Trajet_get_bloque() == 1){
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 = ARRET_DEVANT_OBSTACLE;
evitement_statu = OBSTACLE_CONFIRME;
}
if(Trajet_get_bloque() == 1){
if(!Trajet_get_bloque()){
evitement_statu = PAS_D_OBSTACLE;
}
break;
case ARRET_DEVANT_OBSTACLE:
if(Trajet_get_bloque() == 1){
case OBSTACLE_CONFIRME:
if(!Trajet_get_bloque()){
evitement_statu = PAS_D_OBSTACLE;
}
break;

5
Evitement.h
View File

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

2
Holonome2023.c
View File

@ -13,6 +13,7 @@
#include "Asser_Position.h"
#include "Balise_VL53L1X.h"
#include "Commande_vitesse.h"
#include "Evitement.h"
#include "i2c_annexe.h"
#include "i2c_maitre.h"
#include "Localisation.h"
@ -122,6 +123,7 @@ int main() {
QEI_update();
Localisation_gestion();
AsserMoteur_Gestion(_step_ms);
Evitement_gestion(_step_ms);
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){

9
Strategie.h
View File

@ -44,6 +44,14 @@ extern enum etat_homologation_t{
}etat_homologation;
enum evitement_t{
SANS_EVITEMENT,
PAUSE_DEVANT_OBSTACLE,
ARRET_DEVANT_OBSTACLE,
RETOUR_SI_OBSTABLE,
CONTOURNEMENT
};
enum etat_action_t cerise_accostage(void);
enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_direction);
enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms);
@ -58,6 +66,7 @@ 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);
extern float distance_obstacle;

57
Strategie_deplacement.c
View File

@ -4,7 +4,62 @@
#include "Geometrie.h"
#include "Balise_VL53L1X.h"
enum etat_action_t Strategie_parcourir_trajet(){
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 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 != SANS_EVITEMENT){
angle_avancement = Trajet_get_orientation_avance();
distance_obstacle = Balise_VL53L1X_get_distance_obstacle_mm(angle_avancement);
Trajet_set_obstacle_mm(distance_obstacle);
}
if(Evitement_get_statu() == ARRET_DEVANT_OBSTACLE){
switch(evitement){
case SANS_EVITEMENT:
//printf("Evitement lors trajet SANS_EVITEMENT: ERREUR\n");
break;
case PAUSE_DEVANT_OBSTACLE:
// Rien à faire ici
break;
case ARRET_DEVANT_OBSTACLE:
etat_parcourt = PARCOURS_INIT;
return ACTION_TERMINEE;
case RETOUR_SI_OBSTABLE:
Trajet_inverse();
break;
case CONTOURNEMENT: // TODO
break;
}
}
etat_trajet = Trajet_avance(step_ms/1000.);
if(etat_trajet == TRAJET_TERMINE){
etat_action = ACTION_TERMINEE;
etat_parcourt = PARCOURS_INIT;
}
break;
}
return etat_action;
}

31
Test_strategie.c
View File

@ -5,6 +5,7 @@
#include "Asser_Moteurs.h"
#include "Balise_VL53L1X.h"
#include "Evitement.h"
#include "i2c_annexe.h"
#include "i2c_maitre.h"
#include "gyro.h"
@ -297,8 +298,31 @@ void affichage_test_evitement(){
int test_evitement(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
struct trajectoire_t trajectoire;
enum evitement_t evitement;
enum etat_action_t etat_action;
printf("Evitement\n");
printf("A - Sans evitement.\n");
printf("B - Pause devant obstacle.\n");
printf("C - Arrêt devant obstacle.\n");
printf("D - Retour si obstacle.\n");
printf("E - Contournement.\n");
do{
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
switch(lettre){
case 'a':
case 'A':evitement=SANS_EVITEMENT;break;
case 'b':
case 'B':evitement=PAUSE_DEVANT_OBSTACLE;break;
case 'c':
case 'C':evitement=ARRET_DEVANT_OBSTACLE;break;
case 'd':
case 'D':evitement=RETOUR_SI_OBSTABLE;break;
case 'e':
case 'E':evitement=CONTOURNEMENT;break;
}
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
@ -327,6 +351,7 @@ int test_evitement(){
QEI_update();
Localisation_gestion();
AsserMoteur_Gestion(_step_ms);
Evitement_gestion(_step_ms);
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
@ -339,14 +364,12 @@ int test_evitement(){
1000,200,
0, 0); // Angles
if(parcourt_trajet_simple(trajectoire, _step_ms) == ACTION_TERMINEE){
}
etat_action = Strategie_parcourir_trajet(trajectoire, _step_ms, evitement);
}
lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(1);
}while(etat_action == ACTION_EN_COURS);
printf("STRATEGIE_LOOP_2\n");
printf("Lettre : %d; %c\n", lettre, lettre);

1
Trajet.h
View File

@ -26,3 +26,4 @@ void Trajet_stop(float);
float Trajet_get_orientation_avance(void);
float Trajet_get_abscisse();
uint32_t Trajet_get_bloque();
void Trajet_inverse();