From: Michal Vokac Date: Fri, 6 May 2011 07:59:43 +0000 (+0200) Subject: robofsm: First homologation version. X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/cb077943488a9ae47b809be2c0fe18d509fc0ea6 robofsm: First homologation version. --- diff --git a/src/robofsm/homologation.cc b/src/robofsm/homologation.cc index 4b376ad9..ee1bfc60 100644 --- a/src/robofsm/homologation.cc +++ b/src/robofsm/homologation.cc @@ -1,6 +1,6 @@ /* * homologation.cc 08/04/29 - * + * * Robot's control program intended for homologation (approval phase) on Eurobot 2009. * * Copyright: (c) 2009 CTU Dragons @@ -24,7 +24,6 @@ #include #include #include -#include "corns_configs.h" #include "actuators.h" #include #include "match-timing.h" @@ -47,21 +46,21 @@ struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow; FSM_STATE_DECL(init); FSM_STATE_DECL(wait_for_start); /* movement states */ -FSM_STATE_DECL(climb_the_slope); -FSM_STATE_DECL(sledge_down); -FSM_STATE_DECL(to_container_diag); +FSM_STATE_DECL(aproach_first_fugure); +FSM_STATE_DECL(load_first_figure); +FSM_STATE_DECL(to_red_square); FSM_STATE_DECL(to_container_ortho); -FSM_STATE_DECL(experiment_decider); -FSM_STATE_DECL(approach_next_corn); -FSM_STATE_DECL(rush_the_corn); -FSM_STATE_DECL(turn_around); -FSM_STATE_DECL(zvedej_vidle); +// FSM_STATE_DECL(experiment_decider); +// FSM_STATE_DECL(approach_next_corn); +// FSM_STATE_DECL(rush_the_corn); +// FSM_STATE_DECL(turn_around); +// FSM_STATE_DECL(zvedej_vidle); /************************************************************************ * INITIAL AND STARTING STATES ************************************************************************/ -FSM_STATE(init) +FSM_STATE(init) { switch (FSM_EVENT) { case EV_ENTRY: @@ -75,7 +74,7 @@ FSM_STATE(init) tcVerySlow = trajectoryConstraintsDefault; tcVerySlow.maxv = 0.05; tcVerySlow.maxacc = 0.05; - + FSM_TRANSITION(wait_for_start); break; default: @@ -86,14 +85,18 @@ FSM_STATE(init) void set_initial_position() { //FIXME: - robot_set_est_pos_trans(ROBOT_AXIS_TO_FRONT_M, - PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2), - DEG2RAD(180)); + robot_set_est_pos_trans(0.4 - ROBOT_AXIS_TO_FRONT_M, + PLAYGROUND_HEIGHT_M - 0.21, + 0); } void actuators_home() { - act_vidle(VIDLE_UP, VIDLE_FAST_SPEED); + act_jaws(CLOSE); + // drive lift to home position + act_lift(0, 0, 1); + // unset the homing request + act_lift(0, 0, 0); } #ifdef COMPETITION @@ -127,7 +130,7 @@ FSM_STATE(wait_for_start) sem_post(&robot.start); actuators_home(); set_initial_position(); - FSM_TRANSITION(climb_the_slope); + FSM_TRANSITION(aproach_first_fugure); break; case EV_TIMER: FSM_TIMER(1000); @@ -142,84 +145,75 @@ FSM_STATE(wait_for_start) case EV_RETURN: case EV_MOTION_ERROR: case EV_MOTION_DONE: - case EV_VIDLE_DONE: + //case EV_VIDLE_DONE: case EV_SWITCH_STRATEGY: DBG_PRINT_EVENT("unhandled event"); break; case EV_EXIT: - robot.corns = get_all_corns(robot.corns_conf_side, robot.corns_conf_center); - - /* - //opras na testovani zluteho: - robot_set_est_pos_trans(PLAYGROUND_WIDTH_M - ROBOT_AXIS_TO_FRONT_M, - PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2), - DEG2RAD(0)); - robot.team_color = YELLOW; - */ break; } } -FSM_STATE(zvedej_vidle) -{ - static int cnt = 0; - switch(FSM_EVENT) { - case EV_ENTRY: - case EV_TIMER: - FSM_TIMER(500); - act_vidle((VIDLE_UP - VIDLE_DOWN)*cnt/3 + VIDLE_DOWN, VIDLE_FAST_SPEED); - ul_logdeb("--------------------cnt: %d\n", cnt); - cnt++; - if(cnt >= 3) { - robot_exit(); - //FSM_TRANSITION(sledge_down); - } - break; - case EV_START: - case EV_RETURN: - case EV_VIDLE_DONE: - case EV_MOTION_DONE: - case EV_MOTION_ERROR: - case EV_SWITCH_STRATEGY: - DBG_PRINT_EVENT("unhandled event"); - case EV_EXIT: - break; - } -} +// FSM_STATE(zvedej_vidle) +// { +// static int cnt = 0; +// switch(FSM_EVENT) { +// case EV_ENTRY: +// case EV_TIMER: +// FSM_TIMER(500); +// act_vidle((VIDLE_UP - VIDLE_DOWN)*cnt/3 + VIDLE_DOWN, VIDLE_FAST_SPEED); +// ul_logdeb("--------------------cnt: %d\n", cnt); +// cnt++; +// if(cnt >= 3) { +// robot_exit(); +// //FSM_TRANSITION(sledge_down); +// } +// break; +// case EV_START: +// case EV_RETURN: +// case EV_VIDLE_DONE: +// case EV_MOTION_DONE: +// case EV_MOTION_ERROR: +// case EV_SWITCH_STRATEGY: +// DBG_PRINT_EVENT("unhandled event"); +// case EV_EXIT: +// break; +// } +// } /************************************************************************ * MOVEMENT STATES ************************************************************************/ -FSM_STATE(climb_the_slope) +FSM_STATE(aproach_first_fugure) { switch(FSM_EVENT) { case EV_ENTRY: // disables using side switches on bumpers when going up - robot.use_left_switch = false; - robot.use_right_switch = false; - act_vidle(VIDLE_DOWN, VIDLE_FAST_SPEED); - robot.ignore_hokuyo = true; - robot_trajectory_new_backward(&tcSlow); + robot.use_left_bumper = false; + robot.use_right_bumper = false; + robot.use_back_bumpers = false; + + robot_trajectory_new(&tcSlow); robot_trajectory_add_point_trans( - 0.5 - ROBOT_AXIS_TO_BACK_M, - PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)); - /* position for collecting oranges*/ + 0.4+0.05+0.25, + PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.05); robot_trajectory_add_final_point_trans( - SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M + 0.05, - PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01, - NO_TURN()); + 0.4+0.05+0.25, + 0.29+0.28+0.28, + TURN_CCW(0)); break; case EV_MOTION_DONE: - FSM_TIMER(2000); - act_vidle(VIDLE_UP, VIDLE_FAST_SPEED); + FSM_TIMER(3000); + act_jaws(OPEN); + //FSM_TRANSITION(load_first_figure); break; case EV_START: case EV_TIMER: - FSM_TRANSITION(sledge_down); + FSM_TRANSITION(load_first_figure); break; case EV_RETURN: - case EV_VIDLE_DONE: + //case EV_VI_DONE: case EV_MOTION_ERROR: case EV_SWITCH_STRATEGY: DBG_PRINT_EVENT("unhandled event"); @@ -228,95 +222,58 @@ FSM_STATE(climb_the_slope) } } -FSM_STATE(sledge_down) +FSM_STATE(load_first_figure) { switch(FSM_EVENT) { case EV_ENTRY: - robot_trajectory_new(&tcFast); - robot_trajectory_add_point_trans( - 1 -ROBOT_AXIS_TO_BACK_M, - PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01); - robot_trajectory_add_point_trans( - SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.26, - PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.10); - robot_trajectory_add_final_point_trans(0.50, PLAYGROUND_HEIGHT_M - 0.6, NO_TURN()); + robot_trajectory_new_backward(&tcSlow); + robot_trajectory_add_final_point_trans( + 0.2+0.1+ROBOT_AXIS_TO_BACK_M, + 0.29+0.28+0.28, NO_TURN()); break; case EV_MOTION_DONE: - act_vidle(VIDLE_UP, VIDLE_FAST_SPEED); - FSM_TRANSITION(to_container_diag); - //FSM_TRANSITION(to_container_ortho); + FSM_TIMER(2000); + act_jaws(CATCH); break; case EV_START: case EV_TIMER: + FSM_TRANSITION(to_red_square); case EV_RETURN: - case EV_VIDLE_DONE: + //case EV_VIDLE_DONE: case EV_MOTION_ERROR: case EV_SWITCH_STRATEGY: DBG_PRINT_EVENT("unhandled event"); case EV_EXIT: // enables using side switches on bumpers - robot.use_left_switch = true; - robot.use_right_switch = true; - robot.ignore_hokuyo = false; + //robot.use_left_switch = true; + //robot.use_right_switch = true; + //robot.ignore_hokuyo = false; break; } } -FSM_STATE(to_container_diag) +FSM_STATE(to_red_square) { switch(FSM_EVENT) { case EV_ENTRY: - robot_trajectory_new(&tcFast); + robot_trajectory_new(&tcSlow); // face the rim with front of the robot //robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.12, ARRIVE_FROM(DEG2RAD(-90), 0.10)); // face the rim with back of the robot - robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M-0.6, 0.35); - robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05)); + //robot_trajectory_add_point_trans(0.4+0.05+0.2, 0.35+0.35+0.17); + robot_trajectory_add_final_point_trans(0.4+0.05+0.2 + ROBOT_AXIS_TO_FIGURE_CENTER_M, 0.35+0.35+0.17, NO_TURN()); break; case EV_MOTION_DONE: - FSM_TIMER(6000); - act_vidle(VIDLE_DOWN, VIDLE_FAST_SPEED); + FSM_TIMER(5000); + act_jaws(OPEN); break; case EV_TIMER: - act_vidle(VIDLE_UP, VIDLE_FAST_SPEED); - FSM_TRANSITION(approach_next_corn); - break; - case EV_START: - case EV_RETURN: - case EV_VIDLE_DONE: - case EV_MOTION_ERROR: - case EV_SWITCH_STRATEGY: - DBG_PRINT_EVENT("unhandled event"); - case EV_EXIT: - break; - } -} - -FSM_STATE(to_container_ortho) -{ - switch(FSM_EVENT) { - case EV_ENTRY: - robot_trajectory_new(&tcFast); - robot_trajectory_add_point_trans( - SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.15, - PLAYGROUND_HEIGHT_M - 0.355); - robot_trajectory_add_point_trans(0.55, PLAYGROUND_HEIGHT_M - 0.65); - robot_trajectory_add_point_trans(0.90, PLAYGROUND_HEIGHT_M - 0.75); - robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.60, PLAYGROUND_HEIGHT_M - 0.7); - robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.35, PLAYGROUND_HEIGHT_M - 0.9); - - // face the rim with front of the robot - //robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.12, ARRIVE_FROM(DEG2RAD(-90), 0.10)); - // face the rim with back of the robot - robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.40, TURN_CCW(DEG2RAD(90))); - break; - case EV_MOTION_DONE: - act_vidle(VIDLE_DOWN, VIDLE_FAST_SPEED); + //act_jaws(OPEN); + //FSM_TRANSITION(wait_for_start); break; case EV_START: - case EV_TIMER: case EV_RETURN: - case EV_VIDLE_DONE: + //case EV_VIDLE_DONE: case EV_MOTION_ERROR: case EV_SWITCH_STRATEGY: DBG_PRINT_EVENT("unhandled event"); @@ -325,135 +282,135 @@ FSM_STATE(to_container_ortho) } } -static enum where_to_go { - CORN, - TURN_AROUND, - CONTAINER, - NO_MORE_CORN -} where_to_go = CORN; - -static struct corn *corn_to_get; - -FSM_STATE(experiment_decider) -{ - - switch(FSM_EVENT) { - case EV_ENTRY: - if (where_to_go == CORN) { - FSM_TRANSITION(approach_next_corn); - } else if (where_to_go == CONTAINER) { - FSM_TRANSITION(rush_the_corn); - } else if (where_to_go == TURN_AROUND) { - FSM_TRANSITION(turn_around); - } else /* NO_MORE_CORN */ { - } - break; - case EV_START: - case EV_TIMER: - case EV_RETURN: - case EV_VIDLE_DONE: - case EV_MOTION_DONE: - case EV_MOTION_ERROR: - case EV_SWITCH_STRATEGY: - DBG_PRINT_EVENT("unhandled event"); - case EV_EXIT: - break; - } -} - -static int cnt = 0; -FSM_STATE(approach_next_corn) -{ - switch(FSM_EVENT) { - case EV_ENTRY: { - double x, y, phi; - robot_get_est_pos(&x, &y, &phi); - ul_logdeb("approach_next_corn: puck cnt: %d, est pos %.3f, %.3f, %.3f\n", - cnt, x, y, phi); - - corn_to_get = choose_next_corn(); - if (corn_to_get) { - Pos *p = get_corn_approach_position(corn_to_get); - corn_to_get->was_collected = true; - //robot_trajectory_new(&tcFast); - //robot_trajectory_add_final_point_trans(robot_trajectory_add_final_point_trans(p->x, p->y, TURN(p->phi)); - robot_goto_trans(p->x, p->y, TURN(p->phi), &tcFast); - delete(p); - where_to_go = CONTAINER; - } else { - where_to_go = NO_MORE_CORN; - } - break; - } - case EV_MOTION_DONE: - cnt++; - FSM_TRANSITION(experiment_decider); - break; - case EV_START: - case EV_TIMER: - case EV_RETURN: - case EV_VIDLE_DONE: - case EV_MOTION_ERROR: - case EV_SWITCH_STRATEGY: - DBG_PRINT_EVENT("unhandled event"); - case EV_EXIT: - break; - } -} - -FSM_STATE(rush_the_corn) -{ - switch(FSM_EVENT) { - case EV_ENTRY: - double x; - if (robot.team_color == BLUE) { - x = corn_to_get->position.x; - } else { - x = PLAYGROUND_WIDTH_M - corn_to_get->position.x; - } - ul_logdeb("x = %.3f, %.3f \n", x, corn_to_get->position.y); - remove_wall_around_corn(x, corn_to_get->position.y); - robot_goto_trans(PLAYGROUND_WIDTH_M - 0.4, 0.15, ARRIVE_FROM(DEG2RAD(-90), 0.02), &tcFast); - where_to_go = TURN_AROUND; - break; - case EV_MOTION_DONE: - FSM_TRANSITION(experiment_decider); - break; - case EV_START: - case EV_TIMER: - case EV_RETURN: - case EV_VIDLE_DONE: - case EV_MOTION_ERROR: - case EV_SWITCH_STRATEGY: - DBG_PRINT_EVENT("unhandled event"); - case EV_EXIT: - break; - } -} - -// used to perform the maneuvre -FSM_STATE(turn_around) -{ - switch(FSM_EVENT) { - case EV_ENTRY: - robot_trajectory_new_backward(&tcFast); - robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, TURN_CCW(90)); - break; - case EV_MOTION_DONE: - where_to_go = CORN; - FSM_TRANSITION(experiment_decider); - break; - case EV_START: - case EV_TIMER: - case EV_RETURN: - case EV_VIDLE_DONE: - case EV_MOTION_ERROR: - case EV_SWITCH_STRATEGY: - DBG_PRINT_EVENT("unhandled event"); - case EV_EXIT: - break; - } -} +// static enum where_to_go { +// CORN, +// TURN_AROUND, +// CONTAINER, +// NO_MORE_CORN +// } where_to_go = CORN; +// +// static struct corn *corn_to_get; +// +// FSM_STATE(experiment_decider) +// { +// +// switch(FSM_EVENT) { +// case EV_ENTRY: +// if (where_to_go == CORN) { +// FSM_TRANSITION(approach_next_corn); +// } else if (where_to_go == CONTAINER) { +// FSM_TRANSITION(rush_the_corn); +// } else if (where_to_go == TURN_AROUND) { +// FSM_TRANSITION(turn_around); +// } else /* NO_MORE_CORN */ { +// } +// break; +// case EV_START: +// case EV_TIMER: +// case EV_RETURN: +// case EV_VIDLE_DONE: +// case EV_MOTION_DONE: +// case EV_MOTION_ERROR: +// case EV_SWITCH_STRATEGY: +// DBG_PRINT_EVENT("unhandled event"); +// case EV_EXIT: +// break; +// } +// } + +// static int cnt = 0; +// FSM_STATE(approach_next_corn) +// { +// switch(FSM_EVENT) { +// case EV_ENTRY: { +// double x, y, phi; +// robot_get_est_pos(&x, &y, &phi); +// ul_logdeb("approach_next_corn: puck cnt: %d, est pos %.3f, %.3f, %.3f\n", +// cnt, x, y, phi); +// +// corn_to_get = choose_next_corn(); +// if (corn_to_get) { +// Pos *p = get_corn_approach_position(corn_to_get); +// corn_to_get->was_collected = true; +// //robot_trajectory_new(&tcFast); +// //robot_trajectory_add_final_point_trans(robot_trajectory_add_final_point_trans(p->x, p->y, TURN(p->phi)); +// robot_goto_trans(p->x, p->y, TURN(p->phi), &tcFast); +// delete(p); +// where_to_go = CONTAINER; +// } else { +// where_to_go = NO_MORE_CORN; +// } +// break; +// } +// case EV_MOTION_DONE: +// cnt++; +// FSM_TRANSITION(experiment_decider); +// break; +// case EV_START: +// case EV_TIMER: +// case EV_RETURN: +// case EV_VIDLE_DONE: +// case EV_MOTION_ERROR: +// case EV_SWITCH_STRATEGY: +// DBG_PRINT_EVENT("unhandled event"); +// case EV_EXIT: +// break; +// } +// } + +// FSM_STATE(rush_the_corn) +// { +// switch(FSM_EVENT) { +// case EV_ENTRY: +// double x; +// if (robot.team_color == BLUE) { +// x = corn_to_get->position.x; +// } else { +// x = PLAYGROUND_WIDTH_M - corn_to_get->position.x; +// } +// ul_logdeb("x = %.3f, %.3f \n", x, corn_to_get->position.y); +// remove_wall_around_corn(x, corn_to_get->position.y); +// robot_goto_trans(PLAYGROUND_WIDTH_M - 0.4, 0.15, ARRIVE_FROM(DEG2RAD(-90), 0.02), &tcFast); +// where_to_go = TURN_AROUND; +// break; +// case EV_MOTION_DONE: +// FSM_TRANSITION(experiment_decider); +// break; +// case EV_START: +// case EV_TIMER: +// case EV_RETURN: +// case EV_VIDLE_DONE: +// case EV_MOTION_ERROR: +// case EV_SWITCH_STRATEGY: +// DBG_PRINT_EVENT("unhandled event"); +// case EV_EXIT: +// break; +// } +// } + +// // used to perform the maneuvre +// FSM_STATE(turn_around) +// { +// switch(FSM_EVENT) { +// case EV_ENTRY: +// robot_trajectory_new_backward(&tcFast); +// robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, TURN_CCW(90)); +// break; +// case EV_MOTION_DONE: +// where_to_go = CORN; +// FSM_TRANSITION(experiment_decider); +// break; +// case EV_START: +// case EV_TIMER: +// case EV_RETURN: +// case EV_VIDLE_DONE: +// case EV_MOTION_ERROR: +// case EV_SWITCH_STRATEGY: +// DBG_PRINT_EVENT("unhandled event"); +// case EV_EXIT: +// break; +// } +// } /************************************************************************