127 lines
3.0 KiB
Go
127 lines
3.0 KiB
Go
package pid
|
|
|
|
import (
|
|
"time"
|
|
)
|
|
|
|
type controller struct {
|
|
Kp float64
|
|
Ki float64
|
|
Kd float64
|
|
lastTime time.Time
|
|
SetPoint float64
|
|
SampleTime time.Duration
|
|
WindupGuard float64
|
|
Output float64
|
|
|
|
pTerm float64
|
|
iTerm float64
|
|
dTerm float64
|
|
lastError float64
|
|
}
|
|
|
|
/*
|
|
PID Controller
|
|
*/
|
|
type Controller interface {
|
|
GetKp() float64
|
|
GetKi() float64
|
|
GetKd() float64
|
|
GetSetPoint() float64
|
|
GetSampleTime() time.Duration
|
|
GetWindupGuard() float64
|
|
GetOutput() float64
|
|
|
|
SetKp(float64)
|
|
SetKi(float64)
|
|
SetKd(float64)
|
|
SetSetPoint(float64)
|
|
SetSampleTime(time.Duration)
|
|
SetWindupGuard(float64)
|
|
SetOutput(float64)
|
|
|
|
Clear()
|
|
Update(float64) float64
|
|
}
|
|
|
|
/*
|
|
create PID Controller
|
|
*/
|
|
func New(proportionalGain float64, integralGain float64, derivativeGain float64) Controller {
|
|
return &controller{
|
|
Kp: proportionalGain,
|
|
Ki: integralGain,
|
|
Kd: derivativeGain,
|
|
WindupGuard: 20.0,
|
|
lastTime: time.Now(),
|
|
}
|
|
}
|
|
|
|
/*
|
|
Clears PID computations and coefficients
|
|
*/
|
|
func (c *controller) Clear() {
|
|
c.SetPoint = 0.0
|
|
c.pTerm = 0.0
|
|
c.iTerm = 0.0
|
|
c.dTerm = 0.0
|
|
c.lastError = 0.0
|
|
c.WindupGuard = 20.0
|
|
c.Output = 0.0
|
|
}
|
|
|
|
/*
|
|
Calculates PID value for given reference feedback
|
|
.. math::
|
|
u(t) = K_p e(t) + K_i \int_{0}^{t} e(t)dt + K_d {de}/{dt}
|
|
|
|
.. figure:: images/pid_1.png
|
|
:align: center
|
|
|
|
Test PID with Kp=1.2, Ki=1, Kd=0.001 (test_pid.py)
|
|
*/
|
|
func (c *controller) Update(feedbackValue float64) float64 {
|
|
var (
|
|
errorValue = c.SetPoint - feedbackValue
|
|
currentTime = time.Now()
|
|
deltaTime, deltaError = currentTime.Sub(c.lastTime), errorValue - c.lastError
|
|
)
|
|
|
|
if deltaTime >= c.SampleTime {
|
|
c.pTerm = c.Kp * errorValue
|
|
c.iTerm += errorValue * deltaTime.Seconds()
|
|
|
|
if c.iTerm < -c.WindupGuard {
|
|
c.iTerm = -c.WindupGuard
|
|
} else if c.iTerm > c.WindupGuard {
|
|
c.iTerm = c.WindupGuard
|
|
}
|
|
|
|
c.dTerm = 0.0
|
|
if deltaTime.Seconds() > 0.0 {
|
|
c.dTerm = deltaError / deltaTime.Seconds()
|
|
}
|
|
c.lastTime = currentTime
|
|
c.lastError = errorValue
|
|
|
|
c.Output = c.pTerm + (c.Ki * c.iTerm) + (c.Kd * c.dTerm)
|
|
}
|
|
return c.Output
|
|
}
|
|
|
|
func (c *controller) GetKp() float64 { return c.Kp }
|
|
func (c *controller) GetKi() float64 { return c.Ki }
|
|
func (c *controller) GetKd() float64 { return c.Kd }
|
|
func (c *controller) GetSetPoint() float64 { return c.SetPoint }
|
|
func (c *controller) GetSampleTime() time.Duration { return c.SampleTime }
|
|
func (c *controller) GetWindupGuard() float64 { return c.WindupGuard }
|
|
func (c *controller) GetOutput() float64 { return c.Output }
|
|
|
|
func (c *controller) SetKp(v float64) { c.Kp = v }
|
|
func (c *controller) SetKi(v float64) { c.Ki = v }
|
|
func (c *controller) SetKd(v float64) { c.Kd = v }
|
|
func (c *controller) SetSetPoint(v float64) { c.SetPoint = v }
|
|
func (c *controller) SetSampleTime(v time.Duration) { c.SampleTime = v }
|
|
func (c *controller) SetWindupGuard(v float64) { c.WindupGuard = v }
|
|
func (c *controller) SetOutput(v float64) { c.Output = v }
|