Revision 1:bbabbd997d21, committed 2012-04-20

Comitter:

narshu

Date:

Fri Apr 20 21:56:15 2012 +0000

Parent:

0:f3bf6c7e2283

Child:

2:cffa347bb943

Commit message:

copied everything from secondary;

Changed in this revision

--- a/Kalman/Kalman.cpp	Fri Apr 20 20:39:35 2012 +0000
+++ b/Kalman/Kalman.cpp	Fri Apr 20 21:56:15 2012 +0000
@@ -10,6 +10,7 @@
 #include "globals.h"
 #include "motors.h"
 #include "system.h"
+#include "geometryfuncs.h"
 
 #include <tvmet/Matrix.h>
 #include <tvmet/Vector.h>
@@ -27,7 +28,7 @@
         predictticker( SIGTICKARGS(predictthread, 0x1) ),
 //        sonarthread(sonarloopwrapper, this, osPriorityNormal, 256),
 //        sonarticker( SIGTICKARGS(sonarthread, 0x1) ),
-        updatethread(updateloopwrapper, this, osPriorityNormal, 512) {
+        updatethread(updateloopwrapper, this, osPriorityNormal, 2048) {
 
     //Initilising matrices
 
@@ -38,9 +39,9 @@
         0, 1, 0,
         0, 0, 0.04;
 
-    Q = 0.002, 0, 0, //temporary matrix, Use dt!
-        0, 0.002, 0,
-        0, 0, 0.002;
+    //Q = 0.002, 0, 0, //temporary matrix, Use dt!
+    //    0, 0.002, 0,
+    //    0, 0, 0.002;
 
     //measurment variance R is provided by each sensor when calling runupdate
 
@@ -49,7 +50,7 @@
     sonararray.mcallbackfunc = (void (DummyCT::*)(int beaconnum, float distance, float variance)) &Kalman::runupdate;
 
 
-//    predictticker.start(20);
+    predictticker.start(20);
 //    sonarticker.start(50);
 
 
@@ -57,31 +58,27 @@
 
 
 void Kalman::predictloop() {
-    Matrix<float, 3, 3> F;
-    int lastleft = motors.getEncoder1();
-    int lastright = motors.getEncoder2();
-    int leftenc;
-    int rightenc;
-    float dleft,dright;
-    float dxp, dyp;
-    float thetap,d,r;
+
+    float lastleft = 0;
+    float lastright = 0;
 
     while (1) {
-//        Thread::signal_wait(0x1);
+        Thread::signal_wait(0x1);
         led1 = !led1;
         
-        leftenc = motors.getEncoder1();
-        rightenc = motors.getEncoder2();
+        int leftenc = motors.getEncoder1();
+        int rightenc = motors.getEncoder2();
         
-        dleft = motors.encoderToDistance(leftenc-lastleft)/1000.0f;
-        dright = motors.encoderToDistance(rightenc-lastright)/1000.0f;
+        float dleft = motors.encoderToDistance(leftenc-lastleft)/1000.0f;
+        float dright = motors.encoderToDistance(rightenc-lastright)/1000.0f;
 
         lastleft = leftenc;
         lastright = rightenc;
 
 
         //The below calculation are in body frame (where +x is forward)
-        thetap = (dright - dleft)*PI / (float(robotCircumference)/1000.0f);
+        float dxp, dyp,d,r;
+        float thetap = (dright - dleft)*PI / (float(robotCircumference)/1000.0f);
         if (abs(thetap) < 0.02) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error
             d = (dright + dleft)/2.0f;
             dxp = d*cos(thetap/2.0f);
@@ -102,15 +99,36 @@
         X(2) = rectifyAng(X(2) + thetap);
 
         //Linearising F around X
+        Matrix<float, 3, 3> F;
         F = 1, 0, (dxp * -sin(X(2)) - dyp * cos(X(2))),
             0, 1, (dxp * cos(X(2)) - dyp * sin(X(2))),
             0, 0, 1;
 
+        //Generating forward and rotational variance
+        float varfwd = fwdvarperunit * (dright + dleft) / 2.0f;
+        float varang = varperang * thetap;
+        float varxydt = xyvarpertime * PREDICTPERIOD;
+        float varangdt = angvarpertime * PREDICTPERIOD;
+        
+        //Rotating into cartesian frame
+        Matrix<float, 2, 2> Qsub,Qsubrot,Qrot;
+        Qsub = varfwd + varxydt, 0,
+               0, varxydt;
+               
+        Qrot = Rotmatrix(X(2));
+               
+        Qsubrot = Qrot * Qsub * trans(Qrot);
+
+        //Generate Q
+        Matrix<float, 3, 3> Q;//(Qsubrot);
+        Q = Qsubrot(0,0), Qsubrot(0,1), 0,
+            Qsubrot(1,0), Qsubrot(1,1), 0,
+            0, 0, varang + varangdt;
 
         P = F * P * trans(F) + Q;
 
         statelock.unlock();
-        Thread::wait(20);
+        //Thread::wait(PREDICTPERIOD);
 
         //cout << "predict" << X << endl;
         //cout << P << endl;
@@ -179,7 +197,7 @@
 
                     float dist = value / 1000.0f; //converting to m from mm
                     int sonarid = type;
-                    aborton2stddev = true;
+                    aborton2stddev = false; 
 
                     statelock.lock();
                     SonarMeasures[sonarid] = dist; //update the current sonar readings
@@ -187,7 +205,7 @@
                     rbx = X(0) - beaconpos[sonarid].x/1000.0f;
                     rby = X(1) - beaconpos[sonarid].y/1000.0f;
 
-                    expecdist = sqrt(rbx*rbx + rby*rby);
+                    expecdist = hypot(rbx, rby);//sqrt(rbx*rbx + rby*rby);
                     Y = dist - expecdist;
 
                     dhdx = rbx / expecdist;
@@ -196,7 +214,8 @@
                     H = dhdx, dhdy, 0;
 
                 } else if (type <= IR3) {
-                    
+                  
+                    aborton2stddev = false;
                     int IRidx = type-3;
                     
                     statelock.lock();
@@ -206,11 +225,11 @@
                     rby = X(1) - beaconpos[IRidx].y/1000.0f;
                     
                     float expecang = atan2(-rbx, -rby) - X(2);
-                    Y = rectifyAng(value - expecang);
+                    //printf("expecang: %0.4f, value: %0.4f \n\r", expecang*180/PI,value*180/PI);
+                    Y = rectifyAng(value + expecang);
                     
                     float dstsq = rbx*rbx + rby*rby;
                     H = -rby/dstsq, rbx/dstsq, -1;
-                    
                 }
 
                 Matrix<float, 3, 1> PH (P * trans(H));

--- a/Kalman/Kalman.h	Fri Apr 20 20:39:35 2012 +0000
+++ b/Kalman/Kalman.h	Fri Apr 20 21:56:15 2012 +0000
@@ -10,7 +10,7 @@
 
 class Kalman {
 public:
-    enum measurement_t {SONAR1, SONAR2, SONAR3, IR1, IR2, IR3};
+    enum measurement_t {SONAR1 = 0, SONAR2, SONAR3, IR1, IR2, IR3};
     static const measurement_t maxmeasure = IR3;
     
     Kalman(Motors &motorsin);
@@ -28,7 +28,7 @@
     
 private:
 
-    Matrix<float, 3, 3> Q; //perhaps calculate on the fly? dependant on speed etc?
+    //Matrix<float, 3, 3> Q; //perhaps calculate on the fly? dependant on speed etc?
     
     RFSRF05 sonararray;
     Motors& motors;
@@ -48,13 +48,9 @@
         float value;
         float variance;
     } ;
-     
     
     Mail <measurmentdata, 16> measureMQ;
     
-    
-    
-    
     Thread updatethread;
     void updateloop();
     static void updateloopwrapper(void const *argument){ ((Kalman*)argument)->updateloop(); }

--- a/Kalman/Sonar/RFSRF05.cpp	Fri Apr 20 20:39:35 2012 +0000
+++ b/Kalman/Sonar/RFSRF05.cpp	Fri Apr 20 21:56:15 2012 +0000
@@ -2,8 +2,6 @@
 #include "RFSRF05.h"
 #include "mbed.h"
 #include "globals.h"
-DigitalOut GLED3(LED3);
-DigitalOut GLED4(LED4);
 
 RFSRF05::RFSRF05(PinName trigger,
                  PinName echo0,

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/geometryfuncs.h	Fri Apr 20 21:56:15 2012 +0000
@@ -0,0 +1,28 @@
+#ifndef GEOMETRYFUNCS_H
+#define GEOMETRYFUNCS_H
+
+#include <tvmet/Matrix.h>
+
+template <typename T>
+Matrix <T, 2, 2> Rotmatrix(T theta) {
+     Matrix <T, 2, 2> outmatrix;
+     outmatrix = cos(theta), -sin(theta),
+                 sin(theta), cos(theta);
+     return outmatrix;
+}
+
+// rectifies angle to range -PI to PI
+template <typename T>
+T rectifyAng (T ang_in) {
+    ang_in -= (floor(ang_in/(2*PI)))*2*PI;
+    if (ang_in < -PI) {
+        ang_in += 2*PI;
+    }
+    if (ang_in > PI) {
+        ang_in -= 2*PI;
+    }
+
+    return ang_in;
+}
+
+#endif //GEOMETRYFUNCS_H
\ No newline at end of file

--- a/globals.h	Fri Apr 20 20:39:35 2012 +0000
+++ b/globals.h	Fri Apr 20 21:56:15 2012 +0000
@@ -6,15 +6,25 @@
 #define PI 3.14159265
 
 //Robot constants
-//const int encoderRevCount = 1856;
-//const int wheelmm = 314;
-//const int robotCircumference = 1256;
+const int robot_width = 395;
+const int encoderRevCount = 1856;
+const int wheelmm = 314;
+const int robotCircumference = 1256;
 
 //Robot constants in mm
-const int robot_width = 260;
-const int encoderRevCount = 360;
-const int wheelmm = 226;
-const int robotCircumference = 816;
+//const int robot_width = 260;
+//const int encoderRevCount = 360;
+//const int wheelmm = 226;
+//const int robotCircumference = 816;
+
+//Robot movement constants
+const float fwdvarperunit = 0.005; //1 std dev = 7% //NEEDS TO BE MEASURED AGAIN!
+const float varperang = 3E-5; //around 1 degree stddev per 180 turn
+const float xyvarpertime = 0.001; //(very poorly) accounts for hitting things
+const float angvarpertime = 0.001;
+
+//sonar constants
+static const float sonarvariance = 0.005;
 
 //Arena constants
 struct pos {
@@ -23,8 +33,8 @@
 };
 const pos beaconpos[] = {{3000, 1000},{0,0}, {0,2000}};
 
-//sonar constants
-static const float sonarvariance = 0.004;
+//System constants
+const int PREDICTPERIOD = 20; //ms
 
 //High speed serial port
 extern Serial pc;
@@ -38,13 +48,12 @@
 #define RELI_BOUND_LOW          4
 #define RELI_BOUND_HIGH         25
 
-// Localization estimate torrelences
-#define POSITION_TOR            80
+// Localization estimate tolerences
+#define POSITION_TOR            50
 #define ANGLE_TOR               0.15
 
 // motion control
-#define ROTA_SPEED              40
-#define MOVE_SPEED              50
+#define MOVE_SPEED              30
 #define MAX_STEP_RATIO          0.10 //maximum change in the speed
 //#define TRACK_RATE              10       // +- rate for each wheel when tracking
 
@@ -52,6 +61,4 @@
 #define IR_TURRET_PERIOD        200
 #define MOTION_UPDATE_PERIOD    20
 
-
-
 #endif
\ No newline at end of file

--- a/main.cpp	Fri Apr 20 20:39:35 2012 +0000
+++ b/main.cpp	Fri Apr 20 21:56:15 2012 +0000
@@ -6,6 +6,7 @@
 #include "motors.h"
 #include "math.h"
 #include "system.h"
+#include "geometryfuncs.h"
 
 //#include <iostream>
 
@@ -13,63 +14,77 @@
 //I2C i2c(p28, p27);        // sda, scl
 TSI2C i2c(p28, p27);
 Serial pc(USBTX, USBRX); // tx, rx
-Serial  IRturret(p13, p14);
+Serial  IRturret(p9, p10);
 
 DigitalOut     OLED1(LED1);
 DigitalOut     OLED2(LED2);
 DigitalOut     OLED3(LED3);
 DigitalOut     OLED4(LED4);
 
-int16_t face;
-
-
 Motors motors;
 Kalman kalman(motors);
 
 float targetX = 1000, targetY = 1000, targetTheta = 0;
 
-// bytes packing for IR turret serial comm
+// bytes packing/unpacking for IR turret serial comm
 union IRValue_t {
-    float IR_floats[6];
-    int   IR_ints[6];
-    unsigned char IR_chars[24];
+    float IR_floats[3];
+    int IR_ints[3];
+    unsigned char IR_chars[12];
 } IRValues;
 
+char Alignment_char[4] = {0xFF,0xFE,0xFD,0xFC};
+int Alignment_ptr = 0;
+bool data_flag = false;
+int buff_pointer = 0;
+bool angleInit = false;
+float angleOffset = 0;
+
+void vIRValueISR (void);
+void vKalmanInit(void);
+
 //TODO mutex on kalman state, and on motor commands (i.e. on the i2c bus)
 //NOTE! Recieving data with RF12B now DISABLED due to interferance with rtos!
 
+
 void vMotorThread(void const *argument);
 void vPrintState(void const *argument);
 void ai_thread (void const *argument);
 void motion_thread(void const *argument);
-void vIRValueISR (void);
-void motorSpeedThread(void const *argument);
+
 
 float getAngle (float x, float y);
 void getIRValue(void const *argument);
 
-
-//Thread thr_AI(ai_thread);
-//Thread thr_motion(motion_thread);
-//Thread thr_getIRValue(getIRValue);
+// Thread pointers
+Thread *AI_Thread_Ptr;
+Thread *Motion_Thread_Ptr;
 
 Mutex targetlock;
 bool flag_terminate = false;
 
-
 float temp = 0;
 
 //Main loop
 int main() {
-motors.stop();
     pc.baud(115200);
-    IRturret.baud(9600);
+    IRturret.baud(115200);
     IRturret.format(8,Serial::Odd,1);
-    //Thread thread(motorSpeedThread, NULL, osPriorityAboveNormal); //PID controller task. run at 50ms
-    //IRturret.attach(&vIRValueISR);
-    //Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256);
+    //IRturret.attach(&vIRValueISR,Serial::RxIrq);
+    //vKalmanInit();
+
+    Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256);
     Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024);
 
+    //Thread thr_AI(ai_thread,NULL,osPriorityNormal,1024);
+    //Thread thr_motion(motion_thread,NULL,osPriorityNormal,1024);
+    //AI_Thread_Ptr = &thr_AI;
+    //Motion_Thread_Ptr = &thr_motion;
+
+    //measure cpu usage. output updated once per second to symbol cpupercent
+    //Thread mCPUthread(measureCPUidle, NULL, osPriorityIdle, 1024); //check if stack overflow with such a small staack
+
+
     pc.printf("We got to main! ;D\r\n");
 
     //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!!
@@ -79,18 +94,15 @@
     }
 }
 
-void motorSpeedThread(void const *argument) {
-    while (1) {
-        motors.speedRegulatorTask();
-        Thread::wait(10);
-    }
-}
-
 
 void vMotorThread(void const *argument) {
     motors.resetEncoders();
     while (1) {
-        motors.setSpeed(20,-20);
+        motors.setSpeed(20,20);
+        Thread::wait(2000);
+        motors.stop();
+        Thread::wait(5000);
+        motors.setSpeed(-20,-20);
         Thread::wait(2000);
         motors.stop();
         Thread::wait(5000);
@@ -98,6 +110,10 @@
         Thread::wait(2000);
         motors.stop();
         Thread::wait(5000);
+        motors.setSpeed(20,-20);
+        Thread::wait(2000);
+        motors.stop();
+        Thread::wait(5000);
     }
 }
 
@@ -105,41 +121,28 @@
 
 void vPrintState(void const *argument) {
     float state[3];
-    float sonardist[3];
-    //float x,y;
-    //float d = beaconpos[2].y - beaconpos[1].y;
-    //float i = beaconpos[0].y;
-    //float j = beaconpos[0].x;
+    float SonarMeasures[3];
+    float IRMeasures[3];
+
 
     while (1) {
         kalman.statelock.lock();
         state[0] = kalman.X(0);
         state[1] = kalman.X(1);
         state[2] = kalman.X(2);
-        sonardist[0] = kalman.SonarMeasures[0]*1000.0f;
-        sonardist[1] = kalman.SonarMeasures[1]*1000.0f;
-        sonardist[2] = kalman.SonarMeasures[2]*1000.0f;
+        SonarMeasures[0] = kalman.SonarMeasures[0];
+        SonarMeasures[1] = kalman.SonarMeasures[1];
+        SonarMeasures[2] = kalman.SonarMeasures[2];
+        IRMeasures[0] = kalman.IRMeasures[0];
+        IRMeasures[1] = kalman.IRMeasures[1];
+        IRMeasures[2] = kalman.IRMeasures[2];
         kalman.statelock.unlock();
-
-        // trilateration algorithm
-        //y = ((sonardist[1]*sonardist[1]) - (sonardist[2]*sonardist[2]) + d*d) / (2*d);
-        //x = ((sonardist[1]*sonardist[1]) - (sonardist[0]*sonardist[0]) + i*i + j*j) / (2*j) - (i/j)*y;
-
-
-        //kalman.statelock.lock();
         pc.printf("\r\n");
-        //printf("Position X: %0.1f Y: %0.1f \r\n", x,y);
         pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0], state[1],state[2]);
-        //targetlock.lock();
-        //pc.printf("target : %0.4f %0.4f %0.4f \r\n", targetX/1000, targetY/1000,targetTheta);
-        //targetlock.unlock();
-        printf("Sonar Dist: %0.4f, %0.4f, %0.4f \r\n", sonardist[0],sonardist[1],sonardist[2]);
-
-        //printf("Distance is %0.1f, %0.1f \r\n", motors.encoderToDistance(motors.getEncoder1()), motors.encoderToDistance(motors.getEncoder2()));
-
-        //pc.printf("IR ang : %f \n",temp * 180/3.14);
-        //kalman.statelock.unlock();
-        Thread::wait(200);
+        pc.printf("Sonar: %0.4f %0.4f %0.4f \r\n",SonarMeasures[0],SonarMeasures[1],SonarMeasures[2]);
+        pc.printf("IR   : %0.4f %0.4f %0.4f \r\n",IRMeasures[0]*180/PI,IRMeasures[1]*180/PI,IRMeasures[2]*180/PI);
+        pc.printf("Angle_Offset: %0.4f \r\n",angleOffset*180/PI);
+        Thread::wait(100);
     }
 }
 
@@ -183,7 +186,7 @@
     targetlock.lock();
     targetX     = 1500;
     targetY     = 1000;
-    targetTheta = -PI;
+    targetTheta = -PI/2;
     targetlock.unlock();
 
     // home
@@ -206,10 +209,10 @@
 // motion control thread ------------------------
 void motion_thread(void const *argument) {
     motors.resetEncoders();
-    motors.setSpeed(20,20);
-    Thread::wait(2000);
+    motors.setSpeed(MOVE_SPEED/2,MOVE_SPEED/2);
+    Thread::wait(1000);
     motors.stop();
-    //thr_AI.signal_set(0x01);
+    (*AI_Thread_Ptr).signal_set(0x01);
 
 
 
@@ -231,17 +234,6 @@
         kalman.statelock.unlock();
 
 
-
-        // get IR turret angle --------------------------------------
-        /*        float angEst = getAngle(currX, currY);          // check this
-                if (angEst != 1000){
-                    // here the value should be a valid estimate
-                    // so update kalman state
-
-                }
-        */
-        //!!! add code here to update kalman angle state !!!
-
         // check if target reached ----------------------------------
         if (  ( abs(currX - targetX) < POSITION_TOR )
                 &&( abs(currY - targetY) < POSITION_TOR )
@@ -250,7 +242,6 @@
             diffDir = rectifyAng(currTheta - targetTheta);
             diffSpeed = diffDir / PI;
 
-            //pc.printf("%f \n",diffDir);
             if (abs(diffDir) > ANGLE_TOR) {
                 if (abs(diffSpeed) < 0.5) {
                     diffSpeed = 0.5*diffSpeed/abs(diffSpeed);
@@ -261,7 +252,7 @@
             } else {
                 motors.stop();
                 Thread::wait(4000);
-                //thr_AI.signal_set(0x01);
+                (*AI_Thread_Ptr).signal_set(0x01);
             }
         }
 
@@ -311,64 +302,78 @@
     }
 }
 
+void vIRValueISR (void) {
 
-void getIRValue(void const *argument) {
-    Thread::signal_wait(0x01);
-    if (IRturret.readable() >= 24) {
-        for (int i=0; i < 24; i++) {
-            IRValues.IR_chars[i] = IRturret.getc();
+    OLED3 = !OLED3;
+    // A workaround for mbed UART ISR bug
+    // Clear the RBR flag to make sure the interrupt doesn't loop
+    // UART3 for the port on pins 9/10, UART2 for pins 28/27, and UART1 for pins 13/14.
+    // UART0 for USB UART
+    unsigned char RBR = LPC_UART1->RBR;
+
+
+    if (!data_flag) { // look for alignment bytes
+        if (RBR == Alignment_char[Alignment_ptr]) {
+            Alignment_ptr ++;
         }
+        if (Alignment_ptr >= 4) {
+            Alignment_ptr = 0;
+            data_flag = true; // set the dataflag
+        }
+    } else { // fetch data bytes
+        IRValues.IR_chars[buff_pointer] = RBR;
+        buff_pointer ++;
+        if (buff_pointer >= 12) {
+            buff_pointer = 0;
+            data_flag = false; // dessert the dataflag
+            if (angleInit) {
+                kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),rectifyAng(IRValues.IR_floats[1]+angleOffset),IRValues.IR_floats[2]);
+            } else {
+                kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),IRValues.IR_floats[1],IRValues.IR_floats[2]);
+            }
+        }
+
     }
 }
 
-void vIRValueISR (void) {
-    //thr_getIRValue.signal_set(0x01);
-}
-
-
-float getAngle (float x, float y) {
-    /*  function that returns current robot orientation
-        input:  x, y coords in mm
-        output: orientation in rad
-        note: the angle readings from IR is read in here
-            must be called faster than IR turret frequency (few Hz - currently 3Hz)
-    */
-
-    // variables
-    float   ang0 = 0, ang1 = 0, ang2 = 0;
-    int     sc0 = 0, sc1 = 0, sc2 = 0;
-
-
-
-    // read serial port
-    if (IRturret.readable()) {
-        IRturret.scanf("%f;%f;%f;%d;%d;%d;",&ang0,&ang1,&ang2,&sc0,&sc1,sc2);
+void vKalmanInit(void) {
+    float SonarMeasures[3];
+    float IRMeasures[3];
+    int beacon_cnt = 0;
+    wait(1);
+    IRturret.attach(NULL,Serial::RxIrq);
+    kalman.statelock.lock();
+    SonarMeasures[0] = kalman.SonarMeasures[0]*1000.0f;
+    SonarMeasures[1] = kalman.SonarMeasures[1]*1000.0f;
+    SonarMeasures[2] = kalman.SonarMeasures[2]*1000.0f;
+    IRMeasures[0] = kalman.IRMeasures[0];
+    IRMeasures[1] = kalman.IRMeasures[1];
+    IRMeasures[2] = kalman.IRMeasures[2];
+    kalman.statelock.unlock();
+    //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasures[0]*180/PI, IRMeasures[1]*180/PI, IRMeasures[2]*180/PI);
+    float d = beaconpos[2].y - beaconpos[1].y;
+    float i = beaconpos[0].y - beaconpos[1].y;
+    float j = beaconpos[0].x - beaconpos[1].x;
+    float y_coor = (SonarMeasures[1]*SonarMeasures[1]- SonarMeasures[2]*SonarMeasures[2] + d*d) / (2*d);
+    float x_coor = (SonarMeasures[1]*SonarMeasures[1] - SonarMeasures[0]*SonarMeasures[0] + i*i + j*j)/(2*j) - i*y_coor/j;
+    angleOffset = 0;
+    for (int i = 0; i < 3; i++) {
+        float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
+        if (IRMeasures[i] != 0){
+           beacon_cnt ++;
+           float angle_temp = angle_est - IRMeasures[i];
+           angle_temp -= (floor(angle_temp/(2*PI)))*2*PI; 
+           angleOffset += angle_temp;
+        }
     }
-
-    // if none sampled
-    if (!sc0 && !sc1 && ! sc2) {
-        return 1000;
-    }
-
-    //
-    else {
-        float est0 = 0, est1 = 0, est2 = 0;
-
-        // calc
-        if ((sc0 > RELI_BOUND_LOW)||(sc0 < RELI_BOUND_HIGH)) {
-            est0 = atan2(1000 - y, 3000-x) - ang0;
-        }
-        if ((sc1 > RELI_BOUND_LOW)||(sc1 < RELI_BOUND_HIGH)) {
-            est1 = atan2( -y, -x) - ang1;
-        }
-        if ((sc2 > RELI_BOUND_LOW)||(sc2 < RELI_BOUND_HIGH)) {
-            est2 = atan2(2000 - y, -x) - ang2;
-        }
-
-        // weighted avg
-        return rectifyAng( ((est0 * RELI_BOUND_HIGH-sc0) + (est1 * RELI_BOUND_HIGH-sc1) + (est2 * RELI_BOUND_HIGH-sc2)) / (3*RELI_BOUND_HIGH - sc0 - sc1 - sc2) );
-    }
-
-}
-
-
+    angleOffset = angleOffset/float(beacon_cnt);
+    //printf("\n\r");
+    angleInit = true;
+    kalman.statelock.lock();
+    kalman.X(0) = x_coor/1000.0f;
+    kalman.X(1) = y_coor/1000.0f;
+    kalman.X(2) = 0;
+    kalman.statelock.unlock();
+    //printf("x: %0.4f, y: %0.4f, offset: %0.4f \n\r", x_coor, y_coor, angleOffset*180/PI);
+    IRturret.attach(&vIRValueISR,Serial::RxIrq);
+}
\ No newline at end of file

--- a/system.h	Fri Apr 20 20:39:35 2012 +0000
+++ b/system.h	Fri Apr 20 21:56:15 2012 +0000
@@ -3,28 +3,16 @@
 #define SYSTEM_H
 
 #include "globals.h"
-
-//#include "rtos.h"
+#include "rtos.h"
 
-// rectifies angle to range -PI to PI
-template <typename T>
-T rectifyAng (T ang_in) {
-    ang_in -= (floor(ang_in/(2*PI)))*2*PI;
-    if (ang_in < -PI) {
-        ang_in += 2*PI;
-    }
-    if (ang_in > PI) {
-        ang_in -= 2*PI;
-    }
+//a type which is a pointer to a rtos thread function
+typedef void (*tfuncptr_t)(void const *argument);
 
-    return ang_in;
-}
-
+//---------------------
+//Signal ticker stuff
 #define SIGTICKARGS(thread, signal) \
     (tfuncptr_t) (&Signalsetter::callback), osTimerPeriodic, (void*)(new Signalsetter(thread, signal))
 
-typedef void (*tfuncptr_t)(void const *argument);
-
 class Signalsetter {
 public:
     Signalsetter(Thread& inthread, int insignal) :
@@ -46,4 +34,9 @@
     int signal;
 };
 
+//---------------------
+//cpu usage measurement function
+extern float cpupercent;
+void measureCPUidle (void const* arg);
+
 #endif