From 56e637ec77fc6c220009663dcc35712e399bfd5d Mon Sep 17 00:00:00 2001 From: dufourj Date: Fri, 26 May 2006 08:12:24 +0000 Subject: Général : - commit du programme d'après premier match. --- n/asserv/src/asserv/traj.c | 78 +++++++++++++++++++++++++++++----------------- 1 file changed, 50 insertions(+), 28 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 eb0ef92..d60f7c9 100644 --- a/n/asserv/src/asserv/traj.c +++ b/n/asserv/src/asserv/traj.c @@ -31,6 +31,9 @@ uint8_t traj_mode; /** Holes filter. */ uint8_t traj_holes_filter; +/** Holes ok filter. */ +uint8_t traj_holes_ok; + /* 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 @@ -73,6 +76,7 @@ traj_holes (void) { traj_holes_filter = 255; traj_mode = 12; + traj_holes_ok = 0; } } else if (traj_mode == 12) @@ -84,39 +88,57 @@ traj_holes (void) traj_mode = 11; } /* Determine speed. */ - speed = traj_mode <= 11 ? speed_theta_max : speed_theta_slow; - speed *= 256; - if ((in & 0xf) == 0xc) - { - /* Stop. */ - speed_theta_cons = 0; - speed_alpha_cons = 0; - main_sequence_finish = main_sequence; - traj_mode = 13; - } - else if (in & 1) - { - /* Left. */ - speed_theta_cons = 0; - speed_alpha_cons = speed; - } - else if (in & 2) - { - /* Right. */ - speed_theta_cons = 0; - speed_alpha_cons = -speed; - } - else if (in & 8) + if (traj_mode <= 11) { - /* Back. */ - speed_theta_cons = -speed; + /* Cruise mode. */ + speed = speed_theta_max; + speed *= 256; + speed_theta_cons = speed; speed_alpha_cons = 0; } else { - /* Front. */ - speed_theta_cons = speed; - speed_alpha_cons = 0; + speed = speed_theta_slow; + speed *= 256; + if ((in & 0xf) == 0xc || (in & 0x7) == 0x7) + { + traj_holes_ok++; + if (traj_holes_ok >= 32) + { + /* Stop. */ + speed_theta_cons = 0; + speed_alpha_cons = 0; + main_sequence_finish = main_sequence; + traj_mode = 13; + } + } + else + { + if (in & 1) + { + /* Left. */ + speed_theta_cons = 0; + speed_alpha_cons = speed; + } + else if (in & 2) + { + /* Right. */ + speed_theta_cons = 0; + speed_alpha_cons = -speed; + } + 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