2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
Revision 41:55face19e06b, committed 2015-10-23
- Comitter:
- Vigilance88
- Date:
- Fri Oct 23 09:14:22 2015 +0000
- Parent:
- 40:d62f96ed44c0
- Child:
- 42:b9d26b1218b0
- Commit message:
- added mechanical limits
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Oct 23 07:35:52 2015 +0000 +++ b/main.cpp Fri Oct 23 09:14:22 2015 +0000 @@ -182,6 +182,11 @@ double x_error; double y_error; //Position errors double lambda=0.1; //Damping constant +//Mechanical Limits +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. + /*-------------------------------------------------------------------------------------------------------------------- ---- Filters --------------------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ @@ -357,6 +362,7 @@ pc.printf("=> You chose EMG DLS control \r\n\n"); wait(1); start_sampling(); + x=0; y=0; wait(1); emg_control=true; control_method=2; @@ -509,23 +515,24 @@ void shoulder(){ - pwm_motor1.write(0); - Encoder1.reset(); + pwm_motor1=0; + //Encoder1.reset(); done1 = true; pc.printf("Shoulder Limit hit - shutting down motor 1\r\n"); //mechanical angle limits -> pulses. If 40 degrees -> counts = floor( 40 * (4200/360) ) - //Encoder1.setPulses(467); //edited QEI library: added setPulses() + theta1_cal = floor(theta1_lower * (4200/(2*PI))); + Encoder1.setPulses(theta1_cal); //edited QEI library: added setPulses() } void elbow(){ - pwm_motor2.write(0); - Encoder2.reset(); + pwm_motor2=0; + //Encoder2.reset(); done2 = true; pc.printf("Elbow Limit hit - shutting down motor 2\r\n"); - //Mechanical limit 30 degrees -> 30*(4200/360) = 350 - - //Encoder2.setPulses(350); //edited QEI library: added setPulses() + //Mechanical limit 43 degrees -> 43*(4200/360) = 350 + theta2_cal = floor(theta2_lower * (4200/(2*PI))); + Encoder2.setPulses(theta2_cal); //edited QEI library: added setPulses() } //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position. @@ -805,6 +812,7 @@ //TODO some idle time with static reference before EMG kicks in //emg_control_time += CONTROL_RATE; // if emg_control_time < 5 seconds, reference is x=0; y=0; + //normalize emg to value between 0-1 biceps = (biceps_power - bicepsmin) / (bicepsMVC - bicepsmin); triceps = (triceps_power - tricepsmin) / (tricepsMVC - tricepsmin); @@ -918,29 +926,37 @@ m2_error = q2_error; }//end control method 2 - /* + /* //Set limits to the error! - //lower limit: Negative error not allowed to go further. - if (theta1 < limitangle) - if (error1 > 0) - error1 = error1; + 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. + if (theta1 < theta1_lower){ + if (m1_error > 0) + m1_error = m1_error; else - error1 = 0; - if (theta2 < limitangle) - same as above - + m1_error = 0; } + if (theta2 < theta2_lower){ + if (m2_error > 0) + m2_error = m2_error; + else + m2_error = 0; + } //upper limit: Positive error not allowed to go further - if (theta1 > limitangle) - if (error1 < 0 ) - error1 = error1; + if (theta1 > theta1_upper){ + if (m1_error < 0 ) + m1_error = m1_error; else - error1 = 0; - if (theta2 > limitangle) - same as above - + m1_error = 0; + } + if (theta2 > theta2_upper){ + if (m2_error < 0 ) + m2_error = m2_error; + else + m2_error = 0; + } - */ - //PID controller u1=pid(m1_error,m1_kp,m1_ki,m1_kd,m1_e_int,m1_e_prev); //motor 1