![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
the emg filtering part of the program
Dependencies: HIDScope biquadFilter mbed MODSERIAL
Fork of EMG by
Revision 37:af85a7b57a25, committed 2016-10-26
- Comitter:
- RiP
- Date:
- Wed Oct 26 12:20:24 2016 +0000
- Parent:
- 36:344588e69589
- Commit message:
- limietwaarden toegevoegd, werkt alleen nog niet perfect
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Oct 26 10:04:38 2016 +0000 +++ b/main.cpp Wed Oct 26 12:20:24 2016 +0000 @@ -3,7 +3,6 @@ #include "BiQuad.h" #include "MODSERIAL.h" -MODSERIAL pc(USBTX, USBRX); //Define objects //Define the button interrupt for the calibration @@ -19,6 +18,7 @@ Ticker sample_timer; HIDScope scope( 6 ); +MODSERIAL pc(USBTX, USBRX); //Initialize all variables volatile bool sampletimer = false; @@ -30,13 +30,20 @@ double emg02; double emg12; double emg22; -double ref_x=0.000; -double ref_y=0.000; +double ref_x=0.0000; +double ref_y=0.0000; +double old_ref_x; +double old_ref_y; double speed_emg=0.00002; double speed_key=0.000326; -double speed; +double speed=0.00002; +double theta=0.0; +double radius=0.0; -const int negative=-1; +const double minRadius=0.3; // minimum radius of arm +const double maxRadius=0.6; // maximum radius of arm +const double minAngle=-1.25; // minimum angle for limiting controller + char key; @@ -91,6 +98,9 @@ void sampleflag() { + if (sampletimer==true) { + pc.printf("rate too high error in setgoflag\n\r"); + } //This sets the go flag for when the function sample needs to be called sampletimer=true; } @@ -98,8 +108,8 @@ void calibrate() { //This resets the reference signals so that the robot can be calibrated - ref_x=0.0; - ref_y=0.0; + ref_x=0.0000; + ref_y=0.0000; } void sample(states &mystate) @@ -108,12 +118,11 @@ //This checks if a key is pressed and adjusts the speed to the needed speed if (pc.readable()==1) { + key=pc.getc(); - speed=speed_key; - } else { - key ='p'; - speed=speed_emg; - } + //printf("%c\n\r",key); + } + //Read the emg signals and filter it @@ -127,23 +136,26 @@ emg22=bqc33.step(fabs(bqc31.step(emg3.read()))); //filtered signal 2 scope.set(5, emg22); +emg02=1; //Ensure that enough channels are available at the top (HIDScope scope( 6 )) //Finally, send all channels to the PC at once scope.send(); + old_ref_x=ref_x; + old_ref_y=ref_y; //look if the emg signals go over the threshold and change the state to the cooresponding state. Also change the reference. if (emg02>threshold&&emg12>threshold&&emg22>threshold || key=='d') { mystate = STATE_XY_NEG; - ref_x=ref_x+speed*negative; - ref_y=ref_y+speed*negative; + ref_x=ref_x-speed; + ref_y=ref_y-speed; } else if (emg02>threshold&&emg12>threshold || key == 'a' ) { mystate = STATE_X_NEG; - ref_x=ref_x+speed*negative; + ref_x=ref_x-speed; } else if (emg02>threshold&&emg22>threshold || key == 's') { mystate = STATE_Y_NEG; - ref_y=ref_y+speed*negative; + ref_y=ref_y-speed; } else if (emg12>threshold&&emg22>threshold || key == 'e' ) { mystate = STATE_XY; @@ -158,7 +170,31 @@ mystate = STATE_Y; ref_y=ref_y+speed; } else { - //mystate = STATE_PAUZE; + mystate = STATE_PAUZE; + } + + // convert ref to gearbox angle + theta=atan((ref_y+sin(theta)*minRadius)/(ref_x+cos(theta)*minRadius)); + radius=sqrt(pow(ref_x+cos(theta)*minRadius,2)+pow(ref_y+sin(theta)*minRadius,2)); + if (theta != theta) { + theta=0; + } + if (theta <= minAngle) { + ref_x=old_ref_x; + ref_y=old_ref_y; + pc.printf("fout 1 "); + } else if (radius < minRadius) { + ref_x=old_ref_x; + ref_y=old_ref_y; + pc.printf("fout 2 "); + } /*else if (theta >= 0 ) { + ref_x=old_ref_x; + ref_y=old_ref_y; + pc.printf("fout 3 "); + }*/ else if ( radius > maxRadius) { + ref_x=old_ref_x; + ref_y=old_ref_y; + pc.printf("fout 4 "); } //change newcase so that the state will only be printed once @@ -173,7 +209,7 @@ void my_pos() { //This function is attached to a ticker so that the reference position is printed every second. - pc.printf("x_pos=%.4f\ty_pos=%.4f\n\r",ref_x,ref_y); + pc.printf("x_pos=%.4f\ty_pos=%.4f\tradius=%.4f\tangle=%.4f\n\r",ref_x,ref_y,radius,theta); }