#define FSM_MAIN
-#include <robodata.h>
+#include "robodata.h"
#include <robot.h>
#include <fsm.h>
#include <unistd.h>
#include <string.h>
#include <robodim.h>
#include <error.h>
-#include "corns_configs.h"
#include "actuators.h"
+#include <sharp.h>
#include <trgen.h>
#include "match-timing.h"
-#include "eb2010misc.h"
#include <stdbool.h>
+#include <ul_log.h>
+
+UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file */
#include "common-states.h"
* "wait for start" states in particular strategies.
************************************************************************/
-static void set_initial_position()
+#undef DBG_FSM_STATE
+#define DBG_FSM_STATE(name) do { if (fsm->debug_states) ul_loginf("fsm %s %.1f: %s(%s)\n", \
+ fsm->debug_name, robot_current_time(), \
+ name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
+
+
+void set_initial_position()
{
- robot_set_est_pos_trans(ROBOT_AXIS_TO_FRONT_M,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2),
- DEG2RAD(180));
+ robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M,
+ PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
+ 0);
}
-static void actuators_home()
+void actuators_home()
{
- act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
+// 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
+ //act_lift(0, 0, 0);
}
void start_entry()
void start_exit()
{
- robot.corns = get_all_corns(robot.corns_conf_side, robot.corns_conf_center);
+
}
-/************************************************************************
- * Trajectory constraints used; They are initialized in the main() function in competition.cc
- ************************************************************************/
+bool read_sharp()
+{
+ int sharp_data = robot.orte.jaws_status.act_pos.left;
+ int sharp_dist = 0;
+ sharp_dist = s_gp2y0a21_ir2mmLong(sharp_data);
+ printf("sharp data: %dmm\n", sharp_dist);
+ return (sharp_dist <= 150 ? true : false);
+}
+
+void inline enable_bumpers(bool enabled)
+{
+ robot.use_left_bumper = enabled;
+ robot.use_right_bumper = enabled;
+ robot.use_back_bumpers = enabled;
+}
-struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
-#define VIDLE_TIMEOUT 2000
/************************************************************************
- * States that form the "collect some oranges" subautomaton. Calling automaton
- * SHOULD ALWAYS call the "approach_the_slope" state.
+ * Trajectory constraints used; They are initialized in the main() function in competition.cc
************************************************************************/
-bool inline is_ready_to_climb_the_slope(enum which_side which_slope, double x, double y) {
- bool ret;
- if (which_slope == MINE) {
- ret = x < 0.5 && y > PLAYGROUND_HEIGHT_M - 0.5;
- } else if (which_slope == OPPONENTS) {
- ret = x > 0.5 && y > PLAYGROUND_HEIGHT_M - 0.5;
- } else {
- printf("ERROR: unknown side;");
-#warning Remove the next line
- robot_exit();
+struct TrajectoryConstraints tcFast, tcVeryFast, tcSlow, tcVerySlow;
+double totem_x;
+double totem_y;
+bool up;
+
+FSM_STATE(approach_central_buillon)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ 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_MOTION_DONE:
+ FSM_TIMER(2000);
+ break;
+ case EV_TIMER:
+ FSM_TRANSITION(catch_central_buillon);
+ break;
+ default:
+ break;
}
- return ret;
}
-static struct slope_approach_style *slope_approach_style_p;
-
-/* assures that the robot is near the slope rim; if outside our starting area, approach the slope first */
-FSM_STATE(approach_the_slope)
+FSM_STATE(catch_central_buillon)
{
switch(FSM_EVENT) {
- case EV_ENTRY: {
- slope_approach_style_p = (struct slope_approach_style *) FSM_EVENT_PTR;
- if (slope_approach_style_p == NULL) {
- printf("\n\nit is not allowed to call the approach_the_slope state with NULL data!!\n\n");
-#warning remove the next line
- robot_exit();
- }
- double x, y, phi;
- robot_get_est_pos_trans(&x, &y, &phi);
- /* decide */
- bool ready_to_climb_the_slope = is_ready_to_climb_the_slope(slope_approach_style_p->which_side, x, y);
- /* if necessary, approach the slope */
- if (ready_to_climb_the_slope) {
- FSM_TRANSITION(climb_the_slope);
- } else {
- robot_goto_trans(
- /*
- x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.3, slope_approach_style_p->which_side),
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.05,
- ARRIVE_FROM(DEG2RAD(0),0.05),
- */
- x_coord(0.3, slope_approach_style_p->which_side),
- PLAYGROUND_HEIGHT_M - ROBOT_WIDTH_M/2 - 0.03,
- ARRIVE_FROM(DEG2RAD(0), 0.02),
- &tcFast);
- }
- break;
- }
+ case EV_ENTRY:
+ robot_trajectory_new(&tcSlow); // new trajectory
+ robot_trajectory_add_final_point_trans(
+ 1.3,
+ 0.58,
+ NO_TURN());
+ break;
case EV_MOTION_DONE:
- FSM_TRANSITION(climb_the_slope);
+ FSM_TIMER(2000);
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:
+ FSM_TRANSITION(leave_central_buillon);
+ break;
+ default:
break;
}
}
-void inline enable_switches(bool enabled)
+FSM_STATE(leave_central_buillon)
{
- robot.use_left_switch = enabled;
- robot.use_right_switch = enabled;
- robot.use_back_switch = enabled;
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new_backward(&tcSlow); // new trajectory
+ robot_trajectory_add_final_point_trans(
+ 0.85,
+ 0.38,
+ NO_TURN());
+ break;
+ case EV_MOTION_DONE:
+ enable_bumpers(false);
+ SUBFSM_RET(NULL);
+ break;
+ default:
+ break;
+ }
}
-FSM_STATE(climb_the_slope)
+FSM_STATE(push_bottle_bw)
{
switch(FSM_EVENT) {
- case EV_ENTRY: {
- // disables using side switches on bumpers when going up
- enable_switches(false);
- act_vidle(VIDLE_DOWN, VIDLE_FAST_SPEED);
- robot.ignore_hokuyo = true;
- /* create the trajectory and go */
- robot_trajectory_new_backward(&tcSlow);
- if (slope_approach_style_p->which_oranges == NEAR_PLAYGROUND_BOUNDARY) {
- robot_trajectory_add_point_trans(
- x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01);
- robot_trajectory_add_final_point_trans(
- x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M + 0.07, slope_approach_style_p->which_side),
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01,
- NO_TURN());
- } else if (slope_approach_style_p->which_oranges == NEAR_PLAYGROUND_CENTER) {
- robot_trajectory_add_point_trans(
- x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
- 1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) + 0.01 - 1.85));
- robot_trajectory_add_final_point_trans(
- x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M + 0.07, slope_approach_style_p->which_side),
- 1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) + 0.01 - 1.85),
- NO_TURN());
- }
- break;
- }
- case EV_MOTION_DONE:
- SUBFSM_TRANSITION(load_oranges, NULL);
+ case EV_ENTRY:
+ robot_trajectory_new_backward(&tcSlow); // new trajectory
+ robot_trajectory_add_point_trans(
+ 0.7,
+ 0.3);
+ robot_trajectory_add_final_point_trans(
+ 0.64+0.08,
+ ROBOT_AXIS_TO_BACK_M + 0.05,
+ ARRIVE_FROM(DEG2RAD(270), 0.10));
break;
- case EV_RETURN:
- FSM_TRANSITION(sledge_down);
+ case EV_MOTION_DONE:
+ FSM_TRANSITION(return_home);
break;
- case EV_TIMER:
- case EV_START:
- case EV_VIDLE_DONE:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
+ default:
break;
}
}
-/* one-state-subautomaton to load oranges in two stages */
-FSM_STATE(load_oranges)
+FSM_STATE(return_home)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- FSM_TIMER(500);
- act_vidle(VIDLE_MIDDLE, VIDLE_MEDIUM_SPEED);
+ robot_trajectory_new(&tcSlow); // new trajectory
+ robot_trajectory_add_point_trans(
+ 0.64 + 0.08,
+ 0.7);
+ robot_trajectory_add_final_point_trans(
+ 0.4,
+ 1.0,
+ ARRIVE_FROM(DEG2RAD(180), 0.10));
+ break;
+ case EV_MOTION_DONE:
+ enable_bumpers(true);
+ FSM_TIMER(2000);
break;
case EV_TIMER:
- // FIXME: respond to VIDLE EVENT
- act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
SUBFSM_RET(NULL);
break;
- case EV_MOTION_DONE:
- 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:
+ default:
break;
}
}
-FSM_STATE(sledge_down)
+FSM_STATE(leave_home)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot_trajectory_new(&tcFast);
+ robot_trajectory_new_backward(&tcSlow); // new trajectory
+ robot_trajectory_add_final_point_trans(
+ 0.6,
+ 1.0,
+ NO_TURN());
+ break;
+ case EV_MOTION_DONE:
+ FSM_TRANSITION(leave_home_for_totem);
+ break;
+ default:
+ break;
+ }
+}
- if (slope_approach_style_p->which_oranges == NEAR_PLAYGROUND_BOUNDARY) {
- robot_trajectory_add_point_trans(
- x_coord(1 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
- 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.20, slope_approach_style_p->which_side),
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.08);
+FSM_STATE(leave_home_for_totem)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new(&tcSlow); // new trajectory
+ if(up) {
robot_trajectory_add_final_point_trans(
- x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.26, slope_approach_style_p->which_side),
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.17,
+ 0.64,
+ 1.3,
NO_TURN());
- } else if (slope_approach_style_p->which_oranges == NEAR_PLAYGROUND_CENTER) {
- robot_trajectory_add_point_trans(
- x_coord(1 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
- 1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01 - 1.85));
+ }
+ else {
robot_trajectory_add_final_point_trans(
- x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.2, slope_approach_style_p->which_side),
- 1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01 - 1.85),
+ 0.64,
+ 0.7,
NO_TURN());
}
- /* robot_trajectory_add_point_trans(
- x_coord(1 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side),
- y_coord(PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01, slope_approach_style_p->which_oranges));
- robot_trajectory_add_point_trans(
- x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.20, slope_approach_style_p->which_side),
- y_coord(PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.08, slope_approach_style_p->which_oranges));
- robot_trajectory_add_final_point_trans(
- x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.26, slope_approach_style_p->which_side),
- y_coord(PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.14, slope_approach_style_p->which_oranges),
- NO_TURN()); */
break;
case EV_MOTION_DONE:
- /* just for sure, try to close it one more time */
- act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
- SUBFSM_RET(NULL);
- delete(slope_approach_style_p);
+ if(up) FSM_TRANSITION(approach_totem_up);
+ else FSM_TRANSITION(approach_totem_down);
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:
- // enables using side switches on bumpers
- enable_switches(true);
- robot.ignore_hokuyo = false;
- robot.check_turn_safety = true;
-
+ default:
break;
}
}
-/************************************************************************
- * The "unload our oranges" subautomaton
- ************************************************************************/
-
-FSM_STATE(to_cntainer_and_unld)
+FSM_STATE(approach_totem_down)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- /*
- if (slope_approach_style_p->which_side == 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);
- //}
+ robot_trajectory_new(&tcSlow); // new trajectory
+ robot_trajectory_add_final_point_trans(
+ totem_x,
+ 0.48,
+ TURN_CCW(DEG2RAD(90)));
break;
case EV_MOTION_DONE:
- FSM_TIMER(3000); // FIXME: test this
- act_vidle(VIDLE_DOWN, VIDLE_FAST_SPEED);
+ FSM_TIMER(2000);
break;
case EV_TIMER:
- act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
- SUBFSM_RET(NULL);
+ FSM_TRANSITION(catch_totem_buillon_down);
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:
+ default:
break;
}
}
-
-/************************************************************************
- * The "collect corns" subautomaton
- ************************************************************************/
-
-static enum where_to_go {
- CORN,
- TURN_AROUND,
- CONTAINER,
- NO_MORE_CORN
-} where_to_go = CORN;
-
-static struct corn *corn_to_get;
-
-FSM_STATE(rush_corns_decider)
+
+FSM_STATE(catch_totem_buillon_down)
{
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 */ {
- }
+ robot_trajectory_new(&tcSlow); // new trajectory
+ robot_trajectory_add_final_point_trans(
+ totem_x,
+ totem_y - ROBOT_AXIS_TO_FRONT_M - 0.05,
+ ARRIVE_FROM(DEG2RAD(90), 0.10));
+ break;
+ case EV_MOTION_DONE:
+ FSM_TIMER(2000);
break;
- case EV_START:
case EV_TIMER:
- case EV_RETURN:
- case EV_VIDLE_DONE:
+ FSM_TRANSITION(leave_totem_down);
+ default:
+ break;
+ }
+}
+
+FSM_STATE(leave_totem_down)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new_backward(&tcSlow); // new trajectory
+ robot_trajectory_add_final_point_trans(
+ totem_x,
+ 0.48,
+ NO_TURN());
+ break;
case EV_MOTION_DONE:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
+ FSM_TRANSITION(place_buillon_home);
+ default:
break;
}
}
-static int cnt = 0;
-FSM_STATE(approach_next_corn)
+FSM_STATE(place_buillon_home)
{
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_ENTRY:
+ 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.4,
+ 1.0,
+ ARRIVE_FROM(DEG2RAD(180),0.10));
+ break;
case EV_MOTION_DONE:
- cnt++;
- FSM_TRANSITION(rush_corns_decider);
+ FSM_TIMER(2000);
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:
+ SUBFSM_RET(NULL);
+ default:
break;
}
}
-FSM_STATE(rush_the_corn)
+FSM_STATE(approach_totem_up)
{
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;
+ robot_trajectory_new(&tcSlow); // new trajectory
+ robot_trajectory_add_final_point_trans(
+ totem_x,
+ 1.52,
+ TURN_CW(DEG2RAD(270)));
break;
case EV_MOTION_DONE:
- FSM_TRANSITION(rush_corns_decider);
+ FSM_TIMER(2000);
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:
+ FSM_TRANSITION(catch_totem_buillon_up);
+ break;
+ default:
break;
}
}
-// used to perform the maneuvre
-FSM_STATE(turn_around)
+FSM_STATE(catch_totem_buillon_up)
{
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));
+ robot_trajectory_new(&tcSlow); // new trajectory
+ robot_trajectory_add_final_point_trans(
+ totem_x,
+ totem_y + ROBOT_AXIS_TO_FRONT_M + 0.05,
+ ARRIVE_FROM(DEG2RAD(270), 0.10));
break;
case EV_MOTION_DONE:
- where_to_go = CORN;
- FSM_TRANSITION(rush_corns_decider);
+ FSM_TIMER(2000);
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:
+ FSM_TRANSITION(leave_totem_up);
+ break;
+ default:
+ break;
+ }
+}
+
+FSM_STATE(leave_totem_up)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new_backward(&tcSlow); // new trajectory
+ robot_trajectory_add_final_point_trans(
+ totem_x,
+ 1.52,
+ NO_TURN());
+ break;
+ case EV_MOTION_DONE:
+ SUBFSM_RET(NULL);
+ default:
break;
}
}
+
+/* State for odometry calibration */
+
+FSM_STATE(go_back_for_cal)
+{
+ double x1 = 0, y1 = 0;
+ char buffer [20];
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ 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_MOTION_DONE:
+ 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_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