From d6647a90f4bfe59968cf0f94443335f2673a32f3 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 23 May 2009 00:09:23 +0200 Subject: * digital/asserv, digital/io: - added usage of center sensor. --- digital/asserv/src/asserv/main.c | 9 +++++++++ digital/asserv/src/asserv/traj.c | 33 +++++++++++++++++++++++++++++++-- digital/asserv/src/asserv/traj.h | 3 +++ digital/asserv/src/asserv/twi_proto.c | 8 +++++++- 4 files changed, 50 insertions(+), 3 deletions(-) (limited to 'digital/asserv') diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index b53e6d18..b980899e 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -391,6 +391,15 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; traj_ftw_start (args[0], args[1]); break; + case c ('f', 3): + /* Go to the wall, using center with a delay. + * - b: 0: forward, 1: backward. + * - b: delay. + * - b: sequence number. */ + if (args[1] == state_main.sequence) + break; + traj_ftw_start_center (args[0], args[1], args[2]); + break; case c ('F', 1): /* Go to the dispenser. * - b: sequence number. */ diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c index a00a70c2..11618eca 100644 --- a/digital/asserv/src/asserv/traj.c +++ b/digital/asserv/src/asserv/traj.c @@ -93,6 +93,12 @@ static uint32_t traj_goto_x, traj_goto_y; /** Go to angle. */ static uint32_t traj_goto_a; +/** Use center sensor. */ +static uint8_t traj_use_center; + +/** Center sensor delay. */ +static uint8_t traj_center_delay; + /** Initialise computed factors. */ void traj_init (void) @@ -118,7 +124,7 @@ traj_angle_offset_start (int32_t angle, uint8_t seq) static void traj_ftw (void) { - uint8_t left, right; + uint8_t left, center, right; int16_t speed; speed = speed_theta.slow; speed *= 256; @@ -133,6 +139,17 @@ traj_ftw (void) left = !IO_GET (CONTACT_BACK_LEFT_IO); right = !IO_GET (CONTACT_BACK_RIGHT_IO); } + center = 0; + if (traj_use_center) + { + if (!IO_GET (CONTACT_CENTER_IO)) + { + if (traj_center_delay == 0) + center = 1; + else + traj_center_delay--; + } + } speed_theta.use_pos = speed_alpha.use_pos = 0; speed_theta.cons = speed; speed_alpha.cons = 0; @@ -145,7 +162,7 @@ traj_ftw (void) state_main.variant = 2; #endif } - else if (!left || !right) + else if (!center && (!left || !right)) { #ifndef HOST /* No angular control. */ @@ -178,6 +195,18 @@ traj_ftw_start (uint8_t backward, uint8_t seq) { traj_mode = TRAJ_FTW; traj_backward = backward; + traj_use_center = 0; + state_start (&state_main, MODE_TRAJ, seq); +} + +/** Start go to the wall mode, with center sensor. */ +void +traj_ftw_start_center (uint8_t backward, uint8_t center_delay, uint8_t seq) +{ + traj_mode = TRAJ_FTW; + traj_backward = backward; + traj_use_center = 1; + traj_center_delay = center_delay; state_start (&state_main, MODE_TRAJ, seq); } diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h index 8f9c28ac..706fbdaf 100644 --- a/digital/asserv/src/asserv/traj.h +++ b/digital/asserv/src/asserv/traj.h @@ -45,6 +45,9 @@ traj_angle_offset_start (int32_t angle, uint8_t seq); void traj_ftw_start (uint8_t backward, uint8_t seq); +void +traj_ftw_start_center (uint8_t backward, uint8_t center_delay, uint8_t seq); + void traj_gtd_start (uint8_t seq); diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index dac59f93..c4fa1f0d 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -163,11 +163,17 @@ twi_proto_callback (u8 *buf, u8 size) * - w: angle offset. */ traj_angle_offset_start (((int32_t) (int16_t) v8_to_v16 (buf[2], buf[3])) << 8, 0); break; - case c ('f', 0): + case c ('f', 1): /* Go to the wall. * - b: 0: forward, 1: backward. */ traj_ftw_start (buf[2], 0); break; + case c ('g', 2): + /* Go to the wall using center sensor with delay. + * - b: 0: forward, 1: backward. + * - b: delay. */ + traj_ftw_start_center (buf[2], buf[3], 0); + break; case c ('F', 0): /* Go to the dispenser. */ traj_gtd_start (0); -- cgit v1.2.3