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                  10 // (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     float threshold=0.1;
00051     int number=0;
00052     int step = 0;
00053     int numpeak = 0;
00054     
00055     MatrixXd acc_raw(3,0);
00056     Vector3d acc_new;
00057     MatrixXd C;
00058     MatrixXd vec, val;
00059     int dim = 1;    //dimension of PCA
00060     //use the class defined in pca.h and peak.h
00061     PCA pca;
00062     PEAK peak;
00063     
00064     bqc.add(&mybq);
00065     
00066     //vector<float> res_list;
00067     
00068     while (true) {
00069         
00070         //Blink LED and wait 1 seconds
00071         led1 = !led1;
00072         thread_sleep_for(SLEEP_TIME);
00073         //read and convert date
00074         mpu->ReadConvertAll(AccRead,GyroRead,TempRead);
00075         AccRead[0]= AccRead[0]/1000;
00076         AccRead[1]= AccRead[1]/1000;
00077         AccRead[2]= AccRead[2]/1000;
00078         //printf("acc value is (%f,%f,%f).\n\r",AccRead[0],AccRead[1],AccRead[2]);
00079         //printf("gyro value is (%f,%f,%f).\n\r",GyroRead[0],GyroRead[1],GyroRead[2]);
00080         //printf("temp value is %f.\n\r",TempRead[0]);
00081         
00082         //append new data to matrix acc_raw
00083         //adding the columns
00084         acc_new << AccRead[0],AccRead[1],AccRead[2];
00085         acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
00086         acc_raw.col(acc_raw.cols()-1) = acc_new;
00087         
00088         //////cout << "acc_raw:" << acc_raw << endl;
00089         if(number % 10 ==2)
00090         {
00091             if(number > 2)
00092             {
00093                 //cout << acc_raw << endl;
00094                 //run PCA
00095                 MatrixXd X1=pca.featurnormail(acc_raw);
00096                 pca.ComComputeCov(X1, C);
00097                 pca.ComputEig(C, vec, val);
00098                 //select dim num of eigenvector from right to left. right is important
00099                 //compute the result array
00100                 MatrixXd res = vec.rightCols(dim).transpose()*X1;
00101                 
00102                 //show the result after PCA
00103                 //////cout << "result" << res << endl;
00104                 vector<float> res_list={};
00105                 
00106                 //printf("result of PCA size:%d\n\r",res.cols());
00107                 for(int i = 0; i < res.cols(); i++)
00108                 {
00109                     res_smooth = bqc.step(res(i));
00110                     res_list.push_back(res_smooth);
00111                     //printf("result after filter in for loop %d: %f\n\r",i,res_smooth);
00112                     //std::cout << "\t" << bqc.step(  ) << std::endl;
00113                 }
00114                 int len = res_list.size();
00115                 //printf("len of res:%d\n\r", len);
00116                 numpeak = peak.findPeaks(res_list,len,threshold);
00117                 //printf("height in main: %f\n\r", threshold);
00118                 step = step + numpeak;
00119                 printf("num of step: %d\n\r", step);
00120                 
00121                 //clear the matrix to contain new data
00122                 acc_raw.conservativeResize(3, 0);
00123             }
00124         }
00125         number = number +1;
00126     }
00127 }