70 lines
2.0 KiB
C
70 lines
2.0 KiB
C
|
#include "Strategie.h"
|
||
|
|
#include "Trajet.h"
|
||
|
|
#include "Evitement.h"
|
||
|
|
#include "Geometrie.h"
|
||
|
|
#include "Balise_VL53L1X.h"
|
||
|
|
|
||
|
|
enum etat_action_t Strategie_parcourir_trajet(){
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
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;
|
||
|
|
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:
|
||
|
|
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;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
return etat_action;
|
||
|
|
}
|
||
|
|
|
||
|
|
enum etat_action_t parcourt_trajet_simple_sans_evitement(struct trajectoire_t trajectoire, uint32_t step_ms){
|
||
|
|
enum etat_action_t etat_action = ACTION_EN_COURS;
|
||
|
|
enum etat_trajet_t etat_trajet;
|
||
|
|
static enum {
|
||
|
|
PARCOURS_INIT,
|
||
|
|
PARCOURS_AVANCE,
|
||
|
|
} etat_parcourt=PARCOURS_INIT;
|
||
|
|
|
||
|
|
switch (etat_parcourt){
|
||
|
|
case PARCOURS_INIT:
|
||
|
|
Trajet_debut_trajectoire(trajectoire);
|
||
|
|
Trajet_set_obstacle_mm(distance_pas_obstacle);
|
||
|
|
etat_parcourt = PARCOURS_AVANCE;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PARCOURS_AVANCE:
|
||
|
|
etat_trajet = Trajet_avance(step_ms/1000.);
|
||
|
|
if(etat_trajet == TRAJET_TERMINE){
|
||
|
|
etat_action = ACTION_TERMINEE;
|
||
|
|
etat_parcourt = PARCOURS_INIT;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
return etat_action;
|
||
|
|
}
|