mix code vision 3. Using the previous algorithm to detect peaks as Nikoleta and Shiyao. Adding overlapping windows
Dependencies: mpu9250_i2c biquadFilter peakdetection Eigen
main.cpp
00001 /* 00002 * read and print acc, gyro,temperature date from MPU9250 00003 * and transform accelerate data to one dimension. 00004 * in terminal: 00005 * ls /dev/tty.* 00006 * screen /dev/tty.usbmodem14102 9600 00007 * to see the result 00008 * 00009 * mbed Microcontroller Library 00010 * Eigen Library 00011 */ 00012 00013 #include "mbed.h" 00014 #include "platform/mbed_thread.h" 00015 #include "stats_report.h" 00016 #include "MPU9250.h" 00017 //#include <Eigen/Dense.h> 00018 #include <iostream> 00019 #include <vector> 00020 #include <complex> 00021 #include "BiQuad.h" 00022 #include "pca.h" 00023 #include "peak.h" 00024 00025 using namespace std; 00026 using namespace Eigen; 00027 00028 DigitalOut led1(LED1); 00029 const int addr7bit = 0x68; // 7bit I2C address,AD0 is 0 00030 //the parameter of biquad filter, 40Hz sampling frequence,10Hz cut-off freq, Q:0.719 00031 BiQuad mybq(0.3403575989782886,0.6807151979565772,0.3403575989782886, -1.511491371967327e-16,0.36143039591315457); 00032 BiQuadChain bqc; 00033 00034 #define SLEEP_TIME 80 // (msec) 00035 00036 00037 // main() runs in its own thread in the OS 00038 int main() 00039 { 00040 //new mpu(data,clk,address),in constructor addr7bit<<1 00041 mpu9250 *mpu = new mpu9250(p26,p27,addr7bit); 00042 //scale of acc and gyro 00043 mpu->initMPU9250(0x00,0x00); 00044 00045 float AccRead[3]; 00046 float GyroRead[3]; 00047 float TempRead[1]; 00048 float res_smooth; 00049 float threshold=0.1; 00050 int number=0; 00051 int step = 0; 00052 int numpeak = 0; 00053 00054 MatrixXd acc_raw(3,0); 00055 Vector3d acc_new; 00056 Vector3d acc_previous; 00057 MatrixXd C; 00058 MatrixXd vec, val; 00059 int dim = 1; //dimension of PCA 00060 //use the class defined in pca.h and peak.h 00061 PCA pca; 00062 PEAK peak; 00063 00064 bqc.add(&mybq); 00065 00066 //vector<float> res_list; 00067 acc_new << 0,0,0; 00068 while (true) { 00069 00070 //Blink LED and wait 1 seconds 00071 led1 = !led1; 00072 thread_sleep_for(SLEEP_TIME); 00073 //read and convert date 00074 mpu->ReadConvertAll(AccRead,GyroRead,TempRead); 00075 AccRead[0]= AccRead[0]/1000; 00076 AccRead[1]= AccRead[1]/1000; 00077 AccRead[2]= AccRead[2]/1000; 00078 //printf("acc value is (%f,%f,%f).\n\r",AccRead[0],AccRead[1],AccRead[2]); 00079 //printf("gyro value is (%f,%f,%f).\n\r",GyroRead[0],GyroRead[1],GyroRead[2]); 00080 //printf("temp value is %f.\n\r",TempRead[0]); 00081 00082 //append new data to matrix acc_raw 00083 //adding the columns 00084 acc_previous = acc_new; 00085 acc_new << AccRead[0],AccRead[1],AccRead[2]; 00086 //cout << "acc_previous:"<<acc_previous<<"\n\r"; 00087 //printf("\n\r"); 00088 00089 acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1); 00090 acc_raw.col(acc_raw.cols()-1) = acc_new; 00091 00092 //////cout << "acc_raw:" << acc_raw << endl; 00093 if(number % 10 ==2) 00094 { 00095 if(number > 2) 00096 { 00097 //cout << "acc_raw"<< acc_raw << "\n\r"; 00098 //printf("\n\r"); 00099 00100 //run PCA 00101 MatrixXd X1=pca.featurnormail(acc_raw); 00102 pca.ComComputeCov(X1, C); 00103 pca.ComputEig(C, vec, val); 00104 //select dim num of eigenvector from right to left. right is important 00105 //compute the result array 00106 MatrixXd res = vec.rightCols(dim).transpose()*X1; 00107 00108 //show the result after PCA 00109 //////cout << "result" << res << endl; 00110 vector<float> res_list={}; 00111 00112 //printf("result of PCA size:%d\n\r",res.cols()); 00113 for(int i = 0; i < res.cols(); i++) 00114 { 00115 res_smooth = bqc.step(res(i)); 00116 res_list.push_back(res_smooth); 00117 //printf("result%d: %f\n\r",i,res_smooth); 00118 //std::cout << "\t" << bqc.step( ) << std::endl; 00119 } 00120 int len = res_list.size(); 00121 //printf("len of res:%d\n\r", len); 00122 numpeak = peak.findPeaks(res_list,len,threshold); 00123 //printf("height in main: %f\n\r", threshold); 00124 step = step + numpeak; 00125 printf("num of step: %d\n\r", step); 00126 00127 //clear the matrix to contain new data 00128 acc_raw.conservativeResize(3, 0); 00129 00130 //overlap windows 00131 acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1); 00132 acc_raw.col(acc_raw.cols()-1) = acc_previous; 00133 00134 acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1); 00135 acc_raw.col(acc_raw.cols()-1) = acc_new; 00136 } 00137 } 00138 number = number +1; 00139 } 00140 }
Generated on Sat Aug 6 2022 04:17:41 by 1.7.2