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                         if (!robot.localization_works) {
  41                                 robot_set_est_pos_trans(PLAYGROUND_WIDTH_M/2,
  42                                                         PLAYGROUND_HEIGHT_M/2,
  43                                                         DEG2RAD(0));
  44                         }
  45                         break;
  46                 default:;
  47         }
  48 }
  49
  50 FSM_STATE(robot_goto_test)
  51 {
  52         double goalx, goaly;
  53         static bool direction = false;
  54         struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
  55         switch (FSM_EVENT) {
  56                 case EV_ENTRY:
  57                         double px, py, phi;
  58                         robot_get_est_pos(&px, &py, &phi);
  59                         if (px < 1.5) {
  60                                 goalx = MAP_PLAYGROUND_WIDTH_M - 0.35;
  61                         } else {
  62                                 goalx = 0.35;
  63                         }
  64                         goaly = MAP_PLAYGROUND_HEIGHT_M / 2.0;
  65
  66                         //tc.maxv = 0.5;
  67                         ul_logdeb("making the robot move to %f, %f\n", goalx, goaly);
  68                         robot_goto_notrans(goalx, goaly, NO_TURN(), &tc);
  69
  70                         direction = !direction;
  71                         break;
  72                 case EV_MOTION_ERROR:
  73                         FSM_TRANSITION(error);
  74                         break;
  75                 case EV_MOTION_DONE:
  76                         ul_logdeb("Goal is not reachable\n");
  77                         FSM_TRANSITION(robot_goto_test);
  78                         break;
  79                 case EV_EXIT:
  80                         //ShmapFree();
  81                         break;
  82                 default:
  83                         break;
  84         }
  85 }
  86
  87 FSM_STATE(error)
  88 {
  89 }
  90
  91 int main()
  92 {
  93         int rv;
  94
  95         rv = robot_init();
  96         if (rv) error(1, errno, "robot_init() returned %d\n", rv);
  97
  98         srand(time(0));
  99         robot.fsm.main.debug_states = 1;
 100         robot.fsm.motion.debug_states = 1;
 101         robot.obstacle_avoidance_enabled = true;
 102
 103         robot.fsm.main.state = &fsm_state_main_init;
 104
 105         rv = robot_start();
 106         if (rv) error(1, errno, "robot_start() returned %d\n", rv);
 107
 108         robot_destroy();
 109
 110         return 0;
 111 }