void set_initial_position()
{
- robot_set_est_pos_trans(ROBOT_START_X_M,
- ROBOT_START_Y_M,
- DEG2RAD(ROBOT_START_ANGLE_DEG));
+ robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M,
+ PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0) - 0.05,
+ 0);
}
void actuators_home()
{
- act_jaws(CLOSE);
+// act_jaws(CLOSE);
+
+// bool pokus = robot.fsm.motion.state_name == "wait_and_try_again";
// drive lift to home position
//act_lift(0, 0, 1);
// unset the homing request
robot.use_back_bumpers = enabled;
}
-void enable_my_square_walls(bool enabled)
-{
- for (int i = 0; i < 15; i++) {
- if (robot.team_color == RED)
- ShmapSetCircleFlag(red_sq[i].x, red_sq[i].y, 0.2, enabled*MAP_FLAG_WALL, (!enabled)*MAP_FLAG_WALL);
- else
- ShmapSetCircleFlag(blue_sq[i].x, blue_sq[i].y, 0.2,enabled*MAP_FLAG_WALL, (!enabled)*MAP_FLAG_WALL);
- }
-
- //if (robot.team_color == RED)
- //ShmapSetRectangleFlag(0.45, 0, 0.45 + 0.35 + 0.2, PLAYGROUND_HEIGHT_M, enabled*MAP_FLAG_WALL, (!enabled)*MAP_FLAG_WALL);
- //else
- //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);
-}
/************************************************************************
* Trajectory constraints used; They are initialized in the main() function in competition.cc
************************************************************************/
-struct TrajectoryConstraints tcJerk, tcFast, tcSlow, tcVerySlow;
+struct TrajectoryConstraints tcFast, tcVeryFast, tcSlow, tcVerySlow;
+double totem_x;
+double totem_y;
+bool up;
-/** generate random positions on oponent squares and goto this position */
-FSM_STATE(move_around)
+FSM_STATE(approach_central_buillon)
{
- int goal;
- double goal_x, goal_y;
- static bool entry = true;
-
switch(FSM_EVENT) {
case EV_ENTRY:
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.use_back_bumpers = true;
- robot.ignore_hokuyo = false;
-
- act_jaws(CLOSE);
-
- enable_my_square_walls(true);
-
- srand((int)(robot_current_time()*10000));
-
- do {
- goal = (rand() % (SQ_CNTR - 5)) + 5;
-
- } while (goal == 6 || goal == 11);
-
- printf("goal %d time %f\n", goal, robot_current_time());
- if (robot.team_color == RED) {
- goal_x = blue_sq[goal].x;
- goal_y = blue_sq[goal].y;
- } else {
- goal_x = red_sq[goal].x;
- goal_y = red_sq[goal].y;
- }
-
- robot_goto_notrans(goal_x, goal_y, NO_TURN(), &tcFast);
- break;
- case EV_START:
- case EV_TIMER:
- break;
- case EV_RETURN:
- case EV_MOTION_DONE:
- SUBFSM_RET(NULL);
- break;
- case EV_MOTION_ERROR:
- case EV_EXIT:
- break;
- }
-}
-
-/** securely bypass firt figure in front of starting area */
-FSM_STATE(bypass_figure_in_front_of_start)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.use_back_bumpers = true;
- robot.ignore_hokuyo = false;
-
- robot_trajectory_new(&tcFast);
+ robot_trajectory_new(&tcSlow); // new trajectory
robot_trajectory_add_point_trans(
- 0.45 + 0.3,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2));
- robot_trajectory_add_final_point_trans(
- 0.45 + 0.3,
- 4*0.35,
- NO_TURN());
- break;
- case EV_MOTION_DONE:
- SUBFSM_RET(NULL);
- break;
- case EV_START:
- case EV_TIMER:
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
- break;
- }
-}
-
-/** pick second figure from green area */
-FSM_STATE(approach_second_green_figure)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.use_back_bumpers = true;
- robot.ignore_hokuyo = false;
-
- robot_trajectory_new(&tcFast);
+ 0.5,
+ PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
+ robot_trajectory_add_point_trans(
+ 0.64,
+ 1.2);
+ robot_trajectory_add_point_trans(
+ 0.64,
+ 0.7);
robot_trajectory_add_final_point_trans(
- 0.45 + 0.3,
- 0.29 + 0.28,
- TURN(DEG2RAD(180)));
+ 1.0,
+ 0.45,
+ ARRIVE_FROM(DEG2RAD(24),0.1));
+// robot_trajectory_add_final_point_trans(
+// 1.3,
+// 0.58,
+// NO_TURN());
break;
case EV_MOTION_DONE:
- act_jaws(OPEN);
- FSM_TIMER(1000);
+ FSM_TIMER(2000);
break;
case EV_TIMER:
- FSM_TRANSITION(load_second_green_figure);
+ FSM_TRANSITION(catch_central_buillon);
break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ default:
break;
}
}
-FSM_STATE(load_second_green_figure)
+FSM_STATE(catch_central_buillon)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.use_back_bumpers = true;
- robot.ignore_hokuyo = true;
-
- robot_trajectory_new(&tcSlow);
+ robot_trajectory_new(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M + 0.04,
- 0.29 + 0.28,
- ARRIVE_FROM(DEG2RAD(180), 0.10));
+ 1.3,
+ 0.58,
+ NO_TURN());
break;
case EV_MOTION_DONE:
- FSM_TIMER(1000);
- act_jaws(CATCH);
+ FSM_TIMER(2000);
break;
case EV_TIMER:
- FSM_TRANSITION(go_out_second_green_figure);
+ FSM_TRANSITION(leave_central_buillon);
break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ default:
break;
}
}
-FSM_STATE(go_out_second_green_figure)
+FSM_STATE(leave_central_buillon)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.use_back_bumpers = true;
- robot.ignore_hokuyo = true;
-
- robot_trajectory_new_backward(&tcFast);
+ robot_trajectory_new_backward(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- 0.45 + 0.3,
- 0.7,
+ 0.85,
+ 0.38,
NO_TURN());
break;
case EV_MOTION_DONE:
- case EV_TIMER:
- FSM_TRANSITION(place_figure_to_protected_block);
+ enable_bumpers(false);
+ SUBFSM_RET(NULL);
break;
- case EV_START:
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ default:
break;
}
}
-FSM_STATE(place_figure_to_protected_block)
+FSM_STATE(push_bottle_bw)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.use_back_bumpers = true;
- robot.ignore_hokuyo = false;
-
- robot_trajectory_new(&tcSlow);
+ robot_trajectory_new_backward(&tcSlow); // new trajectory
+ robot_trajectory_add_point_trans(
+ 0.7,
+ 0.3);
robot_trajectory_add_final_point_trans(
- 0.45 + 0.3,
- 0.12 + 0.08 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
- ARRIVE_FROM(DEG2RAD(-90), 0.20));
+ 0.64+0.08,
+ ROBOT_AXIS_TO_BACK_M + 0.05,
+ ARRIVE_FROM(DEG2RAD(270), 0.10));
break;
- case EV_START:
- case EV_TIMER:
- FSM_TRANSITION(leave_protected_figure);
- break;
- case EV_RETURN:
case EV_MOTION_DONE:
- act_jaws(OPEN);
- FSM_TIMER(1000);
+ FSM_TRANSITION(return_home);
break;
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
+ default:
break;
}
}
-FSM_STATE(leave_protected_figure)
+FSM_STATE(return_home)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.use_back_bumpers = true;
- robot.ignore_hokuyo = false;
-
- robot_trajectory_new_backward(&tcFast);
+ robot_trajectory_new(&tcSlow); // new trajectory
robot_trajectory_add_point_trans(
- 0.45 + 0.3,
+ 0.64 + 0.08,
0.7);
robot_trajectory_add_final_point_trans(
- 0.45 + 0.3,
- 0.29 + 2*0.28,
- NO_TURN());
- break;
- case EV_START:
- case EV_TIMER:
+ 0.4,
+ 1.0,
+ ARRIVE_FROM(DEG2RAD(180), 0.10));
break;
- case EV_RETURN:
case EV_MOTION_DONE:
- act_jaws(CLOSE);
+ enable_bumpers(true);
+ FSM_TIMER(2000);
+ break;
+ case EV_TIMER:
SUBFSM_RET(NULL);
break;
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ default:
break;
}
}
-/** pick third figure from green area */
-FSM_STATE(approach_third_green_figure)
+FSM_STATE(leave_home)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.use_back_bumpers = true;
- robot.ignore_hokuyo = false;
-
- robot_trajectory_new(&tcFast);
+ robot_trajectory_new_backward(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- 0.45 + 0.3,
- 0.29 + 2*0.28,
- TURN(DEG2RAD(180)));
+ 0.6,
+ 1.0,
+ NO_TURN());
break;
case EV_MOTION_DONE:
- act_jaws(OPEN);
- FSM_TIMER(1000);
+ FSM_TRANSITION(leave_home_for_totem);
break;
- case EV_TIMER:
- FSM_TRANSITION(load_third_green_figure);
- break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ default:
break;
}
}
-FSM_STATE(load_third_green_figure)
+FSM_STATE(leave_home_for_totem)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.use_back_bumpers = true;
- robot.ignore_hokuyo = false;
-
- robot_trajectory_new(&tcSlow);
- robot_trajectory_add_final_point_trans(
- ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M + 0.04,
- 0.29 + 2*0.28,
- ARRIVE_FROM(DEG2RAD(180), 0.20));
+ robot_trajectory_new(&tcSlow); // new trajectory
+ if(up) {
+ robot_trajectory_add_final_point_trans(
+ 0.64,
+ 1.3,
+ NO_TURN());
+ }
+ else {
+ robot_trajectory_add_final_point_trans(
+ 0.64,
+ 0.7,
+ NO_TURN());
+ }
break;
case EV_MOTION_DONE:
- FSM_TIMER(1000);
- act_jaws(CATCH);
- break;
- case EV_TIMER:
- FSM_TRANSITION(go_out_third_green_figure);
+ if(up) FSM_TRANSITION(approach_totem_up);
+ else FSM_TRANSITION(approach_totem_down);
break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ default:
break;
}
}
-FSM_STATE(go_out_third_green_figure)
+FSM_STATE(approach_totem_down)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.use_back_bumpers = true;
- robot.ignore_hokuyo = false;
-
- robot_trajectory_new_backward(&tcFast);
+ robot_trajectory_new(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- 0.45 + 0.35,
- 0.29 + 2*0.28,
- NO_TURN());
+ totem_x,
+ 0.48,
+ TURN_CCW(DEG2RAD(90)));
break;
case EV_MOTION_DONE:
+ FSM_TIMER(2000);
+ break;
case EV_TIMER:
- FSM_TRANSITION(place_figure_to_bonus_area);
+ FSM_TRANSITION(catch_totem_buillon_down);
break;
- case EV_START:
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.ignore_hokuyo = false;
+ default:
break;
}
}
-
-FSM_STATE(place_figure_to_bonus_area)
+
+FSM_STATE(catch_totem_buillon_down)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.use_back_bumpers = true;
- robot.ignore_hokuyo = false;
-
- robot_trajectory_new(&tcSlow);
+ robot_trajectory_new(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- 0.45 + 0.7 + 0.2,
- 0.08 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
- ARRIVE_FROM(DEG2RAD(-90), 0.20));
- break;
- case EV_START:
- case EV_TIMER:
- FSM_TRANSITION(leave_bonus_figure);
+ totem_x,
+ totem_y - ROBOT_AXIS_TO_FRONT_M - 0.05,
+ ARRIVE_FROM(DEG2RAD(90), 0.10));
break;
- case EV_RETURN:
case EV_MOTION_DONE:
- act_jaws(OPEN);
- FSM_TIMER(1000);
+ FSM_TIMER(2000);
break;
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
+ case EV_TIMER:
+ FSM_TRANSITION(leave_totem_down);
+ default:
break;
}
}
-FSM_STATE(leave_bonus_figure)
+FSM_STATE(leave_totem_down)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.use_back_bumpers = true;
- robot.ignore_hokuyo = false;
-
- robot_trajectory_new_backward(&tcFast);
+ robot_trajectory_new_backward(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- 0.45 + 0.7 + 0.2,
- 0.7,
+ totem_x,
+ 0.48,
NO_TURN());
break;
- case EV_START:
- case EV_TIMER:
- break;
- case EV_RETURN:
case EV_MOTION_DONE:
- act_jaws(CLOSE);
- SUBFSM_RET(NULL);
- break;
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ FSM_TRANSITION(place_buillon_home);
+ default:
break;
}
}
-/** pick fourth green figure from green area */
-FSM_STATE(approach_fourth_green_figure)
+FSM_STATE(place_buillon_home)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.use_back_bumpers = true;
- robot.ignore_hokuyo = false;
-
- robot_trajectory_new(&tcFast);
+ robot_trajectory_new(&tcSlow); // new trajectory
+ if(up) {
+ robot_trajectory_add_point_trans(
+ 0.9,
+ 1.52);
+ }
+ else {
+ robot_trajectory_add_point_trans(
+ 0.9,
+ 0.48);
+ robot_trajectory_add_point_trans(
+ 0.7,
+ 0.8);
+ }
robot_trajectory_add_final_point_trans(
- 0.45 + 0.3,
- 0.29 + 3*0.28,
- TURN(DEG2RAD(180)));
+ 0.4,
+ 1.0,
+ ARRIVE_FROM(DEG2RAD(180),0.10));
break;
case EV_MOTION_DONE:
- act_jaws(OPEN);
- FSM_TIMER(1000);
+ FSM_TIMER(2000);
break;
case EV_TIMER:
- FSM_TRANSITION(load_fourth_green_figure);
- break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ SUBFSM_RET(NULL);
+ default:
break;
}
}
-FSM_STATE(load_fourth_green_figure)
+FSM_STATE(approach_totem_up)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.use_back_bumpers = true;
- robot.ignore_hokuyo = true;
-
- robot_trajectory_new(&tcSlow);
+ robot_trajectory_new(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M + 0.03,
- 0.29 + 3*0.28,
- ARRIVE_FROM(DEG2RAD(180), 0.20));
+ totem_x,
+ 1.52,
+ TURN_CW(DEG2RAD(270)));
break;
case EV_MOTION_DONE:
- FSM_TIMER(1000);
- act_jaws(CATCH);
+ FSM_TIMER(2000);
break;
case EV_TIMER:
- FSM_TRANSITION(go_out_fourth_green_figure);
+ FSM_TRANSITION(catch_totem_buillon_up);
break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ default:
break;
}
}
-FSM_STATE(go_out_fourth_green_figure)
+FSM_STATE(catch_totem_buillon_up)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.use_back_bumpers = true;
- robot.ignore_hokuyo = true;
-
- robot_trajectory_new_backward(&tcFast);
+ robot_trajectory_new(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- 0.45 + 0.175,
- 3*0.35 + 0.175,
- NO_TURN());
+ totem_x,
+ totem_y + ROBOT_AXIS_TO_FRONT_M + 0.05,
+ ARRIVE_FROM(DEG2RAD(270), 0.10));
break;
case EV_MOTION_DONE:
+ FSM_TIMER(2000);
+ break;
case EV_TIMER:
- FSM_TRANSITION(place_figure_to_red_square);
+ FSM_TRANSITION(leave_totem_up);
break;
- case EV_START:
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ default:
break;
}
}
-FSM_STATE(place_figure_to_red_square)
+FSM_STATE(leave_totem_up)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.use_back_bumpers = true;
- robot.ignore_hokuyo = false;
- robot_trajectory_new(&tcSlow);
+ robot_trajectory_new_backward(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- 0.45 + 0.175,
- 0.7 + 0.175 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
- ARRIVE_FROM(DEG2RAD(-90), 0.05));
- break;
- case EV_START:
- case EV_TIMER:
- FSM_TRANSITION(leave_red_square_figure);
+ totem_x,
+ 1.52,
+ NO_TURN());
break;
- case EV_RETURN:
case EV_MOTION_DONE:
- act_jaws(OPEN);
- FSM_TIMER(1000);
- break;
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
+ SUBFSM_RET(NULL);
+ default:
break;
}
}
-FSM_STATE(leave_red_square_figure)
+/* State for odometry calibration */
+
+FSM_STATE(go_back_for_cal)
{
- switch(FSM_EVENT) {
+ double x1 = 0, y1 = 0;
+ char buffer [20];
+ switch (FSM_EVENT) {
case EV_ENTRY:
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.use_back_bumpers = true;
- robot.ignore_hokuyo = false;
-
- robot_trajectory_new_backward(&tcFast);
-// robot_trajectory_add_point_trans(
-// 0.45 + 0.175,
-// 0.7 + 0.7);
- robot_trajectory_add_final_point_trans(
- 0.45 + 0.175,
- 0.7 + 0.7,
- NO_TURN());
- break;
- case EV_START:
- case EV_TIMER:
+ robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M + 1.0,
+ PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
+ 0);
+ robot_move_by(-1.1, NO_TURN(), &tcVerySlow);
+ FSM_TIMER(150);
break;
- case EV_RETURN:
case EV_MOTION_DONE:
- act_jaws(CLOSE);
+ ROBOT_LOCK(est_pos_odo);
+ robot.odo_cal_a = -1.0/robot.odo_distance_a;
+ robot.odo_cal_b = -1.0/robot.odo_distance_b;
+ x1 = robot.odo_distance_a;
+ y1 = robot.odo_distance_b;
+ ROBOT_UNLOCK(est_pos_odo);
+ printf("Distance for calibration: \n");
+ printf("Left%f\n", x1);
+ printf("Right%f\n", y1);
+ FILE * file;
+ file = fopen ("odometry_cal_data","a+");
+ sprintf(buffer, "%4.4f", -1/x1);
+ fputs (buffer,file);
+ fputs ("\n", file);
+ sprintf(buffer, "%4.4f", -1/y1);
+ fputs (buffer,file);
+ fputs ("\n", file);
+ fclose(file);
SUBFSM_RET(NULL);
break;
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ case EV_TIMER:
+ ROBOT_LOCK(est_pos_odo);
+ if(x1 == robot.odo_distance_a){
+ x1 = robot.odo_distance_a;
+ y1 = robot.odo_distance_b;
+ FSM_SIGNAL(MOTION,EV_MOVE_STOP, NULL);
+ FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
+ } else {
+ FSM_TIMER(200);
+ }
+ ROBOT_UNLOCK(est_pos_odo);
+ break;
+ default:
break;
}
}
\ No newline at end of file
projects / eurobot / public.git / blobdiff