45 lines
856 B
Go
45 lines
856 B
Go
package controllers
|
|
|
|
import (
|
|
"math"
|
|
)
|
|
|
|
type PIDController struct {
|
|
kp float64
|
|
ki float64
|
|
kd float64
|
|
lastError float64
|
|
lastOutput float64
|
|
integration float64
|
|
lbound float64
|
|
ubound float64
|
|
}
|
|
|
|
func NewPIDController(p, i, d float64) *PIDController {
|
|
return &PIDController{
|
|
kp: p, // 2.0
|
|
ki: i, // 0.0001
|
|
kd: d, // 2.0
|
|
lbound: -15.0,
|
|
ubound: 15.0,
|
|
}
|
|
}
|
|
|
|
func (p *PIDController) Compute(input float64, setpoint float64) float64 {
|
|
error := setpoint - input
|
|
if p.lastError == 0 {
|
|
p.lastError = error
|
|
}
|
|
|
|
pv := p.kp * error
|
|
dv := -(p.kd * (error - p.lastError))
|
|
p.lastError = error
|
|
if p.lastOutput > p.lbound && p.lastOutput < p.ubound {
|
|
p.integration += error
|
|
}
|
|
|
|
iv := p.integration * p.ki
|
|
|
|
p.lastOutput = math.Max(math.Min(pv+iv+dv, p.ubound), p.lbound)
|
|
return p.lastOutput
|
|
}
|