Diff: source/main.cpp

Revision:

1:28c107f8cf24

Parent:

0:dbcc46819a5e

diff -r dbcc46819a5e -r 28c107f8cf24 source/main.cpp
--- a/source/main.cpp	Fri Nov 15 15:31:52 2019 +0000
+++ b/source/main.cpp	Wed Nov 20 00:55:20 2019 +0000
@@ -20,6 +20,100 @@
 #include "ble/Gap.h"
 #include "pretty_printer.h"
 
+/***
+Library used by PCA
+
+#include "mbed.h"
+#include "platform/mbed_thread.h"
+#include "stats_report.h"
+#include "MPU9250.h"
+#include <Eigen/Dense.h>
+#include <iostream>
+***/
+
+/***
+Fields used by PCA
+
+using namespace std;
+using namespace Eigen;
+
+DigitalOut led1(LED1);
+const int addr7bit = 0x68; // 7bit I2C address,AD0 is 0
+
+#define SLEEP_TIME 5000 // (msec)
+***/
+
+/***
+Methods used by PCA
+
+///*
+//  * 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;
+//}
+***/
+
 static DigitalOut led1(LED1, 1);
 
 const static char DEVICE_NAME[] = "STEP COUNTER";
@@ -46,6 +140,9 @@
 
         _ble.init(this, &StepCounter::on_init_complete);
         _event_queue.call_every(500, this, &StepCounter::blink);
+        
+        // TODO Replace this to sync with data reading in order to update step
+        // counts by exact window.
         _event_queue.call_every(1000, this, &StepCounter::update_step_count);
 
         _event_queue.dispatch_forever();
@@ -119,6 +216,8 @@
 
     void update_step_count() {
         if (_ble.gap().getState().connected) {
+            // TODO Remove step_count increament which would be implemented at 
+            // main() after peak detection.
             step_count++;
 
             _ble.gattServer().write(step_count_state.getValueHandle(), (uint8_t *)&step_count, sizeof(int));
@@ -163,6 +262,73 @@
 
     StepCounter demo(ble, event_queue);
     demo.start();
+    
+    // read data
+    /***
+    PCA parameters init
+    
+        //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];
+    
+    
+    MatrixXd acc_raw(3,0);
+    Vector3d acc_new;
+    MatrixXd C;
+    MatrixXd vec, val;
+    int dim = 1;    //dimension of PCA
+    ***/
+    /***
+    Reading data and do PCA
+    
+    while (true) {
+        
+        //Blink LED and wait 1 seconds
+        // TODO Question actually: Is this to wait for collecting enough data
+        // for a window?
+        led1 = !led1;
+        thread_sleep_for(SLEEP_TIME);
+
+        //read and convert date
+        mpu->ReadConvertAll(AccRead,GyroRead,TempRead);
+        //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;
+        
+        // TODO Check if there are enough data for a single window
+        // if true -> run PCA and peak detection
+        // else    -> sleep for another 1 second maybe?
+        
+        //run PCA
+        MatrixXd X1=featurnormail(acc_raw);
+        ComComputeCov(X1, C);
+        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;
+        
+        // TODO add peak detection algorithm here
+        
+        // TODO add step count.  Also, think about lock of the variable - what
+        // if 
+    }
+    ***/
+    
 
     return 0;
 }