func (d *Driver) toDemand(v float32) int8 {
switch {
case d.inputScale <= 0:
return Missing
case v < -10:
return Missing
}
v = mathex.Clipf(v, -1, 1)
return int8(v / d.inputScale)
}
func (pid *PID) Step(err, dt float32) float32 {
pid.Ti = mathex.Clipf(
pid.Ti+pid.Ki*err*dt,
-pid.TiLimit, pid.TiLimit)
pid.Td = (err - pid.errLast) / dt
pid.errLast = err
u := pid.Kp*err + pid.Ti + pid.Kd*pid.Td
minimum := pid.Deadband * 0.1
if u > minimum {
u += pid.Deadband
} else if u < -minimum {
u -= pid.Deadband
}
u = mathex.Clipf(u, pid.UMin, pid.UMax)
return u
}
func NewJogger(value *float32, dial, offset, scale float32) *Jogger {
j := &Jogger{value: value, original: *value}
dial = mathex.Clipf(dial, -1, 1)
v := *value + *value * (dial + offset) * scale
if v < 0 {
v = 0
}
*value = v
return j
}
