-
Notifications
You must be signed in to change notification settings - Fork 110
/
ultrasonic.go
140 lines (125 loc) · 4.06 KB
/
ultrasonic.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
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
// Package ultrasonic implements an ultrasonic sensor based of the yahboom ultrasonic sensor
package ultrasonic
import (
"context"
"time"
"github.com/edaniels/golog"
"github.com/pkg/errors"
rdkutils "go.viam.com/utils"
"go.viam.com/rdk/components/board"
"go.viam.com/rdk/components/generic"
"go.viam.com/rdk/components/sensor"
"go.viam.com/rdk/config"
"go.viam.com/rdk/registry"
)
const (
modelname = "ultrasonic"
)
// AttrConfig is used for converting config attributes.
type AttrConfig struct {
TriggerPin string `json:"trigger_pin"`
EchoInterrupt string `json:"echo_interrupt_pin"`
Board string `json:"board"`
}
// Validate ensures all parts of the config are valid.
func (config *AttrConfig) Validate(path string) error {
if len(config.Board) == 0 {
return rdkutils.NewConfigValidationFieldRequiredError(path, "board")
}
if len(config.TriggerPin) == 0 {
return rdkutils.NewConfigValidationFieldRequiredError(path, "trigger pin")
}
if len(config.EchoInterrupt) == 0 {
return rdkutils.NewConfigValidationFieldRequiredError(path, "echo interrupt pin")
}
return nil
}
func init() {
registry.RegisterComponent(
sensor.Subtype,
modelname,
registry.Component{Constructor: func(
ctx context.Context,
deps registry.Dependencies,
config config.Component,
logger golog.Logger,
) (interface{}, error) {
return newSensor(ctx, deps, config.Name, config.ConvertedAttributes.(*AttrConfig))
}})
config.RegisterComponentAttributeMapConverter(sensor.SubtypeName, modelname,
func(attributes config.AttributeMap) (interface{}, error) {
var conf AttrConfig
return config.TransformAttributeMapToStruct(&conf, attributes)
}, &AttrConfig{})
}
func newSensor(ctx context.Context, deps registry.Dependencies, name string, config *AttrConfig) (sensor.Sensor, error) {
golog.Global().Debug("building ultrasonic sensor")
s := &Sensor{Name: name, config: config}
res, ok := deps[board.Named(config.Board)]
if !ok {
return nil, errors.Errorf("ultrasonic: board %q missing from dependencies", config.Board)
}
b, ok := res.(board.Board)
if !ok {
return nil, errors.Errorf("ultrasonic: cannot find board %q", config.Board)
}
i, ok := b.DigitalInterruptByName(config.EchoInterrupt)
if !ok {
return nil, errors.Errorf("ultrasonic: cannot grab digital interrupt %q", config.EchoInterrupt)
}
g, err := b.GPIOPinByName(config.TriggerPin)
if err != nil {
return nil, errors.Wrapf(err, "ultrasonic: cannot grab gpio %q", config.TriggerPin)
}
s.echoInterrupt = i
s.triggerPin = g
if err := s.triggerPin.Set(ctx, false, nil); err != nil {
return nil, errors.Wrap(err, "ultrasonic: cannot set trigger pin to low")
}
s.intChan = make(chan bool)
s.echoInterrupt.AddCallback(s.intChan)
return s, nil
}
// Sensor ultrasonic sensor.
type Sensor struct {
Name string
config *AttrConfig
echoInterrupt board.DigitalInterrupt
triggerPin board.GPIOPin
intChan chan bool
generic.Unimplemented
}
// Readings returns the calculated distance.
func (s *Sensor) Readings(ctx context.Context) (map[string]interface{}, error) {
if err := s.triggerPin.Set(ctx, true, nil); err != nil {
return nil, errors.Wrap(err, "ultrasonic cannot set trigger pin to high")
}
rdkutils.SelectContextOrWait(ctx, time.Microsecond*20)
if err := s.triggerPin.Set(ctx, false, nil); err != nil {
return nil, errors.Wrap(err, "ultrasonic cannot set trigger pin to low")
}
var timeA, timeB time.Time
select {
case <-s.intChan:
timeB = time.Now()
case <-ctx.Done():
return nil, errors.New("ultrasonic: context canceled")
case <-time.After(time.Second * 1):
return nil, errors.New("ultrasonic timeout")
}
select {
case <-s.intChan:
timeA = time.Now()
case <-ctx.Done():
return nil, errors.New("ultrasonic: context canceled")
case <-time.After(time.Second * 1):
return nil, errors.New("ultrasonic timeout")
}
dist := timeA.Sub(timeB).Seconds() * 340 / 2
return map[string]interface{}{"distance": dist}, nil
}
// Close remove interrupt callback of ultrasonic sensor.
func (s *Sensor) Close() error {
s.echoInterrupt.RemoveCallback(s.intChan)
return nil
}