From 7b8e674b1e17c3314a7e353c9666bea918455a29 Mon Sep 17 00:00:00 2001 From: schodet Date: Fri, 11 May 2007 21:59:28 +0000 Subject: Ajout du FTW (CQP, je ne voudrais pas ĂȘtre vulgaire dans un commit log). --- n/asserv/src/asserv/main.c | 9 +++++++++ n/asserv/src/asserv/traj.c | 38 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 47 insertions(+) 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; } } -- cgit v1.2.3