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);
}
void actuators_home()
{
- act_jaws(CLOSE);
+// act_jaws(CLOSE);
- bool pokus = robot.fsm.motion.state_name == "wait_and_try_again";
+// 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);
- }
-}
+
/************************************************************************
* Trajectory constraints used; They are initialized in the main() function in competition.cc
************************************************************************/
struct TrajectoryConstraints tcFast, tcVeryFast, tcSlow, tcVerySlow;
+double totem_x;
+double totem_y;
+bool up;
-bool tower = false;
-bool bonus_placed = false;
-
-const double load_dist = 0.04; // TODO distance from side when loading green figures
-const double app_dist = 0.04; // TODO distance from green figures when approach
-
-/* fields with numbers of opponent squares, we have five variations of movement style across opp. squares*/
-const int move_around[][8] = {{12, 14, 11, 13, 11, 8, 6, 9},
- {6, 8, 11, 13, 11, 14, 12, 9},
- {11, 13, 11, 8, 11, 14, 12, 9},
- {6, 8, 11, 13, 11, 14, 12, 9},
- {11, 8, 11, 13, 11, 14, 12, 9}};
-
-/** generate "random" positions on oponent squares and goto this position */
-FSM_STATE(move_around)
+FSM_STATE(approach_central_buillon)
{
- static int next_sq = 0;
- static int max_wait = 0;
- // choose randomly one of five move_around strategies
- static int strategy = rand() % 5;
- int goal;
- double goal_x, goal_y;
- static bool entry = true;
-
switch(FSM_EVENT) {
case EV_ENTRY:
- max_wait = 0;
- FSM_TIMER(2000);
- 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));
-
- // save next square number where to go to
- goal = move_around[strategy][next_sq];
-
- // pick position on opponent squares with goal index
- 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);
- printf("strategy: %d, next_sq: %d, goal: %d\n",strategy, next_sq, goal);
- next_sq = (next_sq + 1 < sizeof(move_around[strategy]) ? next_sq + 1 : next_sq);
- break;
- case EV_START:
- case EV_TIMER:
- // every two seconds check if short time to end
- FSM_TIMER(2000);
- if (robot.short_time_to_end == true) {
- // if short time to end - send stop signal to motion FSM and return to main FSM
- FSM_SIGNAL(MOTION, EV_MOVE_STOP, NULL);
- SUBFSM_RET(NULL);
- printf("return from move around state\n");
- } else if (robot.fsm.motion.state_name == "wait_and_try_again") {
- // if goal position unawailable (obstacle) try max. X-times
- if (++max_wait >= 3) {
- // if goal not awailable, send stop signal to motion FSM and generate new target trom move_around
- printf("go to next square!\n");
- max_wait = 0;
- next_sq = (next_sq + 1 < sizeof(move_around[strategy]) ? next_sq + 1 : next_sq);
- FSM_SIGNAL(MOTION, EV_MOVE_STOP, NULL);
- FSM_TRANSITION(move_around);
- } else {
- printf("waiting for opponent! %d\n", max_wait);
- }
- }
+ robot_trajectory_new(&tcSlow); // new trajectory
+ robot_trajectory_add_point_trans(
+ 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(
+ 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_RETURN:
case EV_MOTION_DONE:
- if (robot.short_time_to_end == true)
- SUBFSM_RET(NULL);
- else
- FSM_TRANSITION(move_around);
+ FSM_TIMER(2000);
+ break;
+ case EV_TIMER:
+ FSM_TRANSITION(catch_central_buillon);
break;
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ default:
break;
}
}
-/** pick figure from opponent bonus square */
-FSM_STATE(approach_opp_bonus_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_goto_trans(
- 0.45 + 2*0.35 + 0.175,
- 0.35 + 0.175,
- TURN(DEG2RAD(-50)), &tcFast);
- break;
- case EV_MOTION_DONE:
- act_jaws(OPEN);
- FSM_TRANSITION(load_opp_bonus_figure);
- 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;
- }
-}
-
-FSM_STATE(load_opp_bonus_figure)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- enable_my_square_walls(false);
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.use_back_bumpers = true;
- robot.ignore_hokuyo = true;
-
- //robot_trajectory_new(&tcSlow);
- robot_move_by(0.35, NO_TURN(), &tcSlow);
- break;
- case EV_MOTION_DONE:
- act_jaws(CLOSE);
- FSM_TRANSITION(place_opp_bonus_figure);
- 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;
- }
-}
-
-FSM_STATE(place_opp_bonus_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 = true;
-
- robot_trajectory_new_backward(&tcSlow);
- robot_trajectory_add_point_trans(
- 0.45 + 2*0.35 + 0.175,
- 0.35 + 0.2);
- robot_trajectory_add_final_point_trans(
- 0.45 + 2*0.35 + 0.175,
- 2*0.35 + 0.175 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
- ARRIVE_FROM(DEG2RAD(90), 0.1));
- break;
- case EV_MOTION_DONE:
- act_jaws(OPEN);
- FSM_TRANSITION(leave_opp_bonus_figure);
- 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;
- }
-}
-
-FSM_STATE(leave_opp_bonus_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 = true;
-
- robot_trajectory_new_backward(&tcSlow);
- robot_trajectory_add_final_point_trans(
- 0.45 + 2*0.35 + 0.175,
- 5*0.35 + 0.1,
- NO_TURN());
- break;
- case EV_MOTION_DONE:
- act_jaws(CLOSE);
- SUBFSM_RET();
- 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;
- }
-}
-
-/** securely bypass firt figure in front of starting area */
-FSM_STATE(bypass_figure_in_front_of_start)
+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 = false;
- robot.ignore_hokuyo = false;
-
- robot_trajectory_new(&tcFast);
- robot_trajectory_add_point_trans(
- 0.45 + 0.3,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2));
+ robot_trajectory_new(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- 0.45 + 0.3,
- 4*0.35,
+ 1.3,
+ 0.58,
NO_TURN());
break;
case EV_MOTION_DONE:
- SUBFSM_RET(NULL);
+ FSM_TIMER(2000);
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:
+ FSM_TRANSITION(leave_central_buillon);
+ break;
+ default:
break;
}
}
-/** pick second figure from green area */
-FSM_STATE(approach_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 = false;
-
- robot_trajectory_new(&tcFast);
+ robot_trajectory_new_backward(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- 0.45 + 0.3 - app_dist,
- 0.29 + 0.28,
- TURN(DEG2RAD(180)));
+ 0.85,
+ 0.38,
+ NO_TURN());
break;
case EV_MOTION_DONE:
- act_jaws(OPEN);
- FSM_TIMER(1000);
- break;
- case EV_TIMER:
- FSM_TRANSITION(load_second_green_figure);
+ enable_bumpers(false);
+ SUBFSM_RET(NULL);
break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ default:
break;
}
}
-FSM_STATE(load_second_green_figure)
+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 = true;
-
- 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(
- ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M + load_dist,
- 0.29 + 0.28,
- ARRIVE_FROM(DEG2RAD(180), 0.10));
+ 0.64+0.08,
+ ROBOT_AXIS_TO_BACK_M + 0.05,
+ ARRIVE_FROM(DEG2RAD(270), 0.10));
break;
case EV_MOTION_DONE:
- FSM_TIMER(1000);
- act_jaws(CLOSE);
- break;
- case EV_TIMER:
- FSM_TRANSITION(go_out_second_green_figure);
+ FSM_TRANSITION(return_home);
break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ default:
break;
}
}
-FSM_STATE(go_out_second_green_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 = true;
-
- robot_trajectory_new_backward(&tcFast);
+ robot_trajectory_new(&tcSlow); // new trajectory
+ robot_trajectory_add_point_trans(
+ 0.64 + 0.08,
+ 0.7);
robot_trajectory_add_final_point_trans(
- 0.45 + 0.3,
- 0.7,
- NO_TURN());
+ 0.4,
+ 1.0,
+ ARRIVE_FROM(DEG2RAD(180), 0.10));
break;
case EV_MOTION_DONE:
- if (read_sharp()) {
- FSM_TRANSITION(place_figure_to_bonus_area);
- } else {
- FSM_TRANSITION(place_figure_to_protected_block);
- }
- break;
+ enable_bumpers(true);
+ FSM_TIMER(2000);
+ break;
case EV_TIMER:
+ 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(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(&tcSlow);
+ robot_trajectory_new_backward(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- 0.45 + 0.175,
- 0.12 + 0.08 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
- ARRIVE_FROM(DEG2RAD(-90), 0.20));
- break;
- case EV_START:
- case EV_TIMER:
- FSM_TRANSITION(leave_protected_figure);
+ 0.6,
+ 1.0,
+ NO_TURN());
break;
- case EV_RETURN:
case EV_MOTION_DONE:
- act_jaws(OPEN);
- FSM_TIMER(1000);
+ FSM_TRANSITION(leave_home_for_totem);
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(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_backward(&tcFast);
- robot_trajectory_add_point_trans(
- 0.45 + 0.175,
- 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:
+ 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_RETURN:
case EV_MOTION_DONE:
- act_jaws(CLOSE);
- SUBFSM_RET(NULL);
+ if(up) FSM_TRANSITION(approach_totem_up);
+ else FSM_TRANSITION(approach_totem_down);
break;
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ default:
break;
}
}
-/** pick third figure from green area */
-FSM_STATE(approach_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(&tcFast);
+ robot_trajectory_new(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- 0.45 + 0.3 - app_dist,
- 0.29 + 2*0.28,
- TURN(DEG2RAD(180)));
+ totem_x,
+ 0.48,
+ TURN_CCW(DEG2RAD(90)));
break;
case EV_MOTION_DONE:
- act_jaws(OPEN);
- FSM_TIMER(1000);
+ FSM_TIMER(2000);
break;
case EV_TIMER:
- FSM_TRANSITION(load_third_green_figure);
+ FSM_TRANSITION(catch_totem_buillon_down);
break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ default:
break;
}
}
-
-FSM_STATE(load_third_green_figure)
+
+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 = 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 + load_dist,
- 0.29 + 2*0.28,
- ARRIVE_FROM(DEG2RAD(180), 0.20));
+ totem_x,
+ totem_y - ROBOT_AXIS_TO_FRONT_M - 0.05,
+ ARRIVE_FROM(DEG2RAD(90), 0.10));
break;
case EV_MOTION_DONE:
- act_jaws(CLOSE);
- FSM_TIMER(1000);
+ FSM_TIMER(2000);
break;
case EV_TIMER:
- FSM_TRANSITION(go_out_third_green_figure);
- break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ FSM_TRANSITION(leave_totem_down);
+ default:
break;
}
}
-FSM_STATE(go_out_third_green_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 = true;
-
- robot_trajectory_new_backward(&tcFast);
+ robot_trajectory_new_backward(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- 0.45 + 0.35,
- 0.29 + 2*0.28,
+ totem_x,
+ 0.48,
NO_TURN());
break;
case EV_MOTION_DONE:
- if (bonus_placed)
- FSM_TRANSITION(place_figure_to_near_area);
- else
- FSM_TRANSITION(place_figure_to_bonus_area);
- break;
- case EV_TIMER:
- break;
- case EV_START:
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
- robot.ignore_hokuyo = false;
+ FSM_TRANSITION(place_buillon_home);
+ default:
break;
}
}
-FSM_STATE(place_figure_to_near_area)
-{
- 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_add_final_point_trans(
- 0.45 + 0.35 + 0.175,
- 0.35 + 0.175 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
- TURN(DEG2RAD(-90)));
- break;
- case EV_MOTION_DONE:
- act_jaws(OPEN);
- FSM_TIMER(1000);
- break;
- case EV_TIMER:
- FSM_TRANSITION(leave_near_figure);
- break;
- case EV_START:
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
- break;
- }
-}
-
-FSM_STATE(leave_near_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_backward(&tcFast);
- robot_trajectory_add_final_point_trans(
- 0.45 + 0.35 + 0.175,
- 0.29 + 3*0.28,
- NO_TURN());
- break;
- case EV_MOTION_DONE:
- act_jaws(CLOSE);
- FSM_TIMER(1000);
- break;
- case EV_TIMER:
- SUBFSM_RET(NULL);
- break;
- case EV_START:
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
- break;
- }
-}
-
-FSM_STATE(place_figure_to_bonus_area)
+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(&tcSlow);
+ 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.7 + 0.175,
- 0.08 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
- ARRIVE_FROM(DEG2RAD(-90), 0.3));
+ 0.4,
+ 1.0,
+ ARRIVE_FROM(DEG2RAD(180),0.10));
break;
- case EV_START:
- case EV_TIMER:
- FSM_TRANSITION(leave_bonus_figure);
- break;
- case EV_RETURN:
case EV_MOTION_DONE:
- bonus_placed = true;
- act_jaws(OPEN);
- FSM_TIMER(1000);
- break;
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
- break;
- }
-}
-
-FSM_STATE(leave_bonus_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 = true;
-
- robot_trajectory_new_backward(&tcFast);
- robot_trajectory_add_final_point_trans(
- 0.45 + 0.7 + 0.2,
- 0.7,
- NO_TURN());
+ FSM_TIMER(2000);
break;
- case EV_START:
case EV_TIMER:
- break;
- case EV_RETURN:
- case EV_MOTION_DONE:
- act_jaws(CLOSE);
- act_lift(DOWN, 0, 0);
SUBFSM_RET(NULL);
- break;
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ default:
break;
}
}
-/** pick fourth green figure from green area */
-FSM_STATE(approach_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 = false;
-
- robot_trajectory_new(&tcFast);
+ robot_trajectory_new(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- 0.45 + 0.3 - app_dist,
- 0.29 + 3*0.28,
- TURN(DEG2RAD(180)));
+ totem_x,
+ 1.52,
+ TURN_CW(DEG2RAD(270)));
break;
case EV_MOTION_DONE:
- tower = read_sharp();
- act_jaws(OPEN);
- FSM_TIMER(1000);
+ FSM_TIMER(2000);
break;
case EV_TIMER:
- FSM_TRANSITION(load_fourth_green_figure);
+ FSM_TRANSITION(catch_totem_buillon_up);
break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ default:
break;
}
}
-FSM_STATE(load_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(&tcSlow);
+ robot_trajectory_new(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M + load_dist,
- 0.29 + 3*0.28,
- ARRIVE_FROM(DEG2RAD(180), 0.20));
+ totem_x,
+ totem_y + ROBOT_AXIS_TO_FRONT_M + 0.05,
+ ARRIVE_FROM(DEG2RAD(270), 0.10));
break;
case EV_MOTION_DONE:
- FSM_TIMER(1000);
- act_jaws(CLOSE);
+ FSM_TIMER(2000);
break;
case EV_TIMER:
- FSM_TRANSITION(go_out_fourth_green_figure);
+ FSM_TRANSITION(leave_totem_up);
break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ default:
break;
}
}
-FSM_STATE(go_out_fourth_green_figure)
+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 = true;
-
- robot_trajectory_new_backward(&tcFast);
+ robot_trajectory_new_backward(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- 0.45 + 0.175,
- 3*0.35 + 0.175,
+ totem_x,
+ 1.52,
NO_TURN());
break;
case EV_MOTION_DONE:
- FSM_TRANSITION(place_figure_to_red_square);
- break;
- case EV_TIMER:
- break;
- case EV_START:
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ SUBFSM_RET(NULL);
+ default:
break;
}
}
-FSM_STATE(place_figure_to_red_square)
-{
- 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(
- 0.45 + 0.175,
- 0.7 + 0.15 + 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);
- 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:
- break;
- }
-}
+/* State for odometry calibration */
-FSM_STATE(leave_red_square_figure)
+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(200);
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);
+ if(x1 != 0 || y1 != 0) {
+ 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 (" ", 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;
}
-}
-
-/** pick fifth green figure from green area */
-FSM_STATE(approach_fifth_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 = true;
-
- robot_trajectory_new(&tcFast);
- robot_trajectory_add_final_point_trans(
- 0.45 + 0.3 - app_dist,
- 0.29 + 4*0.28,
- TURN(DEG2RAD(180)));
- break;
- case EV_MOTION_DONE:
- act_jaws(OPEN);
- FSM_TIMER(1000);
- break;
- case EV_TIMER:
- FSM_TRANSITION(load_fifth_green_figure);
- break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
- break;
- }
-}
-
-FSM_STATE(load_fifth_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 = true;
-
- robot_trajectory_new(&tcSlow);
- robot_trajectory_add_final_point_trans(
- ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M + load_dist,
- 0.29 + 4*0.28,
- ARRIVE_FROM(DEG2RAD(180), 0.20));
- break;
- case EV_MOTION_DONE:
- FSM_TIMER(1000);
- act_jaws(CLOSE);
- break;
- case EV_TIMER:
- FSM_TRANSITION(go_out_fifth_green_figure);
- break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
- break;
- }
-}
-
-FSM_STATE(go_out_fifth_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 = true;
-
- robot_trajectory_new_backward(&tcFast);
- robot_trajectory_add_final_point_trans(
- 0.45 + 2*0.35 + 0.175,
- 4*0.35,
- NO_TURN());
- break;
- case EV_MOTION_DONE:
- break;
- case EV_TIMER:
- break;
- case EV_START:
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
- break;
- }
-}
-
-/** pick center figure */
-FSM_STATE(approach_center_figure)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.use_back_bumpers = false;
- robot.ignore_hokuyo = false;
-
- robot_trajectory_new(&tcVeryFast);
- robot_trajectory_add_point_trans(
- 0.45 + 0.35,
- 5*0.35 + 0.175);
- robot_trajectory_add_final_point_trans(
- 0.45 + 3*0.35,
- 4*0.35 + 0.1,
- TURN(DEG2RAD(-90)));
- FSM_TIMER(2000);
- break;
- case EV_START:
- case EV_TIMER:
- robot.use_back_bumpers = true;
- break;
- case EV_RETURN:
- case EV_MOTION_DONE:
- FSM_TRANSITION(load_center_figure);
- break;
- case EV_MOTION_ERROR:
- case EV_EXIT:
- break;
- }
-}
-
-FSM_STATE(load_center_figure)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- act_jaws(OPEN);
- 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_add_final_point_trans(
- 0.45 + 3*0.35,
- 3*0.35 + 0.1,
- ARRIVE_FROM(DEG2RAD(-90), 0.1));
- break;
- case EV_START:
- case EV_TIMER:
- break;
- case EV_RETURN:
- case EV_MOTION_DONE:
- act_jaws(CLOSE);
- FSM_TRANSITION(place_figure_to_bonus_area);
- break;
- case EV_MOTION_ERROR:
- case EV_EXIT:
- break;
- }
}
\ No newline at end of file
projects / eurobot / public.git / blobdiff