Revision 0:e238496b8073, committed 2012-04-26

Comitter:

narshu

Date:

Thu Apr 26 19:11:11 2012 +0000

Child:

1:4964fa534202

Commit message:

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IR/IR.cpp	Thu Apr 26 19:11:11 2012 +0000
@@ -0,0 +1,77 @@
+#include "IR.h"
+#include "Kalman.h"
+#include "system.h"
+#include "geometryfuncs.h"
+
+IR::IR(Kalman &kalmanin):
+    IRserial(p13, p14),
+    kalman(kalmanin) {
+    
+    //Starting values of IR calibration
+    angleInit = false;
+    angleOffset = 0;
+    
+    //Setting up IR serial
+    IRserial.baud(115200);
+    IRserial.format(8,Serial::Odd,1);
+    IRserial.attach(this, &IR::vIRValueISR, Serial::RxIrq);
+    
+    }
+    
+void IR::detachisr(){
+    IRserial.attach(NULL,Serial::RxIrq);
+}
+
+void IR::attachisr(){
+    IRserial.attach(this, &IR::vIRValueISR, Serial::RxIrq);
+}
+
+void IR::vIRValueISR (void) {
+
+    // bytes packing/unpacking for IR turret serial comm
+    union IRValue_t {
+        float IR_floats[3];
+        int IR_ints[3];
+        unsigned char IR_chars[12];
+    } IRValues;
+
+    
+    // A workaround for mbed UART ISR bug
+    // Clear the RBR flag to make sure the interrupt doesn't loop
+    // UART3 for the port on pins 9/10, UART2 for pins 28/27, and UART1 for pins 13/14.
+    // UART0 for USB UART
+    unsigned char RBR = LPC_UART1->RBR;
+
+    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
+            OLED3 = !OLED3;
+            if (angleInit) {
+                kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),rectifyAng(IRValues.IR_floats[1] - angleOffset),IRValues.IR_floats[2]);
+            } else {
+                //dont bother to update if we dont know the offset of the IR, as it messes up the P matrix
+                //kalman->runupdate(kalman.measurement_t(IRValues.IR_ints[0]+3),IRValues.IR_floats[1],IRValues.IR_floats[2]);
+                
+                //only update the IRMeasures used by kalman init
+                kalman.IRMeasures[IRValues.IR_ints[0]] = IRValues.IR_floats[1];
+            }
+        }
+
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IR/IR.h	Thu Apr 26 19:11:11 2012 +0000
@@ -0,0 +1,28 @@
+
+#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;
+
+bool angleInit; // = false;
+float angleOffset; // = 0;
+
+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.cpp	Thu Apr 26 19:11:11 2012 +0000
@@ -0,0 +1,342 @@
+//***************************************************************************************
+//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) :
+        ir(*this),
+        sonararray(p10,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9),
+        motors(motorsin),
+        predictthread(predictloopwrapper, this, osPriorityNormal, 512),
+        predictticker( SIGTICKARGS(predictthread, 0x1) ),
+//        sonarthread(sonarloopwrapper, this, osPriorityNormal, 256),
+//        sonarticker( SIGTICKARGS(sonarthread, 0x1) ),
+        updatethread(updateloopwrapper, this, osPriorityNormal, 512) {
+
+    Kalman_init = false;
+    //Intialising some arrays to zero
+    for (int kk = 0; kk < 3; kk ++) {
+        SonarMeasure_Offset[kk] = 0;
+    }
+    //Initialising other vars
+
+
+    //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;
+    // set initiating flag to false
+    Kalman_init = false;
+    
+    // init the offset array
+    for (int k = 0; k < 3; k ++) {
+        SonarMeasure_Offset[k] = 0;
+        IRMeasures[k] = 0;
+    }
+
+    LPC_UART0->FCR = LPC_UART0->FCR | 0x06;       // Flush the serial FIFO buffer / OR with FCR
+    //wating untill the IR has reved up and picked up some data
+    wait(1);
+
+    //temporaraly disable IR updates
+    ir.detachisr();
+    //IRturret.attach(NULL,Serial::RxIrq);
+
+    //lock the state throughout the computation, as we will override the state at the end
+    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;
+
+    //Compute sonar offset
+    float Dist_Exp[3];
+    for (int k = 0; k < 3; k++) {
+        Dist_Exp[k] = sqrt((beaconpos[k].y - y_coor)*(beaconpos[k].y - y_coor)+(beaconpos[k].x - x_coor)*(beaconpos[k].x - x_coor));
+        SonarMeasure_Offset[k] = (SonarMeasuresx1000[k]-Dist_Exp[k])/1000.0f;
+    }
+
+    //Compute IR offset
+    ir.angleOffset = 0;
+    for (int i = 0; i < 3; i++) {
+        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.angleOffset += angle_temp;
+        }
+    }
+    ir.angleOffset = ir.angleOffset/float(beacon_cnt);
+    //printf("\n\r");
+
+    //statelock already locked
+    ir.angleInit = true;
+    // set int flag to true
+    Kalman_init = true;
+    X(0) = x_coor/1000.0f;
+    X(1) = y_coor/1000.0f;
+    X(2) = 0;
+    statelock.unlock();
+
+    //printf("x: %0.4f, y: %0.4f, offset: %0.4f \n\r", x_coor, y_coor, angleOffset*180/PI);
+
+    //reattach the IR processing
+    ir.attachisr();
+    //IRturret.attach(&IR::vIRValueISR,Serial::RxIrq);
+}
+
+
+void Kalman::predictloop() {
+
+    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();
+
+        //rotating to cartesian frame and updating state
+        X(0) += dxp * cos(X(2)) - dyp * sin(X(2));
+        X(1) += dxp * sin(X(2)) + dyp * cos(X(2));
+        X(2) = rectifyAng(X(2) + thetap);
+
+        //Linearising F around X
+        Matrix<float, 3, 3> F;
+        F = 1, 0, (dxp * -sin(X(2)) - dyp * cos(X(2))),
+            0, 1, (dxp * cos(X(2)) - dyp * sin(X(2))),
+            0, 0, 1;
+
+        //Generating forward and rotational variance
+        float varfwd = fwdvarperunit * (dright + dleft) / 2.0f;
+        float varang = varperang * thetap;
+        float varxydt = xyvarpertime * PREDICTPERIOD;
+        float varangdt = angvarpertime * PREDICTPERIOD;
+
+        //Rotating into cartesian frame
+        Matrix<float, 2, 2> Qsub,Qsubrot,Qrot;
+        Qsub = varfwd + varxydt, 0,
+               0, varxydt;
+
+        Qrot = Rotmatrix(X(2));
+
+        Qsubrot = Qrot * Qsub * trans(Qrot);
+
+        //Generate Q
+        Matrix<float, 3, 3> Q;//(Qsubrot);
+        Q = Qsubrot(0,0), Qsubrot(0,1), 0,
+            Qsubrot(1,0), Qsubrot(1,1), 0,
+            0, 0, varang + varangdt;
+
+        P = F * P * trans(F) + Q;
+
+        statelock.unlock();
+        //Thread::wait(PREDICTPERIOD);
+
+        //cout << "predict" << X << endl;
+        //cout << P << endl;
+    }
+}
+
+//void Kalman::sonarloop() {
+//    while (1) {
+//        Thread::signal_wait(0x1);
+//        sonararray.startRange();
+//    }
+//}
+
+
+void Kalman::runupdate(measurement_t type, float value, float variance) {
+    //printf("beacon %d dist %f\r\n", sonarid, dist);
+    //led2 = !led2;
+
+    measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
+    if (measured) {
+        measured->mtype = type;
+        measured->value = value;
+        measured->variance = variance;
+
+        osStatus putret = measureMQ.put(measured);
+        if (putret)
+            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() {
+    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) {
+
+                    float dist = value / 1000.0f; //converting to m from mm
+                    int sonarid = type;
+                    aborton2stddev = false;
+
+                    // Remove the offset if possible
+                    if (Kalman_init)
+                        dist = dist - SonarMeasure_Offset[sonarid];
+
+                    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;
+
+                    dhdx = rbx / expecdist;
+                    dhdy = rby / expecdist;
+
+                    H = dhdx, dhdy, 0;
+
+                } else if (type <= IR3) {
+
+                    aborton2stddev = false;
+                    int IRidx = type-3;
+
+                    statelock.lock();
+                    IRMeasures[IRidx] = value;
+
+                    rbx = X(0) - beaconpos[IRidx].x/1000.0f;
+                    rby = X(1) - beaconpos[IRidx].y/1000.0f;
+
+                    float expecang = atan2(-rby, -rbx) - X(2);
+                    Y = rectifyAng(value - expecang);
+
+                    float dstsq = rbx*rbx + rby*rby;
+                    H = -rby/dstsq, rbx/dstsq, -1;
+                }
+
+                Matrix<float, 3, 1> PH (P * trans(H));
+                S = (H * PH)(0,0) + variance;
+
+                if (aborton2stddev && Y*Y > 4 * S) {
+                    statelock.unlock();
+                    continue;
+                }
+
+                Matrix<float, 3, 1> K (PH * (1/S));
+
+                //Updating state
+                X += col(K, 0) * Y;
+                X(2) = rectifyAng(X(2));
+
+                P = (I3 - K * H) * P;
+
+                statelock.unlock();
+
+            }
+
+        } else {
+            OLED4 = 1;
+            //printf("ERROR: in updateloop, code %#x", evt);
+        }
+
+    }
+
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman.h	Thu Apr 26 19:11:11 2012 +0000
@@ -0,0 +1,76 @@
+#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 <tvmet/Matrix.h>
+#include <tvmet/Vector.h>
+using namespace tvmet;
+
+
+class Kalman {
+public:
+    enum measurement_t {SONAR1 = 0, SONAR2, SONAR3, IR1, IR2, IR3};
+    static const measurement_t maxmeasure = IR3;
+    
+    Kalman(Motors &motorsin);
+    
+    void predict();
+    void runupdate(measurement_t type, float value, float variance);
+    
+    //State variables
+    Vector<float, 3> X;
+    Matrix<float, 3, 3> P;
+    Mutex statelock;
+    
+    float SonarMeasures[3];
+    float IRMeasures[3];
+    float SonarMeasure_Offset[3];
+    
+    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;
+    
+    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/Sonar.lib	Thu Apr 26 19:11:11 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/narshu/code/Sonar/#0370ea94b64a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/tvmet.lib	Thu Apr 26 19:11:11 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/madcowswe/code/tvmet/#feb4117d16d8