fsmact: USE_DUMMY_ACTIONS and GIVE_UO_ON_PUCK_PRESENCE_SENSOR #defines added

- to decide on whether to use puck presence sensor or not and
- to decide on whether to use real actuators or timer-based dummy actions at compile time

MAIN FSM:
- look around for puck action commented out, updating the code to take
  into account that there is no sharp sensor present now

diff --git a/src/robofsm/fsmact.c b/src/robofsm/fsmact.c
index f7352e69d4e3c63081df66744aca554f44b3bff4..daf2818c6425128e1efb6f5b5e44f91aa5acc38e 100644 (file)
--- a/src/robofsm/fsmact.c
+++ b/src/robofsm/fsmact.c
@@ -5,6 +5,9 @@
 #include "actuators.h"
 #include <stdbool.h>
 
+#define USE_DUMMY_ACTIONS // if defined, simulate actuators' actions by timeouts only
+//#define GIVE_UP_ON_PUCK_PRESENCE_SENSOR
+
 /************************************************************
  *  Actuator's constants
  ************************************************************/
@@ -21,7 +24,8 @@ typedef enum {
 } BASIC_LIFT_POSITION;
 
 BASIC_LIFT_POSITION basic_lift_position = HIGH;
-int pucks_loaded = 0;
+
+// number of pucks loaded is stored in robot.pucks_inside variable
 bool lintel_present = 1;
 int floor_to_unload;
 
@@ -29,6 +33,8 @@ int floor_to_unload;
  *  STATE DECLARATIONS
  ************************************************************/
 
+FSM_STATE_DECL(init);
+
 FSM_STATE_DECL(wait_for_command);
 
 FSM_STATE_DECL(prepare_for_load);
@@ -40,23 +46,50 @@ FSM_STATE_DECL(store_pucks_in_holder);
 FSM_STATE_DECL(unload_pucks);
 FSM_STATE_DECL(drive_lift_into_floor);
 
+FSM_STATE_DECL(dummy_action);
+FSM_STATE_DECL(dummy_pucks_unload);
+
 /* ***********************************************************
  *  STATE DEFINITIONS
  * ***********************************************************/
 
+FSM_STATE(init)
+{
+       switch (FSM_EVENT) {
+       case EV_ENTRY:
+               robot.pucks_inside = 0;
+               FSM_TRANSITION(wait_for_command);
+               break;
+       case EV_PUCK_INSIDE:
+       case EV_PREPARE_THE_UNLOAD:
+       case EV_UNLOAD_PUCKS:
+       case EV_TIMER:
+       case EV_RETURN:
+       case EV_LIFT_IN_POS:
+       case EV_PUSHER_IN_POS:
+       case EV_LOAD_THE_PUCK:
+       case EV_FREE_SPACE:
+       case EV_SCRABBLE:
+               DBG_PRINT_EVENT("unhandled event");
+               break;
+       case EV_EXIT:
+               break;
+       }
+}
+
 FSM_STATE(wait_for_command)
 {
        switch (FSM_EVENT) {
        case EV_ENTRY:
-               printf("wait_for_command_state_entered, number of pucks stored: %d\n", pucks_loaded);
+               printf("wait_for_command_state_entered, number of pucks stored: %d\n", robot.pucks_inside);
                // put all actuators to defined initial positions
                act_belts(BELTS_OFF, BELTS_OFF);
-               if (pucks_loaded<4) {
+               if (robot.pucks_inside<4) {
                        act_chelae(CHELA_OPEN, CHELA_OPEN);
                } else {
                        act_chelae(CHELA_TIGHT, CHELA_TIGHT);
                }
-               act_holder(HOLDER_LOOSE);
+               act_holder(HOLDER_TIGHT); //act_holder(HOLDER_LOOSE);
                act_lift(basic_lift_position);
                switch(basic_lift_position) {
                case (LOW):
@@ -69,22 +102,30 @@ FSM_STATE(wait_for_command)
                act_pusher(PUSHER_INSIDE);
                act_hokuyo(HOKUYO_PITCH_HORIZONTAL);
                break;
-       case EV_PREPARE_FOR_LOAD:
-               //basic_lift_position = LOW;
-               //FSM_TRANSITION(wait_for_command);
-               break;
        case EV_SCRABBLE:
+#ifdef USE_DUMMY_ACTIONS
+               FSM_TRANSITION(dummy_action);
+#else
                FSM_TRANSITION(scrabble);
+#endif
                break;
        case EV_LOAD_THE_PUCK:
+#ifdef USE_DUMMY_ACTIONS
+               FSM_TRANSITION(dummy_action);
+#else
                FSM_TRANSITION(load_the_puck);
+#endif
                break;
        case EV_PREPARE_THE_UNLOAD:
-               if (pucks_loaded == 0) {
+               if (robot.pucks_inside == 0) {
                        FSM_SIGNAL(MAIN, EV_ACTION_ERROR, NULL);
                } else {
                        floor_to_unload = FSM_EVENT_INT;
+#ifdef USE_DUMMY_ACTIONS
+                       FSM_TRANSITION(dummy_pucks_unload);
+#else
                        FSM_TRANSITION(unload_pucks);
+#endif
                }
                break;
        case EV_LIFT_IN_POS:
@@ -92,9 +133,9 @@ FSM_STATE(wait_for_command)
        case EV_PUCK_INSIDE:     // we do not want react unless MAIN FSM tells us to
                break;
        case EV_UNLOAD_PUCKS:
+       case EV_TIMER:
        case EV_RETURN:
        case EV_FREE_SPACE:
-       case EV_TIMER:
                DBG_PRINT_EVENT("unhandled event");
                break;
        case EV_EXIT:
@@ -122,7 +163,6 @@ FSM_STATE(wait_for_command)
                FSM_TRANSITION(load_the_puck);
                break;
        case EV_PUCK_INSIDE:
-       case EV_PREPARE_FOR_LOAD:
        case EV_PREPARE_THE_UNLOAD:
        case EV_UNLOAD_PUCKS:
        case EV_TIMER:
@@ -147,7 +187,6 @@ FSM_STATE(scrabble)
                break;
        case EV_SCRABBLE:
        case EV_PUCK_INSIDE:
-       case EV_PREPARE_FOR_LOAD:
        case EV_PREPARE_THE_UNLOAD:
        case EV_UNLOAD_PUCKS:
        case EV_TIMER:
@@ -167,11 +206,13 @@ FSM_STATE(load_the_puck)
        switch (FSM_EVENT) {
        case EV_ENTRY:
                printf("load_the_puck_state entered\n");
-               act_lift(LIFT_GROUND_POS);
+               act_lift(LIFT_MIN);
                act_pusher(PUSHER_INSIDE);
                act_chelae(CHELA_OPEN, CHELA_OPEN);
                act_belts(BELTS_OFF, BELTS_OFF);
-               // FIXME: act_holder ?
+               if (robot.pucks_inside > 0) {
+                       act_holder(HOLDER_TIGHT);
+               }
                break;
        case EV_LIFT_IN_POS:
                printf("lift in position\n");
@@ -184,7 +225,6 @@ FSM_STATE(load_the_puck)
        case EV_RETURN:
        case EV_PUSHER_IN_POS:
        case EV_FREE_SPACE:
-       case EV_PREPARE_FOR_LOAD:
        case EV_LOAD_THE_PUCK:
        case EV_SCRABBLE:
                DBG_PRINT_EVENT("unhandled event");
@@ -199,44 +239,45 @@ FSM_STATE(suck_the_puck)
        static int timer_arrival_count;
        switch (FSM_EVENT) {
        case EV_ENTRY:
-               act_lift(LIFT_MIN);
-       case EV_LIFT_IN_POS:
+               // we expect lift is on the ground
                timer_arrival_count = 0;
                printf("suck_the_puck state entered\n");
-               FSM_TIMER(800);
-               //FSM_TIMER(2000);
+               FSM_TIMER(2000);
                act_chelae(CHELA_TIGHT, CHELA_TIGHT);
                act_belts(BELTS_IN, BELTS_IN);
                break;
        case EV_PUCK_INSIDE:
+#ifdef GIVE_UP_ON_PUCK_PRESENCE_SENSOR
        case EV_TIMER:
-               printf("puck inside\n");
+               FSM_TRANSITION(store_pucks_in_holder);
+#else          
                FSM_TRANSITION(store_pucks_in_holder);
                break;
-       /*case EV_TIMER:
+       case EV_TIMER:
+               timer_arrival_count++;
                switch (timer_arrival_count) {
-               case 0:
+               case 1:
                        act_belts(BELTS_OUT, BELTS_OUT);
                        FSM_TIMER(1000);
                        break;
-               case 1:
+               case 2:
                        act_belts(BELTS_IN, BELTS_IN);
                        FSM_TIMER(2000);
                        break;
-               case 2:
+               case 3:
                        FSM_SIGNAL(MAIN, EV_ACTION_ERROR, NULL);
                        FSM_TRANSITION(wait_for_command);
                        break;
                }
-               timer_arrival_count++; */
+#endif
                break;
        case EV_RETURN:
        case EV_LOAD_THE_PUCK:
+       case EV_LIFT_IN_POS:
        case EV_PUSHER_IN_POS:
        case EV_PREPARE_THE_UNLOAD:
        case EV_UNLOAD_PUCKS:
        case EV_FREE_SPACE:
-       case EV_PREPARE_FOR_LOAD:
        case EV_SCRABBLE:
                DBG_PRINT_EVENT("unhandled event");
                break;
@@ -253,11 +294,11 @@ FSM_STATE(store_pucks_in_holder)
        switch (FSM_EVENT) {
        case EV_ENTRY:
                act_holder(HOLDER_OPENED);
-               if (pucks_loaded > 2) {
-                       pucks_loaded++; // FIXME: (?) change this variable only here in this state?
+               if (robot.pucks_inside > 2) {
+                       robot.pucks_inside++; // FIXME: (?) change this variable only here in this state?
                        basic_lift_position = HIGH;
                        FSM_TRANSITION(wait_for_command);
-                       FSM_SIGNAL(MAIN, EV_ACTION_DONE, (void*)pucks_loaded);
+                       FSM_SIGNAL(MAIN, EV_ACTION_DONE, NULL);
                } else {
                        i = 0;
                        act_holder(HOLDER_OPENED);
@@ -271,10 +312,10 @@ FSM_STATE(store_pucks_in_holder)
                        act_lift(LIFT_TRAVEL_POS);
                        break;
                case 1:
-                       pucks_loaded++; // FIXME: (?) change this variable in EV_ENTRY and only in one place?
+                       robot.pucks_inside++; // FIXME: (?) change this variable in EV_ENTRY and only in one place?
                        basic_lift_position = HIGH;
                        FSM_TRANSITION(wait_for_command);
-                       FSM_SIGNAL(MAIN, EV_ACTION_DONE, (void*)pucks_loaded);
+                       FSM_SIGNAL(MAIN, EV_ACTION_DONE, NULL);
                }
                i++;
                break;
@@ -287,7 +328,6 @@ FSM_STATE(store_pucks_in_holder)
        case EV_PUSHER_IN_POS:
        case EV_LOAD_THE_PUCK:
        case EV_FREE_SPACE:
-       case EV_PREPARE_FOR_LOAD:
        case EV_SCRABBLE:
                DBG_PRINT_EVENT("unhandled event");
                break;
@@ -306,7 +346,7 @@ FSM_STATE(unload_pucks)
        case EV_ENTRY:
                act_chelae(CHELA_OPEN, CHELA_OPEN);
                printf("unloading pucks\n");
-               if (pucks_loaded < 4) {
+               if (robot.pucks_inside < 4) {
                        act_lift(LIFT_BELOW_HOLDER_POS);
                } else {
                        // if there are 4 pucks loaded, we do not need to manipulate the lift
@@ -326,7 +366,6 @@ FSM_STATE(unload_pucks)
        case EV_PUSHER_IN_POS:
        case EV_LOAD_THE_PUCK:
        case EV_FREE_SPACE:
-       case EV_PREPARE_FOR_LOAD:
        case EV_SCRABBLE:
                DBG_PRINT_EVENT("unhandled event");
                break;
@@ -363,7 +402,7 @@ FSM_STATE(drive_lift_into_floor)
                break;
        case EV_UNLOAD_PUCKS:
                printf("using pusher\n");
-               pucks_loaded = 0;
+               robot.pucks_inside = 0;
                act_pusher(PUSHER_FULLY_OUTSIDE);
                break;
        case EV_PUSHER_IN_POS:
@@ -380,7 +419,75 @@ FSM_STATE(drive_lift_into_floor)
        case EV_TIMER:
        case EV_RETURN:
        case EV_LOAD_THE_PUCK:
-       case EV_PREPARE_FOR_LOAD:
+       case EV_SCRABBLE:
+               DBG_PRINT_EVENT("unhandled event");
+               break;
+       case EV_EXIT:
+               break;
+       }
+}
+
+/************************************************************
+ * GENERIC DUMMY ACTION
+ ************************************************************/
+
+FSM_STATE(dummy_action)
+{
+       switch (FSM_EVENT) {
+       case EV_ENTRY:
+               FSM_TIMER(1000);
+               break;
+       case EV_TIMER:
+               FSM_SIGNAL(MAIN, EV_ACTION_DONE, NULL);
+               FSM_TRANSITION(wait_for_command);
+               break;
+       case EV_PUCK_INSIDE:
+       case EV_PREPARE_THE_UNLOAD:
+       case EV_UNLOAD_PUCKS:
+       case EV_RETURN:
+       case EV_LIFT_IN_POS:
+       case EV_PUSHER_IN_POS:
+       case EV_LOAD_THE_PUCK:
+       case EV_FREE_SPACE:
+       case EV_SCRABBLE:
+               DBG_PRINT_EVENT("unhandled event");
+               break;
+       case EV_EXIT:
+               break;
+       }
+}
+
+/************************************************************
+ * DUMMY UNLOAD ACTION
+ ************************************************************/
+
+FSM_STATE(dummy_pucks_unload)
+{
+       static int dummy_unload_status;
+       switch (FSM_EVENT) {
+       case EV_ENTRY:
+               dummy_unload_status = 0;
+               printf("dummy: driving lift to floor %d\n", floor_to_unload);
+               FSM_TIMER(3000);
+               break;
+       case EV_TIMER:
+               FSM_SIGNAL(MAIN, EV_ACTION_DONE, NULL);
+               dummy_unload_status++;
+               break;
+       case EV_UNLOAD_PUCKS:
+               if (dummy_unload_status == 1)
+                       FSM_TIMER(2000);
+               break;
+       case EV_FREE_SPACE:
+               if (dummy_unload_status == 2)
+                       FSM_TIMER(1000);
+               break;
+       case EV_LIFT_IN_POS:
+       case EV_PUSHER_IN_POS:
+       case EV_PUCK_INSIDE:
+       case EV_PREPARE_THE_UNLOAD:
+       case EV_RETURN:
+       case EV_LOAD_THE_PUCK:
        case EV_SCRABBLE:
                DBG_PRINT_EVENT("unhandled event");
                break;
@@ -404,7 +511,6 @@ FSM_STATE()
        case EV_PUSHER_IN_POS:
        case EV_LOAD_THE_PUCK:
        case EV_FREE_SPACE:
-       case EV_PREPARE_FOR_LOAD:
        case EV_SCRABBLE:
                DBG_PRINT_EVENT("unhandled event");
                break;