2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
Revision 64:21fbff25d80b, committed 2015-11-03
- Comitter:
- Vigilance88
- Date:
- Tue Nov 03 15:11:42 2015 +0000
- Parent:
- 63:08357f5c497b
- Commit message:
- final - final version
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 08357f5c497b -r 21fbff25d80b main.cpp --- a/main.cpp Fri Oct 30 08:00:09 2015 +0000 +++ b/main.cpp Tue Nov 03 15:11:42 2015 +0000 @@ -31,20 +31,19 @@ DigitalOut blue(LED_BLUE); //EMG shields -AnalogIn emg1(A0); //Analog input - Biceps EMG -AnalogIn emg2(A1); //Analog input - Triceps EMG -AnalogIn emg3(A2); //Analog input - Flexor EMG -AnalogIn emg4(A3); //Analog input - Extensor EMG +AnalogIn emg1(A0); //Analog input - Right Flexor EMG +AnalogIn emg2(A1); //Analog input - Right Extensor EMG +AnalogIn emg3(A2); //Analog input - Left Flexor EMG +AnalogIn emg4(A3); //Analog input - Left Extensor EMG Ticker sample_timer; //Ticker for EMG sampling Ticker control_timer; //Ticker for control loop Ticker servo_timer; //Ticker for servo control Ticker debug_timer; //Ticker for debug printf + +//Turn hidscope off if not needed anymore //HIDScope scope(2); //Scope 4 channels -// AnalogIn potmeter(A4); //potmeters -// AnalogIn potmeter2(A5); // - //Encoders QEI Encoder1(D13,D12,NC,32); //channel A and B from encoder, counts = Encoder.getPulses(); QEI Encoder2(D10,D9,NC,32); //channel A and B from encoder, @@ -63,7 +62,7 @@ //Other buttons InterruptIn debugbtn(PTA4); //using FRDM buttons - debug on or off -DigitalIn button2(PTC6); //using FRDM buttons +DigitalIn button2(PTC6); //using FRDM buttons - not used /*Text colors ASCII code: Set Attribute Mode <ESC>[{attr1};...;{attrn}m @@ -87,7 +86,8 @@ /*-------------------------------------------------------------------------------------------------------------------- ---- DECLARE VARIABLES ----------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ -volatile bool debug = true; +//Debugging on or off +volatile bool debug = true; //default is on //EMG variables: raw EMG - filtered EMG - maximum voluntary contraction - max amplitude during relaxation. double emg_biceps; double biceps_power; double bicepsMVC = 0; double bicepsmin=0; @@ -102,8 +102,8 @@ double dx, dy; //Integral double emg_control_time; //Elapsed time in EMG control -//Threshold moving average -const int window=30; //100 samples +//Threshold moving average window +const int window=30; //30 samples int i=0; //movavg array index double movavg1[window]; //moving average arrays with size of window double movavg2[window]; @@ -122,7 +122,7 @@ double m2_error=0; double m2_e_int=0; double m2_e_prev=0; //Error, integrated error, previous error motor 2 const double m2_kp=1; const double m2_ki=0.01; const double m2_kd=0.05; //Proportional, integral and derivative gains. -//Calibration bools, checks if elbow/shoulder limits are hit +//Calibration bools, checks if elbow/shoulder limits are hit and if calibration is complete volatile bool done1 = false; volatile bool done2 = false; volatile bool calibrating = false; @@ -172,7 +172,7 @@ const double low_a1 = -1.968902268531908; const double low_a2 = 0.9693784555043481; -//Derivative lowpass filter 60 Hz - remove error noise +//Derivative lowpass filter 60 Hz - remove derivative error noise const double derlow_b0 = 0.027859766117136; const double derlow_b1 = 0.0557195322342721; const double derlow_b2 = 0.027859766117136; @@ -190,13 +190,13 @@ //Reference position double x; double y; -//Select whether to use Trig or DLS method, emg true or false +//Select whether to use Trig (1) or DLS method (2), emg control true or false int control_method; bool emg_control; //Inverse Kinematics - Trig / Gonio method. //First convert reference position to angle needed, then compare that angle to current angle. -double reftheta1; double reftheta2; //reference angles +double reftheta1; double reftheta2; //reference angles double costheta1; double sintheta1; //helper variables double costheta2; double sintheta2; // @@ -207,11 +207,11 @@ double q1_error, q2_error; //Angle errors double x_error, y_error; //Position errors double lambda=0.1; //Damping constant -double powlambda2, powlambda4; -double powl1, powl2; -double sintheta1theta2, costheta1theta2; +double powlambda2, powlambda4; //helper variables to reduce calculation time +double powl1, powl2; // +double sintheta1theta2, costheta1theta2; // -//Mechanical Limits +//Mechanical Limits (pulse counts and radians) int theta1_cal, theta2_cal; //Pulse counts at mechanical limits. double theta1_lower=0.698132, theta1_upper=2.35619; //motor1: lower limit 40 degrees, upper limit 135 double theta2_lower=0.750492, theta2_upper=2.40855; //motor2: lower limit 43 degrees, upper limit 138 degrees. @@ -222,6 +222,8 @@ //Using biquadFilter library //Syntax: biquadFilter filter(a1, a2, b0, b1, b2); coefficients. Call with: filter.step(u), with u signal to be filtered. +//Each biquadFilter object can only be used by one signal - memory variables are unique for each emg. +//This means 4 biquads for each muscle. //Biceps biquadFilter highpass( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts biquadFilter notch1( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference @@ -246,7 +248,7 @@ biquadFilter notch2_4( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); // biquadFilter lowpass4( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope -//PID filter (lowpass 60 Hz) +//PID filter (lowpass 60 Hz, 6*crossoverfreq) biquadFilter derfilter1( derlow_a1 , derlow_a2 , derlow_b0 , derlow_b1 , derlow_b2 ); // derivative filter biquadFilter derfilter2( derlow_a1 , derlow_a2 , derlow_b0 , derlow_b1 , derlow_b2 ); // derivative filter @@ -255,39 +257,40 @@ --------------------------------------------------------------------------------------------------------------------*/ void sample_filter(void); //Sampling and filtering -void control(); //Control - reference -> error -> pid -> motor output -void servo_control(); //Mouse alignment with servo +void control(); //Control loop - reference -> error -> pid -> motor output +void servo_control(); //Mouse alignment feed forward for servo void calibrate_emg(); //Instructions + measurement of Min and MVC of each muscle void emg_mvc(); //Helper funcion for storing MVC value void emg_min(); //Helper function for storing Min value void calibrate_arm(void); //Calibration of the arm with limit switches void start_sampling(void); //Attaches the sample_filter function to a 500Hz ticker void stop_sampling(void); //Stops sample_filter -void start_control(void); //Attaches the control function to a 100Hz ticker -void stop_control(void); //Stops control function +void start_control(void); //Attaches the control function to a 100Hz ticker and the servo_control to a 50Hz ticker +void stop_control(void); //Stops control and servo control //Keeps the input between min and max value void keep_in_range(double * in, double min, double max); -//Reusable PID controller, previous and integral error need to be passed by reference +//Reusable PID controller, previous and integral error need to be passed by reference. +//Need two because of different derivative filter biquads. double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev); double pid2(double error, double kp, double ki, double kd,double &e_int, double &e_prev); //Menu functions -void debugging(); -void debug_trigger(); -void mainMenu(); -void caliMenu(); -void controlMenu(); -void controlButtons(); -void clearTerminal(); -void emgInstructions(); -void titleBox(); +void debugging(); //Prints useful debug parameters if debugging is turned on. +void debug_trigger(); //Triggers debug on or off - attach to 1 hz ticker +void mainMenu(); //Prints the main menu +void caliMenu(); //Prints the calibration menu +void controlMenu(); //Prints the control menu with WASD button control +void controlButtons(); // +void clearTerminal(); //Clears the putty window +void emgInstructions(); //Optional - prints instructions for preparing the skin for EMG +void titleBox(); //Prints a fancy box. To view correctly putty translation-character set needs to be set to CP437. //Limit switches - power off motors if switches hit (rising edge interrupt) -void calibrate(void); -void shoulder(); -void elbow(); +void calibrate(void); //Rotates arm clockwise slowly untill switches are hit +void shoulder(); //Functions attached to buttons - when hit: encoder pulses are set to mechanical limit angles and motor turned off. +void elbow(); // /*-------------------------------------------------------------------------------------------------------------------- ---- MAIN LOOP ------------------------------------------------------------------------------------------------------- @@ -300,7 +303,7 @@ servoPwm.Enable(602,20000); //Start position servo, PWM period in usecs - //Set PwmOut frequency to x Hz + //Set PwmOut frequency to 50 Hz pwm_motor1.period(0.02); pwm_motor2.period(0.02); @@ -313,7 +316,7 @@ mainMenu(); char command=0; - + //Main menu: while(command != 'Q' && command != 'q') { if(pc.readable()){ @@ -321,23 +324,25 @@ switch(command){ + //User chooses 'c' case 'c': case 'C': { pc.printf("\n\r => You chose calibration.\r\n\n"); caliMenu(); char command2=0; - + //Calibration menu: while(command2 != 'B' && command2 != 'b'){ command2 = pc.getc(); switch(command2){ + //user chooses 'a' case 'a': case 'A': pc.printf("\n\r => Arm Calibration Starting... please wait \n\r"); calibrate_arm(); wait(1); caliMenu(); break; - + //user chooses 'e' case 'e': case 'E': pc.printf("\n\r => EMG Calibration Starting... please wait \n\r"); @@ -349,7 +354,7 @@ pc.printf("\n\r------------------------- \n\r"); caliMenu(); break; - + //user chooses 'b' case 'b': case 'B': pc.printf("\n\r => Going back to main menu.. \n\r"); @@ -360,6 +365,8 @@ }//end while break; }//end case c C + + //user chooses 't' case 't': case 'T': pc.printf("=> You chose TRIG button control \r\n\n"); wait(1); @@ -372,6 +379,7 @@ } controlButtons(); break; + //user chooses 'd' case 'd': case 'D': pc.printf("=> You chose DLS button control \r\n\n"); wait(1); @@ -384,6 +392,7 @@ } controlButtons(); break; + //user chooses 'e' case 'e': case 'E': pc.printf("=> You chose EMG DLS control \r\n\n"); wait(1); @@ -396,22 +405,12 @@ controlButtons(); break; - case 'R': - red=!red; - pc.printf("=> Red LED triggered \n\r"); - break; - case 'G': - green=!green; - pc.printf("=> Green LED triggered \n\r"); - break; - case 'B': - blue=!blue; - pc.printf("=> Blue LED triggered \n\r"); - break; + //user chooses 'q' case 'q': case 'Q': break; + //other inputs default: pc.printf("=> Invalid Input \n\r"); break; @@ -434,7 +433,6 @@ --------------------------------------------------------------------------------------------------------------------*/ //Debug function: prints important variables to check if things are calculating correctly - find errors - void debug_trigger(){ debug=!debug; printf("Debug triggered: %s \r\n", debug ? "ON" : "OFF"); @@ -443,7 +441,7 @@ void debugging() { if(debug==true){ - //Debugging values: + //Choose which debugging values to show: pc.printf("\r\nRef pos: %f and %f \r\n",x,y); pc.printf("Cur pos: %f and %f \r\n",current_x,current_y); //pc.printf("Pos Error: %f and %f \r\n",x_error,y_error); @@ -474,17 +472,15 @@ servopos = (2100/PI)*theta3 + 600; servoPwm.SetPosition(servopos); - //old: - //theta3 = 1.5*PI - deltatheta1 - deltatheta2; - //servopos = -(2100/PI)*theta3 + 2700; } //Use WASD keys to change reference position, x is a and d, y is w and s. void controlButtons() { controlMenu(); - debug_timer.attach(&debugging,1); + debug_timer.attach(&debugging,1); //debug printing at 1 hz char c=0; + //control menu while(c != 'Q' && c != 'q') { @@ -492,22 +488,26 @@ c = pc.getc(); switch (c) { + //user chooses 'd' case 'd' : x = x + 0.01; break; + //user chooses 'a' case 'a' : x-=0.01; break; - + + //user chooses 'w' case 'w' : y+=0.01; break; + //user chooses 's' case 's' : y-=0.01; @@ -516,6 +516,7 @@ case 'g' : debug=true; break; + //user chooses 'q' case 'q' : case 'Q' : pc.printf("=> Quitting control... \r\n"); wait(1); @@ -545,8 +546,8 @@ emg_flexor = emg3.read(); //Flexor emg_extens = emg4.read(); //Extensor - //Filter: high-pass -> notch -> rectify -> lowpass or moving average - // Can we use same biquadFilter (eg. highpass) for other muscles or does each muscle need its own biquad? + //Filter: high-pass -> notch -> rectify -> lowpass + //each muscle need its own biquad per filter biceps_power = highpass.step(emg_biceps); triceps_power = highpass2.step(emg_triceps); flexor_power = highpass3.step(emg_flexor); extens_power = highpass4.step(emg_extens); biceps_power = notch1.step(biceps_power); triceps_power = notch1_2.step(triceps_power); flexor_power = notch1_3.step(flexor_power); extens_power = notch1_4.step(extens_power); biceps_power = notch2.step(biceps_power); triceps_power = notch2_2.step(triceps_power); flexor_power = notch2_3.step(flexor_power); extens_power = notch2_4.step(extens_power); @@ -569,7 +570,7 @@ } -//Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position. +//Send arm to mechanical limits, and set encoder to these angles. void calibrate_arm(void) { pc.printf("Calibrate_arm() entered\r\n"); @@ -600,7 +601,7 @@ x = l1 * cos(theta1_lower) + l2 * cos(theta1_lower + theta2_lower); y = l1 * sin(theta1_lower) + l2 * sin(theta1_lower + theta2_lower); - //x = 0.2220; + //x = 0.2220; position at limits //y = 0.4088; } } @@ -786,7 +787,6 @@ if(muscle==1){ if(biceps_power>bicepsMVC){ - //pc.printf("+ "); pc.printf("%s+ %s",GREEN_,_GREEN); bicepsMVC=biceps_power; } @@ -829,6 +829,7 @@ } +//Minimum measurement during relaxation void emg_min() { if(biceps_power>bicepsmin){ @@ -866,7 +867,7 @@ } -//PID for motor 2 - needed because of biquadfilter memory variables? +//PID for motor 2 - needed because of biquadfilter memory variables double pid2(double error, double kp, double ki, double kd,double &e_int, double &e_prev) { // Derivative @@ -881,7 +882,8 @@ } -//Analyze filtered EMG, calculate reference position from EMG, compare reference position with current position,convert to angles, send error through pid(), send PWM and DIR to motors +//Analyze filtered EMG, calculate reference position from EMG, compare reference position with current position, +//convert to angles, send error through pid(), send PWM and DIR to motors void control() { @@ -928,7 +930,7 @@ emg_control_time += CONTROL_RATE; - //Move mouse to starting position - bottom right corner - when EMG control starts. After 5 seconds reference can be changed with EMG. + //Move mouse to starting position - bottom right corner of used workspace - when EMG control starts. After 5 seconds reference can be changed with EMG. if(emg_control_time < 5){ x=0; y=0.3; @@ -1010,7 +1012,7 @@ //Reference angle 1 reftheta1 = atan2(y, x) - atan2(k2, k1); - /* alternative: + /* alternative, but extra square root costheta1 = ( x * (l1 + l2 * costheta2) + y * l2 * sintheta2 ) / ( pow(x,2) + pow(y,2) ); sintheta1 = sqrt( abs( 1 - pow(costheta1,2) ) ); @@ -1027,17 +1029,20 @@ /******************************** START OF DLS INVERSE KINEMATICS ****************************************/ if(control_method==2){ //inverse kinematics (error in position to error in angles) - powlambda2 = pow(lambda,2); - powlambda4 = pow(lambda,4); - powl2 = pow(l2,2); - powl1 = pow(l1,2); - sintheta1theta2 = sin(theta1 + theta2); - costheta1theta2 = cos(theta1 + theta2); + powlambda2 = pow(lambda,2); //help functions to reduce amount of calculations + powlambda4 = pow(lambda,4); // + powl2 = pow(l2,2); // + powl1 = pow(l1,2); // + sintheta1theta2 = sin(theta1 + theta2); // + costheta1theta2 = cos(theta1 + theta2); // + + //calculate DLS matrix dls1= -(l2*powlambda2*sintheta1theta2 + l1*powlambda2*sin(theta1) + l1*powl2*pow(costheta1theta2,2)*sin(theta1) - l1*powl2*costheta1theta2*sintheta1theta2*cos(theta1))/(powlambda4 + 2*powl2*powlambda2*pow(costheta1theta2,2) + 2*powl2*powlambda2*pow(sintheta1theta2,2) + powl1*powlambda2*pow(cos(theta1),2) + powl1*powlambda2*pow(sin(theta1),2) + powl1*powl2*pow(costheta1theta2,2)*pow(sin(theta1),2) + powl1*powl2*pow(sintheta1theta2,2)*pow(cos(theta1),2) + 2*l1*l2*powlambda2*costheta1theta2*cos(theta1) + 2*l1*l2*powlambda2*sintheta1theta2*sin(theta1) - 2*powl1*powl2*costheta1theta2*sintheta1theta2*cos(theta1)*sin(theta1)); dls2= (l2*powlambda2*costheta1theta2 + l1*powlambda2*cos(theta1) + l1*powl2*pow(sintheta1theta2,2)*cos(theta1) - l1*powl2*costheta1theta2*sintheta1theta2*sin(theta1))/(powlambda4 + 2*powl2*powlambda2*pow(costheta1theta2,2) + 2*powl2*powlambda2*pow(sintheta1theta2,2) + powl1*powlambda2*pow(cos(theta1),2) + powl1*powlambda2*pow(sin(theta1),2) + powl1*powl2*pow(costheta1theta2,2)*pow(sin(theta1),2) + powl1*powl2*pow(sintheta1theta2,2)*pow(cos(theta1),2) + 2*l1*l2*powlambda2*costheta1theta2*cos(theta1) + 2*l1*l2*powlambda2*sintheta1theta2*sin(theta1) - 2*powl1*powl2*costheta1theta2*sintheta1theta2*cos(theta1)*sin(theta1)); dls3= -(l2*powlambda2*sintheta1theta2 - l1*powl2*pow(costheta1theta2,2)*sin(theta1) + powl1*l2*sintheta1theta2*pow(cos(theta1),2) - powl1*l2*costheta1theta2*cos(theta1)*sin(theta1) + l1*powl2*costheta1theta2*sintheta1theta2*cos(theta1))/(powlambda4 + 2*powl2*powlambda2*pow(costheta1theta2,2) + 2*powl2*powlambda2*pow(sintheta1theta2,2) + powl1*powlambda2*pow(cos(theta1),2) + powl1*powlambda2*pow(sin(theta1),2) + powl1*powl2*pow(costheta1theta2,2)*pow(sin(theta1),2) + powl1*powl2*pow(sintheta1theta2,2)*pow(cos(theta1),2) + 2*l1*l2*powlambda2*costheta1theta2*cos(theta1) + 2*l1*l2*powlambda2*sintheta1theta2*sin(theta1) - 2*powl1*powl2*costheta1theta2*sintheta1theta2*cos(theta1)*sin(theta1)); dls4= (l2*powlambda2*costheta1theta2 - l1*powl2*pow(sintheta1theta2,2)*cos(theta1) + powl1*l2*costheta1theta2*pow(sin(theta1),2) - powl1*l2*sintheta1theta2*cos(theta1)*sin(theta1) + l1*powl2*costheta1theta2*sintheta1theta2*sin(theta1))/(powlambda4 + 2*powl2*powlambda2*pow(costheta1theta2,2) + 2*powl2*powlambda2*pow(sintheta1theta2,2) + powl1*powlambda2*pow(cos(theta1),2) + powl1*powlambda2*pow(sin(theta1),2) + powl1*powl2*pow(costheta1theta2,2)*pow(sin(theta1),2) + powl1*powl2*pow(sintheta1theta2,2)*pow(cos(theta1),2) + 2*l1*l2*powlambda2*costheta1theta2*cos(theta1) + 2*l1*l2*powlambda2*sintheta1theta2*sin(theta1) - 2*powl1*powl2*costheta1theta2*sintheta1theta2*cos(theta1)*sin(theta1)); + //calculate angle errors from position error q1_error = dls1 * x_error + dls2 * y_error; q2_error = dls3 * x_error + dls4 * y_error; @@ -1052,7 +1057,7 @@ motor1: lower limit 40 degrees, upper limit 135 motor2: lower 43 degrees, upper limit 138 degrees. */ - //lower limits: Negative error not allowed to go further. NEEDS MORE TESTING + //lower limits: Negative error not allowed to go further. if (theta1 < theta1_lower){ if (m1_error > 0) m1_error = m1_error; @@ -1082,7 +1087,7 @@ u1=pid(m1_error,m1_kp,m1_ki,m1_kd,m1_e_int,m1_e_prev); //motor 1 u2=pid2(m2_error,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev); //motor 2 - //keep u between limits, sign = direction, PWM = 0-1 + //keep PWM between limits, sign = direction keep_in_range(&u1,-0.3,0.3); keep_in_range(&u2,-0.3,0.3); @@ -1093,8 +1098,7 @@ //send PWM and DIR to motor 2 dir_motor2 = u2>0 ? 0 : 1; //conditional statement, same as if else below pwm_motor2.write(abs(u2)); - - + /*if(u1 > 0) { @@ -1117,7 +1121,7 @@ } - +//Prints the main menu void mainMenu() { //Title Box @@ -1199,7 +1203,7 @@ pc.printf("[H"); // cursor to home } - +//Prints control menu void controlMenu() { //Title Box @@ -1229,6 +1233,7 @@ pc.printf("Please make a choice => \r\n"); } +//prints calibration menu void caliMenu(){ //Title Box @@ -1255,7 +1260,7 @@ pc.printf("Please make a choice => \r\n"); } - +//prints square title box void titleBox(){ //Title Box @@ -1279,6 +1284,7 @@ //endbox } +//prints emg instructions void emgInstructions(){ pc.printf("\r\nPrepare the skin before applying electrodes: \n\r"); pc.printf("-> Shave electrode locations if needed and clean with alcohol \n\r"); wait(1);