main.cpp

00001 /* 
00002  * read and print acc, gyro,temperature date from MPU9250
00003  * and transform accelerate data to one dimension.
00004  * in terminal:
00005  *  ls /dev/tty.*
00006  *  screen /dev/tty.usbmodem14102 9600
00007  * to see the result
00008  *
00009  * mbed Microcontroller Library
00010  * Eigen Library
00011  */
00012 
00013 #include "mbed.h"
00014 #include "platform/mbed_thread.h"
00015 #include "stats_report.h"
00016 #include "MPU9250.h"
00017 //#include <Eigen/Dense.h>
00018 #include <iostream>
00019 #include <vector>  
00020 #include <complex>
00021 #include "BiQuad.h"
00022 #include "pca.h"
00023 #include "peak.h"
00024 
00025 using namespace std;
00026 using namespace Eigen;
00027 
00028 DigitalOut led1(LED1);
00029 const int addr7bit = 0x68; // 7bit I2C address,AD0 is 0
00030 //the parameter of biquad filter, 40Hz sampling frequence,10Hz cut-off freq, Q:0.719
00031 BiQuad mybq(0.3403575989782886,0.6807151979565772,0.3403575989782886, -1.511491371967327e-16,0.36143039591315457);
00032 BiQuadChain bqc;
00033 
00034 
00035 #define SLEEP_TIME                  20 // (msec)
00036 
00037 
00038 // main() runs in its own thread in the OS
00039 int main()
00040 {   
00041     //new mpu(data,clk,address),in constructor addr7bit<<1
00042     mpu9250 *mpu = new mpu9250(p26,p27,addr7bit);
00043     //scale of acc and gyro
00044     mpu->initMPU9250(0x00,0x00);
00045 
00046     float AccRead[3];
00047     float GyroRead[3];
00048     float TempRead[1];
00049     float res_smooth;
00050     //vector<float> res_list;
00051     float number=0;
00052     
00053     static MatrixXd acc_raw(3,0);
00054     Vector3d acc_new;
00055     MatrixXd C;
00056     MatrixXd vec, val;
00057     int dim = 1;    //dimension of PCA
00058     //use the class defined in pca.h and peak.h
00059     PCA pca;
00060     PEAK peak;
00061     
00062     bqc.add(&mybq);
00063     
00064     static vector<float> res_list;
00065     
00066     while (true) {
00067         
00068         //when i add to 600, there accure faugment error
00069         //printf("the %f loop", number);
00070         number = number +1;
00071         //vector<float> res_list;
00072         //Blink LED and wait 1 seconds
00073         led1 = !led1;
00074         thread_sleep_for(SLEEP_TIME);
00075         //read and convert date
00076         mpu->ReadConvertAll(AccRead,GyroRead,TempRead);
00077         AccRead[0]= AccRead[0]/1000;
00078         AccRead[1]= AccRead[1]/1000;
00079         AccRead[2]= AccRead[2]/1000;
00080         printf("acc value is (%f,%f,%f).\n\r",AccRead[0],AccRead[1],AccRead[2]);
00081         //printf("gyro value is (%f,%f,%f).\n\r",GyroRead[0],GyroRead[1],GyroRead[2]);
00082         //printf("temp value is %f.\n\r",TempRead[0]);
00083         
00084         //append new data to matrix acc_raw
00085         //adding the columns
00086         acc_new << AccRead[0],AccRead[1],AccRead[2];
00087         acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
00088         acc_raw.col(acc_raw.cols()-1) = acc_new;
00089         
00090         //////cout << "acc_raw:" << acc_raw << endl;
00091         
00092         //run PCA
00093         MatrixXd X1=pca.featurnormail(acc_raw);
00094         pca.ComComputeCov(X1, C);
00095         pca.ComputEig(C, vec, val);
00096         //select dim num of eigenvector from right to left. right is important
00097         //compute the result array
00098         MatrixXd res = vec.rightCols(dim).transpose()*X1;
00099         
00100         //show the result after PCA
00101         //////cout << "result" << res << endl;
00102         res_list.clear();
00103         for(int i = 0; i < res.cols(); i++)
00104         {
00105             res_smooth = bqc.step(res(i));
00106             res_list.push_back(res_smooth);
00107             //printf("result after filter in for loop %d: %f\n\r",i,res_smooth);
00108             //std::cout << "\t" << bqc.step(  ) << std::endl;
00109         }
00110         int len = res_list.size();
00111         printf("len of res:%d\n\r", len);
00112         peak.findPeaks(res_list,len,0.1);
00113         
00114         
00115     }
00116 }