#define HOKUYO_CENTER_OFFSET_MM 0 /*FIXME*/
#define HOKUYO_CENTER_OFFSET_M (HOKUYO_CENTER_OFFSET_MM/1000.0)
+#define ROBOT_AXIS_TO_PUCK_M 0.2 // puck approach distance
+
/**
* PLAYGROUND DIMENSIONS
*
bin_PROGRAMS += robomain
robomain_SOURCES = main.cc fsmmain.c
+bin_PROGRAMS += homologation
+homologation_SOURCES = homologation.cc
+
# Library with general support functions for the robot
lib_LIBRARIES += robot
robot_SOURCES = robot_orte.c robot.c fsmmove.cc movehelper.cc \
FSM_STATE_DECL(dummy_action);
FSM_STATE_DECL(grasp_the_puck);
+FSM_STATE_DECL(hold_the_puck);
FSM_STATE_DECL(suck_the_puck);
/* ***********************************************************
// put all actuators to defined initial positions
break;
case EV_LOAD_THE_PUCK:
- //FSM_TRANSITION(dummy_action);
+ FSM_TRANSITION(dummy_action);
break;
case EV_LOOK_FOR_PUCK:
- FSM_TRANSITION(look_for_puck);
+ FSM_TRANSITION(dummy_action);
+ //FSM_TRANSITION(look_for_puck);
break;
case EV_GRASP_THE_PUCK:
- FSM_TRANSITION(grasp_the_puck);
+ FSM_TRANSITION(dummy_action);
+ //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
+ case EV_UNLOAD_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_TIMER:
switch (FSM_EVENT) {
case EV_ENTRY:
break;
- case EV_PUCK_REACHABLE:
+ case EV_PUCK_INSIDE: // FIXME: handle this the same way
+ case EV_PUCK_REACHABLE: // like EV_PUCK_REACHABLE? or differently?
FSM_SIGNAL(MAIN, EV_PUCK_REACHABLE, NULL);
FSM_TRANSITION(wait_for_command);
break;
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?
+ case EV_UNLOAD_THE_PUCK:
DBG_PRINT_EVENT("unhandled event");
break;
case EV_EXIT:
case EV_TIMER:
case EV_LOAD_THE_PUCK:
case EV_LOOK_FOR_PUCK:
+ case EV_UNLOAD_THE_PUCK:
case EV_GRASP_THE_PUCK:
DBG_PRINT_EVENT("unhandled event");
break;
break;
case EV_PUCK_INSIDE:
act_belts(BELTS_OFF, BELTS_OFF);
- FSM_SIGNAL(MAIN, EV_PUCK_LOADED_UP, NULL);
+ FSM_SIGNAL(MAIN, EV_ACTION_DONE, NULL);
+ FSM_TRANSITION(hold_the_puck);
break;
+ 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;
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(hold_the_puck)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ break;
+ case EV_PUCK_REACHABLE:
+ case EV_PUCK_INSIDE:
+ break;
case EV_UNLOAD_THE_PUCK:
- FSM_TIMER(800);
+ act_belts(BELTS_OUT+20, BELTS_OUT+20);
+ act_chelae(CHELA_TIGHT, CHELA_TIGHT);
+ FSM_TIMER(1100);
+ break;
case EV_TIMER:
act_belts(BELTS_OFF, BELTS_OFF);
act_chelae(CHELA_LOOSE, CHELA_LOOSE);
FSM_TRANSITION(wait_for_command);
+ DBG_PRINT_EVENT("performing the transition to wait_for_command");
break;
+ case EV_RETURN:
+ case EV_LOAD_THE_PUCK:
case EV_LOOK_FOR_PUCK:
case EV_GRASP_THE_PUCK:
DBG_PRINT_EVENT("unhandled event");
break;
case EV_PUCK_INSIDE:
act_belts(BELTS_OFF, BELTS_OFF);
- FSM_SIGNAL(MAIN, EV_PUCK_LOADED_UP, NULL);
+ FSM_SIGNAL(MAIN, EV_ACTION_DONE, NULL);
FSM_TRANSITION(wait_for_command);
break;
case EV_RETURN:
case EV_LOAD_THE_PUCK:
case EV_GRASP_THE_PUCK:
case EV_LOOK_FOR_PUCK:
+ case EV_UNLOAD_THE_PUCK:
case EV_PUCK_REACHABLE:
DBG_PRINT_EVENT("unhandled event");
break;
FSM_TIMER(2000);
break;
case EV_TIMER:
- FSM_SIGNAL(MAIN, EV_PUCK_LOADED_UP, NULL);
+ FSM_SIGNAL(MAIN, EV_ACTION_DONE, NULL);
FSM_TRANSITION(wait_for_command);
break;
case EV_RETURN:
case EV_LOAD_THE_PUCK:
case EV_GRASP_THE_PUCK:
case EV_LOOK_FOR_PUCK:
+ case EV_UNLOAD_THE_PUCK:
case EV_PUCK_REACHABLE:
case EV_PUCK_INSIDE:
DBG_PRINT_EVENT("unhandled event");
}
}
-
+/*
+FSM_STATE()
+{
+ 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;
+ case EV_EXIT:
+ break;
+ }
+}
+*/
/*
- * fsmmain.cc 08/04/29
+ * fsmmain.cc 09/04/..
*
- * Robot's main control program (Eurobot 2008).
+ * Robot's main control program (Eurobot 2009).
*
- * Copyright: (c) 2008 CTU Dragons
+ * Copyright: (c) 2009 CTU Dragons
* CTU FEE - Department of Control Engineering
* License: GNU GPL v.2
*/
#define TIME_TO_DEPOSITE TIME_TO_DEPOSITE_DEFAULT
/* competition time in seconds */
-enum {
- LEFT = 0,
- RIGHT,
- CENTER
-};
+/************************************************************************
+ * Trajectory constraints used, are initialized in the init state
+ ************************************************************************/
+
+struct TrajectoryConstraints tcFast, tcSlow;
+
+/************************************************************************
+ * Definition of particular "free pucks pick up sequences"
+ ************************************************************************/
+
+
/************************************************************************
* NOTES ON DATA WE NEED TO STORE
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;
+void robot_goto_puck_in_grid(int lot, int nx, int ny, int phi)
+{
+ robot_trajectory_new(&tcSlow);
+ struct puck_pos pp = free_puck_pos(lot, nx, ny); // puck position
+ robot_trajectory_add_point_trans(
+ pp.x + (ROBOT_AXIS_TO_PUCK_M+0.20)*cos(DEG2RAD(phi)),
+ pp.y + (ROBOT_AXIS_TO_PUCK_M+0.20)*sin(DEG2RAD(phi)));
+ robot_trajectory_add_final_point_trans(
+ pp.x + ROBOT_AXIS_TO_PUCK_M*cos(DEG2RAD(phi)),
+ pp.y + ROBOT_AXIS_TO_PUCK_M*sin(DEG2RAD(phi)),
+ TURN(fmod(DEG2RAD(phi+180), 2.0*M_PI)));
+}
/************************************************************************
* FSM STATES DECLARATION
tcFast = trajectoryConstraintsDefault;
tcFast.maxv = 1.5;
tcSlow = trajectoryConstraintsDefault;
- tcSlow.maxv = 0.3;
+ tcSlow.maxv = 0.2;
FSM_TRANSITION(wait_for_start);
break;
default:
ROBOT_LOCK(state);
robot.state = COMPETITION_STARTED;
ROBOT_UNLOCK(state);
- FSM_TRANSITION(approach_first_puck);
+
+ robot_trajectory_new(&tcSlow);
+ struct puck_pos pp = free_puck_pos(0, 2, 0); // puck position
+ robot_trajectory_add_final_point_trans(
+ pp.x,
+ pp.y,
+ TURN(DEG2RAD(0)));
+ //robot_goto_puck_in_grid(0, 1, 2, 270);
+ /* --- this one FSM_TRANSITION(approach_first_puck); */
+ /* temporary:
+ FSM_SIGNAL(ACT, EV_GRASP_THE_PUCK, NULL);
+ FSM_TIMER(5000);
+ */
break;
case EV_RETURN:
case EV_TIMER:
+ /* temporary:
+ FSM_SIGNAL(ACT, EV_UNLOAD_THE_PUCK, NULL);
+ break;
+ */
case EV_OBSTRUCTION_AHEAD:
case EV_LASER_POWER:
case EV_GOAL_NOT_REACHABLE:
case EV_ENEMY_AHEAD:
case EV_STACK_FULL:
case EV_PUCK_REACHABLE:
- case EV_PUCK_LOADED_UP:
- case EV_ACT_ERROR:
+ case EV_ACTION_DONE:
+ case EV_ACTION_ERROR:
case EV_ACT_FATAL_ERROR:
case EV_MOTION_DONE:
DBG_PRINT_EVENT("unhandled event");
case EV_PUCK_REACHABLE:
FSM_SIGNAL(ACT, EV_GRASP_THE_PUCK, NULL);
break;
- case EV_PUCK_LOADED_UP:
+ case EV_ACTION_DONE:
FSM_TRANSITION(simple_construction_zone_approach);
break;
case EV_RETURN:
case EV_SHORT_TIME_TO_END:
case EV_ENEMY_AHEAD:
case EV_STACK_FULL:
- case EV_ACT_ERROR:
+ case EV_ACTION_ERROR:
case EV_ACT_FATAL_ERROR:
+ case EV_START:
DBG_PRINT_EVENT("unhandled event");
break;
case EV_EXIT:
FSM_SIGNAL(ACT, EV_UNLOAD_THE_PUCK, NULL);
break;
case EV_PUCK_REACHABLE:
- case EV_PUCK_LOADED_UP:
+ case EV_ACTION_DONE:
case EV_RETURN:
case EV_TIMER:
case EV_OBSTRUCTION_AHEAD:
case EV_SHORT_TIME_TO_END:
case EV_ENEMY_AHEAD:
case EV_STACK_FULL:
- case EV_ACT_ERROR:
+ case EV_ACTION_ERROR:
case EV_ACT_FATAL_ERROR:
+ case EV_START:
DBG_PRINT_EVENT("unhandled event");
break;
case EV_EXIT:
FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
printf("Arrived to the static dispenser\n");
break;
- case EV_PUCK_LOADED_UP:
+ case EV_ACTION_DONE:
printf("A Puck picked up\n");
break;
case EV_RETURN:
case EV_ENEMY_AHEAD:
case EV_PUCK_REACHABLE: // FIXME: handle this
case EV_STACK_FULL:
- case EV_ACT_ERROR:
+ case EV_ACTION_ERROR:
case EV_ACT_FATAL_ERROR:
case EV_START:
DBG_PRINT_EVENT("unhandled event");
FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
printf("Arrived to the static dispenser\n");
break;
- case EV_PUCK_LOADED_UP:
+ case EV_ACTION_DONE:
printf("A Puck picked up\n");
break;
case EV_RETURN:
case EV_ENEMY_AHEAD:
case EV_PUCK_REACHABLE: // FIXME: handle this
case EV_STACK_FULL:
- case EV_ACT_ERROR:
+ case EV_ACTION_ERROR:
+ case EV_ACT_FATAL_ERROR:
+ case EV_START:
+ DBG_PRINT_EVENT("unhandled event");
+ break;
+ case EV_EXIT:
+ break;
+ }
+}
+
+/*
+FSM_STATE()
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ break;
+ case EV_MOTION_DONE:
+ case EV_ACTION_DONE:
+ 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_PUCK_REACHABLE:
+ case EV_STACK_FULL:
+ case EV_ACTION_ERROR:
case EV_ACT_FATAL_ERROR:
case EV_START:
DBG_PRINT_EVENT("unhandled event");
break;
}
}
+*/
--- /dev/null
+/*
+ * homologation.cc 08/04/29
+ *
+ * Robot's control program intended for homologation (approval phase) on Eurobot 2009.
+ *
+ * Copyright: (c) 2009 CTU Dragons
+ * CTU FEE - Department of Control Engineering
+ * License: GNU GPL v.2
+ */
+
+#ifndef DEBUG
+#define DEBUG
+#endif
+
+#define FSM_MAIN
+#include <robodata.h>
+#include <robot.h>
+#include <fsm.h>
+#include <unistd.h>
+#include <math.h>
+#include <movehelper.h>
+#include <map.h>
+#include <sharp.h>
+#include <robomath.h>
+#include <string.h>
+#include <robodim.h>
+#include <error.h>
+
+/************************************************************************
+ * Trajectory constraints used, are initialized in the init state
+ ************************************************************************/
+
+struct TrajectoryConstraints tcFast, tcSlow;
+
+/************************************************************************
+ * FSM STATES DECLARATION
+ ************************************************************************/
+
+/* initial and starting states */
+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(get_out_the_construction_zone);
+
+/************************************************************************
+ * INITIAL AND STARTING STATES
+ ************************************************************************/
+
+FSM_STATE(init)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ tcFast = trajectoryConstraintsDefault;
+ tcFast.maxv = 1.5;
+ tcSlow = trajectoryConstraintsDefault;
+ tcSlow.maxv = 0.2;
+ FSM_TRANSITION(wait_for_start);
+ break;
+ default:
+ break;
+ }
+}
+
+FSM_STATE(wait_for_start)
+{
+ #ifdef COMPETITON
+ printf("COMPETITION mode set");
+ #endif
+ switch (FSM_EVENT) {
+#ifdef WAIT_FOR_START
+ case EV_START: {
+ pthread_t thid;
+
+ /* start competition timer */
+ pthread_create(&thid, NULL, wait_for_end, NULL);
+ pthread_create(&thid, NULL, wait_to_deposition, NULL);
+ }
+#else
+ case EV_ENTRY:
+#endif
+ 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_TIMER(2000); // wait for the localization to settle down
+ break;
+ case EV_RETURN:
+ case EV_TIMER:
+ FSM_TRANSITION(approach_first_puck);
+ break;
+ 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_PUCK_REACHABLE:
+ case EV_ACTION_DONE:
+ case EV_ACTION_ERROR:
+ case EV_ACT_FATAL_ERROR:
+ case EV_MOTION_DONE:
+ DBG_PRINT_EVENT("unhandled event");
+ break;
+ case EV_EXIT:
+ 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_ACTION_DONE:
+ 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_ACTION_ERROR:
+ case EV_ACT_FATAL_ERROR:
+ case EV_START:
+ 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_ACTION_DONE:
+ FSM_TRANSITION(get_out_the_construction_zone);
+ break;
+ case EV_PUCK_REACHABLE:
+ 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_ACTION_ERROR:
+ case EV_ACT_FATAL_ERROR:
+ case EV_START:
+ DBG_PRINT_EVENT("unhandled event");
+ break;
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(get_out_the_construction_zone)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ robot_move_by(-0.15, NO_TURN(), &tcSlow);
+ break;
+ case EV_MOTION_DONE:
+ break;
+ case EV_ACTION_DONE:
+ 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_PUCK_REACHABLE:
+ case EV_STACK_FULL:
+ case EV_ACTION_ERROR:
+ case EV_ACT_FATAL_ERROR:
+ case EV_START:
+ DBG_PRINT_EVENT("unhandled event");
+ break;
+ case EV_EXIT:
+ break;
+ }
+}
+
+int main()
+{
+ int rv;
+
+ rv = robot_init();
+ if (rv) error(1, errno, "robot_init() returned %d\n", rv);
+
+ robot.obstacle_avoidance_enabled = true;
+
+ robot.fsm.main.debug_states = 1;
+ robot.fsm.motion.debug_states = 1;
+ robot.fsm.act.debug_states = 1;
+
+ robot.fsm.main.state = &fsm_state_main_init;
+ //robot.fsm.main.transition_callback = trans_callback;
+ //robot.fsm.motion.transition_callback = move_trans_callback;
+
+ rv = robot_start();
+ if (rv) error(1, errno, "robot_start() returned %d\n", rv);
+
+ robot_destroy();
+
+ return 0;
+}
+
+
"EV_SHORT_TIME_TO_END" : "Time to go for the deposition regarless of what do we have.",
"EV_LASER_POWER" : "", # FIXME: what's this? (F.J.)
- "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_ACTION_DONE" : "ACT FSM signals that the requested action has finished successfully",
"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_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_STACK_FULL" : "ACT FSM signals that the puck stack is full",
"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.",
projects / eurobot / public.git / commitdiff