diff --git a/Cerveau/Cerveau.ino b/Cerveau/Cerveau.ino index 39715b9..a647d31 100644 --- a/Cerveau/Cerveau.ino +++ b/Cerveau/Cerveau.ino @@ -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,44 +583,67 @@ 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; + + } diff --git a/Doc/Communication I2C.odt b/Doc/Communication I2C.odt index b78cc56..1e3aea1 100644 Binary files a/Doc/Communication I2C.odt and b/Doc/Communication I2C.odt differ diff --git a/Doc/Communication I2C.pdf b/Doc/Communication I2C.pdf index 98843df..81a483a 100644 Binary files a/Doc/Communication I2C.pdf and b/Doc/Communication I2C.pdf differ