namespace sc = boost::statechart;
/**
- * EV_TIMER - Timeout.
+ * evTimer - Basic event for timeout.
*/
-struct EV_TIMER : sc::event< EV_TIMER >{
+struct evTimer : sc::event< evTimer > {
void *ev_ptr;
- EV_TIMER(void *par = NULL) : ev_ptr(par) {};
+ evTimer(void *par = NULL) : ev_ptr(par) {};
};
/**
- * EV_RETURN - Return from submachine.
+ * evReturn - Return from submachine.
*/
-struct EV_RETURN : sc::event< EV_RETURN >{
+struct evReturn : sc::event< evReturn > {
void *ev_ptr;
- EV_RETURN(void *par = NULL) : ev_ptr(par) {};
+ evReturn(void *par = NULL) : ev_ptr(par) {};
};
/**
- * EV_START - Changed state of start connector.
+ * evStart - Changed state of start connector.
*/
-struct EV_START : sc::event< EV_START >{
+struct evStart : sc::event< evStart > {
void *ev_ptr;
- EV_START(void *par = NULL) : ev_ptr(par) {};
+ evStart(void *par = NULL) : ev_ptr(par) {};
};
/**
- * EV_JAWS_DONE - Jaws are in requested position.
+ * evJawsDone - Jaws are in requested position.
*/
-struct EV_JAWS_DONE : sc::event<EV_JAWS_DONE>{
+struct evJawsDone : sc::event< evJawsDone > {
void *ev_ptr;
- EV_JAWS_DONE(void *par = NULL) : ev_ptr(par) {};
+ evJawsDone(void *par = NULL) : ev_ptr(par) {};
};
/**
- * EV_LIFT_DONE - Lift is in requested position.
+ * evLiftDone - Lift is in requested position.
*/
-struct EV_LIFT_DONE : sc::event<EV_LIFT_DONE>{
+struct evLiftDone : sc::event< evLiftDone > {
void *ev_ptr;
- EV_LIFT_DONE(void *par = NULL) : ev_ptr(par) {};
+ evLiftDone(void *par = NULL) : ev_ptr(par) {};
};
/**
- * EV_SWITCH_STRATEGY - Switch between strategies.
+ * evSwitchStrategy - Switch between strategies.
*/
-struct EV_SWITCH_STRATEGY : sc::event<EV_SWITCH_STRATEGY>{
+struct evSwitchStrategy : sc::event< evSwitchStrategy > {
void *ev_ptr;
- EV_SWITCH_STRATEGY(void *par = NULL) : ev_ptr(par) {};
+ evSwitchStrategy(void *par = NULL) : ev_ptr(par) {};
};
/**
- * EV_MOTION_DONE - Previously submitted motion is finished.
+ * evMotionDone - Previously submitted motion is finished.
*/
-struct EV_MOTION_DONE : sc::event<EV_MOTION_DONE>{
+struct evMotionDone : sc::event< evMotionDone > {
void *ev_ptr;
- EV_MOTION_DONE(void *par = NULL) : ev_ptr(par) {};
+ evMotionDone(void *par = NULL) : ev_ptr(par) {};
};
/**
- * EV_MOTION_ERROR - "Motion FSM cannot finish the requested command even after several attempts (obstacles on the road, lost several times etc.).
+ * evMotionError - "Motion FSM cannot finish the requested command even after several attempts (obstacles on the road, lost several times etc.).
*/
-struct EV_MOTION_ERROR : sc::event<EV_MOTION_ERROR>{
+struct evMotionError : sc::event< evMotionError > {
void *ev_ptr;
- EV_MOTION_ERROR(void *par = NULL) : ev_ptr(par) {};
+ evMotionError(void *par = NULL) : ev_ptr(par) {};
};
/**
- * EV_MOVE_STOP - Stop current motion as fast as possible.
+ * evMoveStop - Stop current motion as fast as possible.
*/
-struct EV_MOVE_STOP : sc::event<EV_MOVE_STOP>{
+struct evMoveStop : sc::event< evMoveStop > {
void *ev_ptr;
- EV_MOVE_STOP(void *par = NULL) : ev_ptr(par) {};
+ evMoveStop(void *par = NULL) : ev_ptr(par) {};
};
/**
- * EV_NEW_TARGET - New target (point or trajectory) is sent.
+ * evNewTarget - New target (point or trajectory) is sent.
*/
-struct EV_NEW_TARGET : sc::event<EV_NEW_TARGET>{
+struct evNewTarget : sc::event< evNewTarget > {
void *ev_ptr;
- EV_NEW_TARGET(void *par = NULL) : ev_ptr(par) {};
+ evNewTarget(void *par = NULL) : ev_ptr(par) {};
};
/**
- * EV_TRAJECTORY_LOST - Actual position of the robot is too far from reference.
+ * evTrajectoryLost - Actual position of the robot is too far from reference.
*/
-struct EV_TRAJECTORY_LOST : sc::event<EV_TRAJECTORY_LOST>{
+struct evTrajectoryLost : sc::event< evTrajectoryLost > {
void *ev_ptr;
- EV_TRAJECTORY_LOST(void *par = NULL) : ev_ptr(par) {};
+ evTrajectoryLost(void *par = NULL) : ev_ptr(par) {};
};
/**
- * EV_TRAJECTORY_DONE - Reference position is at the end of the previously submitted trajectory.
+ * evTrajectoryDone - Reference position is at the end of the previously submitted trajectory.
*/
-struct EV_TRAJECTORY_DONE : sc::event<EV_TRAJECTORY_DONE>{
+struct evTrajectoryDone : sc::event< evTrajectoryDone > {
void *ev_ptr;
- EV_TRAJECTORY_DONE(void *par = NULL) : ev_ptr(par) {};
+ evTrajectoryDone(void *par = NULL) : ev_ptr(par) {};
};
/**
- * EV_OBSTACLE - An obstacle is detected on actual trajectory - we must avoid it.
+ * evObstacle - An obstacle is detected on actual trajectory - we must avoid it.
*/
-struct EV_OBSTACLE : sc::event<EV_OBSTACLE>{
+struct evObstacle : sc::event< evObstacle > {
void *ev_ptr;
- EV_OBSTACLE(void *par = NULL) : ev_ptr(par) {};
+ evObstacle(void *par = NULL) : ev_ptr(par) {};
};
/**
- * EV_OBSTACLE_BEHIND - An obstacle is behind the robot.
+ * evObstacleBehind - An obstacle is behind the robot.
*/
-struct EV_OBSTACLE_BEHIND : sc::event<EV_OBSTACLE_BEHIND>{
+struct evObstacleBehind : sc::event< evObstacleBehind > {
void *ev_ptr;
- EV_OBSTACLE_BEHIND(void *par = NULL) : ev_ptr(par) {};
+ evObstacleBehind(void *par = NULL) : ev_ptr(par) {};
};
/**
- * EV_OBSTACLE_SIDE - An obstacle is beside the robot.
+ * evObstacleSide - An obstacle is beside the robot.
*/
-struct EV_OBSTACLE_SIDE : sc::event<EV_OBSTACLE_SIDE>{
+struct evObstacleSide : sc::event< evObstacleSide > {
void *ev_ptr;
- EV_OBSTACLE_SIDE(void *par = NULL) : ev_ptr(par) {};
+ evObstacleSide(void *par = NULL) : ev_ptr(par) {};
};
/**
- * EV_COMPETITON_TOUT - An obstacle is beside the robot.
+ * evCompetitionTimeout - An obstacle is beside the robot.
*/
-struct EV_COMPETITON_TOUT : sc::event<EV_COMPETITON_TOUT>{
+struct evCompetitionTimeout : sc::event< evCompetitionTimeout > {
void *ev_ptr;
- EV_COMPETITON_TOUT(void *par = NULL) : ev_ptr(par) {};
+ evCompetitionTimeout(void *par = NULL) : ev_ptr(par) {};
};
#endif
\ No newline at end of file
struct Competing;
struct approachBuillon;
-struct FSMMain : boost::statechart::asynchronous_state_machine<FSMMain, MainInit, Scheduler> {
+struct FSMMain : sc::asynchronous_state_machine< FSMMain, MainInit, Scheduler > {
FSMMain(my_context ctx) : my_base(ctx) {
- printf("%s\n", __FUNCTION__);
}
};
-struct MainInit : TimedState<MainInit, FSMMain> {
+struct MainInit : TimedState< MainInit, FSMMain > {
Timer setTime;
MainInit(my_context ctx) : base_state(ctx) {
- printf("%s\n", __FUNCTION__);
#ifdef WAIT_FOR_START
- runTimer(setTime, 1000, new EV_TIMER());
+ runTimer(setTime, 1000, new evTimer());
#else
- robot.sched.queue_event(robot.MAIN, new EV_START());
+ robot.sched.queue_event(robot.MAIN, new evStart());
#endif
}
- sc::result react(const EV_START &) {
+ sc::result react(const evStart &) {
// actuators_home();
// set_initial_position();
- printf("%s EV_START\n", __FUNCTION__);
return transit<Competing>();
}
- sc::result react(const EV_TIMER) {
- runTimer(setTime, 1000, new EV_TIMER());
+ sc::result react(const evTimer) {
+ runTimer(setTime, 1000, new evTimer());
// We set initial position periodically in
// order for it to be updated on the display
// if the team color is changed during waiting
return discard_event();
}
typedef boost::mpl::list<
- sc::custom_reaction<EV_START>,
- sc::custom_reaction<EV_TIMER> > reactions;
+ sc::custom_reaction< evStart >,
+ sc::custom_reaction< evTimer > > reactions;
};
-struct Competing : TimedState<Competing, FSMMain, approachBuillon> {
+struct Competing : TimedState< Competing, FSMMain, approachBuillon > {
Timer competeTime;
Competing(my_context ctx) : base_state(ctx) {
clock_gettime(CLOCK_MONOTONIC, &(robot.startTime));
- runTimer(competeTime, 90000, new EV_COMPETITON_TOUT());
+ runTimer(competeTime, 90000, new evCompetitionTimeout());
}
- typedef boost::statechart::transition<EV_COMPETITON_TOUT, MainInit> reactions;
+ typedef boost::statechart::transition< evCompetitionTimeout, MainInit > reactions;
};
#endif
\ No newline at end of file
struct wait_and_try_again;
struct MotionBase;
-struct FSMMotion : boost::statechart::asynchronous_state_machine<FSMMotion, MotionBase, Scheduler> {
+struct FSMMotion : boost::statechart::asynchronous_state_machine< FSMMotion, MotionBase, Scheduler > {
FSMMotion(my_context ctx) : my_base(ctx) {}
};
-struct MotionBase : sc::simple_state<MotionBase, FSMMotion, wait_for_command> {
- sc::result react(const EV_TRAJECTORY_DONE &) {
+struct MotionBase : sc::simple_state< MotionBase, FSMMotion, wait_for_command > {
+ sc::result react(const evTrajectoryDone &) {
// DBG_PRINT_EVENT("unhandled event");
return discard_event();
}
- sc::result react(const EV_TRAJECTORY_LOST &) {
+ sc::result react(const evTrajectoryLost &) {
// DBG_PRINT_EVENT("unhandled event");
return discard_event();
}
- sc::result react(const EV_MOVE_STOP &) {
+ sc::result react(const evMoveStop &) {
// DBG_PRINT_EVENT("unhandled event");
return discard_event();
}
- sc::result react(const EV_OBSTACLE &) {
+ sc::result react(const evObstacle &) {
// DBG_PRINT_EVENT("unhandled event");
return discard_event();
}
- sc::result react(const EV_OBSTACLE_BEHIND &) {
+ sc::result react(const evObstacleBehind &) {
// DBG_PRINT_EVENT("unhandled event");
return discard_event();
}
- sc::result react(const EV_OBSTACLE_SIDE &) {
+ sc::result react(const evObstacleSide &) {
// DBG_PRINT_EVENT("unhandled event");
return discard_event();
}
- sc::result react(const EV_RETURN &) {
+ sc::result react(const evReturn &) {
// DBG_PRINT_EVENT("unhandled event");
return discard_event();
}
- sc::result react(const EV_NEW_TARGET &) {
+ sc::result react(const evNewTarget &) {
// DBG_PRINT_EVENT("unhandled event");
return discard_event();
}
typedef mpl::list<
- sc::custom_reaction<EV_TRAJECTORY_DONE>,
- sc::custom_reaction<EV_TRAJECTORY_LOST>,
- sc::custom_reaction<EV_MOVE_STOP>,
- sc::custom_reaction<EV_OBSTACLE_SIDE>,
- sc::custom_reaction<EV_OBSTACLE_BEHIND>,
- sc::custom_reaction<EV_OBSTACLE>,
- sc::custom_reaction<EV_RETURN>,
- sc::custom_reaction<EV_NEW_TARGET> > reactions;
+ sc::custom_reaction< evTrajectoryDone >,
+ sc::custom_reaction< evTrajectoryLost >,
+ sc::custom_reaction< evMoveStop >,
+ sc::custom_reaction< evObstacleSide >,
+ sc::custom_reaction< evObstacleBehind >,
+ sc::custom_reaction< evObstacle >,
+ sc::custom_reaction< evReturn >,
+ sc::custom_reaction< evNewTarget > > reactions;
};
-struct wait_for_command : TimedSimpleState<wait_for_command, MotionBase>
-{
+struct wait_for_command : TimedSimpleState< wait_for_command, MotionBase > {
target_status ret;
wait_for_command() {
stop();
}
- sc::result react(const EV_NEW_TARGET &event) {
+ sc::result react(const evNewTarget &event) {
ret = new_target((move_target*)event.ev_ptr);
delete event.ev_ptr;
switch (ret) {
case TARGET_OK: return transit<movement>();
case TARGET_INACC: return transit<wait_and_try_again>();
case TARGET_ERROR:
- robot.sched.queue_event(robot.MAIN, new EV_MOTION_DONE());
+ robot.sched.queue_event(robot.MAIN, new evMotionDone());
ul_logerr("Target error\n");
}
return discard_event();
}
- typedef sc::custom_reaction<EV_NEW_TARGET> reactions;
+ typedef sc::custom_reaction< evNewTarget > reactions;
};
-struct movement : TimedSimpleState<movement, MotionBase>
-{
- sc::result react(const EV_TRAJECTORY_DONE &) {
- robot.sched.queue_event(robot.MAIN, new EV_MOTION_DONE());
+struct movement : TimedSimpleState< movement, MotionBase > {
+ sc::result react(const evTrajectoryDone &) {
+ robot.sched.queue_event(robot.MAIN, new evMotionDone());
return transit<wait_for_command>();
}
- sc::result react(const EV_OBSTACLE &) {
+ sc::result react(const evObstacle &) {
switch (recalculate_trajectory()) {
// We can go to the target:
case TARGET_OK: return discard_event();
case TARGET_INACC: return transit<wait_and_try_again>();
// Fatal error during planning
case TARGET_ERROR:
- robot.sched.queue_event(robot.MAIN, new EV_MOTION_DONE());
+ robot.sched.queue_event(robot.MAIN, new evMotionDone());
ul_logerr("Target error\n");
/* FIXME (Filip): shouldn't transition to wait_for_command be here?
No, probably not, if an error occurs, robot won't move any more
}
return discard_event();
}
- sc::result react(const EV_OBSTACLE_BEHIND &) {
+ sc::result react(const evObstacleBehind &) {
if (back_bumpers())
return transit<wait_and_try_again>();
return discard_event();
}
- sc::result react(const EV_OBSTACLE_SIDE &) {
+ sc::result react(const evObstacleSide &) {
if (side_bumpers())
return transit<wait_and_try_again>();
return discard_event();
}
typedef mpl::list<
- sc::transition<EV_TRAJECTORY_LOST, lost>,
- sc::transition<EV_MOVE_STOP, wait_for_command>,
- sc::custom_reaction<EV_OBSTACLE_SIDE>,
- sc::custom_reaction<EV_OBSTACLE_BEHIND>,
- sc::custom_reaction<EV_OBSTACLE>,
- sc::custom_reaction<EV_TRAJECTORY_DONE> > reactions;
+ sc::transition< evTrajectoryLost, lost >,
+ sc::transition< evMoveStop, wait_for_command >,
+ sc::custom_reaction< evObstacleSide >,
+ sc::custom_reaction< evObstacleBehind >,
+ sc::custom_reaction< evObstacle >,
+ sc::custom_reaction< evTrajectoryDone > > reactions;
};
-struct lost : TimedState<lost, MotionBase>{
+struct lost : TimedState< lost, MotionBase > {
Timer lost_tout;
lost(my_context ctx) : base_state(ctx) {
stop();
- runTimer(lost_tout, 1000, new EV_TIMER());
+ runTimer(lost_tout, 1000, new evTimer());
}
- sc::result react(const EV_TIMER &) {
+ sc::result react(const evTimer &) {
switch (recalculate_trajectory()) {
case TARGET_OK: return transit<movement>();
case TARGET_INACC: return transit<wait_and_try_again>();
case TARGET_ERROR:
- robot.sched.queue_event(robot.MAIN, new EV_MOTION_DONE());
+ robot.sched.queue_event(robot.MAIN, new evMotionDone());
ul_logerr("Target error\n");
}
return discard_event();
}
typedef mpl::list<
- sc::custom_reaction<EV_TIMER>,
- sc::transition<EV_MOVE_STOP, wait_for_command> > reactions;
+ sc::custom_reaction< evTimer >,
+ sc::transition< evMoveStop, wait_for_command > > reactions;
};
-struct wait_and_try_again : TimedState<wait_and_try_again, MotionBase> {
+struct wait_and_try_again : TimedState< wait_and_try_again, MotionBase > {
Timer wait_tout;
wait_and_try_again(my_context ctx) : base_state(ctx) {
stop();
- runTimer(wait_tout, 1000, new EV_TIMER());
+ runTimer(wait_tout, 1000, new evTimer());
}
- sc::result react(const EV_TIMER &) {
+ sc::result react(const evTimer &) {
switch (recalculate_trajectory()) {
// We can go to the target:
case TARGET_OK: return transit<movement>();
/* FIXME (Filip): this could happen forever */
// Fatal error during planning
case TARGET_ERROR:
- robot.sched.queue_event(robot.MAIN, new EV_MOTION_DONE());
+ robot.sched.queue_event(robot.MAIN, new evMotionDone());
ul_logerr("Target error\n");
}
return discard_event();
}
- sc::result react(const EV_TRAJECTORY_DONE &) {
- robot.sched.queue_event(robot.MAIN, new EV_MOTION_DONE());
+ sc::result react(const evTrajectoryDone &) {
+ robot.sched.queue_event(robot.MAIN, new evTrajectoryDone());
return transit<wait_for_command>();
}
typedef mpl::list<
- sc::custom_reaction<EV_TIMER>,
- sc::custom_reaction<EV_TRAJECTORY_DONE>,
- sc::transition<EV_MOVE_STOP, wait_for_command> > reactions;
+ sc::custom_reaction< evTimer >,
+ sc::custom_reaction< evTrajectoryDone >,
+ sc::transition< evMoveStop, wait_for_command > > reactions;
};
* License: GNU GPL v.2
*/
-//#define FSM_MAIN
#include <robot.h>
-//#include <fsm.h>
#include <unistd.h>
#include <math.h>
#include <movehelper.h>
#include <error.h>
#include "actuators.h"
#include <trgen.h>
-//#include "match-timing.h"
-//#include "robot.h"
#include "movehelper.h"
#include <ul_log.h>
#include "fsmhead.hpp"
struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
-/************************************************************************
- * FSM STATES DECLARATION
- ************************************************************************/
-
-//FSM_STATE_DECL(init);
-//FSM_STATE_DECL(wait_for_start);
-/* movement states - buillon */
-//FSM_STATE_DECL(aproach_buillon);
-/* Pushing the bottle */
-//FSM_STATE_DECL(push_bottle);
-
-/*
-FSM_STATE(init)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
-
- FSM_TRANSITION(wait_for_start);
- break;
- default:
- break;
- }
-}
-*/
void set_initial_position()
{
// TODO define initial position
// unset the homing request
//act_lift(0, 0, 0);
}
-/*
-FSM_STATE(wait_for_start)
-{
- pthread_t thid;
- #ifdef WAIT_FOR_START
- ul_logdeb("WAIT_FOR_START mode set\n");
- #else
- ul_logdeb("WAIT_FOR_START mode NOT set\n");
- #endif
- #ifdef COMPETITION
- ul_logdeb("COMPETITION mode set\n");
- #else
- ul_logdeb("COMPETITION mode NOT set\n");
- #endif
- switch (FSM_EVENT) {
- case EV_ENTRY:
- pthread_create(&thid, NULL, timing_thread, NULL);
-#ifdef WAIT_FOR_START
- FSM_TIMER(1000);
- break;
-#endif
- case EV_START:*/
- /* start competition timer */
-/* sem_post(&robot.starT);
- actuators_home();
- set_initial_position();
- FSM_TRANSITION(aproach_buillon);
- break;
- case EV_TIMER:
- FSM_TIMER(1000);
- // We set initial position periodically in
- // order for it to be updated on the display
- // if the team color is changed during waiting
- // for start.
- set_initial_position();
- if (robot.start_state == START_PLUGGED_IN)
- actuators_home();
- break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_MOTION_DONE:
- //case EV_VIDLE_DONE:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- break;
- case EV_EXIT:
- break;
- default:
- break;
- }
-}
-
-FSM_STATE(aproach_buillon)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot.move_helper.trajectory_new(&tcSlow).
- add_point_trans( 0.65, PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0)).
- add_point_trans( 0.65, 1.3).
- add_final_point_trans( 0.5, 1.1, NO_TURN());
- break;
- case EV_MOTION_DONE:
- case EV_TIMER:
- FSM_TRANSITION(push_bottle);
- break;
- default:
- break;
- }
-}
-
-FSM_STATE(push_bottle)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot.move_helper.trajectory_new(&tcSlow).
- add_point_trans( 0.64, 0.7).
- add_final_point_trans( 0.64 + 0.08, ROBOT_AXIS_TO_FRONT_M + 0.05, ARRIVE_FROM(DEG2RAD(270), 0.10));
- break;
- case EV_MOTION_DONE:
- break;
- default:
- break;
- }
-}*/
struct push_bottle;
-struct approachBuillon : TimedSimpleState<approachBuillon, Competing> {
+struct approachBuillon : TimedSimpleState< approachBuillon, Competing > {
approachBuillon() {
printf("%s\n", __FUNCTION__);
set_initial_position();
add_point_trans( 0.65, 1.3).
add_final_point_trans( 0.5, 1.1, NO_TURN());
}
- sc::result react(const EV_MOTION_DONE &) {
+ sc::result react(const evMotionDone &) {
return transit<push_bottle>();
}
- typedef sc::custom_reaction<EV_MOTION_DONE> reactions;
+ typedef sc::custom_reaction< evMotionDone > reactions;
};
-struct push_bottle : TimedSimpleState<push_bottle, Competing> {
+struct push_bottle : TimedSimpleState< push_bottle, Competing > {
push_bottle() {
printf("%s\n", __FUNCTION__);
robot.move_helper.trajectory_new(&tcSlow).
add_point_trans( 0.64, 0.7).
add_final_point_trans( 0.64 + 0.08, ROBOT_AXIS_TO_FRONT_M + 0.05, ARRIVE_FROM(DEG2RAD(270), 0.10));
}
- sc::result react(const EV_MOTION_DONE &) {
- //robot.sched.terminate();
+ sc::result react(const evMotionDone &) {
+ robot.sched.terminate();
return discard_event();
}
- typedef sc::custom_reaction<EV_MOTION_DONE> reactions;
+ typedef sc::custom_reaction< evMotionDone > reactions;
};
/************************************************************************
* MAIN FUNCTION
robot.obstacle_avoidance_enabled = true;
robot.team_color = VIOLET;
- //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.team_color = BLUE;
- //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.sched();
robot.destroy();
return 0;
if (error > MAX_POS_ERROR_M) {
if (!lost_sent) {
lost_sent = true;
- robot.sched.queue_event(robot.MOTION, new EV_TRAJECTORY_LOST());
+ robot.sched.queue_event(robot.MOTION, new evTrajectoryLost());
}
} else {
lost_sent = false;
if (done) {
if (error < CLOSE_TO_TARGET_M || !done_sent) {
done_sent = true;
- robot.sched.queue_event(robot.MOTION, new EV_TRAJECTORY_DONE());
+ robot.sched.queue_event(robot.MOTION, new evTrajectoryDone());
}
} else {
done_sent = false;
continue;
if (map->cells[ycell][xcell].detected_obstacle > 0) {
if (sem_trywait(&recalculation_not_running) == 0) {
- robot.sched.queue_event(robot.MOTION, new EV_OBSTACLE());
+ robot.sched.queue_event(robot.MOTION, new evObstacle());
break;
}
}
target->heading = heading;
target->trajectory = t;
target->use_planning = false;
- robot.sched.queue_event(robot.MOTION, new EV_NEW_TARGET(target));
+ robot.sched.queue_event(robot.MOTION, new evNewTarget(target));
t = NULL;
} else {
ul_logerr("%s: Error - No trajectory\n", __FUNCTION__);
*/
void MoveHelper::stop()
{
- robot.sched.queue_event(robot.MOTION, new EV_MOVE_STOP());
+ robot.sched.queue_event(robot.MOTION, new evMoveStop());
}
if (tc) target->tc = *tc;
else target->tc = trajectoryConstraintsDefault;
- robot.sched.queue_event(robot.MOTION, new EV_NEW_TARGET(target));
+ robot.sched.queue_event(robot.MOTION, new evNewTarget(target));
}
/**
target->y = y;
target->heading = heading;
target->trajectory = t;
- robot.sched.queue_event(robot.MOTION, new EV_NEW_TARGET(target));
+ robot.sched.queue_event(robot.MOTION, new evNewTarget(target));
delete t;
}
robottype_roboorte_destroy(&orte);
- sched.terminate();
+// sched.terminate();
// fsm_destroy(&robot.fsm.main);
// fsm_destroy(&robot.fsm.motion);
// fsm_destroy(&robot.fsm.act);
robot.status[COMPONENT_START] = STATUS_OK;
/* Competition starts when plugged out */
if (instance->start_condition) {
- robot.sched.queue_event(robot.MAIN, new EV_START());
+ robot.sched.queue_event(robot.MAIN, new evStart());
//FSM_SIGNAL(MAIN, EV_START, NULL);
robot.start_state = COMPETITION_STARTED;
ul_logmsg("STARTED\n");
instance->response.right != last_response_right &&
instance->response.left == act.jaw_left_get_last_request() &&
instance->response.left == act.jaw_right_get_last_request())
- robot.sched.queue_event(robot.MAIN, new EV_JAWS_DONE());
-
-// FSM_SIGNAL(MAIN, EV_JAWS_DONE, NULL);
-
+ robot.sched.queue_event(robot.MAIN, new evJawsDone());
last_response_left = instance->response.left;
last_response_right = instance->response.right;
break;
if (instance->response != last_response &&
instance->response == act.lift_get_last_request())
- robot.sched.queue_event(robot.MAIN, new EV_LIFT_DONE());
-
-// FSM_SIGNAL(MAIN, EV_LIFT_DONE, NULL);
+ robot.sched.queue_event(robot.MAIN, new evLiftDone());
last_response = instance->response;
break;
case DEADLINE:
if (!last_strategy && instance->strategy) {
/* strategy switching */
- robot.sched.queue_event(robot.MAIN, new EV_SWITCH_STRATEGY());
-
-// FSM_SIGNAL(MAIN, EV_SWITCH_STRATEGY, NULL);
+ robot.sched.queue_event(robot.MAIN, new evSwitchStrategy());
}
last_strategy = instance->strategy;
break;
switch (info->status) {
case NEW_DATA:
if (instance->bumper_right_across || instance->bumper_left_across || instance->bumper_rear_left || instance->bumper_rear_right)
-robot.sched.queue_event(robot.MOTION, new EV_OBSTACLE_BEHIND());
+robot.sched.queue_event(robot.MOTION, new evObstacleBehind());
// FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
if (instance->bumper_left || instance->bumper_right) {
- robot.sched.queue_event(robot.MOTION, new EV_OBSTACLE_SIDE());
+ robot.sched.queue_event(robot.MOTION, new evObstacleSide());
// FSM_SIGNAL(MOTION, EV_OBSTACLE_SIDE, NULL);
}
struct on;
struct off;
-struct FSMMain : boost::statechart::asynchronous_state_machine<FSMMain, on, Scheduler> {
+struct FSMMain : boost::statechart::asynchronous_state_machine< FSMMain, on, Scheduler > {
FSMMain(my_context ctx) : my_base(ctx) {
printf("%s\n", __FUNCTION__);
}
};
-struct on : TimedState <on, FSMMain> {
+struct on : TimedState< on, FSMMain > {
Timer onTime;
on(my_context ctx) : base_state(ctx) {
act.camera_on();
- runTimer(onTime, 10000, new EV_TIMER());
+ runTimer(onTime, 10000, new evTimer());
}
- typedef sc::transition<EV_TIMER, off> reactions;
+ typedef sc::transition< evTimer, off > reactions;
};
-struct off : TimedState <off, FSMMain> {
+struct off : TimedState< off, FSMMain > {
Timer offTime;
off(my_context ctx) : base_state(ctx) {
act.camera_off();
- runTimer(offTime, 10000, new EV_TIMER());
+ runTimer(offTime, 10000, new evTimer());
}
- typedef sc::transition<EV_TIMER, on> reactions;
+ typedef sc::transition< evTimer, on > reactions;
};
int main(int argc, char *argv[])
struct Wait;
struct init;
-struct FSMMain : boost::statechart::asynchronous_state_machine<FSMMain, init, Scheduler> {
+struct FSMMain : boost::statechart::asynchronous_state_machine< FSMMain, init, Scheduler > {
FSMMain(my_context ctx) : my_base(ctx) {
printf("%s\n", __FUNCTION__);
}
};
-struct init : TimedState<init, FSMMain> {
+struct init : TimedState< init, FSMMain > {
Timer timeTest;
init(my_context ctx) : base_state(ctx) {
- runTimer(timeTest, 10, new EV_TIMER());
+ runTimer(timeTest, 10, new evTimer());
}
- sc::result react (const EV_TIMER &) {
+ sc::result react (const evTimer &) {
robot.set_est_pos_trans(0.5, 0.5, DEG2RAD(0));
- return transit<rectangle>();
+ return transit< rectangle >();
}
- typedef sc::custom_reaction<EV_TIMER> reactions;
+ typedef sc::custom_reaction< evTimer > reactions;
};
//backward = !backward;
}
-struct rectangle : TimedSimpleState<rectangle, FSMMain>
+struct rectangle : TimedSimpleState< rectangle, FSMMain >
{
rectangle() {
follow_rectangle();
}
- typedef sc::transition<EV_MOTION_DONE, Wait> reactions;
+ typedef sc::transition< evMotionDone, Wait > reactions;
};
-struct Wait : TimedState<Wait, FSMMain>
+struct Wait : TimedState< Wait, FSMMain >
{
Timer waitTout;
Wait(my_context ctx) : base_state(ctx) {
- runTimer(waitTout, 1000, new EV_TIMER());
+ runTimer(waitTout, 1000, new evTimer());
}
- typedef sc::transition<EV_TIMER, rectangle> reactions;
+ typedef sc::transition< evTimer, rectangle> reactions;
};
/*void transition_callback(struct robo_fsm *fsm)
projects / eurobot / public.git / commitdiff