homologation: experiment with corns rushing (hrnuti)

diff --git a/src/robofsm/eb2010misc.cc b/src/robofsm/eb2010misc.cc
index 66545ba50a1edb68d105e328551551ed63196b36..62e5a6abc2854f844f162ebe75099ac881d06f71 100644 (file)
--- a/src/robofsm/eb2010misc.cc
+++ b/src/robofsm/eb2010misc.cc
@@ -9,7 +9,6 @@
  * Functions to manipulate map, compute distances, choose corns etc.
  ************************************************************************/
 
-
 // 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
 // it is better to try to rush more corns at once
@@ -17,21 +16,24 @@ struct corn * choose_next_corn()
 {
        Point cornPosition;
 
-       double minDistance = 2 * PLAYGROUND_HEIGHT_M; // "infinity"
+       double maxDistance = 0; // 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++) {
                cornPosition.x = corn->position.x;
                cornPosition.y = corn->position.y;
                double distance = cornPosition.distanceTo(containerPosition);
+               printf("maxDistance = %.3f; new corn dist = %.3f, it x position = %.3f; %s\n",
+                       maxDistance, distance, corn->position.x, (corn->was_collected?"W":"N"));
                
-               if (distance < minDistance && corn->was_collected == false) {
-                       minDistance = distance;
+               if (distance > maxDistance && corn->position.x > 2.2 && corn->was_collected == false) {
+                       maxDistance = distance;
                        minCorn = corn;
+                       printf("\tchoose it\n");
                }
        }
 
-       if (minCorn) printf("\tmin distance was: %.3f  ", minDistance);
+       if (minCorn) printf("\tmin distance was: %.3f  ", maxDistance);
        return minCorn;
 }
 
@@ -56,8 +58,8 @@ Pos * get_corn_approach_position(struct corn *corn)
        Point cornPosition(corn->position.x, corn->position.y);
        double a = approxContainerPosition.angleTo(cornPosition);
 
-       p->x = cornPosition.x + cos(a)*(CORN_NEIGHBOURHOOD_RADIUS_M + 0.1);
-       p->y = cornPosition.y + sin(a)*(CORN_NEIGHBOURHOOD_RADIUS_M + 0.1);
+       p->x = cornPosition.x + cos(a)*(CORN_NEIGHBOURHOOD_RADIUS_M + 0.07);
+       p->y = cornPosition.y + sin(a)*(CORN_NEIGHBOURHOOD_RADIUS_M + 0.07);
        p->phi = a + M_PI;
 
        return p;

diff --git a/src/robofsm/homologation.cc b/src/robofsm/homologation.cc
index 7cca88173c23113c5ec9ab93d520ba1581d36254..b852fe5f54de85a88190dbbcc0f53f2b2957a364 100644 (file)
--- a/src/robofsm/homologation.cc
+++ b/src/robofsm/homologation.cc
@@ -221,10 +221,14 @@ FSM_STATE(to_container_diag)
                        robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05));
                        break;
                case EV_MOTION_DONE:
+                       FSM_TIMER(10000);
                        act_vidle(VIDLE_DOWN);
                        break;
-               case EV_START:
                case EV_TIMER:
+                       //act_vidle(VIDLE_UP);
+                       //FSM_TRANSITION(experiment);
+                       break;
+               case EV_START:
                case EV_RETURN:
                case EV_ACTION_DONE:
                case EV_ACTION_ERROR:
@@ -268,26 +272,43 @@ FSM_STATE(to_container_ortho)
        }
 }
 
+enum where_to_go {
+       CORN,
+       CONTAINER
+};
+
 static int cnt = 0;
 FSM_STATE(experiment)
 {
+       static enum where_to_go where_to_go;
+       static struct corn *corn_to_get;
        switch(FSM_EVENT) {
                case EV_ENTRY: {
                                double x, y, phi;
                                robot_get_est_pos(&x, &y, &phi);
-                               printf("experiment: corn cnt: %d, est pos %.3f, %.3f, %.3f\n",
+                               printf("experiment: puck 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++]*/);
-                               corn->was_collected = true;
-                               //robot_trajectory_new(&tcFast);
-                               //robot_trajectory_add_final_point_trans(robot_trajectory_add_final_point_trans(p->x, p->y, TURN(p->phi));
-                               robot_goto_trans(p->x, p->y, TURN(p->phi), &tcFast);
-                               delete(p);
+                               corn_to_get = choose_next_corn();
+                               if (corn_to_get) {
+                                       Pos *p = get_corn_approach_position(corn_to_get);
+                                       corn_to_get->was_collected = true;
+                                       //robot_trajectory_new(&tcFast);
+                                       //robot_trajectory_add_final_point_trans(robot_trajectory_add_final_point_trans(p->x, p->y, TURN(p->phi));
+                                       robot_goto_trans(p->x, p->y, TURN(p->phi), &tcFast);
+                                       delete(p);
+                               }
+                               where_to_go = CONTAINER;
                                break;
                        }
                case EV_MOTION_DONE:
-                       FSM_TRANSITION(experiment);
+                       cnt++;
+                       if (where_to_go == CORN) {
+                               FSM_TRANSITION(experiment);
+                       } else {
+                               remove_wall_around_corn(corn_to_get);
+                               robot_goto_trans(PLAYGROUND_WIDTH_M - 0.4, 0.15, ARRIVE_FROM(DEG2RAD(-90), 0.02), &tcFast);
+                               where_to_go = CORN;
+                       }
                        break;
                case EV_START:
                case EV_TIMER:
@@ -301,8 +322,6 @@ FSM_STATE(experiment)
        }
 }
 
-
-
 /************************************************************************
  * MAIN FUNCTION
  ************************************************************************/