2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
Diff: main.cpp
- Revision:
- 51:e4a0ce7ff4b8
- Parent:
- 50:54f71544964c
- Child:
- 52:8a8c53dc8547
--- a/main.cpp Tue Oct 27 08:44:29 2015 +0000 +++ b/main.cpp Tue Oct 27 09:09:56 2015 +0000 @@ -167,6 +167,13 @@ const double low_a1 = -1.968902268531908; const double low_a2 = 0.9693784555043481; +//Derivative lowpass filter 60 Hz - remove error noise +const double derlow_b0 = 0.027859766117136; +const double derlow_b1 = 0.0557195322342721; +const double derlow_b2 = 0.027859766117136; +const double derlow_a1 = -1.47548044359265; +const double derlow_a2 = 0.58691950806119; + //Forward Kinematics const double l1 = 0.25; const double l2 = 0.25; //Arm lengths double current_x; double current_y; //Current position @@ -229,9 +236,9 @@ 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 ??? Hz) -biquadFilter derfilter1( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // derivative filter -biquadFilter derfilter2( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // derivative filter +//PID filter (lowpass 60 Hz) +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 /*-------------------------------------------------------------------------------------------------------------------- ---- DECLARE FUNCTION NAMES ------------------------------------------------------------------------------------------ @@ -448,7 +455,7 @@ wait(1); }//end if - /*if( pc.readable() ){ + if( pc.readable() ){ c = pc.getc(); switch (c) { @@ -488,7 +495,7 @@ } //end if pc readable - */ + } //end of while loop @@ -520,14 +527,14 @@ } //Limit switch - if hit: set pulsecount of encoder to angle of lower mechanical limit -void shoulder(){ +void shoulder() +{ pwm_motor1=0; 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) ) theta1_cal = floor(theta1_lower * (4200/(2*PI))); - Encoder1.setPulses(theta1_cal); //edited QEI library: added setPulses(int p) - + Encoder1.setPulses(theta1_cal); //edited QEI library: added setPulses(int p) } void elbow(){ @@ -537,8 +544,7 @@ //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(int p) - + Encoder2.setPulses(theta2_cal); //edited QEI library: added setPulses(int p) } //Run motors slowly clockwise to mechanical limit. Attached to 100Hz ticker @@ -552,9 +558,6 @@ pwm_motor2=0.3; //start moving forearm slowly cw pc.printf("Motor 2 running %f \r\n"); } - // if(done1==true && done2==true) //if both limits are hit - // pwm_motor2=0; //stop motor2 - // calibrating=false; //stop calibrating } //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position.