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/test/testgetsamples/main.c | 35 +++++++++++++++++++------------ 1 file changed, 22 insertions(+), 13 deletions(-) (limited to 'digital/io/src/test/testgetsamples/main.c') 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