oldexamplecode
Dependencies: mbed
PSDdetection.cpp@0:6863633bf8a4, 2017-03-24 (annotated)
- Committer:
- rik
- Date:
- Fri Mar 24 11:22:30 2017 +0000
- Revision:
- 0:6863633bf8a4
oldexamplecode;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
rik | 0:6863633bf8a4 | 1 | |
rik | 0:6863633bf8a4 | 2 | #include "PSDdetection.h" |
rik | 0:6863633bf8a4 | 3 | |
rik | 0:6863633bf8a4 | 4 | Arm::Arm(){ |
rik | 0:6863633bf8a4 | 5 | for (int i = 0; i<4; i++) |
rik | 0:6863633bf8a4 | 6 | prevEMG[i] = 0; |
rik | 0:6863633bf8a4 | 7 | |
rik | 0:6863633bf8a4 | 8 | EMGreference = 140; |
rik | 0:6863633bf8a4 | 9 | } |
rik | 0:6863633bf8a4 | 10 | |
rik | 0:6863633bf8a4 | 11 | // Sets a new reference level |
rik | 0:6863633bf8a4 | 12 | void Arm::setReference(int reflevel){ |
rik | 0:6863633bf8a4 | 13 | this->EMGreference = reflevel; |
rik | 0:6863633bf8a4 | 14 | } |
rik | 0:6863633bf8a4 | 15 | |
rik | 0:6863633bf8a4 | 16 | // Updates the array with a new sample and checks wheter a muscle is active |
rik | 0:6863633bf8a4 | 17 | bool Arm::checksample(int emgsample){ |
rik | 0:6863633bf8a4 | 18 | |
rik | 0:6863633bf8a4 | 19 | bool MuscleIsActive = false; |
rik | 0:6863633bf8a4 | 20 | |
rik | 0:6863633bf8a4 | 21 | // Shift all the previous values in time |
rik | 0:6863633bf8a4 | 22 | for (int i = 3; i>0; i--){ |
rik | 0:6863633bf8a4 | 23 | prevEMG[i] = prevEMG[i - 1]; |
rik | 0:6863633bf8a4 | 24 | } |
rik | 0:6863633bf8a4 | 25 | |
rik | 0:6863633bf8a4 | 26 | // Put in the latest sample |
rik | 0:6863633bf8a4 | 27 | prevEMG[0] = emgsample; |
rik | 0:6863633bf8a4 | 28 | |
rik | 0:6863633bf8a4 | 29 | // Weighted average filter to smoothen out the response |
rik | 0:6863633bf8a4 | 30 | int average = (prevEMG[0] * 1 + prevEMG[1] * 0.63 + prevEMG[2] * 0.45 + prevEMG[3] * 0.22) / (1 + 0.63 + 0.45 + 0.22); |
rik | 0:6863633bf8a4 | 31 | |
rik | 0:6863633bf8a4 | 32 | // Check with reference level to determine if it is |
rik | 0:6863633bf8a4 | 33 | if (average > (EMGreference + EMGtriggerlevel)){ |
rik | 0:6863633bf8a4 | 34 | MuscleIsActive = true; |
rik | 0:6863633bf8a4 | 35 | } |
rik | 0:6863633bf8a4 | 36 | |
rik | 0:6863633bf8a4 | 37 | return MuscleIsActive; |
rik | 0:6863633bf8a4 | 38 | } |
rik | 0:6863633bf8a4 | 39 | |
rik | 0:6863633bf8a4 | 40 | int biquad10(int emgsample){ |
rik | 0:6863633bf8a4 | 41 | //initialize filter values |
rik | 0:6863633bf8a4 | 42 | float filteredValue,a0,a1,a2,b1,b2; |
rik | 0:6863633bf8a4 | 43 | float intermediateValue1[2]intermediateValue2[2]; |
rik | 0:6863633bf8a4 | 44 | //filter for 10hz -6bd biquad |
rik | 0:6863633bf8a4 | 45 | a0=0.0009446857782217186; |
rik | 0:6863633bf8a4 | 46 | a1=0.0018893715564434373; |
rik | 0:6863633bf8a4 | 47 | a2=0.0009446857782217186; |
rik | 0:6863633bf8a4 | 48 | b1=-1.9111847961312216; |
rik | 0:6863633bf8a4 | 49 | b2=0.9149635392441086; |
rik | 0:6863633bf8a4 | 50 | //calculations for transposed direct form 2 |
rik | 0:6863633bf8a4 | 51 | intermediateValue1[0] = a2*emgsample-b2*filteredValue; |
rik | 0:6863633bf8a4 | 52 | intermediateValue2[0] = a2*emgsample-b1*filteredValue+intermediateValue1[1]; |
rik | 0:6863633bf8a4 | 53 | filteredValue = a0*filteredValue+intermediateValue2[1]; |
rik | 0:6863633bf8a4 | 54 | intermediateValue1[1] = intermediateValue1[0]; |
rik | 0:6863633bf8a4 | 55 | intermediateValue2[1] = intermediateValue2[0]; |
rik | 0:6863633bf8a4 | 56 | |
rik | 0:6863633bf8a4 | 57 | |
rik | 0:6863633bf8a4 | 58 | return ((int)filteredValue); |
rik | 0:6863633bf8a4 | 59 | } |