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) 
     {