wenchang fei / Mbed OS the-lastest-code

the lastest code by Castle. 27 Nov

Dependencies:   mpu9250_i2c biquadFilter PCA peakdetection Eigen

main.cpp@5:c37def827168, 2019-11-25 (annotated)

Committer:
castlefei
Date:
Mon Nov 25 23:44:37 2019 +0000
Revision:
5:c37def827168
Parent:
4:15d6c7123b09
Child:
6:7982fe5f1806
remove printf, delay time to 10ms

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 1:92f42e198925 34
castlefei 5:c37def827168 35 #define SLEEP_TIME 10 // (msec)
castlefei 1:92f42e198925 36
castlefei 0:44701eab0261 37
castlefei 0:44701eab0261 38 // main() runs in its own thread in the OS
castlefei 0:44701eab0261 39 int main()
castlefei 0:44701eab0261 40 {
castlefei 0:44701eab0261 41 //new mpu(data,clk,address),in constructor addr7bit<<1
castlefei 0:44701eab0261 42 mpu9250 *mpu = new mpu9250(p26,p27,addr7bit);
castlefei 0:44701eab0261 43 //scale of acc and gyro
castlefei 0:44701eab0261 44 mpu->initMPU9250(0x00,0x00);
castlefei 0:44701eab0261 45
castlefei 0:44701eab0261 46 float AccRead[3];
castlefei 0:44701eab0261 47 float GyroRead[3];
castlefei 0:44701eab0261 48 float TempRead[1];
castlefei 2:d4c480d17944 49 float res_smooth;
castlefei 4:15d6c7123b09 50 float threshold=0.1;
castlefei 4:15d6c7123b09 51 int number=0;
castlefei 4:15d6c7123b09 52 int step = 0;
castlefei 4:15d6c7123b09 53 int numpeak = 0;
castlefei 1:92f42e198925 54
castlefei 4:15d6c7123b09 55 MatrixXd acc_raw(3,0);
castlefei 1:92f42e198925 56 Vector3d acc_new;
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 1:92f42e198925 67
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 1:92f42e198925 84 acc_new << AccRead[0],AccRead[1],AccRead[2];
castlefei 1:92f42e198925 85 acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
castlefei 1:92f42e198925 86 acc_raw.col(acc_raw.cols()-1) = acc_new;
castlefei 1:92f42e198925 87
castlefei 2:d4c480d17944 88 //////cout << "acc_raw:" << acc_raw << endl;
castlefei 4:15d6c7123b09 89 if(number % 10 ==2)
castlefei 2:d4c480d17944 90 {
castlefei 4:15d6c7123b09 91 if(number > 2)
castlefei 4:15d6c7123b09 92 {
castlefei 4:15d6c7123b09 93 //cout << acc_raw << endl;
castlefei 4:15d6c7123b09 94 //run PCA
castlefei 4:15d6c7123b09 95 MatrixXd X1=pca.featurnormail(acc_raw);
castlefei 4:15d6c7123b09 96 pca.ComComputeCov(X1, C);
castlefei 4:15d6c7123b09 97 pca.ComputEig(C, vec, val);
castlefei 4:15d6c7123b09 98 //select dim num of eigenvector from right to left. right is important
castlefei 4:15d6c7123b09 99 //compute the result array
castlefei 4:15d6c7123b09 100 MatrixXd res = vec.rightCols(dim).transpose()*X1;
castlefei 4:15d6c7123b09 101
castlefei 4:15d6c7123b09 102 //show the result after PCA
castlefei 4:15d6c7123b09 103 //////cout << "result" << res << endl;
castlefei 4:15d6c7123b09 104 vector<float> res_list={};
castlefei 4:15d6c7123b09 105
castlefei 4:15d6c7123b09 106 //printf("result of PCA size:%d\n\r",res.cols());
castlefei 4:15d6c7123b09 107 for(int i = 0; i < res.cols(); i++)
castlefei 4:15d6c7123b09 108 {
castlefei 4:15d6c7123b09 109 res_smooth = bqc.step(res(i));
castlefei 4:15d6c7123b09 110 res_list.push_back(res_smooth);
castlefei 5:c37def827168 111 //printf("result after filter in for loop %d: %f\n\r",i,res_smooth);
castlefei 4:15d6c7123b09 112 //std::cout << "\t" << bqc.step( ) << std::endl;
castlefei 4:15d6c7123b09 113 }
castlefei 4:15d6c7123b09 114 int len = res_list.size();
castlefei 4:15d6c7123b09 115 //printf("len of res:%d\n\r", len);
castlefei 4:15d6c7123b09 116 numpeak = peak.findPeaks(res_list,len,threshold);
castlefei 5:c37def827168 117 //printf("height in main: %f\n\r", threshold);
castlefei 4:15d6c7123b09 118 step = step + numpeak;
castlefei 4:15d6c7123b09 119 printf("num of step: %d\n\r", step);
castlefei 4:15d6c7123b09 120
castlefei 4:15d6c7123b09 121 //clear the matrix to contain new data
castlefei 4:15d6c7123b09 122 acc_raw.conservativeResize(3, 0);
castlefei 4:15d6c7123b09 123 }
castlefei 2:d4c480d17944 124 }
castlefei 4:15d6c7123b09 125 number = number +1;
castlefei 0:44701eab0261 126 }
castlefei 0:44701eab0261 127 }