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 ++++++++++++++++++++++++++ digital/io/src/asserv.h | 12 ++++++++++++ digital/io/src/top_cb.c | 4 +--- 3 files changed, 39 insertions(+), 3 deletions(-) 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) diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h index 5cb0c12b..04ebe68f 100644 --- a/digital/io/src/asserv.h +++ b/digital/io/src/asserv.h @@ -264,6 +264,18 @@ void asserv_set_speed (uint8_t linear_high, uint8_t angular_high, uint8_t linear_low, uint8_t angular_low); +/** + * Set the complete position of the bot. + * This is an helpful function preventing you from calling multiples other + * ones. + * It calls other class commands. + * @param x X position + * @param y Y position + * @param angle angular position + */ +void +asserv_set_position (int32_t x, int32_t y, int16_t angle); + /** * Go to an absolute position in (X, Y). * @param x the x position on the table. diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c index 7935beed..60ea6c28 100644 --- a/digital/io/src/top_cb.c +++ b/digital/io/src/top_cb.c @@ -70,9 +70,7 @@ top__WAIT_JACK_OUT__jack_removed_from_bot (void) /* Start the chronometer */ chrono_init (); /* Reset the position of the bot */ - asserv_set_x_position (PG_X_START); - asserv_set_y_position (PG_Y_START); - asserv_set_angle_position (PG_A_START); + asserv_set_position (PG_X_START, PG_Y_START, PG_A_START); /* Start the move FSM to our samples distributor */ move_start (PG_DISTRIBUTOR_SAMPLE_OUR_X, PG_DISTRIBUTOR_SAMPLE_OUR_Y); return top_next (WAIT_JACK_OUT, jack_removed_from_bot); -- cgit v1.2.3