robofsm: Competition strategy tuning.

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index cd896d24ef986f1255acbb9799d4a1a92360bc1a..3869b3a76715bd650a72cdfbb278e5e10bfdaf56 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -42,11 +42,11 @@ void set_initial_position()
 
 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);
+       act_lift(0, 0, 1);
        // unset the homing request
        //act_lift(0, 0, 0);
 }
@@ -112,9 +112,9 @@ FSM_STATE(approach_central_buillon)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
-                       robot_trajectory_new(&tcSlow); // new trajectory
+                       robot_trajectory_new(&tcVeryFast); // new trajectory
                        robot_trajectory_add_point_trans(
-                               0.5,
+                               0.6,
                                PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
                        robot_trajectory_add_point_trans(
                                0.64,
@@ -132,7 +132,8 @@ FSM_STATE(approach_central_buillon)
 //                             NO_TURN());
                        break;
                case EV_MOTION_DONE:
-                       FSM_TIMER(2000);
+                        act_jaws(OPEN);
+                       FSM_TIMER(1000);
                        break;
                case EV_TIMER:
                        FSM_TRANSITION(catch_central_buillon);
@@ -146,14 +147,15 @@ FSM_STATE(catch_central_buillon)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
-                       robot_trajectory_new(&tcSlow); // new trajectory
+                       robot_trajectory_new(&tcFast); // new trajectory
                        robot_trajectory_add_final_point_trans(
                                1.3,
                                0.58,
                                NO_TURN());
                        break;
                case EV_MOTION_DONE:
-                       FSM_TIMER(2000);
+                        act_jaws(CATCH);
+                       FSM_TIMER(500);
                        break;
                case EV_TIMER:
                        FSM_TRANSITION(leave_central_buillon);
@@ -167,14 +169,13 @@ FSM_STATE(leave_central_buillon)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
-                       robot_trajectory_new_backward(&tcSlow); // new trajectory
+                       robot_trajectory_new_backward(&tcFast); // 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:
@@ -186,13 +187,14 @@ FSM_STATE(push_bottle_bw)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
+                        robot.use_back_bumpers = false;
                        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,
+                               ROBOT_AXIS_TO_BACK_M + 0.01,
                                ARRIVE_FROM(DEG2RAD(270), 0.10));
                        break;
                case EV_MOTION_DONE:
@@ -207,18 +209,19 @@ FSM_STATE(return_home)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
-                       robot_trajectory_new(&tcSlow); // new trajectory
+                       robot_trajectory_new(&tcFast); // new trajectory
                        robot_trajectory_add_point_trans(
                                0.64 + 0.08,
-                               0.7);
+                               0.8);
                        robot_trajectory_add_final_point_trans(
-                               0.4,
-                               1.0,
-                               ARRIVE_FROM(DEG2RAD(180), 0.10));
+                               0.55,
+                               1.1,
+                               ARRIVE_FROM(DEG2RAD(180), 0.01));
                        break;
                case EV_MOTION_DONE:
-                       enable_bumpers(true);
-                       FSM_TIMER(2000);
+                       robot.use_back_bumpers = true;
+                        act_jaws(OPEN);
+                       FSM_TIMER(1000);
                        break;
                case EV_TIMER:
                        SUBFSM_RET(NULL);
@@ -232,13 +235,11 @@ FSM_STATE(leave_home)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
-                       robot_trajectory_new_backward(&tcSlow); // new trajectory
-                       robot_trajectory_add_final_point_trans(
-                               0.6,
-                               1.0,
-                               NO_TURN());
+                       //robot_trajectory_new_backward(&tcSlow); // new trajectory
+                       robot_move_by(-0.18, NO_TURN(), &tcSlow);
                        break;
                case EV_MOTION_DONE:
+                        act_jaws(CLOSE);
                        FSM_TRANSITION(leave_home_for_totem);
                        break;
                default:
@@ -250,17 +251,17 @@ FSM_STATE(leave_home_for_totem)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
-                       robot_trajectory_new(&tcSlow); // new trajectory
+                       robot_trajectory_new(&tcFast); // new trajectory
                        if(up) {
                                robot_trajectory_add_final_point_trans(
-                                       0.64,
+                                       0.7,
                                        1.3,
                                        NO_TURN());
                        }
                        else {
                                robot_trajectory_add_final_point_trans(
-                                       0.64,
                                        0.7,
+                                       0.9,
                                        NO_TURN());
                        }
                        break;
@@ -277,14 +278,16 @@ FSM_STATE(approach_totem_down)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
-                       robot_trajectory_new(&tcSlow); // new trajectory
+                       robot_trajectory_new(&tcFast); // new trajectory
                        robot_trajectory_add_final_point_trans(
                                totem_x,
-                               0.48,
+                               0.4,
                                TURN_CCW(DEG2RAD(90)));
                        break;
                case EV_MOTION_DONE:
-                       FSM_TIMER(2000);
+                        act_lift(UP, 0, 0);
+                        act_jaws(OPEN);
+                       FSM_TIMER(1000);
                        break;
                case EV_TIMER:
                        FSM_TRANSITION(catch_totem_buillon_down);
@@ -305,7 +308,8 @@ FSM_STATE(catch_totem_buillon_down)
                                ARRIVE_FROM(DEG2RAD(90), 0.10));
                        break;
                case EV_MOTION_DONE:
-                       FSM_TIMER(2000);
+                        act_jaws(CATCH);
+                       FSM_TIMER(500);
                        break;
                case EV_TIMER:
                        FSM_TRANSITION(leave_totem_down);
@@ -321,11 +325,17 @@ FSM_STATE(leave_totem_down)
                        robot_trajectory_new_backward(&tcSlow); // new trajectory
                        robot_trajectory_add_final_point_trans(
                                totem_x,
-                               0.48,
+                               0.42,
                                NO_TURN());
                        break;
                case EV_MOTION_DONE:
+                        act_jaws(OPEN);
+                        act_lift(DOWN, 0, 0);
+                        FSM_TIMER(2000);
+                        break;
+                case EV_TIMER:
                        FSM_TRANSITION(place_buillon_home);
+                        break;
                default:
                        break;
        }
@@ -335,6 +345,7 @@ FSM_STATE(place_buillon_home)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
+                        act_jaws(CATCH);
                        robot_trajectory_new(&tcSlow); // new trajectory
                        if(up) {
                                robot_trajectory_add_point_trans(
@@ -350,12 +361,13 @@ FSM_STATE(place_buillon_home)
                                        0.8);
                        }
                        robot_trajectory_add_final_point_trans(
-                               0.4,
-                               1.0,
-                               ARRIVE_FROM(DEG2RAD(180),0.10));
+                               0.55,
+                               1.1,
+                               ARRIVE_FROM(DEG2RAD(180),0.01));
                        break;
                case EV_MOTION_DONE:
-                       FSM_TIMER(2000);
+                        act_jaws(OPEN);
+                       FSM_TIMER(1000);
                        break;
                case EV_TIMER:
                        SUBFSM_RET(NULL);
@@ -368,13 +380,15 @@ FSM_STATE(approach_totem_up)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
-                       robot_trajectory_new(&tcSlow); // new trajectory
+                       robot_trajectory_new(&tcFast); // new trajectory
                        robot_trajectory_add_final_point_trans(
                                totem_x,
-                               1.52,
+                               1.6,
                                TURN_CW(DEG2RAD(270)));
                        break;
                case EV_MOTION_DONE:
+                        act_jaws(OPEN);
+                        act_lift(UP, 0, 0);
                        FSM_TIMER(2000);
                        break;
                case EV_TIMER:
@@ -396,7 +410,8 @@ FSM_STATE(catch_totem_buillon_up)
                                ARRIVE_FROM(DEG2RAD(270), 0.10));
                        break;
                case EV_MOTION_DONE:
-                       FSM_TIMER(2000);
+                        act_jaws(CATCH);
+                       FSM_TIMER(500);
                        break;
                case EV_TIMER:
                        FSM_TRANSITION(leave_totem_up);
@@ -413,12 +428,49 @@ FSM_STATE(leave_totem_up)
                        robot_trajectory_new_backward(&tcSlow); // new trajectory
                        robot_trajectory_add_final_point_trans(
                                totem_x,
-                               1.52,
+                               1.6,
                                NO_TURN());
                        break;
                case EV_MOTION_DONE:
+                        act_jaws(OPEN);
+                        act_lift(DOWN, 0, 0);
                        SUBFSM_RET(NULL);
                default:
                        break;
        }
+}
+
+
+FSM_STATE(push_second_bottle_fw)
+{
+        switch(FSM_EVENT) {
+                case EV_ENTRY:
+                        robot_trajectory_new(&tcFast); // new trajectory
+//                        robot_trajectory_add_point_trans(0.7, 0.5);
+                        robot_goto_notrans(
+                                1.85,
+                                ROBOT_AXIS_TO_FRONT_M,
+                                ARRIVE_FROM(DEG2RAD(270), 0.15), &tcFast);
+                        break;
+                case EV_MOTION_DONE:
+                        SUBFSM_RET(NULL);
+                default:
+                        break;
+        }
+}
+
+FSM_STATE(go_back_from_home)
+{
+        switch(FSM_EVENT) {
+                case EV_ENTRY:
+                        act_jaws(OPEN);
+                        robot_move_by(-0.2, NO_TURN(), &tcSlow);
+                        break;
+                case EV_MOTION_DONE:
+                        act_jaws(CLOSE);
+                        FSM_TRANSITION(push_second_bottle_fw);
+                        break;
+                default:
+                        break;
+        }
 }
\ No newline at end of file

diff --git a/src/robofsm/common-states.h b/src/robofsm/common-states.h
index 2011e6c42fee9d97b801e26147cb535a7a9baeb9..466c16c7af9b329ed4fcc177bdc62cc5baab0867 100644 (file)
--- a/src/robofsm/common-states.h
+++ b/src/robofsm/common-states.h
@@ -10,7 +10,7 @@ extern struct TrajectoryConstraints tcFast, tcVeryFast, tcSlow, tcVerySlow;
 extern double totem_x, totem_y;
 extern bool up;
 /* strategy FSM */
-FSM_STATE_DECL(get_central_buillon_first);
+FSM_STATE_DECL(central_buillon_wait_for_start);
 FSM_STATE_DECL(homologation_wait_for_start);
 FSM_STATE_DECL(calibrate);
 
@@ -52,12 +52,19 @@ FSM_STATE_DECL(push_second_bottle);
 FSM_STATE_DECL(homologation_approach_buillon);
 /* Pushing the bottle */
 FSM_STATE_DECL(homologation_push_bottle);
+FSM_STATE_DECL(push_second_bottle_fw);
+FSM_STATE_DECL(go_back_from_home);
+
+
 void start_entry();
 void start_timer();
 void start_go();
 void start_exit();
 bool read_sharp();
 
+void robot_calibrate_odometry();
+
+
 
 
 #endif

diff --git a/src/robofsm/competition2012.cc b/src/robofsm/competition2012.cc
index de7c2f4759ebc6ab25fa27fa01798db17742314c..8cede3e99e9432b5703608c6578e17a861b732ae 100644 (file)
--- a/src/robofsm/competition2012.cc
+++ b/src/robofsm/competition2012.cc
@@ -48,17 +48,17 @@ int main()
        robot.fsm.motion.debug_states = 1;
        //robot.fsm.act.debug_states = 1;
 
-       robot.fsm.main.state = &fsm_state_main_get_central_buillon_first;
+       robot.fsm.main.state = &fsm_state_main_central_buillon_wait_for_start;
        //robot.fsm.main.transition_callback = trans_callback;
        //robot.fsm.motion.transition_callback = move_trans_callback;
 
        tcVeryFast = trajectoryConstraintsDefault;
        tcVeryFast.maxv = 1;
        tcVeryFast.maxacc = 0.6;
-       tcVeryFast.maxomega = 2;
+       tcVeryFast.maxomega = 1;
        tcFast = trajectoryConstraintsDefault;
        tcFast.maxv = 0.6;
-       tcFast.maxacc = 0.2;
+       tcFast.maxacc = 0.4;
        tcFast.maxomega = 1;
        tcFast.maxe = 0.02;
        tcSlow = trajectoryConstraintsDefault;

diff --git a/src/robofsm/strategy_get_central_buillon.cc b/src/robofsm/strategy_get_central_buillon.cc
index ddf9315b0b65b62bbd7eedf45d5f7051bc4f78b1..40da20e0ab5e4c321e7efbe93cf6cd24e0141655 100644 (file)
--- a/src/robofsm/strategy_get_central_buillon.cc
+++ b/src/robofsm/strategy_get_central_buillon.cc
@@ -3,11 +3,11 @@
 #include <ul_log.h>
 
 
-UL_LOG_CUST(ulogd_strategy_get_central_buillon); /* Log domain name = ulogd + name of the file */
+UL_LOG_CUST(ulogd_strategy_central_buillon_wait_for_start); /* Log domain name = ulogd + name of the file */
 static FSM_STATE_DECL(pick_central_buillon);
 
 
-FSM_STATE(get_central_buillon_first)
+FSM_STATE(central_buillon_wait_for_start)
 {
        switch (FSM_EVENT) {
                case EV_ENTRY:
@@ -41,6 +41,7 @@ FSM_STATE(get_central_buillon_first)
 FSM_STATE_DECL(bottle_bw);
 FSM_STATE_DECL(pick_buillon_from_totem1);
 FSM_STATE_DECL(pick_buillon_from_totem2);
+FSM_STATE_DECL(push_second_bottle);
 
 
 FSM_STATE(pick_central_buillon)
@@ -81,7 +82,7 @@ FSM_STATE(pick_buillon_from_totem1)
                        SUBFSM_TRANSITION(leave_home,NULL);
                        break;
                case EV_RETURN:
-                       FSM_TRANSITION(pick_buillon_from_totem2);
+                       FSM_TRANSITION(push_second_bottle);
                        break;
                default:
                        break;
@@ -100,4 +101,15 @@ FSM_STATE(pick_buillon_from_totem2)
                default:
                        break;
        }
+}
+
+FSM_STATE(push_second_bottle)
+{
+        switch (FSM_EVENT) {
+                case EV_ENTRY:
+                        SUBFSM_TRANSITION(go_back_from_home, NULL);
+                        break;
+                case EV_RETURN:
+                        break;
+        }
 }
\ No newline at end of file

diff --git a/src/robofsm/strategy_homologation.cc b/src/robofsm/strategy_homologation.cc
index 7d2a9ade0994a4cd996f84d94ebe0a6aa6bbcb7c..e7b10bea4139c39086a5cc3aa7ecafebfc2291ac 100644 (file)
--- a/src/robofsm/strategy_homologation.cc
+++ b/src/robofsm/strategy_homologation.cc
@@ -57,6 +57,7 @@ FSM_STATE(homologation_approach_buillon)
 {
         switch(FSM_EVENT) {
                 case EV_ENTRY:
+                        //robot.use_back_bumpers = false;
                         robot_trajectory_new(&tcSlow); // new trajectory
                         robot_trajectory_add_point_trans(
                                 0.65,
@@ -71,6 +72,7 @@ FSM_STATE(homologation_approach_buillon)
                         break;
                 case EV_MOTION_DONE:
                 case EV_TIMER:
+                        //robot.use_back_bumpers = true;
                         FSM_TRANSITION(homologation_push_bottle);
                         break;
                 default:
@@ -88,7 +90,7 @@ FSM_STATE(homologation_push_bottle)
                                 0.7);
                         robot_trajectory_add_final_point_trans(
                                 0.64 + 0.08,
-                                ROBOT_AXIS_TO_FRONT_M + 0.05,
+                                ROBOT_AXIS_TO_FRONT_M + 0.08,
                                 ARRIVE_FROM(DEG2RAD(270), 0.10));
                         break;
                 case EV_MOTION_DONE:

diff --git a/src/robofsm/strategy_odo_calibration.cc b/src/robofsm/strategy_odo_calibration.cc
index b78750016ab99ae56e4a9f092a36516defaf8a58..996563e5774107407573c30199446cc8912bb6a2 100644 (file)
--- a/src/robofsm/strategy_odo_calibration.cc
+++ b/src/robofsm/strategy_odo_calibration.cc
@@ -70,7 +70,6 @@ FSM_STATE(calibrate)
 //#endif
                 case EV_START:
                         start_go();
-                        robot.obstacle_avoidance_enabled = false;
                         robot.use_back_bumpers = false;
                         robot.odo_cal_a = 1;
                         robot.odo_cal_b = 1;
@@ -84,7 +83,7 @@ FSM_STATE(calibrate)
                         start_exit();
                         break;
                 case EV_SWITCH_STRATEGY:
-                        FSM_TRANSITION(get_central_buillon_first);
+                        FSM_TRANSITION(central_buillon_wait_for_start);
                         break;
                 default:;
         }
@@ -102,7 +101,7 @@ FSM_STATE(go_back_for_cal)
                                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
                                                 0);
                         robot_move_by(-1.1, NO_TURN(), &tcVerySlow);
-                        FSM_TIMER(50);
+                        FSM_TIMER(2000);
                         break;
                 case EV_MOTION_DONE:
                         ROBOT_LOCK(est_pos_odo);
@@ -116,7 +115,7 @@ FSM_STATE(go_back_for_cal)
                                 printf("Left%f\n", x1);
                                 printf("Right%f\n", y1);
                                 FILE * file;
-                                file = fopen ("odometry_cal_data","a+");
+                                file = fopen ("odometry_cal_data","w");
                                 sprintf(buffer, "%4.4f", -1/x1);
                                 fputs (buffer,file);
                                 fputs (" ", file);
@@ -125,13 +124,14 @@ FSM_STATE(go_back_for_cal)
                                 fputs ("\n", file);
                                 fclose(file);
                         } 
-                        FSM_TRANSITION(get_central_buillon_first);
+                        FSM_TRANSITION(central_buillon_wait_for_start);
                         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;
+                                //robot_stop();
                                 FSM_SIGNAL(MOTION,EV_MOVE_STOP, NULL);
                                 FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
                         } else {