Revision 22:167dacfe0b14, committed 2013-04-09

Comitter:

madcowswe

Date:

Tue Apr 09 19:24:31 2013 +0000

Parent:

20:70d651156779

Child:

23:6e3218cf75f8

Commit message:

Working state UI

Changed in this revision

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

--- a/Processes/Kalman/Kalman.h	Tue Apr 09 15:33:36 2013 +0000
+++ b/Processes/Kalman/Kalman.h	Tue Apr 09 19:24:31 2013 +0000
@@ -17,8 +17,8 @@
 State getState();
 
 //Main loops (to be attached as a thread in main)
-void predictloop(void const *dummy);
-void updateloop(void const *dummy);
+void predictloop(void const*);
+void updateloop(void const*);
 
 void start_predict_ticker(Thread* predict_thread_ptr_in);
 

--- a/Processes/Printing/Printing.cpp	Tue Apr 09 15:33:36 2013 +0000
+++ b/Processes/Printing/Printing.cpp	Tue Apr 09 19:24:31 2013 +0000
@@ -1,17 +1,21 @@
 #include "Printing.h"
+#include <iostream>
+
+namespace Printing {
+
 #ifdef PRINTINGOFF
-void printingThread(void const*){Thread::wait(osWaitForever);}
+void printingloop(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;
+size_t idlist[NUMIDS] = {0}; // Stores length of buffer 0 => unassigned
+float* buffarr[NUMIDS] = {0};
+volatile unsigned int newdataflags = 0;
 
 bool registerID(char id, size_t length) {   
     if (id < NUMIDS && !idlist[id]) {//check if the id is already taken
@@ -52,16 +56,16 @@
         return false;
 }
 
-void printingThread(void const*){
-    newdataflags = 0;
-    for (int i = 0; i < NUMIDS; i++) {
-        idlist[i] = 0;
-        buffarr[i] = 0;
-    }
+void printingloop(void const*){
 
-
-    Thread::wait(3500);
-    while(true){   
+    Serial pc(USBTX, USBRX);
+    pc.baud(115200);
+    
+    char sync[] = "ABCD";
+    cout.write(sync, 4);
+    cout << std::endl;
+    
+    while(true){
         // Send number of packets
         char numtosend = 0;
         for (unsigned int v = newdataflags; v; numtosend++){v &= v - 1;}        
@@ -79,7 +83,9 @@
         Thread::wait(200);
     }
 }
-#endif
 
 
+#endif //end PRINTINGOFF
+
+} //end namespace
  

--- a/Processes/Printing/Printing.h	Tue Apr 09 15:33:36 2013 +0000
+++ b/Processes/Printing/Printing.h	Tue Apr 09 19:24:31 2013 +0000
@@ -4,13 +4,17 @@
 #include "mbed.h"
 #include "rtos.h"
 
+namespace Printing {
+
 const size_t NUMIDS = sizeof(unsigned int)*8;
 
 //Function to start in Thread
-void printingThread(void const*); //
+void printingloop(void const*); //
 
 //Functions to use 
 bool registerID(char id, size_t length);
 bool unregisterID(char id);
 bool updateval(char id, float* buffer, size_t length);
 bool updateval(char id, float value);
+
+}

--- a/main.cpp	Tue Apr 09 15:33:36 2013 +0000
+++ b/main.cpp	Tue Apr 09 19:24:31 2013 +0000
@@ -2,12 +2,12 @@
 #include "Kalman.h"
 #include "mbed.h"
 #include "rtos.h"
-#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 "Arm.h"
+#include "MainMotor.h"
+#include "Encoder.h"
+#include "Colour.h"
+#include "CakeSensor.h"
+#include "Printing.h"
 #include "coprocserial.h"
 #include <algorithm>
 
@@ -50,7 +50,7 @@
     
      /*
     DigitalOut l1(LED1);
-    Thread p(printingThread,        NULL,   osPriorityNormal,   2048);
+    Thread p(Printing::printingloop,        NULL,   osPriorityNormal,   2048);
     l1=1;
     Thread a(printingtestthread,    NULL,   osPriorityNormal,   1024);
     Thread b(printingtestthread2,   NULL,   osPriorityNormal,   1024);
@@ -68,7 +68,11 @@
     
     Kalman::start_predict_ticker(&predictthread);
     //Thread::wait(osWaitForever);
-    feedbacktest();
+    //feedbacktest();
+    
+    Thread::wait(3500);
+    Thread printingThread(Printing::printingloop, NULL, osPriorityLow, 2048);
+    Thread::wait(osWaitForever);
     
 }
 
@@ -78,12 +82,12 @@
 void printingtestthread(void const*){
     const char ID = 1;
     float buffer[3] = {ID};
-    registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
+    Printing::registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
     while (true){
         for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
             buffer[i] = ID ;
         }
-        updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
+        Printing::updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
         Thread::wait(200);
     }
 }
@@ -91,12 +95,12 @@
 void printingtestthread2(void const*){
     const char ID = 2;
     float buffer[5] = {ID};
-    registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
+    Printing::registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
     while (true){
         for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
             buffer[i] = ID;
         }
-        updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
+        Printing::updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
         Thread::wait(500);
     }
 }