Diff: Processes/Kalman/Kalman.cpp

Revision:

22:167dacfe0b14

Parent:

20:70d651156779

Child:

23:6e3218cf75f8

--- a/Processes/Kalman/Kalman.cpp	Tue Apr 09 15:33:36 2013 +0000
+++ b/Processes/Kalman/Kalman.cpp	Tue Apr 09 19:24:31 2013 +0000
@@ -6,7 +6,8 @@
 #include "math.h"
 #include "supportfuncs.h"
 #include "Encoder.h"
-//#include "globals.h"
+#include "globals.h"
+#include "Printing.h"
 
 #include "tvmet/Matrix.h"
 using namespace tvmet;
@@ -97,6 +98,10 @@
     X(0,0) = x_coor;
     X(1,0) = y_coor;
     X(2,0) = 0;
+    
+    P = 0.02*0.02, 0, 0,
+        0, 0.02*0.02, 0,
+        0, 0, 0.04;
     statelock.unlock();
     
     Kalman_inited = 1;
@@ -111,11 +116,11 @@
 }
 
 
-void predictloop(void const *dummy)
+void predictloop(void const*)
 {
 
-    //OLED4 = !ui.regid(0, 3);
-    //OLED4 = !ui.regid(1, 4);
+    OLED4 = !Printing::registerID(0, 3);
+    OLED4 = !Printing::registerID(1, 4);
 
     float lastleft = 0;
     float lastright = 0;
@@ -188,12 +193,12 @@
         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,0), X(1,0), X(2,0)};
-        //ui.updateval(0, statecpy, 3);
+        //Update Printing
+        float statecpy[] = {X(0,0), X(1,0), X(2,0)};
+        Printing::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)};
+        Printing::updateval(1, Pcpy, 4);
 
         statelock.unlock();
     }
@@ -244,7 +249,7 @@
 
 }
 /*
-void Kalman::updateloop(void const *dummy)
+void Kalman::updateloop(void const*)
 {
 
     //sonar Y chanels