homologation: working version

diff --git a/src/robofsm/homologation.cc b/src/robofsm/homologation.cc
index 1c922defa2f10c02b2116bb518b6981de9dd5f69..ec5a8e1482d5e6f5fd27e91cf23b3cac4fa99fb5 100644 (file)
--- a/src/robofsm/homologation.cc
+++ b/src/robofsm/homologation.cc
@@ -39,7 +39,6 @@ struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
  ************************************************************************/
 
 static const Point containerPosition(PLAYGROUND_WIDTH_M - 0.25, 0.02); // blue container Position
-static int wait_time;  // variable for waiting in miliseconds when you need
 
 // returns pointer to next real (non-fake) corn which was not yet collected
 // TODO: note: use this for "short_time_to_end: situation only, otherwise
@@ -48,7 +47,7 @@ struct corn * choose_next_corn()
 {
        Point cornPosition;
 
-       double minDistance = 2 * PLAYGROUND_HEIGHT_M; // "infinity"     
+       double minDistance = 2 * PLAYGROUND_HEIGHT_M; // "infinity"
        struct corn *minCorn = NULL, *corn;
        // robot.corns->corns[NUM_OF_FAKE_CORNS] is first non-fake corn in the robot.corns structure
        for(corn = &robot.corns->corns[NUM_OF_FAKE_CORNS]; corn < &robot.corns->corns[robot.corns->corns_count]; corn++) {
@@ -128,7 +127,7 @@ FSM_STATE(init)
                        tcFast.maxomega = 2;
                        tcSlow = trajectoryConstraintsDefault;
                        tcSlow.maxv = 0.3;
-                       tcSlow.maxacc = 0.3;
+                       tcSlow.maxacc = 0.1;
                        tcVerySlow = trajectoryConstraintsDefault;
                        tcVerySlow.maxv = 0.05;
                        tcVerySlow.maxacc = 0.05;
@@ -196,7 +195,7 @@ FSM_STATE(climb_the_slope)
                        /* position for collecting oranges*/
                        robot_trajectory_add_final_point_trans(
                                SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M + 0.04,
-                               PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) + 0.02,
+                               PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) /*+0.02*/,
                                NO_TURN());
                        break;
                case EV_MOTION_DONE:
@@ -205,6 +204,7 @@ FSM_STATE(climb_the_slope)
                case EV_START:
                case EV_TIMER:
                        FSM_TRANSITION(sledge_down);
+                       break;
                case EV_RETURN:
                case EV_ACTION_DONE:
                case EV_ACTION_ERROR:
@@ -258,10 +258,16 @@ FSM_STATE(to_container_diag)
                        break;
                case EV_START:
                case EV_TIMER:
+                       // FIXME experiment tuning 
+                       //FSM_TRANSITION(experiment);
+                       break;
                case EV_RETURN:
                case EV_ACTION_DONE:
                case EV_ACTION_ERROR:
                case EV_MOTION_DONE:
+                       // FIXME experiment tuning
+                       //FSM_TIMER(1000);
+                       //break;
                case EV_GOAL_NOT_REACHABLE:
                        DBG_PRINT_EVENT("unhandled event");
                case EV_EXIT:
@@ -289,10 +295,16 @@ FSM_STATE(to_container_ortho)
                        break;
                case EV_START:
                case EV_TIMER:
+                       // FIXME expetiment tuning
+                       //FSM_TRANSITION(experiment);
+                       //break;
                case EV_RETURN:
                case EV_ACTION_DONE:
                case EV_ACTION_ERROR:
                case EV_MOTION_DONE:
+                        // FIXME expetiment tuning
+                        //FSM_TIMER(1000);
+                        //break;
                case EV_GOAL_NOT_REACHABLE:
                        DBG_PRINT_EVENT("unhandled event");
                case EV_EXIT:
@@ -307,7 +319,7 @@ FSM_STATE(experiment)
                case EV_ENTRY: {
                                double x, y, phi;
                                robot_get_est_pos(&x, &y, &phi);
-                               printf("experiment: puck cnt: %d, est pos %.3f, %.3f, %.3f\n",
+                               printf("experiment: corn cnt: %d, est pos %.3f, %.3f, %.3f\n",
                                        cnt, x, y, phi);
                                struct corn * corn = choose_next_corn();
                                Pos *p = get_corn_approach_position(corn /*&robot.corns->corns[cnt++]*/);