#include <robodim.h>
#include <error.h>
#include "actuators.h"
+#include <sharp.h>
#include <trgen.h>
#include "match-timing.h"
#include <stdbool.h>
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";
// drive lift to home position
//act_lift(0, 0, 1);
// unset the homing request
}
+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_back_bumpers = enabled;
}
-void enable_my_square_walls(bool enabled)
-{
- for (int i = 0; i<6; i++) { /* columns*/
- for (int j = 1; j<5; j++) { /* rows */
- if (!((i+j)%2)) {
- if (robot.team_color == BLUE)
- ShmapSetRectangleFlag(0.45 + i*0.35 + 0.05,
- PLAYGROUND_HEIGHT_M-(j + 1)*0.35 + 0.05,
- (0.45 + i*0.35) + 0.25,
- (PLAYGROUND_HEIGHT_M-(j + 1)*0.35) + 0.25,
- enabled*MAP_FLAG_WALL, (!enabled)*MAP_FLAG_WALL);
- } else {
- if (robot.team_color == RED)
- ShmapSetRectangleFlag(0.45 + i*0.35 + 0.05,
- PLAYGROUND_HEIGHT_M-(j + 1)*0.35 + 0.05,
- (0.45 + i*0.35) + 0.25,
- (PLAYGROUND_HEIGHT_M-(j + 1)*0.35) + 0.25,
- enabled*MAP_FLAG_WALL, (!enabled)*MAP_FLAG_WALL);
- }
- }
- }
-}
+
/************************************************************************
* Trajectory constraints used; They are initialized in the main() function in competition.cc
************************************************************************/
-struct TrajectoryConstraints tcJerk, tcFast, tcSlow, tcVerySlow;
-
-/** generate random positions on oponent squares and goto this position */
-FSM_STATE(move_around)
-{
- int goal;
- double goal_x, goal_y;
-
- switch(FSM_EVENT) {
- case EV_ENTRY:
- act_jaws(CLOSE);
- enable_my_square_walls(true);
- goal = rand() % 12;
-
- 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(), NULL);
- break;
- case EV_START:
- case EV_TIMER:
- break;
- case EV_RETURN:
- case EV_MOTION_DONE:
- SUBFSM_RET(NULL);
- break;
- case EV_MOTION_ERROR:
- case EV_EXIT:
- break;
- }
-}
+struct TrajectoryConstraints tcFast, tcVeryFast, tcSlow, tcVerySlow;
+double totem_x;
+double totem_y;
+bool up;
-/** securely bypass firt figure in front of starting area */
-FSM_STATE(bypass_figure_in_front_of_start)
+FSM_STATE(approach_central_buillon)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot_trajectory_new(&tcFast);
+ robot_trajectory_new(&tcSlow); // new trajectory
robot_trajectory_add_point_trans(
- 0.45 + 0.3,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2));
- robot_trajectory_add_final_point_trans(
- 0.45 + 0.35,
- 0.7 + 0.7,
- NO_TURN());
- break;
- case EV_MOTION_DONE:
- SUBFSM_RET(NULL);
- 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;
- }
-}
-
-/** pick second figure from green area */
-FSM_STATE(approach_second_green_figure)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.use_back_bumpers = true;
-
- robot_trajectory_new(&tcFast);
+ 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(
- 0.45 + 0.35,
- 0.29 + 0.28,
- ARRIVE_FROM(DEG2RAD(180), 0.10));
+ 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:
- act_jaws(OPEN);
FSM_TIMER(2000);
break;
case EV_TIMER:
- FSM_TRANSITION(load_second_green_figure);
+ FSM_TRANSITION(catch_central_buillon);
break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ default:
break;
}
}
-FSM_STATE(load_second_green_figure)
+FSM_STATE(catch_central_buillon)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- 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 + 0.06,
- 0.29 + 0.28,
- ARRIVE_FROM(DEG2RAD(180), 0.20));
+ 1.3,
+ 0.58,
+ NO_TURN());
break;
case EV_MOTION_DONE:
FSM_TIMER(2000);
- act_jaws(CATCH);
break;
case EV_TIMER:
- FSM_TRANSITION(go_out_second_green_figure);
+ FSM_TRANSITION(leave_central_buillon);
break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
- // enables using side switches on bumpers
- //robot.use_left_switch = true;
- //robot.use_right_switch = true;
- //robot.ignore_hokuyo = false;
+ default:
break;
}
}
-FSM_STATE(go_out_second_green_figure)
+FSM_STATE(leave_central_buillon)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot_trajectory_new_backward(&tcFast);
+ robot_trajectory_new_backward(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- 0.45 + 0.2,
- 0.7,
+ 0.85,
+ 0.38,
NO_TURN());
break;
case EV_MOTION_DONE:
- case EV_TIMER:
- FSM_TRANSITION(place_figure_to_protected_block);
+ enable_bumpers(false);
+ 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(push_bottle_bw)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- 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(
- 0.45 + 0.2,
- 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.64+0.08,
+ ROBOT_AXIS_TO_BACK_M + 0.05,
+ ARRIVE_FROM(DEG2RAD(270), 0.10));
break;
- case EV_RETURN:
case EV_MOTION_DONE:
- act_jaws(OPEN);
- FSM_TIMER(2000);
+ FSM_TRANSITION(return_home);
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(return_home)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- FSM_TIMER(2500);
- robot_trajectory_new_backward(&tcFast);
+ robot_trajectory_new(&tcSlow); // new trajectory
robot_trajectory_add_point_trans(
- 0.45 + 0.175,
+ 0.64 + 0.08,
0.7);
robot_trajectory_add_final_point_trans(
- 0.45 + 0.35,
- 0.29 + 2*0.28,
- TURN(DEG2RAD(0)));
- break;
- case EV_START:
- case EV_TIMER:
- act_jaws(CLOSE);
+ 0.4,
+ 1.0,
+ ARRIVE_FROM(DEG2RAD(180), 0.10));
break;
- case EV_RETURN:
case EV_MOTION_DONE:
+ enable_bumpers(true);
+ FSM_TIMER(2000);
+ break;
+ case EV_TIMER:
SUBFSM_RET(NULL);
break;
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ default:
break;
}
}
-/** pick third figure from green area */
-FSM_STATE(approach_third_green_figure)
+FSM_STATE(leave_home)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot.use_left_bumper = false;
- robot.use_right_bumper = false;
- robot.use_back_bumpers = true;
-
- robot_trajectory_new(&tcFast);
+ robot_trajectory_new_backward(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- 0.45 + 0.35,
- 0.29 + 2*0.28,
+ 0.6,
+ 1.0,
NO_TURN());
break;
case EV_MOTION_DONE:
- act_jaws(OPEN);
- FSM_TIMER(2000);
- break;
- case EV_TIMER:
- FSM_TRANSITION(load_third_green_figure);
+ FSM_TRANSITION(leave_home_for_totem);
break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ default:
break;
}
}
-FSM_STATE(load_third_green_figure)
+FSM_STATE(leave_home_for_totem)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot_trajectory_new(&tcSlow);
- robot_trajectory_add_final_point_trans(
- ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M + 0.06,
- 0.29 + 2*0.28,
- ARRIVE_FROM(DEG2RAD(180), 0.20));
+ 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_MOTION_DONE:
- FSM_TIMER(2000);
- act_jaws(CATCH);
- break;
- case EV_TIMER:
- FSM_TRANSITION(go_out_third_green_figure);
+ if(up) FSM_TRANSITION(approach_totem_up);
+ else FSM_TRANSITION(approach_totem_down);
break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
- // enables using side switches on bumpers
- //robot.use_left_switch = true;
- //robot.use_right_switch = true;
- //robot.ignore_hokuyo = false;
+ default:
break;
}
}
-FSM_STATE(go_out_third_green_figure)
+FSM_STATE(approach_totem_down)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot_trajectory_new_backward(&tcFast);
+ robot_trajectory_new(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- 0.45 + 0.35,
- 3*0.35,
- NO_TURN());
+ totem_x,
+ 0.48,
+ TURN_CCW(DEG2RAD(90)));
break;
case EV_MOTION_DONE:
+ FSM_TIMER(2000);
+ break;
case EV_TIMER:
- FSM_TRANSITION(place_figure_to_bonus_area);
+ FSM_TRANSITION(catch_totem_buillon_down);
break;
- case EV_START:
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ default:
break;
}
}
-
-FSM_STATE(place_figure_to_bonus_area)
+
+FSM_STATE(catch_totem_buillon_down)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot_trajectory_new(&tcSlow);
+ robot_trajectory_new(&tcSlow); // new trajectory
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.20));
+ totem_x,
+ totem_y - ROBOT_AXIS_TO_FRONT_M - 0.05,
+ ARRIVE_FROM(DEG2RAD(90), 0.10));
break;
- case EV_START:
- case EV_TIMER:
- FSM_TRANSITION(leave_bonus_figure);
- break;
- case EV_RETURN:
case EV_MOTION_DONE:
- act_jaws(OPEN);
FSM_TIMER(2000);
break;
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
+ case EV_TIMER:
+ FSM_TRANSITION(leave_totem_down);
+ default:
break;
}
}
-FSM_STATE(leave_bonus_figure)
+FSM_STATE(leave_totem_down)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot_trajectory_new_backward(&tcFast);
+ robot_trajectory_new_backward(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- 0.45 + 0.7 + 0.175,
- 0.7,
+ totem_x,
+ 0.48,
NO_TURN());
break;
- case EV_START:
- case EV_TIMER:
- break;
- case EV_RETURN:
case EV_MOTION_DONE:
- act_jaws(CLOSE);
- SUBFSM_RET(NULL);
- break;
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ FSM_TRANSITION(place_buillon_home);
+ default:
break;
}
}
-/** pick fourth green figure from green area */
-FSM_STATE(approach_fourth_green_figure)
+FSM_STATE(place_buillon_home)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot.use_left_bumper = false;
- robot.use_right_bumper = false;
- robot.use_back_bumpers = true;
-
- robot_trajectory_new(&tcFast);
+ 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.35,
- 0.29 + 3*0.28,
- TURN(DEG2RAD(180)));
+ 0.4,
+ 1.0,
+ ARRIVE_FROM(DEG2RAD(180),0.10));
break;
case EV_MOTION_DONE:
- act_jaws(OPEN);
FSM_TIMER(2000);
break;
case EV_TIMER:
- FSM_TRANSITION(load_fourth_green_figure);
- break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ SUBFSM_RET(NULL);
+ default:
break;
}
}
-FSM_STATE(load_fourth_green_figure)
+FSM_STATE(approach_totem_up)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- 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 + 0.06,
- 0.29 + 3*0.28,
- ARRIVE_FROM(DEG2RAD(180), 0.20));
+ totem_x,
+ 1.52,
+ TURN_CW(DEG2RAD(270)));
break;
case EV_MOTION_DONE:
FSM_TIMER(2000);
- act_jaws(CATCH);
break;
case EV_TIMER:
- FSM_TRANSITION(go_out_fourth_green_figure);
+ FSM_TRANSITION(catch_totem_buillon_up);
break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
- // enables using side switches on bumpers
- //robot.use_left_switch = true;
- //robot.use_right_switch = true;
- //robot.ignore_hokuyo = false;
+ default:
break;
}
}
-FSM_STATE(go_out_fourth_green_figure)
+FSM_STATE(catch_totem_buillon_up)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot_trajectory_new_backward(&tcFast);
- robot_trajectory_add_point_trans(
- 0.45 + 0.2,
- 0.29 + 3*0.28);
+ robot_trajectory_new(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- 0.45 + 0.2,
- 4*0.35,
- NO_TURN());
+ totem_x,
+ totem_y + ROBOT_AXIS_TO_FRONT_M + 0.05,
+ ARRIVE_FROM(DEG2RAD(270), 0.10));
break;
case EV_MOTION_DONE:
+ FSM_TIMER(2000);
+ break;
case EV_TIMER:
- FSM_TRANSITION(place_figure_to_red_square);
+ FSM_TRANSITION(leave_totem_up);
break;
- case EV_START:
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ default:
break;
}
}
-FSM_STATE(place_figure_to_red_square)
+FSM_STATE(leave_totem_up)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot_trajectory_new(&tcSlow);
+ robot_trajectory_new_backward(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- 0.45 + 0.175,
- 0.7 + 0.175 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
- ARRIVE_FROM(DEG2RAD(-90), 0.10));
- break;
- case EV_START:
- case EV_TIMER:
- FSM_TRANSITION(leave_red_square_figure);
+ totem_x,
+ 1.52,
+ NO_TURN());
break;
- case EV_RETURN:
case EV_MOTION_DONE:
- act_jaws(OPEN);
- FSM_TIMER(2000);
- break;
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
+ SUBFSM_RET(NULL);
+ default:
break;
}
}
-FSM_STATE(leave_red_square_figure)
+/* State for odometry calibration */
+
+FSM_STATE(go_back_for_cal)
{
- switch(FSM_EVENT) {
+ double x1 = 0, y1 = 0;
+ char buffer [20];
+ switch (FSM_EVENT) {
case EV_ENTRY:
- FSM_TIMER(1500);
- 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.35,
- 0.7 + 0.7,
- NO_TURN());
- break;
- case EV_START:
- case EV_TIMER:
- act_jaws(CLOSE);
+ 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:
- //enable_my_square_walls(true);
+ 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;
}
}
\ No newline at end of file
projects / eurobot / public.git / blobdiff