ICRS Eurobot 2013

Dependencies:   mbed mbed-rtos Servo QEI

Files at this revision

API Documentation at this revision

Comitter:
madcowswe
Date:
Tue Apr 09 15:33:36 2013 +0000
Parent:
19:4b993a9a156e
Commit message:
Predict loop running, update loop not done.

Changed in this revision

Communication/coprocserial.cpp Show annotated file Show diff for this revision Revisions of this file
Communication/coprocserial.h Show annotated file Show diff for this revision Revisions of this file
Processes/Kalman/Kalman.cpp Show annotated file Show diff for this revision Revisions of this file
Processes/Kalman/Kalman.h Show annotated file Show diff for this revision Revisions of this file
Processes/Printing/Printing.cpp Show annotated file Show diff for this revision Revisions of this file
Sensors/Encoders/Encoder.cpp Show annotated file Show diff for this revision Revisions of this file
Sensors/Encoders/Encoder.h Show annotated file Show diff for this revision Revisions of this file
Sensors/Sonar/Sonar.h Show diff for this revision Revisions of this file
globals.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
supportfuncs.h Show annotated file Show diff for this revision Revisions of this file
diff -r 4b993a9a156e -r 70d651156779 Communication/coprocserial.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Communication/coprocserial.cpp	Tue Apr 09 15:33:36 2013 +0000
@@ -0,0 +1,71 @@
+#include "Kalman.h"
+#include "mbed.h"
+#include "globals.h"
+
+Serial coprocserial(NC, P_SERIAL_RX);
+
+//DigitalOut OLED1(LED1);
+DigitalOut OLED2(LED2);
+
+// bytes packing
+typedef union {
+
+    struct _data{
+        unsigned char sync[3];
+        unsigned char ID;
+        float value;
+        float variance;
+    } data;
+    
+    unsigned char type_char[12];
+} bytepack_t;
+
+bytepack_t incbuff;
+
+volatile int buffprintflag = 0;
+void printbuff(){
+    while(!buffprintflag);
+    buffprintflag = 0;
+    for(int i = 0; i < 9; i++){
+        printf("%x ", incbuff.type_char[i]);
+    }
+    printf("\r\n");
+}
+
+void procserial(){
+
+    //Fetch the byte in a "clear interrupt" sense
+    unsigned char c = LPC_UART1->RBR;
+    
+    //OLED1 = !OLED1;
+    
+    static int ctr = 0;
+    
+
+    if (ctr < 3){
+        if (c == 0xFF)
+            ctr++;
+        else
+            ctr = 0;
+    } else {
+        incbuff.type_char[ctr] = c;
+        if (++ctr == 12){
+            ctr = 0;
+            
+            OLED2 = !OLED2;
+            buffprintflag = 1;
+            
+            //runupdate
+            Kalman::runupdate((Kalman::measurement_t)incbuff.data.ID, incbuff.data.value, incbuff.data.variance);
+        }
+    }
+    
+}
+
+void InitSerial(){
+    
+    coprocserial.baud(115200);
+
+    printf("attachserial\r\n");
+    coprocserial.attach(procserial);
+}
\ No newline at end of file
diff -r 4b993a9a156e -r 70d651156779 Communication/coprocserial.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Communication/coprocserial.h	Tue Apr 09 15:33:36 2013 +0000
@@ -0,0 +1,3 @@
+
+void InitSerial();
+void printbuff();
\ No newline at end of file
diff -r 4b993a9a156e -r 70d651156779 Processes/Kalman/Kalman.cpp
--- a/Processes/Kalman/Kalman.cpp	Sun Apr 07 19:26:07 2013 +0000
+++ b/Processes/Kalman/Kalman.cpp	Tue Apr 09 15:33:36 2013 +0000
@@ -5,9 +5,10 @@
 #include "rtos.h"
 #include "math.h"
 #include "supportfuncs.h"
+#include "Encoder.h"
 //#include "globals.h"
 
-#include <tvmet/Matrix.h>
+#include "tvmet/Matrix.h"
 using namespace tvmet;
 
 
@@ -15,6 +16,11 @@
 namespace Kalman
 {
 
+Ticker predictticker;
+
+DigitalOut OLED4(LED4);
+DigitalOut OLED1(LED1);
+
 //State variables
 Matrix<float, 3, 1> X;
 Matrix<float, 3, 3> P;
@@ -23,7 +29,7 @@
 float RawReadings[maxmeasure+1];
 float IRpahseOffset;
 
-bool Kalman_init = 0;
+bool Kalman_inited = 0;
 
 struct measurmentdata {
     measurement_t mtype;
@@ -33,29 +39,41 @@
 
 Mail <measurmentdata, 16> measureMQ;
 
+Thread* predict_thread_ptr = NULL;
 
 
 //Note: this init function assumes that the robot faces east, theta=0, in the +x direction
 void KalmanInit()
 {
+    printf("kalmaninit \r\n");
+    
+    //WARNING: HARDCODED!
+    
     //solve for our position (assume perfect bias)
-    const float d = beaconpos[0].y - beaconpos[1].y;
-    const float i = beaconpos[0].y - beaconpos[2].y;
-    const float j = beaconpos[0].x - beaconpos[2].x;
-    float r1 = RawReadings[SONAR0];
+    const float d = beaconpos[2].y - beaconpos[1].y;
+    const float i = beaconpos[2].y - beaconpos[0].y;
+    const float j = beaconpos[2].x - beaconpos[0].x;
+    float r1 = RawReadings[SONAR2];
     float r2 = RawReadings[SONAR1];
-    float r3 = RawReadings[SONAR2];
+    float r3 = RawReadings[SONAR0];
+    
+    printf("ranges: 0: %0.4f, 1: %0.4f, 2: %0.4f \r\n", r1, r2, r3);
 
     float y_coor = (r1*r1-r2*r2+d*d)/(2*d);
     float x_coor = (r1*r1-r3*r3+i*i+j*j)/(2*j) - (i*y_coor)/j;
-
+    
+    //coordinate system hack (for now)
+    x_coor = beaconpos[2].x - x_coor;
+    y_coor = beaconpos[2].y - y_coor;
+    
+    printf("solved pos from sonar: %f, %f \r\n", x_coor, y_coor);
+    
     //IR
-
     float IRMeasuresloc[3];
     IRMeasuresloc[0] = RawReadings[IR0];
     IRMeasuresloc[1] = RawReadings[IR1];
     IRMeasuresloc[2] = RawReadings[IR2];
-    //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);
+    printf("IR 0: %0.4f, 1: %0.4f, 2: %0.4f \r\n", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);
 
     float IR_Offsets[3];
     float fromb0offset = 0;
@@ -66,10 +84,10 @@
 
         //printf("Angle %d : %f \n\r",i,angle_est*180/PI );
         IR_Offsets[i] = constrainAngle(IRMeasuresloc[i] - angle_est);
-        
+
         fromb0offset += constrainAngle(IR_Offsets[i] - IR_Offsets[0]);
     }
-    
+
     IRpahseOffset = constrainAngle(IR_Offsets[0] + fromb0offset/3);
 
     //debug
@@ -80,14 +98,24 @@
     X(1,0) = y_coor;
     X(2,0) = 0;
     statelock.unlock();
+    
+    Kalman_inited = 1;
 }
 
-/*
-void Kalman::predictloop(void* dummy)
+
+State getState(){
+    statelock.lock();
+    State state = {X(0,0), X(1,0), X(2,0)};
+    statelock.unlock();
+    return state;
+}
+
+
+void predictloop(void const *dummy)
 {
 
-    OLED4 = !ui.regid(0, 3);
-    OLED4 = !ui.regid(1, 4);
+    //OLED4 = !ui.regid(0, 3);
+    //OLED4 = !ui.regid(1, 4);
 
     float lastleft = 0;
     float lastright = 0;
@@ -96,11 +124,11 @@
         Thread::signal_wait(0x1);
         OLED1 = !OLED1;
 
-        int leftenc = encoders.getEncoder1();
-        int rightenc = encoders.getEncoder2();
+        float leftenc = left_encoder.getTicks() * ENCODER_M_PER_TICK;
+        float rightenc = right_encoder.getTicks() * ENCODER_M_PER_TICK;
 
-        float dleft = encoders.encoderToDistance(leftenc-lastleft)/1000.0f;
-        float dright = encoders.encoderToDistance(rightenc-lastright)/1000.0f;
+        float dleft = leftenc-lastleft;
+        float dright = rightenc-lastright;
 
         lastleft = leftenc;
         lastright = rightenc;
@@ -108,8 +136,8 @@
 
         //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
+        float thetap = (dright - dleft) / ENCODER_WHEELBASE;
+        if (abs(thetap) < 0.01f) { //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);
@@ -117,20 +145,20 @@
         } else { //calculate circle arc
             //float r = (right + left) / (4.0f * PI * thetap);
             r = (dright + dleft) / (2.0f*thetap);
-            dxp = abs(r)*sin(thetap);
+            dxp = r*sin(thetap);
             dyp = r - r*cos(thetap);
         }
 
         statelock.lock();
 
-        float tempX2 = X(2);
+        float tempX2 = X(2,0);
         //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);
+        X(0,0) += dxp * cos(X(2,0)) - dyp * sin(X(2,0));
+        X(1,0) += dxp * sin(X(2,0)) + dyp * cos(X(2,0));
+        X(2,0) = constrainAngle(X(2,0) + thetap);
 
         //Linearising F around X
-        float avgX2 = (X(2) + tempX2)/2.0f;
+        float avgX2 = (X(2,0) + 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)),
@@ -139,15 +167,15 @@
         //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;
+        float varxydt = xyvarpertime * KALMAN_PREDICT_PERIOD;
+        float varangdt = angvarpertime * KALMAN_PREDICT_PERIOD;
 
         //Rotating into cartesian frame
         Matrix<float, 2, 2> Qsub,Qsubrot,Qrot;
         Qsub = varfwd + varxydt, 0,
         0, varxydt;
 
-        Qrot = Rotmatrix(X(2));
+        Qrot = Rotmatrix(X(2,0));
 
         Qsubrot = Qrot * Qsub * trans(Qrot);
 
@@ -159,24 +187,42 @@
 
         P = F * P * trans(F) + Q;
 
+        //printf("x: %f, y: %f, t: %f\r\n", X(0,0), X(1,0), X(2,0));
         //Update UI
-        float statecpy[] = {X(0), X(1), X(2)};
-        ui.updateval(0, statecpy, 3);
+        //float statecpy[] = {X(0,0), X(1,0), X(2,0)};
+        //ui.updateval(0, statecpy, 3);
 
-        float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)};
-        ui.updateval(1, Pcpy, 4);
+        //float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)};
+        //ui.updateval(1, Pcpy, 4);
 
         statelock.unlock();
     }
 }
 
-void Kalman::runupdate(measurement_t type, float value, float variance)
+
+void predict_event_setter(){
+    if(predict_thread_ptr)
+        predict_thread_ptr->signal_set(0x1);
+    else
+        OLED4 = 1;
+}
+
+void start_predict_ticker(Thread* predict_thread_ptr_in){
+    predict_thread_ptr = predict_thread_ptr_in;
+    predictticker.attach(predict_event_setter, KALMAN_PREDICT_PERIOD);
+}
+
+void runupdate(measurement_t type, float value, float variance)
 {
-    if (!Kalman_init)
+    if (!Kalman_inited) {
         RawReadings[type] = value;
-    else {
+    } else {
 
-        RawReadings[type] = value - SensorOffsets[type];
+        if (type >= IR0 && type <= IR2)
+            RawReadings[type] = value - IRpahseOffset;
+        else
+            RawReadings[type] = value;
+
 
         measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
         if (measured) {
@@ -185,19 +231,20 @@
             measured->variance = variance;
 
             osStatus putret = measureMQ.put(measured);
-            if (putret)
-                OLED4 = 1;
+            //if (putret)
+                //OLED4 = 1;
             //    printf("putting in MQ error code %#x\r\n", putret);
         } else {
-            OLED4 = 1;
+            //OLED4 = 1;
             //printf("MQalloc returned NULL ptr\r\n");
         }
-
+    
     }
+    
 
 }
-
-void Kalman::updateloop(void* dummy)
+/*
+void Kalman::updateloop(void const *dummy)
 {
 
     //sonar Y chanels
@@ -320,37 +367,6 @@
 
 }
 
-// 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;
-
-    P = 0.05, 0, 0,
-    0, 0.05, 0,
-    0, 0, 0.04;
-
-    // unlocks mutexes
-    statelock.unlock();
-
-}
-
 */
 
 } //Kalman Namespace
-
diff -r 4b993a9a156e -r 70d651156779 Processes/Kalman/Kalman.h
--- a/Processes/Kalman/Kalman.h	Sun Apr 07 19:26:07 2013 +0000
+++ b/Processes/Kalman/Kalman.h	Tue Apr 09 15:33:36 2013 +0000
@@ -2,23 +2,25 @@
 #define KALMAN_H
 
 //#include "globals.h"
+#include "rtos.h"
 
 namespace Kalman
 {
 
-typedef struct state {
+typedef struct State {
     float x;
     float y;
     float theta;
-} state ;
+} State ;
 
 //Accessor function to get the state as one consistent struct
-state getState();
+State getState();
 
 //Main loops (to be attached as a thread in main)
-void predictloop(void* dummy);
-void updateloop(void* dummy);
+void predictloop(void const *dummy);
+void updateloop(void const *dummy);
 
+void start_predict_ticker(Thread* predict_thread_ptr_in);
 
 enum measurement_t {SONAR0 = 0, SONAR1, SONAR2, IR0, IR1, IR2};
 const measurement_t maxmeasure = IR2;
@@ -29,13 +31,11 @@
 extern float RawReadings[maxmeasure+1];
 extern float IRpahseOffset;
 
-extern bool Kalman_init;
+extern bool Kalman_inited;
 
 //Initialises the kalman filter
 void KalmanInit();
 
-// reset kalman states
-void KalmanReset();
 }
 
 #endif //KALMAN_H
\ No newline at end of file
diff -r 4b993a9a156e -r 70d651156779 Processes/Printing/Printing.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Processes/Printing/Printing.cpp	Tue Apr 09 15:33:36 2013 +0000
@@ -0,0 +1,85 @@
+#include "Printing.h"
+#ifdef PRINTINGOFF
+void printingThread(void const*){Thread::wait(osWaitForever);}
+bool registerID(char, size_t){return true;}
+bool unregisterID(char) {return true;}
+bool updateval(char, float*, size_t){return true;}
+bool updateval(char id, float value){return true;}
+#else
+#include <iostream>
+using namespace std;
+
+size_t idlist[NUMIDS]; // Stores length of buffer 0 => unassigned
+float* buffarr[NUMIDS];
+volatile unsigned int newdataflags;
+
+bool registerID(char id, size_t length) {   
+    if (id < NUMIDS && !idlist[id]) {//check if the id is already taken
+        idlist[id] = length;
+        buffarr[id] = new float[length];
+        return true;
+    } else
+        return false;
+}
+bool unregisterID(char id) {
+    if (id < NUMIDS) {
+        idlist[id] = 0;
+        if (buffarr[id])
+            delete buffarr[id];
+        return true;
+    } else
+        return false;
+}
+
+bool updateval(char id, float* buffer, size_t 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 (size_t i = 0; i < length; i++)
+            buffarr[id][i] = buffer[i];
+        newdataflags |= (1<<id);
+        return true;
+    } else
+        return false;
+}
+
+bool 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;
+}
+
+void printingThread(void const*){
+    newdataflags = 0;
+    for (int i = 0; i < NUMIDS; i++) {
+        idlist[i] = 0;
+        buffarr[i] = 0;
+    }
+
+
+    Thread::wait(3500);
+    while(true){   
+        // Send number of packets
+        char numtosend = 0;
+        for (unsigned int v = newdataflags; v; numtosend++){v &= v - 1;}        
+        cout.put(numtosend);
+
+        // Send packets
+        for (char id = 0; id < NUMIDS; id++) {
+            if (newdataflags & (1<<id)) {
+                cout.put(id);
+                cout.write((char*)buffarr[id], idlist[id] * sizeof(float));
+                newdataflags &= ~(1<<id);
+            }
+        }
+        cout << endl;
+        Thread::wait(200);
+    }
+}
+#endif
+
+
+ 
diff -r 4b993a9a156e -r 70d651156779 Sensors/Encoders/Encoder.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Encoders/Encoder.cpp	Tue Apr 09 15:33:36 2013 +0000
@@ -0,0 +1,8 @@
+
+#include "globals.h"
+#include "Encoder.h"
+
+#ifdef ENABLE_GLOBAL_ENCODERS
+    Encoder right_encoder(P_ENC_RIGHT_A, P_ENC_RIGHT_B);
+    Encoder left_encoder(P_ENC_LEFT_A, P_ENC_LEFT_B);
+#endif
\ No newline at end of file
diff -r 4b993a9a156e -r 70d651156779 Sensors/Encoders/Encoder.h
--- a/Sensors/Encoders/Encoder.h	Sun Apr 07 19:26:07 2013 +0000
+++ b/Sensors/Encoders/Encoder.h	Tue Apr 09 15:33:36 2013 +0000
@@ -1,8 +1,10 @@
 
-// Eurobot13 Encoder.cpp
+#ifndef ENCODER_H
+#define ENCODER_H
 
 #include "QEI.h"
 #include "mbed.h"
+#include "globals.h"
 
 class Encoder{
 private:
@@ -19,7 +21,7 @@
     yellow.mode(PullUp);
     }
     
-    int getPoint(void){
+    int getTicks(void){
         return wheel.getPulses();
     }
     
@@ -27,3 +29,10 @@
         return wheel.reset();
     }
 };
+
+#ifdef ENABLE_GLOBAL_ENCODERS
+    extern Encoder right_encoder;
+    extern Encoder left_encoder;
+#endif
+
+#endif //ENCODER_H
diff -r 4b993a9a156e -r 70d651156779 Sensors/Sonar/Sonar.h
--- a/Sensors/Sonar/Sonar.h	Sun Apr 07 19:26:07 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,2 +0,0 @@
-
-// Eurobot13 Sonar.h
\ No newline at end of file
diff -r 4b993a9a156e -r 70d651156779 globals.h
--- a/globals.h	Sun Apr 07 19:26:07 2013 +0000
+++ b/globals.h	Tue Apr 09 15:33:36 2013 +0000
@@ -1,4 +1,82 @@
 
+#ifndef GLOBALS_H
+#define GLOBALS_H
+
+#include "mbed.h"
+
+const float KALMAN_PREDICT_PERIOD = 0.05; //seconds
+
+#define ENABLE_GLOBAL_ENCODERS
+
+const float ENCODER_M_PER_TICK = 0.00084; //TODO: measure this!!
+const float ENCODER_WHEELBASE = 0.068; //TODO: measrue this!!
+const float TURRET_FWD_PLACEMENT = 0.042;
+
+//Robot movement constants
+const float fwdvarperunit = 0.01; //1 std dev = 7% //TODO: measrue this!!
+const float varperang = 0.01; //around 1 degree stddev per 180 turn //TODO: measrue this!!
+const float xyvarpertime = 0.0005; //(very poorly) accounts for hitting things
+const float angvarpertime = 0.001;
+
+/*
+PINOUT Sensors
+5:  RF:SDI
+6 SDO
+7 SCK
+8 NCS
+9 NIRQ
+10-15 6 echo pins
+16 trig
+17 IRin
+18-20 unused
+21 stepper step
+22-27 unused
+28 Serial TX
+29-30 unused
+
+
+PINOUT Main
+5: Lower arm servo
+6: Upper arm servo
+
+14: Serial RX
+15: Cake distance sensor
+16: Fwd distance sensor
+
+20: color sensor in
+21-24: Motors PWM IN 1-4
+25-26: Encoders 
+27-28: Encoders 
+29: Color sensor RED LED
+30: Color sensor BLUE LED
+
+*/
+
+const PinName P_SERVO_LOWER_ARM = p5;
+const PinName P_SERVO_UPPER_ARM = p6;
+
+const PinName P_SERIAL_RX       = p14;
+const PinName P_DISTANCE_SENSOR = p15;
+
+const PinName P_COLOR_SENSOR_IN = p20;
+
+const PinName P_MOT_RIGHT_A     = p21;
+const PinName P_MOT_RIGHT_B     = p22;
+const PinName P_MOT_LEFT_A      = p23;
+const PinName P_MOT_LEFT_B      = p24;
+
+const PinName P_ENC_RIGHT_A     = p28;
+const PinName P_ENC_RIGHT_B     = p27;
+const PinName P_ENC_LEFT_A      = p25;
+const PinName P_ENC_LEFT_B      = p26;
+
+const PinName P_COLOR_SENSOR_RED = p29;
+const PinName P_COLOR_SENSOR_BLUE = p30;
+
+
+
+//a type which is a pointer to a rtos thread function
+typedef void (*tfuncptr_t)(void const *argument);
 
 //Solving for sonar bias is done by entering the following into wolfram alpha
 //(a-f)^2 = x^2 + y^2; (b-f)^2 = (x-3)^2 + y^2; (c-f)^2 = (x-1.5)^2+(y-2)^2: solve for x,y,f
@@ -13,4 +91,6 @@
 
 extern pos beaconpos[3];
 
-const float PI = 3.14159265359;
\ No newline at end of file
+const float PI = 3.14159265359;
+
+#endif //GLOBALS_H
\ No newline at end of file
diff -r 4b993a9a156e -r 70d651156779 main.cpp
--- a/main.cpp	Sun Apr 07 19:26:07 2013 +0000
+++ b/main.cpp	Tue Apr 09 15:33:36 2013 +0000
@@ -1,77 +1,18 @@
-//#pragma Otime // Compiler Optimisations
-
-// Eurobot13 main.cpp
-
-
-
-/*
-PINOUT Sensors
-5:  RF:SDI
-6 SDO
-7 SCK
-8 NCS
-9 NIRQ
-10-15 6 echo pins
-16 trig
-17 IRin
-18-20 unused
-21 stepper step
-22-27 unused
-28 Serial TX
-29-30 unused
-
-
-PINOUT Main
-5: Lower arm servo
-6: Upper arm servo
-
-14: Serial RX
-15: Distance sensor
-
-20: color sensor in
-21-24: Motors PWM IN 1-4
-25-26: Encoders 
-27-28: Encoders 
-29: Color sensor RED LED
-30: Color sensor BLUE LED
-
-*/
+#include "globals.h"
+#include "Kalman.h"
 #include "mbed.h"
 #include "rtos.h"
-#include "globals.h"
-Serial pc(USBTX, USBRX);
-
-const PinName P_SERVO_LOWER_ARM = p5;
-const PinName P_SERVO_UPPER_ARM = p6;
-
-const PinName P_SERIAL_RX       = p14;
-const PinName P_DISTANCE_SENSOR = p15;
-
-const PinName P_COLOR_SENSOR_IN = p20;
-
-const PinName P_MOT_RIGHT_A     = p21;
-const PinName P_MOT_RIGHT_B     = p22;
-const PinName P_MOT_LEFT_A      = p23;
-const PinName P_MOT_LEFT_B      = p24;
-
-const PinName P_ENC_RIGHT_A     = p28;
-const PinName P_ENC_RIGHT_B     = p27;
-const PinName P_ENC_LEFT_A      = p25;
-const PinName P_ENC_LEFT_B      = p26;
-
-const PinName P_COLOR_SENSOR_RED = p29;
-const PinName P_COLOR_SENSOR_BLUE = p30;
-
-pos beaconpos[] = {{0,0}, {0,2}, {3,1}};
-
 #include "Actuators/Arms/Arm.h"
 #include "Actuators/MainMotors/MainMotor.h"
 #include "Sensors/Encoders/Encoder.h"
 #include "Sensors/Colour/Colour.h"
 #include "Sensors/CakeSensor/CakeSensor.h"
 #include "Processes/Printing/Printing.h"
+#include "coprocserial.h"
 #include <algorithm>
 
+pos beaconpos[] = {{0,1}, {3,0}, {3,2}};
+
 void motortest();
 void encodertest();
 void motorencodetest();
@@ -105,9 +46,9 @@
     //ledphototransistortest();
     //colourtest(); // Red SnR too low
     //cakesensortest();
-    feedbacktest();
+    //feedbacktest();
     
-    /*
+     /*
     DigitalOut l1(LED1);
     Thread p(printingThread,        NULL,   osPriorityNormal,   2048);
     l1=1;
@@ -116,11 +57,19 @@
     Thread::wait(osWaitForever);
     */
     
-    //Kalman test threads
-    //Ticker predictticker;
-    //predictthread(predictloopwrapper, this, osPriorityNormal, 512)
-    //updatethread(updateloopwrapper, this, osPriorityNormal, 512)
-    //predictticker( SIGTICKARGS(predictthread, 0x1) ),
+    
+    InitSerial();
+    //while(1)
+    //    printbuff();
+    wait(1);
+    Kalman::KalmanInit();
+    
+    Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k
+    
+    Kalman::start_predict_ticker(&predictthread);
+    //Thread::wait(osWaitForever);
+    feedbacktest();
+    
 }
 
 #include <cstdlib>
@@ -132,7 +81,7 @@
     registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
     while (true){
         for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
-            buffer[i] =ID ;
+            buffer[i] = ID ;
         }
         updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
         Thread::wait(200);
@@ -154,18 +103,21 @@
 
 
 void feedbacktest(){
-    Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
+    //Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
     MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
     
-    float Pgain = -0.005;
+    Kalman::State state;
+    
+    float Pgain = -0.01;
     float fwdspeed = -400/3.0f;
     Timer timer;
     timer.start();
     
     while(true){
         float expecdist = fwdspeed * timer.read();
-        float errleft = Eleft.getPoint() - (expecdist*1.05);
-        float errright = Eright.getPoint() - expecdist;
+        state = Kalman::getState();
+        float errleft = left_encoder.getTicks() - (expecdist);
+        float errright = right_encoder.getTicks() - expecdist;
         
         mleft(max(min(errleft*Pgain, 0.4f), -0.4f));
         mright(max(min(errright*Pgain, 0.4f), -0.4f));
@@ -174,12 +126,12 @@
 
 void cakesensortest(){
     wait(1);
-    pc.printf("cakesensortest");
+    printf("cakesensortest");
     
     CakeSensor cs(P_COLOR_SENSOR_IN);
     while(true){
         wait(0.1);
-        pc.printf("distance: %f\t %f\r\n", cs.Distance(),cs.Distanceincm());
+        printf("distance: %f\t %f\r\n", cs.Distance(),cs.Distanceincm());
         }
 }
 
@@ -191,19 +143,19 @@
         ColourEnum ce = c.getColour();
         switch(ce){
             case BLUE :
-                pc.printf("BLUE\n\r");
+                printf("BLUE\n\r");
                 break;
             case RED:
-                pc.printf("RED\n\r");
+                printf("RED\n\r");
                 break;
             case WHITE:
-                pc.printf("WHITE\n\r");
+                printf("WHITE\n\r");
                 break;
             case INCONCLUSIVE:
-                pc.printf("INCONCLUSIVE\n\r");
+                printf("INCONCLUSIVE\n\r");
                 break;
             default:
-                pc.printf("BUG\n\r");
+                printf("BUG\n\r");
         }
     }
 
@@ -219,23 +171,23 @@
         blue = 0; red = 0;
         for(int i = 0; i != 5; i++){
             wait(0.1);
-            pc.printf("Phototransistor Analog is (none): %f \n\r", pt.read());
+            printf("Phototransistor Analog is (none): %f \n\r", pt.read());
         }
     
         blue = 1; red = 0;
         for(int i = 0; i != 5; i++){
             wait(0.1);
-            pc.printf("Phototransistor Analog is (blue): %f \n\r", pt.read());
+            printf("Phototransistor Analog is (blue): %f \n\r", pt.read());
         }
         blue = 0; red = 1;
         for(int i = 0; i != 5; i++){
             wait(0.1);
-            pc.printf("Phototransistor Analog is (red ): %f \n\r", pt.read());
+            printf("Phototransistor Analog is (red ): %f \n\r", pt.read());
         }
         blue = 1; red = 1;
         for(int i = 0; i != 5; i++){
             wait(0.1);
-            pc.printf("Phototransistor Analog is (both): %f \n\r", pt.read());
+            printf("Phototransistor Analog is (both): %f \n\r", pt.read());
         }
     } 
 }
@@ -244,7 +196,7 @@
     AnalogIn pt(P_COLOR_SENSOR_IN); 
     while(true){
         wait(0.1);
-        pc.printf("Phototransistor Analog is: %f \n\r", pt.read());
+        printf("Phototransistor Analog is: %f \n\r", pt.read());
     }
 
 }
@@ -285,8 +237,8 @@
     mleft(speed); mright(speed);
     servoTimer.start();
     while (true){
-        pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
-        if (Eleft.getPoint() < Eright.getPoint()){
+        printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
+        if (Eleft.getTicks() < Eright.getTicks()){
             mleft(speed);
             mright(speed - dspeed);
         } else {
@@ -330,8 +282,8 @@
     while (true){
     //left 27 cm = 113 -> 0.239 cm/pulse
     //right 27 cm = 72 -> 0.375 cm/pulse
-        pc.printf("Position is: %i \t %i \n\r", (int)(Eleft.getPoint()*0.239), (int)(Eright.getPoint()*0.375));
-        if (Eleft.getPoint()*0.239 < Eright.getPoint()*0.375){
+        printf("Position is: %i \t %i \n\r", (int)(Eleft.getTicks()*0.239), (int)(Eright.getTicks()*0.375));
+        if (Eleft.getTicks()*0.239 < Eright.getTicks()*0.375){
             mright(speed - dspeed);
         } else {
             mright(speed + dspeed);
@@ -349,13 +301,13 @@
     const int enc = -38;
     while(true){
         mleft(speed); mright(0);
-        while(Eleft.getPoint()>enc){
-            pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
+        while(Eleft.getTicks()>enc){
+            printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
         }
         Eleft.reset(); Eright.reset();
         mleft(0); mright(speed);
-        while(Eright.getPoint()>enc){
-            pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
+        while(Eright.getTicks()>enc){
+            printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
         }
         Eleft.reset(); Eright.reset();
     }
@@ -367,7 +319,7 @@
     Serial pc(USBTX, USBRX);
     while(true){
         wait(0.1);
-        pc.printf("Position is: %i \t %i \n\r", E1.getPoint(), 0);//E2.getPoint());
+        printf("Position is: %i \t %i \n\r", E1.getTicks(), 0);//E2.getTicks());
     }
 
 }
diff -r 4b993a9a156e -r 70d651156779 supportfuncs.h
--- a/supportfuncs.h	Sun Apr 07 19:26:07 2013 +0000
+++ b/supportfuncs.h	Tue Apr 09 15:33:36 2013 +0000
@@ -3,7 +3,7 @@
 
 #include <cmath>
 #include "globals.h"
-
+#include "tvmet/Matrix.h"
 
 //Constrains agles to +/- pi
 inline float constrainAngle(float x){
@@ -13,4 +13,12 @@
     return x - PI;
 }
 
+template <typename T>
+tvmet::Matrix <T, 2, 2> Rotmatrix(T theta) {
+     tvmet::Matrix <T, 2, 2> outmatrix;
+     outmatrix = cos(theta), -sin(theta),
+                 sin(theta), cos(theta);
+     return outmatrix;
+}
+
 #endif //SUPPORTFUNCS_H
\ No newline at end of file