CSharp/C#移植Arduino的PID库源代码

using System;
using System.Collections;

namespace ArduinoPid
{
	public class PID_AutoTune
	{
//		  #define AUTOMATIC	1
//		  #define MANUAL	0
		
//		  #define DIRECT  0
//		  #define REVERSE  1
		
//		  #define P_ON_M 0
//		  #define P_ON_E 1
		
		/// <summary>
		/// 工作模式
		/// </summary>
		public enum MODE
		{
			/// <summary>
			/// PID控制开启
			/// </summary>
			 AUTOMATIC = 1,
			 /// <summary>
			 /// PID控制关闭
			 /// </summary>
			 MANUAL = 0
		}
		
		/// <summary>
		/// // 这里定义两个变量分别指代控制量与被控量方向：DIRECT 对应两者同向； REVERSE 对应两者反向
			// 其中同向指： 如果控制量增大，那么被控量也会增大；反之亦然。
			// 其中反向指： 如果控制量增大，那么被控量缺减小；反之亦然。
		/// </summary>
		public enum DIRECTION
		{
			 DIRECT = 0,
			 REVERSE = 1
		}
		
		public enum PON
		{
			 P_ON_M = 0,
			 P_ON_E = 1
		}
		
		System.Diagnostics.Stopwatch watch = new System.Diagnostics.Stopwatch();
		
		double dispKp;				// * we'll hold on to the tuning parameters in user-entered
		double dispKi;				//   format for display purposes
		double dispKd;				//
		double dispError;
	    
		double kp;                  // * (P)roportional Tuning Parameter
	    double ki;                  // * (I)ntegral Tuning Parameter
	    double kd;                  // * (D)erivative Tuning Parameter
	
		DIRECTION controllerDirection;
		PON pOn;
	
//	    double myInput;              // * Pointers to the Input, Output, and Setpoint variables
//	    double myOutput;             //   This creates a hard link between the variables and the 
//	    double mySetpoint;           //   PID, freeing the user from having to constantly tell us
	                                  //   what these values are.  with pointers we'll just know.
		double lastTime;
		double outputSum, lastInput;
	
		double SampleTime;
		double outMin, outMax;
		bool inAuto, pOnE;
		
		public double Output {get;private set;}
		public double Input {get;set;}
		public double Setpoint {get;set;}
		
		public PID_AutoTune(double Kp, double Ki, double Kd, PON POn = PON.P_ON_E, DIRECTION ControllerDirection = DIRECTION.DIRECT)
		{
			watch.Start();
		    inAuto = false;
		
		    SetOutputLimits(0, 255);				//default output limit corresponds to
														//the arduino pwm limits
		
		    SampleTime = 100;							//default Controller Sample Time is 0.1 seconds
		
		    SetControllerDirection(ControllerDirection);
		    SetTunings(Kp, Ki, Kd, POn);
		
		    lastTime = watch.Elapsed.TotalMilliseconds - SampleTime;
		}
		
		/// <summary>
		/// 计算PID， 在每个计算周期都应当调用 ，计算频率和是否计算可以在setMode和SetSampleTime中指定
		/// </summary>
		/// <returns></returns>
		public bool Compute()
		{
			if(!inAuto) {return false;}
		   double now = watch.Elapsed.TotalMilliseconds;
		   double timeChange = now - lastTime;
		   if(timeChange>=SampleTime)
		   {
		      /*Compute all the working error variables*/
		      double input = Input;
		      double error = Setpoint - input;
		      double dInput = (input - lastInput);
		      dispError = error;
		      outputSum+= (ki * error);
		
		      /*Add Proportional on Measurement, if P_ON_M is specified*/
		      if(!pOnE) {outputSum-= kp * dInput;}
		
		      if(outputSum > outMax) {outputSum= outMax;}else if(outputSum < outMin) {outputSum= outMin;}
		
		      /*Add Proportional on Error, if P_ON_E is specified*/
			   double output;
			  if(pOnE) {output = kp * error;} else {output = 0;}
		
		      /*Compute Rest of PID Output*/
		      output += outputSum - kd * dInput;
		
		      if(output > outMax) {output = outMax;}else if(output < outMin) {output = outMin;}
			   Output = output;
		
		      /*Remember some variables for next time*/
		      lastInput = input;
		      lastTime = now;
			  return true;
		   }
		   return false;
		}
		
		/* SetTunings(...)*************************************************************
		 * This function allows the controller's dynamic performance to be adjusted.
		 * it's called automatically from the constructor, but tunings can also
		 * be adjusted on the fly during normal operation
		 ******************************************************************************/
		void SetTunings(double Kp, double Ki, double Kd, PON POn)
		{
			if (Kp<0 || Ki<0 || Kd<0) {return;}
		
		   pOn = POn;
		   pOnE = POn == PON.P_ON_E;
		
		   dispKp = Kp; dispKi = Ki; dispKd = Kd;
		
		   double SampleTimeInSec = SampleTime / 1000;
		   kp = Kp;
		   ki = Ki * SampleTimeInSec;
		   kd = Kd / SampleTimeInSec;
		
		  if(controllerDirection == DIRECTION.REVERSE)
		   {
		      kp = (0 - kp);
		      ki = (0 - ki);
		      kd = (0 - kd);
		   }
		}
		
		/* SetTunings(...)*************************************************************
		 * Set Tunings using the last-rembered POn setting
		 ******************************************************************************/
		/// <summary>
		/// 设定P、I、D参数，可以在运行的时间周期内，指定运行需要的参数
		/// </summary>
		/// <param name="Kp"></param>
		/// <param name="Ki"></param>
		/// <param name="Kd"></param>
		public void SetTunings(double Kp, double Ki, double Kd){
		    SetTunings(Kp, Ki, Kd, pOn); 
		}
		
		/* SetSampleTime(...) *********************************************************
		 * sets the period, in Milliseconds, at which the calculation is performed
		 ******************************************************************************/
		/// <summary>
		/// 指定输出的范围，其中0-255，表示可限制的输出范围
		/// </summary>
		/// <param name="NewSampleTime"></param>
		public void SetSampleTime(int NewSampleTime)
		{
		   if (NewSampleTime > 0)
		   {
		      double ratio  = NewSampleTime / SampleTime;
		      ki *= ratio;
		      kd /= ratio;
		      SampleTime = NewSampleTime;
		   }
		}
		
		/* SetOutputLimits(...)****************************************************
		 *     This function will be used far more often than SetInputLimits.  while
		 *  the input to the controller will generally be in the 0-1023 range (which is
		 *  the default already,)  the output will be a little different.  maybe they'll
		 *  be doing a time window and will need 0-8000 or something.  or maybe they'll
		 *  want to clamp it from 0-125.  who knows.  at any rate, that can all be done
		 *  here.
		 **************************************************************************/
		/// <summary>
		/// 采样周期，以毫秒作为设置单位，默认为100
		/// </summary>
		/// <param name="Min"></param>
		/// <param name="Max"></param>
		public void SetOutputLimits(double Min, double Max)
		{
			if(Min >= Max) {return;}
		   outMin = Min;
		   outMax = Max;
		
		   if(inAuto)
		   {
		   	if(Output > outMax) {Output = outMax;}else if(Output < outMin) {Output = outMin;}
		   	if(outputSum > outMax) {outputSum= outMax;}else if(outputSum < outMin) {outputSum= outMin;}
		   }
		}
		
		/* SetMode(...)****************************************************************
		 * Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
		 * when the transition from manual to auto occurs, the controller is
		 * automatically initialized
		 ******************************************************************************/
		/// <summary>
		/// 设置自动模式还是手动模式
		/// </summary>
		/// <param name="Mode"></param>
		public void SetMode(MODE Mode)
		{
		    bool newAuto = (Mode == MODE.AUTOMATIC);
		    if(newAuto && !inAuto)
		    {  /*we just went from manual to auto*/
		        Initialize();
		    }
		    inAuto = newAuto;
		}
		
		/* Initialize()****************************************************************
		 *	does all the things that need to happen to ensure a bumpless transfer
		 *  from manual to automatic mode.
		 ******************************************************************************/
		void Initialize()
		{
		   outputSum = Output;
		   lastInput = Input;
		   if(outputSum > outMax) {outputSum = outMax;}else if(outputSum < outMin) {outputSum = outMin;}
		}
		
		/* SetControllerDirection(...)*************************************************
		 * The PID will either be connected to a DIRECT acting process (+Output leads
		 * to +Input) or a REVERSE acting process(+Output leads to -Input.)  we need to
		 * know which one, because otherwise we may increase the output when we should
		 * be decreasing.  This is called from the constructor.
		 ******************************************************************************/
		/// <summary>
		/// 设定控制器的方向，限制输出的正反向，仅需要在开始的时候设置一次
		/// </summary>
		/// <param name="Direction"></param>
		public void SetControllerDirection(DIRECTION Direction)
		{
		   if(inAuto && Direction != controllerDirection)
		   {
			  kp = (0 - kp);
		      ki = (0 - ki);
		      kd = (0 - kd);
		   }
		   controllerDirection = Direction;
		}
		
		/* Status Funcions*************************************************************
		 * Just because you set the Kp=-1 doesn't mean it actually happened.  these
		 * functions query the internal state of the PID.  they're here for display
		 * purposes.  this are the functions the PID Front-end uses for example
		 ******************************************************************************/
		public double GetKp(){ return  dispKp;}
		public double GetKi(){ return  dispKi;}
		public double GetKd(){ return  dispKd;}
		public double GetError(){ return  dispError;}
		public MODE GetMode(){ return  inAuto ? MODE.AUTOMATIC : MODE.MANUAL;}
		public DIRECTION GetDirection(){ return controllerDirection;}
	}
}