4 * Movement test: goto test.
6 * Copyright: (c) 2007 CTU Dragons
7 * CTU FEE - Department of Control Engineering
12 #include <movehelper_eb2008.h>
13 #include <robot_eb2008.h>
15 #include <path_planner.h>
25 FSM_STATE_DECL(robot_goto_test);
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);
36 FSM_STATE(robot_goto_test)
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);
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));
54 robot_goto_notrans(goalx, goaly, NO_TURN(), NULL);
56 case EV_GOAL_NOT_REACHABLE:
57 DBG("Goal is not reachable\n");
62 FSM_TRANSITION(robot_goto_test);
77 /* robot initialization */
80 FSM(MAIN)->debug_states = 1;
81 FSM(MOTION)->debug_states = 1;
83 FSM(MAIN)->state = &fsm_state_main_init;
85 /* start threads and wait */