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 ++++++ 2 files changed, 28 insertions(+) (limited to 'digital/ai/src') 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. -- cgit v1.2.3