2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
Diff: main.cpp
- Revision:
- 32:76c4d7bb2022
- Parent:
- 31:7b8b8459bddc
- Child:
- 33:daa6ec305441
--- a/main.cpp Wed Oct 21 10:41:59 2015 +0000 +++ b/main.cpp Thu Oct 22 10:57:50 2015 +0000 @@ -100,6 +100,10 @@ double m2_error=0; double m2_e_int=0; double m2_e_prev=0; //Error, integrated error, previous error const double m2_kp=0.001; const double m2_ki=0.0125; const double m2_kd=0.1; //Proportional, integral and derivative gains. +//Calibration, checks if elbow/shoulder are done +bool done1 = false; +bool done2 = false; + //highpass filter 20 Hz const double high_b0 = 0.956543225556877; const double high_b1 = -1.91308645113754; @@ -158,6 +162,9 @@ double x_error; double y_error; double costheta1; double sintheta1; double costheta2; double sintheta2; +double dls1, dls2, dls3, dls4; +double q1_error, q2_error; +double lambda=0.1; /*-------------------------------------------------------------------------------------------------------------------- ---- Filters --------------------------------------------------------------------------------------------------------- @@ -198,6 +205,7 @@ void sample_filter(void); //Sampling and filtering void control(); //Control - reference -> error -> pid -> motor output +void dlscontrol(); void calibrate_emg(); //Instructions + measurement of MVC of each muscle void emg_mvc(); //Helper funcion for storing MVC value void calibrate_arm(void); //Calibration of the arm with limit switches @@ -221,6 +229,9 @@ void emgInstructions(); void titleBox(); +//Limit switches - power off motors if switches hit (rising edge interrupt) +void shoulder(); +void elbow(); /*-------------------------------------------------------------------------------------------------------------------- ---- MAIN LOOP ------------------------------------------------------------------------------------------------------- @@ -253,12 +264,12 @@ //maybe some stop commands when a button is pressed: detach from timers. //stop_control(); start_sampling(); - wait(60); + //char c; - char command; + char command=0; while(command != 'Q' && command != 'q') { @@ -291,9 +302,9 @@ wait(1); emgInstructions(); calibrate_emg(); - pc.printf("\n\r ------------------------ \n\r"); + pc.printf("\n\r------------------------- \n\r"); pc.printf("\n\r EMG Calibration complete \n\r"); - pc.printf("\n\r ------------------------ \n\r"); + pc.printf("\n\r------------------------- \n\r"); caliMenu(); break; @@ -367,27 +378,23 @@ { case '1' : x = x + 0.01; - //controlMenu(); - //running=false; + break; case '2' : x-=0.01; - //controlMenu(); - //running=false; + break; case '3' : y+=0.01; - //controlMenu(); - //running=false; + break; case '4' : y-=0.01; - //controlMenu(); - //running=false; + break; case 'q' : @@ -420,10 +427,10 @@ //Sample and Filter void sample_filter(void) { - double emg_biceps = emg1.read(); //Biceps - double emg_triceps = emg2.read(); //Triceps - double emg_flexor = emg3.read(); //Flexor - double emg_extens = emg4.read(); //Extensor + emg_biceps = emg1.read(); //Biceps + emg_triceps = emg2.read(); //Triceps + 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? @@ -432,12 +439,17 @@ 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); biceps_power = abs(biceps_power); triceps_power = abs(triceps_power); flexor_power = abs(flexor_power); extens_power = abs(extens_power); biceps_power = lowpass.step(biceps_power); triceps_power = lowpass2.step(triceps_power); flexor_power = lowpass3.step(flexor_power); extens_power = lowpass4.step(extens_power); + biceps_power = biceps_power+0.1; triceps_power = triceps_power+0.1; flexor_power = flexor_power+1; extens_power = extens_power+1; - scope.set(0,emg_biceps); - scope.set(1,biceps_power); - scope.set(2,biceps_power); - scope.set(3,biceps_power); + scope.set(0,biceps_power); + scope.set(1,triceps_power); + scope.set(2,flexor_power); + scope.set(3,extens_power); scope.send(); + + // on - offset. If above a value it is on. + + /* alternative for lowpass filter: moving average window=30; //30 samples int i=0; //buffer index @@ -457,13 +469,30 @@ } +void shoulder(){ + pwm_motor1.write(0); + Encoder1.reset(); + done1 = true; + pc.printf("Shoulder Limit hit - shutting down motor 1\r\n"); +} + +void elbow(){ + pwm_motor2.write(0); + Encoder2.reset(); + done2 = true; + pc.printf("Elbow Limit hit - shutting down motor 2\r\n"); +} + //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position. void calibrate_arm(void) { pc.printf("Calibrate_arm() entered\r\n"); + red=0; blue=0; //Debug light is purple during arm calibration + + done1 = false; + done2 = false; bool calibrating = true; - bool done1 = false; - bool done2 = false; + pc.printf("To start arm calibration, press any key =>"); pc.getc(); pc.printf("\r\n Calibrating... \r\n"); @@ -473,34 +502,21 @@ while(calibrating){ - red=0; blue=0; //Debug light is purple during arm calibration - + shoulder_limit.rise(&shoulder); + elbow_limit.rise(&elbow); if(done1==true){ pwm_motor2.write(0.2); //move forearm slowly cw } - //when limit switches are hit, stop motor and reset encoder - if(shoulder_limit.read() < 0.5){ //polling - pwm_motor1.write(0); - Encoder1.reset(); - done1 = true; - pc.printf("Shoulder Limit hit - shutting down motor 1\r\n"); - } - if(elbow_limit.read() < 0.5){ //polling - pwm_motor2.write(0); - Encoder2.reset(); - done2 = true; - pc.printf("Elbow Limit hit - shutting down motor 2. \r\n"); - } if(done1 && done2){ calibrating=false; //Leave while loop when both limits are reached red=1; blue=1; //Turn debug light off when calibration complete } }//end while - + //TO DO: - //mechanical angle limits -> pulses. If 40 degrees -> counts = floor( 40 / (2*pi/4200) ) + //mechanical angle limits -> pulses. If 40 degrees -> counts = floor( 40 * (2*pi/4200) ) //Encoder1.setPulses(100); //edited QEI library: added setPulses() //Encoder2.setPulses(100); //edited QEI library: added setPulses() //pc.printf("Elbow Limit hit - shutting down motor 2. Current pulsecount: %i \r\n",Encoder1.getPulses()); @@ -664,6 +680,8 @@ biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin) biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin) + //threshold detection! buffer? + //analyze emg (= velocity) if (biceps>triceps && biceps > 0.1) xdir = 0; @@ -745,11 +763,17 @@ dtheta2 = atan2(sintheta2,costheta2); + double k1 = l1 + l2*costheta2; + double k2 = l2*sintheta2; + + dtheta1 = atan2(y, x) - atan2(k2, k1); + + /* alternative: costheta1 = ( x * (l1 + l2 * costheta2) + y * l2 * sintheta2 ) / ( pow(x,2) + pow(y,2) ); sintheta1 = sqrt( abs( 1 - pow(costheta1,2) ) ); dtheta1 = atan2(sintheta1,costheta1); - + */ //Angle error m1_error = dtheta1-theta1; @@ -793,6 +817,142 @@ } +void dlscontrol() +{ + /* + + //normalize emg to value between 0-1 + biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin) + biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin) + biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin) + biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin) + + //analyze emg (= velocity) + if (biceps>triceps && biceps > 0.1) + xdir = 0; + xpower = biceps; + else if (triceps>biceps && triceps>0.1) + xdir = 1; + xpower = triceps; + else + xpower=0; + + if (flexor>extensor && flexor > 0.1){ + ydir = 0; + ypower = flexor; + } + else if (extensor>flexor && extensor > 0.1){ + ydir = 1; + ypower = extensor; + } + else + ypower = 0; + + //We have power: the longer a signal is active, the further you go. So integrate to determine reference position + dx = xpower * CONTROL_RATE; + dy = ypower * CONTROL_RATE; + + //But: direction! Sum dx and dy but make sure xdir and ydir are considered. + if (xdir>0) + x += dx; + else + x += -dx; + + if (ydir>0) + y += dy; + else + y += -dy; + + //now we have x and y -> reference position. + + //Set limits to the error! + //lower limit: Negative error not allowed to go further. + if (theta1 < limitangle) + if (error1 > 0) + error1 = error1; + else + error1 = 0; + if (theta2 < limitangle) + same as above + + //upper limit: Positive error not allowed to go further + if (theta1 > limitangle) + if (error1 < 0 ) + error1 = error1; + else + error1 = 0; + if (theta2 > limitangle) + same as above + + + */ + + //Current position - Encoder counts -> current angle -> Forward Kinematics + rpc=(2*PI)/ENCODER1_CPR; //radians per count (resolution) - 2pi divided by 4200 + theta1 = Encoder1.getPulses() * rpc; //multiply resolution with number of counts + theta2 = Encoder2.getPulses() * rpc; + current_x = l1 * cos(theta1) + l2 * cos(theta1 + theta2); + current_y = l1 * sin(theta1) + l2 * sin(theta1 + theta2); + + + //calculate error (refpos-currentpos) currentpos = forward kinematics + x_error = x - current_x; + y_error = y - current_y; + + + //inverse kinematics (error in position to error in angles) + dls1= -(l2*pow(lambda,2)*sin(theta1 + theta2) + l1*pow(lambda,2)*sin(theta1) + l1*pow(l2,2)*pow(cos(theta1 + theta2),2)*sin(theta1) - l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*pow(cos(theta1),2) + pow(l1,2)*pow(lambda,2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(cos(theta1 + theta2),2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(sin(theta1 + theta2),2)*pow(cos(theta1),2) + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*pow(l1,2)*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1)); + /* + dls2= (l2*pow(lambda,2)*cos(theta1 + theta2) + l1*pow(lambda,2)*cos(theta1) + l1*pow(l2,2)*sin(theta1 + theta2)^2*cos(theta1) - l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*sin(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*l2^2*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*cos(theta1)^2 + pow(l1,2)*pow(lambda,2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(cos(theta1 + theta2),2)*sin(theta1)^2 + l1^2*l2^2*sin(theta1 + theta2)^2*cos(theta1)^2 + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*l1^2*l2^2*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1)); + dls3= -(l2*pow(lambda,2)*sin(theta1 + theta2) - l1*pow(l2,2)*cos(theta1 + theta2)^2*sin(theta1) + pow(l1,2)*l2*sin(theta1 + theta2)*pow(cos(theta1),2) - pow(l1,2)*l2*cos(theta1 + theta2)*cos(theta1)*sin(theta1) + l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*cos(theta1)^2 + l1^2*lambda^2*sin(theta1)^2 + l1^2*l2^2*cos(theta1 + theta2)^2*sin(theta1)^2 + l1^2*l2^2*sin(theta1 + theta2)^2*cos(theta1)^2 + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*l1^2*l2^2*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1)); + dls4= (l2*pow(lambda,2)*cos(theta1 + theta2) - l1*pow(l2,2)*sin(theta1 + theta2)^2*cos(theta1) + pow(l1,2)*l2*cos(theta1 + theta2)*pow(sin(theta1),2) - pow(l1,2)*l2*sin(theta1 + theta2)*cos(theta1)*sin(theta1) + l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*sin(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*cos(theta1)^2 + l1^2*lambda^2*sin(theta1)^2 + l1^2*l2^2*cos(theta1 + theta2)^2*sin(theta1)^2 + l1^2*l2^2*sin(theta1 + theta2)^2*cos(theta1)^2 + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*l1^2*l2^2*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1)); + */ + + q1_error = dls1 * x_error + dls2 * y_error; + q2_error = dls3 * x_error + dls4 * y_error; + + //Angle error + m1_error = q1_error; + m2_error = q2_error; + + + //PID controller + + u1=pid(m1_error,m1_kp,m1_ki,m1_kd,m1_e_int,m1_e_prev); //motor 1 + u2=pid(m2_error,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev); //motor 2 + + keep_in_range(&u1,-0.6,0.6); //keep u between -1 and 1, sign = direction, PWM = 0-1 + keep_in_range(&u2,-0.6,0.6); + + //send PWM and DIR to motor 1 + dir_motor1 = u1>0 ? 1 : 0; //conditional statement dir_motor1 = [condition] ? [if met 1] : [else 0]], same as if else below. + pwm_motor1.write(abs(u1)); + + //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) + { + dir_motor1 = 0; + else{ + dir_motor1 = 1; + } + } + pwm_motor1.write(abs(u1)); + + + if(u2 > 0) + { + dir_motor1 = 1; + else{ + dir_motor1 = 0; + } + } + pwm_motor1.write(abs(u2));*/ + +} + void mainMenu() { //Title Box