* ***********************************************************/
FSM_STATE_DECL(wait_for_command);
+
FSM_STATE_DECL(reactive_demo);
+FSM_STATE_DECL(look_for_puck);
FSM_STATE_DECL(dummy_action);
-FSM_STATE_DECL(suck_the_puck);
+FSM_STATE_DECL(grasp_the_puck);
+
+FSM_STATE_DECL(suck_the_puck);
/* ***********************************************************
* STATE DEFINITIONS
* ***********************************************************/
-FSM_STATE(wait_for_command) {
+FSM_STATE(wait_for_command)
+{
switch (FSM_EVENT) {
case EV_ENTRY:
act_belts(BELTS_OFF, BELTS_OFF);
act_chelae(CHELA_OPEN, CHELA_OPEN);
- FSM_TRANSITION(reactive_demo);
// TODO
// put all actuators to defined initial positions
break;
- case EV_PICK_UP_PUCK:
+ case EV_LOAD_THE_PUCK:
//FSM_TRANSITION(dummy_action);
break;
+ case EV_LOOK_FOR_PUCK:
+ FSM_TRANSITION(look_for_puck);
+ break;
+ case EV_GRASP_THE_PUCK:
+ FSM_TRANSITION(grasp_the_puck);
+ break;
+ case EV_PUCK_REACHABLE: //
+ case EV_PUCK_INSIDE: // we do not want react unless MAIN FSM tell us to
+ break;
case EV_RETURN:
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_REACHABLE:
- case EV_PUCK_INSIDE:
+ FSM_SIGNAL(MAIN, EV_PUCK_REACHABLE, NULL);
+ 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_PUCK_INSIDE: // FIXME: handle this? the same way like EV_PUCK_REACHABLE? or differently?
DBG_PRINT_EVENT("unhandled event");
break;
case EV_EXIT:
}
}
-FSM_STATE(reactive_demo) {
+FSM_STATE(reactive_demo)
+{
switch (FSM_EVENT) {
case EV_ENTRY:
break;
case EV_PUCK_REACHABLE:
+ FSM_SIGNAL(MAIN, EV_PUCK_REACHABLE, NULL);
FSM_TRANSITION(suck_the_puck);
break;
case EV_PUCK_INSIDE:
break;
case EV_RETURN:
case EV_TIMER:
- case EV_PICK_UP_PUCK:
+ case EV_LOAD_THE_PUCK:
+ case EV_LOOK_FOR_PUCK:
+ case EV_GRASP_THE_PUCK:
+ DBG_PRINT_EVENT("unhandled event");
+ break;
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(grasp_the_puck)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ act_belts(BELTS_IN, BELTS_IN);
+ act_chelae(CHELA_TIGHT, CHELA_TIGHT);
+ break;
+ case EV_PUCK_REACHABLE:
+ break;
+ case EV_PUCK_INSIDE:
+ act_belts(BELTS_OFF, BELTS_OFF);
+ FSM_SIGNAL(MAIN, EV_PUCK_LOADED_UP, NULL);
+ break;
+ case EV_RETURN:
+ case EV_LOAD_THE_PUCK:
+ case EV_UNLOAD_THE_PUCK:
+ FSM_TIMER(800);
+ case EV_TIMER:
+ act_belts(BELTS_OFF, BELTS_OFF);
+ act_chelae(CHELA_LOOSE, CHELA_LOOSE);
+ FSM_TRANSITION(wait_for_command);
+ break;
+ case EV_LOOK_FOR_PUCK:
+ case EV_GRASP_THE_PUCK:
DBG_PRINT_EVENT("unhandled event");
break;
case EV_EXIT:
}
}
-FSM_STATE(suck_the_puck) {
+FSM_STATE(suck_the_puck)
+{
switch (FSM_EVENT) {
case EV_ENTRY:
FSM_TIMER(1000);
break;
case EV_PUCK_INSIDE:
act_belts(BELTS_OFF, BELTS_OFF);
- FSM_TRANSITION(reactive_demo);
+ FSM_SIGNAL(MAIN, EV_PUCK_LOADED_UP, NULL);
+ FSM_TRANSITION(wait_for_command);
break;
case EV_RETURN:
case EV_TIMER:
- FSM_TRANSITION(reactive_demo);
+ FSM_TRANSITION(wait_for_command);
break;
- case EV_PICK_UP_PUCK:
+ case EV_LOAD_THE_PUCK:
+ case EV_GRASP_THE_PUCK:
+ case EV_LOOK_FOR_PUCK:
case EV_PUCK_REACHABLE:
DBG_PRINT_EVENT("unhandled event");
break;
}
}
-FSM_STATE(dummy_action) {
+FSM_STATE(dummy_action)
+{
switch (FSM_EVENT) {
case EV_ENTRY:
FSM_TIMER(2000);
FSM_TRANSITION(wait_for_command);
break;
case EV_RETURN:
- case EV_PICK_UP_PUCK:
+ case EV_LOAD_THE_PUCK:
+ case EV_GRASP_THE_PUCK:
+ case EV_LOOK_FOR_PUCK:
case EV_PUCK_REACHABLE:
case EV_PUCK_INSIDE:
DBG_PRINT_EVENT("unhandled event");
robot_trajectory_new_backward(&tc);
robot_trajectory_add_final_point_notrans(des_pos.x, des_pos.y, NO_TURN());
}
+/************************************************************************
+ * Trajectory constraints used, are initialized in the init state
+ ************************************************************************/
+
+struct TrajectoryConstraints tcFast, tcSlow;
/************************************************************************
* FSM STATES DECLARATION
FSM_STATE_DECL(init);
FSM_STATE_DECL(wait_for_start);
/* movement states */
+FSM_STATE_DECL(approach_first_puck);
+FSM_STATE_DECL(simple_construction_zone_approach);
FSM_STATE_DECL(approach_our_static_dispenser);
FSM_STATE_DECL(approach_opponents_static_dispenser);
FSM_STATE(init)
{
switch (FSM_EVENT) {
- case EV_ENTRY:
- //act..
- FSM_TRANSITION(wait_for_start);
- break;
- default:
- break;
+ case EV_ENTRY:
+ tcFast = trajectoryConstraintsDefault;
+ tcFast.maxv = 1.5;
+ tcSlow = trajectoryConstraintsDefault;
+ tcSlow.maxv = 0.3;
+ FSM_TRANSITION(wait_for_start);
+ break;
+ default:
+ break;
}
}
#else
case EV_ENTRY:
#endif
- robot_set_est_pos_trans(ROBOT_WIDTH_M,
- PLAYGROUND_HEIGHT_M - ROBOT_AXIS_TO_BACK_M - 0.1,
+ robot_set_est_pos_trans(0.16,
+ PLAYGROUND_HEIGHT_M - 0.16,
DEG2RAD(-45));
/* start to do something */
ROBOT_LOCK(state);
robot.state = COMPETITION_STARTED;
ROBOT_UNLOCK(state);
- //FSM_TRANSITION(approach_our_static_dispenser);
- //FSM_TRANSITION(approach_opponents_static_dispenser);
+ FSM_TRANSITION(approach_first_puck);
break;
case EV_RETURN:
case EV_TIMER:
case EV_SHORT_TIME_TO_END:
case EV_ENEMY_AHEAD:
case EV_STACK_FULL:
+ case EV_PUCK_REACHABLE:
case EV_PUCK_LOADED_UP:
+ case EV_ACT_ERROR:
+ case EV_ACT_FATAL_ERROR:
case EV_MOTION_DONE:
DBG_PRINT_EVENT("unhandled event");
break;
* MOVEMENT STATES
************************************************************************/
+FSM_STATE(approach_first_puck)
+{
+ switch (FSM_EVENT) {
+ 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_PUCK_REACHABLE:
+ FSM_SIGNAL(ACT, EV_GRASP_THE_PUCK, NULL);
+ break;
+ case EV_PUCK_LOADED_UP:
+ FSM_TRANSITION(simple_construction_zone_approach);
+ break;
+ 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_ACT_ERROR:
+ case EV_ACT_FATAL_ERROR:
+ DBG_PRINT_EVENT("unhandled event");
+ break;
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(simple_construction_zone_approach)
+{
+ switch (FSM_EVENT) {
+ 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());
+ break;
+ case EV_MOTION_DONE:
+ FSM_SIGNAL(ACT, EV_UNLOAD_THE_PUCK, NULL);
+ break;
+ case EV_PUCK_REACHABLE:
+ case EV_PUCK_LOADED_UP:
+ 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_ACT_ERROR:
+ case EV_ACT_FATAL_ERROR:
+ DBG_PRINT_EVENT("unhandled event");
+ break;
+ case EV_EXIT:
+ break;
+ }
+}
+
FSM_STATE(approach_our_static_dispenser)
{
switch (FSM_EVENT) {
}
break;
case EV_MOTION_DONE:
- FSM_SIGNAL(ACT, EV_PICK_UP_PUCK, NULL);
+ FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
printf("Arrived to the static dispenser\n");
break;
case EV_PUCK_LOADED_UP:
case EV_GOAL_NOT_REACHABLE:
case EV_SHORT_TIME_TO_END:
case EV_ENEMY_AHEAD:
+ case EV_PUCK_REACHABLE: // FIXME: handle this
case EV_STACK_FULL:
+ case EV_ACT_ERROR:
+ case EV_ACT_FATAL_ERROR:
case EV_START:
DBG_PRINT_EVENT("unhandled event");
break;
}
break;
case EV_MOTION_DONE:
- FSM_SIGNAL(ACT, EV_PICK_UP_PUCK, NULL);
+ FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
printf("Arrived to the static dispenser\n");
break;
case EV_PUCK_LOADED_UP:
case EV_GOAL_NOT_REACHABLE:
case EV_SHORT_TIME_TO_END:
case EV_ENEMY_AHEAD:
+ case EV_PUCK_REACHABLE: // FIXME: handle this
case EV_STACK_FULL:
+ case EV_ACT_ERROR:
+ case EV_ACT_FATAL_ERROR:
case EV_START:
DBG_PRINT_EVENT("unhandled event");
break;
"main" : {
"EV_START" : "Changed state of start connector.",
"EV_SHORT_TIME_TO_END" : "Time to go for the deposition regarless of what do we have.",
- "EV_LASER_POWER" : "",
- "EV_STACK_FULL" : "ACT FSM signals that the puck stack is full",
- "EV_PUCK_LOADED_UP" : "ACT FSM signals that the puck has been embarked",
+ "EV_LASER_POWER" : "", # FIXME: what's this? (F.J.)
- "EV_MOTION_DONE" : "Previously submitted motion is finished",
+ "EV_STACK_FULL" : "ACT FSM signals that the puck stack is full",
+ "EV_PUCK_LOADED_UP" : "ACT FSM signals that the puck has been embarked",
+ "EV_PUCK_REACHABLE" : "ACT FSM signals that the puck is in reachable position",
+ "EV_ACT_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_MOTION_DONE" : "Previously submitted motion is finished",
"EV_GOAL_NOT_REACHABLE" : "Path planner can't calculate a path to the goal. Probably obstacles are on the way.",
- "EV_OBSTRUCTION_AHEAD" : "",
- "EV_ENEMY_AHEAD" : "",
+ "EV_OBSTRUCTION_AHEAD" : "", # FIXME: what's this exactly? (F.J.)
+ "EV_ENEMY_AHEAD" : "", # FIXME: what's this exactly? (F.J.)
},
"loc" : {
#"EV_ODO_RECEIVED" : "Execute prediction step based on odometry data",
"disp" : {
},
"act" : {
- "EV_PICK_UP_PUCK" : "Signal from the main FSM to initiate the puck picking up procedure",
- "EV_PUCK_REACHABLE" : "Puck (or something) in reach of chelae",
- "EV_PUCK_INSIDE" : "Puck inside the mechanism",
+ "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_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" : "",
}
projects / eurobot / public.git / commitdiff