7 #include <movehelper.h>
14 #include "actuators.h"
17 #include "match-timing.h"
21 UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file */
23 #include "common-states.h"
25 /************************************************************************
26 * Functions used in and called from all the (almost identical)
27 * "wait for start" states in particular strategies.
28 ************************************************************************/
31 #define DBG_FSM_STATE(name) do { if (fsm->debug_states) ul_loginf("fsm %s %.1f: %s(%s)\n", \
32 fsm->debug_name, robot_current_time(), \
33 name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
36 void set_initial_position()
38 robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M,
39 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0) - 0.05,
47 // bool pokus = robot.fsm.motion.state_name == "wait_and_try_again";
48 // drive lift to home position
50 // unset the homing request
57 robot.check_turn_safety = false;
58 pthread_create(&thid, NULL, timing_thread, NULL);
62 // We set initial position periodically in order for it to be updated
63 // on the display if the team color is changed during waiting for
67 set_initial_position();
68 if (robot.start_state == START_PLUGGED_IN)
74 sem_post(&robot.start);
76 set_initial_position();
86 int sharp_data = robot.orte.jaws_status.act_pos.left;
88 sharp_dist = s_gp2y0a21_ir2mmLong(sharp_data);
89 printf("sharp data: %dmm\n", sharp_dist);
90 return (sharp_dist <= 150 ? true : false);
93 void inline enable_bumpers(bool enabled)
95 robot.use_left_bumper = enabled;
96 robot.use_right_bumper = enabled;
97 robot.use_back_bumpers = enabled;
102 /************************************************************************
103 * Trajectory constraints used; They are initialized in the main() function in competition.cc
104 ************************************************************************/
106 struct TrajectoryConstraints tcFast, tcVeryFast, tcSlow, tcVerySlow;
111 FSM_STATE(approach_central_buillon)
115 robot_trajectory_new(&tcSlow); // new trajectory
116 robot_trajectory_add_point_trans(
118 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
119 robot_trajectory_add_point_trans(
122 robot_trajectory_add_point_trans(
125 robot_trajectory_add_final_point_trans(
128 ARRIVE_FROM(DEG2RAD(24),0.1));
129 // robot_trajectory_add_final_point_trans(
138 FSM_TRANSITION(catch_central_buillon);
145 FSM_STATE(catch_central_buillon)
149 robot_trajectory_new(&tcSlow); // new trajectory
150 robot_trajectory_add_final_point_trans(
159 FSM_TRANSITION(leave_central_buillon);
166 FSM_STATE(leave_central_buillon)
170 robot_trajectory_new_backward(&tcSlow); // new trajectory
171 robot_trajectory_add_final_point_trans(
177 enable_bumpers(false);
185 FSM_STATE(push_bottle_bw)
189 robot_trajectory_new_backward(&tcSlow); // new trajectory
190 robot_trajectory_add_point_trans(
193 robot_trajectory_add_final_point_trans(
195 ROBOT_AXIS_TO_BACK_M + 0.05,
196 ARRIVE_FROM(DEG2RAD(270), 0.10));
199 FSM_TRANSITION(return_home);
206 FSM_STATE(return_home)
210 robot_trajectory_new(&tcSlow); // new trajectory
211 robot_trajectory_add_point_trans(
214 robot_trajectory_add_final_point_trans(
217 ARRIVE_FROM(DEG2RAD(180), 0.10));
220 enable_bumpers(true);
231 FSM_STATE(leave_home)
235 robot_trajectory_new_backward(&tcSlow); // new trajectory
236 robot_trajectory_add_final_point_trans(
242 FSM_TRANSITION(leave_home_for_totem);
249 FSM_STATE(leave_home_for_totem)
253 robot_trajectory_new(&tcSlow); // new trajectory
255 robot_trajectory_add_final_point_trans(
261 robot_trajectory_add_final_point_trans(
268 if(up) FSM_TRANSITION(approach_totem_up);
269 else FSM_TRANSITION(approach_totem_down);
276 FSM_STATE(approach_totem_down)
280 robot_trajectory_new(&tcSlow); // new trajectory
281 robot_trajectory_add_final_point_trans(
284 TURN_CCW(DEG2RAD(90)));
290 FSM_TRANSITION(catch_totem_buillon_down);
297 FSM_STATE(catch_totem_buillon_down)
301 robot_trajectory_new(&tcSlow); // new trajectory
302 robot_trajectory_add_final_point_trans(
304 totem_y - ROBOT_AXIS_TO_FRONT_M - 0.05,
305 ARRIVE_FROM(DEG2RAD(90), 0.10));
311 FSM_TRANSITION(leave_totem_down);
317 FSM_STATE(leave_totem_down)
321 robot_trajectory_new_backward(&tcSlow); // new trajectory
322 robot_trajectory_add_final_point_trans(
328 FSM_TRANSITION(place_buillon_home);
334 FSM_STATE(place_buillon_home)
338 robot_trajectory_new(&tcSlow); // new trajectory
340 robot_trajectory_add_point_trans(
345 robot_trajectory_add_point_trans(
348 robot_trajectory_add_point_trans(
352 robot_trajectory_add_final_point_trans(
355 ARRIVE_FROM(DEG2RAD(180),0.10));
367 FSM_STATE(approach_totem_up)
371 robot_trajectory_new(&tcSlow); // new trajectory
372 robot_trajectory_add_final_point_trans(
375 TURN_CW(DEG2RAD(270)));
381 FSM_TRANSITION(catch_totem_buillon_up);
388 FSM_STATE(catch_totem_buillon_up)
392 robot_trajectory_new(&tcSlow); // new trajectory
393 robot_trajectory_add_final_point_trans(
395 totem_y + ROBOT_AXIS_TO_FRONT_M + 0.05,
396 ARRIVE_FROM(DEG2RAD(270), 0.10));
402 FSM_TRANSITION(leave_totem_up);
409 FSM_STATE(leave_totem_up)
413 robot_trajectory_new_backward(&tcSlow); // new trajectory
414 robot_trajectory_add_final_point_trans(
426 /* State for odometry calibration */
428 FSM_STATE(go_back_for_cal)
430 double x1 = 0, y1 = 0;
434 robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M + 1.0,
435 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
437 robot_move_by(-1.1, NO_TURN(), &tcVerySlow);
441 ROBOT_LOCK(est_pos_odo);
442 robot.odo_cal_a = -1.0/robot.odo_distance_a;
443 robot.odo_cal_b = -1.0/robot.odo_distance_b;
444 x1 = robot.odo_distance_a;
445 y1 = robot.odo_distance_b;
446 ROBOT_UNLOCK(est_pos_odo);
447 printf("Distance for calibration: \n");
448 printf("Left%f\n", x1);
449 printf("Right%f\n", y1);
451 file = fopen ("odometry_cal_data","a+");
452 sprintf(buffer, "%4.4f", -1/x1);
455 sprintf(buffer, "%4.4f", -1/y1);
462 ROBOT_LOCK(est_pos_odo);
463 if(x1 == robot.odo_distance_a){
464 x1 = robot.odo_distance_a;
465 y1 = robot.odo_distance_b;
466 FSM_SIGNAL(MOTION,EV_MOVE_STOP, NULL);
467 FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
471 ROBOT_UNLOCK(est_pos_odo);