Diff: main.cpp

Revision:

2:d4c480d17944

Parent:

1:92f42e198925

Child:

4:15d6c7123b09

--- a/main.cpp	Thu Nov 14 01:28:23 2019 +0000
+++ b/main.cpp	Mon Nov 25 14:28:46 2019 +0000
@@ -14,84 +14,25 @@
 #include "platform/mbed_thread.h"
 #include "stats_report.h"
 #include "MPU9250.h"
-#include <Eigen/Dense.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
-
-#define SLEEP_TIME                  5000 // (msec)
+//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;
 
 
-/*
-  * Normalize the Matrix X
-  */
- MatrixXd featurnormail(MatrixXd &X)
-{
-    //I don't know why to use the transpose
-    //compute the mean of every dimension
-    MatrixXd X1 = X.transpose();
-    MatrixXd meanval = X1.colwise().mean();
-    
-    //normalization
-    RowVectorXd meanvecRow = meanval;
-    X1.rowwise() -= meanvecRow;
-    
-    return X1.transpose();
-}
-
- /*
-  * Compute the Covariane Matrix of X, put to C
-  * C = 1/m * X * X.transpose 
-  */
-void ComComputeCov(MatrixXd &X, MatrixXd &C)
-{
-   
-    C = X*X.adjoint();//same as XT*X a
-    //translate to array
-    C = C.array() / X.cols();
-}
- 
- 
-/*
- * Compute the eigenvalue and eigenvector of C
- * val = (first eigenvalue) --smallest --not important
- *              .
- *              .
- *              .
- *       (last eigenvalue)  --largest -- important
- *
- * vec = (first eigenvector, ... , last eigenvector)
- *           not important          important
- */
-void ComputEig(MatrixXd &C, MatrixXd &vec, MatrixXd &val)
-{
-    //SelfAdjointEigenSolver will sort the values automatically
-    SelfAdjointEigenSolver<MatrixXd> eig(C);
-    vec = eig.eigenvectors();
-    val = eig.eigenvalues();
-}
- 
-/* Compute the dimension need to include enough information of raw data.
- * form large index to small index, since the val is sorted from small to large.
- * in some cases, just decide the number of dimension, instead of compute it.
- */
-int ComputDim(MatrixXd &val)
-{
-    int dim;
-    double sum = 0;
-    for (int i = val.rows() - 1; i >= 0;--i)
-    {
-        sum += val(i, 0);
-        dim = i;
-        if (sum / val.sum()>=0.8)//80% of the information
-            break;
-    }
-    return val.rows() - dim;
-}
+#define SLEEP_TIME                  20 // (msec)
 
 
 // main() runs in its own thread in the OS
@@ -105,24 +46,40 @@
     float AccRead[3];
     float GyroRead[3];
     float TempRead[1];
-    
+    float res_smooth;
+    //vector<float> res_list;
+    float number=0;
     
-    MatrixXd acc_raw(3,0);
+    static 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);
+    
+    static vector<float> res_list;
     
     while (true) {
         
+        //when i add to 600, there accure faugment error
+        //printf("the %f loop", number);
+        number = number +1;
+        //vector<float> res_list;
         //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]);
+        //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
@@ -130,17 +87,30 @@
         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;
+        //////cout << "acc_raw:" << acc_raw << endl;
         
         //run PCA
-        MatrixXd X1=featurnormail(acc_raw);
-        ComComputeCov(X1, C);
-        ComputEig(C, vec, val);
+        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;
+        //////cout << "result" << res << endl;
+        res_list.clear();
+        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);
+        peak.findPeaks(res_list,len,0.1);
+        
+        
     }
 }