robofsm/fsmact.c: first version of load_the_puck sequence; test_actuators utility...

diff --git a/src/robofsm/fsmact.c b/src/robofsm/fsmact.c
index 8293cea8c3842044155f378a10eea9ed6da623a7..12b78f52facb80fce2627084458c6bf894248c0a 100644 (file)
--- a/src/robofsm/fsmact.c
+++ b/src/robofsm/fsmact.c
@@ -15,7 +15,7 @@
  *  Actuator's variables
  ************************************************************/
 
-int pucks_in_holder = 0;
+int pucks_loaded = 0;
 bool lintel_present = 1;
 
 /************************************************************
@@ -24,14 +24,15 @@ bool lintel_present = 1;
 
 FSM_STATE_DECL(wait_for_command);
 
-FSM_STATE_DECL(reactive_demo);
-FSM_STATE_DECL(look_for_puck);
+FSM_STATE_DECL(load_the_puck);
+FSM_STATE_DECL(suck_the_puck);
+FSM_STATE_DECL(store_pucks_in_holder);
+
 FSM_STATE_DECL(dummy_action);
 
 FSM_STATE_DECL(grasp_the_puck);
 FSM_STATE_DECL(hold_the_puck);
 
-FSM_STATE_DECL(suck_the_puck);
 
 /* ***********************************************************
  *  STATE DEFINITIONS
@@ -41,6 +42,7 @@ FSM_STATE(wait_for_command)
 {
        switch (FSM_EVENT) {
        case EV_ENTRY:
+               printf("wait_for_command_state_entered\n");
                // put all actuators to defined initial positions
                act_belts(BELTS_OFF, BELTS_OFF);
                act_chelae(CHELA_OPEN, CHELA_OPEN);
@@ -53,7 +55,7 @@ FSM_STATE(wait_for_command)
        case EV_PUSHER_IN_POS:
                break;
        case EV_LOAD_THE_PUCK:
-               FSM_TRANSITION(dummy_action);
+               FSM_TRANSITION(load_the_puck);
                break;
        case EV_GRASP_THE_PUCK:
                FSM_TRANSITION(dummy_action);
@@ -74,27 +76,29 @@ FSM_STATE(wait_for_command)
        }
 }
 
-FSM_STATE(grasp_the_puck)
+/************************************************************
+ * LOAD THE PUCK ACTION
+ ************************************************************/
+
+FSM_STATE(load_the_puck)
 {
        switch (FSM_EVENT) {
        case EV_ENTRY:
-               act_belts(BELTS_IN, BELTS_IN);
-               act_chelae(CHELA_TIGHT, CHELA_TIGHT);
-               FSM_TIMER(2000);
-               break;
-       case EV_PUCK_INSIDE:
-               FSM_TIMER_STOP();
+               printf("load_the_puck_state entered\n");
+               act_lift(LIFT_GROUND_POS);
+               act_pusher(PUSHER_INSIDE);
+               act_chelae(CHELA_OPEN, CHELA_OPEN);
                act_belts(BELTS_OFF, BELTS_OFF);
-               FSM_SIGNAL(MAIN, EV_ACTION_DONE, NULL);
-               FSM_TRANSITION(hold_the_puck);
+               // FIXME: act_holder ?
                break;
-       case EV_TIMER:
-               FSM_SIGNAL(MAIN, EV_ACTION_ERROR, NULL);
-               FSM_TRANSITION(wait_for_command);
+       case EV_LIFT_IN_POS:
+               printf("lift in position\n");
+               FSM_TRANSITION(suck_the_puck);
                break;
+       case EV_PUCK_INSIDE:
        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:
@@ -105,28 +109,72 @@ FSM_STATE(grasp_the_puck)
        }
 }
 
-FSM_STATE(hold_the_puck)
+FSM_STATE(suck_the_puck)
 {
        switch (FSM_EVENT) {
        case EV_ENTRY:
+               printf("suck_the_puck state entered\n");
+               FSM_TIMER(1000);
+               act_chelae(CHELA_TIGHT, CHELA_TIGHT);
+               act_belts(BELTS_IN, BELTS_IN);
                break;
        case EV_PUCK_INSIDE:
+               printf("puck inside\n");
+               act_belts(BELTS_OFF, BELTS_OFF);
+               act_chelae(CHELA_OPEN, CHELA_OPEN);
+               act_holder(HOLDER_OPENED);
+               FSM_TRANSITION(store_pucks_in_holder);
                break;
+       case EV_TIMER:
+               break; // FIXME handle this...
+       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:
-               act_belts(BELTS_OUT+20, BELTS_OUT+20);
-               act_chelae(CHELA_TIGHT, CHELA_TIGHT);
-               FSM_TIMER(1100);
+               DBG_PRINT_EVENT("unhandled event");
                break;
-       case EV_TIMER:
+       case EV_EXIT:
                act_belts(BELTS_OFF, BELTS_OFF);
-               act_chelae(CHELA_LOOSE, CHELA_LOOSE);
-               FSM_TRANSITION(wait_for_command);
-               //DBG_PRINT_EVENT("performing the transition to wait_for_command");
+               act_chelae(CHELA_OPEN, CHELA_OPEN);
+               break;
+       }
+}
+
+FSM_STATE(store_pucks_in_holder)
+{
+       static int i;
+       switch (FSM_EVENT) {
+       case EV_ENTRY:
+               if (pucks_loaded > 2) {
+                       puck_loaded++; // FIXME: (?) change this variable only here in this state?
+                       FSM_TRANSITION(wait_for_command);
+               } else {
+                       i = 0;
+                       act_holder(HOLDER_OPENED);
+                       act_lift(LIFT_IN_HOLDER_POS);
+               }
                break;
-       case EV_RETURN:
-       case EV_LOAD_THE_PUCK:
        case EV_LIFT_IN_POS:
+               switch(i) {
+               case 0:
+                       act_holder(HOLDER_TIGHT);
+                       act_lift(LIFT_TRAVEL_POS);
+                       break;
+               case 1:
+                       pucks_loaded++; // FIXME: (?) change this variable in EV_ENTRY and only in one place?
+                       FSM_TRANSITION(wait_for_command);
+                       FSM_SIGNAL(MAIN, EV_ACTION_DONE,(void*)pucks_loaded);
+               }
+               i++;
+               break;
+       case EV_PUCK_INSIDE:
+       case EV_UNLOAD_THE_PUCK:
+       case EV_TIMER:
+       case EV_RETURN:
        case EV_PUSHER_IN_POS:
+       case EV_LOAD_THE_PUCK:
        case EV_GRASP_THE_PUCK:
                DBG_PRINT_EVENT("unhandled event");
                break;
@@ -135,37 +183,75 @@ FSM_STATE(hold_the_puck)
        }
 }
 
-FSM_STATE(suck_the_puck)
+/************************************************************
+ * GRASP ACTION
+ ************************************************************/
+
+FSM_STATE(grasp_the_puck)
 {
        switch (FSM_EVENT) {
        case EV_ENTRY:
-               FSM_TIMER(1000);
-               act_chelae(CHELA_TIGHT, CHELA_TIGHT);
                act_belts(BELTS_IN, BELTS_IN);
+               act_chelae(CHELA_TIGHT, CHELA_TIGHT);
+               FSM_TIMER(2000);
                break;
        case EV_PUCK_INSIDE:
+               FSM_TIMER_STOP();
                act_belts(BELTS_OFF, BELTS_OFF);
                FSM_SIGNAL(MAIN, EV_ACTION_DONE, NULL);
+               FSM_TRANSITION(hold_the_puck);
+               break;
+       case EV_TIMER:
+               FSM_SIGNAL(MAIN, EV_ACTION_ERROR, NULL);
                FSM_TRANSITION(wait_for_command);
                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");
+               break;
+       case EV_EXIT:
+               break;
+       }
+}
+
+FSM_STATE(hold_the_puck)
+{
+       switch (FSM_EVENT) {
+       case EV_ENTRY:
+               break;
+       case EV_PUCK_INSIDE:
+               break;
+       case EV_UNLOAD_THE_PUCK:
+               act_belts(BELTS_OUT+20, BELTS_OUT+20);
+               act_chelae(CHELA_TIGHT, CHELA_TIGHT);
+               FSM_TIMER(1100);
+               break;
        case EV_TIMER:
+               act_belts(BELTS_OFF, BELTS_OFF);
+               act_chelae(CHELA_LOOSE, CHELA_LOOSE);
                FSM_TRANSITION(wait_for_command);
+               //DBG_PRINT_EVENT("performing the transition to wait_for_command");
                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_belts(BELTS_OFF, BELTS_OFF);
-               act_chelae(CHELA_OPEN, CHELA_OPEN);
                break;
        }
 }
 
+/************************************************************
+ * DUMMY ACTION
+ ************************************************************/
+
 FSM_STATE(dummy_action)
 {
        switch (FSM_EVENT) {

diff --git a/src/robofsm/test/test_actuators.cc b/src/robofsm/test/test_actuators.cc
index 9e1b9fc76784c857b396355f724feb7ba5007020..12461e3e8cee3f6472c6b1427b84396145afcfe5 100644 (file)
--- a/src/robofsm/test/test_actuators.cc
+++ b/src/robofsm/test/test_actuators.cc
@@ -114,6 +114,11 @@ void * handle_keyboard(void *)
                        printf("Sending 'EV_GRASP_THE_PUCK' event to the act fsm\n");
                        FSM_SIGNAL(ACT, EV_GRASP_THE_PUCK, NULL);
                        break;
+               case 'l' :
+               case 'L' :
+                       printf("Sending 'EV_LOAD_THE_PUCK' event to the act fsm\n");
+                       FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
+                       break;
                case 'u' : 
                case 'U' :
                        printf("Sending 'EV_UNLOAD_THE_PUCK' event to the act fsm\n");
@@ -152,7 +157,7 @@ int main()
 
        robot.fsm.main.debug_states = 0;
        robot.fsm.motion.debug_states = 0;
-       robot.fsm.act.debug_states = 1;
+       robot.fsm.act.debug_states = 0;
 
        robot.fsm.main.state = &fsm_state_main_just_receive_events;