Diff: Processes/Kalman/Kalman.cpp

Revision:

85:b0858346d838

Parent:

79:0d3140048526

--- a/Processes/Kalman/Kalman.cpp	Tue Apr 16 10:05:58 2013 +0000
+++ b/Processes/Kalman/Kalman.cpp	Tue Apr 16 10:43:15 2013 +0000
@@ -55,7 +55,7 @@
     printf("waiting for all sonar, and at least 1 IR\r\n");
     while( ((sensorseenflags & 0x7)^0x7) || !(sensorseenflags & 0x7<<3) );
 
-#ifdef TEAM_RED
+//#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;
@@ -63,7 +63,8 @@
     float r1 = RawReadings[SONAR2];
     float r2 = RawReadings[SONAR1];
     float r3 = RawReadings[SONAR0];
-#endif
+//#endif
+/*
 #ifdef  TEAM_BLUE
     const float d = beaconpos[1].y - beaconpos[2].y;
     const float i = beaconpos[0].y - beaconpos[2].y;
@@ -72,21 +73,24 @@
     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
+//#ifdef TEAM_RED
     //coordinate system hack (for now)
     x_coor = beaconpos[2].x - x_coor;
     y_coor = beaconpos[2].y - y_coor;
-#endif
+//#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);