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

Amélioration de l'approche des gradins, une étapes de recalage à 180 mm

Browse Source
This commit is contained in:
Samuel 2025-04-24 21:24:40 +02:00
parent 2513404cfd
commit 802205220a
1 changed files with 19 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
Cerveau/Cerveau.ino
View File

@ -504,6 +504,7 @@ enum etat_action_t gradin_approche(void){
GA_CHERCHE_GAUCHE, GA_CHERCHE_GAUCHE,
GA_CHERCHE_DROIT, GA_CHERCHE_DROIT,
GA_GOTO_LARGE, GA_GOTO_LARGE,
GA_GOTO_EN_FACE,
GA_TOURNE_LARGE, GA_TOURNE_LARGE,
GA_GOTO_PROCHE, GA_GOTO_PROCHE,
GA_GOTO_PREND GA_GOTO_PREND
@ -511,6 +512,7 @@ enum etat_action_t gradin_approche(void){
static float angle_parcouru, angle_mem; static float angle_parcouru, angle_mem;
static int nb_erreur; static int nb_erreur;
int translation_x, translation_y; int translation_x, translation_y;
float distance;
struct detect_gradin_t detect_gradin; struct detect_gradin_t detect_gradin;
Detect_gradin(&detect_gradin); Detect_gradin(&detect_gradin);
@ -557,6 +559,7 @@ enum etat_action_t gradin_approche(void){
} }
break; break;
case GA_GOTO_EN_FACE:
case GA_GOTO_LARGE: case GA_GOTO_LARGE:
Detect_gradin(&detect_gradin); Detect_gradin(&detect_gradin);
if(detect_gradin.status != 2){ if(detect_gradin.status != 2){
@ -566,7 +569,7 @@ enum etat_action_t gradin_approche(void){
while(1); while(1);
} }
} }
translation_x = (detect_gradin.centre_y_mm + 150) * tan(detect_gradin.angle_rad); translation_x = (detect_gradin.centre_y_mm + 150) * tan(fabs(detect_gradin.angle_rad));
translation_y = -(detect_gradin.centre_y_mm + 150) * sin(detect_gradin.angle_rad) - detect_gradin.centre_x_mm; translation_y = -(detect_gradin.centre_y_mm + 150) * sin(detect_gradin.angle_rad) - detect_gradin.centre_x_mm;
if(deplacement_relatif(translation_x, translation_y, 0, 0) == ACTION_TERMINEE){ if(deplacement_relatif(translation_x, translation_y, 0, 0) == ACTION_TERMINEE){
@ -580,7 +583,11 @@ enum etat_action_t gradin_approche(void){
if(detect_gradin.status != 2 || fabs(detect_gradin.angle_rad) > 0.009){ if(detect_gradin.status != 2 || fabs(detect_gradin.angle_rad) > 0.009){
statu_approche_gradin = GA_CHERCHE_GAUCHE; statu_approche_gradin = GA_CHERCHE_GAUCHE;
}else{ }else{
statu_approche_gradin = GA_GOTO_PROCHE; if(detect_gradin.centre_y_mm > 220){
statu_approche_gradin = GA_GOTO_PROCHE;
}else{
statu_approche_gradin = GA_GOTO_PREND;
}
} }
} }
@ -588,12 +595,19 @@ enum etat_action_t gradin_approche(void){
case GA_GOTO_PROCHE: case GA_GOTO_PROCHE:
Detect_gradin(&detect_gradin); Detect_gradin(&detect_gradin);
if(deplacement_relatif((detect_gradin.centre_y_mm - 20), 0, 0, 0) == ACTION_TERMINEE){ if(deplacement_relatif((detect_gradin.centre_y_mm - 180), 0, 0, 0) == ACTION_TERMINEE){
statu_approche_gradin = GA_INIT; statu_approche_gradin = GA_GOTO_EN_FACE;
return ACTION_TERMINEE;
} }
break; break;
case GA_GOTO_PREND:
if(deplacement_relatif((detect_gradin.centre_y_mm - 25), 0, 0, 0) == ACTION_TERMINEE){
statu_approche_gradin = GA_INIT;
return ACTION_TERMINEE;
}
break;
} }
return ACTION_EN_COURS; return ACTION_EN_COURS;