summaryrefslogtreecommitdiff
path: root/n/asserv/src/asserv/traj.c
diff options
context:
space:
mode:
authordufourj2006-05-26 08:12:24 +0000
committerdufourj2006-05-26 08:12:24 +0000
commit56e637ec77fc6c220009663dcc35712e399bfd5d (patch)
treed3ebab75458fdce937169fcd2931283e0c1bd3ee /n/asserv/src/asserv/traj.c
parentdd7faf987331073d71248bd84f5a113ad9629a29 (diff)
Général :
- commit du programme d'après premier match.
Diffstat (limited to 'n/asserv/src/asserv/traj.c')
-rw-r--r--n/asserv/src/asserv/traj.c78
1 files changed, 50 insertions, 28 deletions
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;
+ }
+ }
}
}