Revision 19:4b993a9a156e, committed 2013-04-07

Comitter:

madcowswe

Date:

Sun Apr 07 19:26:07 2013 +0000

Parent:

17:6263e90bf3ba

Child:

20:70d651156779

Commit message:

Kalman init almost ready for testing

Changed in this revision

--- a/Processes/Kalman/Kalman.cpp	Sun Apr 07 17:51:59 2013 +0000
+++ b/Processes/Kalman/Kalman.cpp	Sun Apr 07 19:26:07 2013 +0000
@@ -16,12 +16,12 @@
 {
 
 //State variables
-Vector<float, 3> X;
+Matrix<float, 3, 1> X;
 Matrix<float, 3, 3> P;
 Mutex statelock;
 
 float RawReadings[maxmeasure+1];
-float SensorOffsets[maxmeasure+1] = {0};
+float IRpahseOffset;
 
 bool Kalman_init = 0;
 
@@ -29,7 +29,7 @@
     measurement_t mtype;
     float value;
     float variance;
-}
+};
 
 Mail <measurmentdata, 16> measureMQ;
 
@@ -38,21 +38,15 @@
 //Note: this init function assumes that the robot faces east, theta=0, in the +x direction
 void KalmanInit()
 {
-
-    //Solving for sonar bias is done by entering the following into wolfram alpha
-    //(a-f)^2 = x^2 + y^2; (b-f)^2 = (x-3)^2 + y^2; (c-f)^2 = (x-1.5)^2+(y-2)^2: solve for x,y,f
-    //where a, b, c are the measured distances, and f is the bias
-
-    SensorOffsets[SONAR0] = sonartimebias;
-    SensorOffsets[SONAR1] = sonartimebias;
-    SensorOffsets[SONAR2] = sonartimebias;
-
     //solve for our position (assume perfect bias)
     const float d = beaconpos[0].y - beaconpos[1].y;
     const float i = beaconpos[0].y - beaconpos[2].y;
     const float j = beaconpos[0].x - beaconpos[2].x;
+    float r1 = RawReadings[SONAR0];
+    float r2 = RawReadings[SONAR1];
+    float r3 = RawReadings[SONAR2];
 
-    float y_coor = (r1*r1-r2*r2+d*d)/2d;
+    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;
 
     //IR
@@ -71,32 +65,24 @@
         float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
 
         //printf("Angle %d : %f \n\r",i,angle_est*180/PI );
-        IR_Offset[i] = constrainAngle(IRMeasuresloc[i] - angle_est);
+        IR_Offsets[i] = constrainAngle(IRMeasuresloc[i] - angle_est);
         
-        fromb0offset += IR_Offsets[i] - IR_Offset[0];
+        fromb0offset += constrainAngle(IR_Offsets[i] - IR_Offsets[0]);
     }
-
+    
+    IRpahseOffset = constrainAngle(IR_Offsets[0] + fromb0offset/3);
 
     //debug
-    printf("Offsets IR: %0.4f, Sonar: %0.4f \r\n",IR_Offset*180/PI,Sonar_Offset*1000 );
-
+    printf("Offsets IR: %0.4f\r\n",IRpahseOffset*180/PI);
 
     statelock.lock();
-    X(0) = x_coor/1000.0f;
-    X(1) = y_coor/1000.0f;
-
-    if (Colour)
-        X(2) = 0;
-    else
-        X(2) = PI;
+    X(0,0) = x_coor;
+    X(1,0) = y_coor;
+    X(2,0) = 0;
     statelock.unlock();
-
-
-    //reattach the IR processing
-    ir.attachisr();
 }
 
-
+/*
 void Kalman::predictloop(void* dummy)
 {
 
@@ -355,19 +341,6 @@
     X(0) = x_coor/1000.0f;
     X(1) = y_coor/1000.0f;
 
-
-
-    /*    if (Colour){
-            X(0) = 0.2;
-            X(1) = 0.2;
-            //X(2) = 0;
-            }
-        else {
-            X(0) = 2.8;
-            X(1) = 0.2;
-            //X(2) = PI;
-        }
-        */
     P = 0.05, 0, 0,
     0, 0.05, 0,
     0, 0, 0.04;
@@ -377,5 +350,7 @@
 
 }
 
+*/
+
 } //Kalman Namespace
 

--- a/Processes/Kalman/Kalman.h	Sun Apr 07 17:51:59 2013 +0000
+++ b/Processes/Kalman/Kalman.h	Sun Apr 07 19:26:07 2013 +0000
@@ -21,13 +21,13 @@
 
 
 enum measurement_t {SONAR0 = 0, SONAR1, SONAR2, IR0, IR1, IR2};
-const measurement_t maxmeasure = IR3;
+const measurement_t maxmeasure = IR2;
 
 //Call this to run an update
 void runupdate(measurement_t type, float value, float variance);
 
 extern float RawReadings[maxmeasure+1];
-extern float SensorOffsets[maxmeasure+1];
+extern float IRpahseOffset;
 
 extern bool Kalman_init;
 

--- a/globals.h	Sun Apr 07 17:51:59 2013 +0000
+++ b/globals.h	Sun Apr 07 19:26:07 2013 +0000
@@ -4,11 +4,13 @@
 //(a-f)^2 = x^2 + y^2; (b-f)^2 = (x-3)^2 + y^2; (c-f)^2 = (x-1.5)^2+(y-2)^2: solve for x,y,f
 //where a, b, c are the measured distances, and f is the bias
 
-const float sonartimebias; // TODO: measure
+//const float sonartimebias = 0; // TODO: measure and stick in the coprocessor code
 
 struct pos {
     float x;
     float y;
 };
 
-extern pos beaconpos[3];
\ No newline at end of file
+extern pos beaconpos[3];
+
+const float PI = 3.14159265359;
\ No newline at end of file

--- a/main.cpp	Sun Apr 07 17:51:59 2013 +0000
+++ b/main.cpp	Sun Apr 07 19:26:07 2013 +0000
@@ -38,6 +38,7 @@
 */
 #include "mbed.h"
 #include "rtos.h"
+#include "globals.h"
 Serial pc(USBTX, USBRX);
 
 const PinName P_SERVO_LOWER_ARM = p5;

--- a/supportfuncs.h	Sun Apr 07 17:51:59 2013 +0000
+++ b/supportfuncs.h	Sun Apr 07 19:26:07 2013 +0000
@@ -1,15 +1,16 @@
 #ifndef SUPPORTFUNCS_H
 #define SUPPORTFUNCS_H
 
-#define _USE_MATH_DEFINES
 #include <cmath>
+#include "globals.h"
+
 
 //Constrains agles to +/- pi
 inline float constrainAngle(float x){
-    x = fmod(x + M_PI, 2*M_PI);
+    x = fmod(x + PI, 2*PI);
     if (x < 0)
-        x += 2*M_PI;
-    return x - M_PI;
+        x += 2*PI;
+    return x - PI;
 }
 
 #endif //SUPPORTFUNCS_H
\ No newline at end of file