ACT FSM: added hokuyo initialization (temporarily)

diff --git a/src/robofsm/fsmact.c b/src/robofsm/fsmact.c
index 62890e4c7f6ee6dba1bd4073d025ffd7c91ff67a..8293cea8c3842044155f378a10eea9ed6da623a7 100644 (file)
--- a/src/robofsm/fsmact.c
+++ b/src/robofsm/fsmact.c
@@ -41,20 +41,23 @@ FSM_STATE(wait_for_command)
 {
        switch (FSM_EVENT) {
        case EV_ENTRY:
+               // put all actuators to defined initial positions
                act_belts(BELTS_OFF, BELTS_OFF);
                act_chelae(CHELA_OPEN, CHELA_OPEN);
-               act_holder(HOLDER_OPENED);
-               act_lift(100);
-               act_pusher(100);
-               // TODO
-               // put all actuators to defined initial positions
+               act_holder(HOLDER_LOOSE);
+               act_lift(LIFT_TRAVEL_POS);
+               act_pusher(PUSHER_INSIDE);
+               act_hokuyo(HOKUYO_PITCH_HORIZONTAL);
+               break;
+       case EV_LIFT_IN_POS:
+       case EV_PUSHER_IN_POS:
                break;
        case EV_LOAD_THE_PUCK:
                FSM_TRANSITION(dummy_action);
                break;
        case EV_GRASP_THE_PUCK:
-               //FSM_TRANSITION(dummy_action);
-               FSM_TRANSITION(grasp_the_puck);
+               FSM_TRANSITION(dummy_action);
+               //FSM_TRANSITION(grasp_the_puck);
                break;
        case EV_UNLOAD_THE_PUCK:
                FSM_TRANSITION(dummy_action);
@@ -91,6 +94,8 @@ FSM_STATE(grasp_the_puck)
                break;
        case EV_UNLOAD_THE_PUCK:
        case EV_RETURN:
+       case EV_LIFT_IN_POS:
+       case EV_PUSHER_IN_POS:
        case EV_LOAD_THE_PUCK:
        case EV_GRASP_THE_PUCK:
                DBG_PRINT_EVENT("unhandled event");
@@ -120,6 +125,8 @@ FSM_STATE(hold_the_puck)
                break;
        case EV_RETURN:
        case EV_LOAD_THE_PUCK:
+       case EV_LIFT_IN_POS:
+       case EV_PUSHER_IN_POS:
        case EV_GRASP_THE_PUCK:
                DBG_PRINT_EVENT("unhandled event");
                break;
@@ -146,6 +153,8 @@ FSM_STATE(suck_the_puck)
                FSM_TRANSITION(wait_for_command);
                break;
        case EV_LOAD_THE_PUCK:
+       case EV_LIFT_IN_POS:
+       case EV_PUSHER_IN_POS:
        case EV_GRASP_THE_PUCK:
        case EV_UNLOAD_THE_PUCK:
                DBG_PRINT_EVENT("unhandled event");
@@ -161,6 +170,7 @@ FSM_STATE(dummy_action)
 {
        switch (FSM_EVENT) {
        case EV_ENTRY:
+               act_hokuyo(HOKUYO_PITCH_MAX);
                FSM_TIMER(2000);
                break;
        case EV_TIMER:
@@ -171,11 +181,14 @@ FSM_STATE(dummy_action)
                break;
        case EV_RETURN:
        case EV_LOAD_THE_PUCK:
+       case EV_LIFT_IN_POS:
+       case EV_PUSHER_IN_POS:
        case EV_GRASP_THE_PUCK:
        case EV_UNLOAD_THE_PUCK:
                DBG_PRINT_EVENT("unhandled event");
                break;
        case EV_EXIT:
+               act_hokuyo(HOKUYO_PITCH_HORIZONTAL);
                break;
        }
 }
@@ -190,6 +203,8 @@ FSM_STATE()
        case EV_UNLOAD_THE_PUCK:
        case EV_TIMER:
        case EV_RETURN:
+       case EV_LIFT_IN_POS:
+       case EV_PUSHER_IN_POS:
        case EV_LOAD_THE_PUCK:
        case EV_GRASP_THE_PUCK:
                DBG_PRINT_EVENT("unhandled event");