wenchang fei / Mbed OS the-lastest-code

the lastest code by Castle. 27 Nov

Dependencies:   mpu9250_i2c biquadFilter PCA peakdetection Eigen

main.cpp@6:7982fe5f1806, 2019-11-26 (annotated)

Committer:
castlefei
Date:
Tue Nov 26 14:28:36 2019 +0000
Revision:
6:7982fe5f1806
Parent:
5:c37def827168 
vision 3. adding the overlap windows

Who changed what in which revision?

UserRevisionLine numberNew contents of line
castlefei 0:44701eab0261 1 /*
castlefei 1:92f42e198925 2 * read and print acc, gyro,temperature date from MPU9250
castlefei 1:92f42e198925 3 * and transform accelerate data to one dimension.
castlefei 0:44701eab0261 4 * in terminal:
castlefei 0:44701eab0261 5 * ls /dev/tty.*
castlefei 0:44701eab0261 6 * screen /dev/tty.usbmodem14102 9600
castlefei 1:92f42e198925 7 * to see the result
castlefei 0:44701eab0261 8 *
castlefei 0:44701eab0261 9 * mbed Microcontroller Library
castlefei 1:92f42e198925 10 * Eigen Library
castlefei 0:44701eab0261 11 */
castlefei 0:44701eab0261 12
castlefei 0:44701eab0261 13 #include "mbed.h"
castlefei 0:44701eab0261 14 #include "platform/mbed_thread.h"
castlefei 0:44701eab0261 15 #include "stats_report.h"
castlefei 0:44701eab0261 16 #include "MPU9250.h"
castlefei 2:d4c480d17944 17 //#include <Eigen/Dense.h>
castlefei 1:92f42e198925 18 #include <iostream>
castlefei 2:d4c480d17944 19 #include <vector>
castlefei 2:d4c480d17944 20 #include <complex>
castlefei 2:d4c480d17944 21 #include "BiQuad.h"
castlefei 2:d4c480d17944 22 #include "pca.h"
castlefei 2:d4c480d17944 23 #include "peak.h"
castlefei 1:92f42e198925 24
castlefei 1:92f42e198925 25 using namespace std;
castlefei 1:92f42e198925 26 using namespace Eigen;
castlefei 0:44701eab0261 27
castlefei 0:44701eab0261 28 DigitalOut led1(LED1);
castlefei 0:44701eab0261 29 const int addr7bit = 0x68; // 7bit I2C address,AD0 is 0
castlefei 2:d4c480d17944 30 //the parameter of biquad filter, 40Hz sampling frequence,10Hz cut-off freq, Q:0.719
castlefei 2:d4c480d17944 31 BiQuad mybq(0.3403575989782886,0.6807151979565772,0.3403575989782886, -1.511491371967327e-16,0.36143039591315457);
castlefei 2:d4c480d17944 32 BiQuadChain bqc;
castlefei 1:92f42e198925 33
castlefei 6:7982fe5f1806 34 #define SLEEP_TIME 80 // (msec)
castlefei 1:92f42e198925 35
castlefei 0:44701eab0261 36
castlefei 0:44701eab0261 37 // main() runs in its own thread in the OS
castlefei 0:44701eab0261 38 int main()
castlefei 0:44701eab0261 39 {
castlefei 0:44701eab0261 40 //new mpu(data,clk,address),in constructor addr7bit<<1
castlefei 0:44701eab0261 41 mpu9250 *mpu = new mpu9250(p26,p27,addr7bit);
castlefei 0:44701eab0261 42 //scale of acc and gyro
castlefei 0:44701eab0261 43 mpu->initMPU9250(0x00,0x00);
castlefei 0:44701eab0261 44
castlefei 0:44701eab0261 45 float AccRead[3];
castlefei 0:44701eab0261 46 float GyroRead[3];
castlefei 0:44701eab0261 47 float TempRead[1];
castlefei 2:d4c480d17944 48 float res_smooth;
castlefei 4:15d6c7123b09 49 float threshold=0.1;
castlefei 4:15d6c7123b09 50 int number=0;
castlefei 4:15d6c7123b09 51 int step = 0;
castlefei 4:15d6c7123b09 52 int numpeak = 0;
castlefei 1:92f42e198925 53
castlefei 4:15d6c7123b09 54 MatrixXd acc_raw(3,0);
castlefei 1:92f42e198925 55 Vector3d acc_new;
castlefei 6:7982fe5f1806 56 Vector3d acc_previous;
castlefei 1:92f42e198925 57 MatrixXd C;
castlefei 1:92f42e198925 58 MatrixXd vec, val;
castlefei 1:92f42e198925 59 int dim = 1; //dimension of PCA
castlefei 2:d4c480d17944 60 //use the class defined in pca.h and peak.h
castlefei 2:d4c480d17944 61 PCA pca;
castlefei 2:d4c480d17944 62 PEAK peak;
castlefei 2:d4c480d17944 63
castlefei 2:d4c480d17944 64 bqc.add(&mybq);
castlefei 2:d4c480d17944 65
castlefei 4:15d6c7123b09 66 //vector<float> res_list;
castlefei 6:7982fe5f1806 67 acc_new << 0,0,0;
castlefei 0:44701eab0261 68 while (true) {
castlefei 0:44701eab0261 69
castlefei 0:44701eab0261 70 //Blink LED and wait 1 seconds
castlefei 0:44701eab0261 71 led1 = !led1;
castlefei 0:44701eab0261 72 thread_sleep_for(SLEEP_TIME);
castlefei 0:44701eab0261 73 //read and convert date
castlefei 0:44701eab0261 74 mpu->ReadConvertAll(AccRead,GyroRead,TempRead);
castlefei 2:d4c480d17944 75 AccRead[0]= AccRead[0]/1000;
castlefei 2:d4c480d17944 76 AccRead[1]= AccRead[1]/1000;
castlefei 2:d4c480d17944 77 AccRead[2]= AccRead[2]/1000;
castlefei 4:15d6c7123b09 78 //printf("acc value is (%f,%f,%f).\n\r",AccRead[0],AccRead[1],AccRead[2]);
castlefei 2:d4c480d17944 79 //printf("gyro value is (%f,%f,%f).\n\r",GyroRead[0],GyroRead[1],GyroRead[2]);
castlefei 2:d4c480d17944 80 //printf("temp value is %f.\n\r",TempRead[0]);
castlefei 0:44701eab0261 81
castlefei 1:92f42e198925 82 //append new data to matrix acc_raw
castlefei 1:92f42e198925 83 //adding the columns
castlefei 6:7982fe5f1806 84 acc_previous = acc_new;
castlefei 1:92f42e198925 85 acc_new << AccRead[0],AccRead[1],AccRead[2];
castlefei 6:7982fe5f1806 86 //cout << "acc_previous:"<<acc_previous<<"\n\r";
castlefei 6:7982fe5f1806 87 //printf("\n\r");
castlefei 6:7982fe5f1806 88
castlefei 1:92f42e198925 89 acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
castlefei 1:92f42e198925 90 acc_raw.col(acc_raw.cols()-1) = acc_new;
castlefei 1:92f42e198925 91
castlefei 2:d4c480d17944 92 //////cout << "acc_raw:" << acc_raw << endl;
castlefei 4:15d6c7123b09 93 if(number % 10 ==2)
castlefei 2:d4c480d17944 94 {
castlefei 4:15d6c7123b09 95 if(number > 2)
castlefei 4:15d6c7123b09 96 {
castlefei 6:7982fe5f1806 97 //cout << "acc_raw"<< acc_raw << "\n\r";
castlefei 6:7982fe5f1806 98 //printf("\n\r");
castlefei 6:7982fe5f1806 99
castlefei 4:15d6c7123b09 100 //run PCA
castlefei 4:15d6c7123b09 101 MatrixXd X1=pca.featurnormail(acc_raw);
castlefei 4:15d6c7123b09 102 pca.ComComputeCov(X1, C);
castlefei 4:15d6c7123b09 103 pca.ComputEig(C, vec, val);
castlefei 4:15d6c7123b09 104 //select dim num of eigenvector from right to left. right is important
castlefei 4:15d6c7123b09 105 //compute the result array
castlefei 4:15d6c7123b09 106 MatrixXd res = vec.rightCols(dim).transpose()*X1;
castlefei 4:15d6c7123b09 107
castlefei 4:15d6c7123b09 108 //show the result after PCA
castlefei 4:15d6c7123b09 109 //////cout << "result" << res << endl;
castlefei 4:15d6c7123b09 110 vector<float> res_list={};
castlefei 4:15d6c7123b09 111
castlefei 4:15d6c7123b09 112 //printf("result of PCA size:%d\n\r",res.cols());
castlefei 4:15d6c7123b09 113 for(int i = 0; i < res.cols(); i++)
castlefei 4:15d6c7123b09 114 {
castlefei 4:15d6c7123b09 115 res_smooth = bqc.step(res(i));
castlefei 4:15d6c7123b09 116 res_list.push_back(res_smooth);
castlefei 6:7982fe5f1806 117 //printf("result%d: %f\n\r",i,res_smooth);
castlefei 4:15d6c7123b09 118 //std::cout << "\t" << bqc.step( ) << std::endl;
castlefei 4:15d6c7123b09 119 }
castlefei 4:15d6c7123b09 120 int len = res_list.size();
castlefei 4:15d6c7123b09 121 //printf("len of res:%d\n\r", len);
castlefei 4:15d6c7123b09 122 numpeak = peak.findPeaks(res_list,len,threshold);
castlefei 5:c37def827168 123 //printf("height in main: %f\n\r", threshold);
castlefei 4:15d6c7123b09 124 step = step + numpeak;
castlefei 4:15d6c7123b09 125 printf("num of step: %d\n\r", step);
castlefei 4:15d6c7123b09 126
castlefei 4:15d6c7123b09 127 //clear the matrix to contain new data
castlefei 4:15d6c7123b09 128 acc_raw.conservativeResize(3, 0);
castlefei 6:7982fe5f1806 129
castlefei 6:7982fe5f1806 130 //overlap windows
castlefei 6:7982fe5f1806 131 acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
castlefei 6:7982fe5f1806 132 acc_raw.col(acc_raw.cols()-1) = acc_previous;
castlefei 6:7982fe5f1806 133
castlefei 6:7982fe5f1806 134 acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
castlefei 6:7982fe5f1806 135 acc_raw.col(acc_raw.cols()-1) = acc_new;
castlefei 4:15d6c7123b09 136 }
castlefei 2:d4c480d17944 137 }
castlefei 4:15d6c7123b09 138 number = number +1;
castlefei 0:44701eab0261 139 }
castlefei 0:44701eab0261 140 }