7 #include <movehelper.h>
14 #include "actuators.h"
16 #include "match-timing.h"
20 UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file */
22 #include "common-states.h"
24 /************************************************************************
25 * Functions used in and called from all the (almost identical)
26 * "wait for start" states in particular strategies.
27 ************************************************************************/
30 #define DBG_FSM_STATE(name) do { if (fsm->debug_states) ul_loginf("fsm %s %.1f: %s(%s)\n", \
31 fsm->debug_name, robot_current_time(), \
32 name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
35 void set_initial_position()
37 robot_set_est_pos_trans(ROBOT_START_X_M,
39 DEG2RAD(ROBOT_START_ANGLE_DEG));
45 // drive lift to home position
47 // unset the homing request
54 robot.check_turn_safety = false;
55 pthread_create(&thid, NULL, timing_thread, NULL);
59 // We set initial position periodically in order for it to be updated
60 // on the display if the team color is changed during waiting for
64 set_initial_position();
65 if (robot.start_state == START_PLUGGED_IN)
71 sem_post(&robot.start);
73 set_initial_position();
81 void inline enable_bumpers(bool enabled)
83 robot.use_left_bumper = enabled;
84 robot.use_right_bumper = enabled;
85 robot.use_back_bumpers = enabled;
88 void enable_my_square_walls(bool enabled)
90 for (int i = 0; i < 15; i++) {
91 if (robot.team_color == RED)
92 ShmapSetCircleFlag(red_sq[i].x, red_sq[i].y, 0.2, enabled*MAP_FLAG_WALL, (!enabled)*MAP_FLAG_WALL);
94 ShmapSetCircleFlag(blue_sq[i].x, blue_sq[i].y, 0.2,enabled*MAP_FLAG_WALL, (!enabled)*MAP_FLAG_WALL);
97 //if (robot.team_color == RED)
98 //ShmapSetRectangleFlag(0.45, 0, 0.45 + 0.35 + 0.2, PLAYGROUND_HEIGHT_M, enabled*MAP_FLAG_WALL, (!enabled)*MAP_FLAG_WALL);
100 //ShmapSetRectangleFlag(PLAYGROUND_WIDTH_M - 0.45 - 0.35 - 0.2, 0, PLAYGROUND_WIDTH_M - 0.45, PLAYGROUND_HEIGHT_M, enabled*MAP_FLAG_WALL, (!enabled)*MAP_FLAG_WALL);
104 /************************************************************************
105 * Trajectory constraints used; They are initialized in the main() function in competition.cc
106 ************************************************************************/
108 struct TrajectoryConstraints tcJerk, tcFast, tcSlow, tcVerySlow;
110 /** generate random positions on oponent squares and goto this position */
111 FSM_STATE(move_around)
114 double goal_x, goal_y;
115 static bool entry = true;
119 robot.use_left_bumper = true;
120 robot.use_right_bumper = true;
121 robot.use_back_bumpers = true;
122 robot.ignore_hokuyo = false;
126 enable_my_square_walls(true);
128 srand((int)(robot_current_time()*1000));
129 goal = (rand() % (SQ_CNTR - 5)) + 5;
130 printf("goal %d time %f\n", goal, robot_current_time());
131 if (robot.team_color == RED) {
132 goal_x = blue_sq[goal].x;
133 goal_y = blue_sq[goal].y;
135 goal_x = red_sq[goal].x;
136 goal_y = red_sq[goal].y;
139 robot_goto_notrans(goal_x, goal_y, NO_TURN(), &tcFast);
148 case EV_MOTION_ERROR:
154 /** securely bypass firt figure in front of starting area */
155 FSM_STATE(bypass_figure_in_front_of_start)
159 robot.use_left_bumper = true;
160 robot.use_right_bumper = true;
161 robot.use_back_bumpers = true;
162 robot.ignore_hokuyo = false;
164 robot_trajectory_new(&tcFast);
165 robot_trajectory_add_point_trans(
167 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2));
168 robot_trajectory_add_final_point_trans(
179 case EV_MOTION_ERROR:
180 case EV_SWITCH_STRATEGY:
181 DBG_PRINT_EVENT("unhandled event");
187 /** pick second figure from green area */
188 FSM_STATE(approach_second_green_figure)
192 robot.use_left_bumper = true;
193 robot.use_right_bumper = true;
194 robot.use_back_bumpers = true;
195 robot.ignore_hokuyo = false;
197 robot_trajectory_new(&tcFast);
198 robot_trajectory_add_final_point_trans(
208 FSM_TRANSITION(load_second_green_figure);
211 case EV_MOTION_ERROR:
217 FSM_STATE(load_second_green_figure)
221 robot.use_left_bumper = true;
222 robot.use_right_bumper = true;
223 robot.use_back_bumpers = true;
224 robot.ignore_hokuyo = true;
226 robot_trajectory_new(&tcSlow);
227 robot_trajectory_add_final_point_trans(
228 ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M + 0.04,
230 ARRIVE_FROM(DEG2RAD(180), 0.10));
237 FSM_TRANSITION(go_out_second_green_figure);
240 case EV_MOTION_ERROR:
246 FSM_STATE(go_out_second_green_figure)
250 robot.use_left_bumper = true;
251 robot.use_right_bumper = true;
252 robot.use_back_bumpers = true;
253 robot.ignore_hokuyo = true;
255 robot_trajectory_new_backward(&tcFast);
256 robot_trajectory_add_final_point_trans(
263 FSM_TRANSITION(place_figure_to_protected_block);
267 case EV_MOTION_ERROR:
273 FSM_STATE(place_figure_to_protected_block)
277 robot.use_left_bumper = true;
278 robot.use_right_bumper = true;
279 robot.use_back_bumpers = true;
280 robot.ignore_hokuyo = false;
282 robot_trajectory_new(&tcSlow);
283 robot_trajectory_add_final_point_trans(
285 0.12 + 0.08 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
286 ARRIVE_FROM(DEG2RAD(-90), 0.20));
290 FSM_TRANSITION(leave_protected_figure);
297 case EV_MOTION_ERROR:
298 case EV_SWITCH_STRATEGY:
299 DBG_PRINT_EVENT("unhandled event");
305 FSM_STATE(leave_protected_figure)
309 robot.use_left_bumper = true;
310 robot.use_right_bumper = true;
311 robot.use_back_bumpers = true;
312 robot.ignore_hokuyo = false;
314 robot_trajectory_new_backward(&tcFast);
315 robot_trajectory_add_point_trans(
318 robot_trajectory_add_final_point_trans(
331 case EV_MOTION_ERROR:
337 /** pick third figure from green area */
338 FSM_STATE(approach_third_green_figure)
342 robot.use_left_bumper = true;
343 robot.use_right_bumper = true;
344 robot.use_back_bumpers = true;
345 robot.ignore_hokuyo = false;
347 robot_trajectory_new(&tcFast);
348 robot_trajectory_add_final_point_trans(
358 FSM_TRANSITION(load_third_green_figure);
361 case EV_MOTION_ERROR:
367 FSM_STATE(load_third_green_figure)
371 robot.use_left_bumper = true;
372 robot.use_right_bumper = true;
373 robot.use_back_bumpers = true;
374 robot.ignore_hokuyo = false;
376 robot_trajectory_new(&tcSlow);
377 robot_trajectory_add_final_point_trans(
378 ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M + 0.04,
380 ARRIVE_FROM(DEG2RAD(180), 0.20));
387 FSM_TRANSITION(go_out_third_green_figure);
390 case EV_MOTION_ERROR:
396 FSM_STATE(go_out_third_green_figure)
400 robot.use_left_bumper = true;
401 robot.use_right_bumper = true;
402 robot.use_back_bumpers = true;
403 robot.ignore_hokuyo = false;
405 robot_trajectory_new_backward(&tcFast);
406 robot_trajectory_add_final_point_trans(
413 FSM_TRANSITION(place_figure_to_bonus_area);
417 case EV_MOTION_ERROR:
419 robot.use_left_bumper = true;
420 robot.use_right_bumper = true;
421 robot.ignore_hokuyo = false;
426 FSM_STATE(place_figure_to_bonus_area)
430 robot.use_left_bumper = true;
431 robot.use_right_bumper = true;
432 robot.use_back_bumpers = true;
433 robot.ignore_hokuyo = false;
435 robot_trajectory_new(&tcSlow);
436 robot_trajectory_add_final_point_trans(
438 0.08 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
439 ARRIVE_FROM(DEG2RAD(-90), 0.20));
443 FSM_TRANSITION(leave_bonus_figure);
450 case EV_MOTION_ERROR:
451 case EV_SWITCH_STRATEGY:
452 DBG_PRINT_EVENT("unhandled event");
458 FSM_STATE(leave_bonus_figure)
462 robot.use_left_bumper = true;
463 robot.use_right_bumper = true;
464 robot.use_back_bumpers = true;
465 robot.ignore_hokuyo = false;
467 robot_trajectory_new_backward(&tcFast);
468 robot_trajectory_add_final_point_trans(
481 case EV_MOTION_ERROR:
487 /** pick fourth green figure from green area */
488 FSM_STATE(approach_fourth_green_figure)
492 robot.use_left_bumper = true;
493 robot.use_right_bumper = true;
494 robot.use_back_bumpers = true;
495 robot.ignore_hokuyo = false;
497 robot_trajectory_new(&tcFast);
498 robot_trajectory_add_final_point_trans(
508 FSM_TRANSITION(load_fourth_green_figure);
511 case EV_MOTION_ERROR:
517 FSM_STATE(load_fourth_green_figure)
521 robot.use_left_bumper = true;
522 robot.use_right_bumper = true;
523 robot.use_back_bumpers = true;
524 robot.ignore_hokuyo = true;
526 robot_trajectory_new(&tcSlow);
527 robot_trajectory_add_final_point_trans(
528 ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M + 0.03,
530 ARRIVE_FROM(DEG2RAD(180), 0.20));
537 FSM_TRANSITION(go_out_fourth_green_figure);
540 case EV_MOTION_ERROR:
546 FSM_STATE(go_out_fourth_green_figure)
550 robot.use_left_bumper = true;
551 robot.use_right_bumper = true;
552 robot.use_back_bumpers = true;
553 robot.ignore_hokuyo = true;
555 robot_trajectory_new_backward(&tcFast);
556 robot_trajectory_add_final_point_trans(
563 FSM_TRANSITION(place_figure_to_red_square);
567 case EV_MOTION_ERROR:
573 FSM_STATE(place_figure_to_red_square)
577 robot.use_left_bumper = true;
578 robot.use_right_bumper = true;
579 robot.use_back_bumpers = true;
580 robot.ignore_hokuyo = false;
581 robot_trajectory_new(&tcSlow);
582 robot_trajectory_add_final_point_trans(
584 0.7 + 0.175 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
585 ARRIVE_FROM(DEG2RAD(-90), 0.05));
589 FSM_TRANSITION(leave_red_square_figure);
596 case EV_MOTION_ERROR:
597 case EV_SWITCH_STRATEGY:
598 DBG_PRINT_EVENT("unhandled event");
604 FSM_STATE(leave_red_square_figure)
608 robot.use_left_bumper = true;
609 robot.use_right_bumper = true;
610 robot.use_back_bumpers = true;
611 robot.ignore_hokuyo = false;
613 robot_trajectory_new_backward(&tcFast);
614 // robot_trajectory_add_point_trans(
617 robot_trajectory_add_final_point_trans(
630 case EV_MOTION_ERROR: