summaryrefslogtreecommitdiff
path: root/n/asserv/src/asserv/traj.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src/asserv/traj.c')
-rw-r--r--n/asserv/src/asserv/traj.c38
1 files 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;
}
}