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 #define SLEEP_TIME                  80 // (msec)
00035 
00036 
00037 // main() runs in its own thread in the OS
00038 int main()
00039 {   
00040     //new mpu(data,clk,address),in constructor addr7bit<<1
00041     mpu9250 *mpu = new mpu9250(p26,p27,addr7bit);
00042     //scale of acc and gyro
00043     mpu->initMPU9250(0x00,0x00);
00044 
00045     float AccRead[3];
00046     float GyroRead[3];
00047     float TempRead[1];
00048     float res_smooth;
00049     float threshold=0.1;
00050     int number=0;
00051     int step = 0;
00052     int numpeak = 0;
00053     
00054     MatrixXd acc_raw(3,0);
00055     Vector3d acc_new;
00056     Vector3d acc_previous;
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     acc_new << 0,0,0;
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_previous = acc_new;
00085         acc_new << AccRead[0],AccRead[1],AccRead[2];
00086         //cout << "acc_previous:"<<acc_previous<<"\n\r";
00087         //printf("\n\r");
00088         
00089         acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
00090         acc_raw.col(acc_raw.cols()-1) = acc_new;
00091         
00092         //////cout << "acc_raw:" << acc_raw << endl;
00093         if(number % 10 ==2)
00094         {
00095             if(number > 2)
00096             {
00097                 //cout << "acc_raw"<< acc_raw << "\n\r";
00098                 //printf("\n\r");
00099                 
00100                 //run PCA
00101                 MatrixXd X1=pca.featurnormail(acc_raw);
00102                 pca.ComComputeCov(X1, C);
00103                 pca.ComputEig(C, vec, val);
00104                 //select dim num of eigenvector from right to left. right is important
00105                 //compute the result array
00106                 MatrixXd res = vec.rightCols(dim).transpose()*X1;
00107                 
00108                 //show the result after PCA
00109                 //////cout << "result" << res << endl;
00110                 vector<float> res_list={};
00111                 
00112                 //printf("result of PCA size:%d\n\r",res.cols());
00113                 for(int i = 0; i < res.cols(); i++)
00114                 {
00115                     res_smooth = bqc.step(res(i));
00116                     res_list.push_back(res_smooth);
00117                     //printf("result%d: %f\n\r",i,res_smooth);
00118                     //std::cout << "\t" << bqc.step(  ) << std::endl;
00119                 }
00120                 int len = res_list.size();
00121                 //printf("len of res:%d\n\r", len);
00122                 numpeak = peak.findPeaks(res_list,len,threshold);
00123                 //printf("height in main: %f\n\r", threshold);
00124                 step = step + numpeak;
00125                 printf("num of step: %d\n\r", step);
00126                 
00127                 //clear the matrix to contain new data
00128                 acc_raw.conservativeResize(3, 0);
00129                 
00130                 //overlap windows
00131                 acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
00132                 acc_raw.col(acc_raw.cols()-1) = acc_previous;
00133                 
00134                 acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
00135                 acc_raw.col(acc_raw.cols()-1) = acc_new;
00136             }
00137         }
00138         number = number +1;
00139     }
00140 }