Revision 0:fbfafa6bf5f9, committed 2012-04-20

Comitter:

narshu

Date:

Fri Apr 20 21:32:24 2012 +0000

Commit message:

Changed in this revision

diff -r 000000000000 -r fbfafa6bf5f9 Kalman/Kalman.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Kalman.cpp	Fri Apr 20 21:32:24 2012 +0000
@@ -0,0 +1,262 @@
+//***************************************************************************************
+//Kalman Filter implementation
+//***************************************************************************************
+#include "Kalman.h"
+#include "rtos.h"
+#include "RFSRF05.h"
+//#include "MatrixMath.h"
+//#include "Matrix.h"
+#include "math.h"
+#include "globals.h"
+#include "motors.h"
+#include "system.h"
+#include "geometryfuncs.h"
+
+#include <tvmet/Matrix.h>
+#include <tvmet/Vector.h>
+using namespace tvmet;
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+
+Kalman::Kalman(Motors &motorsin) :
+        sonararray(p10,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9),
+        motors(motorsin),
+        predictthread(predictloopwrapper, this, osPriorityNormal, 512),
+        predictticker( SIGTICKARGS(predictthread, 0x1) ),
+//        sonarthread(sonarloopwrapper, this, osPriorityNormal, 256),
+//        sonarticker( SIGTICKARGS(sonarthread, 0x1) ),
+        updatethread(updateloopwrapper, this, osPriorityNormal, 2048) {
+
+    //Initilising matrices
+
+    // X = x, y, theta;
+    X = 0.5, 0, 0;
+
+    P = 1, 0, 0,
+        0, 1, 0,
+        0, 0, 0.04;
+
+    //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
+
+    //attach callback
+    sonararray.callbackobj = (DummyCT*)this;
+    sonararray.mcallbackfunc = (void (DummyCT::*)(int beaconnum, float distance, float variance)) &Kalman::runupdate;
+
+
+    predictticker.start(20);
+//    sonarticker.start(50);
+
+
+}
+
+
+void Kalman::predictloop() {
+
+    float lastleft = 0;
+    float lastright = 0;
+
+    while (1) {
+        Thread::signal_wait(0x1);
+        led1 = !led1;
+        
+        int leftenc = motors.getEncoder1();
+        int rightenc = motors.getEncoder2();
+        
+        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)
+        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);
+            dyp = d*sin(thetap/2.0f);
+
+        } else { //calculate circle arc
+            //float r = (right + left) / (4.0f * PI * thetap);
+            r = (dright + dleft) / (2.0f*thetap);
+            dxp = abs(r)*sin(thetap);
+            dyp = r - r*cos(thetap);
+        }
+
+        statelock.lock();
+
+        //rotating to cartesian frame and updating state
+        X(0) += dxp * cos(X(2)) - dyp * sin(X(2));
+        X(1) += dxp * sin(X(2)) + dyp * cos(X(2));
+        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(PREDICTPERIOD);
+
+        //cout << "predict" << X << endl;
+        //cout << P << endl;
+    }
+}
+
+//void Kalman::sonarloop() {
+//    while (1) {
+//        Thread::signal_wait(0x1);
+//        sonararray.startRange();
+//    }
+//}
+
+
+void Kalman::runupdate(measurement_t type, float value, float variance) {
+    //printf("beacon %d dist %f\r\n", sonarid, dist);
+    //led2 = !led2;
+
+    measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
+    if (measured) {
+        measured->mtype = type;
+        measured->value = value;
+        measured->variance = variance;
+
+        osStatus putret = measureMQ.put(measured);
+        if (putret)
+            led4 = 1;
+        //    printf("putting in MQ error code %#x\r\n", putret);
+    } else {
+        led4 = 1;
+        //printf("MQalloc returned NULL ptr\r\n");
+    }
+
+}
+
+void Kalman::updateloop() {
+    measurement_t type;
+    float value,variance,rbx,rby,expecdist,Y;
+    float dhdx,dhdy;
+    bool aborton2stddev = false;
+
+    Matrix<float, 1, 3> H;
+
+    float S;
+    Matrix<float, 3, 3> I3( identity< Matrix<float, 3, 3> >() );
+
+
+    while (1) {
+        led2 = !led2;
+
+        osEvent evt = measureMQ.get();
+
+        if (evt.status == osEventMail) {
+
+            measurmentdata &measured = *(measurmentdata*)evt.value.p;
+            type = measured.mtype; //Note, may support more measurment types than sonar in the future!
+            value = measured.value;
+            variance = measured.variance;
+
+            // don't forget to free the memory
+            measureMQ.free(&measured);
+
+            if (type <= maxmeasure) {
+
+                if (type <= SONAR3) {
+
+                    float dist = value / 1000.0f; //converting to m from mm
+                    int sonarid = type;
+                    aborton2stddev = false; 
+
+                    statelock.lock();
+                    SonarMeasures[sonarid] = dist; //update the current sonar readings
+
+                    rbx = X(0) - beaconpos[sonarid].x/1000.0f;
+                    rby = X(1) - beaconpos[sonarid].y/1000.0f;
+
+                    expecdist = hypot(rbx, rby);//sqrt(rbx*rbx + rby*rby);
+                    Y = dist - expecdist;
+
+                    dhdx = rbx / expecdist;
+                    dhdy = rby / expecdist;
+
+                    H = dhdx, dhdy, 0;
+
+                } else if (type <= IR3) {
+                  
+                    aborton2stddev = false;
+                    int IRidx = type-3;
+                    
+                    statelock.lock();
+                    IRMeasures[IRidx] = value;
+                    
+                    rbx = X(0) - beaconpos[IRidx].x/1000.0f;
+                    rby = X(1) - beaconpos[IRidx].y/1000.0f;
+                    
+                    float expecang = atan2(-rbx, -rby) - X(2);
+                    //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));
+                S = (H * PH)(0,0) + variance;
+
+                if (aborton2stddev && Y*Y > 4 * S) {
+                    statelock.unlock();
+                    continue;
+                }
+
+                Matrix<float, 3, 1> K (PH * (1/S));
+
+                //Updating state
+                X += col(K, 0) * Y;
+                X(2) = rectifyAng(X(2));
+
+                P = (I3 - K * H) * P;
+
+                statelock.unlock();
+
+            }
+
+        } else {
+            led4 = 1;
+            //printf("ERROR: in updateloop, code %#x", evt);
+        }
+
+    }
+
+}
\ No newline at end of file

diff -r 000000000000 -r fbfafa6bf5f9 Kalman/Kalman.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Kalman.h	Fri Apr 20 21:32:24 2012 +0000
@@ -0,0 +1,59 @@
+#include "rtos.h"
+//#include "Matrix.h"
+#include "motors.h"
+#include "RFSRF05.h"
+
+#include <tvmet/Matrix.h>
+#include <tvmet/Vector.h>
+using namespace tvmet;
+
+
+class Kalman {
+public:
+    enum measurement_t {SONAR1 = 0, SONAR2, SONAR3, IR1, IR2, IR3};
+    static const measurement_t maxmeasure = IR3;
+    
+    Kalman(Motors &motorsin);
+    
+    void predict();
+    void runupdate(measurement_t type, float value, float variance);
+    
+    //State variables
+    Vector<float, 3> X;
+    Matrix<float, 3, 3> P;
+    Mutex statelock;
+    
+    float SonarMeasures[3];
+    float IRMeasures[3];
+    
+private:
+
+    //Matrix<float, 3, 3> Q; //perhaps calculate on the fly? dependant on speed etc?
+    
+    RFSRF05 sonararray;
+    Motors& motors;
+    
+    Thread predictthread;
+    void predictloop();
+    static void predictloopwrapper(void const *argument){ ((Kalman*)argument)->predictloop(); }
+    RtosTimer predictticker;
+    
+//    Thread sonarthread;
+//    void sonarloop();
+//    static void sonarloopwrapper(void const *argument){ ((Kalman*)argument)->sonarloop(); }
+//    RtosTimer sonarticker;
+    
+    struct measurmentdata{
+        measurement_t mtype;
+        float value;
+        float variance;
+    } ;
+    
+    Mail <measurmentdata, 16> measureMQ;
+    
+    Thread updatethread;
+    void updateloop();
+    static void updateloopwrapper(void const *argument){ ((Kalman*)argument)->updateloop(); }
+    
+    
+};
\ No newline at end of file

diff -r 000000000000 -r fbfafa6bf5f9 Kalman/Sonar/RF12B/RF12B.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Sonar/RF12B/RF12B.cpp	Fri Apr 20 21:32:24 2012 +0000
@@ -0,0 +1,393 @@
+#include "RF12B.h"
+
+#include "RF_defs.h"
+#include <algorithm>
+
+//#include "globals.h"
+
+//DigitalOut DBG2(LED2);
+//DigitalOut DBG3(LED3);
+//DigitalOut DBG4(LED4);
+
+RF12B::RF12B(PinName _SDI,
+             PinName _SDO,
+             PinName _SCK,
+             PinName _NCS,
+             PinName _NIRQ):spi(_SDI, _SDO, _SCK),
+        NCS(_NCS), NIRQ(_NIRQ), NIRQ_in(_NIRQ){// rfled(LED3) {
+
+    /* SPI frequency, word lenght, polarity and phase */
+    spi.format(16,0);
+    spi.frequency(2000000);
+
+    /* Set ~CS high */
+    NCS = 1;
+
+    /* Initialise RF Module */
+    init();
+
+    /* Setup interrupt to happen on falling edge of NIRQ */
+    NIRQ.fall(this, &RF12B::rxISR);
+}
+
+/* Returns the packet length if data is available in the receive buffer, 0 otherwise*/
+unsigned int RF12B::available() {
+    return fifo.size();
+}
+
+/* Reads a packet of data, with length "size" Returns false if read failed. TODO: make a metafifo to isolate packets*/
+bool RF12B::read(unsigned char* data, unsigned int size) {
+    if (fifo.size() == 0) {
+        return false;
+    } else {
+        unsigned int i = 0;
+        while (fifo.size() > 0 && i < size) {
+            data[i++] = fifo.front();
+            fifo.pop();
+        }
+        return true;
+    }
+}
+
+/* Reads a byte of data from the receive buffer */
+unsigned char RF12B::read() {
+    if (available()) {
+        unsigned char data = fifo.front();
+        fifo.pop();
+        return data;
+    } else {
+        return 0xFF; // Error val although could also be data...
+    }
+}
+
+/* Sends a packet of data to the RF module for transmission TODO: Make asych*/
+void RF12B::write(unsigned char *data, unsigned char length) {
+    unsigned char crc = 0;
+       
+    /* Transmitter mode */
+    changeMode(TX);
+
+    writeCmd(0x0000);
+    send(0xAA); // PREAMBLE
+    send(0xAA);
+    send(0xAA);
+    send(0x2D); // SYNC
+    send(0xD4);
+    /* Packet Length */
+    send(length);
+    crc = crc8(crc, length);
+    send(crc);
+    crc = crc8(crc, crc);
+    /* Packet Data */
+    for (unsigned char i=0; i<length; i++) {
+        send(data[i]);
+        crc = crc8(crc, data[i]);
+    }
+    send(crc);
+    send(0xAA); // DUMMY BYTES
+    send(0xAA);
+    send(0xAA);
+
+    /* Back to receiver mode */
+    changeMode(RX);
+    status();
+    
+
+}
+
+/* Transmit a 1-byte data packet */
+void RF12B::write(unsigned char data) {
+    write(&data, 1);
+}
+
+void RF12B::write(queue<char> &data, int length) {
+    char crc = 0;
+    char length_byte = 0;
+    
+    /* -1 means try to transmit everything in the queue */
+    if(length == -1) {
+        length = data.size();
+    }
+    
+    /* max length of packet is 255 */
+    length_byte = min(length, 255);
+    
+    /* Transmitter mode */
+    changeMode(TX);
+
+    writeCmd(0x0000);
+    send(0xAA); // PREAMBLE
+    send(0xAA);
+    send(0xAA);
+    send(0x2D); // SYNC
+    send(0xD4);
+    /* Packet Length */
+    send(length_byte);
+    crc = crc8(crc, length_byte);
+    send(crc);
+    crc = crc8(crc, crc);
+    /* Packet Data */
+    for (char i=0; i<length_byte; i++) {
+        send(data.front());
+        crc = crc8(crc, data.front());
+        data.pop();
+    }
+    send(crc);
+    send(0xAA); // DUMMY BYTES
+    send(0xAA);
+    send(0xAA);
+
+    /* Back to receiver mode */
+    changeMode(RX);
+    status();
+}
+
+/**********************************************************************
+ *  PRIVATE FUNCTIONS
+ *********************************************************************/
+
+/* Initialises the RF12B module */
+void RF12B::init() {
+    /* writeCmd(0x80E7); //EL,EF,868band,12.0pF
+     changeMode(RX);
+     writeCmd(0xA640); //frequency select
+     writeCmd(0xC647); //4.8kbps
+     writeCmd(0x94A0); //VDI,FAST,134kHz,0dBm,-103dBm
+     writeCmd(0xC2AC); //AL,!ml,DIG,DQD4
+     writeCmd(0xCA81); //FIFO8,SYNC,!ff,DR
+     writeCmd(0xCED4); //SYNC=2DD4
+     writeCmd(0xC483); //@PWR,NO RSTRIC,!st,!fi,OE,EN
+     writeCmd(0x9850); //!mp,90kHz,MAX OUT
+     writeCmd(0xCC17); //OB1, COB0, LPX, Iddy, CDDIT&#65533;CBW0
+     writeCmd(0xE000); //NOT USED
+     writeCmd(0xC800); //NOT USED
+     writeCmd(0xC040); //1.66MHz,2.2V */
+
+    writeCmd(
+        RFM_CONFIG_EL           |
+        RFM_CONFIG_EF           |
+        RFM_CONFIG_BAND_433     //|
+        //RFM_CONFIG_X_11_0pf // meh, using default
+    );
+
+    // 2. Power Management Command
+    // leave everything switched off for now
+    /*
+    writeCmd(
+        RFM_POWER_MANAGEMENT     // switch all off
+    );
+    */
+
+    // 3. Frequency Setting Command
+    writeCmd(
+        RFM_FREQUENCY            |
+        RFM_FREQ_433Band(435.7)  //I totally made this value up... if someone knows where the sweetspots are in this band, tell me!
+    );
+
+
+    // 4. Data Rate Command
+    writeCmd(RFM_DATA_RATE_9600);
+
+
+    // 5. Receiver Control Command
+    writeCmd(
+        RFM_RX_CONTROL_P20_VDI  |
+        RFM_RX_CONTROL_VDI_FAST |
+        //RFM_RX_CONTROL_BW(RFM_BAUD_RATE) |
+        RFM_RX_CONTROL_BW_134   |     // CHANGE THIS TO 67 TO IMPROVE RANGE! (though the bitrate must then be below 8kbaud, and fsk modulation changed)
+        RFM_RX_CONTROL_GAIN_0   |
+        RFM_RX_CONTROL_RSSI_103      // Might need adjustment. Datasheet says around 10^-5 bit error rate at this level and baudrate.
+    );
+
+    // 6. Data Filter Command
+    writeCmd(
+        RFM_DATA_FILTER_AL      |
+        RFM_DATA_FILTER_ML      |
+        RFM_DATA_FILTER_DIG     //|
+        //RFM_DATA_FILTER_DQD(4)
+    );
+
+    // 7. FIFO and Reset Mode Command
+    writeCmd(
+        RFM_FIFO_IT(8) |
+        RFM_FIFO_DR    |
+        0x8 //turn on 16bit sync word
+    );
+
+    // 8. FIFO Syncword
+    // Leave as default: 0xD4
+
+    // 9. Receiver FIFO Read
+    // when the interupt goes high, (and if we can assume that it was a fifo fill interrupt) we can read a byte using:
+    // result = RFM_READ_FIFO();
+
+    // 10. AFC Command
+    writeCmd(
+        //RFM_AFC_AUTO_VDI        |  //Note this might be changed to improve range. Refer to datasheet.
+        RFM_AFC_AUTO_INDEPENDENT    |
+        RFM_AFC_RANGE_LIMIT_7_8     |
+        RFM_AFC_EN                  |
+        RFM_AFC_OE                  |
+        RFM_AFC_FI
+    );
+
+    // 11. TX Configuration Control Command
+    writeCmd(
+        RFM_TX_CONTROL_MOD_60 |
+        RFM_TX_CONTROL_POW_0
+    );
+
+
+    // 12. PLL Setting Command
+    writeCmd(
+        0xCC77 & ~0x01 // Setting the PLL bandwith, less noise, but max bitrate capped at 86.2
+        // I think this will slow down the pll's reaction time. Not sure, check with someone!
+    );
+
+    changeMode(RX);
+    resetRX();
+    status();
+}
+
+/* Write a command to the RF Module */
+unsigned int RF12B::writeCmd(unsigned int cmd) {
+    NCS = 0;
+    unsigned int recv = spi.write(cmd);
+    NCS = 1;
+    return recv;
+}
+
+/* Sends a byte of data across RF */
+void RF12B::send(unsigned char data) {
+    while (NIRQ);
+    writeCmd(0xB800 + data);
+}
+
+/* Change the mode of the RF module to Transmitting or Receiving */
+void RF12B::changeMode(rfmode_t _mode) {
+    mode = _mode;
+    if (_mode == TX) {
+        writeCmd(0x8239); //!er,!ebb,ET,ES,EX,!eb,!ew,DC
+    } else { /* mode == RX */
+        writeCmd(0x8299); //er,!ebb,ET,ES,EX,!eb,!ew,DC
+    }
+}
+
+/* Interrupt routine for data reception */
+void RF12B::rxISR() {
+    
+    //static int cnt = 0;
+    //printf("%d hits\r\n", cnt);
+    //cnt++;
+    
+    //DBG2 = !(cnt%3);
+    //DBG3 = !((cnt+1)%3);
+    //DBG4 = !((cnt+2)%3);
+
+    unsigned int data = 0;
+    static int i = -2;
+    static unsigned char packet_length = 0;
+    static unsigned char crc = 0;
+
+    //Loop while interrupt is asserted
+    while (!NIRQ_in && mode == RX) {
+        
+        // Grab the packet's length byte
+        if (i == -2) {
+            data = writeCmd(0x0000);
+            if ( (data&0x8000) ) {
+                data = writeCmd(0xB000);
+                packet_length = (data&0x00FF);
+                crc = crc8(crc, packet_length);
+                i++;
+            }
+        }
+
+        //If we exhaust the interrupt, exit
+        if (NIRQ_in)
+            break;
+
+        // Check that packet length was correct
+        if (i == -1) {
+            data = writeCmd(0x0000);
+            if ( (data&0x8000) ) {
+                data = writeCmd(0xB000);
+                unsigned char crcofsize = (data&0x00FF);
+                if (crcofsize != crc) {
+                    //It was wrong, start over
+                    i = -2;
+                    packet_length = 0;
+                    crc = 0;
+                    //temp = queue<unsigned char>();
+                    resetRX();
+                } else {
+                    crc = crc8(crc, crcofsize);
+                    i++;
+                }
+            }
+        }
+
+        //If we exhaust the interrupt, exit
+        if (NIRQ_in)
+            break;
+
+        // Grab the packet's data 
+        if (i >= 0 && i < packet_length) {
+            data = writeCmd(0x0000);
+            if ( (data&0x8000) ) {
+                data = writeCmd(0xB000);
+                //temp.push(data&0x00FF);
+                crc = crc8(crc, (unsigned char)(data&0x00FF));
+                i++;
+            }
+        }
+
+        //If we exhaust the interrupt, exit
+        if (NIRQ_in)
+            break;
+
+        if (i >= packet_length) {
+            data = writeCmd(0x0000);
+            if ( (data&0x8000) ) {
+                data = writeCmd(0xB000);
+                if ((unsigned char)(data & 0x00FF) == crc) {
+                    //If the checksum is correct, add our data to the end of the output buffer
+                    //while (!temp.empty()) {
+                    //    fifo.push(temp.front());
+                    //    temp.pop();
+                    //}
+                }
+
+                // Tell RF Module we are finished, and clean up
+                i = -2;
+                packet_length = 0;
+                crc = 0;
+                //temp = queue<unsigned char>();
+                resetRX();
+            }
+        }
+    }
+}
+
+unsigned int RF12B::status() {
+    return writeCmd(0x0000);
+}
+
+/* Tell the RF Module this packet is received and wait for the next */
+void RF12B::resetRX() {
+    writeCmd(0xCA81);
+    writeCmd(0xCA83);
+};
+
+/* Calculate CRC8 */
+unsigned char RF12B::crc8(unsigned char crc, unsigned char data) {
+    crc = crc ^ data;
+    for (int i = 0; i < 8; i++) {
+        if (crc & 0x01) {
+            crc = (crc >> 1) ^ 0x8C;
+        } else {
+            crc >>= 1;
+        }
+    }
+    return crc;
+}
\ No newline at end of file

diff -r 000000000000 -r fbfafa6bf5f9 Kalman/Sonar/RF12B/RF12B.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Sonar/RF12B/RF12B.h	Fri Apr 20 21:32:24 2012 +0000
@@ -0,0 +1,75 @@
+#ifndef _RF12B_H
+#define _RF12B_H
+
+#include "mbed.h"
+#include <queue>
+
+enum rfmode_t{RX, TX};
+
+class RF12B {
+public:
+    /* Constructor */
+    RF12B(PinName SDI,
+          PinName SDO,
+          PinName SCK,
+          PinName NCS,
+          PinName NIRQ);
+          
+          
+    /* Reads a packet of data. Returns false if read failed. Use available() to check how much space to allocate for buffer */
+    bool read(unsigned char* data, unsigned int size);
+
+    /* Reads a byte of data from the receive buffer 
+        Returns 0xFF if there is no data */
+    unsigned char read();
+
+    /* Transmits a packet of data */
+    void write(unsigned char* data, unsigned char length);
+    void write(unsigned char data); /* 1-byte packet */
+    void write(std::queue<char> &data, int length = -1); /* sends a whole queue */
+    
+    /* Returns the packet length if data is available in the receive buffer, 0 otherwise*/
+    unsigned int available();
+
+protected:
+    /* Receive FIFO buffer */
+    std::queue<unsigned char> fifo;
+    std::queue<unsigned char> temp; //for storing stuff mid-packet
+    
+    /* SPI module */
+    SPI spi;
+    
+    /* Other digital pins */
+    DigitalOut NCS;
+    InterruptIn NIRQ;
+    DigitalIn NIRQ_in;
+    //DigitalOut rfled;
+    
+    rfmode_t mode;
+
+    /* Initialises the RF12B module */
+    void init();
+
+    /* Write a command to the RF Module */
+    unsigned int writeCmd(unsigned int cmd);
+    
+    /* Sends a byte of data across RF */
+    void send(unsigned char data);
+    
+    /* Switch module between receive and transmit modes */
+    void changeMode(rfmode_t mode);
+    
+    /* Interrupt routine for data reception */
+    void rxISR();
+    
+    /* Tell the RF Module this packet is received and wait for the next */
+    void resetRX();
+    
+    /* Return the RF Module Status word */
+    unsigned int status();
+    
+    /* Calculate CRC8 */
+    unsigned char crc8(unsigned char crc, unsigned char data);
+};
+
+#endif /* _RF12B_H */
\ No newline at end of file

diff -r 000000000000 -r fbfafa6bf5f9 Kalman/Sonar/RF12B/RFSerial.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Sonar/RF12B/RFSerial.cpp	Fri Apr 20 21:32:24 2012 +0000
@@ -0,0 +1,25 @@
+
+/* Constructor */
+#include "RFSerial.h"
+
+RFSerial::RFSerial(PinName _SDI,
+                   PinName _SDO,
+                   PinName _SCK,
+                   PinName _NCS,
+                   PinName _NIRQ)
+:RF12B(_SDI, _SDO, _SCK, _NCS, _NIRQ) {
+    
+}
+
+// Stream implementation functions
+int RFSerial::_putc(int value) {
+    RF12B::write((unsigned char) value);
+    return value;
+}
+int RFSerial::_getc() {
+    if(available()) {
+        return RF12B::read();
+    } else {
+        return EOF;
+    }
+}
\ No newline at end of file

diff -r 000000000000 -r fbfafa6bf5f9 Kalman/Sonar/RF12B/RFSerial.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Sonar/RF12B/RFSerial.h	Fri Apr 20 21:32:24 2012 +0000
@@ -0,0 +1,17 @@
+#include "mbed.h"
+#include "RF12B.h"
+
+class RFSerial : public Stream, public RF12B {
+public:
+    /* Constructor */
+    RFSerial(PinName _SDI,
+          PinName _SDO,
+          PinName _SCK,
+          PinName _NCS,
+          PinName _NIRQ);
+     
+protected:
+    // Stream implementation functions
+    virtual int _putc(int value);
+    virtual int _getc();    
+};
\ No newline at end of file

diff -r 000000000000 -r fbfafa6bf5f9 Kalman/Sonar/RF12B/RF_defs.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Sonar/RF12B/RF_defs.h	Fri Apr 20 21:32:24 2012 +0000
@@ -0,0 +1,478 @@
+/*
+ *  Open HR20
+ *
+ *  target:     ATmega169 @ 4 MHz in Honnywell Rondostat HR20E
+ *
+ *  compiler:   WinAVR-20071221
+ *              avr-libc 1.6.0
+ *              GCC 4.2.2
+ *
+ *  copyright:  2008 Dario Carluccio (hr20-at-carluccio-dot-de)
+ *              2008 Jiri Dobry (jdobry-at-centrum-dot-cz)
+ *              2008 Mario Fischer (MarioFischer-at-gmx-dot-net)
+ *              2007 Michael Smola (Michael-dot-Smola-at-gmx-dot-net)
+ *
+ *  license:    This program is free software; you can redistribute it and/or
+ *              modify it under the terms of the GNU Library General Public
+ *              License as published by the Free Software Foundation; either
+ *              version 2 of the License, or (at your option) any later version.
+ *
+ *              This program is distributed in the hope that it will be useful,
+ *              but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *              MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ *              GNU General Public License for more details.
+ *
+ *              You should have received a copy of the GNU General Public License
+ *              along with this program. If not, see http:*www.gnu.org/licenses
+ */
+
+/*
+ * \file       rfm.h
+ * \brief      functions to control the RFM12 Radio Transceiver Module
+ * \author     Mario Fischer <MarioFischer-at-gmx-dot-net>; Michael Smola <Michael-dot-Smola-at-gmx-dot-net>
+ * \date       $Date: 2010/04/17 17:57:02 $
+ * $Rev: 260 $
+ */
+
+
+//#pragma once // multi-iclude prevention. gcc knows this pragma
+#ifndef rfm_H
+#define rfm_H
+
+
+#define RFM_SPI_16(OUTVAL)            rfm_spi16(OUTVAL) //<! a function that gets a uint16_t (clocked out value) and returns a uint16_t (clocked in value)
+
+#define RFM_CLK_OUTPUT 0
+
+/*
+#define RFM_TESTPIN_INIT
+#define RFM_TESTPIN_ON
+#define RFM_TESTPIN_OFF
+#define RFM_TESTPIN_TOG
+
+#define RFM_CONFIG_DISABLE            0x00 //<! RFM_CONFIG_*** are combinable flags, what the RFM shold do
+#define RFM_CONFIG_BROADCASTSTATUS    0x01 //<! Flag that enables the HR20's status broadcast every minute
+
+#define RFM_CONFIG_ENABLEALL        0xff
+*/
+
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// RFM status bits
+//
+///////////////////////////////////////////////////////////////////////////////
+
+// Interrupt bits, latched ////////////////////////////////////////////////////
+
+#define RFM_STATUS_FFIT 0x8000 // RX FIFO reached the progr. number of bits
+                               // Cleared by any FIFO read method
+
+#define RFM_STATUS_RGIT 0x8000 // TX register is ready to receive
+                               // Cleared by TX write
+
+#define RFM_STATUS_POR  0x4000 // Power On reset
+                               // Cleared by read status
+
+#define RFM_STATUS_RGUR 0x2000 // TX register underrun, register over write
+                               // Cleared by read status
+
+#define RFM_STATUS_FFOV 0x2000 // RX FIFO overflow
+                               // Cleared by read status
+
+#define RFM_STATUS_WKUP 0x1000 // Wake up timer overflow
+                               // Cleared by read status
+
+#define RFM_STATUS_EXT  0x0800 // Interupt changed to low
+                               // Cleared by read status
+
+#define RFM_STATUS_LBD  0x0400 // Low battery detect
+
+// Status bits ////////////////////////////////////////////////////////////////
+
+#define RFM_STATUS_FFEM 0x0200 // FIFO is empty
+#define RFM_STATUS_ATS  0x0100 // TX mode: Strong enough RF signal
+#define RFM_STATUS_RSSI 0x0100 // RX mode: signal strength above programmed limit
+#define RFM_STATUS_DQD  0x0080 // Data Quality detector output
+#define RFM_STATUS_CRL  0x0040 // Clock recovery lock
+#define RFM_STATUS_ATGL 0x0020 // Toggling in each AFC cycle
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 1. Configuration Setting Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_CONFIG               0x8000
+
+#define RFM_CONFIG_EL            0x8080 // Enable TX Register
+#define RFM_CONFIG_EF            0x8040 // Enable RX FIFO buffer
+#define RFM_CONFIG_BAND_315      0x8000 // Frequency band
+#define RFM_CONFIG_BAND_433      0x8010
+#define RFM_CONFIG_BAND_868      0x8020
+#define RFM_CONFIG_BAND_915      0x8030
+#define RFM_CONFIG_X_8_5pf       0x8000 // Crystal Load Capacitor
+#define RFM_CONFIG_X_9_0pf       0x8001
+#define RFM_CONFIG_X_9_5pf       0x8002
+#define RFM_CONFIG_X_10_0pf      0x8003
+#define RFM_CONFIG_X_10_5pf      0x8004
+#define RFM_CONFIG_X_11_0pf      0x8005
+#define RFM_CONFIG_X_11_5pf      0x8006
+#define RFM_CONFIG_X_12_0pf      0x8007
+#define RFM_CONFIG_X_12_5pf      0x8008
+#define RFM_CONFIG_X_13_0pf      0x8009
+#define RFM_CONFIG_X_13_5pf      0x800A
+#define RFM_CONFIG_X_14_0pf      0x800B
+#define RFM_CONFIG_X_14_5pf      0x800C
+#define RFM_CONFIG_X_15_0pf      0x800D
+#define RFM_CONFIG_X_15_5pf      0x800E
+#define RFM_CONFIG_X_16_0pf      0x800F
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 2. Power Management Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_POWER_MANAGEMENT     0x8200
+
+#define RFM_POWER_MANAGEMENT_ER  0x8280 // Enable receiver
+#define RFM_POWER_MANAGEMENT_EBB 0x8240 // Enable base band block
+#define RFM_POWER_MANAGEMENT_ET  0x8220 // Enable transmitter
+#define RFM_POWER_MANAGEMENT_ES  0x8210 // Enable synthesizer
+#define RFM_POWER_MANAGEMENT_EX  0x8208 // Enable crystal oscillator
+#define RFM_POWER_MANAGEMENT_EB  0x8204 // Enable low battery detector
+#define RFM_POWER_MANAGEMENT_EW  0x8202 // Enable wake-up timer
+#define RFM_POWER_MANAGEMENT_DC  0x8201 // Disable clock output of CLK pin
+
+#ifndef RFM_CLK_OUTPUT
+    #error RFM_CLK_OUTPUT must be defined to 0 or 1
+#endif
+#if RFM_CLK_OUTPUT
+    #define RFM_TX_ON_PRE() RFM_SPI_16( \
+                                RFM_POWER_MANAGEMENT_ES | \
+                                RFM_POWER_MANAGEMENT_EX )
+    #define RFM_TX_ON()     RFM_SPI_16( \
+                                RFM_POWER_MANAGEMENT_ET | \
+                                RFM_POWER_MANAGEMENT_ES | \
+                                RFM_POWER_MANAGEMENT_EX )
+    #define RFM_RX_ON()     RFM_SPI_16( \
+                                RFM_POWER_MANAGEMENT_ER | \
+                                RFM_POWER_MANAGEMENT_EBB | \
+                                RFM_POWER_MANAGEMENT_ES | \
+                                RFM_POWER_MANAGEMENT_EX )
+    #define RFM_OFF()       RFM_SPI_16( \
+                                RFM_POWER_MANAGEMENT_EX )
+#else
+    #define RFM_TX_ON_PRE() RFM_SPI_16( \
+                                RFM_POWER_MANAGEMENT_DC | \
+                                RFM_POWER_MANAGEMENT_ES | \
+                                RFM_POWER_MANAGEMENT_EX )
+    #define RFM_TX_ON()     RFM_SPI_16( \
+                                RFM_POWER_MANAGEMENT_DC | \
+                                RFM_POWER_MANAGEMENT_ET | \
+                                RFM_POWER_MANAGEMENT_ES | \
+                                RFM_POWER_MANAGEMENT_EX )
+    #define RFM_RX_ON()     RFM_SPI_16( \
+                                RFM_POWER_MANAGEMENT_DC  | \
+                                RFM_POWER_MANAGEMENT_ER | \
+                                RFM_POWER_MANAGEMENT_EBB | \
+                                RFM_POWER_MANAGEMENT_ES | \
+                                RFM_POWER_MANAGEMENT_EX )
+    #define RFM_OFF()       RFM_SPI_16(RFM_POWER_MANAGEMENT_DC)
+#endif
+///////////////////////////////////////////////////////////////////////////////
+//
+// 3. Frequency Setting Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_FREQUENCY            0xA000
+
+#define RFM_FREQ_315Band(v) (uint16_t)((v/10.0-31)*4000)
+#define RFM_FREQ_433Band(v) (uint16_t)((v/10.0-43)*4000)
+#define RFM_FREQ_868Band(v) (uint16_t)((v/20.0-43)*4000)
+#define RFM_FREQ_915Band(v) (uint16_t)((v/30.0-30)*4000)
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 4. Data Rate Command
+//
+/////////////////////////////////////////////////////////////////////////////////
+
+#define RFM_BAUD_RATE            9600
+
+#define RFM_DATA_RATE            0xC600
+
+#define RFM_DATA_RATE_CS         0xC680
+#define RFM_DATA_RATE_4800       0xC647
+#define RFM_DATA_RATE_9600       0xC623
+#define RFM_DATA_RATE_19200      0xC611
+#define RFM_DATA_RATE_38400      0xC608
+#define RFM_DATA_RATE_57600      0xC605
+
+#define RFM_SET_DATARATE(baud)        ( ((baud)<5400) ? (RFM_DATA_RATE_CS|((43104/(baud))-1)) : (RFM_DATA_RATE|((344828UL/(baud))-1)) )
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 5. Receiver Control Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_RX_CONTROL           0x9000
+
+#define RFM_RX_CONTROL_P20_INT   0x9000 // Pin20 = ExternalInt
+#define RFM_RX_CONTROL_P20_VDI   0x9400 // Pin20 = VDI out
+
+#define RFM_RX_CONTROL_VDI_FAST  0x9000 // fast   VDI Response time
+#define RFM_RX_CONTROL_VDI_MED   0x9100 // medium
+#define RFM_RX_CONTROL_VDI_SLOW  0x9200 // slow
+#define RFM_RX_CONTROL_VDI_ON    0x9300 // Always on
+
+#define RFM_RX_CONTROL_BW_400    0x9020 // bandwidth 400kHz
+#define RFM_RX_CONTROL_BW_340    0x9040 // bandwidth 340kHz
+#define RFM_RX_CONTROL_BW_270    0x9060 // bandwidth 270kHz
+#define RFM_RX_CONTROL_BW_200    0x9080 // bandwidth 200kHz
+#define RFM_RX_CONTROL_BW_134    0x90A0 // bandwidth 134kHz
+#define RFM_RX_CONTROL_BW_67     0x90C0 // bandwidth 67kHz
+
+#define RFM_RX_CONTROL_GAIN_0    0x9000 // LNA gain  0db
+#define RFM_RX_CONTROL_GAIN_6    0x9008 // LNA gain -6db
+#define RFM_RX_CONTROL_GAIN_14   0x9010 // LNA gain -14db
+#define RFM_RX_CONTROL_GAIN_20   0x9018 // LNA gain -20db
+
+#define RFM_RX_CONTROL_RSSI_103  0x9000 // DRSSI threshold -103dbm
+#define RFM_RX_CONTROL_RSSI_97   0x9001 // DRSSI threshold -97dbm
+#define RFM_RX_CONTROL_RSSI_91   0x9002 // DRSSI threshold -91dbm
+#define RFM_RX_CONTROL_RSSI_85   0x9003 // DRSSI threshold -85dbm
+#define RFM_RX_CONTROL_RSSI_79   0x9004 // DRSSI threshold -79dbm
+#define RFM_RX_CONTROL_RSSI_73   0x9005 // DRSSI threshold -73dbm
+//#define RFM_RX_CONTROL_RSSI_67   0x9006 // DRSSI threshold -67dbm // RF12B reserved
+//#define RFM_RX_CONTROL_RSSI_61   0x9007 // DRSSI threshold -61dbm // RF12B reserved
+
+#define RFM_RX_CONTROL_BW(baud)        (((baud)<8000) ? \
+                                    RFM_RX_CONTROL_BW_67 : \
+                                    ( \
+                                        ((baud)<30000) ? \
+                                        RFM_RX_CONTROL_BW_134 : \
+                                        RFM_RX_CONTROL_BW_200 \
+                                    ))
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 6. Data Filter Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_DATA_FILTER          0xC228
+
+#define RFM_DATA_FILTER_AL       0xC2A8 // clock recovery auto-lock
+#define RFM_DATA_FILTER_ML       0xC268 // clock recovery fast mode
+#define RFM_DATA_FILTER_DIG      0xC228 // data filter type digital
+#define RFM_DATA_FILTER_ANALOG   0xC238 // data filter type analog
+#define RFM_DATA_FILTER_DQD(level) (RFM_DATA_FILTER | (level & 0x7))
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 7. FIFO and Reset Mode Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_FIFO                 0xCA00
+
+#define RFM_FIFO_AL              0xCA04 // FIFO Start condition sync-word/always
+#define RFM_FIFO_FF              0xCA02 // Enable FIFO fill
+#define RFM_FIFO_DR              0xCA01 // Disable hi sens reset mode
+#define RFM_FIFO_IT(level)       (RFM_FIFO | (( (level) & 0xF)<<4))
+
+#define RFM_FIFO_OFF()            RFM_SPI_16(RFM_FIFO_IT(8) |               RFM_FIFO_DR)
+#define RFM_FIFO_ON()             RFM_SPI_16(RFM_FIFO_IT(8) | RFM_FIFO_FF | RFM_FIFO_DR)
+
+/////////////////////////////////////////////////////////////////////////////
+//
+// 8. Receiver FIFO Read
+//
+/////////////////////////////////////////////////////////////////////////////
+
+#define RFM_READ_FIFO()           (RFM_SPI_16(0xB000) & 0xFF)
+
+/////////////////////////////////////////////////////////////////////////////
+//
+// 9. AFC Command
+//
+/////////////////////////////////////////////////////////////////////////////
+
+#define RFM_AFC                  0xC400
+
+#define RFM_AFC_EN               0xC401
+#define RFM_AFC_OE               0xC402
+#define RFM_AFC_FI               0xC404
+#define RFM_AFC_ST               0xC408
+
+// Limits the value of the frequency offset register to the next values:
+
+#define RFM_AFC_RANGE_LIMIT_NO    0xC400 // 0: No restriction
+#define RFM_AFC_RANGE_LIMIT_15_16 0xC410 // 1: +15 fres to -16 fres
+#define RFM_AFC_RANGE_LIMIT_7_8   0xC420 // 2: +7 fres to -8 fres
+#define RFM_AFC_RANGE_LIMIT_3_4   0xC430 // 3: +3 fres to -4 fres
+
+// fres=2.5 kHz in 315MHz and 433MHz Bands
+// fres=5.0 kHz in 868MHz Band
+// fres=7.5 kHz in 915MHz Band
+
+#define RFM_AFC_AUTO_OFF         0xC400 // 0: Auto mode off (Strobe is controlled by microcontroller)
+#define RFM_AFC_AUTO_ONCE        0xC440 // 1: Runs only once after each power-up
+#define RFM_AFC_AUTO_VDI         0xC480 // 2: Keep the foffset only during receiving(VDI=high)
+#define RFM_AFC_AUTO_INDEPENDENT 0xC4C0 // 3: Keep the foffset value independently trom the state of the VDI signal
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 10. TX Configuration Control Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_TX_CONTROL           0x9800
+
+#define RFM_TX_CONTROL_POW_0     0x9800
+#define RFM_TX_CONTROL_POW_3     0x9801
+#define RFM_TX_CONTROL_POW_6     0x9802
+#define RFM_TX_CONTROL_POW_9     0x9803
+#define RFM_TX_CONTROL_POW_12    0x9804
+#define RFM_TX_CONTROL_POW_15    0x9805
+#define RFM_TX_CONTROL_POW_18    0x9806
+#define RFM_TX_CONTROL_POW_21    0x9807
+#define RFM_TX_CONTROL_MOD_15    0x9800
+#define RFM_TX_CONTROL_MOD_30    0x9810
+#define RFM_TX_CONTROL_MOD_45    0x9820
+#define RFM_TX_CONTROL_MOD_60    0x9830
+#define RFM_TX_CONTROL_MOD_75    0x9840
+#define RFM_TX_CONTROL_MOD_90    0x9850
+#define RFM_TX_CONTROL_MOD_105   0x9860
+#define RFM_TX_CONTROL_MOD_120   0x9870
+#define RFM_TX_CONTROL_MOD_135   0x9880
+#define RFM_TX_CONTROL_MOD_150   0x9890
+#define RFM_TX_CONTROL_MOD_165   0x98A0
+#define RFM_TX_CONTROL_MOD_180   0x98B0
+#define RFM_TX_CONTROL_MOD_195   0x98C0
+#define RFM_TX_CONTROL_MOD_210   0x98D0
+#define RFM_TX_CONTROL_MOD_225   0x98E0
+#define RFM_TX_CONTROL_MOD_240   0x98F0
+#define RFM_TX_CONTROL_MP        0x9900
+
+#define RFM_TX_CONTROL_MOD(baud)    (((baud)<8000) ? \
+                                    RFM_TX_CONTROL_MOD_45 : \
+                                    ( \
+                                        ((baud)<20000) ? \
+                                        RFM_TX_CONTROL_MOD_60 : \
+                                        ( \
+                                            ((baud)<30000) ? \
+                                            RFM_TX_CONTROL_MOD_75 : \
+                                            ( \
+                                                ((baud)<40000) ? \
+                                                RFM_TX_CONTROL_MOD_90 : \
+                                                RFM_TX_CONTROL_MOD_120 \
+                                            ) \
+                                        ) \
+                                    ))
+
+/////////////////////////////////////////////////////////////////////////////
+//
+// 11. Transmitter Register Write Command
+//
+/////////////////////////////////////////////////////////////////////////////
+
+//#define RFM_WRITE(byte)  RFM_SPI_16(0xB800 | ((byte) & 0xFF))
+#define RFM_WRITE(byte)  RFM_SPI_16(0xB800 | (byte) )
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 12. Wake-up Timer Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_WAKEUP_TIMER         0xE000
+#define RFM_WAKEUP_SET(time)     RFM_SPI_16(RFM_WAKEUP_TIMER | (time))
+
+#define RFM_WAKEUP_480s          (RFM_WAKEUP_TIMER |(11 << 8)| 234)
+#define RFM_WAKEUP_240s          (RFM_WAKEUP_TIMER |(10 << 8)| 234)
+#define RFM_WAKEUP_120s          (RFM_WAKEUP_TIMER |(9  << 8)| 234)
+#define RFM_WAKEUP_119s          (RFM_WAKEUP_TIMER |(9  << 8)| 232)
+
+#define RFM_WAKEUP_60s           (RFM_WAKEUP_TIMER |(8 << 8) | 235)
+#define RFM_WAKEUP_59s           (RFM_WAKEUP_TIMER |(8 << 8) | 230)
+
+#define RFM_WAKEUP_30s           (RFM_WAKEUP_TIMER |(7 << 8) | 235)
+#define RFM_WAKEUP_29s           (RFM_WAKEUP_TIMER |(7 << 8) | 227)
+
+#define RFM_WAKEUP_8s            (RFM_WAKEUP_TIMER |(5 << 8) | 250)
+#define RFM_WAKEUP_7s            (RFM_WAKEUP_TIMER |(5 << 8) | 219)
+#define RFM_WAKEUP_6s            (RFM_WAKEUP_TIMER |(6 << 8) |  94)
+#define RFM_WAKEUP_5s            (RFM_WAKEUP_TIMER |(5 << 8) | 156)
+#define RFM_WAKEUP_4s            (RFM_WAKEUP_TIMER |(5 << 8) | 125)
+#define RFM_WAKEUP_1s            (RFM_WAKEUP_TIMER |(2 << 8) | 250)
+#define RFM_WAKEUP_900ms         (RFM_WAKEUP_TIMER |(2 << 8) | 225)
+#define RFM_WAKEUP_800ms         (RFM_WAKEUP_TIMER |(2 << 8) | 200)
+#define RFM_WAKEUP_700ms         (RFM_WAKEUP_TIMER |(2 << 8) | 175)
+#define RFM_WAKEUP_600ms         (RFM_WAKEUP_TIMER |(2 << 8) | 150)
+#define RFM_WAKEUP_500ms         (RFM_WAKEUP_TIMER |(2 << 8) | 125)
+#define RFM_WAKEUP_400ms         (RFM_WAKEUP_TIMER |(2 << 8) | 100)
+#define RFM_WAKEUP_300ms         (RFM_WAKEUP_TIMER |(2 << 8) |  75)
+#define RFM_WAKEUP_200ms         (RFM_WAKEUP_TIMER |(2 << 8) |  50)
+#define RFM_WAKEUP_100ms         (RFM_WAKEUP_TIMER |(2 << 8) |  25)
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 13. Low Duty-Cycle Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_LOW_DUTY_CYCLE       0xC800
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 14. Low Battery Detector Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_LOW_BATT_DETECT           0xC000
+#define RFM_LOW_BATT_DETECT_D_1MHZ    0xC000
+#define RFM_LOW_BATT_DETECT_D_1_25MHZ 0xC020
+#define RFM_LOW_BATT_DETECT_D_1_66MHZ 0xC040
+#define RFM_LOW_BATT_DETECT_D_2MHZ    0xC060
+#define RFM_LOW_BATT_DETECT_D_2_5MHZ  0xC080
+#define RFM_LOW_BATT_DETECT_D_3_33MHZ 0xC0A0
+#define RFM_LOW_BATT_DETECT_D_5MHZ    0xC0C0
+#define RFM_LOW_BATT_DETECT_D_10MHZ   0xC0E0
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 15. Status Read Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_READ_STATUS()       RFM_SPI_16(0x0000)
+#define RFM_READ_STATUS_FFIT()  SPI_1 (0x00)
+#define RFM_READ_STATUS_RGIT    RFM_READ_STATUS_FFIT
+
+///////////////////////////////////////////////////////////////////////////////
+
+// RFM air protocol flags:
+
+#define RFMPROTO_FLAGS_BITASK_PACKETTYPE        0b11000000 //!< the uppermost 2 bits of the flags field encode the packettype
+#define RFMPROTO_FLAGS_PACKETTYPE_BROADCAST        0b00000000 //!< broadcast packettype (message from hr20, protocol; step 1)
+#define RFMPROTO_FLAGS_PACKETTYPE_COMMAND        0b01000000 //!< command packettype (message to hr20, protocol; step 2)
+#define RFMPROTO_FLAGS_PACKETTYPE_REPLY            0b10000000 //!< reply packettype (message from hr20, protocol; step 3)
+#define RFMPROTO_FLAGS_PACKETTYPE_SPECIAL        0b11000000 //!< currently unused packettype
+
+#define RFMPROTO_FLAGS_BITASK_DEVICETYPE        0b00011111 //!< the lowermost 5 bytes denote the device type. this way other sensors and actors may coexist
+#define RFMPROTO_FLAGS_DEVICETYPE_OPENHR20        0b00010100 //!< topen HR20 device type. 10100 is for decimal 20
+
+#define RFMPROTO_IS_PACKETTYPE_BROADCAST(FLAGS)    ( RFMPROTO_FLAGS_PACKETTYPE_BROADCAST == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) )
+#define RFMPROTO_IS_PACKETTYPE_COMMAND(FLAGS)    ( RFMPROTO_FLAGS_PACKETTYPE_COMMAND   == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) )
+#define RFMPROTO_IS_PACKETTYPE_REPLY(FLAGS)        ( RFMPROTO_FLAGS_PACKETTYPE_REPLY     == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) )
+#define RFMPROTO_IS_PACKETTYPE_SPECIAL(FLAGS)    ( RFMPROTO_FLAGS_PACKETTYPE_SPECIAL   == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) )
+#define RFMPROTO_IS_DEVICETYPE_OPENHR20(FLAGS)    ( RFMPROTO_FLAGS_DEVICETYPE_OPENHR20  == ((FLAGS) & RFMPROTO_FLAGS_BITASK_DEVICETYPE) )
+
+///////////////////////////////////////////////////////////////////////////////
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r fbfafa6bf5f9 Kalman/Sonar/RFSRF05.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Sonar/RFSRF05.cpp	Fri Apr 20 21:32:24 2012 +0000
@@ -0,0 +1,139 @@
+
+#include "RFSRF05.h"
+#include "mbed.h"
+#include "globals.h"
+
+RFSRF05::RFSRF05(PinName trigger,
+                 PinName echo0,
+                 PinName echo1,
+                 PinName echo2,
+                 PinName echo3,
+                 PinName echo4,
+                 PinName echo5,
+                 PinName SDI,
+                 PinName SDO,
+                 PinName SCK,
+                 PinName NCS,
+                 PinName NIRQ)
+        : _rf(SDI,SDO,SCK,NCS,NIRQ),
+        _trigger(trigger),
+        _echo0(echo0),
+        _echo1(echo1),
+        _echo2(echo2),
+        _echo3(echo3),
+        _echo4(echo4),
+        _echo5(echo5) {
+        
+    // initialises codes
+    _code[0] = CODE0;
+    _code[1] = CODE1;
+    _code[2] = CODE2;
+
+    //set callback execute to true
+    ValidPulse = false;
+
+    // Attach interrupts
+    _echo0.rise(this, &RFSRF05::_rising);
+    _echo0.fall(this, &RFSRF05::_falling);
+    _echo1.fall(this, &RFSRF05::_falling);
+    _echo2.fall(this, &RFSRF05::_falling);
+    _echo3.fall(this, &RFSRF05::_falling);
+    _echo4.fall(this, &RFSRF05::_falling);
+    _echo5.fall(this, &RFSRF05::_falling);
+
+
+    //init callabck function
+    callbackfunc = NULL;
+    callbackobj = NULL;
+    mcallbackfunc = NULL;
+
+    // innitialises beacon counter
+    _beacon_counter = 0;
+
+    //Interrupts every 50ms
+    _ticker.attach(this, &RFSRF05::_startRange, 0.05);
+}
+
+
+void RFSRF05::_startRange() {
+
+    //printf("Srange\r\r");
+
+    // increments counter
+    _beacon_counter = (_beacon_counter + 1) % 3;
+
+
+    // set flags
+    ValidPulse = false;
+    expValidPulse = true;
+
+    // writes code to RF port
+    _rf.write(_code[_beacon_counter]);
+    
+
+
+    // send a trigger pulse, 10uS long
+    _trigger = 1;
+    wait_us (10);
+    _trigger = 0;
+
+}
+
+
+// Clear and start the timer at the begining of the echo pulse
+void RFSRF05::_rising(void) {
+
+    _timer.reset();
+    _timer.start();
+    
+    //Set callback execute to ture
+    if (expValidPulse) {
+        ValidPulse = true;
+        expValidPulse = false;
+    }
+}
+
+// Stop and read the timer at the end of the pulse
+void RFSRF05::_falling(void) {
+    _timer.stop();
+
+    if (ValidPulse) {
+        //printf("Validpulse trig!\r\n");
+        ValidPulse = false;
+
+        //Calucate distance
+        _dist[_beacon_counter] =  _timer.read_us()/2.9 + 300;
+
+        if (callbackfunc)
+            (*callbackfunc)(_beacon_counter, _dist[_beacon_counter]);
+
+        if (callbackobj && mcallbackfunc)
+            (callbackobj->*mcallbackfunc)(_beacon_counter, _dist[_beacon_counter], sonarvariance);
+
+    }
+
+}
+
+float RFSRF05::read0() {
+    // returns distance
+    return (_dist[0]);
+}
+
+float RFSRF05::read1() {
+    // returns distance
+    return (_dist[1]);
+}
+
+float RFSRF05::read2() {
+    // returns distance
+    return (_dist[2]);
+}
+
+float RFSRF05::read(unsigned int beaconnum) {
+    // returns distance
+    return (_dist[beaconnum]);
+}
+
+//SRF05::operator float() {
+//    return read();
+//}

diff -r 000000000000 -r fbfafa6bf5f9 Kalman/Sonar/RFSRF05.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Sonar/RFSRF05.h	Fri Apr 20 21:32:24 2012 +0000
@@ -0,0 +1,91 @@
+
+#ifndef MBED_RFSRF05_H
+#define MBED_RFSRF05_H
+
+#include "mbed.h"
+#include "RF12B.h"
+
+#define CODE0 0x22
+#define CODE1 0x44
+#define CODE2 0x88
+
+/* SAMPLE IMPLEMENTATION!
+RFSRF05 my_srf(p13,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9);
+
+
+void callbinmain(int num, float dist) {
+    //Here is where you deal with your brand new reading ;D
+}
+
+int main() {
+    pc.printf("Hello World of RobotSonar!\r\n");
+    my_srf.callbackfunc = callbinmain;
+    
+    while (1);
+}
+
+ */
+ 
+class DummyCT;
+ 
+class RFSRF05 {
+public:
+
+    RFSRF05(
+    PinName trigger, 
+    PinName echo0,
+    PinName echo1,
+    PinName echo2,
+    PinName echo3,
+    PinName echo4,
+    PinName echo5,
+    PinName SDI,
+    PinName SDO,
+    PinName SCK,
+    PinName NCS,
+    PinName NIRQ);
+    
+    /** A non-blocking function that will return the last measurement
+     *
+     * @returns floating point representation of distance in mm
+     */
+    float read0();
+    float read1();
+    float read2();
+    float read(unsigned int beaconnum);
+
+    
+    /** A assigns a callback function when a new reading is available **/
+    void (*callbackfunc)(int beaconnum, float distance);
+    DummyCT* callbackobj;
+    void (DummyCT::*mcallbackfunc)(int beaconnum, float distance, float variance);
+    
+    //triggers a read
+    
+
+    /** A short hand way of using the read function */
+    //operator float();
+    
+private :
+    RF12B _rf;
+    DigitalOut _trigger;
+    InterruptIn _echo0;
+    InterruptIn _echo1;
+    InterruptIn _echo2;
+    InterruptIn _echo3;
+    InterruptIn _echo4;
+    InterruptIn _echo5;
+    Timer _timer;
+    Ticker _ticker;
+    void _startRange();
+    void _rising (void);
+    void _falling (void);
+    float _dist[3];
+    char _code[3];
+    int _beacon_counter;
+    bool ValidPulse;
+    bool expValidPulse;
+    
+};
+
+#endif

diff -r 000000000000 -r fbfafa6bf5f9 Kalman/tvmet.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/tvmet.lib	Fri Apr 20 21:32:24 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/madcowswe/code/tvmet/#feb4117d16d8

diff -r 000000000000 -r fbfafa6bf5f9 TSH.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TSH.h	Fri Apr 20 21:32:24 2012 +0000
@@ -0,0 +1,62 @@
+#ifndef TSH_H
+#define TSH_H
+
+#include "rtos.h"
+
+//Thread Safe Hardware
+
+class TSI2C : public I2C {
+public:
+
+    TSI2C( PinName sda,
+           PinName scl,
+           const char* name=NULL )
+            : I2C(sda, scl, name) { }
+
+
+    int read( int address,
+              char* data,
+              int    length,
+              bool repeated = false ) {
+
+        rlock.lock();
+        int retval = I2C::read(address, data, length, repeated);
+        rlock.unlock();
+
+        return retval;
+    }
+
+    int read(int ack) {
+        rlock.lock();
+        int retval = I2C::read(ack);
+        rlock.unlock();
+
+        return retval;
+    }
+
+    int write( int address,
+               const char*    data,
+               int length,
+               bool repeated = false ) {
+
+        wlock.lock();
+        int retval = I2C::write(address, data, length, repeated);
+        wlock.unlock();
+
+        return retval;
+    }
+
+    int write(int data) {
+        wlock.lock();
+        int retval = I2C::write(data);
+        wlock.unlock();
+
+        return retval;
+    }
+
+private:
+    Mutex rlock;
+    Mutex wlock;
+};
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r fbfafa6bf5f9 geometryfuncs.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/geometryfuncs.h	Fri Apr 20 21:32:24 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

diff -r 000000000000 -r fbfafa6bf5f9 globals.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/globals.h	Fri Apr 20 21:32:24 2012 +0000
@@ -0,0 +1,63 @@
+#ifndef GLOBALS_H
+#define GLOBALS_H
+
+#include "mbed.h"
+
+#define PI 3.14159265
+
+//Robot constants
+//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;
+
+//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 {
+    int x;
+    int y;
+};
+const pos beaconpos[] = {{3000, 1000},{0,0}, {0,2000}};
+
+//System constants
+const int PREDICTPERIOD = 20; //ms
+
+//High speed serial port
+extern Serial pc;
+
+//I2C mutex
+//extern Mutex i2c_rlock;
+//extern Mutex i2c_wlock;
+
+
+// IR angle calc
+#define RELI_BOUND_LOW          4
+#define RELI_BOUND_HIGH         25
+
+// Localization estimate tolerences
+#define POSITION_TOR            50
+#define ANGLE_TOR               0.15
+
+// motion control
+#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
+
+// Task suspend periods
+#define IR_TURRET_PERIOD        200
+#define MOTION_UPDATE_PERIOD    20
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r fbfafa6bf5f9 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 20 21:32:24 2012 +0000
@@ -0,0 +1,379 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "TSH.h"
+#include "Kalman.h"
+#include "globals.h"
+#include "motors.h"
+#include "math.h"
+#include "system.h"
+#include "geometryfuncs.h"
+
+//#include <iostream>
+
+//Interface declaration
+//I2C i2c(p28, p27);        // sda, scl
+TSI2C i2c(p28, p27);
+Serial pc(USBTX, USBRX); // tx, rx
+Serial  IRturret(p13, p14);
+
+DigitalOut     OLED1(LED1);
+DigitalOut     OLED2(LED2);
+DigitalOut     OLED3(LED3);
+DigitalOut     OLED4(LED4);
+
+Motors motors(i2c);
+Kalman kalman(motors);
+
+float targetX = 1000, targetY = 1000, targetTheta = 0;
+
+// bytes packing/unpacking for IR turret serial comm
+union IRValue_t {
+    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);
+
+
+float getAngle (float x, float y);
+void getIRValue(void const *argument);
+
+// Thread pointers
+Thread *AI_Thread_Ptr;
+Thread *Motion_Thread_Ptr;
+
+Mutex targetlock;
+bool flag_terminate = false;
+
+float temp = 0;
+
+//Main loop
+int main() {
+    pc.baud(115200);
+    IRturret.baud(115200);
+    IRturret.format(8,Serial::Odd,1);
+    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!!!!!!!!!!!!!!
+    while (1) {
+        // do nothing
+        Thread::wait(osWaitForever);
+    }
+}
+
+
+void vMotorThread(void const *argument) {
+    motors.resetEncoders();
+    while (1) {
+        motors.setSpeed(20,20);
+        Thread::wait(2000);
+        motors.stop();
+        Thread::wait(5000);
+        motors.setSpeed(-20,-20);
+        Thread::wait(2000);
+        motors.stop();
+        Thread::wait(5000);
+        motors.setSpeed(-20,20);
+        Thread::wait(2000);
+        motors.stop();
+        Thread::wait(5000);
+        motors.setSpeed(20,-20);
+        Thread::wait(2000);
+        motors.stop();
+        Thread::wait(5000);
+    }
+}
+
+
+
+void vPrintState(void const *argument) {
+    float state[3];
+    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);
+        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();
+        pc.printf("\r\n");
+        pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0], state[1],state[2]);
+        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);
+    }
+}
+
+
+// AI thread ------------------------------------
+void ai_thread (void const *argument) {
+    // goes to the mid
+    Thread::signal_wait(0x01);
+    targetlock.lock();
+    targetX     = 1500;
+    targetY     = 1000;
+    targetTheta = PI/2;
+    targetlock.unlock();
+
+    // left roll
+    Thread::signal_wait(0x01);
+    targetlock.lock();
+    targetX     = 500;
+    targetY     = 1700;
+    targetTheta = PI/2;
+    targetlock.unlock();
+
+    // mid
+    Thread::signal_wait(0x01);
+    targetlock.lock();
+    targetX     = 1500;
+    targetY     = 1000;
+    targetTheta = PI/2;
+    targetlock.unlock();
+
+    // map
+    Thread::signal_wait(0x01);
+    targetlock.lock();
+    targetX     = 1500;
+    targetY     = 1700;
+    targetTheta = PI/2;
+    targetlock.unlock();
+
+    // mid
+    Thread::signal_wait(0x01);
+    targetlock.lock();
+    targetX     = 1500;
+    targetY     = 1000;
+    targetTheta = -PI/2;
+    targetlock.unlock();
+
+    // home
+    Thread::signal_wait(0x01);
+    targetlock.lock();
+    targetX     = 500;
+    targetY     = 500;
+    targetTheta = 0;
+    targetlock.unlock();
+
+    Thread::signal_wait(0x01);
+    flag_terminate = true;
+    //OLED3 = true;
+
+    while (true) {
+        Thread::wait(osWaitForever);
+    }
+}
+
+// motion control thread ------------------------
+void motion_thread(void const *argument) {
+    motors.resetEncoders();
+    motors.setSpeed(MOVE_SPEED/2,MOVE_SPEED/2);
+    Thread::wait(1000);
+    motors.stop();
+    (*AI_Thread_Ptr).signal_set(0x01);
+
+
+
+    float currX, currY,currTheta;
+    float speedL,speedR;
+    float diffDir,diffSpeed;
+    float lastdiffSpeed = 0;
+
+    while (1) {
+        if (flag_terminate) {
+            terminate();
+        }
+
+        // get kalman localization estimate ------------------------
+        kalman.statelock.lock();
+        currX = kalman.X(0)*1000.0f;
+        currY = kalman.X(1)*1000.0f;
+        currTheta = kalman.X(2);
+        kalman.statelock.unlock();
+
+
+        // check if target reached ----------------------------------
+        if (  ( abs(currX - targetX) < POSITION_TOR )
+                &&( abs(currY - targetY) < POSITION_TOR )
+           ) {
+
+            diffDir = rectifyAng(currTheta - targetTheta);
+            diffSpeed = diffDir / PI;
+
+            if (abs(diffDir) > ANGLE_TOR) {
+                if (abs(diffSpeed) < 0.5) {
+                    diffSpeed = 0.5*diffSpeed/abs(diffSpeed);
+                }
+                motors.setSpeed( int(diffSpeed*MOVE_SPEED),  -int(diffSpeed*MOVE_SPEED));
+
+
+            } else {
+                motors.stop();
+                Thread::wait(4000);
+                (*AI_Thread_Ptr).signal_set(0x01);
+            }
+        }
+
+        // adjust motion to reach target ----------------------------
+        else {
+
+            // calc direction to target
+            float targetDir = atan2(targetY - currY, targetX - currX);
+
+
+            diffDir = rectifyAng(currTheta - targetDir);
+            diffSpeed = diffDir / PI;
+
+            if (abs(diffDir) > ANGLE_TOR*2) {
+                if (abs(diffSpeed) < 0.5) {
+                    diffSpeed = 0.5*diffSpeed/abs(diffSpeed);
+                }
+                motors.setSpeed( int(diffSpeed*MOVE_SPEED),  -int(diffSpeed*MOVE_SPEED));
+            } else {
+
+
+                if (abs(diffSpeed-lastdiffSpeed) > MAX_STEP_RATIO  ) {
+                    if (diffSpeed-lastdiffSpeed >= 0) {
+                        diffSpeed = lastdiffSpeed + MAX_STEP_RATIO;
+                    } else {
+                        diffSpeed = lastdiffSpeed - MAX_STEP_RATIO;
+                    }
+                }
+                lastdiffSpeed = diffSpeed;
+
+                // calculte the motor speeds
+                if (diffSpeed <= 0) {
+                    speedL = (1-2*abs(diffSpeed))*MOVE_SPEED;
+                    speedR = MOVE_SPEED;
+
+                } else {
+                    speedR = (1-2*abs(diffSpeed))*MOVE_SPEED;
+                    speedL = MOVE_SPEED;
+                }
+
+                motors.setSpeed( int(speedL),  int(speedR));
+            }
+        }
+
+        // wait
+        Thread::wait(MOTION_UPDATE_PERIOD);
+    }
+}
+
+void vIRValueISR (void) {
+
+    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 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;
+        }
+    }
+    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

diff -r 000000000000 -r fbfafa6bf5f9 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Apr 20 21:32:24 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/7495d544864f

diff -r 000000000000 -r fbfafa6bf5f9 motors.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motors.cpp	Fri Apr 20 21:32:24 2012 +0000
@@ -0,0 +1,179 @@
+/**********************************************************************
+ * @file        motors.cpp
+ * @purpose     Eurobot 2012 - Secondary Robot MD25 Interface
+ * @version     0.2
+ * @date        4th April 2012
+ * @author      Crispian Poon
+ * @email       pooncg@gmail.com
+ ______________________________________________________________________
+
+ Setup information:
+ 1. Put pull up 2.2k resistors to +3.3v on I2C SCL and SDA
+ 2. Connect P28 SDA to MD25 yellow cable, P27 SCL to MD25 blue cable
+
+ **********************************************************************/
+
+#include "mbed.h"
+#include "motors.h"
+#include "globals.h"
+#include "TSH.h"
+
+Motors::Motors(TSI2C &i2cin) : i2c(i2cin) {
+
+}
+
+//***************************************************************************************
+//Secondary robot specific instructions
+//***************************************************************************************
+
+void Motors::move(int distance, int speed) {
+    //resetEncoders(); TODO use kalman as feedback instead!
+
+    int tempEndEncoder = 0;
+    int startEncoderCount = 0;
+
+    tempEndEncoder = distanceToEncoder(abs(distance));
+    startEncoderCount = getEncoder1();
+
+    setSpeed(getSignOfInt(distance) * speed);
+
+    while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {
+        setSpeed(getSignOfInt(distance) * speed);
+    }
+
+    //resetEncoders();
+    stop();
+}
+
+void Motors::turn(int angle, int speed) {
+    //resetEncoders(); TODO use kalman as feedback instead!
+    int tempDistance = int((float(angle) / 360) * float(robotCircumference));
+    int tempEndEncoder = 0;
+    int startEncoderCount = 0;
+
+    tempEndEncoder = distanceToEncoder(abs(tempDistance));
+    startEncoderCount = getEncoder1();
+    setSpeed(getSignOfInt(tempDistance) * speed, -getSignOfInt(tempDistance) * speed);
+
+    while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {
+        setSpeed(getSignOfInt(tempDistance) * speed,-getSignOfInt(tempDistance) * speed);
+
+    }
+
+    //resetEncoders();
+    stop();
+}
+
+//***************************************************************************************
+//Secondary robot specific helper functions
+//***************************************************************************************
+
+
+int Motors::getSignOfInt(int direction) {
+
+    direction = (direction < 0);
+
+    switch (direction) {
+        case 1:
+            return -1;
+        case 0:
+            return 1;
+    }
+
+    return 0;
+}
+
+// returns distance in mm.
+float Motors::encoderToDistance(int encoder) {
+    return (float(encoder) / float(encoderRevCount)) * wheelmm;
+}
+
+int Motors::distanceToEncoder(float distance) {
+    return int((distance / float(wheelmm)) * encoderRevCount);
+}
+
+
+//***************************************************************************************
+//MD25 instructions
+//***************************************************************************************
+
+void Motors::stop() {
+    sendCommand(cmdSetMotor1, 0);
+    sendCommand(cmdSetMotor2, 0);
+}
+
+void Motors::setSpeed(int speed) {
+    setMode(1);
+    ///sendCommand(cmdByte, 0x30);
+    sendCommand(cmdSetMotor1, speed);
+    sendCommand(cmdSetMotor2, speed);
+}
+
+void Motors::setSpeed(int speed1, int speed2) {
+    setMode(1),
+    // sendCommand(cmdByte, 0x30);
+    sendCommand(cmdSetMotor1, speed1);
+    sendCommand(cmdSetMotor2, speed2);
+}
+
+void Motors::setMode(int mode) {
+    sendCommand(cmdSetMode, mode);
+}
+
+void Motors::resetEncoders() {
+    sendCommand(cmdByte, cmdResetEncoders);
+}
+
+int Motors::getEncoder1() {
+    return get4Bytes(cmdGetEncoder1);
+}
+
+int Motors::getEncoder2() {
+    return get4Bytes(cmdGetEncoder2);
+}
+
+void Motors::disableAcceleration() {
+    sendCommand(cmdByte, cmdDisableAcceleration);
+}
+
+
+
+//***************************************************************************************
+//Abstract MD25 communication methods and functions
+//***************************************************************************************
+
+int Motors::get4Bytes(char command) {
+    long tempWord = 0;
+    char cmd[4];
+
+    //i2c request
+    sendCommand(command);
+
+    //i2c read data back
+    i2c.read(md25Address, cmd, 4);// Request 4 bytes from MD25
+
+    //FIXED 22FEB2012 CRISPIAN Taken 0.07 delay off.
+
+    //First byte is largest, shift 4 bytes into tempWord
+    tempWord += cmd[0] << 24;
+    tempWord += cmd[1] << 16;
+    tempWord += cmd[2] << 8;
+    tempWord += cmd[3] ;
+
+    return tempWord;
+}
+
+void Motors::sendCommand(char command) {
+    char buffer[1];
+    buffer[0] = command;
+    i2c.write(md25Address, &buffer[0], 1);
+}
+
+void Motors::sendCommand(char command1, char command2 ) {
+
+    char buffer[2];
+    buffer[0] = command1;
+    buffer[1] = command2;
+
+    i2c.write(md25Address, &buffer[0], 2);
+}
\ No newline at end of file

diff -r 000000000000 -r fbfafa6bf5f9 motors.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motors.h	Fri Apr 20 21:32:24 2012 +0000
@@ -0,0 +1,47 @@
+#ifndef MOTORS_H
+#define MOTORS_H
+
+#include "mbed.h"
+#include "TSH.h"
+
+class Motors {
+public:
+    Motors(TSI2C &i2cin);//, Mutex );
+    //Motors(I2C &i2cin);
+
+    //Functions declaration
+    void disableAcceleration();
+    void resetEncoders();
+    int getEncoder1();
+    int getEncoder2();
+    void move(int distance, int speed);
+    void turn(int angle, int speed);
+    int getSignOfInt(int direction);
+    void stop();
+    void setSpeed(int speed);
+    void setSpeed(int speed1, int speed2);
+    void setMode(int mode);
+    float encoderToDistance(int encoder);
+    int distanceToEncoder(float distance);
+    void sendCommand(char command);
+    void sendCommand(char command1, char command2 );
+    int get4Bytes(char command);
+
+
+private:
+    I2C& i2c;
+    //Mutex&
+
+    //Constants declaration
+    static const int md25Address = 0xB0;
+    static const char cmdSetMotor1 = 0x00;
+    static const char cmdSetMotor2 = 0x01;
+    static const char cmdByte = 0x10;
+    static const char cmdSetMode = 0x0F;
+    static const char cmdResetEncoders = 0x20;
+    static const char cmdGetEncoder1 = 0x02;
+    static const char cmdGetEncoder2 = 0x06;
+    static const char cmdDisableAcceleration = 0x30;
+};
+
+#endif

diff -r 000000000000 -r fbfafa6bf5f9 system.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/system.cpp	Fri Apr 20 21:32:24 2012 +0000
@@ -0,0 +1,17 @@
+#include "system.h"
+
+float cpupercent; //defining the extern
+void measureCPUidle (void const* arg) {
+    
+    Timer timer;
+    cpupercent = 0; //defined in system.h
+    
+    while(1) {
+        timer.reset();
+        timer.start();
+        wait(1); // looks like wait uses a RTC
+        
+        int thistime = timer.read_us()-1000000;
+        cpupercent = thistime;
+    }
+}
\ No newline at end of file

diff -r 000000000000 -r fbfafa6bf5f9 system.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/system.h	Fri Apr 20 21:32:24 2012 +0000
@@ -0,0 +1,42 @@
+
+#ifndef SYSTEM_H
+#define SYSTEM_H
+
+#include "globals.h"
+#include "rtos.h"
+
+//a type which is a pointer to a rtos thread function
+typedef void (*tfuncptr_t)(void const *argument);
+
+//---------------------
+//Signal ticker stuff
+#define SIGTICKARGS(thread, signal) \
+    (tfuncptr_t) (&Signalsetter::callback), osTimerPeriodic, (void*)(new Signalsetter(thread, signal))
+
+class Signalsetter {
+public:
+    Signalsetter(Thread& inthread, int insignal) :
+            thread(inthread) {
+        signal = insignal;
+        //pc.printf("ptr saved as %#x \r\n", (int)(&(inthread)));
+    }
+
+    static void callback(void* thisin) {
+
+        Signalsetter* fthis = (Signalsetter*)thisin;
+        //pc.printf("callback will signal thread object at %#x \r\n", (int)(&(fthis->thread)));
+        fthis->thread.signal_set(fthis->signal);
+        //delete fthis; //this is useful for single fire tickers!
+    }
+
+private:
+    Thread& thread;
+    int signal;
+};
+
+//---------------------
+//cpu usage measurement function
+extern float cpupercent;
+void measureCPUidle (void const* arg);
+
+#endif