From 3137a08b306026315565a68bde9e18e00bf454bd Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 21 May 2006 16:59:23 +0000 Subject: Attend de sortir d'un trou avant de tenter d'en trouver. --- n/asserv/src/asserv/traj.c | 38 +++++++++++++++++++++++++++----------- 1 file changed, 27 insertions(+), 11 deletions(-) diff --git a/n/asserv/src/asserv/traj.c b/n/asserv/src/asserv/traj.c index 7c4ba65..eb0ef92 100644 --- a/n/asserv/src/asserv/traj.c +++ b/n/asserv/src/asserv/traj.c @@ -24,7 +24,7 @@ * }}} */ /** Traj mode: - * 10, 11 and 12: find a hole. + * 10, 11, 12 and 13: find a hole. */ uint8_t traj_mode; @@ -35,10 +35,11 @@ uint8_t traj_holes_filter; * French: On avance, et si on voit un "pas vert" à gauche, on tourne à * gauche. Pareil pour la droite. Au final, on centre le "pas vert" entre deux * capteurs, c'est le trou. Il y a aussi les capteurs à l'avant et à l'arrière. - * Le robot commence par avancer à vive allure jusqu'à trouver un trou avec le - * capteur avant, puis il ralenti et passe dans le mode ou il avance s'il voit - * du "pas vert" à l'avant et il recule s'il voit du "pas-vert" à l'arrière. - * Enfin, il s'arrète. + * Le robot commence par avancer à vive allure jusqu'à avoir tous les capteurs + * verts (afin de ne pas trouver le trou sur lequel on comence). Puis il + * continue jusqu'à trouver un trou avec l'un des capteurs, puis il ralenti et + * passe dans le mode ou il avance s'il voit du "pas vert" à l'avant et il + * recule s'il voit du "pas-vert" à l'arrière. Enfin, il s'arrète. */ static void traj_holes (void) @@ -50,26 +51,40 @@ traj_holes (void) /* Change state if needed. */ if (traj_mode == 10) { - if (in & 0xf) + /* Cruise mode until not over a hole. */ + if ((in & 0xf) == 0) traj_holes_filter++; else traj_holes_filter = 0; if (traj_holes_filter >= 8) { - traj_holes_filter = 255; + traj_holes_filter = 0; traj_mode = 11; } } else if (traj_mode == 11) + { + /* Cruise mode until over a hole. */ + if (in & 0xf) + traj_holes_filter++; + else + traj_holes_filter = 0; + if (traj_holes_filter >= 8) + { + traj_holes_filter = 255; + traj_mode = 12; + } + } + else if (traj_mode == 12) { /* Return to cruise mode if no hole for too long. */ if (!(in & 0xf)) traj_holes_filter--; if (traj_holes_filter == 0) - traj_mode = 10; + traj_mode = 11; } /* Determine speed. */ - speed = traj_mode == 10 ? speed_theta_max : speed_theta_slow; + speed = traj_mode <= 11 ? speed_theta_max : speed_theta_slow; speed *= 256; if ((in & 0xf) == 0xc) { @@ -77,7 +92,7 @@ traj_holes (void) speed_theta_cons = 0; speed_alpha_cons = 0; main_sequence_finish = main_sequence; - traj_mode = 12; + traj_mode = 13; } else if (in & 1) { @@ -113,8 +128,9 @@ traj_update (void) { case 10: case 11: - traj_holes (); case 12: + traj_holes (); + case 13: break; } } -- cgit v1.2.3