From f5a73b4196986315975484ede4620bff8f5f3218 Mon Sep 17 00:00:00 2001
From: Michal Sojka <sojkam1@fel.cvut.cz>
Date: Wed, 26 May 2010 21:11:44 +0200
Subject: [PATCH] load_oranges OK

---
 src/robofsm/common-states.cc | 36 ++++++++++++++++++++++++++++++++----
 1 file changed, 32 insertions(+), 4 deletions(-)

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index d2e51925..7f438c62 100644
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -207,11 +207,12 @@ FSM_STATE(climb_the_slope)
 
 /* subautomaton to load oranges in two stages */
 FSM_STATE_DECL(load_oranges2);
+FSM_STATE_DECL(load_oranges3);
 FSM_STATE(load_oranges)
 {
 	switch(FSM_EVENT) {
 		case EV_ENTRY:
-			FSM_TIMER(2000);
+			FSM_TIMER(1000);
 			act_vidle(VIDLE_MIDDLE, VIDLE_MEDIUM_SPEED);
 			break;
 		case EV_VIDLE_DONE:
@@ -235,8 +236,35 @@ FSM_STATE(load_oranges2)
 {
 	switch(FSM_EVENT) {
 		case EV_ENTRY:
-			act_vidle(VIDLE_UP, 5);
-			FSM_TIMER(2000);
+			act_vidle(VIDLE_UP, VIDLE_MEDIUM_SPEED);
+			FSM_TIMER(1000);
+			break;
+		case EV_VIDLE_DONE:
+			FSM_TRANSITION(load_oranges3);
+			//SUBFSM_RET(NULL);
+			break;
+		case EV_TIMER:
+			FSM_TRANSITION(load_oranges3);
+			//SUBFSM_RET(NULL);
+			break;
+		case EV_MOTION_DONE:
+		case EV_START:
+		case EV_RETURN:
+		case EV_MOTION_ERROR:
+		case EV_SWITCH_STRATEGY:
+			DBG_PRINT_EVENT("unhandled event");
+		case EV_EXIT:
+			act_vidle(VIDLE_UP-1, VIDLE_FAST_SPEED);
+			break;
+	}
+}
+
+FSM_STATE(load_oranges3)
+{
+	switch(FSM_EVENT) {
+		case EV_ENTRY:
+			act_vidle(VIDLE_MIDDLE, 0);
+			FSM_TIMER(500);
 			break;
 		case EV_VIDLE_DONE:
 			SUBFSM_RET(NULL);
@@ -251,7 +279,7 @@ FSM_STATE(load_oranges2)
 		case EV_SWITCH_STRATEGY:
 			DBG_PRINT_EVENT("unhandled event");
 		case EV_EXIT:
-			act_vidle(VIDLE_UP-1, VIDLE_FAST_SPEED);
+			act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
 			break;
 	}
 }
-- 
2.39.2