working version but stripped of most unnecessary code like print statements
Dependencies: HIDScope MODSERIAL biquadFilter mbed FastPWM QEI
Diff: main.cpp
- Revision:
- 39:955929e25858
- Parent:
- 38:0c8a6b0834da
- Child:
- 40:1ca50c657cc5
diff -r 0c8a6b0834da -r 955929e25858 main.cpp --- a/main.cpp Wed Oct 26 13:00:12 2016 +0000 +++ b/main.cpp Wed Oct 26 13:33:11 2016 +0000 @@ -3,6 +3,7 @@ #include "BiQuad.h" #include "MODSERIAL.h" +MODSERIAL pc(USBTX, USBRX); //Define objects //Define the button interrupt for the calibration @@ -14,42 +15,30 @@ AnalogIn emg3( A2 ); //Define the Tickers -Ticker pos_timer; -Ticker sample_timer; - -HIDScope scope( 6 ); -MODSERIAL pc(USBTX, USBRX); +Ticker pos_timer; // the timer which is used to print the position every second +Ticker sample_timer; // the timer which is used to decide when a sample needs to be taken //Initialize all variables -volatile bool sampletimer = false; -volatile bool buttonflag = false; -volatile bool newcase = false; +volatile bool sampletimer = false; // a variable which is changed when a sample needs to be taken -double threshold = 0.04; -double samplefreq=0.002; -double emg02; -double emg12; -double emg22; -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=0.00002; -double theta=0.0; -double radius=0.0; +double threshold = 0.04; // the threshold which the emg signals need to surpass to do something +double samplefreq=0.002; // every 0.002 sec a sample will be taken this is a frequency of 500 Hz +double emg02; // the first emg signal +double emg12; // the second emg signal +double emg22; // the third emg signal +double ref_x=0.0000; // the x reference position +double ref_y=0.0000; // the y reference position +double old_ref_x; // the old x reference +double old_ref_y; // the old y reference +double speed=0.00002; // the variable with which a speed is reached of 1cm/s +double theta=0.0; // angle of the arm +double radius=0.0; // radius of the arm -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 +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; - - -// create a variable called 'mystate', define it -typedef enum { STATE_CALIBRATION, STATE_PAUZE, STATE_X, STATE_X_NEG, STATE_Y, STATE_Y_NEG, STATE_XY, STATE_XY_NEG } states; -states mystate = STATE_PAUZE; +char key; // variable to place the keyboard input //Define the needed Biquad chains BiQuadChain bqc11; @@ -77,11 +66,11 @@ BiQuad bq211 = bq111; BiQuad bq212 = bq112; BiQuad bq213 = bq113; -/* High pass filter*/ +//High pass filter BiQuad bq221 = bq121; BiQuad bq222 = bq122; BiQuad bq223 = bq123; -/* Low pass filter*/ +//Low pass filter BiQuad bq231 = bq131; //Define the BiQuads for the filter of the third emg signal @@ -99,7 +88,8 @@ void sampleflag() { if (sampletimer==true) { - pc.printf("rate too high error in setgoflag\n\r"); + // this if statement is used to see if the code takes too long before it is called again + pc.printf("rate too high error in sampleflag\n\r"); } //This sets the go flag for when the function sample needs to be called sampletimer=true; @@ -112,89 +102,60 @@ ref_y=0.0000; } -void sample(states &mystate) +void sample() { - states myoldstate=mystate; - - //This checks if a key is pressed and adjusts the speed to the needed speed + //This checks if a key is pressed and changes the variable key in the pressed key if (pc.readable()==1) { - key=pc.getc(); - //printf("%c\n\r",key); } - - //Read the emg signals and filter it - scope.set(0, emg1.read()); //original signal 0 emg02=bqc13.step(fabs(bqc11.step(emg1.read()))); //filtered signal 0 - scope.set(1, emg02); - scope.set(2, emg2.read()); //original signal 1 emg12=bqc23.step(fabs(bqc21.step(emg2.read()))); //filtered signal 1 - scope.set(3, emg12); - scope.set(4, emg3.read()); //original signal 2 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(); - + //remember what the reference was 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. + //look if the emg signals go over the threshold and change the reference accordingly if (emg02>threshold&&emg12>threshold&&emg22>threshold || key=='d') { - mystate = STATE_XY_NEG; 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; } else if (emg02>threshold&&emg22>threshold || key == 's') { - mystate = STATE_Y_NEG; ref_y=ref_y-speed; } else if (emg12>threshold&&emg22>threshold || key == 'e' ) { - mystate = STATE_XY; ref_x=ref_x+speed; ref_y=ref_y+speed; } else if (emg12>threshold || key == 'q' ) { - mystate = STATE_X; ref_x=ref_x+speed; - + } else if (emg22>threshold || key == 'w') { - mystate = STATE_Y; ref_y=ref_y+speed; - } else { - mystate = STATE_PAUZE; } - // convert ref to gearbox angle + // convert the x and y reference to the theta and radius reference theta=atan(ref_y/(ref_x+minRadius)); radius=sqrt(pow(ref_x+minRadius,2)+pow(ref_y,2)); + //look if the new reference is outside the possible range and revert back to the old reference if it is outside the range if (theta < minAngle) { ref_x=old_ref_x; ref_y=old_ref_y; + } else if (radius < minRadius) { ref_x=old_ref_x; ref_y=old_ref_y; + } else if ( radius > maxRadius) { ref_x=old_ref_x; ref_y=old_ref_y; } - - //change newcase so that the state will only be printed once - if (myoldstate==mystate) { - newcase=false; - - } else { - newcase=true; - } } void my_pos() @@ -204,46 +165,12 @@ } -void print_state() -{ - //This code looks in which state the robot is in and prints it to the screen - if (newcase==true) { - switch (mystate) { - case STATE_CALIBRATION : { // calibration - pc.printf("calibration\n\r"); - break; - } - case STATE_X : // X direction - pc.printf("X\n\r"); - break; - case STATE_X_NEG : // negative X direction - pc.printf("Xneg\n\r"); - break; - case STATE_Y : // Y direction - pc.printf("Y\n\r"); - break; - case STATE_Y_NEG : // negative Y direction - pc.printf("Yneg\n\r"); - break; - case STATE_XY : // X&Y direction - pc.printf("XY\n\r"); - break; - case STATE_XY_NEG : // negative X&Y direction - pc.printf("XYneg\n\r"); - break; - case STATE_PAUZE : // Pauze: do nothing - pc.printf("PAUZE\n\r"); - break; - } - } -} - int main() { pc.printf("RESET\n\r"); pc.baud(115200); - //Initialize the Biquad chains + //Attach the Biquads to the Biquad chains bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 ).add( &bq122 ).add( &bq123 ); bqc13.add( &bq131); bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 ).add( &bq222 ).add( &bq223 ); @@ -261,11 +188,10 @@ pos_timer.attach(&my_pos, 1); while(1) { - //Only take samples when the go flag is true. + //Only take a sample when the go flag is true. if (sampletimer==true) { - sample(mystate); - print_state(); - sampletimer = false; + sample(); + sampletimer = false; //change sampletimer to false if sample() is finished } } } \ No newline at end of file