Revision 20:70d651156779, committed 2013-04-09

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

--- /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

--- /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

--- 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
-

--- 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

--- /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
+
+
+ 

--- /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

--- 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

--- 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

--- 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

--- 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());
     }
 
 }

--- 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