108 lines
3.4 KiB
C
108 lines
3.4 KiB
C
#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;
|
|
|
|
}
|
|
|
|
enum etat_action_t Strategie_recale(int sens_avant, uint32_t step_ms){
|
|
const float vitesse_recale = 200;
|
|
static int timer_recale_ms;
|
|
static enum {
|
|
RECALE_INIT,
|
|
RECALE_AVANCE,
|
|
} etat_recale=RECALE_INIT;
|
|
|
|
switch(etat_recale){
|
|
case RECALE_INIT:
|
|
if(sens_avant){
|
|
AsserMoteur_setConsigne_mm_s(MOTEUR_A, vitesse_recale);
|
|
AsserMoteur_setConsigne_mm_s(MOTEUR_B, vitesse_recale);
|
|
}else{
|
|
AsserMoteur_setConsigne_mm_s(MOTEUR_A, -vitesse_recale);
|
|
AsserMoteur_setConsigne_mm_s(MOTEUR_B, -vitesse_recale);
|
|
}
|
|
timer_recale_ms = 800;
|
|
etat_recale=RECALE_AVANCE;
|
|
break;
|
|
|
|
case RECALE_AVANCE:
|
|
timer_recale_ms -= step_ms;
|
|
if(timer_recale_ms <= 0){
|
|
AsserMoteur_setConsigne_mm_s(MOTEUR_A, 0);
|
|
AsserMoteur_setConsigne_mm_s(MOTEUR_B, 0);
|
|
etat_recale=RECALE_INIT;
|
|
return ACTION_TERMINEE;
|
|
}
|
|
break;
|
|
}
|
|
return ACTION_EN_COURS;
|
|
|
|
} |