fermentord/internal/controllers/pid.go

56 lines
1.2 KiB
Go
Raw Normal View History

2021-08-30 20:46:38 +00:00
package controllers
import (
"math"
2022-02-18 20:59:32 +00:00
2024-06-15 14:31:02 +00:00
"git.joco.dk/snr/fermentord/internal/metrics"
2021-08-30 20:46:38 +00:00
)
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
2022-02-18 20:59:32 +00:00
2021-08-30 20:46:38 +00:00
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)
2022-02-18 20:59:32 +00:00
2022-03-05 19:42:17 +00:00
metrics.PID.WithLabelValues("pv", "main").Set(pv)
metrics.PID.WithLabelValues("dv", "main").Set(dv)
metrics.PID.WithLabelValues("iv", "main").Set(iv)
metrics.PID.WithLabelValues("offset", "main").Set(p.lastOutput)
metrics.PID.WithLabelValues("setpoint", "main").Set(setpoint)
2022-02-18 20:59:32 +00:00
2021-08-30 20:46:38 +00:00
return p.lastOutput
}