Diff: main.cpp

Revision:

4:15d6c7123b09

Parent:

2:d4c480d17944

Child:

5:c37def827168

--- a/main.cpp	Mon Nov 25 14:34:22 2019 +0000
+++ b/main.cpp	Mon Nov 25 23:38:03 2019 +0000
@@ -32,7 +32,7 @@
 BiQuadChain bqc;
 
 
-#define SLEEP_TIME                  20 // (msec)
+#define SLEEP_TIME                  100 // (msec)
 
 
 // main() runs in its own thread in the OS
@@ -47,10 +47,12 @@
     float GyroRead[3];
     float TempRead[1];
     float res_smooth;
-    //vector<float> res_list;
-    float number=0;
+    float threshold=0.1;
+    int number=0;
+    int step = 0;
+    int numpeak = 0;
     
-    static MatrixXd acc_raw(3,0);
+    MatrixXd acc_raw(3,0);
     Vector3d acc_new;
     MatrixXd C;
     MatrixXd vec, val;
@@ -61,14 +63,10 @@
     
     bqc.add(&mybq);
     
-    static vector<float> res_list;
+    //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);
@@ -77,7 +75,7 @@
         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("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]);
         
@@ -88,29 +86,42 @@
         acc_raw.col(acc_raw.cols()-1) = acc_new;
         
         //////cout << "acc_raw:" << 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;
-        res_list.clear();
-        for(int i = 0; i < res.cols(); i++)
+        if(number % 10 ==2)
         {
-            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;
+            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);
+            }
         }
-        int len = res_list.size();
-        printf("len of res:%d\n\r", len);
-        peak.findPeaks(res_list,len,0.1);
-        
-        
+        number = number +1;
     }
 }