Marco Rubio / Mbed 2 deprecated RTOS_Controller_v3

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.

Dependencies:   IMU MODSERIAL Servo mbed

Fork of RTOS_Controller_v2 by Andrew Olguin

Diff: PID.cpp

Revision:
4:b37fd183e46a
Parent:
3:5ffe7e9c0bb3
Child:
5:07bbe020eb65
diff -r 5ffe7e9c0bb3 -r b37fd183e46a PID.cpp
--- a/PID.cpp	Mon Jul 04 18:56:23 2016 +0000
+++ b/PID.cpp	Sat Jul 09 20:41:49 2016 +0000
@@ -28,7 +28,7 @@
     PID::SetControllerDirection(ControllerDirection);
     PID::SetTunings(Kp, Ki, Kd);
 
-    lastTime = t.read_us()-SampleTime;				
+    lastTime = t.read_us()-SampleTime;			
 }
  
  
@@ -38,6 +38,9 @@
  *   pid Output needs to be computed.  returns true when the output is computed,
  *   false when nothing has been done.
  **********************************************************************************/ 
+ 
+Serial pt(USBTX, USBRX); // tx, rx
+
 bool PID::Compute()
 {
    if(!inAuto) return false;
@@ -49,7 +52,10 @@
    {
       /*Compute all the working error variables*/
 	  double input = *myInput;
+	  
+      
       double error = *mySetpoint - input;
+      pt.printf("pid1: %f, %f, %f, %f\n\r", error,  error, *mySetpoint, input);
       ITerm+= (ki * error);
       if(ITerm > outMax) ITerm= outMax;
       else if(ITerm < outMin) ITerm= outMin;
@@ -58,10 +64,14 @@
       /*Compute PID Output*/
       double output = kp * error + ITerm- kd * dInput;
       
+      pt.printf("pid2: %f, %f, %f, %f, %f\n\r", error, output, *mySetpoint, input, ITerm);
+      
 	  if(output > outMax) output = outMax;
       else if(output < outMin) output = outMin;
 	  *myOutput = output;
 	  
+      pt.printf("pid3: %f, %f, %f, %f, %f\n\r", error, output, *mySetpoint, input, ki);
+	  
       /*Remember some variables for next time*/
       lastInput = input;
       lastTime = now;
@@ -93,6 +103,7 @@
       ki = (0 - ki);
       kd = (0 - kd);
    }
+   ITerm = 0.0;	
 }
   
 /* SetSampleTime(...) *********************************************************