Merge branch 'master' of rtime.felk.cvut.cz:/var/git/eurobot

[eurobot/public.git] / src / robofsm / common-states.cc

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index b9c2d6d7824147e0216bb904b614b075091479f2..b63b41a7302730f2425e22eb4321f7505cc9985e 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -34,7 +34,9 @@ static void set_initial_position()
 
 static void actuators_home()
 {
-       act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
+       static int tmp = 0;
+       act_vidle(VIDLE_UP - tmp, VIDLE_FAST_SPEED);
+       tmp = 1 - tmp;          // Force movement (we need to change the target position)
 }
 
 void start_entry()
@@ -65,6 +67,7 @@ void start_go()
 void start_exit()
 {
        robot.corns = get_all_corns(robot.corns_conf_side, robot.corns_conf_center);
+       act_camera_off();
 }
 
 /************************************************************************
@@ -156,7 +159,7 @@ FSM_STATE(climb_the_slope)
                case EV_ENTRY: {
                                // disables using side switches on bumpers when going up
                                enable_switches(false);
-                               act_vidle(VIDLE_DOWN, VIDLE_FAST_SPEED);
+                               act_vidle(VIDLE_LOAD_PREPARE, VIDLE_FAST_SPEED);
                                robot.ignore_hokuyo = true;
                                /* create the trajectory and go */
                                robot_trajectory_new_backward(&tcSlow);
@@ -196,23 +199,44 @@ FSM_STATE(climb_the_slope)
        }
 }
 
-/* one-state-subautomaton to load oranges in two stages */
+/* subautomaton to load oranges in two stages */
+FSM_STATE_DECL(load_oranges2);
 FSM_STATE(load_oranges)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
-                       FSM_TIMER(500);
                        act_vidle(VIDLE_MIDDLE, VIDLE_MEDIUM_SPEED);
                        break;
+               case EV_VIDLE_DONE:
+                       FSM_TIMER(500);
+                       break;
                case EV_TIMER:
-                       // FIXME: respond to VIDLE EVENT
+                       FSM_TRANSITION(load_oranges2);
+                       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:
+                       break;
+       }
+}
+
+FSM_STATE(load_oranges2)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
                        act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
+                       break;
+               case EV_VIDLE_DONE:
                        SUBFSM_RET(NULL);
                        break;
+               case EV_TIMER:
                case EV_MOTION_DONE:
                case EV_START:
                case EV_RETURN:
-               case EV_VIDLE_DONE:
                case EV_MOTION_ERROR:
                case EV_SWITCH_STRATEGY:
                        DBG_PRINT_EVENT("unhandled event");