From 2f89b61f644303d832c674912908c941b1fc8c9b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 13 May 2012 16:45:49 +0200 Subject: digital/io-hub/src/guybrush: go back to playground before closing the door --- digital/io-hub/src/guybrush/top.c | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) (limited to 'digital') diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c index 945b6998..0fcf9b03 100644 --- a/digital/io-hub/src/guybrush/top.c +++ b/digital/io-hub/src/guybrush/top.c @@ -96,7 +96,9 @@ FSM_STATES ( /* Going to an unload position. */ TOP_UNLOAD_GOING, /* Unloading, waiting for elements to fall. */ - TOP_UNLOADING) + TOP_UNLOADING, + /* Unloading, going back to playground. */ + TOP_UNLOADING_GOING_BACK) FSM_START_WITH (TOP_START) @@ -442,15 +444,21 @@ FSM_TRANS (TOP_UNLOAD_GOING, move_success, TOP_UNLOADING) return FSM_NEXT (TOP_UNLOAD_GOING, move_success); } -FSM_TRANS_TIMEOUT (TOP_UNLOADING, 250, - totem, TOP_TOTEM_GOING, - bottle, TOP_BOTTLE_GOING, - unload, TOP_UNLOAD_GOING) +FSM_TRANS_TIMEOUT (TOP_UNLOADING, 250, TOP_UNLOADING_GOING_BACK) { strat_success (); + asserv_move_linearly (100); + return FSM_NEXT_TIMEOUT (TOP_UNLOADING); +} + +FSM_TRANS (TOP_UNLOADING_GOING_BACK, robot_move_success, + totem, TOP_TOTEM_GOING, + bottle, TOP_BOTTLE_GOING, + unload, TOP_UNLOAD_GOING) +{ IO_CLR (OUTPUT_DOOR_OPEN); IO_SET (OUTPUT_DOOR_CLOSE); - RETURN_TOP_DECISION_SWITCH (TOP_UNLOADING, TOP_UNLOADING_TIMEOUT); + RETURN_TOP_DECISION_SWITCH (TOP_UNLOADING_GOING_BACK, robot_move_success); } /** UNLOAD failures. */ -- cgit v1.2.3