From: Filip Jares Date: Fri, 24 Apr 2009 19:08:08 +0000 (+0200) Subject: fsmact: USE_DUMMY_ACTIONS and GIVE_UO_ON_PUCK_PRESENCE_SENSOR #defines added X-Git-Tag: eb2009~22^2~9 X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/9851629bd942c9541e82ad5928fb27baa8cda6c7 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 f7352e69..daf2818c 100644 --- a/src/robofsm/fsmact.c +++ b/src/robofsm/fsmact.c @@ -5,6 +5,9 @@ #include "actuators.h" #include +#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;