forked from foxis/EasyRobot
-
Notifications
You must be signed in to change notification settings - Fork 0
/
pid.go
47 lines (39 loc) · 1.01 KB
/
pid.go
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
package pid
import (
"github.com/itohio/EasyRobot/pkg/core/math/vec"
)
type PID struct {
P, I, D vec.Vector
min, max vec.Vector
Input, lastInput, Output, Target vec.Vector
iTerm vec.Vector
}
func New(p, i, d, min, max vec.Vector) PID {
N := len(p)
if N != len(i) || N != len(d) || N != len(min) || N != len(max) {
panic(-1)
}
return PID{
P: p,
I: i,
D: d,
min: min,
max: max,
Input: vec.New(N),
Output: vec.New(N),
Target: vec.New(N),
}
}
func (p *PID) Reset() *PID {
copy(p.lastInput, p.Input)
p.iTerm.FillC(0)
return p
}
func (p *PID) Update(samplePeriod float32) *PID {
E := p.Target.Clone().Sub(p.Input)
D := p.Input.Clone().Sub(p.lastInput)
p.iTerm.Add(p.I.Clone().Multiply(E).MulC(samplePeriod)).Clamp(p.min, p.max)
p.Output.CopyFrom(0, p.P.Clone().Multiply(E).Add(p.iTerm).Sub(p.D.Clone().Multiply(D).DivC(samplePeriod)).Clamp(p.min, p.max))
copy(p.lastInput, p.Input)
return p
}