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.c50
1 files changed, 41 insertions, 9 deletions
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;
}