Takumi Nakagawara / PID

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Diff: PID.cpp

Revision:
3:c690a8974246
Parent:
2:c8ab3e8d4c51
--- a/PID.cpp	Sun Apr 14 14:03:46 2019 +0000
+++ b/PID.cpp	Mon Apr 15 05:23:54 2019 +0000
@@ -16,6 +16,8 @@
     setParameter_pid(_Kp,_Ki,_Kd);
     setPIDMode(_mode);
     Initialize();
+    derror[0]=0;
+    derror[1]=0;
 }
 
 void PID::control()
@@ -28,7 +30,7 @@
     {    
         double error = *Setpoint - input;
         double dInput = input - lastInput;
-        double derror = error - lastError;
+        derror[0] = error - lastError;
         outputSum += (Ki*error);
         double output = 0;
         
@@ -38,9 +40,12 @@
         switch(mode)
         {
             case pos_PID:
-                output = outputSum + Kp*error + Kd*derror;
+                output = outputSum + Kp*error + Kd*derror[0];
                 break; 
                 
+            case vec_PID:
+                output = *Output + Kp*derror[0] + Kd*error + Ki*(derror[0] - derror[1]);
+                
             case P_D:
                 output = Kp*error - Kd*dInput;
                 break;
@@ -54,7 +59,7 @@
                 break;
                 
         }
-        
+        derror[1] = derror[0];
         *Output = output;
         lastTime = nowtime;
         lastInput = input;