Revision 1:1a434ea61a65, committed 2016-07-19

Comitter:

tsippel

Date:

Tue Jul 19 05:10:51 2016 +0000

Parent:

0:6e12a3e5af19

Commit message:

Initial robot arm control working with PID

Changed in this revision

diff -r 6e12a3e5af19 -r 1a434ea61a65 PID.cpp
--- a/PID.cpp	Thu Sep 02 16:48:10 2010 +0000
+++ b/PID.cpp	Tue Jul 19 05:10:51 2016 +0000
@@ -224,6 +224,8 @@
 
     //Pull in the input and setpoint, and scale them into percent span.
     float scaledPV = (processVariable_ - inMin_) / inSpan_;
+    
+    //printf ("{PV: %.2f ", scaledPV);
 
     if (scaledPV > 1.0) {
         scaledPV = 1.0;
@@ -232,6 +234,8 @@
     }
 
     float scaledSP = (setPoint_ - inMin_) / inSpan_;
+    //printf ("SP: %.2f }  ", scaledSP);
+
     if (scaledSP > 1.0) {
         scaledSP = 1;
     } else if (scaledSP < 0.0) {
@@ -250,21 +254,38 @@
     float dMeas = (scaledPV - prevProcessVariable_) / tSample_;
 
     float scaledBias = 0.0;
-
+    //printf ( "usingFeedForward: %d  ", usingFeedForward);
     if (usingFeedForward) {
         scaledBias = (bias_ - outMin_) / outSpan_;
     }
 
     //Perform the PID calculation.
-    controllerOutput_ = scaledBias + Kc_ * (error + (tauR_ * accError_) - (tauD_ * dMeas));
+    controllerOutput_ = scaledBias + Kc_ * (error + (tauR_ * accError_) + (tauD_ * dMeas)); // changed last "+" from "-"
+
+    // oldest
+    //printf ( "controllerOutput_: %.4f   scaledBias: %.4f    Kc_: %.4f   error: %.4f  tauR_: %.4f  accErr_: %.4f  tauD_: %.2f  dMeas: %.4f\n",
+    //    controllerOutput_, scaledBias, Kc_, error, tauR_, accError_, tauD_, dMeas);
+    
+    // recent
+    //printf ( "%6.3f = %.4f + %.4f * ( %6.3f + (%.2f * %5.2f) + (%.2f * %5.2f))  {PV: %.2f  SP: %.2f}",
+    //    controllerOutput_, scaledBias, Kc_, error, tauR_, accError_, tauD_, dMeas, scaledPV, scaledSP);
 
     //Make sure the computed output is within output constraints.
-    if (controllerOutput_ < 0.0) {
-        controllerOutput_ = 0.0;
+    /*
+    if (controllerOutput_ < outMin_) {
+        controllerOutput_ = outMin_;
+    } else if (controllerOutput_ > outMax_) {
+        controllerOutput_ = outMax_;
+    }    
+    */
+
+    if (controllerOutput_ < -1.0) {
+        controllerOutput_ = -1.0;
     } else if (controllerOutput_ > 1.0) {
         controllerOutput_ = 1.0;
     }
 
+
     //Remember this output for the windup check next time.
     prevControllerOutput_ = controllerOutput_;
     //Remember the input for the derivative calculation next time.