7 #include <movehelper.h>
14 #include "corns_configs.h"
15 #include "actuators.h"
17 #include "match-timing.h"
18 #include "eb2010misc.h"
21 #include "common-states.h"
23 /************************************************************************
24 * Functions used in and called from all the (almost identical)
25 * "wait for start" states in particular strategies.
26 ************************************************************************/
28 static void set_initial_position()
30 robot_set_est_pos_trans(ROBOT_AXIS_TO_FRONT_M,
31 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2),
35 static void actuators_home()
37 act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
43 robot.check_turn_safety = false;
44 pthread_create(&thid, NULL, timing_thread, NULL);
48 // We set initial position periodically in order for it to be updated
49 // on the display if the team color is changed during waiting for
53 set_initial_position();
54 if (robot.start_state == START_PLUGGED_IN)
60 sem_post(&robot.start);
62 set_initial_position();
67 robot.corns = get_all_corns(robot.corns_conf_side, robot.corns_conf_center);
70 /************************************************************************
71 * Trajectory constraints used; They are initialized in the main() function in competition.cc
72 ************************************************************************/
74 struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
76 #define VIDLE_TIMEOUT 2000
78 /************************************************************************
79 * States that form the "collect some oranges" subautomaton. Calling automaton
80 * SHOULD ALWAYS call the "approach_the_slope" state.
81 ************************************************************************/
83 bool inline is_ready_to_climb_the_slope(enum which_side which_slope, double x, double y) {
85 if (which_slope == MINE) {
86 ret = x < 0.5 && y > PLAYGROUND_HEIGHT_M - 0.5;
87 } else if (which_slope == OPPONENTS) {
88 ret = x > 0.5 && y > PLAYGROUND_HEIGHT_M - 0.5;
90 printf("ERROR: unknown side;"); robot_exit();
95 static enum which_side which_slope;
97 /* assures that the robot is near the slope rim; if outside our starting area, approach the slope first */
98 FSM_STATE(approach_the_slope)
102 which_slope = (enum which_side) FSM_EVENT_INT;
104 robot_get_est_pos_trans(&x, &y, &phi);
106 bool ready_to_climb_the_slope = is_ready_to_climb_the_slope(which_slope, x, y);
107 /* if necessary, approach the slope */
108 if (ready_to_climb_the_slope) {
109 FSM_TRANSITION(climb_the_slope);
112 x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.1, which_slope),
113 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.05,
114 ARRIVE_FROM(DEG2RAD(0),0.05),
120 FSM_TRANSITION(climb_the_slope);
126 case EV_ACTION_ERROR:
127 case EV_GOAL_NOT_REACHABLE:
128 DBG_PRINT_EVENT("unhandled event");
134 void inline enable_switches(bool enabled)
136 robot.use_left_switch = enabled;
137 robot.use_right_switch = enabled;
138 robot.use_back_switch = enabled;
141 FSM_STATE(climb_the_slope)
145 // disables using side switches on bumpers when going up
146 enable_switches(false);
147 act_vidle(VIDLE_DOWN, VIDLE_FAST_SPEED);
148 robot.ignore_hokuyo = true;
149 /* create the trajectory and go */
150 robot_trajectory_new_backward(&tcSlow);
151 robot_trajectory_add_point_trans(
152 x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M, which_slope),
153 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01);
154 robot_trajectory_add_final_point_trans(
155 x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M + 0.07, which_slope),
156 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01,
161 SUBFSM_TRANSITION(load_oranges, NULL);
164 FSM_TRANSITION(sledge_down);
169 case EV_ACTION_ERROR:
170 case EV_GOAL_NOT_REACHABLE:
171 DBG_PRINT_EVENT("unhandled event");
177 /* one-state-subautomaton to load oranges in two stages */
178 FSM_STATE(load_oranges)
183 act_vidle(VIDLE_MIDDLE, VIDLE_MEDIUM_SPEED);
186 // FIXME: respond to VIDLE EVENT
187 act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
194 case EV_ACTION_ERROR:
195 case EV_GOAL_NOT_REACHABLE:
196 DBG_PRINT_EVENT("unhandled event");
202 FSM_STATE(sledge_down)
206 robot_trajectory_new(&tcFast);
207 robot_trajectory_add_point_trans(
208 x_coord(1 - ROBOT_AXIS_TO_BACK_M, which_slope),
209 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01);
211 robot_trajectory_add_point_trans(
212 x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.26, which_slope),
213 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.10);
214 robot_trajectory_add_final_point_trans(
215 x_coord(0.50, which_slope),
216 PLAYGROUND_HEIGHT_M - 0.6,
219 robot_trajectory_add_point_trans(
220 x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.20, which_slope),
221 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.08);
222 robot_trajectory_add_final_point_trans(
223 x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.26, which_slope),
224 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.14,
228 /* just for sure, try to close it one more time */
229 act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
236 case EV_ACTION_ERROR:
237 case EV_GOAL_NOT_REACHABLE:
238 DBG_PRINT_EVENT("unhandled event");
240 // enables using side switches on bumpers
241 enable_switches(true);
242 robot.ignore_hokuyo = false;
243 robot.check_turn_safety = true;
249 /************************************************************************
250 * The "unload our oranges" subautomaton
251 ************************************************************************/
253 FSM_STATE(to_cntainer_and_unld)
258 if (which_slope == MINE) {
259 robot_trajectory_new(&tcFast);
260 // face the rim with front of the robot
261 //robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.12, ARRIVE_FROM(DEG2RAD(-90), 0.10));
262 // face the rim with back of the robot
263 robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M-0.6, 0.35);
264 robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05));
266 robot_goto_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05), &tcFast);
270 FSM_TIMER(3000); // FIXME: test this
271 act_vidle(VIDLE_DOWN, VIDLE_FAST_SPEED);
274 act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
280 case EV_ACTION_ERROR:
281 case EV_GOAL_NOT_REACHABLE:
282 DBG_PRINT_EVENT("unhandled event");
288 /************************************************************************
289 * The "collect corns" subautomaton
290 ************************************************************************/
292 static enum where_to_go {
297 } where_to_go = CORN;
299 static struct corn *corn_to_get;
301 FSM_STATE(rush_corns_decider)
305 if (where_to_go == CORN) {
306 FSM_TRANSITION(approach_next_corn);
307 } else if (where_to_go == CONTAINER) {
308 FSM_TRANSITION(rush_the_corn);
309 } else if (where_to_go == TURN_AROUND) {
310 FSM_TRANSITION(turn_around);
311 } else /* NO_MORE_CORN */ {
318 case EV_ACTION_ERROR:
320 case EV_GOAL_NOT_REACHABLE:
321 DBG_PRINT_EVENT("unhandled event");
328 FSM_STATE(approach_next_corn)
333 robot_get_est_pos(&x, &y, &phi);
334 printf("approach_next_corn: puck cnt: %d, est pos %.3f, %.3f, %.3f\n",
337 corn_to_get = choose_next_corn();
339 Pos *p = get_corn_approach_position(corn_to_get);
340 corn_to_get->was_collected = true;
341 //robot_trajectory_new(&tcFast);
342 //robot_trajectory_add_final_point_trans(robot_trajectory_add_final_point_trans(p->x, p->y, TURN(p->phi));
343 robot_goto_trans(p->x, p->y, TURN(p->phi), &tcFast);
345 where_to_go = CONTAINER;
347 where_to_go = NO_MORE_CORN;
353 FSM_TRANSITION(rush_corns_decider);
359 case EV_ACTION_ERROR:
360 case EV_GOAL_NOT_REACHABLE:
361 DBG_PRINT_EVENT("unhandled event");
367 FSM_STATE(rush_the_corn)
372 if (robot.team_color == BLUE) {
373 x = corn_to_get->position.x;
375 x = PLAYGROUND_WIDTH_M - corn_to_get->position.x;
377 remove_wall_around_corn(x, corn_to_get->position.y);
378 robot_goto_trans(PLAYGROUND_WIDTH_M - 0.4, 0.15, ARRIVE_FROM(DEG2RAD(-90), 0.02), &tcFast);
379 where_to_go = TURN_AROUND;
382 FSM_TRANSITION(rush_corns_decider);
388 case EV_ACTION_ERROR:
389 case EV_GOAL_NOT_REACHABLE:
390 DBG_PRINT_EVENT("unhandled event");
396 // used to perform the maneuvre
397 FSM_STATE(turn_around)
401 robot_trajectory_new_backward(&tcFast);
402 robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, TURN_CCW(90));
406 FSM_TRANSITION(rush_corns_decider);
412 case EV_ACTION_ERROR:
413 case EV_GOAL_NOT_REACHABLE:
414 DBG_PRINT_EVENT("unhandled event");