alles ingevoegd
Dependencies: FastPWM HIDScope MODSERIAL QEI mbed biquadFilter
Fork of deelPID by
Diff: main.cpp
- Revision:
- 7:de221f894d3b
- Parent:
- 6:405ec2bba6d0
- Child:
- 8:fa268d163bbd
--- a/main.cpp Thu Nov 01 18:50:26 2018 +0000 +++ b/main.cpp Fri Nov 02 08:15:25 2018 +0000 @@ -21,16 +21,17 @@ DigitalOut Motor2Direction(D4); //EMG +/* AnalogIn a0(A0); AnalogIn a1(A1); AnalogIn a2(A2); AnalogIn a3(A3); - +*/ -/*AnalogIn Ppot(A0); +AnalogIn Ppot(A0); AnalogIn Ipot(A1); AnalogIn Dpot(A2); -*/ + MODSERIAL pc(USBTX, USBRX); @@ -65,7 +66,9 @@ BiQuad LP3(1,2,1,1.911197067426073,0.914975834801434); BiQuad LP4(1,2,1,1.911197067426073,0.914975834801434); - +// lowpass derivative +BiQuad LPD1(1,2,1,-1.734725768809275,0.766006600943264); +BiQuad LPD2(1,2,1,-1.734725768809275,0.766006600943264); ///Variables @@ -123,7 +126,7 @@ //States enum states {WaitModusState, EMGCalibrationState, NormalOperatingModusState, DemoModusState}; -states State = WaitModusState; +states State = DemoModusState; //Calibration Time @@ -135,12 +138,12 @@ void ReadEMG() { - +/* EMGdata1 = a0.read(); // store values in the scope EMGdata2 = a1.read(); EMGdata3 = a2.read(); EMGdata4 = a3.read(); - +*/ } // EMG High pass filters @@ -356,16 +359,20 @@ //D-Control //First smoothen the error signal + double MovAvq1_1 = 0.0; double MovAvq1_2 = 0.0; + for (int i=0; i<=n-2; i++){ // Creates two mov. av. with one element shifted MovAvq1_1 = MovAvq1_1 + PrevErrorq1[i]/((double) (n-1)); MovAvq1_2 = MovAvq1_2 + PrevErrorq1[i+1]/((double)(n-1)); } - DerivativeErrorq1 = (MovAvq1_2 - MovAvq1_1)/TickerPeriod; + DerivativeErrorq1 = (MovAvq1_2 - MovAvq1_1)/TickerPeriod; double D_error1 = GainD1 * DerivativeErrorq1; + // Derivative error sum over all time? - + + PIDerrorq1 = P_error1 + I_error1 + D_error1; // PID control motor 2 @@ -378,6 +385,7 @@ //D-Control //First smoothen the error signal + double MovAvq2_1 = 0.0; double MovAvq2_2 = 0.0; for (int i=0; i<=n-2; i++){ // Creates two mov. av. with one element shifted @@ -386,6 +394,8 @@ } DerivativeErrorq2 = (MovAvq2_2 - MovAvq2_1)/TickerPeriod; double D_error2 = GainD2 * DerivativeErrorq2; + + // Derivative error sum over all time? PIDerrorq2 = P_error2 + I_error2 + D_error2; @@ -484,19 +494,10 @@ } } void TestPath() -{ - /* GainP1 = Ppot.read()*60.0; - GainI1 = Ipot.read()*20.0; - GainD1 = Dpot.read(); - GainP2 = Ppot.read()*30.0; - GainI2 = Ipot.read()*20.0; - GainD2 = Dpot.read(); - - pc.printf("GainP = %1f, GainI = %1f, GainD = %1f", GainP1, GainI1, GainD1); - // Also think about how to move from whatever position to (40,5) - if (DemoStage == 0) //From (40,40) to (40,-5) +{ + if (DemoStage == 0) //From (40,40) to (40,-5) { - if (yRef >0) + if (yRef > 20.0) { yRef = yRef - vyMax * TickerPeriod; } @@ -505,22 +506,25 @@ DemoStage = 1; } } + else if (DemoStage == 1) { - if (yRef < 30) //From (65,-5) to (65, 10) + if (yRef < 35.0) //From (65,-5) to (65, 10) { - yRef = yRef + vyMax * TickerPeriod; + yRef = yRef + TickerPeriod; } else { DemoStage = 0; } } - */ + } + + void CalibrationButton() { ledred = 1; @@ -581,12 +585,12 @@ void DemoModus() // The ticker should call this function (in the switch statement) { - //GainP1 = pot3.read() * 30; - //GainI1 = pot2.read() * 10; - //GainD1 = pot1.read() ; - //GainP2 = pot3.read() * 10; - //GainI2 = pot2.read() * 10; - //GainD2 = pot1.read() ; + GainP1 = 40.0;//Ppot.read()*100.0; + GainI1 = 17.0;//Ipot.read()*20.0; + GainD1 = 2.0;//Dpot.read()*20.0; + GainP2 = Ppot.read()*100.0; + GainI2 = Ipot.read()*20.0; + GainD2 = Dpot.read()*20.0; //DemonstrationPath(); TestPath(); @@ -604,7 +608,7 @@ count++; if (count ==400) { - pc.printf("GainP = %lf, GainI = %lf, GainD = %lf, q1Pos = %lf, q2Pos = %lf, q1Ref = %lf, q2Ref = %lf, xRef = %lf, yRef = %lf \n\r", GainP1, GainI1, GainD1, q1Pos, q2Pos, q1Ref, q2Ref, xRef, yRef); + pc.printf("GainP = %lf, GainI = %lf, GainD = %lf, q1Pos = %lf, q2Pos = %lf, q1Ref = %lf, q2Ref = %lf, xRef = %lf, yRef = %lf \n\r", GainP2, GainI2, GainD2, q1Pos, q2Pos, q1Ref, q2Ref, xRef, yRef); count = 0; } } @@ -674,7 +678,7 @@ break; case DemoModusState: DemoModus(); - pc.printf("Demo \n\r"); + //pc.printf("Demo \n\r"); break; default : } @@ -705,14 +709,14 @@ bqc4.add( &HP_emg4 ).add( &Notch_emg4 );//.add( &LP4); //Initialize array errors to 0 - for (int i = 0; i <= 9; i++){ - PrevErrorq2[i] = 0; + for (int i = 0; i <= 99; i++){ + PrevErrorq1[i] = 0; PrevErrorq2[i] = 0; } - int frequency_pwm = 16700; //16.7 kHz PWM - Motor1PWM.period(1.0/frequency_pwm); // T = 1/f - Motor2PWM.period(1.0/frequency_pwm); // T = 1/f + double frequency_pwm = 16700.0; //16.7 kHz PWM + Motor1PWM.period_us(1.0/frequency_pwm); // T = 1/f + Motor2PWM.period_us(1.0/frequency_pwm); // T = 1/f //Emg Calibratie button //ButtonCal.fall(&CalibrationButton);