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

Amélioration de l'algo d'approche des gradins. Il met entre 8 et 20 secondes pour s'approcher d'un gradin

Browse Source
This commit is contained in:
Samuel 2025-04-28 23:27:07 +02:00
parent 802205220a
commit fc4e40da02
3 changed files with 58 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
Cerveau/Cerveau.ino
View File

@ -25,6 +25,9 @@
#define COULEUR_BLEU 0
#define COULEUR_JAUNE 1
#define GRADIN_PRECISION_ANGLE_RAD 0.009
#define GRADIN_PRECISION_X_MM 10
#define gst_server;
extern const char *ssid;
@ -507,7 +510,8 @@ enum etat_action_t gradin_approche(void){
GA_GOTO_EN_FACE,
GA_TOURNE_LARGE,
GA_GOTO_PROCHE,
GA_GOTO_PREND
GA_GOTO_PREND,
GA_RECULE
} statu_approche_gradin = GA_INIT;
static float angle_parcouru, angle_mem;
static int nb_erreur;
@ -515,6 +519,7 @@ enum etat_action_t gradin_approche(void){
float distance;
struct detect_gradin_t detect_gradin;
delay(150);
Detect_gradin(&detect_gradin);
char chaine[200];
sprintf(chaine, "I2C OK\nStatus:%d\nCentre X:%4d\nCentre Y:%4d\nAngle:%2.2f\n", detect_gradin.status,
@ -533,13 +538,18 @@ enum etat_action_t gradin_approche(void){
statu_approche_gradin = GA_GOTO_LARGE;
angle_mem = detect_gradin.angle_rad;
nb_erreur = 0;
}else if(detect_gradin.centre_y_mm < 180){
statu_approche_gradin = GA_RECULE;
}else if(detect_gradin.status == 3){
// On a un grain sur la droite
statu_approche_gradin = GA_CHERCHE_DROIT;
}else if(detect_gradin.status == 0){
// On a perdu la détection
while(deplacement_relatif(0, 0, -3. * M_PI / 180., 0) == ACTION_EN_COURS);
while(deplacement_relatif(0, 0, 3. * M_PI / 180., 0) == ACTION_EN_COURS);
statu_approche_gradin = GA_CHERCHE_DROIT;
}else{
// On tourne à gauche de quelques degrés
while(deplacement_relatif(0, 0, 3. * M_PI / 180., 0) == ACTION_EN_COURS);
while(deplacement_relatif(0, 0, -3. * M_PI / 180., 0) == ACTION_EN_COURS);
}
break;
@ -549,13 +559,18 @@ enum etat_action_t gradin_approche(void){
statu_approche_gradin = GA_GOTO_LARGE;
angle_mem = detect_gradin.angle_rad;
nb_erreur = 0;
}else if(detect_gradin.centre_y_mm < 180){
statu_approche_gradin = GA_RECULE;
}else if(detect_gradin.status == 4){
// On a un grain sur la gauche
statu_approche_gradin = GA_CHERCHE_GAUCHE;
}else if(detect_gradin.status == 0){
// On a perdu la détection
statu_approche_gradin = GA_INIT;
return ACTION_ECHEC;
}else{
// On tourne à gauche de quelques degrés
while(deplacement_relatif(0, 0, -3. * M_PI / 180., 0) == ACTION_EN_COURS);
while(deplacement_relatif(0, 0, 3. * M_PI / 180., 0) == ACTION_EN_COURS);
}
break;
@ -568,45 +583,68 @@ enum etat_action_t gradin_approche(void){
affiche_erreur("Gradin Approche", "GA_GOTO_LARGE\n Status != 2");
while(1);
}
}
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;
}else if(detect_gradin.centre_y_mm < 180){
// On est trop près, on ne détectera pas le gradin si on est devant
statu_approche_gradin = GA_RECULE;
}else{
// On se centre par rapport au gradin
//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_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;
if(deplacement_relatif(translation_x, translation_y, 0, 0) == ACTION_TERMINEE){
while(deplacement_relatif(translation_x, translation_y, 0, 0) == ACTION_TERMINEE);
statu_approche_gradin = GA_TOURNE_LARGE;
angle_mem = detect_gradin.angle_rad;
}
break;
case GA_TOURNE_LARGE:
if(deplacement_relatif(0, 0, -angle_mem, 0) == ACTION_TERMINEE){
Detect_gradin(&detect_gradin);
if(detect_gradin.status != 2 || fabs(detect_gradin.angle_rad) > 0.009){
statu_approche_gradin = GA_CHERCHE_GAUCHE;
while(deplacement_relatif(0, 0, -angle_mem, 0) != ACTION_TERMINEE);
delay(150);
Detect_gradin(&detect_gradin);
if(detect_gradin.status != 2){
statu_approche_gradin = GA_CHERCHE_GAUCHE;
}else if(detect_gradin.centre_y_mm < 180){
statu_approche_gradin = GA_RECULE;
}else if(fabs(detect_gradin.angle_rad) > GRADIN_PRECISION_ANGLE_RAD){
angle_mem = detect_gradin.angle_rad;
}else{
if(detect_gradin.centre_y_mm > 240){
statu_approche_gradin = GA_GOTO_PROCHE;
}else{
if(detect_gradin.centre_y_mm > 220){
statu_approche_gradin = GA_GOTO_PROCHE;
if(fabs(detect_gradin.centre_x_mm) > 10){
statu_approche_gradin = GA_GOTO_LARGE;
}else{
statu_approche_gradin = GA_GOTO_PREND;
}
}
}
break;
case GA_GOTO_PROCHE:
Detect_gradin(&detect_gradin);
if(deplacement_relatif((detect_gradin.centre_y_mm - 180), 0, 0, 0) == ACTION_TERMINEE){
if(deplacement_relatif((detect_gradin.centre_y_mm - 200), 0, 0, 0) == ACTION_TERMINEE){
statu_approche_gradin = GA_GOTO_EN_FACE;
}
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;
}
while(deplacement_relatif((detect_gradin.centre_y_mm - 25), 0, 0, 0) != ACTION_TERMINEE);
statu_approche_gradin = GA_INIT;
delay(5000);
return ACTION_TERMINEE;
break;
case GA_RECULE:
while(deplacement_relatif((detect_gradin.centre_y_mm - 220), 0, 0, 0) != ACTION_TERMINEE);
statu_approche_gradin = GA_CHERCHE_GAUCHE;
break;
}
return ACTION_EN_COURS;

BIN
Doc/Communication I2C.odt
View File

Binary file not shown.

BIN
Doc/Communication I2C.pdf
View File

Binary file not shown.