Biorobotics / Mbed 2 deprecated EMG_TESTING_SIMPLE

a biquad working

Dependencies:   HIDScope mbed QEI

main.cpp@0:fc207e186e8b, 2015-10-27 (annotated)

Committer:
yohoo15
Date:
Tue Oct 27 09:57:27 2015 +0000
Revision:
0:fc207e186e8b
Child:
1:ee877d784c40
A biqaud working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yohoo15 0:fc207e186e8b 1 #include "mbed.h"
yohoo15 0:fc207e186e8b 2 //#include "read_filter_emg.h"
yohoo15 0:fc207e186e8b 3 //included for fabs() function
yohoo15 0:fc207e186e8b 4 #include <math.h>
yohoo15 0:fc207e186e8b 5 #include "HIDScope.h"
yohoo15 0:fc207e186e8b 6 #include <iostream>
yohoo15 0:fc207e186e8b 7
yohoo15 0:fc207e186e8b 8 Ticker HIDScope_timer;
yohoo15 0:fc207e186e8b 9 Ticker Filteren_timer;
yohoo15 0:fc207e186e8b 10 HIDScope scope(2);
yohoo15 0:fc207e186e8b 11
yohoo15 0:fc207e186e8b 12 // defining flags
yohoo15 0:fc207e186e8b 13 volatile bool Flag_filteren = false;
yohoo15 0:fc207e186e8b 14 volatile bool Flag_HIDScope = false;
yohoo15 0:fc207e186e8b 15
yohoo15 0:fc207e186e8b 16 // making function flags.
yohoo15 0:fc207e186e8b 17 void Go_flag_filteren()
yohoo15 0:fc207e186e8b 18 {
yohoo15 0:fc207e186e8b 19 Flag_filteren = true;
yohoo15 0:fc207e186e8b 20 }
yohoo15 0:fc207e186e8b 21
yohoo15 0:fc207e186e8b 22 void Go_flag_HIDScope()
yohoo15 0:fc207e186e8b 23 {
yohoo15 0:fc207e186e8b 24 Flag_HIDScope = true;
yohoo15 0:fc207e186e8b 25 }
yohoo15 0:fc207e186e8b 26
yohoo15 0:fc207e186e8b 27 AnalogIn analog_emg_left(A0);
yohoo15 0:fc207e186e8b 28 //AnalogIn analog_emg_right(A1);
yohoo15 0:fc207e186e8b 29 double input = 0;
yohoo15 0:fc207e186e8b 30 double filter_signal_hid = 0;
yohoo15 0:fc207e186e8b 31 //double input_right = 0;
yohoo15 0:fc207e186e8b 32
yohoo15 0:fc207e186e8b 33 double v1=0;
yohoo15 0:fc207e186e8b 34 double v2=0;
yohoo15 0:fc207e186e8b 35 //double v1_right=0;
yohoo15 0:fc207e186e8b 36 //double v2_right=0;
yohoo15 0:fc207e186e8b 37
yohoo15 0:fc207e186e8b 38 double filter_left;
yohoo15 0:fc207e186e8b 39 double filter_right;
yohoo15 0:fc207e186e8b 40
yohoo15 0:fc207e186e8b 41 //general biquad filter that can be called in all the filter functions
yohoo15 0:fc207e186e8b 42 double biquad(double u, double &v1, double &v2, const double a1,
yohoo15 0:fc207e186e8b 43 const double a2, const double b0, const double b1, const double b2)
yohoo15 0:fc207e186e8b 44 {
yohoo15 0:fc207e186e8b 45 double v = u - a1*v1 - a2*v2;
yohoo15 0:fc207e186e8b 46 double y = b0*v + b1*v1 + b2*v2;
yohoo15 0:fc207e186e8b 47 //values of v2 and v1 are updated, as they are passed by reference
yohoo15 0:fc207e186e8b 48 //they update globally
yohoo15 0:fc207e186e8b 49 v2 = v1;
yohoo15 0:fc207e186e8b 50 v1 = v;
yohoo15 0:fc207e186e8b 51 return y;
yohoo15 0:fc207e186e8b 52 }
yohoo15 0:fc207e186e8b 53
yohoo15 0:fc207e186e8b 54
yohoo15 0:fc207e186e8b 55
yohoo15 0:fc207e186e8b 56 /* lowpass filter consists of three cascaded biquads
yohoo15 0:fc207e186e8b 57 below the coefficients for those three biquads */
yohoo15 0:fc207e186e8b 58 //first high pass biquad
yohoo15 0:fc207e186e8b 59 const double lowp1_a1 = -1.75927361117;
yohoo15 0:fc207e186e8b 60 const double lowp1_a2 = 0.78528639300;
yohoo15 0:fc207e186e8b 61 const double lowp1_b0 = 1.00000000000;
yohoo15 0:fc207e186e8b 62 const double lowp1_b1 = 2.00000000000;
yohoo15 0:fc207e186e8b 63 const double lowp1_b2 = 1.00000000000;
yohoo15 0:fc207e186e8b 64
yohoo15 0:fc207e186e8b 65 /*
yohoo15 0:fc207e186e8b 66 //second high pass biquad
yohoo15 0:fc207e186e8b 67 const double lowp2_a1 = -1.16338171052;
yohoo15 0:fc207e186e8b 68 const double lowp2_a2 = 0.42191097989;
yohoo15 0:fc207e186e8b 69 const double lowp2_b0 = 1.00000000000;
yohoo15 0:fc207e186e8b 70 const double lowp2_b1 = 2.00000000000;
yohoo15 0:fc207e186e8b 71 const double lowp2_b2 = 1.00000000000;
yohoo15 0:fc207e186e8b 72
yohoo15 0:fc207e186e8b 73 */
yohoo15 0:fc207e186e8b 74
yohoo15 0:fc207e186e8b 75
yohoo15 0:fc207e186e8b 76
yohoo15 0:fc207e186e8b 77
yohoo15 0:fc207e186e8b 78
yohoo15 0:fc207e186e8b 79
yohoo15 0:fc207e186e8b 80
yohoo15 0:fc207e186e8b 81 //highpass
yohoo15 0:fc207e186e8b 82
yohoo15 0:fc207e186e8b 83
yohoo15 0:fc207e186e8b 84
yohoo15 0:fc207e186e8b 85 /*
yohoo15 0:fc207e186e8b 86 //rectifier
yohoo15 0:fc207e186e8b 87 double rectify(double y6)
yohoo15 0:fc207e186e8b 88 {
yohoo15 0:fc207e186e8b 89 y6 = fabs(y6);
yohoo15 0:fc207e186e8b 90 return y6;
yohoo15 0:fc207e186e8b 91 }
yohoo15 0:fc207e186e8b 92 */
yohoo15 0:fc207e186e8b 93 //lowpass
yohoo15 0:fc207e186e8b 94
yohoo15 0:fc207e186e8b 95
yohoo15 0:fc207e186e8b 96
yohoo15 0:fc207e186e8b 97
yohoo15 0:fc207e186e8b 98 // double y8 = biquad(y7, v1, v2, lowp2_a1, lowp2_a2, lowp2_b0, lowp2_b1, lowp2_b2);
yohoo15 0:fc207e186e8b 99
yohoo15 0:fc207e186e8b 100
yohoo15 0:fc207e186e8b 101
yohoo15 0:fc207e186e8b 102
yohoo15 0:fc207e186e8b 103
yohoo15 0:fc207e186e8b 104
yohoo15 0:fc207e186e8b 105 double Filteren()
yohoo15 0:fc207e186e8b 106 {
yohoo15 0:fc207e186e8b 107 input = analog_emg_left.read();
yohoo15 0:fc207e186e8b 108 //input = input-0.45; //FIRST SUBTRACT MEAN THEN FILTER
yohoo15 0:fc207e186e8b 109 //input_right = analog_emg_right.read();
yohoo15 0:fc207e186e8b 110 double filter_signal = biquad(input, v1, v2, lowp1_a1, lowp1_a2, lowp1_b0, lowp1_b1, lowp1_b2);
yohoo15 0:fc207e186e8b 111 //filter_right = filter(input_right, v1_right, v2_right);
yohoo15 0:fc207e186e8b 112
yohoo15 0:fc207e186e8b 113 return(filter_signal);
yohoo15 0:fc207e186e8b 114 }
yohoo15 0:fc207e186e8b 115 void HIDScope_kijken()
yohoo15 0:fc207e186e8b 116 {
yohoo15 0:fc207e186e8b 117 scope.set(0, input);
yohoo15 0:fc207e186e8b 118 scope.set(1, filter_signal_hid);
yohoo15 0:fc207e186e8b 119 scope.send();
yohoo15 0:fc207e186e8b 120 }
yohoo15 0:fc207e186e8b 121 int main()
yohoo15 0:fc207e186e8b 122 {
yohoo15 0:fc207e186e8b 123 HIDScope_timer.attach(&Go_flag_HIDScope, 0.002);
yohoo15 0:fc207e186e8b 124 Filteren_timer.attach(&Go_flag_filteren,0.004);
yohoo15 0:fc207e186e8b 125 while(1){
yohoo15 0:fc207e186e8b 126 if(Flag_filteren) {
yohoo15 0:fc207e186e8b 127 Flag_filteren = false;
yohoo15 0:fc207e186e8b 128 filter_signal_hid = Filteren();
yohoo15 0:fc207e186e8b 129 }
yohoo15 0:fc207e186e8b 130
yohoo15 0:fc207e186e8b 131 if(Flag_HIDScope) {
yohoo15 0:fc207e186e8b 132 Flag_HIDScope = false;
yohoo15 0:fc207e186e8b 133 HIDScope_kijken();
yohoo15 0:fc207e186e8b 134 }
yohoo15 0:fc207e186e8b 135 }
yohoo15 0:fc207e186e8b 136 }