src/robofsm/eb2008/test/goto.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_eb2008.h>
  13 #include <robot_eb2008.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
  24
  25 FSM_STATE_DECL(robot_goto_test);
  26
  27 FSM_STATE(init)
  28 {
  29         if (FSM_EVENT == EV_ENTRY) {
  30                 /* Where we are at the begining? */
  31                 robot_set_est_pos_trans(1, 0.5, DEG2RAD(0));
  32                 FSM_TRANSITION(robot_goto_test);
  33         }
  34 }
  35
  36 FSM_STATE(robot_goto_test)
  37 {
  38         double goalx, goaly;
  39         switch (FSM_EVENT) {
  40                 case EV_ENTRY:
  41                         ShmapSetRectangleFlag(0.0, 0.0, 0.199, 2.1, MAP_FLAG_WALL, 0);
  42                         ShmapSetRectangleFlag(0.0, 0.0, 3.0, 0.199, MAP_FLAG_WALL, 0);
  43                         ShmapSetRectangleFlag(0.0, 1.901, 3.0, 2.1, MAP_FLAG_WALL, 0);
  44                         ShmapSetRectangleFlag(2.801, 0.0, 3.0, 2.1, MAP_FLAG_WALL, 0);
  45                         ShmapSetRectangleFlag(1.3, 0.7, 1.7, 1.0, MAP_FLAG_SIMULATED_WALL, 0);
  46                         //ShmapSetRectangleFlag(1.8, 1.5, 2.3, 1.8, MAP_FLAG_SIMULATED_WALL, 0);
  47                         do {
  48                                 goalx = (rand()%(MAP_PLAYGROUND_WIDTH_MM-50)+25)/1000.0;
  49                                 goaly = ((rand()%(MAP_PLAYGROUND_HEIGHT_MM-50)+25)/1000.0);
  50 //                              goalx = robot.act_pos->x + (rand()%100)/1000.0;
  51 //                              goaly = robot.act_pos->y + (rand()%100)/1000.0;
  52                         } while (!ShmapIsFreePoint(goalx, goaly));
  53
  54                         robot_goto_notrans(goalx, goaly, NO_TURN(), NULL);
  55                         break;
  56                 case EV_GOAL_NOT_REACHABLE:
  57                         DBG("Goal is not reachable\n");
  58                         FSM_TIMER(1000);
  59                         break;
  60                 case EV_MOTION_DONE:
  61                 case EV_TIMER:
  62                         FSM_TRANSITION(robot_goto_test);
  63                         break;
  64                 case EV_START:
  65                         /* do nothing */
  66                         break;
  67                 case EV_EXIT:
  68                         //ShmapFree();
  69                         break;
  70                 default:
  71                         break;
  72         }
  73 }
  74
  75 int main()
  76 {
  77         /* robot initialization */
  78         robot_init();
  79         srand(time(0));
  80         FSM(MAIN)->debug_states = 1;
  81         FSM(MOTION)->debug_states = 1;
  82
  83         FSM(MAIN)->state = &fsm_state_main_init;
  84
  85         /* start threads and wait */
  86         robot_start();
  87         robot_wait();
  88
  89         /* clean up */
  90         robot_destroy();
  91
  92         return 0;
  93 }