38 lines
1.1 KiB
C
38 lines
1.1 KiB
C
#include "pico/stdlib.h"
|
|
#include "Asser_Moteurs.h"
|
|
#include "Evitement.h"
|
|
#include "Trajet.h"
|
|
|
|
// 250 ms
|
|
#define TEMPS_VALIDE_OBSTACLE_US 250000
|
|
|
|
enum evitement_statu_t evitement_statu=PAS_D_OBSTACLE;
|
|
|
|
void Evitement_gestion(int step_ms){
|
|
static uint32_t temps_obstacle;
|
|
switch(evitement_statu){
|
|
case PAS_D_OBSTACLE:
|
|
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 = OBSTACLE_CONFIRME;
|
|
}
|
|
if(!Trajet_get_bloque()){
|
|
evitement_statu = PAS_D_OBSTACLE;
|
|
}
|
|
break;
|
|
case OBSTACLE_CONFIRME:
|
|
if(!Trajet_get_bloque()){
|
|
evitement_statu = PAS_D_OBSTACLE;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
enum evitement_statu_t Evitement_get_statu(){
|
|
return evitement_statu;
|
|
} |