Bouke Scheltinga / Mbed 2 deprecated Werk_college_23sept

Werkcollege opgave 23 september BMT K9

Dependencies:   Encoder HIDScope MODSERIAL mbed QEI biquadFilter

main.cpp@32:97cf6cb8d054, 2015-10-14 (annotated)

Committer:
bscheltinga
Date:
Wed Oct 14 14:13:52 2015 +0000
Revision:
32:97cf6cb8d054
Parent:
27:85e5d36bb6c5
Child:
36:f29a36683b1a
Zelfde script: biquads verandert, nieuwe van online biquad calculator

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bscheltinga 0:fe3896c6eeb0 1 #include "mbed.h"
bscheltinga 12:0a079e86348e 2 #include "HIDScope.h"
bscheltinga 0:fe3896c6eeb0 3 #include "MODSERIAL.h"
bscheltinga 13:04e10692e239 4 #include "biquadFilter.h" //Filter direct form II
bscheltinga 0:fe3896c6eeb0 5
bscheltinga 26:1090acf98efc 6 // [DEFINE INPUTS] //
ThomasBNL 27:85e5d36bb6c5 7 //AnalogIn emgL(A0); //Analog input left arm
bscheltinga 18:68067ffd169e 8 //AnalogIn emgR(PTB1); //Analog input right arm
bscheltinga 19:bd453bee03f6 9 DigitalOut led1(LED_GREEN);
bscheltinga 21:594915ba2bf9 10
bscheltinga 26:1090acf98efc 11 // [DEFINE CONSTANTS] //
bscheltinga 26:1090acf98efc 12 float emgL_L, emgL_LH, emgLeft, emgL_H, emgL_Notch;
bscheltinga 26:1090acf98efc 13 double B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, MOVAVG;
ThomasBNL 27:85e5d36bb6c5 14 HIDScope scope(3); // Aantal HIDScope kanalen
bscheltinga 18:68067ffd169e 15 MODSERIAL pc(USBTX,USBRX);
bscheltinga 20:d5f5c60adc43 16 volatile bool control_go = false;
bscheltinga 20:d5f5c60adc43 17 Ticker control_tick;
ThomasBNL 27:85e5d36bb6c5 18 Ticker T1;
bscheltinga 15:7870f7912904 19
bscheltinga 26:1090acf98efc 20 // [BIQUAD FILTERS] //
ThomasBNL 27:85e5d36bb6c5 21 int Fs = 512; // sampling frequency
bscheltinga 32:97cf6cb8d054 22 const double low_b0 = 0.05892937945281792
bscheltinga 32:97cf6cb8d054 23 const double low_b1 = 0.11785875890563584
bscheltinga 32:97cf6cb8d054 24 const double low_b2 = 0.05892937945281792
bscheltinga 32:97cf6cb8d054 25 const double low_a1 = -1.205716572226748
bscheltinga 32:97cf6cb8d054 26 const double low_a2 = 0.44143409003801976 // VIA online biquad calculator Lowpas 520-48-0.7071-6
bscheltinga 15:7870f7912904 27
bscheltinga 32:97cf6cb8d054 28 const double high_b0 = 0.6389437261127494
bscheltinga 32:97cf6cb8d054 29 const double high_b1 = -1.2778874522254988
bscheltinga 32:97cf6cb8d054 30 const double high_b2 = 0.6389437261127494
bscheltinga 32:97cf6cb8d054 31 const double high_a1 = -1.1429772843080923
bscheltinga 32:97cf6cb8d054 32 const double high_a2 = 0.41279762014290533 // VIA online biquad calculator Highpas 520-52-0.7071-6
bscheltinga 32:97cf6cb8d054 33
bscheltinga 32:97cf6cb8d054 34 biquadFilter highpass1(high_a1, high_a2, high_b0, high_b1, high_b2);
bscheltinga 32:97cf6cb8d054 35 biquadFilter lowpass1(low_a1, low_a2, low_b0, low_b1, low_b2);
ThomasBNL 27:85e5d36bb6c5 36
ThomasBNL 27:85e5d36bb6c5 37 AnalogIn input1(A0); // declaring the 4 inputs
ThomasBNL 27:85e5d36bb6c5 38
ThomasBNL 27:85e5d36bb6c5 39 double u1; double y1; // declaring the input variables
bscheltinga 22:14abcfdd1554 40
bscheltinga 26:1090acf98efc 41 // [FUNCTIONS] //
bscheltinga 21:594915ba2bf9 42 void ControlGo() //Control flag
bscheltinga 21:594915ba2bf9 43 {
bscheltinga 21:594915ba2bf9 44 control_go = true;
bscheltinga 21:594915ba2bf9 45 led1 = 0;
bscheltinga 21:594915ba2bf9 46 }
bscheltinga 0:fe3896c6eeb0 47
ThomasBNL 27:85e5d36bb6c5 48 void sample_filter()
bscheltinga 13:04e10692e239 49 {
ThomasBNL 27:85e5d36bb6c5 50 u1 = input1;
ThomasBNL 27:85e5d36bb6c5 51 y1 = highpass1.step(u1); // filter order is: high-pass --> rectify --> low-pass
ThomasBNL 27:85e5d36bb6c5 52 y1 = fabs(y1);
ThomasBNL 27:85e5d36bb6c5 53 y1 = lowpass1.step(y1);// roughly normalize to a scale of 0 - 1, where 0 is minimum and 1 is roughly the maximum output of dennis.
ThomasBNL 27:85e5d36bb6c5 54
ThomasBNL 27:85e5d36bb6c5 55 B0 = y1;
bscheltinga 25:38ab6dd19d9b 56 MOVAVG=B0*0.1+B1*0.1+B2*0.1+B3*0.1+B4*0.1+B5*0.1+B6*0.1+B7*0.1+B8*0.1+B9*0.1;
bscheltinga 24:a1ce6a87103c 57 B9=B8;
bscheltinga 24:a1ce6a87103c 58 B8=B7;
bscheltinga 24:a1ce6a87103c 59 B7=B6;
bscheltinga 24:a1ce6a87103c 60 B6=B5;
bscheltinga 24:a1ce6a87103c 61 B5=B4;
bscheltinga 24:a1ce6a87103c 62 B4=B3;
bscheltinga 24:a1ce6a87103c 63 B3=B2;
bscheltinga 24:a1ce6a87103c 64 B2=B1;
bscheltinga 24:a1ce6a87103c 65 B1=B0;
bscheltinga 23:c9c9c1d7864a 66
bscheltinga 21:594915ba2bf9 67 led1 = 1; //De led gaat flikkeren wanneer deze loop uitgevoerd wordt
ThomasBNL 27:85e5d36bb6c5 68 }
bscheltinga 20:d5f5c60adc43 69
ThomasBNL 27:85e5d36bb6c5 70
ThomasBNL 27:85e5d36bb6c5 71 // [MAIN FUNCTION] //
ThomasBNL 27:85e5d36bb6c5 72 int main()
ThomasBNL 27:85e5d36bb6c5 73 {
ThomasBNL 27:85e5d36bb6c5 74 control_tick.attach(&ControlGo, (float)1/Fs);
ThomasBNL 27:85e5d36bb6c5 75 pc.baud(9600);
ThomasBNL 27:85e5d36bb6c5 76 while(1)
ThomasBNL 27:85e5d36bb6c5 77 {
ThomasBNL 27:85e5d36bb6c5 78 led1=0;
ThomasBNL 27:85e5d36bb6c5 79 if(control_go)
ThomasBNL 27:85e5d36bb6c5 80 {
ThomasBNL 27:85e5d36bb6c5 81 sample_filter();
ThomasBNL 27:85e5d36bb6c5 82 scope.set(0,u1);
ThomasBNL 27:85e5d36bb6c5 83 scope.set(1,y1);
ThomasBNL 27:85e5d36bb6c5 84 scope.set(2,MOVAVG);
ThomasBNL 27:85e5d36bb6c5 85 scope.send();
ThomasBNL 27:85e5d36bb6c5 86 control_go = 0;
ThomasBNL 27:85e5d36bb6c5 87 }
ThomasBNL 27:85e5d36bb6c5 88 } // while end
ThomasBNL 27:85e5d36bb6c5 89 } // main end
ThomasBNL 27:85e5d36bb6c5 90