From 011fd3e4e4d252d02df156a7679c6156cdc72c6a Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Mon, 14 Apr 2008 12:04:01 +0200 Subject: * digital/io/src * asserv - add an goto_angle (not implemented yet). * get sample FSM - the get sample FSM will not call any other FSM (like the move). Instead, it assumes we are already at the right place, we just ensure the angle is correct ; - update the first state in consequences ; - add an angle as parameters (maybe we can remove the X/Y). * general defines - add define to ease the opening of the input hole. --- digital/io/src/asserv.c | 10 +++++++++ digital/io/src/asserv.h | 8 +++++++ digital/io/src/getsamples.c | 1 + digital/io/src/getsamples.fsm | 19 +++++++++++------ digital/io/src/getsamples.h | 4 ++++ digital/io/src/getsamples_cb.c | 35 +++++++++++++++---------------- digital/io/src/giboulee.h | 7 +++++++ digital/io/src/test/testgetsamples/main.c | 35 +++++++++++++++++++------------ 8 files changed, 82 insertions(+), 37 deletions(-) diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c index d7d383c4..0b801a13 100644 --- a/digital/io/src/asserv.c +++ b/digital/io/src/asserv.c @@ -350,6 +350,16 @@ asserv_move_angularly (int16_t angle) asserv_twi_send_command ('a', 2); } +/* Move the bot to a specific angle. */ +void +asserv_goto_angle (int16_t angle) +{ + /* Put angle as parameter */ + asserv_twi_buffer_param[0] = v16_to_v8 (angle, 1); + asserv_twi_buffer_param[1] = v16_to_v8 (angle, 0); + /* TODO */ +} + /* Go to the wall (moving backward). */ void asserv_go_to_the_wall (void) diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h index e6bc06c4..46448a59 100644 --- a/digital/io/src/asserv.h +++ b/digital/io/src/asserv.h @@ -176,6 +176,14 @@ asserv_move_linearly (int32_t distance); void asserv_move_angularly (int16_t angle); +/** + * Move the bot to a specific angle. + * The angle is absolute and not a difference with the current one. + * @param a the absolute angle in degrees + */ +void +asserv_goto_angle (int16_t angle); + /** * Go to the wall (moving backward). * Move class command. diff --git a/digital/io/src/getsamples.c b/digital/io/src/getsamples.c index 9feed1eb..e68857a0 100644 --- a/digital/io/src/getsamples.c +++ b/digital/io/src/getsamples.c @@ -41,6 +41,7 @@ getsamples_start (struct getsamples_data_t data) /* Set parameters */ getsamples_data.distributor_x = data.distributor_x; getsamples_data.distributor_y = data.distributor_y; + getsamples_data.distributor_angle = data.distributor_angle; getsamples_data.sample_bitfield = data.sample_bitfield; getsamples_data.event = data.event; diff --git a/digital/io/src/getsamples.fsm b/digital/io/src/getsamples.fsm index 4c833bc0..1fdfaa98 100644 --- a/digital/io/src/getsamples.fsm +++ b/digital/io/src/getsamples.fsm @@ -1,12 +1,18 @@ # Get samples FSM +# A general remark on this FSM: it is not its responsibility to put the bot in +# front of the distributor, it the responsibility of the top FSM using the +# move FSM. +# For example, the top FSM will order the bot to move in front of the +# distributor using the move FSM. Then, when it is arrived, it will the get +# sample FSM that will ensure we well placed (in term of angle for example). get_samples Get samples from the distributor States: IDLE waiting for the beginning of the get samples FSM - GO_IN_FRONT_OF_DISTRIBUTOR - move the bot in front of a distributor (not in contact with the distributor) + FACE_DISTRIBUTOR + face the distributor OPEN_INPUT_HOLE move the arm to let the samples enter into the bot by the input hole APPROACH_DISTRIBUTOR @@ -21,6 +27,8 @@ States: Events: start initialize the FSM +# For the moment, this event is currently not managed. +# TODO: manage it bot_move_failed the bot movement failed (blocked by something for example) bot_move_succeed @@ -31,10 +39,10 @@ Events: the arm has just passed the 'noted' position IDLE: - start -> GO_IN_FRONT_OF_DISTRIBUTOR - start going in front of the desired distributor + start -> FACE_DISTRIBUTOR + do a goto angle to make the bot facing the distributor -GO_IN_FRONT_OF_DISTRIBUTOR: +FACE_DISTRIBUTOR: bot_move_succeed -> OPEN_INPUT_HOLE move the arm to open the input hole @@ -57,7 +65,6 @@ MOVE_AWAY_FROM_DISTRIBUTOR: bot_move_succeed -> CLOSE_INPUT_HOLE close input hole - CLOSE_INPUT_HOLE: arm_move_succeed -> IDLE tell the top FSM we have finished diff --git a/digital/io/src/getsamples.h b/digital/io/src/getsamples.h index c033665d..f7dd06a1 100644 --- a/digital/io/src/getsamples.h +++ b/digital/io/src/getsamples.h @@ -38,6 +38,10 @@ struct getsamples_data_t * Y position of the distributor where to get samples from. */ uint32_t distributor_y; + /** + * The angle to approach the distributor. + */ + int16_t distributor_angle; /** * Bit field to indicate where to put the sample. * If bit 0 is set to 1, a sample will be put into the out_right_box. If diff --git a/digital/io/src/getsamples_cb.c b/digital/io/src/getsamples_cb.c index 0de105e4..5e963ce6 100644 --- a/digital/io/src/getsamples_cb.c +++ b/digital/io/src/getsamples_cb.c @@ -33,6 +33,19 @@ #include "giboulee.h" /* BOT_ */ #include "playground.h" /* PG_* */ +/* + * FACE_DISTRIBUTOR =bot_move_succeed=> + * => OPEN_INPUT_HOLE + * move the arm to open the input hole + */ +fsm_branch_t +getsamples__FACE_DISTRIBUTOR__bot_move_succeed (void) +{ + /* Move the arm to open the input hole to be able to take some samples */ + asserv_move_arm (-BOT_ARM_MIN_TO_OPEN, BOT_ARM_SPEED); + return getsamples_next (FACE_DISTRIBUTOR, bot_move_succeed); +} + /* * OPEN_INPUT_HOLE =arm_move_succeed=> * => APPROACH_DISTRIBUTOR @@ -59,19 +72,6 @@ getsamples__CLOSE_INPUT_HOLE__arm_move_succeed (void) return getsamples_next (CLOSE_INPUT_HOLE, arm_move_succeed); } -/* - * GO_IN_FRONT_OF_DISTRIBUTOR =bot_move_succeed=> - * => OPEN_INPUT_HOLE - * move the arm to open the input hole - */ -fsm_branch_t -getsamples__GO_IN_FRONT_OF_DISTRIBUTOR__bot_move_succeed (void) -{ - /* Move the arm to open the input hole to be able to take some samples */ - asserv_close_input_hole (); - return getsamples_next (GO_IN_FRONT_OF_DISTRIBUTOR, bot_move_succeed); -} - /* * TAKE_SAMPLES =arm_pass_noted_position=> * no_more => MOVE_AWAY_FROM_DISTRIBUTOR @@ -102,15 +102,14 @@ getsamples__TAKE_SAMPLES__arm_pass_noted_position (void) /* * IDLE =start=> - * => GO_IN_FRONT_OF_DISTRIBUTOR - * start going in front of the desired distributor + * => FACE_DISTRIBUTOR + * do a goto angle to make the bot facing the distributor */ fsm_branch_t getsamples__IDLE__start (void) { - /* Move to the desired distributor */ - move_start (getsamples_data.distributor_x, - getsamples_data.distributor_y); + /* Face the distributor */ + asserv_goto_angle (getsamples_data.distributor_angle); return getsamples_next (IDLE, start); } diff --git a/digital/io/src/giboulee.h b/digital/io/src/giboulee.h index 45744f99..bda60c8f 100644 --- a/digital/io/src/giboulee.h +++ b/digital/io/src/giboulee.h @@ -47,6 +47,13 @@ */ #define BOT_ARM_THIRD_ROUND BOT_ARM_STEP_ROUND / 3 +/** + * The number of steps to do to open the input hole when the arm is closing + * it. + * For the moment, 15% of a complete turn + */ +#define BOT_ARM_MIN_TO_OPEN (BOT_ARM_STEP_ROUND / 100 * 15) + /** * How to compute a angle for giboulee? * One degree is 65536 / 360 diff --git a/digital/io/src/test/testgetsamples/main.c b/digital/io/src/test/testgetsamples/main.c index 76f8627f..90eeabb3 100644 --- a/digital/io/src/test/testgetsamples/main.c +++ b/digital/io/src/test/testgetsamples/main.c @@ -47,8 +47,8 @@ getsamples_print_test (fsm_t *getsamples) case GETSAMPLES_STATE_IDLE: printf ("IDLE"); break; - case GETSAMPLES_STATE_GO_IN_FRONT_OF_DISTRIBUTOR: - printf ("GO_IN_FRONT_OF_DISTRIBUTOR"); + case GETSAMPLES_STATE_FACE_DISTRIBUTOR: + printf ("FACE_DISTRIBUTOR"); break; case GETSAMPLES_STATE_OPEN_INPUT_HOLE: printf ("OPEN_INPUT_HOLE"); @@ -82,6 +82,7 @@ main (void) /* Go to our distributor */ data.distributor_x = PG_DISTRIBUTOR_SAMPLE_OUR_X; data.distributor_y = PG_DISTRIBUTOR_SAMPLE_OUR_Y; + data.distributor_angle = PG_DISTRIBUTOR_SAMPLE_OUR_A; data.sample_bitfield = 0; /* We want to put the sample into the 0, 2 and 4 box */ data.sample_bitfield |= _BV(0); @@ -98,9 +99,10 @@ main (void) getsamples_print_test (&getsamples_fsm); /* The move to the front of the distributor failed */ - fsm_handle_event (&getsamples_fsm, - GETSAMPLES_EVENT_bot_move_failed); - getsamples_print_test (&getsamples_fsm); + /* TODO: manage it! */ +// fsm_handle_event (&getsamples_fsm, +// GETSAMPLES_EVENT_bot_move_failed); +// getsamples_print_test (&getsamples_fsm); /* We are in front of the distributor */ fsm_handle_event (&getsamples_fsm, @@ -143,6 +145,9 @@ main (void) return 0; } + +static uint16_t asserv_arm_position = 0; + /* Define functions for debug */ void trap_setup_path_to_box (uint8_t box_id) @@ -151,21 +156,25 @@ trap_setup_path_to_box (uint8_t box_id) } void -asserv_close_input_hole (void) +asserv_move_linearly (int32_t distance) { - printf ("[asserv] Put the arm in front of the input hole.\n"); + printf ("[asserv] Make the bot move linearly of %d mm.\n", distance); } void -asserv_move_linearly (int32_t distance) +asserv_move_arm (uint16_t position, uint8_t speed) { - printf ("[asserv] Make the bot move linearly of %d mm.\n", distance); + asserv_arm_position += position; + printf ("[asserv] Move arm at %d (speed: %d).\n", + asserv_arm_position, speed); } void -asserv_move_arm (uint16_t position, uint8_t speed) +asserv_close_input_hole (void) { - printf ("[asserv] Move arm at %d (speed: %d).\n", position, speed); + printf ("[asserv] Put the arm in front of the input hole.\n"); + asserv_move_arm (asserv_arm_position % + BOT_ARM_THIRD_ROUND, BOT_ARM_SPEED); } void @@ -187,9 +196,9 @@ asserv_goto (uint32_t x, uint32_t y) } void -move_start (uint32_t x, uint32_t y) +asserv_goto_angle (int16_t angle) { - printf ("[FSM:move] Move the bot to (%d; %d).\n", x, y); + printf ("[asserv] Move the bot to face %X.\n", angle); } void -- cgit v1.2.3