Gestion de l'arrêt sur obstable fonctionnel - angle plus souple
This commit is contained in:
parent
896df5c089
commit
d7e13c8576
@ -835,8 +835,47 @@ int detection_adversaire(float angle_deplacement){
|
||||
return 0;
|
||||
}
|
||||
|
||||
#define SEUIL_OBSTACLE_CM 50
|
||||
|
||||
/// @brief A partir des déplacements X et Y, selection des capteurs à utiliser pour détecter l'adversaire
|
||||
/// @return 1 obstable, 0 pas d'obstacle
|
||||
int presence_obstacle(int distance_x_mm, int distance_y_mm){
|
||||
struct detect_adv_reception_t detect_adv_reception;
|
||||
Detect_adv_lire(&detect_adv_reception);
|
||||
float angle = atan2f(distance_y_mm, distance_x_mm);
|
||||
if(angle < 0){
|
||||
angle += 2 * M_PI;
|
||||
}
|
||||
// Angle entre 0 et 2 PI
|
||||
int segment_central = 23 - angle / 2. / M_PI * 24;
|
||||
// segment_central entre 0 et 23;
|
||||
if(segment_central == 23){
|
||||
segment_central = 0;
|
||||
}
|
||||
// segment_central entre 0 et 22;
|
||||
int capteur_central = segment_central / 2;
|
||||
int capteur_suivant = capteur_central + 1;
|
||||
int capteur_precedent = capteur_central - 1;
|
||||
|
||||
if(capteur_suivant > 11){
|
||||
capteur_suivant = 0;
|
||||
}
|
||||
if(capteur_precedent < 0){
|
||||
capteur_precedent = 11;
|
||||
}
|
||||
|
||||
if(detect_adv_reception.distance_cm[capteur_precedent] < SEUIL_OBSTACLE_CM ||
|
||||
detect_adv_reception.distance_cm[capteur_central] < SEUIL_OBSTACLE_CM ||
|
||||
detect_adv_reception.distance_cm[capteur_suivant] < SEUIL_OBSTACLE_CM ){
|
||||
// Arrêt du mouvement
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
/// @brief Deplacement dans le repère du robot, pouvant prendre en compte la detection de l'adversaire
|
||||
/// evitement : 1 pour s'arreter si adversaire detecté, 0 pour ignorer l'adversaire
|
||||
/// @param evitement : 1 pour s'arreter si adversaire detecté, 0 pour ignorer l'adversaire
|
||||
enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, float rotation_rad, int evitement){
|
||||
static enum{
|
||||
DR_INIT,
|
||||
@ -845,7 +884,6 @@ enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, flo
|
||||
|
||||
struct chassis_emission_t chassis_emission;
|
||||
struct chassis_reception_t chassis_reception;
|
||||
struct detect_adv_reception_t detect_adv_reception;
|
||||
|
||||
switch(etat_deplacement){
|
||||
case DR_INIT:
|
||||
@ -863,25 +901,11 @@ enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, flo
|
||||
|
||||
case DR_MVT_EN_COUR:
|
||||
if(evitement){
|
||||
// On lit les capteurs
|
||||
Detect_adv_lire(&detect_adv_reception);
|
||||
// On analyse les valeurs - TODO : créer une fonction plus évoluée
|
||||
if(distance_x_mm > 0){
|
||||
if(detect_adv_reception.distance_cm[0] < 50){
|
||||
// Arrêt du mouvement
|
||||
chassis_emission.status = MOUVEMENT_INTERRUPTION;
|
||||
send_Chassis(&chassis_emission);
|
||||
etat_deplacement = DR_INIT;
|
||||
return ACTION_ECHEC;
|
||||
}
|
||||
}else if(distance_x_mm < 0){
|
||||
if(detect_adv_reception.distance_cm[6] < 50){
|
||||
// Arrêt du mouvement
|
||||
chassis_emission.status = MOUVEMENT_INTERRUPTION;
|
||||
send_Chassis(&chassis_emission);
|
||||
etat_deplacement = DR_INIT;
|
||||
return ACTION_ECHEC;
|
||||
}
|
||||
if(presence_obstacle(distance_x_mm, distance_y_mm)){
|
||||
chassis_emission.status = MOUVEMENT_INTERRUPTION;
|
||||
send_Chassis(&chassis_emission);
|
||||
etat_deplacement = DR_INIT;
|
||||
return ACTION_ECHEC;
|
||||
}
|
||||
}
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user