Amélioration de l'approche des gradins - correction d'un vilain bug

This commit is contained in:
Samuel 2025-05-26 23:03:54 +02:00
parent d7e13c8576
commit 8bdffe821d

View File

@ -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){