From becf1a33459af05d60f70a9b18bdeb05a987c558 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 07:09:25 +0200 Subject: digital/io-hub: stop robot when a element is taken --- digital/io-hub/src/robospierre/move.c | 32 ++++++++++++++++++++++++++++++++ digital/io-hub/src/robospierre/move.h | 4 ++++ digital/io-hub/src/robospierre/top.c | 22 ++++++++++++++++++++++ 3 files changed, 58 insertions(+) (limited to 'digital') diff --git a/digital/io-hub/src/robospierre/move.c b/digital/io-hub/src/robospierre/move.c index 623dabdf..a44b5715 100644 --- a/digital/io-hub/src/robospierre/move.c +++ b/digital/io-hub/src/robospierre/move.c @@ -101,6 +101,13 @@ move_start_noangle (vect_t position, uint8_t backward, int16_t shorten) fsm_queue_post_event (FSM_EVENT (AI, move_start)); } +void +move_stop (void) +{ + /* Stop the FSM. */ + fsm_queue_post_event (FSM_EVENT (AI, move_stop)); +} + void move_obstacles_update (void) { @@ -144,6 +151,8 @@ FSM_EVENTS ( robot_move_failure, /* Initialize the FSM and start the movement directly. */ move_start, + /* Stop movement. */ + move_stop, /* Movement success. */ move_success, /* Movement failure. */ @@ -343,6 +352,12 @@ FSM_TRANS_TIMEOUT (MOVE_ROTATING, 1250, return FSM_NEXT_TIMEOUT (MOVE_ROTATING); } +FSM_TRANS (MOVE_ROTATING, move_stop, MOVE_IDLE) +{ + asserv_stop_motor (); + return FSM_NEXT (MOVE_ROTATING, move_stop); +} + FSM_TRANS (MOVE_MOVING, robot_move_success, done, MOVE_IDLE, path_found_rotate, MOVE_ROTATING, @@ -415,6 +430,12 @@ FSM_TRANS (MOVE_MOVING, obstacle_in_front, return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryagain); } +FSM_TRANS (MOVE_MOVING, move_stop, MOVE_IDLE) +{ + asserv_stop_motor (); + return FSM_NEXT (MOVE_MOVING, move_stop); +} + FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, tryout, MOVE_IDLE, path_found_rotate, MOVE_ROTATING, @@ -479,6 +500,12 @@ FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, } } +FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, move_stop, MOVE_IDLE) +{ + asserv_stop_motor (); + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, move_stop); +} + FSM_TRANS_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, 250, path_found_rotate, MOVE_ROTATING, path_found, MOVE_MOVING, @@ -507,3 +534,8 @@ FSM_TRANS_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, 250, } } +FSM_TRANS (MOVE_WAIT_FOR_CLEAR_PATH, move_stop, MOVE_IDLE) +{ + return FSM_NEXT (MOVE_WAIT_FOR_CLEAR_PATH, move_stop); +} + diff --git a/digital/io-hub/src/robospierre/move.h b/digital/io-hub/src/robospierre/move.h index 877002fd..72af5b84 100644 --- a/digital/io-hub/src/robospierre/move.h +++ b/digital/io-hub/src/robospierre/move.h @@ -48,6 +48,10 @@ move_start (position_t position, uint8_t backward); void move_start_noangle (vect_t position, uint8_t backward, int16_t shorten); +/** Stop movement. */ +void +move_stop (void); + /** To be called when obstacles positions are computed. */ void move_obstacles_update (void); diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 4554c12b..05568099 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -160,6 +160,14 @@ top_decision (void) return top_go_element (); } +static void +top_taken_pawn (void) +{ + position_t robot_pos; + asserv_get_position (&robot_pos); + element_taken (element_nearest_element_id (robot_pos), ELEMENT_PAWN); +} + FSM_TRANS (TOP_GOING_OUT2, robot_move_success, clamp_working, TOP_WAITING_CLAMP, drop, TOP_GOING_TO_DROP, @@ -224,6 +232,13 @@ FSM_TRANS (TOP_GOING_TO_DROP, move_failure, } } +FSM_TRANS (TOP_GOING_TO_DROP, clamp_working, TOP_WAITING_CLAMP) +{ + top_taken_pawn (); + move_stop (); + return FSM_NEXT (TOP_GOING_TO_DROP, clamp_working); +} + FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success, clamp_working, TOP_WAITING_CLAMP, drop, TOP_GOING_TO_DROP, @@ -254,6 +269,13 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_failure, } } +FSM_TRANS (TOP_GOING_TO_ELEMENT, clamp_working, TOP_WAITING_CLAMP) +{ + top_taken_pawn (); + move_stop (); + return FSM_NEXT (TOP_GOING_TO_ELEMENT, clamp_working); +} + FSM_TRANS (TOP_WAITING_CLAMP, clamp_done, drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT) -- cgit v1.2.3