robofsm: jerk faster, go slower through the diagonal, add rush_last_cms for corns

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index 431fd989f83a281291d52937d0ce9f153ae46317..f117a2148d2407e1cec25f7cb9f9e69186699d2f 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -356,9 +356,11 @@ FSM_STATE(sledge_down)
 
 FSM_STATE(to_cntainer_and_unld)
 {
+       TrajectoryConstraints tcNotSoFast = trajectoryConstraintsDefault;
+       tcNotSoFast.maxv = 0.5; tcNotSoFast.maxacc = 0.3; tcNotSoFast.maxomega = 2;
        switch(FSM_EVENT) {
                case EV_ENTRY:
-                       robot_goto_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05), &tcFast);
+                       robot_goto_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05), &tcNotSoFast);
                        break;
                case EV_MOTION_DONE:
                        FSM_TIMER(1000); // FIXME: test this
@@ -384,7 +386,7 @@ FSM_STATE(jerk)
        switch(FSM_EVENT) {
                case EV_ENTRY:
                        move_cnt = 0;
-                       robot_move_by(-0.05, NO_TURN(), &tcSlow);
+                       robot_move_by(-0.05, NO_TURN(), &tcJerk);
                        break;
                case EV_MOTION_DONE:
                        if (move_cnt == 0) {
@@ -501,7 +503,7 @@ FSM_STATE(rush_the_corn)
                        where_to_go = TURN_AROUND;
                        break;
                case EV_MOTION_DONE:
-                       FSM_TRANSITION(rush_corns_decider);
+                       FSM_TRANSITION(rush_last_cms);
                        break;
                case EV_START:
                case EV_TIMER:
@@ -515,6 +517,38 @@ FSM_STATE(rush_the_corn)
        }
 }
 
+FSM_STATE(rush_last_cms)
+{
+       static char move_cnt;
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot.ignore_hokuyo = true;
+                       robot.check_turn_safety = false;
+                       move_cnt = 0;
+                       robot_move_by(0.08, NO_TURN(), &tcSlow);
+                       break;
+               case EV_MOTION_DONE:
+                       if (move_cnt == 0) {
+                               robot_move_by(-0.08, NO_TURN(), &tcSlow);
+                       } else if (move_cnt > 0) {
+                               FSM_TRANSITION(rush_corns_decider);
+                       }
+                       move_cnt++;
+                       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:
+                       robot.ignore_hokuyo = false;
+                       robot.check_turn_safety = true;
+                       break;
+       }
+}
+
 // used to perform the maneuvre
 FSM_STATE(turn_around)
 {

diff --git a/src/robofsm/common-states.h b/src/robofsm/common-states.h
index 952a40bdbd37ce4ff9217a02f745721336d32f3a..4d683ad82b8870bbb8590b21a9bf37c780a35ca7 100644 (file)
--- a/src/robofsm/common-states.h
+++ b/src/robofsm/common-states.h
@@ -22,6 +22,7 @@ FSM_STATE_DECL(jerk);
 FSM_STATE_DECL(rush_corns_decider);
 FSM_STATE_DECL(approach_next_corn);
 FSM_STATE_DECL(rush_the_corn);
+FSM_STATE_DECL(rush_last_cms);
 FSM_STATE_DECL(turn_around);
 FSM_STATE_DECL(approach_the_slope);
 

diff --git a/src/robofsm/competition.cc b/src/robofsm/competition.cc
index bc9279b511d08d851272fe478cd65aad1658be3b..8edd27cc54ca879bf2dfd4c75d324559e4bc7977 100644 (file)
--- a/src/robofsm/competition.cc
+++ b/src/robofsm/competition.cc
@@ -52,8 +52,8 @@ int main()
        //robot.fsm.motion.transition_callback = move_trans_callback;
 
        tcJerk = trajectoryConstraintsDefault;
-       tcJerk.maxv = 1;
-       tcJerk.maxacc = 1.5;
+       tcJerk.maxv = 1.5;
+       tcJerk.maxacc = 5;
        tcJerk.maxomega = 2;
        tcFast = trajectoryConstraintsDefault;
        tcFast.maxv = 1;