File content as of revision 4:15d6c7123b09:

/* 
 * read and print acc, gyro,temperature date from MPU9250
 * and transform accelerate data to one dimension.
 * in terminal:
 *  ls /dev/tty.*
 *  screen /dev/tty.usbmodem14102 9600
 * to see the result
 *
 * mbed Microcontroller Library
 * Eigen Library
 */

#include "mbed.h"
#include "platform/mbed_thread.h"
#include "stats_report.h"
#include "MPU9250.h"
//#include <Eigen/Dense.h>
#include <iostream>
#include <vector>  
#include <complex>
#include "BiQuad.h"
#include "pca.h"
#include "peak.h"

using namespace std;
using namespace Eigen;

DigitalOut led1(LED1);
const int addr7bit = 0x68; // 7bit I2C address,AD0 is 0
//the parameter of biquad filter, 40Hz sampling frequence,10Hz cut-off freq, Q:0.719
BiQuad mybq(0.3403575989782886,0.6807151979565772,0.3403575989782886, -1.511491371967327e-16,0.36143039591315457);
BiQuadChain bqc;


#define SLEEP_TIME                  100 // (msec)


// main() runs in its own thread in the OS
int main()
{   
    //new mpu(data,clk,address),in constructor addr7bit<<1
    mpu9250 *mpu = new mpu9250(p26,p27,addr7bit);
    //scale of acc and gyro
    mpu->initMPU9250(0x00,0x00);

    float AccRead[3];
    float GyroRead[3];
    float TempRead[1];
    float res_smooth;
    float threshold=0.1;
    int number=0;
    int step = 0;
    int numpeak = 0;
    
    MatrixXd acc_raw(3,0);
    Vector3d acc_new;
    MatrixXd C;
    MatrixXd vec, val;
    int dim = 1;    //dimension of PCA
    //use the class defined in pca.h and peak.h
    PCA pca;
    PEAK peak;
    
    bqc.add(&mybq);
    
    //vector<float> res_list;
    
    while (true) {
        
        //Blink LED and wait 1 seconds
        led1 = !led1;
        thread_sleep_for(SLEEP_TIME);
        //read and convert date
        mpu->ReadConvertAll(AccRead,GyroRead,TempRead);
        AccRead[0]= AccRead[0]/1000;
        AccRead[1]= AccRead[1]/1000;
        AccRead[2]= AccRead[2]/1000;
        //printf("acc value is (%f,%f,%f).\n\r",AccRead[0],AccRead[1],AccRead[2]);
        //printf("gyro value is (%f,%f,%f).\n\r",GyroRead[0],GyroRead[1],GyroRead[2]);
        //printf("temp value is %f.\n\r",TempRead[0]);
        
        //append new data to matrix acc_raw
        //adding the columns
        acc_new << AccRead[0],AccRead[1],AccRead[2];
        acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
        acc_raw.col(acc_raw.cols()-1) = acc_new;
        
        //////cout << "acc_raw:" << acc_raw << endl;
        if(number % 10 ==2)
        {
            if(number > 2)
            {
                //cout << acc_raw << endl;
                //run PCA
                MatrixXd X1=pca.featurnormail(acc_raw);
                pca.ComComputeCov(X1, C);
                pca.ComputEig(C, vec, val);
                //select dim num of eigenvector from right to left. right is important
                //compute the result array
                MatrixXd res = vec.rightCols(dim).transpose()*X1;
                
                //show the result after PCA
                //////cout << "result" << res << endl;
                vector<float> res_list={};
                
                //printf("result of PCA size:%d\n\r",res.cols());
                for(int i = 0; i < res.cols(); i++)
                {
                    res_smooth = bqc.step(res(i));
                    res_list.push_back(res_smooth);
                    printf("result after filter in for loop %d: %f\n\r",i,res_smooth);
                    //std::cout << "\t" << bqc.step(  ) << std::endl;
                }
                int len = res_list.size();
                //printf("len of res:%d\n\r", len);
                numpeak = peak.findPeaks(res_list,len,threshold);
                printf("height in main: %f\n\r", threshold);
                step = step + numpeak;
                printf("num of step: %d\n\r", step);
                
                //clear the matrix to contain new data
                acc_raw.conservativeResize(3, 0);
            }
        }
        number = number +1;
    }
}