load_oranges OK

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index d2e519259c63d678900c6dbdcb7a0d60c4b77b9d..7f438c62cf52d9d44fab8e3df6674c4c85d8a455 100644 (file)
--- 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;
        }
 }