#include <error.h>
#include "corns_configs.h"
#include "actuators.h"
-#include <trgen.h>
#include "match-timing.h"
#include "eb2010misc.h"
-
-/************************************************************************
- * Trajectory constraints used, are initialized in the init state
- ************************************************************************/
-
-struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
-
-/************************************************************************
- * FSM STATES DECLARATION
- ************************************************************************/
-
-/* initial and starting states */
-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(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(approach_the_slope);
-
-/************************************************************************
- * INITIAL AND STARTING STATES
- ************************************************************************/
-
-FSM_STATE(init)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- tcFast = trajectoryConstraintsDefault;
- tcFast.maxv = 1;
- tcFast.maxacc = 0.3;
- tcFast.maxomega = 2;
- tcSlow = trajectoryConstraintsDefault;
- tcSlow.maxv = 0.3;
- tcSlow.maxacc = 0.1;
- tcVerySlow = trajectoryConstraintsDefault;
- tcVerySlow.maxv = 0.05;
- tcVerySlow.maxacc = 0.05;
-
- FSM_TRANSITION(wait_for_start);
- break;
- default:
- break;
- }
-}
-
-void set_initial_position()
-{
- //FIXME:
- robot_set_est_pos_trans(ROBOT_AXIS_TO_FRONT_M,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2),
- DEG2RAD(180));
-}
-
-void actuators_home()
-{
- act_vidle(VIDLE_UP);
-}
-
-#ifdef COMPETITION
-#define WAIT_FOR_START
-#else
-#undef WAIT_FOR_START
-#endif
-
-FSM_STATE(wait_for_start)
-{
- pthread_t thid;
- switch (FSM_EVENT) {
- case EV_ENTRY:
- #ifdef WAIT_FOR_START
- printf("WAIT_FOR_START mode set\n");
- #else
- printf("WAIT_FOR_START mode NOT set\n");
- #endif
- #ifdef COMPETITION
- printf("COMPETITION mode set\n");
- #else
- printf("COMPETITION mode NOT set\n");
- #endif
- robot.check_turn_safety = false;
- pthread_create(&thid, NULL, timing_thread, NULL);
-#ifdef WAIT_FOR_START
- FSM_TIMER(1000);
- break;
-#endif
- case EV_START:
- /* start competition timer */
- sem_post(&robot.start);
- actuators_home();
- set_initial_position();
- FSM_TRANSITION(approach_the_slope);
- //FSM_TRANSITION(climb_the_slope);
- break;
- case EV_TIMER:
- FSM_TIMER(1000);
- // We set initial position periodically in
- // order for it to be updated on the display
- // if the team color is changed during waiting
- // for start.
- set_initial_position();
- if (robot.start_state == START_PLUGGED_IN)
- actuators_home();
- break;
- case EV_RETURN:
- case EV_GOAL_NOT_REACHABLE:
- case EV_ACTION_DONE:
- case EV_ACTION_ERROR:
- case EV_MOTION_DONE:
- 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);
- printf("--------------------cnt: %d\n", cnt);
- cnt++;
- if(cnt >= 3) {
- robot_exit();
- //FSM_TRANSITION(sledge_down);
- }
- break;
- case EV_START:
- case EV_RETURN:
- case EV_ACTION_DONE:
- case EV_ACTION_ERROR:
- case EV_MOTION_DONE:
- case EV_GOAL_NOT_REACHABLE:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
- break;
- }
-}
-
-/************************************************************************
- * MOVEMENT STATES
- ************************************************************************/
-
-static enum strategy {
- OPPONENTS_ORAGES_TOO,
- CORNS_ONLY
-} strategy = OPPONENTS_ORAGES_TOO;
-
-static enum which_side which_slope;
-static int slope_cnt = 0;
-FSM_STATE(approach_the_slope)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- if (strategy == OPPONENTS_ORAGES_TOO) {
- if (slope_cnt == 0) {
- which_slope = MINE;
- FSM_TRANSITION(climb_the_slope);
- } else if (slope_cnt == 1) {
- which_slope = OPPONENTS;
- robot_goto_trans(
- x_coord(0.3, which_slope),
- PLAYGROUND_HEIGHT_M - ROBOT_WIDTH_M/2 - 0.03,
- ARRIVE_FROM(DEG2RAD(0), 0.02),
- &tcFast);
- } else {
- which_slope = MINE;
- FSM_TRANSITION(approach_next_corn);
- }
- } else if (strategy == CORNS_ONLY) {
- if (slope_cnt == 0) {
- which_slope = MINE;
- FSM_TRANSITION(climb_the_slope);
- } else if (slope_cnt == 1) {
- FSM_TRANSITION(approach_next_corn);
- }
- }
- slope_cnt++;
- break;
- case EV_MOTION_DONE:
- FSM_TRANSITION(climb_the_slope);
- break;
- case EV_START:
- case EV_TIMER:
- case EV_RETURN:
- case EV_ACTION_DONE:
- case EV_ACTION_ERROR:
- case EV_GOAL_NOT_REACHABLE:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
- break;
- }
-}
-
-FSM_STATE(climb_the_slope)
-{
- 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;
- robot.use_back_switch = false;
- act_vidle(VIDLE_DOWN);
- robot.ignore_hokuyo = true;
- robot_trajectory_new_backward(&tcSlow);
- if (which_slope == MINE) {
- robot_trajectory_add_point_trans(
- x_coord(0.5 - ROBOT_AXIS_TO_BACK_M, which_slope),
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2));
- } else {
- robot_trajectory_add_point_trans(
- x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M, which_slope),
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01);
- }
- /* position for collecting oranges*/
- robot_trajectory_add_final_point_trans(
- x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M + 0.07, which_slope),
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01,
- NO_TURN());
- break;
- case EV_MOTION_DONE:
- FSM_TIMER(2000);
- act_vidle(VIDLE_UP);
- break;
- case EV_START:
- case EV_TIMER:
- FSM_TRANSITION(sledge_down);
- break;
- case EV_RETURN:
- case EV_ACTION_DONE:
- case EV_ACTION_ERROR:
- case EV_GOAL_NOT_REACHABLE:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
- break;
- }
-}
-
-FSM_STATE(sledge_down)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcFast);
- robot_trajectory_add_point_trans(
- x_coord(1 - ROBOT_AXIS_TO_BACK_M, which_slope),
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01);
- robot_trajectory_add_point_trans(
- x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.26, which_slope),
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.10);
- robot_trajectory_add_final_point_trans(
- x_coord(0.50, which_slope),
- PLAYGROUND_HEIGHT_M - 0.6,
- NO_TURN());
- break;
- case EV_MOTION_DONE:
- act_vidle(VIDLE_UP);
- FSM_TRANSITION(to_container_diag);
- //FSM_TRANSITION(to_container_ortho);
- break;
- case EV_START:
- case EV_TIMER:
- case EV_RETURN:
- case EV_ACTION_DONE:
- case EV_ACTION_ERROR:
- case EV_GOAL_NOT_REACHABLE:
- 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.use_back_switch = true;
- robot.ignore_hokuyo = false;
- robot.check_turn_safety = true;
-
- break;
- }
-}
-
-FSM_STATE(to_container_diag)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- if (which_slope == MINE) {
- robot_trajectory_new(&tcFast);
- // 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));
- } else {
- robot_goto_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05), &tcFast);
- }
- break;
- case EV_MOTION_DONE:
- FSM_TIMER(6000);
- act_vidle(VIDLE_VYSIP);
- break;
- case EV_TIMER:
- act_vidle(VIDLE_UP);
- FSM_TRANSITION(approach_the_slope);
- //FSM_TRANSITION(approach_next_corn);
- break;
- case EV_START:
- case EV_RETURN:
- case EV_ACTION_DONE:
- case EV_ACTION_ERROR:
- case EV_GOAL_NOT_REACHABLE:
- 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_VYSIP);
- break;
- case EV_START:
- case EV_TIMER:
- case EV_RETURN:
- case EV_ACTION_DONE:
- case EV_ACTION_ERROR:
- case EV_GOAL_NOT_REACHABLE:
- 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_ACTION_DONE:
- case EV_ACTION_ERROR:
- case EV_MOTION_DONE:
- case EV_GOAL_NOT_REACHABLE:
- 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);
- printf("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_ACTION_DONE:
- case EV_ACTION_ERROR:
- case EV_GOAL_NOT_REACHABLE:
- 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;
- }
- 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_ACTION_DONE:
- case EV_ACTION_ERROR:
- case EV_GOAL_NOT_REACHABLE:
- 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_ACTION_DONE:
- case EV_ACTION_ERROR:
- case EV_GOAL_NOT_REACHABLE:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
- break;
- }
-}
-
-
-/************************************************************************
- * MAIN FUNCTION
- ************************************************************************/
+#include "common-states.h"
int main()
{
robot.fsm.motion.debug_states = 1;
//robot.fsm.act.debug_states = 1;
- robot.fsm.main.state = &fsm_state_main_init;
+ //robot.fsm.main.state = &fsm_state_main_start_opp_corn;
+ //robot.fsm.main.state = &fsm_state_main_start_opp_oranges;
+ robot.fsm.main.state = &fsm_state_main_start_six_oranges;
+
//robot.fsm.main.transition_callback = trans_callback;
//robot.fsm.motion.transition_callback = move_trans_callback;
+ tcFast = trajectoryConstraintsDefault;
+ tcFast.maxv = 1;
+ tcFast.maxacc = 0.3;
+ tcFast.maxomega = 2;
+ tcSlow = trajectoryConstraintsDefault;
+ tcSlow.maxv = 0.3;
+ tcSlow.maxacc = 0.1;
+ tcVerySlow = trajectoryConstraintsDefault;
+ tcVerySlow.maxv = 0.05;
+ tcVerySlow.maxacc = 0.05;
+
rv = robot_start();
if (rv) error(1, errno, "robot_start() returned %d\n", rv);
case EV_ACTION_DONE:
case EV_ACTION_ERROR:
case EV_MOTION_DONE:
- case EV_GOAL_NOT_REACHABLE:
+ case EV_MOTION_ERROR:
DBG_PRINT_EVENT("unhandled event");
case EV_EXIT:
break;
projects / eurobot / public.git / blobdiff