From f753bc39682f980feb49b7ff1bb9e9594f986e6b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 19 May 2011 23:24:08 +0200 Subject: digital/asserv: add push the wall traj mode, closes #167 --- digital/ai/src/twi_master/asserv.c | 22 ++++++++++++++ digital/ai/src/twi_master/asserv.h | 6 ++++ digital/asserv/src/asserv/main.c | 21 +++++++++++++ digital/asserv/src/asserv/traj.c | 57 +++++++++++++++++++++++++++++++++++ digital/asserv/src/asserv/traj.h | 4 +++ digital/asserv/src/asserv/twi_proto.c | 18 +++++++++++ digital/asserv/tools/asserv/asserv.py | 19 ++++++++++-- 7 files changed, 145 insertions(+), 2 deletions(-) (limited to 'digital') diff --git a/digital/ai/src/twi_master/asserv.c b/digital/ai/src/twi_master/asserv.c index d9ae9330..839b1d23 100644 --- a/digital/ai/src/twi_master/asserv.c +++ b/digital/ai/src/twi_master/asserv.c @@ -300,6 +300,28 @@ asserv_go_to_the_wall (uint8_t backward) twi_master_send_buffer (2); } +void +asserv_push_the_wall (uint8_t backward, uint32_t init_x, uint32_t init_y, + uint16_t init_a) +{ + if (init_x != (uint32_t) -1) + init_x = fixed_mul_f824 (init_x, asserv_scale_inv); + if (init_y != (uint32_t) -1) + init_y = fixed_mul_f824 (init_y, asserv_scale_inv); + uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE); + buffer[0] = 'G'; + buffer[1] = backward; + buffer[2] = v32_to_v8 (init_x, 2); + buffer[3] = v32_to_v8 (init_x, 1); + buffer[4] = v32_to_v8 (init_x, 0); + buffer[5] = v32_to_v8 (init_y, 2); + buffer[6] = v32_to_v8 (init_y, 1); + buffer[7] = v32_to_v8 (init_y, 0); + buffer[8] = v16_to_v8 (init_a, 1); + buffer[9] = v16_to_v8 (init_a, 0); + twi_master_send_buffer (10); +} + void asserv_move_motor0_absolute (uint16_t position, uint8_t speed) { diff --git a/digital/ai/src/twi_master/asserv.h b/digital/ai/src/twi_master/asserv.h index 2aa7a6f8..c9a52a11 100644 --- a/digital/ai/src/twi_master/asserv.h +++ b/digital/ai/src/twi_master/asserv.h @@ -198,6 +198,12 @@ asserv_goto_xya (uint32_t x, uint32_t y, int16_t a, uint8_t backward); void asserv_go_to_the_wall (uint8_t backward); +/** Push the wall and initialise position. Use -1 for coordinates to keep + * unchanged. */ +void +asserv_push_the_wall (uint8_t backward, uint32_t init_x, uint32_t init_y, + uint16_t init_a); + /** * Move the motor0. * Motor0 class command. diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 2d53e2fa..677fea84 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -400,6 +400,27 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; traj_ftw_start_center (args[0], args[1], args[2]); break; + case c ('f', 12): + /* Push the wall. + * - b: 0: forward, 1: backward. + * - d: init_x, f24.8. + * - d: init_y, f24.8. + * - w: init_a, f0.16. + * - b: sequence number. */ + { + if (args[11] == state_main.sequence) + break; + int32_t angle; + if (args[9] == 0xff && args[10] == 0xff) + angle = -1; + else + angle = v8_to_v32 (0, args[9], args[10], 0); + traj_ptw_start (args[0], + v8_to_v32 (args[1], args[2], args[3], args[4]), + v8_to_v32 (args[5], args[6], args[7], args[8]), + angle, args[11]); + } + 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 11618eca..e147ad5d 100644 --- a/digital/asserv/src/asserv/traj.c +++ b/digital/asserv/src/asserv/traj.c @@ -37,6 +37,7 @@ #include "pos.h" #include "speed.h" #include "postrack.h" +#include "pwm.h" #include "contacts.h" @@ -59,6 +60,8 @@ enum TRAJ_FTW, /* Go to the dispenser. */ TRAJ_GTD, + /* Push the wall. */ + TRAJ_PTW, /* Go to position. */ TRAJ_GOTO, /* Go to angle. */ @@ -99,6 +102,9 @@ static uint8_t traj_use_center; /** Center sensor delay. */ static uint8_t traj_center_delay; +/** Initial values for x, y and angle, or -1. */ +static int32_t traj_init_x, traj_init_y, traj_init_a; + /** Initialise computed factors. */ void traj_init (void) @@ -210,6 +216,54 @@ traj_ftw_start_center (uint8_t backward, uint8_t center_delay, uint8_t seq) state_start (&state_main, MODE_TRAJ, seq); } +/** Push the wall mode. */ +static void +traj_ptw (void) +{ + /* If blocking, the wall was found. */ + if (pos_theta.blocked_counter >= pos_theta.blocked_counter_limit) + { + /* Initialise position. */ + if (traj_init_x != -1) + postrack_x = traj_init_x; + if (traj_init_y != -1) + postrack_y = traj_init_y; + if (traj_init_a != -1) + postrack_a = traj_init_a; + /* Stop motor control. */ + pos_reset (&pos_theta); + pos_reset (&pos_alpha); + state_main.variant = 0; + state_main.mode = MODE_PWM; + pwm_set (&pwm_left, 0); + pwm_set (&pwm_right, 0); + state_finish (&state_main); + traj_mode = TRAJ_DONE; + } +} + +/** Start push the wall mode. Position is initialised unless -1. */ +void +traj_ptw_start (uint8_t backward, int32_t init_x, int32_t init_y, + int32_t init_a, uint8_t seq) +{ + int16_t speed; + traj_mode = TRAJ_PTW; + traj_init_x = init_x; + traj_init_y = init_y; + traj_init_a = init_a; + state_start (&state_main, MODE_TRAJ, seq); + /* Use slow speed, without alpha control. */ + speed = speed_theta.slow; + speed *= 256; + if (backward) + speed = -speed; + speed_theta.use_pos = speed_alpha.use_pos = 0; + speed_theta.cons = speed; + speed_alpha.cons = 0; + state_main.variant = 2; +} + /** Go to the dispenser mode. */ static void traj_gtd (void) @@ -392,6 +446,9 @@ traj_update (void) case TRAJ_FTW: traj_ftw (); break; + case TRAJ_PTW: + traj_ptw (); + break; case TRAJ_GTD: traj_gtd (); break; diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h index 706fbdaf..ec345d0c 100644 --- a/digital/asserv/src/asserv/traj.h +++ b/digital/asserv/src/asserv/traj.h @@ -48,6 +48,10 @@ 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_ptw_start (uint8_t backward, int32_t init_x, int32_t init_y, + int32_t init_a, 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 ff15c82f..fcf0a9cc 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -168,6 +168,24 @@ twi_proto_callback (u8 *buf, u8 size) * - b: 0: forward, 1: backward. */ traj_ftw_start (buf[2], 0); break; + case c ('G', 9): + /* Push the wall. + * - b: 0: forward, 1: backward. + * - 3b: init_x. + * - 3b: init_y. + * - w: init_a. */ + { + int32_t angle; + if (buf[9] == 0xff && buf[10] == 0xff) + angle = -1; + else + angle = v8_to_v32 (0, buf[9], buf[10], 0); + traj_ptw_start (buf[2], + v8_to_v32 (buf[3], buf[4], buf[5], 0xff), + v8_to_v32 (buf[6], buf[7], buf[8], 0xff), + angle, 0); + } + break; case c ('g', 2): /* Go to the wall using center sensor with delay. * - b: 0: forward, 1: backward. diff --git a/digital/asserv/tools/asserv/asserv.py b/digital/asserv/tools/asserv/asserv.py index 997b05fb..4f386d7a 100644 --- a/digital/asserv/tools/asserv/asserv.py +++ b/digital/asserv/tools/asserv/asserv.py @@ -269,6 +269,15 @@ class Proto: self.proto.send ('f', 'BB', backward and 1 or 0, self.mseq) self.wait (self.finished, auto = True) + def ptw (self, backward = True, init_x = None, init_y = None, + init_a = None): + """Push the wall.""" + self.mseq += 1 + self.proto.send ('f', 'BllHB', backward and 1 or 0, + self._dist_f248 (init_x), self._dist_f248 (init_y), + self._angle_f16 (init_a), self.mseq) + self.wait (self.finished, auto = True) + def set_simu_pos (self, x, y, a): """Set simulated position.""" self.proto.send ('h', 'chhh', 'X', int (round (x)), int (round (y)), @@ -381,10 +390,16 @@ class Proto: return int (round (d / self.param['scale'])) def _dist_f248 (self, d): - return int (round ((1 << 8) * d / self.param['scale'])) + if d is None: + return -1 + else: + return int (round ((1 << 8) * d / self.param['scale'])) def _angle_f16 (self, a): - return int (round ((1 << 16) * a / (2 * math.pi))) & 0xffff + if a is None: + return 0xffff + else: + return int (round ((1 << 16) * a / (2 * math.pi))) & 0xffff def _angle_f824 (self, a): return int (round ((1 << 24) * a / (2 * math.pi))) -- cgit v1.2.3