RED team color has a separate binary

diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc
index 165e07044dec19eecf2523ff45d7f791bc95407e..7ec91e1f5195e3bc0212b28f4a498eaed947b285 100644 (file)
--- a/src/robofsm/motion-control.cc
+++ b/src/robofsm/motion-control.cc
@@ -174,11 +174,27 @@ static void check_for_collision_in_future(Trajectory *traj, double current_time)
 static void do_estimation()
 {
        static bool virgo = true;
+
+//#define WE_ARE_RED
+#define WE_ARE_GREEN
+
+#ifdef WE_ARE_GREEN
+#ifndef WE_ARE_RED
        static real_t beacon_xy[3][2] = {
                { 3.062, -0.05},
                {-0.062,  1.05},
                { 3.062,  2.162},
        };
+#endif
+#else
+#ifdef WE_ARE_RED
+       static real_t beacon_xy[3][2] = {
+               {-0.062, -0.05},
+               { 3.062,  1.05},
+               {-0.062,  2.162},
+       };
+#endif
+#endif
        static ekf8_t ekf8;
        static uint32_t odo0[2];
        static real_t y[5] = {0.0, 0.0, 0.0, 0.0, 0.0};
@@ -187,6 +203,12 @@ static void do_estimation()
        real_t x[8], P[8*8];
        int i, odo_received, err[5];
 
+#ifdef WE_ARE_RED
+       robot.team_color = RED;
+#else
+       robot.team_color = GREEN;
+#endif
+
        /* locks should not be necessary, however... */
        ROBOT_LOCK(corr_distances);
        t[0] = robot.corr_distances.t1;