Eurobot_shared pubulished from Eurobot Primary

Files at this revision

API Documentation at this revision

Comitter:
narshu
Date:
Tue Aug 07 10:25:53 2012 +0000
Commit message:
[mbed] converted /Eurobot_2012_Primary/Eurobot_shared

Changed in this revision

Kalman/IR/IR.cpp Show annotated file Show diff for this revision Revisions of this file
Kalman/IR/IR.h Show annotated file Show diff for this revision Revisions of this file
Kalman/Kalman.cpp Show annotated file Show diff for this revision Revisions of this file
Kalman/Kalman.h Show annotated file Show diff for this revision Revisions of this file
Kalman/Sonar/RF12B/RF12B.cpp Show annotated file Show diff for this revision Revisions of this file
Kalman/Sonar/RF12B/RF12B.h Show annotated file Show diff for this revision Revisions of this file
Kalman/Sonar/RF12B/RF_defs.h Show annotated file Show diff for this revision Revisions of this file
Kalman/Sonar/RFSRF05.cpp Show annotated file Show diff for this revision Revisions of this file
Kalman/Sonar/RFSRF05.h Show annotated file Show diff for this revision Revisions of this file
Motion/motion.cpp Show annotated file Show diff for this revision Revisions of this file
Motion/motion.h Show annotated file Show diff for this revision Revisions of this file
TSH.h Show annotated file Show diff for this revision Revisions of this file
ai/ai.cpp Show annotated file Show diff for this revision Revisions of this file
ai/ai.h Show annotated file Show diff for this revision Revisions of this file
geometryfuncs/geometryfuncs.h Show annotated file Show diff for this revision Revisions of this file
system/system.cpp Show annotated file Show diff for this revision Revisions of this file
system/system.h Show annotated file Show diff for this revision Revisions of this file
ui/ui.cpp Show annotated file Show diff for this revision Revisions of this file
ui/ui.h Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/IR/IR.cpp	Tue Aug 07 10:25:53 2012 +0000
@@ -0,0 +1,74 @@
+#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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/IR/IR.h	Tue Aug 07 10:25:53 2012 +0000
@@ -0,0 +1,25 @@
+
+#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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Kalman.cpp	Tue Aug 07 10:25:53 2012 +0000
@@ -0,0 +1,467 @@
+//***************************************************************************************
+//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;
+    if (Colour)
+        X = 0.5, 0, 0;
+    else
+        X = 2.5, 0, PI;
+
+    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() {
+    motors.stop();
+    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
+    //Thread::wait(1000);
+    wait(2);
+
+    //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 origin_x = beaconpos[1].x;
+    float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d);
+    float x_coor = origin_x + (SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j;
+
+    //debug for trilateration
+    printf("Cal at x: %0.4f, y: %0.4f \r\n",x_coor,y_coor );
+
+    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);
+        if (!Colour)
+        angle_est -= PI;
+        //printf("Angle %d : %f \n\r",i,angle_est*180/PI );
+        // 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);
+
+    //debug
+    printf("Offsets IR: %0.4f, Sonar: %0.4f \r\n",IR_Offset*180/PI,Sonar_Offset*1000 );
+
+    //statelock already locked
+    X(0) = x_coor/1000.0f;
+    X(1) = y_coor/1000.0f;
+    
+    if (Colour)
+        X(2) = 0;
+    else
+        X(2) = PI;
+
+    // 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();
+
+        float tempX2 = X(2);
+        //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
+        float avgX2 = (X(2) + tempX2)/2.0f;
+        Matrix<float, 3, 3> F;
+        F = 1, 0, (dxp * -sin(avgX2) - dyp * cos(avgX2)),
+            0, 1, (dxp * cos(avgX2) - dyp * sin(avgX2)),
+            0, 0, 1;
+
+        //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);
+        }
+
+    }
+
+}
+
+// reset kalman states
+void Kalman::KalmanReset() {
+    float SonarMeasuresx1000[3];
+    statelock.lock();
+    SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f;
+    SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f;
+    SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f;
+    //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 origin_x = beaconpos[1].x;
+    float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d);
+    float x_coor = origin_x +(SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j;
+
+    //statelock already locked
+    X(0) = x_coor/1000.0f;
+    X(1) = y_coor/1000.0f;
+   
+    
+
+/*    if (Colour){
+        X(0) = 0.2;
+        X(1) = 0.2;
+        //X(2) = 0;
+        }
+    else {
+        X(0) = 2.8;
+        X(1) = 0.2;
+        //X(2) = PI;
+    }
+    */
+    P = 0.05, 0, 0,
+        0, 0.05, 0,
+        0, 0, 0.04;
+
+    // unlocks mutexes
+    statelock.unlock();
+
+}
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Kalman.h	Tue Aug 07 10:25:53 2012 +0000
@@ -0,0 +1,100 @@
+#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();
+    
+    // reset kalman states
+    void KalmanReset();
+
+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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Sonar/RF12B/RF12B.cpp	Tue Aug 07 10:25:53 2012 +0000
@@ -0,0 +1,400 @@
+#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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Sonar/RF12B/RF12B.h	Tue Aug 07 10:25:53 2012 +0000
@@ -0,0 +1,83 @@
+#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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Sonar/RF12B/RF_defs.h	Tue Aug 07 10:25:53 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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Sonar/RFSRF05.cpp	Tue Aug 07 10:25:53 2012 +0000
@@ -0,0 +1,183 @@
+
+#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 100, we put 300 so circles overlap
+        _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]);
+}
+
+void RFSRF05::setCode(int code_index, unsigned char code) {
+    codes[code_index] = code;
+}
+
+//SRF05::operator float() {
+//    return read();
+//}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Sonar/RFSRF05.h	Tue Aug 07 10:25:53 2012 +0000
@@ -0,0 +1,102 @@
+
+#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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motion/motion.cpp	Tue Aug 07 10:25:53 2012 +0000
@@ -0,0 +1,237 @@
+#include "motion.h"
+#include "geometryfuncs.h"
+#include "system.h"
+#include "PID.h"
+
+AnalogIn ObsAvoidPin(p20);
+
+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.setSpeed(5,5);
+    motors.stop();
+    // Thread::wait(1500);
+    //ai.thr_AI.signal_set(0x01);
+
+    //PID declare
+    PID PIDControllerMotorTheta2(FWD_MOVE_P, FWD_MOVE_P/10.0f, 0.000005, MOTION_UPDATE_PERIOD/1000.0f);     //Going forward
+    PID PIDControllerMotorTheta(SPIN_MOVE_P, SPIN_MOVE_P/10.0f, 0.000005, 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;
+    float xBuffer, yBuffer;
+    float xOriginalBuffer = 0, yOriginalBuffer = 0;
+    int initiateFlag = 1;
+    int dontSpinFlag = 0;
+    int atTargetFlag = 0;
+    int atTargetDirectionFlag = 0;
+
+    while (1) {
+        //kalman.statelock.lock();
+        if (ai.flag_terminate) {
+            // stops motors and teminates the thread
+            motors.stop();
+            //motors.coastStop();
+            terminate();
+        }
+
+        // stops motor
+        if ((ai.flag_motorStop) || (ObsAvoidPin > 0.4)) {
+            motors.stop();
+        } else if (ai.flag_manOverride) {
+
+        } else {
+
+
+            // 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();
+
+            // make a local copy of the target
+            ai.targetlock.lock();
+            AI::Target loctarget = ai.gettarget();
+            ai.targetlock.unlock();
+            /*
+            //PID Tuning Code
+            if (pc.readable() == 1) {
+                float cmd;
+                pc.scanf("%f", &cmd);
+                //Tune PID referece
+                PIDControllerMotorTheta2.setTunings(cmd, 0, 0);
+            }
+            */
+
+
+            if (initiateFlag == 1) {
+                xOriginalBuffer = currX;
+                yOriginalBuffer = currY;
+
+                xBuffer = ai.gettarget().x;
+                yBuffer = ai.gettarget().y;
+
+                initiateFlag = 0;
+            }
+
+            if (xBuffer != loctarget.x || yBuffer != loctarget.y) {
+                //target changed
+                //update xOriginal and yOriginal buffers
+                xOriginalBuffer = currX;
+                yOriginalBuffer = currY;
+
+                xBuffer = loctarget.x;
+                yBuffer = loctarget.y;
+
+                atTargetFlag = 0;
+                atTargetDirectionFlag = 0;
+
+            }
+
+            // check if target reached ----------------------------------
+            if (atTargetFlag || hypot(currX - loctarget.x, currY - loctarget.y) < POSITION_TOR) {
+
+                if (atTargetFlag == 0) {
+                    motors.stop();
+                    Thread::wait(100);
+                }
+
+
+                if (hypot(currX - loctarget.x, currY - loctarget.y) < POSITION_TOR) {
+                    atTargetFlag = 1;
+                }
+                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 (atTargetDirectionFlag == 0) {
+                        ai.thr_AI.signal_set(0x01);
+                        atTargetDirectionFlag = 1;
+                    }
+
+                    /*
+                    if (!loctarget.reached) {
+                        static int counter = 10;
+                        // guarding counter for reaching target
+                        if (counter-- == 0) {
+                            counter = 10;
+                            ai.target.reached = true;
+                            ai.thr_AI.signal_set(0x01);
+
+                        }
+                    }
+                    */
+                }
+            }
+
+            // adjust motion to reach target ----------------------------
+            else {
+
+                OLED3 = 1;
+
+                /*
+                if ((hypot(xOriginalBuffer - loctarget.x, yOriginalBuffer - loctarget.y) - hypot(xOriginalBuffer - currX, yOriginalBuffer - currY)) < 0) {
+                    loctarget.facing = !loctarget.facing;
+                    dontSpinFlag = 1;
+                } else {
+                    dontSpinFlag = 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) && (dontSpinFlag == 0)) {   //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 > 200) && motors.accelerationRegister == 1) {
+                        MoveSpeedLimiter = (distanceToTarget)/400;
+                    } else if (distanceToTarget <= 200) {
+                        MoveSpeedLimiter  = 0.5;
+                    }
+
+
+
+
+                    // 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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motion/motion.h	Tue Aug 07 10:25:53 2012 +0000
@@ -0,0 +1,21 @@
+#include "motors.h"
+#include "ai.h"
+#include "Kalman.h"
+
+#ifndef MOTION
+#define MOTION
+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(); }
+
+};
+#endif
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TSH.h	Tue Aug 07 10:25:53 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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ai/ai.cpp	Tue Aug 07 10:25:53 2012 +0000
@@ -0,0 +1,52 @@
+#include "ai.h"
+#include "rtos.h"
+#include "globals.h"
+
+
+
+AI::AI() :
+        thr_AI(aithreadwrapper,this,osPriorityNormal,1024) {
+    flag_terminate = false;
+    flag_motorStop = true;
+    flag_manOverride = false;
+    //printf("aistart\r\n");
+}
+
+
+void AI::settarget(float targetX, float targetY, float targetTheta, bool targetfacing, bool colour, int maxSpeed) {
+    targetlock.lock();
+    MOVE_SPEED = maxSpeed;
+    target.x = targetX;
+    target.y = targetY;
+    target.theta = targetTheta;
+    target.facing = targetfacing;
+    target.reached = false;
+    if (!colour) {
+        target.x = 3000 - target.x;
+        target.theta = PI - target.theta;
+
+        target.theta -= (floor(target.theta/(2*PI)))*2*PI;
+        if (target.theta < -PI) {
+            target.theta += 2*PI;
+        }
+        if (target.theta > PI) {
+            target.theta -= 2*PI;
+        }
+
+
+    }
+    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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ai/ai.h	Tue Aug 07 10:25:53 2012 +0000
@@ -0,0 +1,37 @@
+#ifndef AI_H
+#define AI_H
+
+#include "rtos.h"
+//#include "Kalman.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, bool colour = true, int maxSpeed = 35);
+void settarget(Target);
+Target gettarget();
+
+bool flag_terminate;// = false;
+bool flag_motorStop; // = true;
+bool flag_manOverride; // = false;
+
+private:
+
+void ai_thread ();
+static void aithreadwrapper(void const *arg){ ((AI*)arg)->ai_thread(); }
+
+};
+
+#endif //AI_H
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/geometryfuncs/geometryfuncs.h	Tue Aug 07 10:25:53 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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/system/system.cpp	Tue Aug 07 10:25:53 2012 +0000
@@ -0,0 +1,29 @@
+#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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/system/system.h	Tue Aug 07 10:25:53 2012 +0000
@@ -0,0 +1,51 @@
+
+#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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ui/ui.cpp	Tue Aug 07 10:25:53 2012 +0000
@@ -0,0 +1,103 @@
+
+#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(3500);
+#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);
+    }
+
+}
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ui/ui.h	Tue Aug 07 10:25:53 2012 +0000
@@ -0,0 +1,30 @@
+
+#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