Version for EMG Threshold finding
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of prog_BioRobotics_Group9_StampRobot by
Diff: main.cpp
- Revision:
- 35:a21c1cab8086
- Parent:
- 34:96bcbddc7d2d
diff -r 96bcbddc7d2d -r a21c1cab8086 main.cpp --- a/main.cpp Tue Nov 01 13:45:31 2016 +0000 +++ b/main.cpp Tue Nov 01 15:47:40 2016 +0000 @@ -57,7 +57,7 @@ const int cw = 0; //values for cw and ccw are inverted!! cw=0 and ccw=1 const int ccw = 1; -//set lengths (VALUES HAVE TO BE CHANGED) +//set lengths and constants (VALUES HAVE TO BE CHANGED) float x0 = 1.0; float L0 = 1.0; float L1 = 1.0; @@ -67,7 +67,6 @@ float y_stampup = 0.1; //height stamp while not stamping: 10cm above table surface float y_stampdown = 0.0; //height stamp while stamping: at table surface - //set initial conditions float biceps_l = 0; float biceps_r = 0; @@ -107,6 +106,10 @@ float MotorGain = 8.4; // rad/s for PWM, is max motor speed (motor value of 1) float t_sample = 0.1; //seconds const float maxStampDistance = 2.0; +float Kp = 3; //potMeter2.read(); +float Ki = 0.2*Kp; //0.01*Kp; //potMeter2.read(); +float Kd = 0.05; //0.1;(unstable!) //0.05; //0.02; //0.04*Kp; //potMeter2.read(); +float N = 100; //N=1/Tf, Higher N is faster derivative action but more sensitive to noise. float q1_refOutNew = 0; float q1_refOutMin = -100; //WRONG values @@ -122,6 +125,7 @@ BiQuadChain bqc; BiQuad bq1(0.0186, 0.0743, 0.1114, 0.0743, 0.0186); //get numbers from butter filter MATLAB BiQuad bq2(1.0000, -1.5704, 1.2756, -0.4844, 0.0762); +BiQuad pidf; //for lowpass filtering of PID controller //set go-Ticker settings volatile bool MeasureTicker_go=false, BiQuadTicker_go=false, FeedbackTicker_go=false, TimeTracker_go=false; // sampleT_go=false; @@ -211,7 +215,7 @@ //led1 = 0; //led2 = 0; ReferencePosition_xnew = ReferencePosition_x; - ReferencePosition_ynew = ReferencePosition_y; //y_stampup; + ReferencePosition_ynew = y_stampup; //ReferencePosition_y; } // } @@ -286,17 +290,17 @@ // linear feedback control float q1_error = q1_ref - q1; //referencePosition1 - Position1; // proportional angular error in radians float q2_error = q2_ref - q2; //referencePosition1 - Position1; // proportional angular error in radians - float Kp = 3; //potMeter2.read(); - + + float TotalError1 = pidf.step(q1_error); + float TotalError2 = pidf.step(q2_error); + float q1IntError = q1IntError + q1_error*t_sample; // integrated error in radians float q2IntError = q2IntError + q2_error*t_sample; // integrated error in radians //float maxKi = 0.2; - float Ki = 0.04*Kp; //0.01*Kp; //potMeter2.read(); float q1DerivativeError = (q1_error - q1_error_prev)/t_sample; // derivative of error in radians float q2DerivativeError = (q2_error_prev + q2_error)/t_sample; // derivative of error in radians //float maxKd = 0.2; - float Kd = 0.0; //0.04*Kp; //potMeter2.read(); //scope.set(0,referencePosition1); //scope.set(1,Position1); @@ -307,20 +311,6 @@ TotalError2 = q2_error * Kp + q2IntError * Ki + q2DerivativeError * Kd; //total controller output in radians = motor input /* - if (fabs(TotalError1) < TotalErrorMin) { - TotalError1=0; - } - else { - TotalError1=TotalError1; - } - if (fabs(TotalError2) < TotalErrorMin) { - TotalError2=0; - } - else { - TotalError2=TotalError2; - } - */ - /* DerTotalError1 = (TotalError1 - TotalError1_prev)/t_sample; DerTotalError1 = (TotalError1 - TotalError1_prev)/t_sample; motorValue1Out = DerTotalError1/MotorGain; @@ -441,8 +431,8 @@ pc.baud(115200); pc.printf("Test putty IK"); MeasureTicker.attach(&MeasureTicker_act, 0.1f); - bqc.add(&bq1).add(&bq2); - + bqc.add(&bq1).add(&bq2); //set BiQuad chain + pidf.PIDF( Kp, Ki, Kd, N, t_sample ); //set PID filter while(1) { if (MeasureTicker_go){