src/robofsm/test/rectangle.cc

   1 /*
   2  * rectangle.cc                 07/08/01
   3  *
   4  * Movement test: move in a rectangle.
   5  *
   6  * Copyright: (c) 2007 CTU Dragons
   7  *            CTU FEE - Department of Control Engineering
   8  * License: GNU GPL v.2
   9  */
  10
  11 //#define FSM_MAIN
  12 #include <movehelper.h>
  13 #include <robot.h>
  14 #include <trgen.h>
  15 #include <robomath.h>
  16 #include <string.h>
  17 #include <error.h>
  18 #include <ul_log.h>
  19 #include <events.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"
  24 #include "../robot.h"
  25
  26 UL_LOG_CUST(ulogd_rectangle); /* Log domain name = ulogd + name of the file */
  27
  28 struct rectangle;
  29 struct Wait;
  30 struct init;
  31
  32 struct FSMMain : boost::statechart::asynchronous_state_machine< FSMMain, init, Scheduler > {
  33         FSMMain(my_context ctx) : my_base(ctx) {
  34                 printf("%s\n", __FUNCTION__);
  35         }
  36 };
  37
  38 struct init : TimedState< init, FSMMain > {
  39         Timer timeTest;
  40         init(my_context ctx) : base_state(ctx) {
  41                 runTimer(timeTest, 10, new evTimer());
  42         }
  43         sc::result react (const evTimer &) {
  44                 robot.set_est_pos_trans(0.5, 0.5, DEG2RAD(0));
  45                 return transit< rectangle >();
  46         }
  47         typedef sc::custom_reaction< evTimer > reactions;
  48 };
  49
  50
  51 void follow_rectangle()
  52 {
  53         double rectangle[][2] = { { 0.5, 0.5 },
  54                                   { 1.5, 0.5 },
  55                                   { 1.5, 1.5 },
  56                                   { 0.5, 1.5 }};
  57         static bool backward = false;
  58         struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
  59         tc.maxv = 1.0;
  60         tc.maxacc = 0.5;
  61         tc.maxomega = 2;
  62         tc.maxe = 0.200;
  63
  64         /* Allocate new trajectory */
  65         if (!backward)
  66                 robot.move_helper.trajectory_new(&tc);
  67         else
  68                 robot.move_helper.trajectory_new_backward(&tc);
  69
  70         unsigned i, j;
  71         for (i=0; i<sizeof(rectangle)/sizeof(rectangle[0]); i++) {
  72                 if (!backward) j=(i+1)%4;
  73                 else j=(3-i);
  74                 if (i<3) {
  75                         robot.move_helper.add_point_trans(rectangle[j][0], rectangle[j][1]);
  76                 } else {
  77                         robot.move_helper.add_final_point_notrans(rectangle[j][0], rectangle[j][1], TURN(0));
  78                 }
  79         }
  80         //backward = !backward;
  81 }
  82
  83 struct rectangle : TimedSimpleState< rectangle, FSMMain >
  84 {
  85         rectangle() {
  86                 follow_rectangle();
  87         }
  88         typedef sc::transition< evMotionDone, Wait > reactions;
  89 };
  90
  91 struct Wait : TimedState< Wait, FSMMain >
  92 {
  93         Timer waitTout;
  94         Wait(my_context ctx) : base_state(ctx) {
  95                 runTimer(waitTout, 1000, new evTimer());
  96         }
  97         typedef sc::transition< evTimer, rectangle> reactions;
  98 };
  99
 100 /*void transition_callback(struct robo_fsm *fsm)
 101 {
 102     ul_loginf("fsmcb %s: %s(%s)\n", fsm->debug_name, fsm->state_name, fsm_event_str(fsm->events[fsm->ev_head]));
 103 }*/
 104
 105 int main()
 106 {
 107         int rv;
 108         robot.MAIN = robot.sched.create_processor<FSMMain>();
 109         rv = robot.init();
 110         if (rv) error(1, errno, "robot_init() returned %d\n", rv);
 111
 112         //robot.fsm.main.transition_callback = transition_callback;
 113         //robot.fsm.motion.transition_callback = transition_callback;
 114         //robot.fsm.main.debug_states = 1;
 115         rv = robot.start();
 116         if (rv) error(1, errno, "robot_start() returned %d\n", rv);
 117
 118         robot.destroy();
 119
 120         return 0;
 121 }