Revision 16:b3dd4e0b3100, committed 2012-04-28

Comitter:

narshu

Date:

Sat Apr 28 23:15:34 2012 +0000

Parent:

15:acae5c0e9ca8

Child:

17:bafcef1c3579

Commit message:

functioning code with interchangedable shared kalman lib;

Changed in this revision

--- a/Eurobot_shared/.lib	Sat Apr 28 22:21:03 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
- 
\ No newline at end of file

--- a/Eurobot_shared/Kalman/IR/IR.cpp	Sat Apr 28 22:21:03 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,74 +0,0 @@
-#include "IR.h"
-#include "Kalman.h"
-#include "system.h"
-#include "geometryfuncs.h"
-#include "globals.h"
-#include "mbed.h"
-
-IR::IR(Kalman &kalmanin):
-#ifdef ROBOT_PRIMARY
-        IRserial(p9, p10),
-#else
-        IRserial(p13, p14),
-#endif
-        kalman(kalmanin) {
-
-    //Setting up IR serial
-    IRserial.baud(115200);
-    IRserial.format(8,Serial::Odd,1);
-}
-
-void IR::detachisr() {
-    IRserial.attach(NULL,Serial::RxIrq);
-}
-
-void IR::attachisr() {
-    IRserial.attach(this, &IR::vIRValueISR, Serial::RxIrq);
-}
-
-void IR::vIRValueISR (void) {
-
-    // 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
-
-#ifdef ROBOT_PRIMARY
-    unsigned char RBR = LPC_UART3->RBR;
-#else
-    unsigned char RBR = LPC_UART1->RBR;
-#endif
-
-    // bytes packing/unpacking for IR turret serial comm
-    static union IRValue_t {
-        float IR_floats[3];
-        int IR_ints[3];
-        unsigned char IR_chars[12];
-    } IRValues;
-
-    const char Alignment_char[4] = {0xFF,0xFE,0xFD,0xFC};
-    static int Alignment_ptr = 0;
-    static bool data_flag = false;
-    static int buff_pointer = 0;
-
-    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
-                kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),IRValues.IR_floats[1],IRvariance);
-
-
-        }
-
-    }
-}
\ No newline at end of file

--- a/Eurobot_shared/Kalman/IR/IR.h	Sat Apr 28 22:21:03 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,25 +0,0 @@
-
-#ifndef IR_H
-#define IR_H
-
-#include "mbed.h"
-
-//forward declaration of class Kalman to avoid cyclic include
-class Kalman;
-
-class IR {
-public:
-
-    Serial IRserial;
-
-    IR(Kalman &kalmanin);
-    void detachisr();
-    void attachisr();
-    void vIRValueISR (void);
-
-private:
-//reference to the kalman object to run the updates on
-    Kalman& kalman;
-};
-
-#endif //IR_H
\ No newline at end of file

--- a/Eurobot_shared/Kalman/Kalman.cpp	Sat Apr 28 22:21:03 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,404 +0,0 @@
-//***************************************************************************************
-//Kalman Filter implementation
-//***************************************************************************************
-#include "Kalman.h"
-#include "rtos.h"
-#include "RFSRF05.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;
-
-Kalman::Kalman(Motors &motorsin,
-               UI &uiin,
-               PinName Sonar_Trig,
-               PinName Sonar_Echo0,
-               PinName Sonar_Echo1,
-               PinName Sonar_Echo2,
-               PinName Sonar_Echo3,
-               PinName Sonar_Echo4,
-               PinName Sonar_Echo5,
-               PinName Sonar_SDI,
-               PinName Sonar_SDO,
-               PinName Sonar_SCK,
-               PinName Sonar_NCS,
-               PinName Sonar_NIRQ) :
-        ir(*this),
-        sonararray(Sonar_Trig,
-                   Sonar_Echo0,
-                   Sonar_Echo1,
-                   Sonar_Echo2,
-                   Sonar_Echo3,
-                   Sonar_Echo4,
-                   Sonar_Echo5,
-                   Sonar_SDI,
-                   Sonar_SDO,
-                   Sonar_SCK,
-                   Sonar_NCS,
-                   Sonar_NIRQ),
-        motors(motorsin),
-        ui(uiin),
-        predictthread(predictloopwrapper, this, osPriorityNormal, 512),
-        predictticker( SIGTICKARGS(predictthread, 0x1) ),
-//        sonarthread(sonarloopwrapper, this, osPriorityNormal, 256),
-//        sonarticker( SIGTICKARGS(sonarthread, 0x1) ),
-        updatethread(updateloopwrapper, this, osPriorityNormal, 512) {
-
-    //Initilising offsets
-    InitLock.lock();
-    IR_Offset = 0;
-    Sonar_Offset = 0;
-    InitLock.unlock();
-
-
-    //Initilising matrices
-
-    // X = x, y, theta;
-    X = 0.5, 0, 0;
-
-    P = 1, 0, 0,
-        0, 1, 0,
-        0, 0, 0.04;
-
-    //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);
-
-}
-
-
-//Note: this init function assumes that the robot faces east, theta=0, in the +x direction
-void Kalman::KalmanInit() {
-    float SonarMeasuresx1000[3];
-    float IRMeasuresloc[3];
-    int beacon_cnt = 0;
-
-
-// doesn't work since they break the ISR
-/*
-#ifdef ROBOT_PRIMARY
-    LPC_UART3->FCR = LPC_UART3->FCR | 0x06;       // Flush the serial FIFO buffer / OR with FCR
-#else
-    LPC_UART1->FCR = LPC_UART1->FCR | 0x06;       // Flush the serial FIFO buffer / OR with FCR
-#endif
-*/
-    // zeros the measurements
-    for (int i = 0; i < 3; i++) {
-    SonarMeasures[i] = 0;
-    IRMeasures[i] = 0;
-    }
-    
-    InitLock.lock();
-    //zeros offsets
-    IR_Offset = 0;
-    Sonar_Offset = 0;
-    InitLock.unlock();
-    
-    // attaches ir interrup
-    ir.attachisr();
-
-    //wating untill the IR has reved up and picked up some valid data
-    wait(1);
-
-    //temporaraly disable IR updates
-    ir.detachisr();
-    
-    //lock the state throughout the computation, as we will override the state at the end
-    InitLock.lock();
-    statelock.lock();
-    
-    
-
-    SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f;
-    SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f;
-    SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f;
-    IRMeasuresloc[0] = IRMeasures[0];
-    IRMeasuresloc[1] = IRMeasures[1];
-    IRMeasuresloc[2] = IRMeasures[2];
-    //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[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 = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d);
-    float x_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j;
-
-    float Dist_Exp[3];
-    for (int i = 0; i < 3; i++) {
-        //Compute sonar offset
-        Dist_Exp[i] = hypot(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
-        Sonar_Offset += (SonarMeasuresx1000[i]-Dist_Exp[i])/3000.0f;
-        
-        //Compute IR offset
-        float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
-        // take average offset angle from valid readings
-        if (IRMeasuresloc[i] != 0) {
-            beacon_cnt ++;
-            // changed to current angle - estimated angle
-            float angle_temp = IRMeasuresloc[i] - angle_est;
-            angle_temp -= (floor(angle_temp/(2*PI)))*2*PI;
-            IR_Offset += angle_temp;
-        }
-    }
-    IR_Offset /= float(beacon_cnt);
-
-    //statelock already locked
-    X(0) = x_coor/1000.0f;
-    X(1) = y_coor/1000.0f;
-    X(2) = 0;
-    
-    // unlocks mutexes
-    InitLock.unlock();
-    statelock.unlock();
-    
-
-    //reattach the IR processing
-    ir.attachisr();
-}
-
-
-void Kalman::predictloop() {
-
-    OLED4 = !ui.regid(0, 3);
-    OLED4 = !ui.regid(1, 4);
-
-    float lastleft = 0;
-    float lastright = 0;
-
-    while (1) {
-        Thread::signal_wait(0x1);
-        OLED1 = !OLED1;
-
-        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();
-        
-        //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;
-
-        //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);
-        
-        //Generating forward and rotational variance
-        float varfwd = fwdvarperunit * abs(dright + dleft) / 2.0f;
-        float varang = varperang * abs(thetap);
-        float varxydt = xyvarpertime * PREDICTPERIOD/1000.0f;
-        float varangdt = angvarpertime * PREDICTPERIOD/1000.0f;
-
-        //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;
-        
-        //Update UI
-        float statecpy[] = {X(0), X(1), X(2)};
-        ui.updateval(0, statecpy, 3);
-        
-        float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)};
-        ui.updateval(1, Pcpy, 4);
-
-        statelock.unlock();
-    }
-}
-
-//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)
-            OLED4 = 1;
-        //    printf("putting in MQ error code %#x\r\n", putret);
-    } else {
-        OLED4 = 1;
-        //printf("MQalloc returned NULL ptr\r\n");
-    }
-
-}
-
-void Kalman::updateloop() {
-
-    //sonar Y chanels
-    ui.regid(2, 1);
-    ui.regid(3, 1);
-    ui.regid(4, 1);
-    
-    //IR Y chanels
-    ui.regid(5, 1);
-    ui.regid(6, 1);
-    ui.regid(7, 1);
-
-    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) {
-        OLED2 = !OLED2;
-
-        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) {
-
-                    InitLock.lock();
-                    float dist = value / 1000.0f - Sonar_Offset; //converting to m from mm,subtract the offset
-                    InitLock.unlock();
-                    
-                    int sonarid = type;
-                    aborton2stddev = true;
-
-                    statelock.lock();
-                    //update the current sonar readings
-                    SonarMeasures[sonarid] = dist;
-
-                    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;
-                    
-                    //send to ui
-                    ui.updateval(sonarid+2, Y);
-
-                    dhdx = rbx / expecdist;
-                    dhdy = rby / expecdist;
-
-                    H = dhdx, dhdy, 0;
-
-                } else if (type <= IR3) {
-
-                    aborton2stddev = false;
-                    int IRidx = type-3;
-                    
-                    // subtract the IR offset
-                    InitLock.lock();
-                    value -= IR_Offset;
-                    InitLock.unlock();
-
-                    statelock.lock();
-                    IRMeasures[IRidx] = value;
-
-                    rbx = X(0) - beaconpos[IRidx].x/1000.0f;
-                    rby = X(1) - beaconpos[IRidx].y/1000.0f;
-
-                    float expecang = atan2(-rby, -rbx) - X(2);
-                    Y = rectifyAng(value - expecang);
-                    
-                    //send to ui
-                    ui.updateval(IRidx + 5, Y);
-
-                    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 {
-            OLED4 = 1;
-            //printf("ERROR: in updateloop, code %#x", evt);
-        }
-
-    }
-
-}
\ No newline at end of file

--- a/Eurobot_shared/Kalman/Kalman.h	Sat Apr 28 22:21:03 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,97 +0,0 @@
-#ifndef KALMAN_H
-#define KALMAN_H
-
-#include "globals.h"
-
-
-#include "rtos.h"
-//#include "Matrix.h"
-#include "motors.h"
-#include "RFSRF05.h"
-#include "IR.h"
-#include "ui.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,
-           UI &uiin,
-           PinName Sonar_Trig,
-           PinName Sonar_Echo0,
-           PinName Sonar_Echo1,
-           PinName Sonar_Echo2,
-           PinName Sonar_Echo3,
-           PinName Sonar_Echo4,
-           PinName Sonar_Echo5,
-           PinName Sonar_SDI,
-           PinName Sonar_SDO,
-           PinName Sonar_SCK,
-           PinName Sonar_NCS,
-           PinName Sonar_NIRQ);
-
-    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];
-    float IR_Offset;
-    float Sonar_Offset;
-    Mutex InitLock;
-
-    bool Kalman_init;
-
-    //The IR is public so it's possible to print the offset in the print function
-    IR ir;
-
-    //Initialises the kalman filter
-    void KalmanInit();
-
-private:
-
-    //Sensor interfaces
-    RFSRF05 sonararray;
-    Motors& motors;
-    UI& ui;
-
-    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();
-    }
-
-
-};
-
-#endif //KALMAN_H
\ No newline at end of file

--- a/Eurobot_shared/Kalman/Sonar/RF12B/RF12B.cpp	Sat Apr 28 22:21:03 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,400 +0,0 @@
-#include "RF12B.h"
-
-#include "RF_defs.h"
-#include <algorithm>
-#include "system.h"
-#include "globals.h"
-
-
-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);
-    writeCmd(RFM_DATA_RATE_57600);
-
-
-    // 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() {
-
-    unsigned int data = 0;
-    static int i = -2;
-    static unsigned char packet_length = 0;
-    static unsigned char crc = 0;
-    #ifdef ROBOT_SECONDARY
-    static unsigned char temp;
-    #endif
-
-    //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);
-                #ifdef ROBOT_SECONDARY
-                temp = data&0x00FF;
-                #endif
-                //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);
-                     //   temp.pop();
-#ifdef ROBOT_SECONDARY
-                        if (callbackfunc)
-                            (*callbackfunc)(temp);
-
-                        if (callbackobj && mcallbackfunc)
-                            (callbackobj->*mcallbackfunc)(temp);
-#endif
-                   // }
-                }
-
-                // 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

--- a/Eurobot_shared/Kalman/Sonar/RF12B/RF12B.h	Sat Apr 28 22:21:03 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,83 +0,0 @@
-#ifndef _RF12B_H
-#define _RF12B_H
-
-#include "mbed.h"
-//#include <queue>
-
-enum rfmode_t{RX, TX};
-
-class DummyCT;
-
-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();
-    
-    /** A assigns a callback function when a new reading is available **/
-    void (*callbackfunc)(unsigned char rx_code);
-    DummyCT* callbackobj;
-    void (DummyCT::*mcallbackfunc)(unsigned char rx_code);    
-
-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

--- a/Eurobot_shared/Kalman/Sonar/RF12B/RF_defs.h	Sat Apr 28 22:21:03 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,478 +0,0 @@
-/*
- *  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

--- a/Eurobot_shared/Kalman/Sonar/RFSRF05.cpp	Sat Apr 28 22:21:03 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,183 +0,0 @@
-
-#include "RFSRF05.h"
-#include "mbed.h"
-#include "globals.h"
-#include "system.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
-    codes[0] = CODE0;
-    codes[1] = CODE1;
-    codes[2] = CODE2;
-
-    //set callback execute to true
-    ValidPulse = false;
-
-    // Attach interrupts
-#ifdef SONAR_ECHO_INV
-    // inverted sonar inputs
-    _echo5.fall(this, &RFSRF05::_rising);
-    _echo0.rise(this, &RFSRF05::_falling);
-    _echo1.rise(this, &RFSRF05::_falling);
-    _echo2.rise(this, &RFSRF05::_falling);
-    _echo3.rise(this, &RFSRF05::_falling);
-    _echo4.rise(this, &RFSRF05::_falling);
-    _echo5.rise(this, &RFSRF05::_falling);
-#else
-    _echo5.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);
-#endif
-
-
-    //init callabck function
-    callbackfunc = NULL;
-    callbackobj = NULL;
-    mcallbackfunc = NULL;
-
-    // innitialises beacon counter
-    _beacon_counter = 0;
-
-#ifdef ROBOT_PRIMARY
-    //Interrupts every 50ms for primary robot
-    _ticker.attach(this, &RFSRF05::_startRange, 0.05);
-#else
-    //attach callback
-    _rf.callbackobj = (DummyCT*)this;
-    _rf.mcallbackfunc = (void (DummyCT::*)(unsigned char rx_data)) &RFSRF05::startRange;
-#endif
-
-}
-
-#ifdef ROBOT_PRIMARY
-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(codes[_beacon_counter]);
-
-    // send a trigger pulse, 10uS long
-    _trigger = 1;
-    wait_us (10);
-    _trigger = 0;
-
-}
-#else
-
-void RFSRF05::startRange(unsigned char rx_code) {
-    for (int i = 0; i < 3; i++) {
-        if (rx_code == codes[i]) {
-       
-            // assign beacon_counter
-            _beacon_counter = i;
-
-            // set flags
-            ValidPulse = false;
-            expValidPulse = true;
-
-            // send a trigger pulse, 10uS long
-            _trigger = 1;
-            wait_us (10);
-            _trigger = 0;
-            break;
-        }
-    }
-}
-#endif
-
-// 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
-        //true offset is about 330, we put 450 so circles overlap
-        _dist[_beacon_counter] =  _timer.read_us()/2.9 + 450;
-
-        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]);
-}
-
-void RFSRF05::setCode(int code_index, unsigned char code) {
-    codes[code_index] = code;
-}
-
-//SRF05::operator float() {
-//    return read();
-//}

--- a/Eurobot_shared/Kalman/Sonar/RFSRF05.h	Sat Apr 28 22:21:03 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,102 +0,0 @@
-
-#ifndef MBED_RFSRF05_H
-#define MBED_RFSRF05_H
-
-
-
-#include "mbed.h"
-#include "RF12B.h"
-#include "globals.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
-    #ifndef ROBOT_PRIMARY
-    void startRange(unsigned char rx_code);
-    #endif
-    
-    //set codes
-    void setCode(int code_index, unsigned char code);
-    unsigned char codes[3];
-
-    /** 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;
-    #ifdef ROBOT_PRIMARY
-    void _startRange(void);
-    #endif
-    void _rising (void);
-    void _falling (void);
-    float _dist[3];
-    int _beacon_counter;
-    bool ValidPulse;
-    bool expValidPulse;
-    
-};
-
-#endif

--- a/Eurobot_shared/Motion/motion.cpp	Sat Apr 28 22:21:03 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,166 +0,0 @@
-#include "motion.h"
-#include "geometryfuncs.h"
-#include "system.h"
-
-
-Motion::Motion(Motors &motorsin, AI &aiin, Kalman &kalmanin):
-        thr_motion(mtwrapper,this,osPriorityNormal,1024),
-        motors(motorsin),
-        ai(aiin),
-        kalman(kalmanin) { }
-
-// motion control thread ------------------------
-void Motion::motion_thread() {
-    motors.resetEncoders();
-    motors.stop();
-    Thread::wait(1500);
-    ai.thr_AI.signal_set(0x01);
-
-    //PID declare
-    PID PIDControllerMotorTheta2(FWD_MOVE_P, 0, 0, MOTION_UPDATE_PERIOD/1000.0f);     //Going forward
-    PID PIDControllerMotorTheta(SPIN_MOVE_P, 0, 0, MOTION_UPDATE_PERIOD/1000.0f);    //Spinning on the spot
-
-    //PID Initialisation
-    PIDControllerMotorTheta2.setMode(MANUAL_MODE);
-    PIDControllerMotorTheta.setMode(MANUAL_MODE);
-
-    PIDControllerMotorTheta2.setBias(0);
-    PIDControllerMotorTheta.setBias(0);
-
-    PIDControllerMotorTheta2.setOutputLimits(-1, 1);
-    PIDControllerMotorTheta.setOutputLimits(-1, 1);
-
-    PIDControllerMotorTheta2.setInputLimits(-PI, PI);
-    PIDControllerMotorTheta.setInputLimits(-PI, PI);
-
-    PIDControllerMotorTheta.setSetPoint(0);
-    PIDControllerMotorTheta2.setSetPoint(0);
-
-    float currX, currY,currTheta;
-    float speedL,speedR;
-    float diffDir;
-    int atTargetFlag = 0;
-
-    while (1) {
-        kalman.statelock.lock();
-        if (ai.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();
-        
-        AI::Target loctarget = ai.gettarget();
-
-        /*
-        //PID Tuning Code
-        if (pc.readable() == 1) {
-            float cmd;
-            pc.scanf("%f", &cmd);
-            //Tune PID referece
-            PIDControllerMotorTheta2.setTunings(cmd, 0, 0);
-        }
-        */
-        
-        // check if target reached ----------------------------------
-        if (atTargetFlag || hypot(currX - loctarget.x, currY - loctarget.y) < POSITION_TOR) {
-                
-            atTargetFlag = loctarget.reached;
-            OLED4 = 1;
-            
-            diffDir = rectifyAng(currTheta - loctarget.theta);
-            //diffSpeed = diffDir / PI;
-
-            PIDControllerMotorTheta.setProcessValue(diffDir);
-            float tempPidVar = PIDControllerMotorTheta.compute();
-            motors.setSpeed( -int(tempPidVar*MOVE_SPEED),  int(tempPidVar*MOVE_SPEED));
-
-            if (abs(diffDir) < ANGLE_TOR) {
-
-                if (!loctarget.reached) {
-                    static int counter = 10;
-                    if (counter-- == 0){
-                    counter = 10;
-                    ai.target.reached = true;
-                    ai.thr_AI.signal_set(0x01);
-                    
-                    }
-                }
-            }
-        }
-
-        // adjust motion to reach target ----------------------------
-        else {
-        
-            OLED4 = 0;
-
-            // calc direction to target
-            float targetDir = atan2(loctarget.y - currY, loctarget.x - currX);
-            if (!loctarget.facing) targetDir =  targetDir + PI;
-
-            //Angle differene in -PI to PI
-            diffDir = rectifyAng(currTheta - targetDir);
-
-            //Set PID process variable
-            PIDControllerMotorTheta.setProcessValue(diffDir);
-            PIDControllerMotorTheta2.setProcessValue(diffDir);
-
-            //if diffDIr is neg, spin right
-            //if diffDir is pos, spin left
-
-            if (abs(diffDir) > ANGLE_TOR*4) {   //roughly 32 degrees
-                //ANGLE_TOR*4
-                float tempPidVar = PIDControllerMotorTheta.compute();
-                motors.setSpeed( -int(tempPidVar*MOVE_SPEED),  int(tempPidVar*MOVE_SPEED));
-                //pc.printf("spin,%f\n",diffDir);
-
-            } else {
-
-                float tempPidVar = PIDControllerMotorTheta2.compute();
-                float MoveSpeedLimiter = 1;
-                //pc.printf("turn,%f\n",diffDir);
-
-                float distanceToX = (float)abs(currX - loctarget.x);
-                float distanceToY = (float)abs(currY - loctarget.y);
-
-                float distanceToTarget = hypot(distanceToX, distanceToY);
-
-                if ((distanceToTarget < 400) && (distanceToTarget > 100)) {
-                    MoveSpeedLimiter = (distanceToTarget)/400;
-                }
-                else if((distanceToTarget < 400) && (distanceToTarget > 100)) {
-                    MoveSpeedLimiter  = 0.25;
-                }
-                
-                
-                
-
-                // calculte the motor speeds
-                if (tempPidVar >= 0) {
-                    //turn left
-                    speedL = (1-abs(tempPidVar))*MOVE_SPEED*MoveSpeedLimiter;
-                    speedR = MOVE_SPEED*MoveSpeedLimiter;
-
-                } else {
-                    //turn right
-                    speedR = (1-abs(tempPidVar))*MOVE_SPEED*MoveSpeedLimiter;
-                    speedL = MOVE_SPEED*MoveSpeedLimiter;
-                }
-
-
-
-
-                if (loctarget.facing) motors.setSpeed( int(speedL),  int(speedR));
-                else motors.setSpeed( -int(speedR),  -int(speedL));
-
-            }
-        }
-        kalman.statelock.unlock();
-        // wait
-        Thread::wait(MOTION_UPDATE_PERIOD);
-    }
-}
\ No newline at end of file

--- a/Eurobot_shared/Motion/motion.h	Sat Apr 28 22:21:03 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,18 +0,0 @@
-#include "motors.h"
-#include "ai.h"
-#include "Kalman.h"
-
-class Motion {
-public:
-    Motion(Motors &motorsin, AI &aiin, Kalman &kalmanin);
-    Thread thr_motion;
-
-private:
-    Motors& motors;
-    AI& ai;
-    Kalman& kalman;
-    
-    void motion_thread();
-    static void mtwrapper(void const *arg){ ((Motion*)arg)->motion_thread(); }
-
-};
\ No newline at end of file

--- a/Eurobot_shared/TSH.h	Sat Apr 28 22:21:03 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,62 +0,0 @@
-#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

--- a/Eurobot_shared/ai/ai.cpp	Sat Apr 28 22:21:03 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,33 +0,0 @@
-
-#include "ai.h"
-#include "rtos.h"
-#include "globals.h"
-
-AI::AI() :
-        thr_AI(aithreadwrapper,this,osPriorityNormal,1024) {
-    flag_terminate = false;
-    //printf("aistart\r\n");
-}
-
-void AI::settarget(float targetX, float targetY, float targetTheta, bool targetfacing) {
-    targetlock.lock();
-    target.x = targetX;
-    target.y = targetY;
-    target.theta = targetTheta;
-    target.facing = targetfacing;
-    target.reached = false;
-    targetlock.unlock();
-}
-
-void AI::settarget(Target targetin) {
-    targetlock.lock();
-    target = targetin;
-    targetlock.unlock();
-}
-
-AI::Target AI::gettarget() {
-    targetlock.lock();
-    Target temptarget = target;
-    targetlock.unlock();
-    return temptarget;
-}
\ No newline at end of file

--- a/Eurobot_shared/ai/ai.h	Sat Apr 28 22:21:03 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,34 +0,0 @@
-#ifndef AI_H
-#define AI_H
-
-#include "rtos.h"
-
-class AI {
-public:
-AI();
-
-Mutex targetlock;
-Thread thr_AI;
-
-struct Target {
-    float x;
-    float y;
-    float theta;
-    bool facing;
-    bool reached;
-} target;
-
-void settarget(float targetX, float targetY, float targetTheta, bool targetfacing = true);
-void settarget(Target);
-Target gettarget();
-
-bool flag_terminate;// = false;
-
-private:
-
-void ai_thread ();
-static void aithreadwrapper(void const *arg){ ((AI*)arg)->ai_thread(); }
-
-};
-
-#endif //AI_H
\ No newline at end of file

--- a/Eurobot_shared/geometryfuncs/geometryfuncs.h	Sat Apr 28 22:21:03 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,28 +0,0 @@
-#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/Eurobot_shared/system/system.cpp	Sat Apr 28 22:21:03 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,29 +0,0 @@
-#include "system.h"
-
-//Defining the externs
-DigitalOut     OLED1(LED1);
-DigitalOut     OLED2(LED2);
-DigitalOut     OLED3(LED3);
-DigitalOut     OLED4(LED4);
-
-//nop style wait function
-void nopwait(int ms){
-while(ms--)
-    for (volatile int i = 0; i < 24000; i++);
-}
-
-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);
-        
-        int thistime = timer.read_us()-1000000;
-        cpupercent = thistime;
-    }
-}
\ No newline at end of file

--- a/Eurobot_shared/system/system.h	Sat Apr 28 22:21:03 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,51 +0,0 @@
-
-#ifndef SYSTEM_H
-#define SYSTEM_H
-
-#include "globals.h"
-#include "rtos.h"
-
-//Declaring the onboard LED's for everyone to use
-extern DigitalOut     OLED1;//(LED1);
-extern DigitalOut     OLED2;//(LED2);
-extern DigitalOut     OLED3;//(LED3);
-extern DigitalOut     OLED4;//(LED4);
-
-//nop style wait function
-void nopwait(int ms);
-
-//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

--- a/Eurobot_shared/ui/ui.cpp	Sat Apr 28 22:21:03 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,103 +0,0 @@
-
-#include "ui.h"
-#include <iostream>
-#include "system.h"
-
-UI::UI() :
-        tUI(printtw,this,osPriorityNormal,2048) {
-    newdataflags = 0;
-    for (int i = 0; i < NUMIDS; i++) {
-        idlist[i] = 0;
-        buffarr[i] = 0;
-    }
-
-}
-
-bool UI::regid(char id, unsigned int length) {
-
-    //check if the id is already taken
-    if (id < NUMIDS && !idlist[id]) {
-        idlist[id] = length;
-        buffarr[id] = new float[length];
-        return true;
-    } else
-        return false;
-}
-
-bool UI::updateval(char id, float* buffer, unsigned int length) {
-    
-    //check if the id is registered, and has buffer of correct length
-    if (id < NUMIDS && idlist[id] == length && buffarr[id] && !(newdataflags & (1<<id))) {
-        for (int i = 0; i < length; i++)
-            buffarr[id][i] = buffer[i];
-        newdataflags |= (1<<id);
-        return true;
-    } else{
-        return false;
-    }
-}
-
-bool UI::updateval(char id, float value) {
-
-    //check if the id is registered, and the old value has been written
-    if (id < NUMIDS && idlist[id] == 1 && buffarr[id] && !(newdataflags & (1<<id))) {
-        buffarr[id][0] = value;
-        newdataflags |= (1<<id);
-        return true;
-    } else
-        return false;
-
-}
-
-bool UI::unregid(char id) {
-    if (id < NUMIDS) {
-        idlist[id] = 0;
-        if (buffarr[id])
-            delete buffarr[id];
-        return true;
-    } else
-        return false;
-}
-
-void UI::printloop() {
-
-#ifdef UION
-    Thread::wait(1500);
-#else
-    Thread::wait(osWaitForever);
-#endif
-
-    char* sync = "ABCD";
-    std::cout.write(sync, 4);
-    //std::cout.flush();
-    std::cout << std::endl;
-    //printf("\r\n");
-
-    while (1) {
-    
-        OLED3 = !OLED3;
-    
-        //send number of packets
-        char numtosend = 0;
-        for (int id = 0; id < NUMIDS; id++)
-            if (newdataflags & (1<<id))
-                numtosend++;
-                
-        std::cout.put(numtosend);
-
-        //send packets
-        for (char id = 0; id < NUMIDS; id++) {
-            if (newdataflags & (1<<id)) {
-                std::cout.put(id);
-                std::cout.write((char*)buffarr[id], idlist[id] * sizeof(float));
-                newdataflags &= ~(1<<id);
-            }
-        }
-        
-        std::cout << std::endl;
-        //std::cout.flush();
-        Thread::wait(200);
-    }
-
-}
-

--- a/Eurobot_shared/ui/ui.h	Sat Apr 28 22:21:03 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,30 +0,0 @@
-
-#ifndef UI_H
-#define UI_H
-
-#include "rtos.h"
-
-#define NUMIDS 32
-
-class UI {
-public:
-    Thread tUI;
-    
-    UI();
-    
-    bool regid(char id, unsigned int length);
-    bool updateval(char id, float* buffer, unsigned int length);
-    bool updateval(char id, float value);
-    bool unregid(char id);
-    
-private:
-    Mutex printlock;
-    char idlist[NUMIDS];
-    float* buffarr[NUMIDS];
-    volatile int newdataflags; //Only works for NUMID = 32
-    
-    void printloop();
-    static void printtw(void const *arg){ ((UI*)arg)->printloop(); }
-};
-
-#endif //UI_H