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, 38 insertions, 0 deletions
diff --git a/n/asserv/src/asserv/traj.c b/n/asserv/src/asserv/traj.c
index d60f7c9..ce08aeb 100644
--- a/n/asserv/src/asserv/traj.c
+++ b/n/asserv/src/asserv/traj.c
@@ -25,6 +25,7 @@
/** Traj mode:
* 10, 11, 12 and 13: find a hole.
+ * 20, 21: we fuck the wall.
*/
uint8_t traj_mode;
@@ -142,6 +143,39 @@ traj_holes (void)
}
}
+/** We fuck the wall mode. */
+void
+traj_ftw (void)
+{
+ uint16_t speed;
+ speed = speed_theta_slow;
+ speed *= 256;
+ if (PINA & _BV (0) && PINA & _BV (7))
+ {
+ speed_theta_cons = -speed;
+ speed_alpha_cons = 0;
+ }
+ else if (PINA & _BV (0))
+ {
+ speed_theta_cons = -speed / 2;
+ speed_alpha_cons = -speed / 2;
+ }
+ else if (PINA & _BV (7))
+ {
+ speed_theta_cons = -speed / 2;
+ speed_alpha_cons = speed / 2;
+ }
+ else
+ {
+ speed_theta_cons = 0;
+ speed_alpha_cons = 0;
+ speed_theta_cur = 0;
+ speed_alpha_cur = 0;
+ main_sequence_finish = main_sequence;
+ traj_mode = 21;
+ }
+}
+
/* Compute new speed according the defined trajectory. */
static void
traj_update (void)
@@ -154,6 +188,10 @@ traj_update (void)
traj_holes ();
case 13:
break;
+ case 20:
+ traj_ftw ();
+ case 21:
+ break;
}
}