fermentord/internal/controllers/pid.go
Søren Rasmussen 87f684db9a
Some checks reported errors
continuous-integration/drone/push Build encountered an error
Rename package from sng to snr
2024-06-15 16:31:02 +02:00

55 lines
1.2 KiB
Go

package controllers
import (
"math"
"git.joco.dk/snr/fermentord/internal/metrics"
)
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)
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)
return p.lastOutput
}