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