add corns field (type struct corns_group *) to robot.h

- add choose_next_corn() function to homologation.cc:
  this method returns next corn that has not been tried
  to collect.

diff --git a/src/robofsm/homologation.cc b/src/robofsm/homologation.cc
index b9bcfffdfcc2808a106b8b56a72b031c95d3606d..ae3f9c1be139e9071b2e18d86c7fb64ec9a9fbe5 100644 (file)
--- a/src/robofsm/homologation.cc
+++ b/src/robofsm/homologation.cc
@@ -25,6 +25,8 @@
 #include <string.h>
 #include <robodim.h>
 #include <error.h>
+#include "corns_configs.h"
+#include <trgen.h>
 
 /************************************************************************
  * Trajectory constraints used, are initialized in the init state
@@ -32,6 +34,41 @@
 
 struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
 
+/************************************************************************
+ * Functions to manipulate map etc.
+ ************************************************************************/
+
+static const Point containerPosition(PLAYGROUND_WIDTH_M - 0.25, 0.02); // blue container Position
+
+// returns pointer to next real (non-fake) corn which was not yet collected
+struct corn * choose_next_corn()
+{
+       Point cornPosition;
+
+       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++) {
+               cornPosition.x = corn->position.x;
+               cornPosition.y = corn->position.y;
+               double distance = cornPosition.distanceTo(containerPosition);
+               
+               if (distance < minDistance && corn->was_collected == false) {
+                       minDistance = distance;
+                       minCorn = corn;
+               }
+       }
+
+       if (minCorn) printf("\tmin distance was: %.3f  ", minDistance);
+       return minCorn;
+}
+
+Pos * get_corn_approach_position(struct corn *corn)
+{
+       
+}
+
+
 /************************************************************************
  * FSM STATES DECLARATION
  ************************************************************************/
@@ -105,6 +142,7 @@ FSM_STATE(wait_for_start)
                        DBG_PRINT_EVENT("unhandled event");
                        break;
                case EV_EXIT:
+                       robot.corns = get_all_corns(robot.corns_conf_side, robot.corns_conf_center);
                        break;
        }
 }

diff --git a/src/robofsm/robot.h b/src/robofsm/robot.h
index b859249ea753a0ce965906d6d177c97388f413a0..88fb6a88950bab55aae010623466d1d9dbb78e23 100644 (file)
--- a/src/robofsm/robot.h
+++ b/src/robofsm/robot.h
@@ -209,6 +209,7 @@ struct robot {
 
        char corns_conf_center;
        char corns_conf_side;
+       struct corns_group *corns;
 
        bool obstacle_avoidance_enabled;
 }; /* robot */