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 +++++++- digital/io/doc/proto_asserv.txt | 4 ++++ digital/io/src/asserv.c | 9 ++++++--- digital/io/src/top_cb.c | 2 +- 7 files changed, 61 insertions(+), 7 deletions(-) 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); diff --git a/digital/io/doc/proto_asserv.txt b/digital/io/doc/proto_asserv.txt index fe21bdad..9fcf9808 100644 --- a/digital/io/doc/proto_asserv.txt +++ b/digital/io/doc/proto_asserv.txt @@ -73,6 +73,10 @@ This table describe the list of supported commands by the *asserv* card: | 'f' | Move | - direction (b) | Go to the wall | | | | | - direction: 0 = forward, 1 = backward | +---------+-------+-----------------+------------------------------------------+ +| 'g' | Move | - direction (b) | Go to the wall, using center sensor | +| | | - delay (b) | - direction: 0 = forward, 1 = backward | +| | | | - delay: center sensor trigger delay | ++---------+-------+-----------------+------------------------------------------+ | 'F' | Move | None | Go to a distributor (forward movement) | +---------+-------+-----------------+------------------------------------------+ | 'x' | Move | - x (3b) | Go to an absolute position | diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c index 6f849054..0853a11b 100644 --- a/digital/io/src/asserv.c +++ b/digital/io/src/asserv.c @@ -503,7 +503,7 @@ asserv_goto_xya (uint32_t x, uint32_t y, int16_t a, uint8_t backward) asserv_twi_send_command ('X', 9); } -/* Go to the wall (moving backward). */ +/* Go to the wall. */ void asserv_go_to_the_wall (uint8_t backward) { @@ -517,8 +517,11 @@ asserv_go_to_the_wall (uint8_t backward) void asserv_go_to_distributor (void) { - /* Send the go the distributor command to the asserv board */ - asserv_twi_send_command ('F', 0); + /* Put direction and delay as parameters */ + asserv_twi_buffer_param[0] = 0; + asserv_twi_buffer_param[1] = 25; + /* Send the go the wall command to the asserv board */ + asserv_twi_send_command ('g', 2); } /* Move the arm. */ diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c index ce6d6db4..0530ae97 100644 --- a/digital/io/src/top_cb.c +++ b/digital/io/src/top_cb.c @@ -743,7 +743,7 @@ top__CLEAN_FRONT_OF_DISTRIBUTOR__move_fsm_succeed (void) /* Open cylinder. */ cylinder_close_order = 0; /* Fuck the distributor. */ - asserv_go_to_the_wall (0); + asserv_go_to_distributor (); return top_next (CLEAN_FRONT_OF_DISTRIBUTOR, move_fsm_succeed); } -- cgit v1.2.3