BMT M9 Groep01 / Mbed 2 deprecated test_emg

emg

Dependencies:   HIDScope MODSERIAL mbed-dsp mbed TouchButton

Fork of test by BMT M9 Groep01

main.cpp@1:6a8b45298e54, 2014-10-21 (annotated)

Committer:
s1340735
Date:
Tue Oct 21 14:27:50 2014 +0000
Revision:
1:6a8b45298e54
Parent:
0:abe0bc5c43b7
Child:
2:a86b09b00008
biceps klaar. nu nog triceps toevoegen

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1340735 0:abe0bc5c43b7 1 #include "mbed.h"
s1340735 0:abe0bc5c43b7 2 #include "HIDScope.h"
s1340735 0:abe0bc5c43b7 3 #include "MODSERIAL.h"
s1340735 0:abe0bc5c43b7 4 #include "arm_math.h"
s1340735 0:abe0bc5c43b7 5
s1340735 0:abe0bc5c43b7 6 HIDScope scope(2);//is dit 4 voor 2 spieren? en hoe zit het met scope.set?
s1340735 0:abe0bc5c43b7 7
s1340735 0:abe0bc5c43b7 8 AnalogIn emgB(PTB0);
s1340735 0:abe0bc5c43b7 9
s1340735 0:abe0bc5c43b7 10 //*** OBJECTS ***
s1340735 0:abe0bc5c43b7 11 float (filtered_emgB);
s1340735 0:abe0bc5c43b7 12 float drempelwaardeB1, drempelwaardeB2, drempelwaardeB3;//B1=snelheidsstand 1, B2=snelheidsstand 2, B3=snelheidsstand 3
s1340735 0:abe0bc5c43b7 13 int yB1, yB2, yB3;
s1340735 0:abe0bc5c43b7 14 float B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, MOVAVG_B;//moving average objects
s1340735 1:6a8b45298e54 15 int snelheidsstand;
s1340735 0:abe0bc5c43b7 16
s1340735 0:abe0bc5c43b7 17 //*** FILTERS ***
s1340735 0:abe0bc5c43b7 18 arm_biquad_casd_df1_inst_f32 lowpass;
s1340735 0:abe0bc5c43b7 19 //constants for 50Hz lowpass
s1340735 0:abe0bc5c43b7 20 float lowpass_const[] = {0.2928920553, 0.5857841107, 0.2928920554, -0, -0.17156822136};//{a0 a1 a2 -b1 -b2} van online calculator
s1340735 0:abe0bc5c43b7 21 float lowpass_states[4];
s1340735 0:abe0bc5c43b7 22
s1340735 0:abe0bc5c43b7 23 arm_biquad_casd_df1_inst_f32 highpass;
s1340735 0:abe0bc5c43b7 24 //constants for 10Hz highpass
s1340735 0:abe0bc5c43b7 25 float highpass_const[] = {0.8005910267, -1.6011820533, 0.8005910267, 1.5610153913, -0.6413487154};//{a0 a1 a2 -b1 -b2}
s1340735 0:abe0bc5c43b7 26 float highpass_states[4];
s1340735 0:abe0bc5c43b7 27
s1340735 0:abe0bc5c43b7 28 //*** CALIBRATIE ***
s1340735 0:abe0bc5c43b7 29
s1340735 0:abe0bc5c43b7 30 //
s1340735 0:abe0bc5c43b7 31
s1340735 0:abe0bc5c43b7 32 //*** BICEP EMG ***
s1340735 0:abe0bc5c43b7 33 void Biceps()
s1340735 0:abe0bc5c43b7 34 {
s1340735 0:abe0bc5c43b7 35 uint16_t emg_valueB;
s1340735 0:abe0bc5c43b7 36 float emg_value_f32B;
s1340735 0:abe0bc5c43b7 37
s1340735 0:abe0bc5c43b7 38
s1340735 0:abe0bc5c43b7 39 //lezen
s1340735 0:abe0bc5c43b7 40 emg_valueB = emgB.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
s1340735 0:abe0bc5c43b7 41 emg_value_f32B = emgB.read();
s1340735 0:abe0bc5c43b7 42
s1340735 0:abe0bc5c43b7 43
s1340735 0:abe0bc5c43b7 44 //filteren
s1340735 0:abe0bc5c43b7 45 arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32B, &filtered_emgB, 1 );
s1340735 0:abe0bc5c43b7 46
s1340735 0:abe0bc5c43b7 47 arm_biquad_cascade_df1_f32(&lowpass, &filtered_emgB, &filtered_emgB, 1 );
s1340735 0:abe0bc5c43b7 48
s1340735 0:abe0bc5c43b7 49 filtered_emgB = fabs(filtered_emgB);
s1340735 0:abe0bc5c43b7 50
s1340735 0:abe0bc5c43b7 51
s1340735 0:abe0bc5c43b7 52 //moving average
s1340735 0:abe0bc5c43b7 53 B0=filtered_emgB;
s1340735 0:abe0bc5c43b7 54
s1340735 0:abe0bc5c43b7 55 MOVAVG_B=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;
s1340735 0:abe0bc5c43b7 56
s1340735 0:abe0bc5c43b7 57 B9=B8;
s1340735 0:abe0bc5c43b7 58 B8=B7;
s1340735 0:abe0bc5c43b7 59 B7=B6;
s1340735 0:abe0bc5c43b7 60 B6=B5;
s1340735 0:abe0bc5c43b7 61 B5=B4;
s1340735 0:abe0bc5c43b7 62 B4=B3;
s1340735 0:abe0bc5c43b7 63 B3=B2;
s1340735 0:abe0bc5c43b7 64 B2=B1;
s1340735 0:abe0bc5c43b7 65 B1=B0;
s1340735 0:abe0bc5c43b7 66
s1340735 0:abe0bc5c43b7 67 //naar scherm sturen
s1340735 0:abe0bc5c43b7 68 scope.set(0,emg_valueB);
s1340735 0:abe0bc5c43b7 69 scope.set(1,MOVAVG_B);
s1340735 0:abe0bc5c43b7 70 scope.send();
s1340735 0:abe0bc5c43b7 71
s1340735 1:6a8b45298e54 72 // *** TRICEP EMG ***
s1340735 1:6a8b45298e54 73 void Triceps(){
s1340735 0:abe0bc5c43b7 74
s1340735 0:abe0bc5c43b7 75 // *** MAIN ***
s1340735 0:abe0bc5c43b7 76 int main() {
s1340735 0:abe0bc5c43b7 77
s1340735 1:6a8b45298e54 78 Ticker log_timer;
s1340735 1:6a8b45298e54 79 arm_biquad_cascade_df1_init_f32(&lowpass,1,lowpass_const,lowpass_states);
s1340735 1:6a8b45298e54 80 arm_biquad_cascade_df1_init_f32(&highpass,1,highpass_const,highpass_states);
s1340735 0:abe0bc5c43b7 81
s1340735 1:6a8b45298e54 82 log_timer.attach(Biceps, 0.005);
s1340735 1:6a8b45298e54 83 while(1) {}
s1340735 0:abe0bc5c43b7 84
s1340735 1:6a8b45298e54 85 //aansturing
s1340735 0:abe0bc5c43b7 86 if (filtered_emgB >= drempelwaardeB1) {
s1340735 0:abe0bc5c43b7 87 yB1=1;
s1340735 0:abe0bc5c43b7 88 if (filtered_emgB >= drempelwaardeB2) {
s1340735 0:abe0bc5c43b7 89 yB2=1;
s1340735 0:abe0bc5c43b7 90 if (filtered_emgB >= drempelwaardeB3) {
s1340735 0:abe0bc5c43b7 91 yB3=1;
s1340735 0:abe0bc5c43b7 92 } else {
s1340735 0:abe0bc5c43b7 93 yB3=0;
s1340735 0:abe0bc5c43b7 94 }
s1340735 0:abe0bc5c43b7 95 } else {
s1340735 0:abe0bc5c43b7 96 yB2=0;
s1340735 0:abe0bc5c43b7 97 }
s1340735 0:abe0bc5c43b7 98 } else {
s1340735 0:abe0bc5c43b7 99 yB1=0;
s1340735 0:abe0bc5c43b7 100 }
s1340735 0:abe0bc5c43b7 101
s1340735 1:6a8b45298e54 102 //controle
s1340735 1:6a8b45298e54 103 snelheidsstand=yB1+yB2+yB3;
s1340735 0:abe0bc5c43b7 104
s1340735 1:6a8b45298e54 105 if (snelheidsstand==1) {
s1340735 1:6a8b45298e54 106 pc.printf("Motor 1 beweegt met snelheid 1\n");
s1340735 1:6a8b45298e54 107 } else {
s1340735 1:6a8b45298e54 108 pc.printf("Motor 1 beweegt niet met snelheid 1\n");
s1340735 1:6a8b45298e54 109 }
s1340735 1:6a8b45298e54 110 if (snelheidsstand==2) {
s1340735 1:6a8b45298e54 111 pc.printf("Motor 1 beweegt met snelheid 2\n");
s1340735 1:6a8b45298e54 112 } else {
s1340735 1:6a8b45298e54 113 pc.printf("Motor 1 beweegt niet met snelheid 2\n");
s1340735 1:6a8b45298e54 114 }
s1340735 1:6a8b45298e54 115 if (snelheidsstand==3) {
s1340735 1:6a8b45298e54 116 pc.printf("Motor 1 beweegt met snelheid 3\n");
s1340735 1:6a8b45298e54 117 } else {
s1340735 1:6a8b45298e54 118 pc.printf("Motor 1 beweegt niet met snelheid 3\n");
s1340735 1:6a8b45298e54 119 }
s1340735 1:6a8b45298e54 120 }
s1340735 1:6a8b45298e54 121 }
s1340735 1:6a8b45298e54 122