-}
-
-/** 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;
- }