From d7f00da54371f98f3ceab047485a3467d6024fbc Mon Sep 17 00:00:00 2001 From: Filip Jares Date: Fri, 24 Apr 2009 21:10:53 +0200 Subject: [PATCH] warnings in homologation.cc avoided MAIN FSM: - look around for puck action commented out, updating the code to take into account that there is no sharp sensor present now EV_PREPARE_FOR_LOAD action removed --- src/robofsm/fsmmain.c | 61 +++++++++++++++++-------------------- src/robofsm/homologation.cc | 3 ++ src/robofsm/roboevent.py | 1 - 3 files changed, 31 insertions(+), 34 deletions(-) diff --git a/src/robofsm/fsmmain.c b/src/robofsm/fsmmain.c index 62efebb4..2a047a65 100644 --- a/src/robofsm/fsmmain.c +++ b/src/robofsm/fsmmain.c @@ -48,8 +48,8 @@ typedef enum { LOAD_SUCCESS = 0, LOAD_FAIL, - LOOK_AROUND_SUCCESS, - LOOK_AROUND_FAIL + //LOOK_AROUND_SUCCESS, // obsolete, no sharp sensor + //LOOK_AROUND_FAIL // obsolete, no sharp sensor } SUBFSM_RET_VAL; #define FSM_EVENT_RET_VAL ((SUBFSM_RET_VAL)FSM_EVENT_PTR) @@ -61,10 +61,10 @@ typedef enum { struct TrajectoryConstraints tcFast, tcSlow; /************************************************************************ - * Trajectory constraints used, are initialized in the init state + * Variables related to puck collecting ************************************************************************/ -int free_puck_to_get_next = 0; // next free puck number (index of the free_puck_pick_up_sequence array) to pick up +int free_puck_to_try_to_get_next = 0; // next free puck number (index of the free_puck_pick_up_sequence array) to pick up /************************************************************************ * Definition of particular "free pucks pick up sequences" @@ -358,53 +358,39 @@ static void robot_goto_next_puck_in_sequence(int lot, int puck_number) FSM_STATE(collect_free_pucks) { static const int lot = 7; // this variable location is temporary...; going to be received from the camera - static int puck_approach_attempt_no; switch (FSM_EVENT) { case EV_ENTRY: - puck_approach_attempt_no = 0; - FSM_SIGNAL(ACT, EV_PREPARE_FOR_LOAD, NULL); - robot_goto_next_puck_in_sequence(lot, free_puck_to_get_next); + robot_goto_next_puck_in_sequence(lot, free_puck_to_try_to_get_next); break; case EV_MOTION_DONE: { - printf("-----arrived where the free puck no. %d should be\n", free_puck_to_get_next); - if (PUCK_REACHABLE(robot.puck_distance)) { - SUBFSM_TRANSITION(load_the_puck, NULL); - } else { - switch (puck_approach_attempt_no) { - case 0: - printf("-----moving forward by 5 cm\n"); - robot_move_by(0.05, NO_TURN(), &tcSlow); - puck_approach_attempt_no++; - break; - case 1: - SUBFSM_TRANSITION(look_around_for_puck, NULL); - break; - } - } + printf("-----arrived where the free puck no. %d should be\n", free_puck_to_try_to_get_next); + SUBFSM_TRANSITION(load_the_puck, NULL); } break; case EV_RETURN: switch(FSM_EVENT_RET_VAL) { + /* obsolete, no sharp sensor present case LOOK_AROUND_SUCCESS: // FIXME: test number of pucks loaded!! printf(">>>>>> Look around succeeded\n"); SUBFSM_TRANSITION(load_the_puck, NULL); - break; + break; */ case LOAD_SUCCESS: printf(">>>>>> Loading the puck succeeded\n"); - if(free_puck_to_get_next<4) { - free_puck_to_get_next++; - robot_goto_next_puck_in_sequence(lot, free_puck_to_get_next); + if(free_puck_to_try_to_get_next<4) { + free_puck_to_try_to_get_next++; + robot_goto_next_puck_in_sequence(lot, free_puck_to_try_to_get_next); } - puck_approach_attempt_no = 0; break; + /* obsolete, no sharp sensor present case LOOK_AROUND_FAIL: printf(">>>>>> Looking around for the puck FAILED\n"); break; // FIXME: remove the break + */ case LOAD_FAIL: printf(">>>>>> Loading the puck FAILED\n"); - if(free_puck_to_get_next<6) { // FIXME: test number of pucks loaded!! - free_puck_to_get_next++; - robot_goto_next_puck_in_sequence(lot, free_puck_to_get_next); + if(free_puck_to_try_to_get_next<6) { // FIXME: test number of pucks loaded!! + free_puck_to_try_to_get_next++; + robot_goto_next_puck_in_sequence(lot, free_puck_to_try_to_get_next); } else { // FIXME (TODO): transition to next strategy state } @@ -413,7 +399,7 @@ FSM_STATE(collect_free_pucks) break; case EV_PUCK_REACHABLE: robot_stop(); - printf("-----reached some free puck, hopefully no. %d\n", free_puck_to_get_next); + printf("-----reached some free puck, hopefully no. %d\n", free_puck_to_try_to_get_next); SUBFSM_TRANSITION(load_the_puck, NULL); break; case EV_ACTION_DONE: @@ -547,8 +533,10 @@ FSM_STATE(approach_opponents_static_dispenser) FSM_STATE(load_the_puck) { + static int puck_load_attempt_count; switch (FSM_EVENT) { case EV_ENTRY: + puck_load_attempt_count = 0; FSM_SIGNAL(ACT, EV_SCRABBLE, NULL); FSM_TIMER(200); break; @@ -562,7 +550,12 @@ FSM_STATE(load_the_puck) SUBFSM_RET((void *)LOAD_SUCCESS); break; case EV_ACTION_ERROR: - SUBFSM_RET((void *)LOAD_FAIL); + puck_load_attempt_count++; + if (puck_load_attempt_count > 2) { + SUBFSM_RET((void *)LOAD_FAIL); + } else { + robot_move_by(0.02, NO_TURN(), &tcSlow); + } break; case EV_RETURN: case EV_LASER_POWER: @@ -578,6 +571,7 @@ FSM_STATE(load_the_puck) } } +/* of no use without the sharp sensor measuring "puck distance" FSM_STATE(look_around_for_puck) { static int lfp_status = 0; @@ -630,6 +624,7 @@ FSM_STATE(look_around_for_puck) break; } } +*/ /* diff --git a/src/robofsm/homologation.cc b/src/robofsm/homologation.cc index e86385ef..09c324b0 100644 --- a/src/robofsm/homologation.cc +++ b/src/robofsm/homologation.cc @@ -72,6 +72,8 @@ FSM_STATE(wait_for_start) #endif switch (FSM_EVENT) { #ifdef WAIT_FOR_START + case EV_ENTRY: + break case EV_START: { pthread_t thid; @@ -80,6 +82,7 @@ FSM_STATE(wait_for_start) pthread_create(&thid, NULL, wait_to_deposition, NULL); } #else + case EV_START: case EV_ENTRY: #endif robot_set_est_pos_trans(0.16, diff --git a/src/robofsm/roboevent.py b/src/robofsm/roboevent.py index 20ba59af..8d1852df 100644 --- a/src/robofsm/roboevent.py +++ b/src/robofsm/roboevent.py @@ -37,7 +37,6 @@ events = { "disp" : { }, "act" : { - "EV_PREPARE_FOR_LOAD" : "Signal from the main FSM to set lift to position suited for sharp measurement", "EV_SCRABBLE" : "Signal from the main FSM to set the chelae to not so opened position", "EV_LOAD_THE_PUCK" : "Signal from the main FSM to initiate the puck picking up procedure", -- 2.39.2