Rick Poppe / Mbed 2 deprecated EMG_filter

working version but stripped of most unnecessary code like print statements

Dependencies:   HIDScope MODSERIAL biquadFilter mbed FastPWM QEI

main.cpp@40:1ca50c657cc5, 2016-10-27 (annotated)

Committer:
RiP
Date:
Thu Oct 27 12:41:00 2016 +0000
Revision:
40:1ca50c657cc5
Parent:
39:955929e25858
Child:
41:a89907bb3f70
before combining motor control and emg filtering

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:32bb76391d89 1 #include "mbed.h"
vsluiter 11:ce72ec658a95 2 #include "HIDScope.h"
RiP 21:3aecd735319d 3 #include "BiQuad.h"
RiP 22:f38a15e851d2 4 #include "MODSERIAL.h"
vsluiter 0:32bb76391d89 5
RiP 40:1ca50c657cc5 6 // in gebruik: D(0,1,4,5,6,7,8,10,11,12,13) A(0,1,2)
RiP 40:1ca50c657cc5 7
RiP 39:955929e25858 8 MODSERIAL pc(USBTX, USBRX);
RiP 40:1ca50c657cc5 9 HIDScope scope(3); // aantal scopes in hidscope opzetten
RiP 36:344588e69589 10
vsluiter 4:8b298dfada81 11 //Define objects
RiP 31:9c3b022f1dc3 12 //Define the button interrupt for the calibration
RiP 31:9c3b022f1dc3 13 InterruptIn button_calibrate(PTA4);
RiP 24:01b4b51b5dc6 14
RiP 31:9c3b022f1dc3 15 //Define the EMG inputs
RiP 21:3aecd735319d 16 AnalogIn emg1( A0 );
RiP 21:3aecd735319d 17 AnalogIn emg2( A1 );
RiP 21:3aecd735319d 18 AnalogIn emg3( A2 );
tomlankhorst 19:2bf824669684 19
RiP 31:9c3b022f1dc3 20 //Define the Tickers
RiP 39:955929e25858 21 Ticker pos_timer; // the timer which is used to print the position every second
RiP 39:955929e25858 22 Ticker sample_timer; // the timer which is used to decide when a sample needs to be taken
vsluiter 2:e314bb3b2d99 23
RiP 36:344588e69589 24 //Initialize all variables
RiP 39:955929e25858 25 volatile bool sampletimer = false; // a variable which is changed when a sample needs to be taken
RiP 25:1a71424b05ff 26
RiP 39:955929e25858 27 double threshold = 0.04; // the threshold which the emg signals need to surpass to do something
RiP 40:1ca50c657cc5 28 double samplefreq=0.002; // every 0.002 sec a sample will be taken this is a frequency of 500 Hz
RiP 39:955929e25858 29 double emg02; // the first emg signal
RiP 39:955929e25858 30 double emg12; // the second emg signal
RiP 39:955929e25858 31 double emg22; // the third emg signal
RiP 39:955929e25858 32 double ref_x=0.0000; // the x reference position
RiP 39:955929e25858 33 double ref_y=0.0000; // the y reference position
RiP 39:955929e25858 34 double old_ref_x; // the old x reference
RiP 39:955929e25858 35 double old_ref_y; // the old y reference
RiP 39:955929e25858 36 double speed=0.00002; // the variable with which a speed is reached of 1cm/s
RiP 39:955929e25858 37 double theta=0.0; // angle of the arm
RiP 39:955929e25858 38 double radius=0.0; // radius of the arm
RiP 36:344588e69589 39
RiP 39:955929e25858 40 const double minRadius=0.3; // minimum radius of arm
RiP 39:955929e25858 41 const double maxRadius=0.6; // maximum radius of arm
RiP 39:955929e25858 42 const double minAngle=-1.25; // minimum angle for limiting controller
RiP 40:1ca50c657cc5 43 const double min_y=-0.28; // minimum height which the spatula can reach
RiP 39:955929e25858 44 char key; // variable to place the keyboard input
RiP 28:3b1b29193851 45
RiP 31:9c3b022f1dc3 46 //Define the needed Biquad chains
RiP 21:3aecd735319d 47 BiQuadChain bqc11;
RiP 21:3aecd735319d 48 BiQuadChain bqc13;
RiP 21:3aecd735319d 49 BiQuadChain bqc21;
RiP 21:3aecd735319d 50 BiQuadChain bqc23;
RiP 21:3aecd735319d 51 BiQuadChain bqc31;
RiP 21:3aecd735319d 52 BiQuadChain bqc33;
RiP 31:9c3b022f1dc3 53
RiP 31:9c3b022f1dc3 54 //Define the BiQuads for the filter of the first emg signal
RiP 31:9c3b022f1dc3 55 //Notch filter
RiP 21:3aecd735319d 56 BiQuad bq111(0.9795, -1.5849, 0.9795, 1.0000, -1.5849, 0.9589);
RiP 21:3aecd735319d 57 BiQuad bq112(0.9833, -1.5912, 0.9833, 1.0000, -1.5793, 0.9787);
RiP 21:3aecd735319d 58 BiQuad bq113(0.9957, -1.6111, 0.9957, 1.0000, -1.6224, 0.9798);
RiP 31:9c3b022f1dc3 59 //High pass filter
RiP 36:344588e69589 60 //BiQuad bq121( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 ); //Old biquad values
RiP 36:344588e69589 61 BiQuad bq121( 0.8956, -1.7911, 0.8956, 1.0000, -1.7814, 0.7941);
RiP 36:344588e69589 62 BiQuad bq122( 0.9192, -1.8385, 0.9192, 1.0000, -1.8319, 0.8450);
RiP 36:344588e69589 63 BiQuad bq123( 0.9649, -1.9298, 0.9649, 1.0000, -1.9266, 0.9403);
RiP 31:9c3b022f1dc3 64 //Low pass filter
RiP 21:3aecd735319d 65 BiQuad bq131( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
RiP 21:3aecd735319d 66
RiP 31:9c3b022f1dc3 67 //Define the BiQuads for the filter of the second emg signal
RiP 31:9c3b022f1dc3 68 //Notch filter
RiP 31:9c3b022f1dc3 69 BiQuad bq211 = bq111;
RiP 31:9c3b022f1dc3 70 BiQuad bq212 = bq112;
RiP 31:9c3b022f1dc3 71 BiQuad bq213 = bq113;
RiP 39:955929e25858 72 //High pass filter
RiP 31:9c3b022f1dc3 73 BiQuad bq221 = bq121;
RiP 36:344588e69589 74 BiQuad bq222 = bq122;
RiP 36:344588e69589 75 BiQuad bq223 = bq123;
RiP 39:955929e25858 76 //Low pass filter
RiP 31:9c3b022f1dc3 77 BiQuad bq231 = bq131;
RiP 21:3aecd735319d 78
RiP 31:9c3b022f1dc3 79 //Define the BiQuads for the filter of the third emg signal
RiP 31:9c3b022f1dc3 80 //notch filter
RiP 31:9c3b022f1dc3 81 BiQuad bq311 = bq111;
RiP 31:9c3b022f1dc3 82 BiQuad bq312 = bq112;
RiP 31:9c3b022f1dc3 83 BiQuad bq313 = bq113;
RiP 31:9c3b022f1dc3 84 //High pass filter
RiP 31:9c3b022f1dc3 85 BiQuad bq321 = bq121;
RiP 36:344588e69589 86 BiQuad bq323 = bq122;
RiP 36:344588e69589 87 BiQuad bq322 = bq123;
RiP 31:9c3b022f1dc3 88 //low pass filter
RiP 31:9c3b022f1dc3 89 BiQuad bq331 = bq131;
RiP 22:f38a15e851d2 90
RiP 22:f38a15e851d2 91 void sampleflag()
RiP 22:f38a15e851d2 92 {
mefix 38:0c8a6b0834da 93 if (sampletimer==true) {
RiP 39:955929e25858 94 // this if statement is used to see if the code takes too long before it is called again
RiP 39:955929e25858 95 pc.printf("rate too high error in sampleflag\n\r");
RiP 37:af85a7b57a25 96 }
RiP 36:344588e69589 97 //This sets the go flag for when the function sample needs to be called
RiP 22:f38a15e851d2 98 sampletimer=true;
RiP 22:f38a15e851d2 99 }
RiP 22:f38a15e851d2 100
RiP 36:344588e69589 101 void calibrate()
RiP 24:01b4b51b5dc6 102 {
RiP 36:344588e69589 103 //This resets the reference signals so that the robot can be calibrated
RiP 37:af85a7b57a25 104 ref_x=0.0000;
RiP 37:af85a7b57a25 105 ref_y=0.0000;
RiP 24:01b4b51b5dc6 106 }
RiP 22:f38a15e851d2 107
RiP 39:955929e25858 108 void sample()
vsluiter 2:e314bb3b2d99 109 {
RiP 39:955929e25858 110 //This checks if a key is pressed and changes the variable key in the pressed key
RiP 33:fcd4568f1c86 111 if (pc.readable()==1) {
RiP 33:fcd4568f1c86 112 key=pc.getc();
mefix 38:0c8a6b0834da 113 }
RiP 36:344588e69589 114 //Read the emg signals and filter it
RiP 22:f38a15e851d2 115
RiP 36:344588e69589 116 emg02=bqc13.step(fabs(bqc11.step(emg1.read()))); //filtered signal 0
RiP 36:344588e69589 117 emg12=bqc23.step(fabs(bqc21.step(emg2.read()))); //filtered signal 1
RiP 36:344588e69589 118 emg22=bqc33.step(fabs(bqc31.step(emg3.read()))); //filtered signal 2
RiP 29:ac08c1a32c54 119
RiP 39:955929e25858 120 //remember what the reference was
RiP 37:af85a7b57a25 121 old_ref_x=ref_x;
RiP 37:af85a7b57a25 122 old_ref_y=ref_y;
RiP 39:955929e25858 123 //look if the emg signals go over the threshold and change the reference accordingly
RiP 33:fcd4568f1c86 124 if (emg02>threshold&&emg12>threshold&&emg22>threshold || key=='d') {
RiP 37:af85a7b57a25 125 ref_x=ref_x-speed;
RiP 37:af85a7b57a25 126 ref_y=ref_y-speed;
RiP 31:9c3b022f1dc3 127
RiP 33:fcd4568f1c86 128 } else if (emg02>threshold&&emg12>threshold || key == 'a' ) {
RiP 37:af85a7b57a25 129 ref_x=ref_x-speed;
RiP 23:54d28f9eef53 130
RiP 33:fcd4568f1c86 131 } else if (emg02>threshold&&emg22>threshold || key == 's') {
RiP 37:af85a7b57a25 132 ref_y=ref_y-speed;
RiP 31:9c3b022f1dc3 133
RiP 33:fcd4568f1c86 134 } else if (emg12>threshold&&emg22>threshold || key == 'e' ) {
RiP 31:9c3b022f1dc3 135 ref_x=ref_x+speed;
RiP 31:9c3b022f1dc3 136 ref_y=ref_y+speed;
RiP 31:9c3b022f1dc3 137
RiP 33:fcd4568f1c86 138 } else if (emg12>threshold || key == 'q' ) {
RiP 31:9c3b022f1dc3 139 ref_x=ref_x+speed;
RiP 40:1ca50c657cc5 140
RiP 33:fcd4568f1c86 141 } else if (emg22>threshold || key == 'w') {
RiP 31:9c3b022f1dc3 142 ref_y=ref_y+speed;
RiP 37:af85a7b57a25 143 }
RiP 37:af85a7b57a25 144
RiP 39:955929e25858 145 // convert the x and y reference to the theta and radius reference
mefix 38:0c8a6b0834da 146 theta=atan(ref_y/(ref_x+minRadius));
mefix 38:0c8a6b0834da 147 radius=sqrt(pow(ref_x+minRadius,2)+pow(ref_y,2));
mefix 38:0c8a6b0834da 148
RiP 39:955929e25858 149 //look if the new reference is outside the possible range and revert back to the old reference if it is outside the range
mefix 38:0c8a6b0834da 150 if (theta < minAngle) {
RiP 37:af85a7b57a25 151 ref_x=old_ref_x;
RiP 37:af85a7b57a25 152 ref_y=old_ref_y;
RiP 40:1ca50c657cc5 153
RiP 37:af85a7b57a25 154 } else if (radius < minRadius) {
RiP 37:af85a7b57a25 155 ref_x=old_ref_x;
RiP 37:af85a7b57a25 156 ref_y=old_ref_y;
RiP 40:1ca50c657cc5 157
mefix 38:0c8a6b0834da 158 } else if ( radius > maxRadius) {
RiP 37:af85a7b57a25 159 ref_x=old_ref_x;
RiP 37:af85a7b57a25 160 ref_y=old_ref_y;
RiP 40:1ca50c657cc5 161 } else if (ref_y<min_y) {
RiP 40:1ca50c657cc5 162 ref_x=old_ref_x;
RiP 40:1ca50c657cc5 163 ref_y=old_ref_y;
RiP 22:f38a15e851d2 164 }
vsluiter 2:e314bb3b2d99 165 }
vsluiter 0:32bb76391d89 166
RiP 27:1ff7fa636f1c 167 void my_pos()
RiP 27:1ff7fa636f1c 168 {
RiP 36:344588e69589 169 //This function is attached to a ticker so that the reference position is printed every second.
RiP 37:af85a7b57a25 170 pc.printf("x_pos=%.4f\ty_pos=%.4f\tradius=%.4f\tangle=%.4f\n\r",ref_x,ref_y,radius,theta);
RiP 27:1ff7fa636f1c 171
RiP 27:1ff7fa636f1c 172 }
RiP 26:91d48c0b722d 173
vsluiter 0:32bb76391d89 174 int main()
RiP 21:3aecd735319d 175 {
RiP 29:ac08c1a32c54 176 pc.printf("RESET\n\r");
RiP 22:f38a15e851d2 177 pc.baud(115200);
RiP 22:f38a15e851d2 178
RiP 39:955929e25858 179 //Attach the Biquads to the Biquad chains
RiP 36:344588e69589 180 bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 ).add( &bq122 ).add( &bq123 );
RiP 21:3aecd735319d 181 bqc13.add( &bq131);
RiP 36:344588e69589 182 bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 ).add( &bq222 ).add( &bq223 );
RiP 21:3aecd735319d 183 bqc23.add( &bq231);
RiP 36:344588e69589 184 bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 ).add( &bq322 ).add( &bq323 );
RiP 21:3aecd735319d 185 bqc33.add( &bq331);
RiP 36:344588e69589 186
RiP 36:344588e69589 187 //Attach the 'sample' function to the timer 'sample_timer'.
RiP 36:344588e69589 188 //this ensures that 'sample' is executed every 0.002 seconds = 500 Hz
RiP 23:54d28f9eef53 189 sample_timer.attach(&sampleflag, samplefreq);
RiP 36:344588e69589 190 //Attach the function calibrate to the button interrupt
RiP 36:344588e69589 191 button_calibrate.fall(&calibrate);
RiP 36:344588e69589 192 //Attach the function my_pos to the timer pos_timer.
RiP 36:344588e69589 193 //This ensures that the position is printed every second.
RiP 31:9c3b022f1dc3 194 pos_timer.attach(&my_pos, 1);
tomlankhorst 15:0da764eea774 195
RiP 22:f38a15e851d2 196 while(1) {
RiP 39:955929e25858 197 //Only take a sample when the go flag is true.
RiP 22:f38a15e851d2 198 if (sampletimer==true) {
RiP 39:955929e25858 199 sample();
RiP 39:955929e25858 200 sampletimer = false; //change sampletimer to false if sample() is finished
RiP 25:1a71424b05ff 201 }
RiP 22:f38a15e851d2 202 }
vsluiter 0:32bb76391d89 203 }