2 * rectangle.cc 07/08/01
4 * Movement test: move in a rectangle.
6 * Copyright: (c) 2007 CTU Dragons
7 * CTU FEE - Department of Control Engineering
12 #include <movehelper.h>
20 #include <boost/statechart/asynchronous_state_machine.hpp>
21 #include <boost/statechart/custom_reaction.hpp>
22 #include <boost/statechart/transition.hpp>
23 #include "../timedFSM.h"
26 UL_LOG_CUST(ulogd_rectangle); /* Log domain name = ulogd + name of the file */
32 struct FSMMain : boost::statechart::asynchronous_state_machine<FSMMain, init, Scheduler> {
33 FSMMain(my_context ctx) : my_base(ctx) {
34 printf("%s\n", __FUNCTION__);
38 struct init : TimedState<init, FSMMain> {
40 init(my_context ctx) : base_state(ctx) {
41 runTimer(timeTest, 10, new EV_TIMER());
43 sc::result react (const EV_TIMER &) {
44 robot.set_est_pos_trans(0.5, 0.5, DEG2RAD(0));
45 return transit<rectangle>();
47 typedef sc::custom_reaction<EV_TIMER> reactions;
51 void follow_rectangle()
53 double rectangle[][2] = { { 0.5, 0.5 },
57 static bool backward = false;
58 struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
64 /* Allocate new trajectory */
66 robot.move_helper.trajectory_new(&tc);
68 robot.move_helper.trajectory_new_backward(&tc);
71 for (i=0; i<sizeof(rectangle)/sizeof(rectangle[0]); i++) {
72 if (!backward) j=(i+1)%4;
75 robot.move_helper.add_point_trans(rectangle[j][0], rectangle[j][1]);
77 robot.move_helper.add_final_point_notrans(rectangle[j][0], rectangle[j][1], TURN(0));
80 //backward = !backward;
83 struct rectangle : TimedSimpleState<rectangle, FSMMain>
88 typedef sc::transition<EV_MOTION_DONE, Wait> reactions;
91 struct Wait : TimedState<Wait, FSMMain>
94 Wait(my_context ctx) : base_state(ctx) {
95 runTimer(waitTout, 1000, new EV_TIMER());
97 typedef sc::transition<EV_TIMER, rectangle> reactions;
100 /*void transition_callback(struct robo_fsm *fsm)
102 ul_loginf("fsmcb %s: %s(%s)\n", fsm->debug_name, fsm->state_name, fsm_event_str(fsm->events[fsm->ev_head]));
110 if (rv) error(1, errno, "robot_init() returned %d\n", rv);
112 //robot.fsm.main.transition_callback = transition_callback;
113 //robot.fsm.motion.transition_callback = transition_callback;
114 //robot.fsm.main.debug_states = 1;
116 if (rv) error(1, errno, "robot_start() returned %d\n", rv);