diff --git a/Cerveau/Cerveau.ino b/Cerveau/Cerveau.ino index 026e183..b9f4362 100644 --- a/Cerveau/Cerveau.ino +++ b/Cerveau/Cerveau.ino @@ -624,10 +624,12 @@ enum etat_action_t gradin_approche(void){ // 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; + /// Attention, réflection dans le repère des capteurs + translation_x = (detect_gradin.centre_y_mm + 75) * sin(detect_gradin.angle_rad) * cos(detect_gradin.angle_rad) + detect_gradin.centre_x_mm; + translation_y = (detect_gradin.centre_y_mm + 75) * sin(detect_gradin.angle_rad) * sin(detect_gradin.angle_rad) ; - while(deplacement_relatif(translation_x, translation_y, 0, 0) == ACTION_TERMINEE); + // Changement de repère : capteurs => Chassis + while(deplacement_relatif(translation_y, -translation_x, 0, 0) != ACTION_TERMINEE); statu_approche_gradin = GA_TOURNE_LARGE; angle_mem = detect_gradin.angle_rad; @@ -636,6 +638,10 @@ enum etat_action_t gradin_approche(void){ case GA_TOURNE_LARGE: while(deplacement_relatif(0, 0, -angle_mem, 0) != ACTION_TERMINEE); + /*//// !!! DEBUG + statu_approche_gradin = GA_INIT; + return ACTION_ECHEC; + /// FIN DEBUG*/ delay(150); Detect_gradin(&detect_gradin); if(detect_gradin.status != 2){