#define HOKUYO_CENTER_OFFSET_M (HOKUYO_CENTER_OFFSET_MM/1000.0)
#define ROBOT_AXIS_TO_PUCK_M 0.2 // puck approach distance
+#define PUCK_REACHABLE(x) ((x<0.13)?true:false)
+#define PUCK_INSIDE(x) ((x<=0.055)?true:false)
/**
* PLAYGROUND DIMENSIONS
case EV_ENTRY:
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
break;
case EV_LOAD_THE_PUCK:
FSM_TRANSITION(dummy_action);
break;
- case EV_LOOK_FOR_PUCK:
- //FSM_TRANSITION(dummy_action);
- FSM_TRANSITION(look_for_puck);
- break;
case EV_GRASP_THE_PUCK:
//FSM_TRANSITION(dummy_action);
FSM_TRANSITION(grasp_the_puck);
FSM_TRANSITION(dummy_action);
//FSM_TRANSITION(...);
break;
- case EV_PUCK_REACHABLE: // we do not want react unless MAIN FSM tell us to
case EV_PUCK_INSIDE: // we do not want react unless MAIN FSM tell us to
break;
case EV_RETURN:
- case EV_STOP_LOOKING:
- case EV_TIMER:
- DBG_PRINT_EVENT("unhandled event");
- break;
- case EV_EXIT:
- break;
- }
-}
-
-FSM_STATE(look_for_puck)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- break;
- case EV_PUCK_INSIDE: // FIXME: handle this the same way
- case EV_PUCK_REACHABLE: // like EV_PUCK_REACHABLE? or differently?
- FSM_SIGNAL(MAIN, EV_ACTION_DONE, NULL);
- FSM_TRANSITION(wait_for_command);
- break;
- case EV_STOP_LOOKING:
- FSM_TRANSITION(wait_for_command);
- break;
- case EV_LOAD_THE_PUCK:
- case EV_GRASP_THE_PUCK:
- case EV_LOOK_FOR_PUCK:
- case EV_RETURN:
- case EV_TIMER:
- case EV_UNLOAD_THE_PUCK:
- DBG_PRINT_EVENT("unhandled event");
- break;
- case EV_EXIT:
- break;
- }
-}
-
-FSM_STATE(reactive_demo)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- break;
- case EV_PUCK_REACHABLE:
- FSM_SIGNAL(MAIN, EV_ACTION_DONE, NULL);
- FSM_TRANSITION(suck_the_puck);
- break;
- case EV_PUCK_INSIDE:
- break;
- case EV_RETURN:
case EV_TIMER:
- case EV_LOAD_THE_PUCK:
- case EV_LOOK_FOR_PUCK:
- case EV_STOP_LOOKING:
- case EV_UNLOAD_THE_PUCK:
- case EV_GRASP_THE_PUCK:
DBG_PRINT_EVENT("unhandled event");
break;
case EV_EXIT:
act_chelae(CHELA_TIGHT, CHELA_TIGHT);
FSM_TIMER(2000);
break;
- case EV_PUCK_REACHABLE:
- 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);
case EV_UNLOAD_THE_PUCK:
case EV_RETURN:
case EV_LOAD_THE_PUCK:
- case EV_LOOK_FOR_PUCK:
- case EV_STOP_LOOKING:
case EV_GRASP_THE_PUCK:
DBG_PRINT_EVENT("unhandled event");
break;
case EV_EXIT:
- //FSM_TIMER_STOP();
break;
}
}
switch (FSM_EVENT) {
case EV_ENTRY:
break;
- case EV_PUCK_REACHABLE:
case EV_PUCK_INSIDE:
break;
case EV_UNLOAD_THE_PUCK:
break;
case EV_RETURN:
case EV_LOAD_THE_PUCK:
- case EV_LOOK_FOR_PUCK:
- case EV_STOP_LOOKING:
case EV_GRASP_THE_PUCK:
DBG_PRINT_EVENT("unhandled event");
break;
break;
case EV_LOAD_THE_PUCK:
case EV_GRASP_THE_PUCK:
- case EV_LOOK_FOR_PUCK:
- case EV_STOP_LOOKING:
case EV_UNLOAD_THE_PUCK:
- case EV_PUCK_REACHABLE:
DBG_PRINT_EVENT("unhandled event");
break;
case EV_EXIT:
FSM_SIGNAL(MAIN, EV_ACTION_DONE, NULL);
FSM_TRANSITION(wait_for_command);
break;
- case EV_PUCK_REACHABLE:
case EV_PUCK_INSIDE:
break;
case EV_RETURN:
case EV_LOAD_THE_PUCK:
case EV_GRASP_THE_PUCK:
- case EV_LOOK_FOR_PUCK:
- case EV_STOP_LOOKING:
case EV_UNLOAD_THE_PUCK:
DBG_PRINT_EVENT("unhandled event");
break;
switch (FSM_EVENT) {
case EV_ENTRY:
break;
- case EV_PUCK_REACHABLE:
case EV_PUCK_INSIDE:
case EV_UNLOAD_THE_PUCK:
case EV_TIMER:
case EV_RETURN:
case EV_LOAD_THE_PUCK:
- case EV_LOOK_FOR_PUCK:
case EV_GRASP_THE_PUCK:
DBG_PRINT_EVENT("unhandled event");
break;
FSM_STATE_DECL(approach_opponents_static_dispenser);
/* States handling ACT's actions (SUBFSMs) */
FSM_STATE_DECL(grasp_the_puck);
-FSM_STATE_DECL(look_for_puck);
+FSM_STATE_DECL(look_for_puck_ahead);
+FSM_STATE_DECL(look_around_for_puck);
/************************************************************************
* INITIAL AND STARTING STATES
case EV_STACK_FULL:
case EV_ACTION_DONE:
case EV_ACTION_ERROR:
- case EV_ACT_FATAL_ERROR:
+ case EV_PUCK_REACHABLE:
case EV_MOTION_DONE:
DBG_PRINT_EVENT("unhandled event");
break;
{
static const int lot = 3; // this variable location is temporary...; going to be received from the camera
static int free_puck_to_get = 0; // next free puck number to pick up
- static struct puck_pos pp;
switch (FSM_EVENT) {
case EV_ENTRY:
robot_goto_puck_in_grid(
break;
case EV_MOTION_DONE:
printf("arrived to the free puck no. %d\n", free_puck_to_get);
+ break;
+ case EV_RETURN:
if(free_puck_to_get<4) {
robot_goto_puck_in_grid(
free_puck_pick_up_sequence[lot][free_puck_to_get][0],
}
break;
case EV_ACTION_DONE:
- case EV_RETURN:
case EV_TIMER:
case EV_OBSTRUCTION_AHEAD:
case EV_LASER_POWER:
case EV_ENEMY_AHEAD:
case EV_STACK_FULL:
case EV_ACTION_ERROR:
- case EV_ACT_FATAL_ERROR:
+ case EV_PUCK_REACHABLE:
case EV_START:
DBG_PRINT_EVENT("unhandled event");
break;
break;
case EV_MOTION_DONE:
//FSM_SIGNAL(ACT, EV_LOOK_FOR_PUCK, NULL);
- SUBFSM_TRANSITION(look_for_puck, NULL);
+ SUBFSM_TRANSITION(look_around_for_puck, NULL);
break;
case EV_ACTION_DONE:
//SUBFSM_TRANSITION(grasp_the_puck, NULL);
case EV_ENEMY_AHEAD:
case EV_STACK_FULL:
case EV_ACTION_ERROR:
- case EV_ACT_FATAL_ERROR:
+ case EV_PUCK_REACHABLE:
case EV_START:
DBG_PRINT_EVENT("unhandled event");
break;
case EV_ENEMY_AHEAD:
case EV_STACK_FULL:
case EV_ACTION_ERROR:
- case EV_ACT_FATAL_ERROR:
+ case EV_PUCK_REACHABLE:
case EV_START:
DBG_PRINT_EVENT("unhandled event");
break;
//case EV_PUCK_REACHABLE: // FIXME: handle this
case EV_STACK_FULL:
case EV_ACTION_ERROR:
- case EV_ACT_FATAL_ERROR:
+ case EV_PUCK_REACHABLE:
case EV_START:
DBG_PRINT_EVENT("unhandled event");
break;
//case EV_PUCK_REACHABLE: // FIXME: handle this
case EV_STACK_FULL:
case EV_ACTION_ERROR:
- case EV_ACT_FATAL_ERROR:
+ case EV_PUCK_REACHABLE:
case EV_START:
DBG_PRINT_EVENT("unhandled event");
break;
}
/************************************************************************
- * STATES HANDLING ACT's ACTIONS
+ * STATES HANDLING ACT's ACTIONS (to be used as SUB FSMs)
************************************************************************/
FSM_STATE(grasp_the_puck)
case EV_SHORT_TIME_TO_END:
case EV_ENEMY_AHEAD:
case EV_STACK_FULL:
- case EV_ACT_FATAL_ERROR:
+ case EV_PUCK_REACHABLE:
case EV_START:
DBG_PRINT_EVENT("unhandled event");
break;
}
}
-FSM_STATE(look_for_puck)
+FSM_STATE(look_around_for_puck)
{
static int lfp_status = 0;
const static int scatter_angle = 20;
static struct ref_pos_type orig_position;
switch (FSM_EVENT) {
case EV_ENTRY:
- FSM_SIGNAL(ACT, EV_LOOK_FOR_PUCK, NULL);
ROBOT_LOCK(ref_pos);
orig_position = robot.ref_pos;
ROBOT_UNLOCK(ref_pos);
- printf("original angle of rotation of the robot: %f degrees\n", RAD2DEG(orig_position.phi));
+ //printf("original angle of rotation of the robot: %f degrees\n", RAD2DEG(orig_position.phi));
robot_move_by(0, TURN_CW(DEG2RAD(RAD2DEG(orig_position.phi)-scatter_angle)), &tcSlow);
- printf("lfp_status: %d\n", lfp_status);
+ //printf("lfp_status: %d\n", lfp_status);
break;
case EV_MOTION_DONE:
switch (lfp_status) {
case 0:
- printf("2. original angle of rotation of the robot: %f degrees\n", RAD2DEG(orig_position.phi));
- printf("robot.fer_pos angle of rotation of the robot: %f degrees\n", RAD2DEG(robot.ref_pos.phi));
- printf("--- robot move by ... turn cw\n");
+ //printf("2. original angle of rotation of the robot: %f degrees\n", RAD2DEG(orig_position.phi));
+ //printf("robot.fer_pos angle of rotation of the robot: %f degrees\n", RAD2DEG(robot.ref_pos.phi));
+ //printf("--- robot move by ... turn cw\n");
robot_move_by(0, TURN_CCW(DEG2RAD(RAD2DEG(orig_position.phi)+scatter_angle)), &tcSlow);
lfp_status++;
break;
SUBFSM_RET((void *)false);
break;
}
- printf("lfp_status: %d\n", lfp_status);
+ //printf("lfp_status: %d\n", lfp_status);
break;
- case EV_ACTION_DONE: // puck found
+ case EV_PUCK_REACHABLE: // puck found
robot_stop();
SUBFSM_RET((void *)true);
break;
+ case EV_ACTION_DONE:
case EV_ACTION_ERROR: // look for puck does not send this event
case EV_RETURN:
case EV_TIMER:
case EV_SHORT_TIME_TO_END:
case EV_ENEMY_AHEAD:
case EV_STACK_FULL:
- case EV_ACT_FATAL_ERROR:
case EV_START:
DBG_PRINT_EVENT("unhandled event");
break;
case EV_ENEMY_AHEAD:
case EV_STACK_FULL:
case EV_ACTION_ERROR:
- case EV_ACT_FATAL_ERROR:
+ case EV_PUCK_REACHABLE:
case EV_START:
DBG_PRINT_EVENT("unhandled event");
break;
FSM_STATE_DECL(get_out_of_the_construction_zone);
FSM_STATE_DECL(grasp_the_puck);
+FSM_STATE_DECL(look_around_for_puck);
+
/************************************************************************
* INITIAL AND STARTING STATES
************************************************************************/
case EV_STACK_FULL:
case EV_ACTION_DONE:
case EV_ACTION_ERROR:
- case EV_ACT_FATAL_ERROR:
+ case EV_PUCK_REACHABLE:
case EV_MOTION_DONE:
DBG_PRINT_EVENT("unhandled event");
break;
case EV_ENTRY:
robot_move_by(0.42, NO_TURN(), &tcSlow);
break;
- case EV_MOTION_DONE:
- FSM_SIGNAL(ACT, EV_LOOK_FOR_PUCK, NULL);
- break;
- case EV_ACTION_DONE:
- SUBFSM_TRANSITION(grasp_the_puck, NULL);
+ case EV_MOTION_DONE: {
+ int i = 0;
+ if (PUCK_REACHABLE(robot.puck_distance)) {
+ printf("Puck reachable, telling act to grasp the puck\n");
+ SUBFSM_TRANSITION(grasp_the_puck, NULL);
+ } else {
+ switch(i) {
+ case (0):
+ printf("making transition to look_around_for_puck state\n");
+ SUBFSM_TRANSITION(look_around_for_puck, NULL);
+ break;
+ case (1):
+ printf("telling the robot to move by 5 cm and try again\n");
+ robot_move_by(0.05, NO_TURN(), &tcSlow);
+ break;
+ }
+ i++;
+ }
+ }
break;
+ case EV_PUCK_REACHABLE:
case EV_RETURN:
FSM_TRANSITION(simple_construction_zone_approach);
break;
+ case EV_ACTION_DONE:
case EV_TIMER:
case EV_OBSTRUCTION_AHEAD:
case EV_LASER_POWER:
case EV_SHORT_TIME_TO_END:
case EV_ENEMY_AHEAD:
case EV_STACK_FULL:
- case EV_ACTION_ERROR:
- case EV_ACT_FATAL_ERROR:
case EV_START:
DBG_PRINT_EVENT("unhandled event");
break;
case EV_ENTRY:
robot_trajectory_new(&tcFast);
robot_trajectory_add_point_trans(0.9, 1);
- robot_trajectory_add_final_point_trans(0.9, ROBOT_AXIS_TO_FRONT_M + 0.05, NO_TURN());
+ robot_trajectory_add_final_point_trans(0.9, ROBOT_AXIS_TO_FRONT_M + 0.00, NO_TURN());
break;
case EV_MOTION_DONE:
FSM_SIGNAL(ACT, EV_UNLOAD_THE_PUCK, NULL);
case EV_ENEMY_AHEAD:
case EV_STACK_FULL:
case EV_ACTION_ERROR:
- case EV_ACT_FATAL_ERROR:
+ case EV_PUCK_REACHABLE:
case EV_START:
DBG_PRINT_EVENT("unhandled event");
break;
case EV_ENEMY_AHEAD:
case EV_STACK_FULL:
case EV_ACTION_ERROR:
- case EV_ACT_FATAL_ERROR:
+ case EV_PUCK_REACHABLE:
case EV_START:
DBG_PRINT_EVENT("unhandled event");
break;
}
}
+/************************************************************************
+ * STATES HANDLING ACT's ACTIONS (to be used as SUB FSMs)
+ ************************************************************************/
+
FSM_STATE(grasp_the_puck)
{
switch (FSM_EVENT) {
case EV_ENEMY_AHEAD:
case EV_STACK_FULL:
case EV_ACTION_ERROR:
- case EV_ACT_FATAL_ERROR:
+ case EV_PUCK_REACHABLE:
+ case EV_START:
+ DBG_PRINT_EVENT("unhandled event");
+ break;
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(look_around_for_puck)
+{
+ static int lfp_status = 0;
+ const static int scatter_angle = 20;
+ static struct ref_pos_type orig_position;
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ ROBOT_LOCK(ref_pos);
+ orig_position = robot.ref_pos;
+ ROBOT_UNLOCK(ref_pos);
+ //printf("original angle of rotation of the robot: %f degrees\n", RAD2DEG(orig_position.phi));
+ robot_move_by(0, TURN_CW(DEG2RAD(RAD2DEG(orig_position.phi)-scatter_angle)), &tcSlow);
+ //printf("lfp_status: %d\n", lfp_status);
+ break;
+ case EV_MOTION_DONE:
+ switch (lfp_status) {
+ case 0:
+ //printf("2. original angle of rotation of the robot: %f degrees\n", RAD2DEG(orig_position.phi));
+ //printf("robot.fer_pos angle of rotation of the robot: %f degrees\n", RAD2DEG(robot.ref_pos.phi));
+ //printf("--- robot move by ... turn cw\n");
+ robot_move_by(0, TURN_CCW(DEG2RAD(RAD2DEG(orig_position.phi)+scatter_angle)), &tcSlow);
+ lfp_status++;
+ break;
+ case 1:
+ robot_move_by(0, TURN(orig_position.phi), &tcSlow);
+ lfp_status++;
+ break;
+ case 2: // puck not found
+ SUBFSM_RET((void *)false);
+ break;
+ }
+ //printf("lfp_status: %d\n", lfp_status);
+ break;
+ case EV_PUCK_REACHABLE: // puck found
+ robot_stop();
+ SUBFSM_RET((void *)true);
+ break;
+ case EV_ACTION_DONE:
+ case EV_ACTION_ERROR: // look for puck does not send this event
+ case EV_RETURN:
+ case EV_TIMER:
+ case EV_OBSTRUCTION_AHEAD:
+ case EV_LASER_POWER:
+ case EV_GOAL_NOT_REACHABLE:
+ case EV_SHORT_TIME_TO_END:
+ case EV_ENEMY_AHEAD:
+ case EV_STACK_FULL:
case EV_START:
DBG_PRINT_EVENT("unhandled event");
break;
}
}
+/************************************************************************
+ * MAIN FUNCTION
+ ************************************************************************/
+
int main()
{
int rv;
"EV_ACTION_DONE" : "ACT FSM signals that the requested action has finished successfully",
"EV_ACTION_ERROR" : "ACT FSM signals that the operation did not succeed",
- "EV_ACT_FATAL_ERROR" : "ACT FSM signals that there is some fatal error (HW signalling problems, ...)",
+ "EV_PUCK_REACHABLE" : "Received through ORTE. Sharp sensor informs us, something (preferably the puck) is in reach of chelae",
"EV_STACK_FULL" : "ACT FSM signals that the puck stack is full",
"EV_MOTION_DONE" : "Previously submitted motion is finished",
"act" : {
"EV_LOAD_THE_PUCK" : "Signal from the main FSM to initiate the puck picking up procedure",
"EV_GRASP_THE_PUCK": "Signal from the main FSM to make the mechanism grasp the puck and keep it holding tight",
- "EV_LOOK_FOR_PUCK" : "Signal from the main FSM telling ACT FSM to use the Sharp sensor to indicate puck presence",
- "EV_STOP_LOOKING" : "Signal from the main FSM telling ACT FSM to return back to the wait_for_command state",
+ # we don't want this to be implemented using ACT FSM: "EV_LOOK_FOR_PUCK" : "Signal from the main FSM telling ACT FSM to use the Sharp sensor to indicate puck presence",
+ # "EV_STOP_LOOKING" : "Signal from the main FSM telling ACT FSM to return back to the wait_for_command state",
"EV_UNLOAD_THE_PUCK" : "Signal from the main FSM telling ACT FSM to unload the puck",
- "EV_PUCK_REACHABLE": "Received through ORTE. Sharp sensor informs us, something (preferably the puck) is in reach of chelae",
"EV_PUCK_INSIDE" : "Received through ORTE. Sharp sensor informs us that the puck is inside",
#"EV_LIFT_GO_POSITION" : "",
#"EV_LIFT_GROUND_POSITION" : "",
/* sensors */
struct motion_irc_type motion_irc; /* odometry */
struct corr_distances_type corr_distances; /* ultrasound */
+ double puck_distance; /* sharp sensor to puck distance in meters */
struct hokuyo_scan_type hokuyo;
struct cmu_type cmu;
void *recvCallBackParam)
{
switch (info->status) {
- case NEW_DATA:
- //printf("Sharp distance: %f\n", robot.orte.puck_distance.distance);
- if(robot.orte.puck_distance.distance <= 0.055) {
+ case NEW_DATA: {
+ double old_distance_value = robot.puck_distance;
+
+ ROBOT_LOCK(sharps);
+ robot.puck_distance = robot.orte.puck_distance.distance;
+ ROBOT_UNLOCK(sharps);
+
+ if (PUCK_INSIDE(robot.orte.puck_distance.distance) && !PUCK_INSIDE(old_distance_value)) {
+ // signal puck is inside
FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
}
- if(robot.orte.puck_distance.distance < 0.13 && robot.orte.puck_distance.distance > 0.055) {
- FSM_SIGNAL(ACT, EV_PUCK_REACHABLE, NULL);
+ if (PUCK_REACHABLE(robot.orte.puck_distance.distance) && !PUCK_REACHABLE(old_distance_value)) {
+ // signal puck is reachable
+ FSM_SIGNAL(MAIN, EV_PUCK_REACHABLE, NULL);
}
- break;
- case DEADLINE:
- // FIXME F.J. (?) inform display in case of invalid data?
- // FIXME F.J. (?) limited space on the display...
- //robot.hw_status[STATUS_ACTUATORS]=HW_STATUS_FAILED;
- DBG("ORTE deadline occurred - servos receive\n");
- break;
+ }
+ break;
+ case DEADLINE:
+ // FIXME F.J. (?) inform display in case of invalid data?
+ // FIXME F.J. (?) limited space on the display...
+ //robot.hw_status[STATUS_ACTUATORS]=HW_STATUS_FAILED;
+ DBG("ORTE deadline occurred - servos receive\n");
+ break;
}
}
case EV_ACTION_ERROR:
printf("MAIN: EV_ACTION_ERROR arrived\n");
break;
- case EV_ACT_FATAL_ERROR:
- printf("MAIN: EV_ACT_FATAL_ERROR arrived\n");
+ case EV_PUCK_REACHABLE:
+ printf("MAIN: EV_PUCK_REACHABLE arrived\n");
break;
case EV_MOTION_DONE:
case EV_RETURN:
printf("Sending 'EV_GRASP_THE_PUCK' event to the act fsm\n");
FSM_SIGNAL(ACT, EV_GRASP_THE_PUCK, NULL);
break;
- case 's' :
- case 'S' :
- printf("Sending 'EV_LOOK_FOR_PUCK' event to the act fsm\n");
- FSM_SIGNAL(ACT, EV_LOOK_FOR_PUCK, NULL);
- break;
case 'u' :
case 'U' :
printf("Sending 'EV_UNLOAD_THE_PUCK' event to the act fsm\n");
projects / eurobot / public.git / commitdiff