src/robofsm/test/lineavoid.cc

   1 /*
   2  * goto.cc                      07/08/01
   3  *
   4  * Movement test: goto test.
   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 <path_planner.h>
  16 #include <stdio.h>
  17 #include <stdlib.h>
  18 #include <sys/shm.h>
  19 #include <sys/stat.h>
  20 #include <math.h>
  21 #include <robomath.h>
  22 #include <map.h>
  23 #include <string.h>
  24 #include <error.h>
  25 #include <ul_log.h>
  26
  27 UL_LOG_CUST(ulogd_lineavoid); /* Log domain name = ulogd + name of the file */
  28
  29 FSM_STATE_DECL(robot_goto_test);
  30 FSM_STATE_DECL(error);
  31
  32 FSM_STATE(init) {
  33         /* Where we are at the begining? */
  34         switch (FSM_EVENT) {
  35                 case EV_ENTRY:
  36                         FSM_TIMER(2000);
  37                         break;
  38                 case EV_TIMER:
  39                         FSM_TRANSITION(robot_goto_test);
  40                         robot_set_est_pos_trans(PLAYGROUND_WIDTH_M/2,
  41                                                 PLAYGROUND_HEIGHT_M/2,
  42                                                 DEG2RAD(0));
  43                         break;
  44                 default:;
  45         }
  46 }
  47
  48 FSM_STATE(robot_goto_test)
  49 {
  50         double goalx, goaly;
  51         static bool direction = false;
  52         struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
  53         switch (FSM_EVENT) {
  54                 case EV_ENTRY:
  55                         double px, py, phi;
  56                         robot_get_est_pos(&px, &py, &phi);
  57                         if (px < 1.5) {
  58                                 goalx = MAP_PLAYGROUND_WIDTH_M - 0.35;
  59                         } else {
  60                                 goalx = 0.35;
  61                         }
  62                         goaly = MAP_PLAYGROUND_HEIGHT_M / 2.0;
  63
  64                         //tc.maxv = 0.5;
  65                         ul_logdeb("making the robot move to %f, %f\n", goalx, goaly);
  66                         robot_goto_notrans(goalx, goaly, NO_TURN(), &tc);
  67
  68                         direction = !direction;
  69                         break;
  70                 case EV_MOTION_ERROR:
  71                         FSM_TRANSITION(error);
  72                         break;
  73                 case EV_MOTION_DONE:
  74                         ul_logdeb("Goal is not reachable\n");
  75                         FSM_TRANSITION(robot_goto_test);
  76                         break;
  77                 case EV_EXIT:
  78                         //ShmapFree();
  79                         break;
  80                 default:
  81                         break;
  82         }
  83 }
  84
  85 FSM_STATE(error)
  86 {
  87 }
  88
  89 int main()
  90 {
  91         int rv;
  92
  93         rv = robot_init();
  94         if (rv) error(1, errno, "robot_init() returned %d\n", rv);
  95
  96         srand(time(0));
  97         robot.fsm.main.debug_states = 1;
  98         robot.fsm.motion.debug_states = 1;
  99         robot.obstacle_avoidance_enabled = true;
 100
 101         robot.fsm.main.state = &fsm_state_main_init;
 102
 103         rv = robot_start();
 104         if (rv) error(1, errno, "robot_start() returned %d\n", rv);
 105
 106         robot_destroy();
 107
 108         return 0;
 109 }