thijs ruikes / Mbed 2 deprecated PROJECT-BUTTFILTER

script zover

Dependencies:   HIDScope MODSERIAL mbed

main.cpp@16:f7881b420c83, 2014-10-23 (annotated)

Committer:
wiesdat
Date:
Thu Oct 23 09:44:01 2014 +0000
Revision:
16:f7881b420c83
Parent:
15:2adb036a191f 
v1.04

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wiesdat 0:bed29da02e8b 1 #include "mbed.h"
wiesdat 0:bed29da02e8b 2 #include "MODSERIAL.h"
wiesdat 0:bed29da02e8b 3 #include "HIDScope.h"
wiesdat 5:9415e5be8235 4 #include <iostream>
wiesdat 0:bed29da02e8b 5
wiesdat 16:f7881b420c83 6 PwmOut m_speed1(PTA5);
wiesdat 16:f7881b420c83 7 DigitalOut m_dir1(PTA4);
wiesdat 16:f7881b420c83 8 PwmOut m_speed2(PTC8);
wiesdat 16:f7881b420c83 9 DigitalOut m_dir2(PTC9);
wiesdat 16:f7881b420c83 10
wiesdat 16:f7881b420c83 11 float s_speed1;
wiesdat 16:f7881b420c83 12 float s_dir1;
wiesdat 16:f7881b420c83 13 float s_speed2;
wiesdat 16:f7881b420c83 14 float s_dir2;
wiesdat 16:f7881b420c83 15
wiesdat 13:23130c3a37b1 16 #define A1LP 0.018180963222803
wiesdat 13:23130c3a37b1 17 #define A0LP 0.016544013176248
wiesdat 13:23130c3a37b1 18 #define B1LP -1.718913340044714
wiesdat 13:23130c3a37b1 19 #define B0LP 0.753638316443765
wiesdat 2:620ff9f02d62 20
wiesdat 13:23130c3a37b1 21 #define A1HP -1.999801878951505
wiesdat 13:23130c3a37b1 22 #define A0HP 0.999801878951505
wiesdat 13:23130c3a37b1 23 #define B1HP -1.971717601075000
wiesdat 13:23130c3a37b1 24 #define B0HP 0.972111984032897
wiesdat 6:6f0bc2e465b0 25
wiesdat 6:6f0bc2e465b0 26 #define A0N 0.99436777112
wiesdat 6:6f0bc2e465b0 27 #define A1N -1.89139989664
wiesdat 6:6f0bc2e465b0 28 #define A2N 0.99436777112
wiesdat 6:6f0bc2e465b0 29 #define B1N 1.89070035439
wiesdat 6:6f0bc2e465b0 30 #define B2N -0.988036
wiesdat 1:bc2db3bff4bb 31
wiesdat 5:9415e5be8235 32 #define TSAMP 0.001
wiesdat 15:2adb036a191f 33 #define TRESHOLD 0.03
wiesdat 11:91d827c62a50 34 AnalogIn emg1(PTB1);
wiesdat 14:4c33f6240118 35 AnalogIn emg2(PTB2);
wiesdat 14:4c33f6240118 36
wiesdat 0:bed29da02e8b 37 MODSERIAL pc(USBTX,USBRX);
wiesdat 16:f7881b420c83 38 HIDScope scope(5);
wiesdat 0:bed29da02e8b 39 Ticker timer;
wiesdat 13:23130c3a37b1 40 volatile bool looptimerflag;
wiesdat 13:23130c3a37b1 41
wiesdat 12:8604a4c1c708 42 float emg_value2, ylp2, yhp2, yn2;
wiesdat 11:91d827c62a50 43 float emg_value1, ylp1, yhp1, yn1;
wiesdat 13:23130c3a37b1 44
wiesdat 11:91d827c62a50 45 float ysum1 = 0, yave1=0 ;
wiesdat 12:8604a4c1c708 46 float ysum2 = 0, yave2=0 ;
wiesdat 5:9415e5be8235 47
wiesdat 14:4c33f6240118 48 int set;
wiesdat 15:2adb036a191f 49 int y1, y2;
wiesdat 14:4c33f6240118 50
wiesdat 13:23130c3a37b1 51 float readEMG1()
wiesdat 0:bed29da02e8b 52 {
wiesdat 2:620ff9f02d62 53
wiesdat 11:91d827c62a50 54 emg_value1=emg1.read();
wiesdat 11:91d827c62a50 55 return emg_value1;
wiesdat 0:bed29da02e8b 56 }
wiesdat 0:bed29da02e8b 57
wiesdat 12:8604a4c1c708 58
wiesdat 12:8604a4c1c708 59 float readEMG2()
wiesdat 12:8604a4c1c708 60 {
wiesdat 12:8604a4c1c708 61
wiesdat 12:8604a4c1c708 62 emg_value2=emg2.read();
wiesdat 12:8604a4c1c708 63 return emg_value2;
wiesdat 12:8604a4c1c708 64 }
wiesdat 12:8604a4c1c708 65
wiesdat 12:8604a4c1c708 66 float notchfilter2(float ylp2)
wiesdat 12:8604a4c1c708 67 {
wiesdat 12:8604a4c1c708 68 static float x1=0,x2=0,y1=0,y2=0,x;
wiesdat 12:8604a4c1c708 69 x = ylp2;
wiesdat 12:8604a4c1c708 70 yn2 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
wiesdat 12:8604a4c1c708 71 x2 = x1;
wiesdat 12:8604a4c1c708 72 x1 = x;
wiesdat 12:8604a4c1c708 73 y2 = y1;
wiesdat 12:8604a4c1c708 74 y1 = yn2;
wiesdat 12:8604a4c1c708 75 return yn2;
wiesdat 12:8604a4c1c708 76 }
wiesdat 12:8604a4c1c708 77
wiesdat 11:91d827c62a50 78 float notchfilter1(float ylp1)
wiesdat 8:37563d2ec529 79 {
wiesdat 11:91d827c62a50 80 static float yn1,x1=0,x2=0,y1=0,y2=0,x;
wiesdat 8:37563d2ec529 81 x = ylp1;
wiesdat 11:91d827c62a50 82 yn1 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
wiesdat 8:37563d2ec529 83 x2 = x1;
wiesdat 8:37563d2ec529 84 x1 = x;
wiesdat 8:37563d2ec529 85 y2 = y1;
wiesdat 11:91d827c62a50 86 y1 = yn1;
wiesdat 11:91d827c62a50 87 return yn1;
wiesdat 8:37563d2ec529 88 }
wiesdat 8:37563d2ec529 89
wiesdat 12:8604a4c1c708 90 float hpfilter2(float yn2)
wiesdat 12:8604a4c1c708 91 {
wiesdat 12:8604a4c1c708 92 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 12:8604a4c1c708 93 x = yn2;
wiesdat 13:23130c3a37b1 94 yhp2 = x + A1HP*x1 + A0HP*x2 - B1HP*y1 - B0HP*y2;
wiesdat 12:8604a4c1c708 95 x2 = x1;
wiesdat 12:8604a4c1c708 96 x1 = x;
wiesdat 12:8604a4c1c708 97 y2 = y1;
wiesdat 12:8604a4c1c708 98 y1 = yhp2;
wiesdat 12:8604a4c1c708 99 return yhp2;
wiesdat 12:8604a4c1c708 100 }
wiesdat 11:91d827c62a50 101 float hpfilter1(float yn1)
wiesdat 0:bed29da02e8b 102 {
wiesdat 6:6f0bc2e465b0 103 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 11:91d827c62a50 104 x = yn1;
wiesdat 13:23130c3a37b1 105 yhp1 = x + A1HP*x1 + A0HP*x2 - B1HP*y1 - B0HP*y2;
wiesdat 0:bed29da02e8b 106 x2 = x1;
wiesdat 6:6f0bc2e465b0 107 x1 = x;
wiesdat 0:bed29da02e8b 108 y2 = y1;
wiesdat 6:6f0bc2e465b0 109 y1 = yhp1;
wiesdat 2:620ff9f02d62 110 return yhp1;
wiesdat 2:620ff9f02d62 111 }
wiesdat 2:620ff9f02d62 112
wiesdat 12:8604a4c1c708 113 float lpfilter2(float yhp2)
wiesdat 12:8604a4c1c708 114 {
wiesdat 12:8604a4c1c708 115 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 12:8604a4c1c708 116 x = yhp2;
wiesdat 13:23130c3a37b1 117 ylp2 = A1LP*x1-B1LP*y1+A0LP*x2-B0LP*y2;
wiesdat 12:8604a4c1c708 118 x2 = x1;
wiesdat 12:8604a4c1c708 119 x1 = x;
wiesdat 12:8604a4c1c708 120 y2 = y1;
wiesdat 12:8604a4c1c708 121 y1 = ylp2;
wiesdat 12:8604a4c1c708 122 return ylp2;
wiesdat 12:8604a4c1c708 123 }
wiesdat 12:8604a4c1c708 124
wiesdat 8:37563d2ec529 125 float lpfilter1(float yhp1)
wiesdat 2:620ff9f02d62 126 {
wiesdat 6:6f0bc2e465b0 127 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 8:37563d2ec529 128 x = yhp1;
wiesdat 13:23130c3a37b1 129 ylp1 = A1LP*x1-B1LP*y1+A0LP*x2-B0LP*y2;
wiesdat 6:6f0bc2e465b0 130 x2 = x1;
wiesdat 2:620ff9f02d62 131 x1 = x;
wiesdat 6:6f0bc2e465b0 132 y2 = y1;
wiesdat 6:6f0bc2e465b0 133 y1 = ylp1;
wiesdat 2:620ff9f02d62 134 return ylp1;
wiesdat 0:bed29da02e8b 135 }
wiesdat 0:bed29da02e8b 136
wiesdat 0:bed29da02e8b 137 void viewer()
wiesdat 0:bed29da02e8b 138 {
wiesdat 14:4c33f6240118 139 scope.set(0,ylp1);
wiesdat 14:4c33f6240118 140 scope.set(1,yave1);
wiesdat 14:4c33f6240118 141 scope.set(2,ylp2);
wiesdat 14:4c33f6240118 142 scope.set(3,yave2);
wiesdat 16:f7881b420c83 143 scope.set(4,y1);
wiesdat 0:bed29da02e8b 144 scope.send();
wiesdat 0:bed29da02e8b 145 }
wiesdat 0:bed29da02e8b 146
wiesdat 1:bc2db3bff4bb 147 void setlooptimerflag(void)
wiesdat 1:bc2db3bff4bb 148 {
wiesdat 1:bc2db3bff4bb 149 looptimerflag = true;
wiesdat 1:bc2db3bff4bb 150 }
wiesdat 1:bc2db3bff4bb 151
wiesdat 14:4c33f6240118 152 float filter1(float emg_value1)
wiesdat 14:4c33f6240118 153 {
wiesdat 14:4c33f6240118 154 static int n;
wiesdat 14:4c33f6240118 155 yn1 = notchfilter1(emg_value1);
wiesdat 14:4c33f6240118 156 yhp1 = hpfilter1(yn1);
wiesdat 14:4c33f6240118 157 ylp1 = lpfilter1(yhp1);
wiesdat 14:4c33f6240118 158 ylp1 = fabs(ylp1);
wiesdat 14:4c33f6240118 159 ysum1 = ysum1+ylp1;
wiesdat 14:4c33f6240118 160 n++;
wiesdat 14:4c33f6240118 161
wiesdat 15:2adb036a191f 162 if(n==200) {
wiesdat 15:2adb036a191f 163 yave1 = ysum1/200;
wiesdat 14:4c33f6240118 164 ysum1 = 0;
wiesdat 14:4c33f6240118 165 n = 0;
wiesdat 14:4c33f6240118 166 }
wiesdat 14:4c33f6240118 167 return yave1;
wiesdat 14:4c33f6240118 168 }
wiesdat 14:4c33f6240118 169
wiesdat 14:4c33f6240118 170 float filter2(float emg_value2)
wiesdat 14:4c33f6240118 171 {
wiesdat 14:4c33f6240118 172 static int n;
wiesdat 14:4c33f6240118 173 yn2 = notchfilter2(emg_value2);
wiesdat 14:4c33f6240118 174 yhp2 = hpfilter2(yn2);
wiesdat 14:4c33f6240118 175 ylp2 = lpfilter2(yhp2);
wiesdat 14:4c33f6240118 176 ylp2 = fabs(ylp2);
wiesdat 14:4c33f6240118 177 ysum2 = ysum2 + ylp2;
wiesdat 14:4c33f6240118 178 n++;
wiesdat 14:4c33f6240118 179
wiesdat 14:4c33f6240118 180 if(n==100) {
wiesdat 14:4c33f6240118 181 yave2 = ysum2/100;
wiesdat 14:4c33f6240118 182 ysum2 = 0;
wiesdat 14:4c33f6240118 183 n = 0;
wiesdat 14:4c33f6240118 184 }
wiesdat 14:4c33f6240118 185 return yave2;
wiesdat 14:4c33f6240118 186 }
wiesdat 14:4c33f6240118 187
wiesdat 0:bed29da02e8b 188 int main()
wiesdat 0:bed29da02e8b 189 {
wiesdat 0:bed29da02e8b 190 pc.baud(115200);
wiesdat 1:bc2db3bff4bb 191 timer.attach(setlooptimerflag,TSAMP);
wiesdat 16:f7881b420c83 192 m_speed1=0;
wiesdat 16:f7881b420c83 193 m_speed2=0;
wiesdat 16:f7881b420c83 194
wiesdat 14:4c33f6240118 195
wiesdat 0:bed29da02e8b 196 while(1) {
wiesdat 13:23130c3a37b1 197 while(!looptimerflag);
wiesdat 15:2adb036a191f 198
wiesdat 2:620ff9f02d62 199 looptimerflag = false;
wiesdat 15:2adb036a191f 200
wiesdat 14:4c33f6240118 201 emg_value1 = readEMG1();
wiesdat 14:4c33f6240118 202 emg_value2 = readEMG2();
wiesdat 16:f7881b420c83 203 yave1 = filter1(emg_value1);
wiesdat 14:4c33f6240118 204 yave2 = filter2(emg_value2);
wiesdat 16:f7881b420c83 205
wiesdat 15:2adb036a191f 206 if(yave1>TRESHOLD){
wiesdat 15:2adb036a191f 207 y1 = 1;
wiesdat 16:f7881b420c83 208 cout<<y1<<endl;
wiesdat 16:f7881b420c83 209 m_speed1=y1;
wiesdat 16:f7881b420c83 210 m_dir1=0;
wiesdat 15:2adb036a191f 211 }
wiesdat 15:2adb036a191f 212 else{
wiesdat 15:2adb036a191f 213 y1 = 0;
wiesdat 16:f7881b420c83 214 cout<<y1<<endl;
wiesdat 16:f7881b420c83 215 m_speed1=y1;
wiesdat 16:f7881b420c83 216 m_dir1=1;
wiesdat 15:2adb036a191f 217 }
wiesdat 15:2adb036a191f 218 if(yave2>TRESHOLD){
wiesdat 15:2adb036a191f 219 y2 = 1;
wiesdat 15:2adb036a191f 220
wiesdat 15:2adb036a191f 221 }
wiesdat 15:2adb036a191f 222 else{
wiesdat 15:2adb036a191f 223 y2 = 0;
wiesdat 15:2adb036a191f 224 }
wiesdat 15:2adb036a191f 225
wiesdat 15:2adb036a191f 226
wiesdat 15:2adb036a191f 227
wiesdat 15:2adb036a191f 228
wiesdat 14:4c33f6240118 229 viewer();
wiesdat 1:bc2db3bff4bb 230 }
wiesdat 0:bed29da02e8b 231 }