#include <robodim.h>
#include <error.h>
#include "actuators.h"
+#include <sharp.h>
#include <trgen.h>
#include "match-timing.h"
#include <stdbool.h>
{
robot_set_est_pos_trans(ROBOT_START_X_M,
ROBOT_START_Y_M,
- 0);
+ DEG2RAD(ROBOT_START_ANGLE_DEG));
}
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;
}
+
+
/************************************************************************
* Trajectory constraints used; They are initialized in the main() function in competition.cc
************************************************************************/
-struct TrajectoryConstraints tcJerk, tcFast, tcSlow, tcVerySlow;
+struct TrajectoryConstraints tcFast, tcVeryFast, tcSlow, tcVerySlow;
+double totem_x;
+double totem_y;
+bool up;
-/* assures that the robot is near the slope rim; if outside our starting area, approach the slope first */
-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.35,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2));
+ 0.5,
+ PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
robot_trajectory_add_point_trans(
- 0.45 + 0.35 + 0.175,
- PLAYGROUND_HEIGHT_M - 0.35);
+ 0.64,
+ 1.2);
+ robot_trajectory_add_point_trans(
+ 0.64,
+ 0.7);
robot_trajectory_add_final_point_trans(
- 0.45 + 0.35 + 0.175,
- 0.7 + 0.175,
+ 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;
+ }
+}
+
+FSM_STATE(catch_central_buillon)
+{
+ switch(FSM_EVENT) {
+ 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:
- SUBFSM_RET(NULL);
+ FSM_TIMER(2000);
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:
+ FSM_TRANSITION(leave_central_buillon);
+ break;
+ default:
break;
}
}
-FSM_STATE(approach_second_green_figure)
+FSM_STATE(leave_central_buillon)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.use_back_bumpers = true;
+ 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(push_bottle_bw)
+{
+ switch(FSM_EVENT) {
+ 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_MOTION_DONE:
+ FSM_TRANSITION(return_home);
+ break;
+ default:
+ break;
+ }
+}
- robot_trajectory_new(&tcFast);
+FSM_STATE(return_home)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new(&tcSlow); // new trajectory
+ robot_trajectory_add_point_trans(
+ 0.64 + 0.08,
+ 0.7);
robot_trajectory_add_final_point_trans(
- 0.45 + 0.35,
- 0.29 + 0.28,
+ 0.4,
+ 1.0,
ARRIVE_FROM(DEG2RAD(180), 0.10));
break;
case EV_MOTION_DONE:
- act_jaws(OPEN);
+ enable_bumpers(true);
FSM_TIMER(2000);
break;
case EV_TIMER:
- FSM_TRANSITION(load_second_green_figure);
+ SUBFSM_RET(NULL);
break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ default:
break;
}
}
-FSM_STATE(load_second_green_figure)
+FSM_STATE(leave_home)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot_trajectory_new(&tcSlow);
+ robot_trajectory_new_backward(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- ROBOT_AXIS_TO_FIGURE_CENTER_M + 0.05,
- 0.29 + 0.28,
+ 0.6,
+ 1.0,
NO_TURN());
break;
+ case EV_MOTION_DONE:
+ FSM_TRANSITION(leave_home_for_totem);
+ break;
+ default:
+ break;
+ }
+}
+
+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(
+ 0.64,
+ 1.3,
+ NO_TURN());
+ }
+ else {
+ robot_trajectory_add_final_point_trans(
+ 0.64,
+ 0.7,
+ NO_TURN());
+ }
+ break;
+ case EV_MOTION_DONE:
+ if(up) FSM_TRANSITION(approach_totem_up);
+ else FSM_TRANSITION(approach_totem_down);
+ break;
+ default:
+ break;
+ }
+}
+
+FSM_STATE(approach_totem_down)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ 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(2000);
- act_jaws(CATCH);
break;
case EV_TIMER:
- FSM_TRANSITION(go_out_second_green_figure);
+ FSM_TRANSITION(catch_totem_buillon_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_second_green_figure)
+
+FSM_STATE(catch_totem_buillon_down)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot_trajectory_new_backward(&tcFast);
- robot_trajectory_add_final_point_trans(0.45 + 0.35, 0.7, NO_TURN());
+ 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_TIMER:
- FSM_TRANSITION(place_figure_to_protected_block);
+ FSM_TRANSITION(leave_totem_down);
+ default:
break;
- case EV_START:
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_EXIT:
+ }
+}
+
+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:
+ FSM_TRANSITION(place_buillon_home);
+ default:
break;
}
}
-FSM_STATE(place_figure_to_protected_block)
+FSM_STATE(place_buillon_home)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot_trajectory_new(&tcSlow);
+ 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.2,
- 0.12 + 0.08 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
- ARRIVE_FROM(DEG2RAD(-90), 0.20));
+ 0.4,
+ 1.0,
+ ARRIVE_FROM(DEG2RAD(180),0.10));
+ break;
+ case EV_MOTION_DONE:
+ FSM_TIMER(2000);
break;
- case EV_START:
case EV_TIMER:
- FSM_TRANSITION(leave_protected_figure);
+ SUBFSM_RET(NULL);
+ default:
+ break;
+ }
+}
+
+FSM_STATE(approach_totem_up)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_trajectory_new(&tcSlow); // new trajectory
+ robot_trajectory_add_final_point_trans(
+ totem_x,
+ 1.52,
+ TURN_CW(DEG2RAD(270)));
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(catch_totem_buillon_up);
+ break;
+ default:
break;
}
}
-FSM_STATE(leave_protected_figure)
+FSM_STATE(catch_totem_buillon_up)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- FSM_TIMER(2000);
- robot_trajectory_new_backward(&tcFast);
- robot_trajectory_add_point_trans(
- 0.45 + 0.175,
- 0.7);
+ robot_trajectory_new(&tcSlow); // new trajectory
robot_trajectory_add_final_point_trans(
- 0.45 + 0.35,
- 0.35 + 0.35 + 0.175,
- TURN_CW(DEG2RAD(0)));
+ 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_START:
case EV_TIMER:
- act_jaws(CLOSE);
- //FSM_TRANSITION(load_second_figure);
+ 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_RETURN:
case EV_MOTION_DONE:
- //FSM_TRANSITION(load_second_figure);
+ 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);
+ 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 ("\n", file);
+ sprintf(buffer, "%4.4f", -1/y1);
+ fputs (buffer,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