P-controller geordend
Dependencies: Encoder HIDScope MODSERIAL mbed
Diff: main.cpp
- Revision:
- 2:b504e35af662
- Parent:
- 1:609671b1c96c
- Child:
- 3:f755b4d41aa8
--- a/main.cpp Fri Oct 13 08:41:14 2017 +0000 +++ b/main.cpp Fri Oct 13 09:46:44 2017 +0000 @@ -17,7 +17,9 @@ MODSERIAL pc(USBTX,USBRX); float PwmPeriod = 1.0/5000.0; //PWM periode instellen (5000 Hz, want 5000 periodes in 1 seconde) - +const float Ts = 0.1; // tickettijd/ sample time +float e_prev = 0; +float e_int = 0; float GetReferencePosition() { @@ -27,11 +29,22 @@ return refP; // bepaalde waarde tussen 0 en 4096 } -float FeedBackControl(float error) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking) +float FeedBackControl(float error, float &e_prev, float &e_int) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking) { - float kp = 50; // moet worden geschaald - float motorValue= kp*error; - // alleen nog maar P, D controllor moet nog worden gemaakt. + float kp = 2.5; // moet worden geschaald + float Proportional= kp*error; + + float kd = 3; // moet nog worden geschaald + float VelocityError = (error - e_prev)/Ts; + float Derivative = kd*VelocityError; + e_prev = error; + + float ki = 1; // moet nog worden geschaald + e_int = e_int+Ts*error; + float Integrator = ki*e_int; + + + float motorValue = Proportional + Integrator + Derivative; return motorValue; } @@ -65,18 +78,17 @@ void MeasureAndControl(void) { // hier wordt ons regelsysteem aangestuurd - // volatile float Huidigepostie = 0; float refP = GetReferencePosition(); float Huidigepositie = Encoder(); float error = (refP - Huidigepositie);// maak een error aan - float motorValue = FeedBackControl(error); + float motorValue = FeedBackControl(error, e_prev, e_int); SetMotor1(motorValue); } int main() { - Treecko.attach(MeasureAndControl, 0.1); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende + Treecko.attach(MeasureAndControl, Ts); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd. while(1) {