main.cpp

00001 #include "stdio.h"
00002 #include "mbed.h"
00003 #include "IMU.h"
00004 #include "KalmanFilter.h"
00005 
00006 //DigitalOut myled(LED_RED);
00007 Serial pc(USBTX, USBRX);
00008 
00009 IMU imu(PTE25,PTE24);
00010 
00011 KalmanFilter kf;
00012 
00013 Ticker triger1; //przerwanie filtracji
00014 Ticker triger2; //przerwanie wysyłania danych
00015 
00016 float d[9];
00017 double D[9];
00018 float kf_update;
00019 char buff[40];
00020 
00021 void task1()
00022 {
00023     imu.readData(d);
00024     //Filtr Buterwortha
00025     imu.filterData(d, D);
00026     //sprintf(buff, "%f\n%f\r", d[3], D[3]);
00027     
00028     //Filtr Kalmana
00029     kf_update = kf.update(d[2], d[5]);
00030     //Wyslanie wartosci w protekole szeregowym do Labview (zmienic liczbe danych w labView jest sprintf wysyla wiecej wartosci).
00031     sprintf(buff, "%f\n%f\n%f\r", d[5], D[5], (float) kf_update);
00032 //    sprintf(buff, "%f\n%f\n%f\r", d[6], d[7], d[8]);
00033 }
00034 
00035 void task2()
00036 {
00037     pc.printf(buff);
00038 }
00039 
00040         
00041 int main()
00042 {
00043     //char buff[10];
00044     //float d[9]; //tablica wartosci przed filtracja
00045     //double D[9]; //tablica wartosci po filtracji
00046     //float kf_update;
00047     imu.init();
00048     //imu.readData(d);
00049     //imu.filterData(d,D); 
00050     kf.reset();    
00051     pc.baud(115200);
00052 
00053     triger1.attach(&task1, 0.0025);
00054     triger2.attach(&task2, 0.05);
00055     
00056     while (true) {        
00057         //pc.printf("%f\n%f\n%f\r", d[3], kf_update, kf_estimated_data);
00058         wait(1.0f);
00059         // Komentarz wprowadzam w jezyku angielskim, poniewaz program jest dostepny publicznie:::
00060         // Hex char 0xD = \r is the termination char in the LabVIEW VISA Configure Serial Port vi.
00061         // It points to the end of string and separates single vector of acquired data. 
00062         // Hex char 0xA = \n points to the new line. LabVIEW Pick Line vi selects the line with a float represented by string.
00063         // Then the float is encoded from the string and put into some array until all lines are processed.
00064         // The array elemnts are sent to the Waveform Chart to plot the data.
00065         // Using that syntax below, three series will be ploted.
00066         // pc.printf("%f\n%f\n%f\r", D[3],D[4],D[5]); //accelerations [m/s^2] from accel
00067         //pc.printf("%f\n%f\n%f\r", D[0],D[1],D[2]); //angular velocitites [rad/s] from gyro
00068         //pc.printf("%f\n%f\n%f\r", D[6],D[7],D[8]); //angle [rad] from magneto
00069         //
00070         //myled = !myled; // toggle a led
00071     }
00072 }