Dan August / Mbed 2 deprecated EMGtovelocity

nu met gains, threshold en eerst opvullen

Dependencies:   MODSERIAL mbed

Fork of EMGStdevV3 by Dan August

main.cpp@5:7644d6b16ceb, 2013-11-05 (annotated)

Committer:
DanAuhust
Date:
Tue Nov 05 08:38:03 2013 +0000
Revision:
5:7644d6b16ceb
Parent:
4:117c0bb09513
Child:
6:33f0741dbb5b
nu met gains en thresholds, en vult eerst array

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DanAuhust 0:59e2ea255892 1 #include "mbed.h"
DanAuhust 0:59e2ea255892 2 #include "MODSERIAL.h"
DanAuhust 0:59e2ea255892 3
DanAuhust 0:59e2ea255892 4 //Define objects
DanAuhust 0:59e2ea255892 5 AnalogIn emg_biceps(PTB0);
DanAuhust 0:59e2ea255892 6 AnalogIn emg_triceps(PTB1);
DanAuhust 0:59e2ea255892 7 AnalogIn emg_flexoren(PTB2);
DanAuhust 0:59e2ea255892 8 AnalogIn emg_extensoren(PTB3); //Analog input
DanAuhust 0:59e2ea255892 9 PwmOut red(LED_RED); //PWM output
DanAuhust 0:59e2ea255892 10 Ticker timer;
DanAuhust 0:59e2ea255892 11 MODSERIAL pc(USBTX,USBRX,64,1024);
DanAuhust 0:59e2ea255892 12
DanAuhust 0:59e2ea255892 13 #define offset_biceps 0 // offset ruwe invoer met adapter motoren, waar toepassen?
DanAuhust 0:59e2ea255892 14
DanAuhust 0:59e2ea255892 15 //high pass filter constantes 15Hz cutoff 4e orde, Fs = 312,5Hz (geeft een wat mooiere waarde voor periode en is geen veelvoud van 50Hz)
DanAuhust 0:59e2ea255892 16 #define NUM0 0.6731 // constante
DanAuhust 0:59e2ea255892 17 #define NUM1 -2.6925 // z^-1
DanAuhust 0:59e2ea255892 18 #define NUM2 4.0388 // z^-2etc.
DanAuhust 0:59e2ea255892 19 #define NUM3 -2.6925
DanAuhust 0:59e2ea255892 20 #define NUM4 0.6731
DanAuhust 0:59e2ea255892 21
DanAuhust 0:59e2ea255892 22 #define DEN0 1 // constante
DanAuhust 0:59e2ea255892 23 #define DEN1 -3.2133
DanAuhust 0:59e2ea255892 24 #define DEN2 3.9348
DanAuhust 0:59e2ea255892 25 #define DEN3 -2.1689
DanAuhust 0:59e2ea255892 26 #define DEN4 0.4531
DanAuhust 0:59e2ea255892 27
DanAuhust 1:1ffb9e3ae00f 28 //lowpass filter constantes 4Hz 4e orde, Fs = 312,5 Hz
DanAuhust 0:59e2ea255892 29 #define NUM0_3 0.00000236 // constante
DanAuhust 0:59e2ea255892 30 #define NUM1_3 0.00000944 // z^-1
DanAuhust 0:59e2ea255892 31 #define NUM2_3 0.00001415 // z^-2etc.
DanAuhust 0:59e2ea255892 32 #define NUM3_3 0.00000944
DanAuhust 0:59e2ea255892 33 #define NUM4_3 0.00000236
DanAuhust 0:59e2ea255892 34
DanAuhust 0:59e2ea255892 35 #define DEN0_3 1 // constante
DanAuhust 0:59e2ea255892 36 #define DEN1_3 -3.7899
DanAuhust 0:59e2ea255892 37 #define DEN2_3 5.3914
DanAuhust 0:59e2ea255892 38 #define DEN3_3 -3.4119
DanAuhust 0:59e2ea255892 39 #define DEN4_3 0.8104
DanAuhust 0:59e2ea255892 40
DanAuhust 5:7644d6b16ceb 41 double std_dev(float value, int number) {
DanAuhust 5:7644d6b16ceb 42 int n = 20;
DanAuhust 5:7644d6b16ceb 43 static int startcount = 0;
DanAuhust 5:7644d6b16ceb 44 float sum;
DanAuhust 5:7644d6b16ceb 45 float sq_sum;
DanAuhust 5:7644d6b16ceb 46 float mean;
DanAuhust 5:7644d6b16ceb 47 float variance;
DanAuhust 0:59e2ea255892 48 static int count_biceps=0;
DanAuhust 0:59e2ea255892 49 static int count_triceps=0;
DanAuhust 0:59e2ea255892 50 static int count_flexoren=0;
DanAuhust 0:59e2ea255892 51 static int count_extensoren=0;
DanAuhust 0:59e2ea255892 52 static float keeper_biceps[20];
DanAuhust 0:59e2ea255892 53 static float keeper_triceps[20];
DanAuhust 0:59e2ea255892 54 static float keeper_flexoren[20];
DanAuhust 0:59e2ea255892 55 static float keeper_extensoren[20];
DanAuhust 5:7644d6b16ceb 56
DanAuhust 5:7644d6b16ceb 57 if (startcount >= n)
DanAuhust 5:7644d6b16ceb 58 {switch (number){
DanAuhust 0:59e2ea255892 59 case 1:
DanAuhust 5:7644d6b16ceb 60 //int n = sizeof(keeper_biceps)/sizeof(float);
DanAuhust 5:7644d6b16ceb 61 keeper_biceps[count_biceps]=value;
DanAuhust 0:59e2ea255892 62 count_biceps++;
DanAuhust 5:7644d6b16ceb 63 if ( count_biceps >= n)
DanAuhust 5:7644d6b16ceb 64 count_biceps = 0;
DanAuhust 5:7644d6b16ceb 65 double sum = 0;
DanAuhust 5:7644d6b16ceb 66 double sq_sum = 0;
DanAuhust 5:7644d6b16ceb 67 for(int i = 0; i < n; ++i) {
DanAuhust 5:7644d6b16ceb 68 sum += keeper_biceps[i];
DanAuhust 5:7644d6b16ceb 69 sq_sum += keeper_biceps[i] * keeper_biceps[i];
DanAuhust 0:59e2ea255892 70 }
DanAuhust 5:7644d6b16ceb 71 double mean = sum / n;
DanAuhust 5:7644d6b16ceb 72 double variance = sq_sum / n - mean * mean;
DanAuhust 5:7644d6b16ceb 73 return sqrt(variance);
DanAuhust 0:59e2ea255892 74 break;
DanAuhust 5:7644d6b16ceb 75
DanAuhust 0:59e2ea255892 76 case 2:
DanAuhust 5:7644d6b16ceb 77 //int n = sizeof(keeper_triceps)/sizeof(float);
DanAuhust 5:7644d6b16ceb 78 keeper_triceps[count_triceps]=value;
DanAuhust 0:59e2ea255892 79 count_triceps++;
DanAuhust 5:7644d6b16ceb 80 if ( count_triceps >= n)
DanAuhust 5:7644d6b16ceb 81 count_triceps = 0;
DanAuhust 5:7644d6b16ceb 82 sum = 0;
DanAuhust 5:7644d6b16ceb 83 sq_sum = 0;
DanAuhust 5:7644d6b16ceb 84 for(int i = 0; i < n; ++i) {
DanAuhust 5:7644d6b16ceb 85 sum += keeper_triceps[i];
DanAuhust 5:7644d6b16ceb 86 sq_sum += keeper_triceps[i] * keeper_triceps[i];
DanAuhust 0:59e2ea255892 87 }
DanAuhust 5:7644d6b16ceb 88 mean = sum / n;
DanAuhust 5:7644d6b16ceb 89 variance = sq_sum / n - mean * mean;
DanAuhust 5:7644d6b16ceb 90 return sqrt(variance);
DanAuhust 0:59e2ea255892 91 break;
DanAuhust 5:7644d6b16ceb 92
DanAuhust 3:e609cd999fd2 93 case 3:
DanAuhust 5:7644d6b16ceb 94 //n = sizeof(keeper_flexoren)/sizeof(float);
DanAuhust 5:7644d6b16ceb 95 keeper_flexoren[count_flexoren]=value;
DanAuhust 0:59e2ea255892 96 count_flexoren++;
DanAuhust 5:7644d6b16ceb 97 if ( count_flexoren >= n)
DanAuhust 5:7644d6b16ceb 98 count_flexoren = 0;
DanAuhust 5:7644d6b16ceb 99 sum = 0;
DanAuhust 5:7644d6b16ceb 100 sq_sum = 0;
DanAuhust 5:7644d6b16ceb 101 for(int i = 0; i < n; ++i) {
DanAuhust 5:7644d6b16ceb 102 sum += keeper_flexoren[i];
DanAuhust 5:7644d6b16ceb 103 sq_sum += keeper_flexoren[i] * keeper_flexoren[i];
DanAuhust 0:59e2ea255892 104 }
DanAuhust 5:7644d6b16ceb 105 mean = sum / n;
DanAuhust 5:7644d6b16ceb 106 variance = sq_sum / n - mean * mean;
DanAuhust 5:7644d6b16ceb 107 return sqrt(variance);
DanAuhust 0:59e2ea255892 108 break;
DanAuhust 5:7644d6b16ceb 109
DanAuhust 0:59e2ea255892 110 case 4:
DanAuhust 5:7644d6b16ceb 111 //n = sizeof(keeper_extensoren)/sizeof(float);
DanAuhust 5:7644d6b16ceb 112 keeper_triceps[count_extensoren]=value;
DanAuhust 0:59e2ea255892 113 count_extensoren++;
DanAuhust 5:7644d6b16ceb 114 if ( count_extensoren >= n)
DanAuhust 5:7644d6b16ceb 115 count_extensoren = 0;
DanAuhust 5:7644d6b16ceb 116 sum = 0;
DanAuhust 5:7644d6b16ceb 117 sq_sum = 0;
DanAuhust 5:7644d6b16ceb 118 for(int i = 0; i < n; ++i) {
DanAuhust 5:7644d6b16ceb 119 sum += keeper_extensoren[i];
DanAuhust 5:7644d6b16ceb 120 sq_sum += keeper_extensoren[i] * keeper_extensoren[i];
DanAuhust 0:59e2ea255892 121 }
DanAuhust 5:7644d6b16ceb 122 mean = sum / n;
DanAuhust 5:7644d6b16ceb 123 variance = sq_sum / n - mean * mean;
DanAuhust 5:7644d6b16ceb 124 return sqrt(variance);
DanAuhust 3:e609cd999fd2 125 break;
DanAuhust 5:7644d6b16ceb 126 } // einde switch
DanAuhust 5:7644d6b16ceb 127 } // einde if
DanAuhust 5:7644d6b16ceb 128 else
DanAuhust 0:59e2ea255892 129 {startcount+=1;
DanAuhust 0:59e2ea255892 130 switch(sig_number){
DanAuhust 0:59e2ea255892 131 case 1:
DanAuhust 0:59e2ea255892 132 keeper_biceps[count_biceps]=input;
DanAuhust 0:59e2ea255892 133 count_biceps++;
DanAuhust 5:7644d6b16ceb 134 if(count_biceps >= n)
DanAuhust 0:59e2ea255892 135 count_biceps=0;
DanAuhust 0:59e2ea255892 136 break;
DanAuhust 0:59e2ea255892 137 case 2:
DanAuhust 0:59e2ea255892 138 keeper_triceps[count_triceps]=input;
DanAuhust 0:59e2ea255892 139 count_triceps++;
DanAuhust 5:7644d6b16ceb 140 if(count_triceps >= n)
DanAuhust 0:59e2ea255892 141 count_triceps=0;
DanAuhust 0:59e2ea255892 142 break;
DanAuhust 3:e609cd999fd2 143 case 3:
DanAuhust 3:e609cd999fd2 144 keeper_flexoren[count_flexoren]=input;
DanAuhust 3:e609cd999fd2 145 count_flexoren++;
DanAuhust 5:7644d6b16ceb 146 if(count_flexoren >= n)
DanAuhust 3:e609cd999fd2 147 count_flexoren=0;
DanAuhust 3:e609cd999fd2 148 break;
DanAuhust 3:e609cd999fd2 149 case 4:
DanAuhust 3:e609cd999fd2 150 keeper_extensoren[count_extensoren]=input;
DanAuhust 3:e609cd999fd2 151 count_extensoren++;
DanAuhust 5:7644d6b16ceb 152 if(count_extensoren >= n)
DanAuhust 3:e609cd999fd2 153 count_extensoren=0;
DanAuhust 3:e609cd999fd2 154 break;
DanAuhust 0:59e2ea255892 155 } // einde switch
DanAuhust 0:59e2ea255892 156 } // einde else
DanAuhust 5:7644d6b16ceb 157 }
DanAuhust 5:7644d6b16ceb 158
DanAuhust 0:59e2ea255892 159 float filter(int sig_number){
DanAuhust 0:59e2ea255892 160 float sig_out;
DanAuhust 0:59e2ea255892 161 // eerst variabelen definieren
DanAuhust 0:59e2ea255892 162 //biceps
DanAuhust 0:59e2ea255892 163 //filter 1
DanAuhust 0:59e2ea255892 164 float in0_biceps =0;
DanAuhust 0:59e2ea255892 165 static float in1_biceps =0, in2_biceps = 0, in3_biceps = 0, in4_biceps = 0;
DanAuhust 0:59e2ea255892 166 static float out0_biceps = 0, out1_biceps = 0 , out2_biceps = 0, out3_biceps = 0, out4_biceps = 0;
DanAuhust 5:7644d6b16ceb 167
DanAuhust 0:59e2ea255892 168 //filter 3
DanAuhust 0:59e2ea255892 169 float in0_3_biceps =0;
DanAuhust 0:59e2ea255892 170 static float in1_3_biceps =0, in2_3_biceps = 0, in3_3_biceps = 0, in4_3_biceps = 0;
DanAuhust 0:59e2ea255892 171 static float out0_3_biceps = 0, out1_3_biceps = 0 , out2_3_biceps = 0, out3_3_biceps = 0, out4_3_biceps = 0;
DanAuhust 5:7644d6b16ceb 172
DanAuhust 0:59e2ea255892 173 //triceps
DanAuhust 0:59e2ea255892 174 //filter 1
DanAuhust 0:59e2ea255892 175 float in0_triceps =0;
DanAuhust 0:59e2ea255892 176 static float in1_triceps =0, in2_triceps = 0, in3_triceps = 0, in4_triceps = 0;
DanAuhust 0:59e2ea255892 177 static float out0_triceps = 0, out1_triceps = 0 , out2_triceps = 0, out3_triceps = 0, out4_triceps = 0;
DanAuhust 5:7644d6b16ceb 178
DanAuhust 0:59e2ea255892 179 //filter 3
DanAuhust 0:59e2ea255892 180 float in0_3_triceps =0;
DanAuhust 0:59e2ea255892 181 static float in1_3_triceps =0, in2_3_triceps = 0, in3_3_triceps = 0, in4_3_triceps = 0;
DanAuhust 0:59e2ea255892 182 static float out0_3_triceps = 0, out1_3_triceps = 0 , out2_3_triceps = 0, out3_3_triceps = 0, out4_3_triceps = 0;
DanAuhust 5:7644d6b16ceb 183
DanAuhust 0:59e2ea255892 184 //flexoren
DanAuhust 0:59e2ea255892 185 //filter 1
DanAuhust 0:59e2ea255892 186 float in0_flexoren =0;
DanAuhust 0:59e2ea255892 187 static float in1_flexoren =0, in2_flexoren = 0, in3_flexoren = 0, in4_flexoren = 0;
DanAuhust 0:59e2ea255892 188 static float out0_flexoren = 0, out1_flexoren = 0 , out2_flexoren = 0, out3_flexoren = 0, out4_flexoren = 0;
DanAuhust 0:59e2ea255892 189
DanAuhust 0:59e2ea255892 190 //filter 3
DanAuhust 0:59e2ea255892 191 float in0_3_flexoren =0;
DanAuhust 0:59e2ea255892 192 static float in1_3_flexoren =0, in2_3_flexoren = 0, in3_3_flexoren = 0, in4_3_flexoren = 0;
DanAuhust 0:59e2ea255892 193 static float out0_3_flexoren = 0, out1_3_flexoren = 0 , out2_3_flexoren = 0, out3_3_flexoren = 0, out4_3_flexoren = 0;
DanAuhust 5:7644d6b16ceb 194
DanAuhust 0:59e2ea255892 195 //extensoren
DanAuhust 0:59e2ea255892 196 //filter 1
DanAuhust 0:59e2ea255892 197 float in0_extensoren =0;
DanAuhust 0:59e2ea255892 198 static float in1_extensoren =0, in2_extensoren = 0, in3_extensoren = 0, in4_extensoren = 0;
DanAuhust 0:59e2ea255892 199 static float out0_extensoren = 0, out1_extensoren = 0 , out2_extensoren = 0, out3_extensoren = 0, out4_extensoren = 0;
DanAuhust 5:7644d6b16ceb 200
DanAuhust 0:59e2ea255892 201 //filter 3
DanAuhust 0:59e2ea255892 202 float in0_3_extensoren =0;
DanAuhust 0:59e2ea255892 203 static float in1_3_extensoren =0, in2_3_extensoren = 0, in3_3_extensoren = 0, in4_3_extensoren = 0;
DanAuhust 0:59e2ea255892 204 static float out0_3_extensoren = 0, out1_3_extensoren = 0 , out2_3_extensoren = 0, out3_3_extensoren = 0, out4_3_extensoren = 0;
DanAuhust 5:7644d6b16ceb 205
DanAuhust 0:59e2ea255892 206 switch(sig_number){
DanAuhust 0:59e2ea255892 207 case 1:
DanAuhust 0:59e2ea255892 208 // signaal filteren op 15 Hz HIGHPASS
DanAuhust 0:59e2ea255892 209 in4_biceps = in3_biceps; in3_biceps = in2_biceps; in2_biceps = in1_biceps; in1_biceps = in0_biceps;
DanAuhust 0:59e2ea255892 210 in0_biceps = emg_biceps.read() - offset_biceps;
DanAuhust 0:59e2ea255892 211 out4_biceps = out3_biceps; out3_biceps = out2_biceps; out2_biceps = out1_biceps; out1_biceps = out0_biceps;
DanAuhust 0:59e2ea255892 212 out0_biceps = (NUM0*in0_biceps + NUM1*in1_biceps + NUM2*in2_biceps + NUM3*in3_biceps + NUM4*in4_biceps - DEN1*out1_biceps - DEN2*out2_biceps - DEN3*out3_biceps - DEN4*out4_biceps ) / DEN0;
DanAuhust 5:7644d6b16ceb 213
DanAuhust 0:59e2ea255892 214 //signaal filteren op 5Hz LOWPASS
DanAuhust 0:59e2ea255892 215 in4_3_biceps = in3_3_biceps; in3_3_biceps = in2_3_biceps; in2_3_biceps = in1_3_biceps; in1_3_biceps = in0_3_biceps;
DanAuhust 0:59e2ea255892 216 in0_3_biceps = abs(out0_biceps); // ruw - offset -> filter 1 -> stdev (-> filter 3)
DanAuhust 0:59e2ea255892 217 out4_3_biceps = out3_3_biceps; out3_3_biceps = out2_3_biceps; out2_3_biceps = out1_3_biceps; out1_3_biceps = out0_3_biceps;
DanAuhust 5:7644d6b16ceb 218 out0_3_biceps = (NUM0_3*in0_3_biceps + NUM1_3*in1_3_biceps + NUM2_3*in2_3_biceps + NUM3_3*in3_3_biceps + NUM4_3*in4_3_biceps - DEN1_3*out1_3_biceps - DEN2_3*out2_3_biceps - DEN3_3*out3_3_biceps - DEN4_3*out4_3_biceps ) / DEN0_3;
DanAuhust 5:7644d6b16ceb 219
DanAuhust 5:7644d6b16ceb 220 sig_out = out0_3_biceps;
DanAuhust 0:59e2ea255892 221 break;
DanAuhust 5:7644d6b16ceb 222
DanAuhust 0:59e2ea255892 223 case 2:
DanAuhust 0:59e2ea255892 224 // signaal filteren op 15 Hz HIGHPASS
DanAuhust 0:59e2ea255892 225 in4_triceps = in3_triceps; in3_triceps = in2_triceps; in2_triceps = in1_triceps; in1_triceps = in0_triceps;
DanAuhust 0:59e2ea255892 226 in0_triceps = emg_triceps.read() - offset_biceps;
DanAuhust 0:59e2ea255892 227 out4_triceps = out3_triceps; out3_triceps = out2_triceps; out2_triceps = out1_triceps; out1_triceps = out0_triceps;
DanAuhust 0:59e2ea255892 228 out0_triceps = (NUM0*in0_triceps + NUM1*in1_triceps + NUM2*in2_triceps + NUM3*in3_triceps + NUM4*in4_triceps - DEN1*out1_triceps - DEN2*out2_triceps - DEN3*out3_triceps - DEN4*out4_triceps ) / DEN0;
DanAuhust 5:7644d6b16ceb 229
DanAuhust 0:59e2ea255892 230 //signaal filteren op 5Hz LOWPASS
DanAuhust 5:7644d6b16ceb 231 in4_3_triceps = in3_3_triceps; in3_3_triceps = in2_3_triceps; in2_3_triceps = in1_3_triceps; in1_3_triceps = in0_3_triceps;
DanAuhust 0:59e2ea255892 232 in0_3_triceps = abs(out0_triceps);
DanAuhust 0:59e2ea255892 233 out4_3_triceps = out3_3_triceps; out3_3_triceps = out2_3_triceps; out2_3_triceps = out1_3_triceps; out1_3_triceps = out0_3_triceps;
DanAuhust 0:59e2ea255892 234 out0_3_triceps = (NUM0_3*in0_3_triceps + NUM1_3*in1_3_triceps + NUM2_3*in2_3_triceps + NUM3_3*in3_3_triceps + NUM4_3*in4_3_triceps - DEN1_3*out1_3_triceps - DEN2_3*out2_3_triceps - DEN3_3*out3_3_triceps - DEN4_3*out4_3_triceps ) / DEN0_3;
DanAuhust 5:7644d6b16ceb 235
DanAuhust 5:7644d6b16ceb 236 sig_out = out0_3_triceps;
DanAuhust 0:59e2ea255892 237 break;
DanAuhust 5:7644d6b16ceb 238
DanAuhust 0:59e2ea255892 239 case 3:
DanAuhust 0:59e2ea255892 240 // signaal filteren op 15 Hz HIGHPASS
DanAuhust 0:59e2ea255892 241 in4_flexoren = in3_flexoren; in3_flexoren = in2_flexoren; in2_flexoren = in1_flexoren; in1_flexoren = in0_flexoren;
DanAuhust 0:59e2ea255892 242 in0_flexoren = emg_flexoren.read();
DanAuhust 0:59e2ea255892 243 out4_flexoren = out3_flexoren; out3_flexoren = out2_flexoren; out2_flexoren = out1_flexoren; out1_flexoren = out0_flexoren;
DanAuhust 0:59e2ea255892 244 out0_flexoren = (NUM0*in0_flexoren + NUM1*in1_flexoren + NUM2*in2_flexoren + NUM3*in3_flexoren + NUM4*in4_flexoren - DEN1*out1_flexoren - DEN2*out2_flexoren - DEN3*out3_flexoren - DEN4*out4_flexoren ) / DEN0;
DanAuhust 5:7644d6b16ceb 245
DanAuhust 0:59e2ea255892 246 //signaal filteren op 5Hz LOWPASS
DanAuhust 0:59e2ea255892 247 in4_3_flexoren = in3_3_flexoren; in3_3_flexoren = in2_3_flexoren; in2_3_flexoren = in1_3_flexoren; in1_3_flexoren = in0_3_flexoren;
DanAuhust 5:7644d6b16ceb 248 in0_3_flexoren = abs(out0_flexoren);
DanAuhust 0:59e2ea255892 249 out4_3_flexoren = out3_3_flexoren; out3_3_flexoren = out2_3_flexoren; out2_3_flexoren = out1_3_flexoren; out1_3_flexoren = out0_3_flexoren;
DanAuhust 0:59e2ea255892 250 out0_3_flexoren = (NUM0_3*in0_3_flexoren + NUM1_3*in1_3_flexoren + NUM2_3*in2_3_flexoren + NUM3_3*in3_3_flexoren + NUM4_3*in4_3_flexoren - DEN1_3*out1_3_flexoren - DEN2_3*out2_3_flexoren - DEN3_3*out3_3_flexoren - DEN4_3*out4_3_flexoren ) / DEN0_3;
DanAuhust 5:7644d6b16ceb 251
DanAuhust 5:7644d6b16ceb 252 sig_out = out0_3_flexoren;
DanAuhust 0:59e2ea255892 253 break;
DanAuhust 5:7644d6b16ceb 254
DanAuhust 0:59e2ea255892 255 case 4:
DanAuhust 0:59e2ea255892 256 // signaal filteren op 15 Hz HIGHPASS
DanAuhust 0:59e2ea255892 257 in4_extensoren = in3_extensoren; in3_extensoren = in2_extensoren; in2_extensoren = in1_extensoren; in1_extensoren = in0_extensoren;
DanAuhust 0:59e2ea255892 258 in0_extensoren = emg_extensoren.read();
DanAuhust 0:59e2ea255892 259 out4_extensoren = out3_extensoren; out3_extensoren = out2_extensoren; out2_extensoren = out1_extensoren; out1_extensoren = out0_extensoren;
DanAuhust 0:59e2ea255892 260 out0_extensoren = (NUM0*in0_extensoren + NUM1*in1_extensoren + NUM2*in2_extensoren + NUM3*in3_extensoren + NUM4*in4_extensoren - DEN1*out1_extensoren - DEN2*out2_extensoren - DEN3*out3_extensoren - DEN4*out4_extensoren ) / DEN0;
DanAuhust 5:7644d6b16ceb 261
DanAuhust 0:59e2ea255892 262 //signaal filteren op 5Hz LOWPASS
DanAuhust 0:59e2ea255892 263 in4_3_extensoren = in3_3_extensoren; in3_3_extensoren = in2_3_extensoren; in2_3_extensoren = in1_3_extensoren; in1_3_extensoren = in0_3_extensoren;
DanAuhust 5:7644d6b16ceb 264 in0_3_extensoren = abs(out0_extensoren);
DanAuhust 0:59e2ea255892 265 out4_3_extensoren = out3_3_extensoren; out3_3_extensoren = out2_3_extensoren; out2_3_extensoren = out1_3_extensoren; out1_3_extensoren = out0_3_extensoren;
DanAuhust 0:59e2ea255892 266 out0_3_extensoren = (NUM0_3*in0_3_extensoren + NUM1_3*in1_3_extensoren + NUM2_3*in2_3_extensoren + NUM3_3*in3_3_extensoren + NUM4_3*in4_3_extensoren - DEN1_3*out1_3_extensoren - DEN2_3*out2_3_extensoren - DEN3_3*out3_3_extensoren - DEN4_3*out4_3_extensoren ) / DEN0_3;
DanAuhust 5:7644d6b16ceb 267
DanAuhust 5:7644d6b16ceb 268 sig_out = out0_3_extensoren;
DanAuhust 0:59e2ea255892 269 break;
DanAuhust 0:59e2ea255892 270 }
DanAuhust 0:59e2ea255892 271 return sig_out;
DanAuhust 0:59e2ea255892 272 }
DanAuhust 0:59e2ea255892 273
DanAuhust 0:59e2ea255892 274 void looper()
DanAuhust 0:59e2ea255892 275 { float emg_value_biceps;
DanAuhust 0:59e2ea255892 276 float emg_value_triceps;
DanAuhust 0:59e2ea255892 277 float emg_value_flexoren;
DanAuhust 0:59e2ea255892 278 float emg_value_extensoren;
DanAuhust 0:59e2ea255892 279 float dy;
DanAuhust 5:7644d6b16ceb 280 float dx;
DanAuhust 0:59e2ea255892 281 //static int sig_count = 1;
DanAuhust 5:7644d6b16ceb 282
DanAuhust 5:7644d6b16ceb 283 emg_value_biceps = std_dev(filter(1),1);
DanAuhust 5:7644d6b16ceb 284 emg_value_triceps = std_dev(filter(2),2);
DanAuhust 5:7644d6b16ceb 285 emg_value_extensoren = std_dev(filter(3),3);
DanAuhust 5:7644d6b16ceb 286 emg_value_flexoren = std_dev(filter(4),4);
DanAuhust 3:e609cd999fd2 287
DanAuhust 3:e609cd999fd2 288 if(emg_value_biceps < 4.5)
DanAuhust 3:e609cd999fd2 289 emg_value_biceps=0;
DanAuhust 3:e609cd999fd2 290 else if (emg_value_biceps > 13)
DanAuhust 3:e609cd999fd2 291 emg_value_biceps = 13;
DanAuhust 3:e609cd999fd2 292 //emg_value_biceps = emg_value_biceps;
DanAuhust 0:59e2ea255892 293
DanAuhust 3:e609cd999fd2 294 if(emg_value_triceps < 2.5)
DanAuhust 0:59e2ea255892 295 emg_value_biceps=0;
DanAuhust 3:e609cd999fd2 296 else if (emg_value_biceps > 10)
DanAuhust 3:e609cd999fd2 297 emg_value_biceps = 10;
DanAuhust 3:e609cd999fd2 298 //emg_value_triceps = emg_value_triceps;
DanAuhust 3:e609cd999fd2 299
DanAuhust 3:e609cd999fd2 300 if(emg_value_flexoren < 2)
DanAuhust 3:e609cd999fd2 301 emg_value_flexoren=0;
DanAuhust 3:e609cd999fd2 302 else if (emg_value_flexoren > 8)
DanAuhust 3:e609cd999fd2 303 emg_value_flexoren = 8;
DanAuhust 3:e609cd999fd2 304 emg_value_flexoren = 2*emg_value_flexoren;
DanAuhust 3:e609cd999fd2 305
DanAuhust 3:e609cd999fd2 306 if(emg_value_extensoren < 5)
DanAuhust 3:e609cd999fd2 307 emg_value_extensoren=0;
DanAuhust 3:e609cd999fd2 308 else if (emg_value_extensoren > 13)
DanAuhust 3:e609cd999fd2 309 emg_value_extensoren = 13;
DanAuhust 3:e609cd999fd2 310
DanAuhust 3:e609cd999fd2 311 dy = emg_value_biceps - emg_value_triceps;
DanAuhust 3:e609cd999fd2 312 dx = 2*(emg_value_flexoren - emg_value_extensoren);
DanAuhust 5:7644d6b16ceb 313 if(emg_value_biceps < 4.5)
DanAuhust 5:7644d6b16ceb 314 emg_value_biceps=0;
DanAuhust 5:7644d6b16ceb 315 else if (emg_value_biceps > 13)
DanAuhust 5:7644d6b16ceb 316 emg_value_biceps = 13;
DanAuhust 5:7644d6b16ceb 317 //emg_value_biceps = emg_value_biceps;
DanAuhust 3:e609cd999fd2 318
DanAuhust 5:7644d6b16ceb 319 if(emg_value_triceps < 2.5)
DanAuhust 5:7644d6b16ceb 320 emg_value_biceps=0;
DanAuhust 5:7644d6b16ceb 321 else if (emg_value_biceps > 10)
DanAuhust 5:7644d6b16ceb 322 emg_value_biceps = 10;
DanAuhust 5:7644d6b16ceb 323 //emg_value_triceps = emg_value_triceps;
DanAuhust 5:7644d6b16ceb 324
DanAuhust 5:7644d6b16ceb 325 if(emg_value_flexoren < 2)
DanAuhust 5:7644d6b16ceb 326 emg_value_flexoren=0;
DanAuhust 5:7644d6b16ceb 327 else if (emg_value_flexoren > 8)
DanAuhust 5:7644d6b16ceb 328 emg_value_flexoren = 8;
DanAuhust 5:7644d6b16ceb 329 emg_value_flexoren = 2*emg_value_flexoren;
DanAuhust 5:7644d6b16ceb 330
DanAuhust 5:7644d6b16ceb 331 if(emg_value_extensoren < 5)
DanAuhust 5:7644d6b16ceb 332 emg_value_extensoren=0;
DanAuhust 5:7644d6b16ceb 333 else if (emg_value_extensoren > 13)
DanAuhust 5:7644d6b16ceb 334 emg_value_extensoren = 13;
DanAuhust 5:7644d6b16ceb 335
DanAuhust 5:7644d6b16ceb 336 dy = emg_value_biceps - emg_value_triceps;
DanAuhust 5:7644d6b16ceb 337 dx = 2*(emg_value_flexoren - emg_value_extensoren);
DanAuhust 5:7644d6b16ceb 338
DanAuhust 5:7644d6b16ceb 339 dy = emg_value_biceps-emg_value_triceps;
DanAuhust 5:7644d6b16ceb 340 dx = emg_value_extensoren - emg_value_flexoren;
DanAuhust 0:59e2ea255892 341 if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30)
DanAuhust 5:7644d6b16ceb 342 pc.printf("%.6f\n",dy);
DanAuhust 0:59e2ea255892 343 /**When not using the LED, the above could also have been done this way:
DanAuhust 0:59e2ea255892 344 * pc.printf("%.6\n", emg0.read());
DanAuhust 0:59e2ea255892 345 */
DanAuhust 0:59e2ea255892 346 }
DanAuhust 0:59e2ea255892 347
DanAuhust 0:59e2ea255892 348 int main()
DanAuhust 5:7644d6b16ceb 349
DanAuhust 0:59e2ea255892 350 {
DanAuhust 0:59e2ea255892 351 /*setup baudrate. Choose the same in your program on PC side*/
DanAuhust 0:59e2ea255892 352 pc.baud(115200);
DanAuhust 0:59e2ea255892 353 /*set the period for the PWM to the red LED*/
DanAuhust 5:7644d6b16ceb 354 //red.period_ms(2);
DanAuhust 0:59e2ea255892 355 /**Here you attach the 'void looper(void)' function to the Ticker object
DanAuhust 0:59e2ea255892 356 * The looper() function will be called every 1/Fs seconds.
DanAuhust 0:59e2ea255892 357 * Please mind that the parentheses after looper are omitted when using attach.
DanAuhust 0:59e2ea255892 358 */
DanAuhust 0:59e2ea255892 359 timer.attach(looper,0.0032); //invullen in seconde. .0032 is niet eens afgerond, dus vandaar die frequentie.
DanAuhust 0:59e2ea255892 360 while(1) //Loop
DanAuhust 0:59e2ea255892 361 {
DanAuhust 0:59e2ea255892 362 /*Empty!*/
DanAuhust 0:59e2ea255892 363 /*Everything is handled by the interrupt routine now!*/
DanAuhust 0:59e2ea255892 364 }
DanAuhust 0:59e2ea255892 365 }