summaryrefslogtreecommitdiff
path: root/n
diff options
context:
space:
mode:
Diffstat (limited to 'n')
-rw-r--r--n/asserv/src/asserv/main.c9
-rw-r--r--n/asserv/src/asserv/traj.c38
2 files changed, 47 insertions, 0 deletions
diff --git a/n/asserv/src/asserv/main.c b/n/asserv/src/asserv/main.c
index 943c2e8..f6a30ce 100644
--- a/n/asserv/src/asserv/main.c
+++ b/n/asserv/src/asserv/main.c
@@ -285,6 +285,15 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
traj_holes_filter = 0;
main_sequence = args[0];
break;
+ case c ('f', 1):
+ /* Fuck the wall.
+ * - b: sequence number. */
+ if (args[0] == main_sequence)
+ break;
+ main_mode = 3;
+ traj_mode = 20;
+ main_sequence = args[0];
+ break;
case c ('a', 1):
/* Set acknoledge.
* - b: ack sequence number. */
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;
}
}