Diff: main.cpp

Revision:

5:8c276289611c

Parent:

4:83c7e402fff1

Child:

6:4437975b9cf9

--- a/main.cpp	Tue Nov 26 16:23:08 2019 +0000
+++ b/main.cpp	Thu Nov 28 17:56:09 2019 +0000
@@ -31,6 +31,7 @@
 using namespace std;
 using namespace Eigen;
 
+AnalogIn ain(A0);
 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
@@ -38,7 +39,7 @@
 BiQuadChain bqc;
 
 
-#define SLEEP_TIME                  20 // (msec)
+#define SLEEP_TIME                  80 // (msec)
 
 
 const static char DEVICE_NAME[] = "STEP COUNTER";
@@ -73,7 +74,7 @@
         t.start(callback(&_event_queue, &EventQueue::dispatch_forever));
     }
     void update_step_count() {
-        printf("updating step count %d\n\r", step_count);
+        //printf("updating step count %d\n\r", step_count);
         if (_ble.gap().getState().connected) {
             _ble.gattServer().write(step_count_state.getValueHandle(), (uint8_t *)&step_count, sizeof(int));
         }
@@ -82,7 +83,7 @@
 private:
     /** Callback triggered when the ble initialization process has finished */
     void on_init_complete(BLE::InitializationCompleteCallbackContext *params) {
-        printf("on init complete\n\r");
+        //printf("on init complete\n\r");
         if (params->error != BLE_ERROR_NONE) {
             print_error(params->error, "Ble initialization failed.");
             return;
@@ -96,7 +97,7 @@
     }
 
     void start_advertising() {
-        printf("start advertising\n\r");
+        //printf("start advertising\n\r");
         /* Create advertising parameters and payload */
 
         ble::AdvertisingParameters adv_parameters(
@@ -197,13 +198,13 @@
     float GyroRead[3];
     float TempRead[1];
     float res_smooth;
-    //vector<float> res_list;
     float threshold=0.1;
     int number=0;
     int numpeak = 0;
     
-    static MatrixXd acc_raw(3,0);
+    MatrixXd acc_raw(3,0);
     Vector3d acc_new;
+    Vector3d acc_previous;
     MatrixXd C;
     MatrixXd vec, val;
     int dim = 1;    //dimension of PCA
@@ -213,17 +214,12 @@
     
     bqc.add(&mybq);
     
-    static vector<float> res_list;
-    
+    //vector<float> res_list;
+    acc_new << 0,0,0;
     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);
+        thread_sleep_for(SLEEP_TIME);
         //read and convert date
         mpu->ReadConvertAll(AccRead,GyroRead,TempRead);
         AccRead[0]= AccRead[0]/1000;
@@ -235,24 +231,29 @@
         
         //append new data to matrix acc_raw
         //adding the columns
+        acc_previous = acc_new;
         acc_new << AccRead[0],AccRead[1],AccRead[2];
+        //cout << "acc_previous:"<<acc_previous<<"\n\r";
+        //printf("\n\r");
+        
         acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
         acc_raw.col(acc_raw.cols()-1) = acc_new;
-        printf("before if\n\r");
-        printf("number: %d\n\r", number);
+        
+        //////cout << "acc_raw:" << acc_raw << endl;
         if(number % 10 ==2)
         {
-            printf("in if\n\r");
             if(number > 2)
             {
-                //cout << acc_raw << endl;
+                //cout << "acc_raw"<< acc_raw << "\n\r";
+                //printf("\n\r");
+                
                 //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;
+                MatrixXd res = -vec.rightCols(dim).transpose()*X1;
                 
                 //show the result after PCA
                 //////cout << "result" << res << endl;
@@ -261,21 +262,28 @@
                 //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);
+                    res_list.push_back(res(i));
+                    //res_smooth = bqc.step(res(i));
+                    //res_list.push_back(res_smooth);
+                    //printf("result%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("NUMBER OF PEAKS: %d\n\r", numpeak);
                 //printf("height in main: %f\n\r", threshold);
                 step_count += numpeak;
-                printf("num of step: %d\n\r", step_count);
+                //printf("num of step: %d\n\r", step_count);
                 
                 //clear the matrix to contain new data
                 acc_raw.conservativeResize(3, 0);
+                
+                //overlap windows
+                //acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
+//                acc_raw.col(acc_raw.cols()-1) = acc_previous;
+//                
+//                acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
+//                acc_raw.col(acc_raw.cols()-1) = acc_new;
             }
         }
         number = number +1;