*

Unity/Assets/simulation_framework/PID.cs

@@ -0,0 +1,59 @@
+using UnityEngine;
+using System.Collections;
+using System;
+
+[Serializable]
+public class PID {
+	public bool enabled = true;
+	public float kp = 1.0f;
+	public float ki = 1.0f;
+	public float kd = 1.0f;
+
+	public float outMin = -0.1f;
+	public float outMax = 0.1f;
+
+	private float _errorIntegral;
+	private float _lastError;
+	private float _setpoint;
+
+	public float LastError {get {return _lastError;}}
+
+	private float _p;
+	public float P {get {return _p;}}
+	private float _i;
+	public float I {get {return _i;}}
+	private float _d;
+	public float D {get {return _d;}}
+
+	public float Setpoint{
+		get{ return _setpoint; }
+		set{ _setpoint = value; }
+	}
+
+	public float Process(float dt, float pv){
+		if (!enabled) {
+			return 0;
+		}
+		float error = Setpoint - pv;
+		float derivative = (error - _lastError) / dt;
+
+		//derivative = Mathf.Clamp(derivative, outMin, outMax);
+
+		/*if (Mathf.Abs(integralChange) > 0.001f) {
+			integralChange = 0.001f * Mathf.Sign(integralChange);
+		}*/
+		_errorIntegral += error * dt;
+
+
+		//_errorIntegral = Mathf.Clamp(_errorIntegral, outMin, outMax);
+
+		_lastError = error;
+
+		_p = kp * error;
+		_i = ki * _errorIntegral;
+		_d = kd * derivative;
+
+		var output = _p + _i + _d;
+		return Mathf.Clamp(output, outMin, outMax);
+	}
+}