From 2376aeb614d69d7050209f858818bfa7d36be398 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Mon, 2 May 2011 23:32:51 +0200 Subject: digital/io: adapt FSM to new FSM system (AngFSM) --- digital/io/src/loader.c | 613 +++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 610 insertions(+), 3 deletions(-) (limited to 'digital/io/src/loader.c') diff --git a/digital/io/src/loader.c b/digital/io/src/loader.c index 4157023e..8dd557da 100644 --- a/digital/io/src/loader.c +++ b/digital/io/src/loader.c @@ -22,10 +22,16 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#define FSM_NAME AI + #include "common.h" #include "loader.h" - #include "fsm.h" +#include "asserv.h" +#include "mimot.h" +#include "bot.h" +#include "main.h" +#include "food.h" uint8_t loader_elements; uint8_t loader_want_up; @@ -34,13 +40,614 @@ void loader_up (void) { loader_want_up = 1; - fsm_handle_event (&ai_fsm, AI_EVENT_loader_up); + FSM_HANDLE (AI, loader_up); } void loader_down (void) { loader_want_up = 0; - fsm_handle_event (&ai_fsm, AI_EVENT_loader_down); + FSM_HANDLE (AI, loader_down); +} + +FSM_STATES ( + /* wait start */ + LOADER_IDLE, + /* wait until the jack is inserted for the first time */ + LOADER_WAIT_JACK_IN, + /* wait until the jack is removed to initialise robot */ + LOADER_WAIT_JACK_OUT, + /* move the elevator down to find zero position */ + LOADER_INIT_ELEVATOR_ZERO, + /* test clamp closing */ + LOADER_INIT_CLAMP_CLOSE, + /* find clamp zero position */ + LOADER_INIT_CLAMP_ZERO, + /* find gate zero */ + LOADER_INIT_GATE_ZERO, + /* move the elevator up so that the robot can go to the wall */ + LOADER_INIT_ELEVATOR_UP, + /* wait with gate open */ + LOADER_INIT_GATE_WAIT, + /* close gate before start */ + LOADER_INIT_GATE_CLOSE, + /* up position, clamp is protected */ + LOADER_UP, + /* down position, ready to take an element */ + LOADER_DOWN, + /* moving to the up position */ + LOADER_UPING, + /* moving to the down position */ + LOADER_DOWNING, + /* error while moving up, stop in an unknown state */ + LOADER_ERROR, + /* error recovery moving down */ + LOADER_ERROR_DOWNING, + /* error recovery moving down, open clamp */ + LOADER_ERROR_DOWNING_OPEN, + /* error recovery moving up */ + LOADER_ERROR_UPING, + /* error recovery moving up, open clamp */ + LOADER_ERROR_UPING_OPEN, + /* close clamp */ + LOADER_LOAD_CLOSING, + /* move load up to the unload position */ + LOADER_LOAD_UPING, + /* open clamp to unload, wait until unload position */ + LOADER_LOAD_UNLOADING, + /* check clamp is open */ + LOADER_LOAD_UNLOADING_OPEN, + /* open clamp due to no elements to pick */ + LOADER_LOAD_EMPTY_OPEN) + +FSM_EVENTS ( + /* asserv success result */ + elevator_succeed, + /* asserv failure result */ + elevator_failed, + /* elevator above unloading position */ + elevator_unload_position, + /* asserv failure result */ + clamp_failed, + /* asserv success result */ + gate_succeed, + /* asserv failure result */ + gate_failed, + /* element sensed between clamp to be loaded */ + loader_element, + /* make the loader ready to load an element */ + loader_down, + /* protect the clamp by moving it up */ + loader_up, + /* posted when loader successfully moved down */ + loader_downed, + /* posted when loader successfully moved up */ + loader_uped, + /* posted when loader in an unknown state due to error while moving up */ + loader_errored, + /* loader blocked, due to a black corn */ + loader_black) + +FSM_START_WITH (LOADER_IDLE) + +FSM_TRANS (LOADER_IDLE, start, LOADER_WAIT_JACK_IN) +{ + return FSM_NEXT (LOADER_IDLE, start); +} + +FSM_TRANS (LOADER_WAIT_JACK_IN, + jack_inserted_into_bot, + LOADER_WAIT_JACK_OUT) +{ + return FSM_NEXT (LOADER_WAIT_JACK_IN, jack_inserted_into_bot); +} + +FSM_TRANS (LOADER_WAIT_JACK_IN, + hola_start, + LOADER_IDLE) +{ + return FSM_NEXT (LOADER_WAIT_JACK_IN, hola_start); +} + +/* + * find elevator zero + * close clamp + * find gate zero + */ +FSM_TRANS (LOADER_WAIT_JACK_OUT, + jack_removed_from_bot, + LOADER_INIT_ELEVATOR_ZERO) +{ + asserv_motor0_zero_position (-BOT_ELEVATOR_ZERO_SPEED); + mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0); + mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0); + asserv_motor1_zero_position (-BOT_GATE_SPEED); + return FSM_NEXT (LOADER_WAIT_JACK_OUT, jack_removed_from_bot); +} + +FSM_TRANS (LOADER_INIT_ELEVATOR_ZERO, + elevator_succeed, + LOADER_INIT_CLAMP_CLOSE) +{ + return FSM_NEXT (LOADER_INIT_ELEVATOR_ZERO, elevator_succeed); +} + +/* + * move elevator up + * find clamp zero + */ +FSM_TRANS (LOADER_INIT_CLAMP_CLOSE, + clamp_succeed, + LOADER_INIT_CLAMP_ZERO) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, + BOT_ELEVATOR_ZERO_SPEED); + mimot_motor0_zero_position (-BOT_CLAMP_ZERO_SPEED); + mimot_motor1_zero_position (-BOT_CLAMP_ZERO_SPEED); + return FSM_NEXT (LOADER_INIT_CLAMP_CLOSE, clamp_succeed); +} + +FSM_TRANS (LOADER_INIT_CLAMP_ZERO, + clamp_succeed, + LOADER_INIT_GATE_ZERO) +{ + return FSM_NEXT (LOADER_INIT_CLAMP_ZERO, clamp_succeed); +} + +FSM_TRANS (LOADER_INIT_GATE_ZERO, + gate_succeed, + LOADER_INIT_ELEVATOR_UP) +{ + return FSM_NEXT (LOADER_INIT_GATE_ZERO, gate_succeed); +} + +FSM_TRANS (LOADER_INIT_ELEVATOR_UP, + elevator_succeed, + LOADER_INIT_GATE_WAIT) +{ + return FSM_NEXT (LOADER_INIT_ELEVATOR_UP, elevator_succeed); +} + +/* + * initialisation failure + */ +FSM_TRANS (LOADER_INIT_ELEVATOR_UP, + elevator_failed, + LOADER_IDLE) +{ + /* Move so that the operator notice it. */ + asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP / 3, + BOT_ELEVATOR_ZERO_SPEED); + return FSM_NEXT (LOADER_INIT_ELEVATOR_UP, elevator_failed); } +/* + * close gate + */ +FSM_TRANS_TIMEOUT (LOADER_INIT_GATE_WAIT, 225, + LOADER_INIT_GATE_CLOSE) +{ + asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED); + return FSM_NEXT_TIMEOUT (LOADER_INIT_GATE_WAIT); +} + +FSM_TRANS (LOADER_INIT_GATE_CLOSE, + gate_succeed, + LOADER_UP) +{ + return FSM_NEXT (LOADER_INIT_GATE_CLOSE, gate_succeed); +} + +/* + * move down + */ +FSM_TRANS (LOADER_UP, + loader_down, + LOADER_DOWNING) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED); + return FSM_NEXT (LOADER_UP, loader_down); +} + +/* + * move up + */ +FSM_TRANS (LOADER_DOWN, + loader_up, + LOADER_UPING) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED); + return FSM_NEXT (LOADER_DOWN, loader_up); +} + +/* + * clamp + */ +FSM_TRANS (LOADER_DOWN, + loader_element, + LOADER_LOAD_CLOSING) +{ + mimot_motor0_clamp (BOT_CLAMP_SPEED, BOT_CLAMP_PWM); + mimot_motor1_clamp (BOT_CLAMP_SPEED, BOT_CLAMP_PWM); + return FSM_NEXT (LOADER_DOWN, loader_element); +} + +/* + * post loader_uped event + */ +FSM_TRANS (LOADER_UPING, + elevator_succeed, + LOADER_UP) +{ + main_post_event (FSM_EVENT (AI,loader_uped)); + return FSM_NEXT (LOADER_UPING, elevator_succeed); +} + +/* + * post loader_errored event + */ +FSM_TRANS (LOADER_UPING, + elevator_failed, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT (LOADER_UPING, elevator_failed); +} + +/* + * move down + */ +FSM_TRANS (LOADER_UPING, + loader_down, + LOADER_DOWNING) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED); + return FSM_NEXT (LOADER_UPING, loader_down); +} + +/* + * release elevator motor + * post loader_downed event + */ +FSM_TRANS (LOADER_DOWNING, + elevator_succeed, + LOADER_DOWN) +{ + asserv_motor0_free (); + main_post_event (FSM_EVENT (AI, loader_downed)); + return FSM_NEXT (LOADER_DOWNING, elevator_succeed); +} + +/* + * post loader_errored event + */ +FSM_TRANS (LOADER_DOWNING, + elevator_failed, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT (LOADER_DOWNING, elevator_failed); +} + +/* + * emergency stop + * post loader_errored event + */ +FSM_TRANS (LOADER_DOWNING, + loader_element, + LOADER_ERROR) +{ + asserv_motor0_free (); + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT (LOADER_DOWNING, loader_element); +} + +/* + * move up + */ +FSM_TRANS (LOADER_DOWNING, + loader_up, + LOADER_UPING) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED); + return FSM_NEXT (LOADER_DOWNING, loader_up); +} + +/* + * move down + */ +FSM_TRANS (LOADER_ERROR, + loader_down, + LOADER_ERROR_DOWNING) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED); + return FSM_NEXT (LOADER_ERROR, loader_down); +} + +/* + * move up + */ +FSM_TRANS (LOADER_ERROR, + loader_up, + LOADER_ERROR_UPING) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP, BOT_ELEVATOR_SPEED); + return FSM_NEXT (LOADER_ERROR, loader_up); +} + +/* + * release elevator motor + * open clamp + */ +FSM_TRANS (LOADER_ERROR_DOWNING, + elevator_succeed, + LOADER_ERROR_DOWNING_OPEN) +{ + asserv_motor0_free (); + mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + return FSM_NEXT (LOADER_ERROR_DOWNING, elevator_succeed); +} + +/* + * post loader_errored event + */ +FSM_TRANS (LOADER_ERROR_DOWNING, + elevator_failed, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT (LOADER_ERROR_DOWNING, elevator_failed); +} + +/* + * post loader_errored event + */ +FSM_TRANS_TIMEOUT (LOADER_ERROR_DOWNING, 225, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT_TIMEOUT (LOADER_ERROR_DOWNING); +} + +/* + * post loader_downed event + */ +FSM_TRANS (LOADER_ERROR_DOWNING_OPEN, + clamp_succeed, + LOADER_DOWN) +{ + main_post_event (FSM_EVENT (AI, loader_downed)); + return FSM_NEXT (LOADER_ERROR_DOWNING_OPEN, clamp_succeed); +} + +/* + * post loader_errored event + */ +FSM_TRANS (LOADER_ERROR_DOWNING_OPEN, + clamp_failed, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT (LOADER_ERROR_DOWNING_OPEN, clamp_failed); +} + +/* + * post loader_errored event + */ +FSM_TRANS_TIMEOUT (LOADER_ERROR_DOWNING_OPEN, 225, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT_TIMEOUT (LOADER_ERROR_DOWNING_OPEN); +} + +/* + * open clamp + */ +FSM_TRANS (LOADER_ERROR_UPING, + elevator_succeed, + LOADER_ERROR_UPING_OPEN) +{ + mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + return FSM_NEXT (LOADER_ERROR_UPING, elevator_succeed); +} + +/* + * post loader_errored event + */ +FSM_TRANS (LOADER_ERROR_UPING, + elevator_failed, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT (LOADER_ERROR_UPING, elevator_failed); +} + +/* + * post loader_errored event + */ +FSM_TRANS_TIMEOUT (LOADER_ERROR_UPING, 225, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT_TIMEOUT (LOADER_ERROR_UPING); +} + +/* + * post loader_uped event + */ +FSM_TRANS (LOADER_ERROR_UPING_OPEN, + clamp_succeed, + LOADER_UP) +{ + main_post_event (FSM_EVENT (AI, loader_uped)); + return FSM_NEXT (LOADER_ERROR_UPING_OPEN, clamp_succeed); +} + +/* + * post loader_errored event + */ +FSM_TRANS (LOADER_ERROR_UPING_OPEN, + clamp_failed, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT (LOADER_ERROR_UPING_OPEN, clamp_failed); +} + +/* + * post loader_errored event + */ +FSM_TRANS_TIMEOUT (LOADER_ERROR_UPING_OPEN, 225, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT_TIMEOUT (LOADER_ERROR_UPING_OPEN); +} + +FSM_TRANS (LOADER_LOAD_CLOSING, clamp_succeed, + /* move up + * count one element */ + full, LOADER_LOAD_UPING, + /* open clamp */ + empty, LOADER_LOAD_EMPTY_OPEN) +{ + /* Measure load using clamp position. */ + uint16_t tickness = BOT_CLAMP_WIDTH_STEP + - mimot_get_motor0_position () + - mimot_get_motor1_position (); + if (tickness > BOT_CLAMP_EMPTY_STEP) + { + asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP, + BOT_ELEVATOR_SPEED); + position_t robot_position; + asserv_get_position (&robot_position); + food_taken (robot_position); + loader_elements++; + return FSM_NEXT (LOADER_LOAD_CLOSING, clamp_succeed, full); + } + else + { + mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + return FSM_NEXT (LOADER_LOAD_CLOSING, clamp_succeed, empty); + } +} + +/* + * open clamp + */ +FSM_TRANS (LOADER_LOAD_UPING, + elevator_unload_position, + LOADER_LOAD_UNLOADING) +{ + mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + return FSM_NEXT (LOADER_LOAD_UPING, elevator_unload_position); +} + +/* + * post loader_errored or loader_black event + * remove one element + * open clamp + */ +FSM_TRANS (LOADER_LOAD_UPING, + elevator_failed, + LOADER_ERROR) +{ + if (asserv_get_motor0_position () < BOT_ELEVATOR_BLACK_THRESHOLD_STEP) + { + if (loader_elements) + loader_elements--; + main_post_event (FSM_EVENT (AI, loader_black)); + } + else + main_post_event (FSM_EVENT (AI, loader_errored)); + mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + return FSM_NEXT (LOADER_LOAD_UPING, elevator_failed); +} + +FSM_TRANS (LOADER_LOAD_UNLOADING, + elevator_succeed, + LOADER_LOAD_UNLOADING_OPEN) +{ + return FSM_NEXT (LOADER_LOAD_UNLOADING, elevator_succeed); +} + +/* + * post loader_errored event + * open clamp + */ +FSM_TRANS (LOADER_LOAD_UNLOADING, + elevator_failed, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + return FSM_NEXT (LOADER_LOAD_UNLOADING, elevator_failed); +} + +FSM_TRANS (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, + /* move down */ + down, LOADER_DOWNING, + /* move up */ + up, LOADER_UPING) +{ + if (loader_want_up) + { + asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, + BOT_ELEVATOR_SPEED); + return FSM_NEXT (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, up); + } + else + { + asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, + BOT_ELEVATOR_SPEED); + return FSM_NEXT (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, down); + } +} + +/* + * post loader_errored event + */ +FSM_TRANS (LOADER_LOAD_UNLOADING_OPEN, + clamp_failed, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT (LOADER_LOAD_UNLOADING_OPEN, clamp_failed); +} + +FSM_TRANS (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, + /* post loader_downed event */ + down, LOADER_DOWN, + /* move up */ + up, LOADER_UPING) +{ + if (loader_want_up) + { + asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, + BOT_ELEVATOR_SPEED); + return FSM_NEXT (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, up); + } + else + { + main_post_event (FSM_EVENT (AI, loader_downed)); + return FSM_NEXT (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, down); + } +} + +/* + * post loader_errored event + */ +FSM_TRANS (LOADER_LOAD_EMPTY_OPEN, + clamp_failed, + LOADER_ERROR) +{ + main_post_event (FSM_EVENT (AI, loader_errored)); + return FSM_NEXT (LOADER_LOAD_EMPTY_OPEN, clamp_failed); +} -- cgit v1.2.3