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 FSM_STATE_DECL(robot_goto_test);
  25
  26 FSM_STATE(init)
  27 {
  28         if (FSM_EVENT == EV_ENTRY) {
  29                 /* Where we are at the begining? */
  30                 robot_set_est_pos_trans(ROBOT_WIDTH_M/2,
  31                                         PLAYGROUND_HEIGHT_M - ROBOT_AXIS_TO_BACK_M,
  32                                         DEG2RAD(-45));
  33                 FSM_TRANSITION(robot_goto_test);
  34         }
  35 }
  36
  37 FSM_STATE(robot_goto_test)
  38 {
  39         double goalx, goaly;
  40         switch (FSM_EVENT) {
  41                 case EV_ENTRY:
  42                         ShmapSetRectangleFlag(0.0, 0.0, 0.199, 2.1, MAP_FLAG_WALL, 0);
  43                         ShmapSetRectangleFlag(0.0, 0.0, 3.0, 0.199, MAP_FLAG_WALL, 0);
  44                         ShmapSetRectangleFlag(0.0, 1.901, 3.0, 2.1, MAP_FLAG_WALL, 0);
  45                         ShmapSetRectangleFlag(2.801, 0.0, 3.0, 2.1, MAP_FLAG_WALL, 0);
  46                         //ShmapSetRectangleFlag(1.3, 0.7, 1.7, 1.0, MAP_FLAG_SIMULATED_WALL, 0);
  47                         //ShmapSetRectangleFlag(1.8, 1.5, 2.3, 1.8, MAP_FLAG_SIMULATED_WALL, 0);
  48                         do {
  49                                 goalx = (rand()%(MAP_PLAYGROUND_WIDTH_MM-50)+25)/1000.0;
  50                                 goaly = ((rand()%(MAP_PLAYGROUND_HEIGHT_MM-50)+25)/1000.0);
  51 //                              goalx = robot.act_pos->x + (rand()%100)/1000.0;
  52 //                              goaly = robot.act_pos->y + (rand()%100)/1000.0;
  53                         } while (!ShmapIsFreePoint(goalx, goaly));
  54
  55                         robot_goto_notrans(goalx, goaly, NO_TURN(), NULL);
  56                         break;
  57                 case EV_GOAL_NOT_REACHABLE:
  58                         DBG("Goal is not reachable\n");
  59                         FSM_TIMER(1000);
  60                         break;
  61                 case EV_MOTION_DONE:
  62                 case EV_TIMER:
  63                         FSM_TRANSITION(robot_goto_test);
  64                         break;
  65                 case EV_START:
  66                         /* do nothing */
  67                         break;
  68                 case EV_EXIT:
  69                         //ShmapFree();
  70                         break;
  71                 default:
  72                         break;
  73         }
  74 }
  75
  76 void trans_callback(struct robo_fsm *fsm)
  77 {
  78         if(fsm->state_name!=NULL)
  79                 strncpy(robot.act_fsm_state_name, fsm->state_name, FSM_STATE_NAME_MAX_LEN);
  80
  81 }
  82 void move_trans_callback(struct robo_fsm *fsm)
  83 {
  84         if(fsm->state_name!=NULL)
  85                 strncpy(robot.move_fsm_state_name, fsm->state_name, FSM_STATE_NAME_MAX_LEN);
  86
  87 }
  88 int main()
  89 {
  90         /* robot initialization */
  91         robot_init();
  92         srand(time(0));
  93         FSM(MAIN)->debug_states = 1;
  94         FSM(MOTION)->debug_states = 1;
  95         robot.obstacle_avoidance_enabled = true;
  96
  97         robot.fsm[FSM_ID_MAIN].transition_callback = trans_callback;
  98         robot.fsm[FSM_ID_MOTION].transition_callback = move_trans_callback;
  99         FSM(MAIN)->state = &fsm_state_main_init;
 100
 101         /* start threads and wait */
 102         robot_start();
 103         robot_wait();
 104
 105         /* clean up */
 106         robot_destroy();
 107
 108         return 0;
 109 }