Revision 72:7996aa8286ae, committed 2013-04-15

Comitter:

madcowswe

Date:

Mon Apr 15 13:37:32 2013 +0000

Parent:

71:eb1956c2d316

Child:

73:265d3cc6b0b1

Commit message:

Working both sides beacons

Changed in this revision

--- a/Processes/Kalman/Kalman.cpp	Mon Apr 15 10:52:36 2013 +0000
+++ b/Processes/Kalman/Kalman.cpp	Mon Apr 15 13:37:32 2013 +0000
@@ -55,6 +55,7 @@
     printf("waiting for all sonar, and at least 1 IR\r\n");
     while( ((sensorseenflags & 0x7)^0x7) || !(sensorseenflags & 0x7<<3) );
 
+#ifdef TEAM_RED
     //solve for our position (assume perfect bias)
     const float d = beaconpos[2].y - beaconpos[1].y;
     const float i = beaconpos[2].y - beaconpos[0].y;
@@ -62,15 +63,30 @@
     float r1 = RawReadings[SONAR2];
     float r2 = RawReadings[SONAR1];
     float r3 = RawReadings[SONAR0];
+#endif
+#ifdef  TEAM_BLUE
+    const float d = beaconpos[1].y - beaconpos[2].y;
+    const float i = beaconpos[0].y - beaconpos[2].y;
+    const float j = beaconpos[0].x - beaconpos[2].x;
+    float r1 = RawReadings[SONAR2];
+    float r2 = RawReadings[SONAR1];
+    float r3 = RawReadings[SONAR0];
+#endif
 
     printf("ranges: 0: %0.4f, 1: %0.4f, 2: %0.4f \r\n", r1, r2, r3);
 
     float y_coor = (r1*r1-r2*r2+d*d)/(2*d);
     float x_coor = (r1*r1-r3*r3+i*i+j*j)/(2*j) - (i*y_coor)/j;
 
+#ifdef TEAM_RED
     //coordinate system hack (for now)
     x_coor = beaconpos[2].x - x_coor;
     y_coor = beaconpos[2].y - y_coor;
+#endif
+#ifdef  TEAM_BLUE
+    x_coor = x_coor - beaconpos[2].x;
+    y_coor = y_coor - beaconpos[2].y;
+#endif
 
     printf("solved pos from sonar: %f, %f \r\n", x_coor, y_coor);
 

--- a/Processes/Motion/motion.cpp	Mon Apr 15 10:52:36 2013 +0000
+++ b/Processes/Motion/motion.cpp	Mon Apr 15 13:37:32 2013 +0000
@@ -162,7 +162,7 @@
         float d = ADS.Distanceincm();
         if(d > 10)
         {
-            forward_v *= max(min((d-15)*(1.0f/20.0f),1.0f),-0.1f);
+            forward_v *= max(min((d-15)*(1.0f/25.0f),1.0f),-0.1f);
         }
     }
     // end of Oskar hack

--- a/globals.cpp	Mon Apr 15 10:52:36 2013 +0000
+++ b/globals.cpp	Mon Apr 15 13:37:32 2013 +0000
@@ -2,4 +2,9 @@
 #include "globals.h"
 
 //Store global objects here
+#ifdef TEAM_RED
 pos beaconpos[] = {{-0.040,1}, {3.040,-0.040}, {3.040,2.040}};
+#endif
+#ifdef  TEAM_BLUE
+pos beaconpos[] = {{3.040,1}, {-0.040,2.040}, {-0.040,-0.040}};
+#endif

--- a/globals.h	Mon Apr 15 10:52:36 2013 +0000
+++ b/globals.h	Mon Apr 15 13:37:32 2013 +0000
@@ -2,6 +2,9 @@
 #ifndef GLOBALS_H
 #define GLOBALS_H
 
+//#define TEAM_RED
+#define TEAM_BLUE
+
 #include "mbed.h"
 
 const float KALMAN_PREDICT_PERIOD = 0.05; //seconds