-
Notifications
You must be signed in to change notification settings - Fork 110
/
encoder_speed.go
69 lines (59 loc) · 1.74 KB
/
encoder_speed.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
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
package control
import (
"context"
"sync"
"time"
"github.com/edaniels/golog"
"github.com/pkg/errors"
)
type encoderToRPM struct {
mu sync.Mutex
cfg BlockConfig
y []*Signal
ticksPerRevolution int
prevEncCount int
logger golog.Logger
}
func newEncoderSpeed(config BlockConfig, logger golog.Logger) (Block, error) {
e := &encoderToRPM{cfg: config, logger: logger}
if err := e.reset(); err != nil {
return nil, err
}
return e, nil
}
func (b *encoderToRPM) Next(ctx context.Context, x []*Signal, dt time.Duration) ([]*Signal, bool) {
currEncCount := int(x[0].GetSignalValueAt(0))
b.y[0].SetSignalValueAt(0, (float64(currEncCount-b.prevEncCount)/float64(b.ticksPerRevolution))*60.0/(dt.Seconds()))
b.prevEncCount = currEncCount
return b.y, true
}
func (b *encoderToRPM) reset() error {
if !b.cfg.Attribute.Has("ticks_per_revolution") {
return errors.Errorf("encoderToRPM block %s doesn't have a ticks_per_revolution field", b.cfg.Name)
}
if len(b.cfg.DependsOn) != 1 {
return errors.Errorf("invalid number of inputs for encoderToRPM block %s expected 1 got %d", b.cfg.Name, len(b.cfg.DependsOn))
}
b.ticksPerRevolution = b.cfg.Attribute.Int("ticks_per_revolution", 0)
b.prevEncCount = 0
b.y = make([]*Signal, 1)
b.y[0] = makeSignal(b.cfg.Name)
return nil
}
func (b *encoderToRPM) Reset(ctx context.Context) error {
b.mu.Lock()
defer b.mu.Unlock()
return b.reset()
}
func (b *encoderToRPM) UpdateConfig(ctx context.Context, config BlockConfig) error {
b.mu.Lock()
defer b.mu.Unlock()
b.cfg = config
return b.reset()
}
func (b *encoderToRPM) Output(ctx context.Context) []*Signal {
return b.y
}
func (b *encoderToRPM) Config(ctx context.Context) BlockConfig {
return b.cfg
}