60 lines
1.2 KiB
C#
60 lines
1.2 KiB
C#
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);
|
|
}
|
|
}
|