From 4e9abfc25206587a3e96cfb2251dae5e81d34b5f Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Mon, 21 Apr 2008 23:55:41 +0200 Subject: * digital/io/src - add a simple function to set the complete bot position ; - integrate its usage in the top FSM. --- digital/io/src/asserv.c | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) (limited to 'digital/io/src/asserv.c') diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c index edc504eb..edf08a5d 100644 --- a/digital/io/src/asserv.c +++ b/digital/io/src/asserv.c @@ -494,6 +494,32 @@ asserv_set_speed (uint8_t linear_high, uint8_t angular_high, asserv_twi_send_command ('p', 6); } +/* Set the complete position of the bot. */ +void +asserv_set_position (int32_t x, int32_t y, int16_t angle) +{ + /* 'X' subcommand */ + asserv_twi_buffer_param[0] = 'X'; + /* Put x position as parameter */ + asserv_twi_buffer_param[1] = v32_to_v8 (x, 2); + asserv_twi_buffer_param[2] = v32_to_v8 (x, 1); + asserv_twi_buffer_param[3] = v32_to_v8 (x, 0); + /* 'Y' subcommand */ + asserv_twi_buffer_param[4] = 'X'; + /* Put x position as parameter */ + asserv_twi_buffer_param[5] = v32_to_v8 (x, 2); + asserv_twi_buffer_param[6] = v32_to_v8 (x, 1); + asserv_twi_buffer_param[7] = v32_to_v8 (x, 0); + /* 'A' subcommand */ + asserv_twi_buffer_param[8] = 'A'; + /* Put angle position as parameter */ + asserv_twi_buffer_param[9] = v32_to_v8 (angle, 1); + asserv_twi_buffer_param[10] = v32_to_v8 (angle, 0); + asserv_twi_buffer_param[11] = 0; + /* Send the whole things in a one time shot */ + asserv_twi_send_command ('p', 12); +} + /* Go to an absolute position in (X, Y). */ void asserv_goto (uint32_t x, uint32_t y) -- cgit v1.2.3