Diff: main.cpp

Revision:

8:7a22b8294c5d

Parent:

7:986d5298b118

Child:

9:3694ca4b48a7

--- a/main.cpp	Wed Jul 16 09:59:53 2014 +0000
+++ b/main.cpp	Mon Dec 08 13:21:33 2014 +0000
@@ -1,17 +1,70 @@
+#include "stdio.h"
 #include "mbed.h"
+#include "IMU.h"
+#include "KalmanFilter.h"
 
-DigitalOut myled(LED_GREEN);
+//DigitalOut myled(LED_RED);
 Serial pc(USBTX, USBRX);
 
+IMU imu(PTE25,PTE24);
+
+KalmanFilter kf;
+
+Ticker triger1; //przerwanie filtracji
+Ticker triger2; //przerwanie wysyłania danych
+
+float d[9];
+double D[9];
+float kf_update;
+char buff[40];
+
+void task1()
+{
+    imu.readData(d);
+    //Filtr Buterwortha
+    imu.filterData(d, D);
+    //sprintf(buff, "%f\n%f\r", d[3], D[3]);
+    
+    //Filtr Kalmana
+    kf_update = kf.update(d[2], d[5]);
+    sprintf(buff, "%f\n%f\n%f\r", d[5], D[5], (float) kf_update);
+//    sprintf(buff, "%f\n%f\n%f\r", d[6], d[7], d[8]);
+}
+
+void task2()
+{
+    pc.printf(buff);
+}
+
+        
 int main()
 {
-    int i = 0;
-    pc.printf("Hello World!\n");
+    //char buff[10];
+    //float d[9]; //tablica wartosci przed filtracja
+    //double D[9]; //tablica wartosci po filtracji
+    //float kf_update;
+    imu.init();
+    //imu.readData(d);
+    //imu.filterData(d,D); 
+    kf.reset();    
+    pc.baud(115200);
 
-    while (true) {
-        wait(0.5f); // wait a small period of time
-        pc.printf("%d \n", i); // print the value of variable i
-        i++; // increment the variable
-        myled = !myled; // toggle a led
+    triger1.attach(&task1, 0.0025);
+    triger2.attach(&task2, 0.05);
+    
+    while (true) {        
+        //pc.printf("%f\n%f\n%f\r", d[3], kf_update, kf_estimated_data);
+        wait(1.0f);
+        // Hex char 0xD = \r is the termination char in the LabVIEW VISA Configure Serial Port vi.
+        // It points to the end of string and separates single vector of acquired data. 
+        // Hex char 0xA = \n points to the new line. LabVIEW Pick Line vi selects the line with a float represented by string.
+        // Then the float is encoded from the string and put into some array until all lines are processed.
+        // The array elemnts are sent to the Waveform Chart to plot the data.
+        // Using that syntax below, three series will be ploted.
+        // pc.printf("%f\n%f\n%f\r", D[3],D[4],D[5]); //accelerations [m/s^2] from accel
+        //pc.printf("%f\n%f\n%f\r", D[0],D[1],D[2]); //angular velocitites [rad/s] from gyro
+        //pc.printf("%f\n%f\n%f\r", D[6],D[7],D[8]); //angle [rad] from magneto
+        //
+        //myled = !myled; // toggle a led
     }
 }