Amélioration de l'approche des gradins - correction d'un vilain bug
This commit is contained in:
parent
d7e13c8576
commit
8bdffe821d
@ -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){
|
||||
|
Loading…
Reference in New Issue
Block a user