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@39:955929e25858, 2016-10-26 (annotated)

Committer:
RiP
Date:
Wed Oct 26 13:33:11 2016 +0000
Revision:
39:955929e25858
Parent:
38:0c8a6b0834da
Child:
40:1ca50c657cc5
working version but stripped of most of the print statements

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