From bc5944190b7ae0af55caa84f86d63bf95e40712f Mon Sep 17 00:00:00 2001 From: schodet Date: Tue, 16 May 2006 19:35:51 +0000 Subject: Ajout de plein de trucs bien pour les trous. Ajout du système de ack. --- n/asserv/src/asserv/traj.c | 50 +++++++++++++++++++++++++++++++++++++--------- 1 file changed, 41 insertions(+), 9 deletions(-) (limited to 'n/asserv/src/asserv/traj.c') diff --git a/n/asserv/src/asserv/traj.c b/n/asserv/src/asserv/traj.c index 8f2413b..cfdbbc4 100644 --- a/n/asserv/src/asserv/traj.c +++ b/n/asserv/src/asserv/traj.c @@ -28,6 +28,9 @@ */ uint8_t traj_mode; +/** Holes filter. */ +uint8_t traj_holes_filter; + /* Find a hole in the table. * 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 @@ -41,33 +44,62 @@ static void traj_holes (void) { int16_t speed; + uint8_t in; speed_pos = 0; + in = PINF; /* Change state if needed. */ - if (traj_mode == 10 && (PINF & 0xf)) - traj_mode++; - else if (traj_mode == 11 && (PINF & 0xf) == 0) - traj_mode++; + if (traj_mode == 10) + { + if (in & 0xf) + traj_holes_filter++; + else + traj_holes_filter = 0; + if (traj_holes_filter >= 8) + { + traj_holes_filter = 255; + traj_mode = 11; + } + } + else if (traj_mode == 11) + { + /* Return to cruise mode if no hole for too long. */ + if (!(in & 0xf)) + traj_holes_filter--; + if (traj_holes_filter == 0) + traj_mode = 10; + } /* Determine speed. */ - speed = traj_mode == 10 ? speed_theta_max : speed_theta_max / 2; + speed = traj_mode == 10 ? speed_theta_max : speed_theta_slow; speed *= 256; - /* Left or right. */ - if (PINF & 1) + if ((in & 0xf) == 0xc) + { + /* Stop. */ + speed_theta_cons = 0; + speed_alpha_cons = 0; + main_sequence_ack = main_sequence; + traj_mode = 12; + } + else if (in & 1) { + /* Left. */ speed_theta_cons = speed / 2; speed_alpha_cons = speed / 2; } - else if (PINF & 2) + else if (in & 2) { + /* Right. */ speed_theta_cons = speed / 2; speed_alpha_cons = -speed / 2; } - else if (PINF & 8) + else if (in & 8) { + /* Back. */ speed_theta_cons = -speed; speed_alpha_cons = 0; } else { + /* Front. */ speed_theta_cons = speed; speed_alpha_cons = 0; } -- cgit v1.2.3