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
  20 UL_LOG_CUST(ulogd_rectangle); /* Log domain name = ulogd + name of the file */
  21
  22 FSM_STATE_DECL(rectangle);
  23 FSM_STATE_DECL(turn);
  24 FSM_STATE_DECL(wait);
  25
  26 FSM_STATE(init) {
  27         /* Where we are at the begining? */
  28         switch (FSM_EVENT) {
  29                 case EV_ENTRY:
  30                         FSM_TIMER(10);
  31                         break;
  32                 case EV_TIMER:
  33                         FSM_TRANSITION(rectangle);
  34                         robot_set_est_pos_trans(0.5, 0.5, DEG2RAD(0));
  35                         break;
  36                 default:;
  37         }
  38 }
  39
  40
  41 void follow_rectangle()
  42 {
  43         double rectangle[][2] = { { 0.5, 0.5 },
  44                                   { 1.5, 0.5 },
  45                                   { 1.5, 1.5 },
  46                                   { 0.5, 1.5 }};
  47         static bool backward = false;
  48         struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
  49         tc.maxv = 1.0;
  50         tc.maxacc = 0.5;
  51         tc.maxomega = 2;
  52         tc.maxe = 0.200;
  53
  54         /* Allocate new trajectory */
  55         if (!backward)
  56                 robot_trajectory_new(&tc);
  57         else
  58                 robot_trajectory_new_backward(&tc);
  59
  60         unsigned i, j;
  61         for (i=0; i<sizeof(rectangle)/sizeof(rectangle[0]); i++) {
  62                 if (!backward) j=(i+1)%4;
  63                 else j=(3-i);
  64                 if (i<3) {
  65                         robot_trajectory_add_point_trans(rectangle[j][0], rectangle[j][1]);
  66                 } else {
  67                         robot_trajectory_add_final_point_trans(rectangle[j][0], rectangle[j][1], TURN(0));
  68                 }
  69         }
  70         //backward = !backward;
  71 }
  72
  73 FSM_STATE(rectangle)
  74 {
  75         switch (FSM_EVENT) {
  76                 case EV_ENTRY:
  77                         follow_rectangle();
  78                         break;
  79                 case EV_MOTION_DONE:
  80                         //FSM_TRANSITION(turn);
  81                         FSM_TRANSITION(wait);
  82                         break;
  83                 default:
  84                         break;
  85         }
  86 }
  87
  88 FSM_STATE(turn)
  89 {
  90         switch (FSM_EVENT) {
  91                 case EV_ENTRY:
  92                         robot_trajectory_new(NULL);
  93                         robot_trajectory_add_final_point_trans(1, 0.5,
  94                                                 TURN_CCW(DEG2RAD(0)));
  95                         break;
  96                 case EV_MOTION_DONE:
  97                         FSM_TRANSITION(wait);
  98                         break;
  99                 default:
 100                         break;
 101         }
 102 }
 103
 104
 105 FSM_STATE(wait)
 106 {
 107         FSM_TIMER(1000);
 108         switch (FSM_EVENT) {
 109                 case EV_TIMER:
 110                         FSM_TRANSITION(rectangle);
 111                         break;
 112                 default:
 113                         break;
 114         }
 115 }
 116
 117 void transition_callback(struct robo_fsm *fsm)
 118 {
 119     ul_loginf("fsmcb %s: %s(%s)\n", fsm->debug_name, fsm->state_name, fsm_event_str(fsm->events[fsm->ev_head]));
 120 }
 121
 122
 123
 124 int main()
 125 {
 126         int rv;
 127
 128         rv = robot_init();
 129         if (rv) error(1, errno, "robot_init() returned %d\n", rv);
 130
 131         //robot.fsm.main.transition_callback = transition_callback;
 132         //robot.fsm.motion.transition_callback = transition_callback;
 133         //robot.fsm.main.debug_states = 1;
 134         robot.fsm.motion.debug_states = 1;
 135
 136         robot.fsm.main.state = &fsm_state_main_init;
 137
 138         rv = robot_start();
 139         if (rv) error(1, errno, "robot_start() returned %d\n", rv);
 140
 141         robot_destroy();
 142
 143         return 0;
 144 }