gpio(hcsr04): add driver for ultrasonic ranging module (#1012)
This commit is contained in:
parent
f7f482010b
commit
f219a4055d
|
@ -270,7 +270,7 @@ Support for many devices that use General Purpose Input/Output (GPIO) have
|
|||
a shared set of drivers provided using the `gobot/drivers/gpio` package:
|
||||
|
||||
- [GPIO](https://en.wikipedia.org/wiki/General_Purpose_Input/Output) <=> [Drivers](https://github.com/hybridgroup/gobot/tree/master/drivers/gpio)
|
||||
- AIP1640 LED
|
||||
- AIP1640 LED Dot Matrix/7 Segment Controller
|
||||
- Button
|
||||
- Buzzer
|
||||
- Direct Pin
|
||||
|
@ -281,8 +281,11 @@ a shared set of drivers provided using the `gobot/drivers/gpio` package:
|
|||
- Grove Magnetic Switch
|
||||
- Grove Relay
|
||||
- Grove Touch Sensor
|
||||
- HC-SR04 Ultrasonic Ranging Module
|
||||
- HD44780 LCD controller
|
||||
- LED
|
||||
- Makey Button
|
||||
- MAX7219 LED Dot Matrix
|
||||
- Motor
|
||||
- Proximity Infra Red (PIR) Motion Sensor
|
||||
- Relay
|
||||
|
|
|
@ -15,10 +15,9 @@ before_test:
|
|||
build_script:
|
||||
- go test -v -cpu=2 .
|
||||
- go test -v -cpu=2 ./drivers/aio/...
|
||||
- go test -v -cpu=2 ./drivers/i2c/...
|
||||
- go test -v -cpu=2 ./platforms/ble/...
|
||||
- go test -v -cpu=2 ./platforms/dji/...
|
||||
- go test -v -cpu=2 ./platforms/firmata/...
|
||||
- go test -v -cpu=2 ./platforms/ble/...
|
||||
- go test -v -cpu=2 ./platforms/joystick/...
|
||||
- go test -v -cpu=2 ./platforms/parrot/...
|
||||
- go test -v -cpu=2 ./platforms/sphero/...
|
||||
|
|
|
@ -12,17 +12,22 @@ Please refer to the main [README.md](https://github.com/hybridgroup/gobot/blob/r
|
|||
|
||||
Gobot has a extensible system for connecting to hardware devices. The following GPIO devices are currently supported:
|
||||
|
||||
- AIP1640 LED Dot Matrix/7 Segment Controller
|
||||
- Button
|
||||
- Buzzer
|
||||
- Direct Pin
|
||||
- EasyDriver
|
||||
- Grove Button
|
||||
- Grove Buzzer
|
||||
- Grove LED
|
||||
- Grove Magnetic Switch
|
||||
- Grove Relay
|
||||
- Grove Touch Sensor
|
||||
- HC-SR04 Ultrasonic Ranging Module
|
||||
- HD44780 LCD controller
|
||||
- LED
|
||||
- Makey Button
|
||||
- MAX7219 LED Dot Matrix
|
||||
- Motor
|
||||
- Proximity Infra Red (PIR) Motion Sensor
|
||||
- Relay
|
||||
|
@ -30,5 +35,3 @@ Gobot has a extensible system for connecting to hardware devices. The following
|
|||
- Servo
|
||||
- Stepper Motor
|
||||
- TM1638 LED Controller
|
||||
|
||||
More drivers are coming soon...
|
||||
|
|
|
@ -2,6 +2,9 @@ package gpio
|
|||
|
||||
import (
|
||||
"errors"
|
||||
"sync"
|
||||
|
||||
"gobot.io/x/gobot/v2"
|
||||
)
|
||||
|
||||
var (
|
||||
|
@ -61,3 +64,62 @@ type DigitalWriter interface {
|
|||
type DigitalReader interface {
|
||||
DigitalRead(string) (val int, err error)
|
||||
}
|
||||
|
||||
// Driver implements the interface gobot.Driver.
|
||||
type Driver struct {
|
||||
name string
|
||||
connection gobot.Adaptor
|
||||
afterStart func() error
|
||||
beforeHalt func() error
|
||||
gobot.Commander
|
||||
mutex *sync.Mutex // mutex often needed to ensure that write-read sequences are not interrupted
|
||||
}
|
||||
|
||||
// NewDriver creates a new generic and basic gpio gobot driver.
|
||||
func NewDriver(a gobot.Adaptor, name string) *Driver {
|
||||
d := &Driver{
|
||||
name: gobot.DefaultName(name),
|
||||
connection: a,
|
||||
afterStart: func() error { return nil },
|
||||
beforeHalt: func() error { return nil },
|
||||
Commander: gobot.NewCommander(),
|
||||
mutex: &sync.Mutex{},
|
||||
}
|
||||
|
||||
return d
|
||||
}
|
||||
|
||||
// Name returns the name of the gpio device.
|
||||
func (d *Driver) Name() string {
|
||||
return d.name
|
||||
}
|
||||
|
||||
// SetName sets the name of the gpio device.
|
||||
func (d *Driver) SetName(name string) {
|
||||
d.name = name
|
||||
}
|
||||
|
||||
// Connection returns the connection of the gpio device.
|
||||
func (d *Driver) Connection() gobot.Connection {
|
||||
return d.connection.(gobot.Connection)
|
||||
}
|
||||
|
||||
// Start initializes the gpio device.
|
||||
func (d *Driver) Start() error {
|
||||
d.mutex.Lock()
|
||||
defer d.mutex.Unlock()
|
||||
|
||||
// currently there is nothing to do here for the driver
|
||||
|
||||
return d.afterStart()
|
||||
}
|
||||
|
||||
// Halt halts the gpio device.
|
||||
func (d *Driver) Halt() error {
|
||||
d.mutex.Lock()
|
||||
defer d.mutex.Unlock()
|
||||
|
||||
// currently there is nothing to do after halt for the driver
|
||||
|
||||
return d.beforeHalt()
|
||||
}
|
|
@ -0,0 +1,78 @@
|
|||
package gpio
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"testing"
|
||||
|
||||
"github.com/stretchr/testify/assert"
|
||||
"gobot.io/x/gobot/v2"
|
||||
)
|
||||
|
||||
var _ gobot.Driver = (*Driver)(nil)
|
||||
|
||||
func initTestDriverWithStubbedAdaptor() (*Driver, *gpioTestAdaptor) {
|
||||
a := newGpioTestAdaptor()
|
||||
d := NewDriver(a, "GPIO_BASIC")
|
||||
return d, a
|
||||
}
|
||||
|
||||
func initTestDriver() *Driver {
|
||||
d, _ := initTestDriverWithStubbedAdaptor()
|
||||
return d
|
||||
}
|
||||
|
||||
func TestNewDriver(t *testing.T) {
|
||||
// arrange
|
||||
a := newGpioTestAdaptor()
|
||||
// act
|
||||
var di interface{} = NewDriver(a, "GPIO_BASIC")
|
||||
// assert
|
||||
d, ok := di.(*Driver)
|
||||
if !ok {
|
||||
t.Errorf("NewDriver() should have returned a *Driver")
|
||||
}
|
||||
assert.Contains(t, d.name, "GPIO_BASIC")
|
||||
assert.Equal(t, a, d.connection)
|
||||
assert.NoError(t, d.afterStart())
|
||||
assert.NoError(t, d.beforeHalt())
|
||||
assert.NotNil(t, d.Commander)
|
||||
assert.NotNil(t, d.mutex)
|
||||
}
|
||||
|
||||
func TestSetName(t *testing.T) {
|
||||
// arrange
|
||||
d := initTestDriver()
|
||||
// act
|
||||
d.SetName("TESTME")
|
||||
// assert
|
||||
assert.Equal(t, "TESTME", d.Name())
|
||||
}
|
||||
|
||||
func TestConnection(t *testing.T) {
|
||||
// arrange
|
||||
d, a := initTestDriverWithStubbedAdaptor()
|
||||
// act, assert
|
||||
assert.Equal(t, a, d.Connection())
|
||||
}
|
||||
|
||||
func TestStart(t *testing.T) {
|
||||
// arrange
|
||||
d := initTestDriver()
|
||||
// act, assert
|
||||
assert.NoError(t, d.Start())
|
||||
// arrange after start function
|
||||
d.afterStart = func() error { return fmt.Errorf("after start error") }
|
||||
// act, assert
|
||||
assert.ErrorContains(t, d.Start(), "after start error")
|
||||
}
|
||||
|
||||
func TestHalt(t *testing.T) {
|
||||
// arrange
|
||||
d := initTestDriver()
|
||||
// act, assert
|
||||
assert.NoError(t, d.Halt())
|
||||
// arrange after start function
|
||||
d.beforeHalt = func() error { return fmt.Errorf("before halt error") }
|
||||
// act, assert
|
||||
assert.ErrorContains(t, d.Halt(), "before halt error")
|
||||
}
|
|
@ -0,0 +1,218 @@
|
|||
package gpio
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"sync"
|
||||
"time"
|
||||
|
||||
"gobot.io/x/gobot/v2"
|
||||
"gobot.io/x/gobot/v2/system"
|
||||
)
|
||||
|
||||
const (
|
||||
hcsr04SoundSpeed = 343 // in [m/s]
|
||||
// the device can measure 2 cm .. 4 m, this means sweep distances between 4 cm and 8 m
|
||||
// this cause pulse duration between 0.12 ms and 24 ms (at 34.3 cm/ms, ~0.03 ms/cm, ~3 ms/m)
|
||||
// so we use 60 ms as a limit for timeout and 100 ms for duration between 2 consecutive measurements
|
||||
hcsr04StartTransmitTimeout time.Duration = 100 * time.Millisecond // unfortunately takes sometimes longer than 60 ms
|
||||
hcsr04ReceiveTimeout time.Duration = 60 * time.Millisecond
|
||||
hcsr04EmitTriggerDuration time.Duration = 10 * time.Microsecond // according to specification
|
||||
hcsr04MonitorUpdate time.Duration = 200 * time.Millisecond
|
||||
// the resolution of the device is ~3 mm, which relates to 10 us (343 mm/ms = 0.343 mm/us)
|
||||
// the poll interval increases the reading interval to this value and adds around 3 mm inaccuracy
|
||||
// it takes only an effect for fast systems, because reading inputs is typically much slower, e.g. 30-50 us on raspi
|
||||
// so, using the internal edge detection with "cdev" is more precise
|
||||
hcsr04PollInputIntervall time.Duration = 10 * time.Microsecond
|
||||
)
|
||||
|
||||
// HCSR04Driver is a driver for ultrasonic range measurement.
|
||||
type HCSR04Driver struct {
|
||||
*Driver
|
||||
triggerPinID string
|
||||
echoPinID string
|
||||
useEdgePolling bool // use discrete edge polling instead "cdev" from gpiod
|
||||
measureMutex *sync.Mutex // to ensure that only one measurement is done at a time
|
||||
triggerPin gobot.DigitalPinner
|
||||
echoPin gobot.DigitalPinner
|
||||
lastMeasureMicroSec int64 // ~120 .. 24000 us
|
||||
distanceMonitorStopChan chan struct{}
|
||||
distanceMonitorStopWaitGroup *sync.WaitGroup
|
||||
delayMicroSecChan chan int64 // channel for event handler return value
|
||||
pollQuitChan chan struct{} // channel for quit the continuous polling
|
||||
}
|
||||
|
||||
// NewHCSR04Driver creates a new instance of the driver for HC-SR04 (same as SEN-US01).
|
||||
//
|
||||
// Datasheet: https://www.makershop.de/download/HCSR04-datasheet-version-1.pdf
|
||||
func NewHCSR04Driver(a gobot.Adaptor, triggerPinID string, echoPinID string, useEdgePolling bool) *HCSR04Driver {
|
||||
h := HCSR04Driver{
|
||||
Driver: NewDriver(a, "HCSR04"),
|
||||
triggerPinID: triggerPinID,
|
||||
echoPinID: echoPinID,
|
||||
useEdgePolling: useEdgePolling,
|
||||
measureMutex: &sync.Mutex{},
|
||||
}
|
||||
|
||||
h.afterStart = func() error {
|
||||
tpin, err := a.(gobot.DigitalPinnerProvider).DigitalPin(triggerPinID)
|
||||
if err != nil {
|
||||
return fmt.Errorf("error on get trigger pin: %v", err)
|
||||
}
|
||||
if err := tpin.ApplyOptions(system.WithPinDirectionOutput(0)); err != nil {
|
||||
return fmt.Errorf("error on apply output for trigger pin: %v", err)
|
||||
}
|
||||
h.triggerPin = tpin
|
||||
|
||||
// pins are inputs by default
|
||||
epin, err := a.(gobot.DigitalPinnerProvider).DigitalPin(echoPinID)
|
||||
if err != nil {
|
||||
return fmt.Errorf("error on get echo pin: %v", err)
|
||||
}
|
||||
|
||||
epinOptions := []func(gobot.DigitalPinOptioner) bool{system.WithPinEventOnBothEdges(h.createEventHandler())}
|
||||
if h.useEdgePolling {
|
||||
h.pollQuitChan = make(chan struct{})
|
||||
epinOptions = append(epinOptions, system.WithPinPollForEdgeDetection(hcsr04PollInputIntervall, h.pollQuitChan))
|
||||
}
|
||||
if err := epin.ApplyOptions(epinOptions...); err != nil {
|
||||
return fmt.Errorf("error on apply options for echo pin: %v", err)
|
||||
}
|
||||
h.echoPin = epin
|
||||
|
||||
h.delayMicroSecChan = make(chan int64)
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
h.beforeHalt = func() error {
|
||||
if useEdgePolling {
|
||||
close(h.pollQuitChan)
|
||||
}
|
||||
|
||||
if err := h.stopDistanceMonitor(); err != nil {
|
||||
fmt.Printf("no need to stop distance monitoring: %v\n", err)
|
||||
}
|
||||
|
||||
// note: Unexport() of all pins will be done on adaptor.Finalize()
|
||||
|
||||
close(h.delayMicroSecChan)
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
return &h
|
||||
}
|
||||
|
||||
// MeasureDistance retrieves the distance in front of sensor in meters and returns the measure. It is not designed
|
||||
// to work in a fast loop! For this specific usage, use StartDistanceMonitor() associated with Distance() instead.
|
||||
func (h *HCSR04Driver) MeasureDistance() (float64, error) {
|
||||
err := h.measureDistance()
|
||||
if err != nil {
|
||||
return 0, err
|
||||
}
|
||||
return h.Distance(), nil
|
||||
}
|
||||
|
||||
// Distance returns the last distance measured in meter, it does not trigger a distance measurement
|
||||
func (h *HCSR04Driver) Distance() float64 {
|
||||
distMm := h.lastMeasureMicroSec * hcsr04SoundSpeed / 1000 / 2
|
||||
return float64(distMm) / 1000.0
|
||||
}
|
||||
|
||||
// StartDistanceMonitor starts continuous measurement. The current value can be read by Distance()
|
||||
func (h *HCSR04Driver) StartDistanceMonitor() error {
|
||||
// ensure that start and stop can not interfere
|
||||
h.mutex.Lock()
|
||||
defer h.mutex.Unlock()
|
||||
|
||||
if h.distanceMonitorStopChan != nil {
|
||||
return fmt.Errorf("distance monitor already started for '%s'", h.name)
|
||||
}
|
||||
|
||||
h.distanceMonitorStopChan = make(chan struct{})
|
||||
h.distanceMonitorStopWaitGroup = &sync.WaitGroup{}
|
||||
h.distanceMonitorStopWaitGroup.Add(1)
|
||||
|
||||
go func(name string) {
|
||||
defer h.distanceMonitorStopWaitGroup.Done()
|
||||
for {
|
||||
select {
|
||||
case <-h.distanceMonitorStopChan:
|
||||
h.distanceMonitorStopChan = nil
|
||||
return
|
||||
default:
|
||||
if err := h.measureDistance(); err != nil {
|
||||
fmt.Printf("continuous measure distance skipped for '%s': %v\n", name, err)
|
||||
}
|
||||
time.Sleep(hcsr04MonitorUpdate)
|
||||
}
|
||||
}
|
||||
}(h.name)
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
// StopDistanceMonitor stop the monitor process
|
||||
func (h *HCSR04Driver) StopDistanceMonitor() error {
|
||||
// ensure that start and stop can not interfere
|
||||
h.mutex.Lock()
|
||||
defer h.mutex.Unlock()
|
||||
|
||||
return h.stopDistanceMonitor()
|
||||
}
|
||||
|
||||
func (h *HCSR04Driver) createEventHandler() func(int, time.Duration, string, uint32, uint32) {
|
||||
var startTimestamp time.Duration
|
||||
return func(offset int, t time.Duration, et string, sn uint32, lsn uint32) {
|
||||
switch et {
|
||||
case system.DigitalPinEventRisingEdge:
|
||||
startTimestamp = t
|
||||
case system.DigitalPinEventFallingEdge:
|
||||
// unfortunately there is an additional falling edge at each start trigger, so we need to filter this
|
||||
// we use the start duration value for filtering
|
||||
if startTimestamp == 0 {
|
||||
return
|
||||
}
|
||||
h.delayMicroSecChan <- (t - startTimestamp).Microseconds()
|
||||
startTimestamp = 0
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (h *HCSR04Driver) stopDistanceMonitor() error {
|
||||
if h.distanceMonitorStopChan == nil {
|
||||
return fmt.Errorf("distance monitor is not yet started for '%s'", h.name)
|
||||
}
|
||||
|
||||
h.distanceMonitorStopChan <- struct{}{}
|
||||
h.distanceMonitorStopWaitGroup.Wait()
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
func (h *HCSR04Driver) measureDistance() error {
|
||||
h.measureMutex.Lock()
|
||||
defer h.measureMutex.Unlock()
|
||||
|
||||
if err := h.emitTrigger(); err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
// stop the loop if the measure is done or the timeout is elapsed
|
||||
timeout := hcsr04StartTransmitTimeout + hcsr04ReceiveTimeout
|
||||
select {
|
||||
case <-time.After(timeout):
|
||||
return fmt.Errorf("timeout %s reached while waiting for value with echo pin %s", timeout, h.echoPinID)
|
||||
case h.lastMeasureMicroSec = <-h.delayMicroSecChan:
|
||||
}
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
func (h *HCSR04Driver) emitTrigger() error {
|
||||
if err := h.triggerPin.Write(1); err != nil {
|
||||
return err
|
||||
}
|
||||
time.Sleep(hcsr04EmitTriggerDuration)
|
||||
return h.triggerPin.Write(0)
|
||||
}
|
|
@ -0,0 +1,338 @@
|
|||
package gpio
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"strings"
|
||||
"sync"
|
||||
"testing"
|
||||
"time"
|
||||
|
||||
"github.com/stretchr/testify/assert"
|
||||
"github.com/stretchr/testify/require"
|
||||
"gobot.io/x/gobot/v2/system"
|
||||
)
|
||||
|
||||
func initTestHCSR04DriverWithStubbedAdaptor(triggerPinID string, echoPinID string) (*HCSR04Driver, *digitalPinMock) {
|
||||
a := newGpioTestAdaptor()
|
||||
tpin := a.addDigitalPin(triggerPinID)
|
||||
_ = a.addDigitalPin(echoPinID)
|
||||
d := NewHCSR04Driver(a, triggerPinID, echoPinID, false)
|
||||
if err := d.Start(); err != nil {
|
||||
panic(err)
|
||||
}
|
||||
return d, tpin
|
||||
}
|
||||
|
||||
func TestNewHCSR04Driver(t *testing.T) {
|
||||
// arrange
|
||||
const (
|
||||
triggerPinID = "3"
|
||||
echoPinID = "4"
|
||||
)
|
||||
a := newGpioTestAdaptor()
|
||||
tpin := a.addDigitalPin(triggerPinID)
|
||||
epin := a.addDigitalPin(echoPinID)
|
||||
// act
|
||||
d := NewHCSR04Driver(a, triggerPinID, echoPinID, false)
|
||||
// assert
|
||||
assert.IsType(t, &HCSR04Driver{}, d)
|
||||
assert.NotNil(t, d.Driver)
|
||||
assert.True(t, strings.HasPrefix(d.name, "HCSR04"))
|
||||
assert.Equal(t, a, d.connection)
|
||||
assert.NoError(t, d.afterStart())
|
||||
assert.NoError(t, d.beforeHalt())
|
||||
assert.NotNil(t, d.Commander)
|
||||
assert.NotNil(t, d.mutex)
|
||||
assert.Equal(t, triggerPinID, d.triggerPinID)
|
||||
assert.Equal(t, echoPinID, d.echoPinID)
|
||||
assert.Equal(t, false, d.useEdgePolling)
|
||||
assert.Equal(t, tpin, d.triggerPin)
|
||||
assert.Equal(t, epin, d.echoPin)
|
||||
}
|
||||
|
||||
func TestHCSR04MeasureDistance(t *testing.T) {
|
||||
tests := map[string]struct {
|
||||
measureMicroSec int64
|
||||
simulateWriteErr string
|
||||
wantCallsWrite int
|
||||
wantVal float64
|
||||
wantErr string
|
||||
}{
|
||||
"measure_ok": {
|
||||
measureMicroSec: 5831,
|
||||
wantCallsWrite: 2,
|
||||
wantVal: 1.0,
|
||||
},
|
||||
"error_timeout": {
|
||||
measureMicroSec: 170000, // > 160 ms
|
||||
wantCallsWrite: 2,
|
||||
wantErr: "timeout 160ms reached",
|
||||
},
|
||||
"error_write": {
|
||||
measureMicroSec: 5831,
|
||||
simulateWriteErr: "write error",
|
||||
wantCallsWrite: 1,
|
||||
wantErr: "write error",
|
||||
},
|
||||
}
|
||||
for name, tc := range tests {
|
||||
t.Run(name, func(t *testing.T) {
|
||||
// arrange
|
||||
d, tpin := initTestHCSR04DriverWithStubbedAdaptor("3", "4")
|
||||
// arrange sensor and event handler simulation
|
||||
waitForTriggerChan := make(chan struct{})
|
||||
loopWg := sync.WaitGroup{}
|
||||
defer func() {
|
||||
close(waitForTriggerChan)
|
||||
loopWg.Wait()
|
||||
}()
|
||||
loopWg.Add(1)
|
||||
go func() {
|
||||
<-waitForTriggerChan
|
||||
m := tc.measureMicroSec // to prevent data race together with wait group
|
||||
loopWg.Done()
|
||||
time.Sleep(time.Duration(m) * time.Microsecond)
|
||||
d.delayMicroSecChan <- m
|
||||
}()
|
||||
// arrange writes
|
||||
numCallsWrite := 0
|
||||
var oldVal int
|
||||
tpin.writeFunc = func(val int) error {
|
||||
numCallsWrite++
|
||||
if val == 0 && oldVal == 1 {
|
||||
// falling edge detected
|
||||
waitForTriggerChan <- struct{}{}
|
||||
}
|
||||
oldVal = val
|
||||
var err error
|
||||
if tc.simulateWriteErr != "" {
|
||||
err = fmt.Errorf(tc.simulateWriteErr)
|
||||
}
|
||||
return err
|
||||
}
|
||||
// act
|
||||
got, err := d.MeasureDistance()
|
||||
// assert
|
||||
assert.Equal(t, tc.wantCallsWrite, numCallsWrite)
|
||||
if tc.wantErr != "" {
|
||||
assert.ErrorContains(t, err, tc.wantErr)
|
||||
} else {
|
||||
require.NoError(t, err)
|
||||
}
|
||||
assert.Equal(t, tc.wantVal, got)
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
func TestHCSR04Distance(t *testing.T) {
|
||||
tests := map[string]struct {
|
||||
measureMicroSec int64
|
||||
simulateWriteErr string
|
||||
wantVal float64
|
||||
wantErr string
|
||||
}{
|
||||
"distance_0mm": {
|
||||
measureMicroSec: 0, // no validity test yet
|
||||
wantVal: 0.0,
|
||||
},
|
||||
"distance_2cm": {
|
||||
measureMicroSec: 117, // 117us ~ 0.12ms => ~2cm
|
||||
wantVal: 0.02,
|
||||
},
|
||||
"distance_4m": {
|
||||
measureMicroSec: 23324, // 23324us ~ 24ms => ~4m
|
||||
wantVal: 4.0,
|
||||
},
|
||||
}
|
||||
for name, tc := range tests {
|
||||
t.Run(name, func(t *testing.T) {
|
||||
// arrange
|
||||
d := HCSR04Driver{lastMeasureMicroSec: tc.measureMicroSec}
|
||||
// act
|
||||
got := d.Distance()
|
||||
// assert
|
||||
assert.Equal(t, tc.wantVal, got)
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
func TestHCSR04StartDistanceMonitor(t *testing.T) {
|
||||
tests := map[string]struct {
|
||||
simulateIsStarted bool
|
||||
simulateWriteErr bool
|
||||
wantErr string
|
||||
}{
|
||||
"start_ok": {},
|
||||
"start_ok_measure_error": {
|
||||
simulateWriteErr: true,
|
||||
},
|
||||
"error_already_started": {
|
||||
simulateIsStarted: true,
|
||||
wantErr: "already started for 'HCSR04-",
|
||||
},
|
||||
}
|
||||
for name, tc := range tests {
|
||||
t.Run(name, func(t *testing.T) {
|
||||
// arrange
|
||||
d, tpin := initTestHCSR04DriverWithStubbedAdaptor("3", "4")
|
||||
defer func() {
|
||||
if d.distanceMonitorStopChan != nil {
|
||||
close(d.distanceMonitorStopChan)
|
||||
}
|
||||
if d.distanceMonitorStopWaitGroup != nil {
|
||||
d.distanceMonitorStopWaitGroup.Wait()
|
||||
}
|
||||
}()
|
||||
if tc.simulateIsStarted {
|
||||
d.distanceMonitorStopChan = make(chan struct{})
|
||||
}
|
||||
tpin.writeFunc = func(val int) error {
|
||||
if tc.simulateWriteErr {
|
||||
return fmt.Errorf("write error")
|
||||
}
|
||||
return nil
|
||||
}
|
||||
// act
|
||||
err := d.StartDistanceMonitor()
|
||||
time.Sleep(1 * time.Millisecond) // < 160 ms
|
||||
// assert
|
||||
if tc.wantErr != "" {
|
||||
assert.ErrorContains(t, err, tc.wantErr)
|
||||
} else {
|
||||
require.NoError(t, err)
|
||||
assert.NotNil(t, d.distanceMonitorStopChan)
|
||||
assert.NotNil(t, d.distanceMonitorStopWaitGroup)
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
func TestHCSR04StopDistanceMonitor(t *testing.T) {
|
||||
tests := map[string]struct {
|
||||
start bool
|
||||
wantErr string
|
||||
}{
|
||||
"stop_ok": {
|
||||
start: true,
|
||||
},
|
||||
"error_not_started": {
|
||||
wantErr: "not yet started for 'HCSR04-",
|
||||
},
|
||||
}
|
||||
for name, tc := range tests {
|
||||
t.Run(name, func(t *testing.T) {
|
||||
// arrange
|
||||
d, _ := initTestHCSR04DriverWithStubbedAdaptor("3", "4")
|
||||
defer func() {
|
||||
if d.distanceMonitorStopChan != nil {
|
||||
close(d.distanceMonitorStopChan)
|
||||
}
|
||||
if d.distanceMonitorStopWaitGroup != nil {
|
||||
d.distanceMonitorStopWaitGroup.Wait()
|
||||
}
|
||||
}()
|
||||
if tc.start {
|
||||
err := d.StartDistanceMonitor()
|
||||
require.NoError(t, err)
|
||||
}
|
||||
// act
|
||||
err := d.StopDistanceMonitor()
|
||||
time.Sleep(1 * time.Millisecond) // < 160 ms
|
||||
// assert
|
||||
if tc.wantErr != "" {
|
||||
assert.ErrorContains(t, err, tc.wantErr)
|
||||
} else {
|
||||
require.NoError(t, err)
|
||||
assert.Nil(t, d.distanceMonitorStopChan)
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
func TestHCSR04_createEventHandler(t *testing.T) {
|
||||
type eventCall struct {
|
||||
timeStamp time.Duration
|
||||
eventType string
|
||||
}
|
||||
tests := map[string]struct {
|
||||
calls []eventCall
|
||||
wants []int64
|
||||
}{
|
||||
"only_rising": {
|
||||
calls: []eventCall{
|
||||
{timeStamp: 1 * time.Microsecond, eventType: system.DigitalPinEventRisingEdge},
|
||||
{timeStamp: 2 * time.Microsecond, eventType: system.DigitalPinEventRisingEdge},
|
||||
},
|
||||
},
|
||||
"only_falling": {
|
||||
calls: []eventCall{
|
||||
{timeStamp: 2 * time.Microsecond, eventType: system.DigitalPinEventFallingEdge},
|
||||
{timeStamp: 3 * time.Microsecond, eventType: system.DigitalPinEventFallingEdge},
|
||||
},
|
||||
},
|
||||
"event_normal": {
|
||||
calls: []eventCall{
|
||||
{timeStamp: 1 * time.Microsecond, eventType: system.DigitalPinEventRisingEdge},
|
||||
{timeStamp: 10 * time.Microsecond, eventType: system.DigitalPinEventFallingEdge},
|
||||
},
|
||||
wants: []int64{9},
|
||||
},
|
||||
"event_falling_before": {
|
||||
calls: []eventCall{
|
||||
{timeStamp: 1 * time.Microsecond, eventType: system.DigitalPinEventFallingEdge},
|
||||
{timeStamp: 2 * time.Microsecond, eventType: system.DigitalPinEventRisingEdge},
|
||||
{timeStamp: 10 * time.Microsecond, eventType: system.DigitalPinEventFallingEdge},
|
||||
},
|
||||
wants: []int64{8},
|
||||
},
|
||||
"event_falling_after": {
|
||||
calls: []eventCall{
|
||||
{timeStamp: 1 * time.Microsecond, eventType: system.DigitalPinEventRisingEdge},
|
||||
{timeStamp: 10 * time.Microsecond, eventType: system.DigitalPinEventFallingEdge},
|
||||
{timeStamp: 12 * time.Microsecond, eventType: system.DigitalPinEventFallingEdge},
|
||||
},
|
||||
wants: []int64{9},
|
||||
},
|
||||
"event_rising_before": {
|
||||
calls: []eventCall{
|
||||
{timeStamp: 1 * time.Microsecond, eventType: system.DigitalPinEventRisingEdge},
|
||||
{timeStamp: 5 * time.Microsecond, eventType: system.DigitalPinEventRisingEdge},
|
||||
{timeStamp: 10 * time.Microsecond, eventType: system.DigitalPinEventFallingEdge},
|
||||
},
|
||||
wants: []int64{5},
|
||||
},
|
||||
"event_rising_after": {
|
||||
calls: []eventCall{
|
||||
{timeStamp: 1 * time.Microsecond, eventType: system.DigitalPinEventRisingEdge},
|
||||
{timeStamp: 10 * time.Microsecond, eventType: system.DigitalPinEventFallingEdge},
|
||||
{timeStamp: 12 * time.Microsecond, eventType: system.DigitalPinEventRisingEdge},
|
||||
},
|
||||
wants: []int64{9},
|
||||
},
|
||||
"event_multiple": {
|
||||
calls: []eventCall{
|
||||
{timeStamp: 1 * time.Microsecond, eventType: system.DigitalPinEventRisingEdge},
|
||||
{timeStamp: 10 * time.Microsecond, eventType: system.DigitalPinEventFallingEdge},
|
||||
{timeStamp: 11 * time.Microsecond, eventType: system.DigitalPinEventRisingEdge},
|
||||
{timeStamp: 13 * time.Microsecond, eventType: system.DigitalPinEventFallingEdge},
|
||||
},
|
||||
wants: []int64{9, 2},
|
||||
},
|
||||
}
|
||||
for name, tc := range tests {
|
||||
t.Run(name, func(t *testing.T) {
|
||||
// arrange
|
||||
d := HCSR04Driver{delayMicroSecChan: make(chan int64, len(tc.wants))}
|
||||
// act
|
||||
eh := d.createEventHandler()
|
||||
for _, call := range tc.calls {
|
||||
eh(0, call.timeStamp, call.eventType, 0, 0)
|
||||
}
|
||||
// assert
|
||||
for _, want := range tc.wants {
|
||||
got := <-d.delayMicroSecChan
|
||||
assert.Equal(t, want, got)
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
|
@ -1,6 +1,11 @@
|
|||
package gpio
|
||||
|
||||
import "sync"
|
||||
import (
|
||||
"fmt"
|
||||
"sync"
|
||||
|
||||
"gobot.io/x/gobot/v2"
|
||||
)
|
||||
|
||||
type gpioTestBareAdaptor struct{}
|
||||
|
||||
|
@ -9,8 +14,13 @@ func (t *gpioTestBareAdaptor) Finalize() (err error) { return }
|
|||
func (t *gpioTestBareAdaptor) Name() string { return "" }
|
||||
func (t *gpioTestBareAdaptor) SetName(n string) {}
|
||||
|
||||
type digitalPinMock struct {
|
||||
writeFunc func(val int) (err error)
|
||||
}
|
||||
|
||||
type gpioTestAdaptor struct {
|
||||
name string
|
||||
pinMap map[string]gobot.DigitalPinner
|
||||
port string
|
||||
mtx sync.Mutex
|
||||
digitalReadFunc func(ping string) (val int, err error)
|
||||
|
@ -21,8 +31,9 @@ type gpioTestAdaptor struct {
|
|||
|
||||
func newGpioTestAdaptor() *gpioTestAdaptor {
|
||||
t := gpioTestAdaptor{
|
||||
name: "gpio_test_adaptor",
|
||||
port: "/dev/null",
|
||||
name: "gpio_test_adaptor",
|
||||
pinMap: make(map[string]gobot.DigitalPinner),
|
||||
port: "/dev/null",
|
||||
digitalWriteFunc: func(pin string, val byte) (err error) {
|
||||
return nil
|
||||
},
|
||||
|
@ -73,3 +84,44 @@ func (t *gpioTestAdaptor) Finalize() (err error) { return }
|
|||
func (t *gpioTestAdaptor) Name() string { return t.name }
|
||||
func (t *gpioTestAdaptor) SetName(n string) { t.name = n }
|
||||
func (t *gpioTestAdaptor) Port() string { return t.port }
|
||||
|
||||
// DigitalPin (interface DigitalPinnerProvider) return a pin object
|
||||
func (t *gpioTestAdaptor) DigitalPin(id string) (gobot.DigitalPinner, error) {
|
||||
if pin, ok := t.pinMap[id]; ok {
|
||||
return pin, nil
|
||||
}
|
||||
return nil, fmt.Errorf("pin '%s' not found in '%s'", id, t.name)
|
||||
}
|
||||
|
||||
// ApplyOptions (interface DigitalPinOptionApplier by DigitalPinner) apply all given options to the pin immediately
|
||||
func (d *digitalPinMock) ApplyOptions(options ...func(gobot.DigitalPinOptioner) bool) error {
|
||||
return nil
|
||||
}
|
||||
|
||||
// Export (interface DigitalPinner) exports the pin for use by the adaptor
|
||||
func (d *digitalPinMock) Export() error {
|
||||
return nil
|
||||
}
|
||||
|
||||
// Unexport (interface DigitalPinner) releases the pin from the adaptor, so it is free for the operating system
|
||||
func (d *digitalPinMock) Unexport() error {
|
||||
return nil
|
||||
}
|
||||
|
||||
// Read (interface DigitalPinner) reads the current value of the pin
|
||||
func (d *digitalPinMock) Read() (n int, err error) {
|
||||
return 0, err
|
||||
}
|
||||
|
||||
// Write (interface DigitalPinner) writes to the pin
|
||||
func (d *digitalPinMock) Write(b int) error {
|
||||
return d.writeFunc(b)
|
||||
}
|
||||
|
||||
func (t *gpioTestAdaptor) addDigitalPin(id string) *digitalPinMock {
|
||||
dpm := &digitalPinMock{
|
||||
writeFunc: func(val int) (err error) { return nil },
|
||||
}
|
||||
t.pinMap[id] = dpm
|
||||
return dpm
|
||||
}
|
||||
|
|
|
@ -0,0 +1,89 @@
|
|||
//go:build example
|
||||
// +build example
|
||||
|
||||
//
|
||||
// Do not build by default.
|
||||
|
||||
package main
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"log"
|
||||
"os"
|
||||
"time"
|
||||
|
||||
"gobot.io/x/gobot/v2"
|
||||
"gobot.io/x/gobot/v2/drivers/gpio"
|
||||
"gobot.io/x/gobot/v2/platforms/raspi"
|
||||
)
|
||||
|
||||
func main() {
|
||||
const (
|
||||
triggerOutput = "11"
|
||||
echoInput = "13"
|
||||
)
|
||||
|
||||
// this is mandatory for systems with defunct edge detection, although the "cdev" is used with an newer Kernel
|
||||
// keep in mind, that this cause more inaccurate measurements
|
||||
const pollEdgeDetection = true
|
||||
|
||||
a := raspi.NewAdaptor()
|
||||
hcsr04 := gpio.NewHCSR04Driver(a, triggerOutput, echoInput, pollEdgeDetection)
|
||||
|
||||
work := func() {
|
||||
if pollEdgeDetection {
|
||||
fmt.Println("Please note that measurements are CPU consuming and will be more inaccurate with this setting.")
|
||||
fmt.Println("After startup the system is under load and the measurement is very inaccurate, so wait a bit...")
|
||||
time.Sleep(2000 * time.Millisecond)
|
||||
}
|
||||
|
||||
if err := hcsr04.StartDistanceMonitor(); err != nil {
|
||||
log.Fatal(err)
|
||||
}
|
||||
|
||||
// first single shot
|
||||
if v, err := hcsr04.MeasureDistance(); err != nil {
|
||||
log.Fatal(err)
|
||||
} else {
|
||||
fmt.Printf("first single shot done: %5.3f m\n", v)
|
||||
}
|
||||
|
||||
ticker := gobot.Every(1*time.Second, func() {
|
||||
fmt.Printf("continuous measurement: %5.3f m\n", hcsr04.Distance())
|
||||
})
|
||||
|
||||
gobot.After(5*time.Second, func() {
|
||||
if err := hcsr04.StopDistanceMonitor(); err != nil {
|
||||
log.Fatal(err)
|
||||
}
|
||||
ticker.Stop()
|
||||
})
|
||||
|
||||
gobot.After(7*time.Second, func() {
|
||||
// second single shot
|
||||
if v, err := hcsr04.MeasureDistance(); err != nil {
|
||||
log.Fatal(err)
|
||||
} else {
|
||||
fmt.Printf("second single shot done: %5.3f m\n", v)
|
||||
}
|
||||
// cleanup
|
||||
if err := hcsr04.Halt(); err != nil {
|
||||
log.Println(err)
|
||||
}
|
||||
if err := a.Finalize(); err != nil {
|
||||
log.Println(err)
|
||||
}
|
||||
os.Exit(0)
|
||||
})
|
||||
}
|
||||
|
||||
robot := gobot.NewRobot("distanceBot",
|
||||
[]gobot.Connection{a},
|
||||
[]gobot.Device{hcsr04},
|
||||
work,
|
||||
)
|
||||
|
||||
if err := robot.Start(); err != nil {
|
||||
log.Fatal(err)
|
||||
}
|
||||
}
|
|
@ -0,0 +1,93 @@
|
|||
//go:build example
|
||||
// +build example
|
||||
|
||||
//
|
||||
// Do not build by default.
|
||||
|
||||
package main
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"log"
|
||||
"os"
|
||||
"time"
|
||||
|
||||
"gobot.io/x/gobot/v2"
|
||||
"gobot.io/x/gobot/v2/drivers/gpio"
|
||||
"gobot.io/x/gobot/v2/platforms/tinkerboard"
|
||||
)
|
||||
|
||||
// Wiring
|
||||
// PWR Tinkerboard: 2(+5V), 6, 9, 14, 20 (GND)
|
||||
// GPIO Tinkerboard: header pin 7 is the trigger output, pin 22 used as echo input
|
||||
// HC-SR04: the power is wired to +5V and GND of tinkerboard, the same for trigger output and the echo input pin
|
||||
func main() {
|
||||
const (
|
||||
triggerOutput = "7"
|
||||
echoInput = "22"
|
||||
)
|
||||
|
||||
// this is mandatory for systems with defunct edge detection, although the "cdev" is used with an newer Kernel
|
||||
// keep in mind, that this cause more inaccurate measurements
|
||||
const pollEdgeDetection = true
|
||||
|
||||
a := tinkerboard.NewAdaptor()
|
||||
hcsr04 := gpio.NewHCSR04Driver(a, triggerOutput, echoInput, pollEdgeDetection)
|
||||
|
||||
work := func() {
|
||||
if pollEdgeDetection {
|
||||
fmt.Println("Please note that measurements are CPU consuming and will be more inaccurate with this setting.")
|
||||
fmt.Println("After startup the system is under load and the measurement is very inaccurate, so wait a bit...")
|
||||
time.Sleep(2000 * time.Millisecond)
|
||||
}
|
||||
|
||||
if err := hcsr04.StartDistanceMonitor(); err != nil {
|
||||
log.Fatal(err)
|
||||
}
|
||||
|
||||
// first single shot
|
||||
if v, err := hcsr04.MeasureDistance(); err != nil {
|
||||
log.Fatal(err)
|
||||
} else {
|
||||
fmt.Printf("first single shot done: %5.3f m\n", v)
|
||||
}
|
||||
|
||||
ticker := gobot.Every(1*time.Second, func() {
|
||||
fmt.Printf("continuous measurement: %5.3f m\n", hcsr04.Distance())
|
||||
})
|
||||
|
||||
gobot.After(5*time.Second, func() {
|
||||
if err := hcsr04.StopDistanceMonitor(); err != nil {
|
||||
log.Fatal(err)
|
||||
}
|
||||
ticker.Stop()
|
||||
})
|
||||
|
||||
gobot.After(7*time.Second, func() {
|
||||
// second single shot
|
||||
if v, err := hcsr04.MeasureDistance(); err != nil {
|
||||
log.Fatal(err)
|
||||
} else {
|
||||
fmt.Printf("second single shot done: %5.3f m\n", v)
|
||||
}
|
||||
// cleanup
|
||||
if err := hcsr04.Halt(); err != nil {
|
||||
log.Println(err)
|
||||
}
|
||||
if err := a.Finalize(); err != nil {
|
||||
log.Println(err)
|
||||
}
|
||||
os.Exit(0)
|
||||
})
|
||||
}
|
||||
|
||||
robot := gobot.NewRobot("distanceBot",
|
||||
[]gobot.Connection{a},
|
||||
[]gobot.Device{hcsr04},
|
||||
work,
|
||||
)
|
||||
|
||||
if err := robot.Start(); err != nil {
|
||||
log.Fatal(err)
|
||||
}
|
||||
}
|
|
@ -6,6 +6,7 @@ import (
|
|||
"time"
|
||||
|
||||
"github.com/hashicorp/go-multierror"
|
||||
|
||||
"gobot.io/x/gobot/v2"
|
||||
"gobot.io/x/gobot/v2/system"
|
||||
)
|
||||
|
@ -31,6 +32,7 @@ type digitalPinsOptioner interface {
|
|||
detectedEdge string, seqno uint32, lseqno uint32))
|
||||
prepareDigitalPinEventOnBothEdges(pin string, handler func(lineOffset int, timestamp time.Duration,
|
||||
detectedEdge string, seqno uint32, lseqno uint32))
|
||||
prepareDigitalPinPollForEdgeDetection(pin string, pollInterval time.Duration, pollQuitChan chan struct{})
|
||||
}
|
||||
|
||||
// DigitalPinsAdaptor is a adaptor for digital pins, normally used for composition in platforms.
|
||||
|
@ -196,6 +198,17 @@ func WithGpioEventOnBothEdges(pin string, handler func(lineOffset int, timestamp
|
|||
}
|
||||
}
|
||||
|
||||
// WithGpioPollForEdgeDetection prepares the given input pin to use a discrete input pin polling function together with
|
||||
// edge detection.
|
||||
func WithGpioPollForEdgeDetection(pin string, pollInterval time.Duration, pollQuitChan chan struct{}) func(Optioner) {
|
||||
return func(o Optioner) {
|
||||
a, ok := o.(digitalPinsOptioner)
|
||||
if ok {
|
||||
a.prepareDigitalPinPollForEdgeDetection(pin, pollInterval, pollQuitChan)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Connect prepare new connection to digital pins.
|
||||
func (a *DigitalPinsAdaptor) Connect() error {
|
||||
a.mutex.Lock()
|
||||
|
@ -370,6 +383,18 @@ func (a *DigitalPinsAdaptor) prepareDigitalPinEventOnBothEdges(id string, handle
|
|||
a.pinOptions[id] = append(a.pinOptions[id], system.WithPinEventOnBothEdges(handler))
|
||||
}
|
||||
|
||||
func (a *DigitalPinsAdaptor) prepareDigitalPinPollForEdgeDetection(
|
||||
id string,
|
||||
pollInterval time.Duration,
|
||||
pollQuitChan chan struct{},
|
||||
) {
|
||||
if a.pinOptions == nil {
|
||||
a.pinOptions = make(map[string][]func(gobot.DigitalPinOptioner) bool)
|
||||
}
|
||||
|
||||
a.pinOptions[id] = append(a.pinOptions[id], system.WithPinPollForEdgeDetection(pollInterval, pollQuitChan))
|
||||
}
|
||||
|
||||
func (a *DigitalPinsAdaptor) digitalPin(id string, opts ...func(gobot.DigitalPinOptioner) bool) (gobot.DigitalPinner, error) {
|
||||
if a.pins == nil {
|
||||
return nil, fmt.Errorf("not connected for pin %s", id)
|
||||
|
|
|
@ -215,6 +215,7 @@ func TestDigitalWrite(t *testing.T) {
|
|||
WithGpiosPullUp("7")(a)
|
||||
WithGpiosOpenDrain("7")(a)
|
||||
WithGpioEventOnFallingEdge("7", gpioEventHandler)(a)
|
||||
WithGpioPollForEdgeDetection("7", 0, nil)(a)
|
||||
err := a.DigitalWrite("7", 1)
|
||||
assert.NoError(t, err)
|
||||
assert.Equal(t, "1", fs.Files["/sys/class/gpio/gpio18/value"].Contents)
|
||||
|
|
|
@ -1,3 +1,6 @@
|
|||
//go:build !windows
|
||||
// +build !windows
|
||||
|
||||
package firmata
|
||||
|
||||
import (
|
||||
|
|
|
@ -1,3 +1,6 @@
|
|||
//go:build !windows
|
||||
// +build !windows
|
||||
|
||||
package firmata
|
||||
|
||||
import (
|
||||
|
|
|
@ -1,3 +1,6 @@
|
|||
//go:build !windows
|
||||
// +build !windows
|
||||
|
||||
package firmata
|
||||
|
||||
import (
|
||||
|
|
|
@ -1,3 +1,6 @@
|
|||
//go:build !windows
|
||||
// +build !windows
|
||||
|
||||
package firmata
|
||||
|
||||
import (
|
||||
|
|
|
@ -1,3 +1,6 @@
|
|||
//go:build !windows
|
||||
// +build !windows
|
||||
|
||||
package firmata
|
||||
|
||||
import (
|
||||
|
|
|
@ -1,3 +1,6 @@
|
|||
//go:build !windows
|
||||
// +build !windows
|
||||
|
||||
package firmata
|
||||
|
||||
import (
|
||||
|
|
Loading…
Reference in New Issue