Keuronde/Robot2025
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Gestion de l'arrêt sur obstable fonctionnel - angle plus souple

Browse Source
This commit is contained in:
Samuel 2025-05-26 20:51:08 +02:00
parent 896df5c089
commit d7e13c8576
1 changed files with 45 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
Cerveau/Cerveau.ino
View File

@ -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;
}
}