Skip to content

Commit

Permalink
gpio(hcsr04): add driver for ultrasonic ranging module
Browse files Browse the repository at this point in the history
  • Loading branch information
gen2thomas committed Oct 23, 2023
1 parent ee4368b commit 004f9aa
Show file tree
Hide file tree
Showing 5 changed files with 397 additions and 4 deletions.
6 changes: 4 additions & 2 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -270,19 +270,21 @@ 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
- 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
Expand Down
6 changes: 4 additions & 2 deletions drivers/gpio/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@ 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
Expand All @@ -21,14 +22,15 @@ Gobot has a extensible system for connecting to hardware devices. The following
- 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
- RGB LED
- Servo
- Stepper Motor
- TM1638 LED Controller

More drivers are coming soon...
62 changes: 62 additions & 0 deletions drivers/gpio/gpio.go → drivers/gpio/gpio_driver.go
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,9 @@ package gpio

import (
"errors"
"sync"

"gobot.io/x/gobot/v2"
)

var (
Expand Down Expand Up @@ -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 i2c 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 i2c 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()
}
255 changes: 255 additions & 0 deletions drivers/gpio/hcsr04_driver.go
Original file line number Diff line number Diff line change
@@ -0,0 +1,255 @@
package gpio

import (
"fmt"
"log"
"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
hcsr04PollInputIntervall time.Duration = 10 * time.Microsecond // this adds around 2 mm inaccuracy (resolution 3 mm)
hcsr04EmitTriggerDuration time.Duration = 10 * time.Microsecond // according to specification
hcsr04MonitorUpdate time.Duration = 200 * time.Millisecond
)

// HCSR04 is a driver for ultrasonic range measurement.
type HCSR04 struct {
*Driver
triggerPinID string
echoPinID string
useEventHandler bool // use gpiod event handler instead own implementation
triggerPin gobot.DigitalPinner
echoPin gobot.DigitalPinner
lastDistanceMm int // distance in mm, ~20..4000
continuousStop chan struct{}
continuousStopWaitGroup *sync.WaitGroup
measureMutex *sync.Mutex // to ensure that only one measurement is done at a time
delayMicroSecChan chan int64 // channel for event handler return value
distanceMonitorStarted bool
}

// NewHCSR04 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 NewHCSR04(a gobot.Adaptor, triggerPinID string, echoPinID string, useEventHandler bool) *HCSR04 {
h := HCSR04{
Driver: NewDriver(a, "HCSR04"),
triggerPinID: triggerPinID,
echoPinID: echoPinID,
useEventHandler: useEventHandler,
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)
}

if useEventHandler {
if err := epin.ApplyOptions(system.WithPinEventOnBothEdges(h.createEventHandler())); err != nil {
return fmt.Errorf("error on apply event handler for echo pin: %v", err)
}
}
h.echoPin = epin

h.delayMicroSecChan = make(chan int64)

return nil
}

h.beforeHalt = func() error {
// TODO: create summarized error
if err := h.stopDistanceMonitor(); err != nil {
fmt.Printf("no need to stop distance monitoring: %v\n", err)
}

if err := h.triggerPin.Unexport(); err != nil {
fmt.Printf("error on unexport trigger pin: %v\n", err)
}

if err := h.echoPin.Unexport(); err != nil {
fmt.Printf("error on unexport echo pin: %v\n", err)
}

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 GetDistance() instead.
func (h *HCSR04) MeasureDistance() (float64, error) {
err := h.measureDistance()
if err != nil {
return 0, err
}
return h.GetDistance(), nil
}

// StartDistanceMonitor starts continuous measurement. The current value can be read by GetDistance()
func (h *HCSR04) StartDistanceMonitor() error {
// ensure that start and stop can not interfere
h.mutex.Lock()
defer h.mutex.Unlock()

if h.distanceMonitorStarted {
return fmt.Errorf("distance monitor already started for '%s'", h.name)
}

h.distanceMonitorStarted = true
h.continuousStop = make(chan struct{})
h.continuousStopWaitGroup = &sync.WaitGroup{}
h.continuousStopWaitGroup.Add(1)

go func(name string) {
defer h.continuousStopWaitGroup.Done()
for {
select {
case <-h.continuousStop:
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 *HCSR04) StopDistanceMonitor() error {
// ensure that start and stop can not interfere
h.mutex.Lock()
defer h.mutex.Unlock()

return h.stopDistanceMonitor()
}

func (h *HCSR04) stopDistanceMonitor() error {
if !h.distanceMonitorStarted {
return fmt.Errorf("distance monitor is not yet started for '%s'", h.name)
}

h.continuousStop <- struct{}{}
h.continuousStopWaitGroup.Wait()
h.distanceMonitorStarted = false

return nil
}

// GetDistance returns the last distance measured in meter, it does not trigger a distance measurement
func (h *HCSR04) GetDistance() float64 {
return float64(h.lastDistanceMm) / 1000.0
}

func (h *HCSR04) measureDistance() error {
h.measureMutex.Lock()
defer h.measureMutex.Unlock()

errChan := make(chan error)
quitChan := make(chan struct{})
defer close(quitChan)
if !h.useEventHandler {
go h.edgePolling(hcsr04PollInputIntervall, errChan, quitChan, h.createEventHandler())
}

if err := h.emitTrigger(); err != nil {
return err
}

// stop the loop if the state is on target state, the timeout is elapsed or an error occurred
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 err := <-errChan:
return fmt.Errorf("error while detect echo: %v", err)
case durMicro := <-h.delayMicroSecChan:
log.Println(durMicro, "us")
h.lastDistanceMm = int(durMicro * hcsr04SoundSpeed / 1000 / 2)
}

return nil
}

func (h *HCSR04) 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 *HCSR04) emitTrigger() error {
if err := h.triggerPin.Write(1); err != nil {
return err
}
time.Sleep(hcsr04EmitTriggerDuration)
return h.triggerPin.Write(0)
}

// TODO: move this function to system level and add a "WithEdgePolling()" option
func (h *HCSR04) edgePolling(
pollIntervall time.Duration,
errChan chan error,
quitChan chan struct{},
eventHandler func(offset int, t time.Duration, et string, sn uint32, lsn uint32),
) {
var oldState int
for {
select {
case <-quitChan:
return
default:
readedValue, err := h.echoPin.Read()
if err != nil {
errChan <- fmt.Errorf("an error occurred while reading the pin %s: %v", h.echoPinID, err)
return
}
if readedValue > oldState {
eventHandler(0, time.Duration(time.Now().UnixNano()), system.DigitalPinEventRisingEdge, 0, 0)
} else if readedValue < oldState {
eventHandler(0, time.Duration(time.Now().UnixNano()), system.DigitalPinEventFallingEdge, 0, 0)
}
oldState = readedValue
time.Sleep(pollIntervall)
}
}
}
Loading

0 comments on commit 004f9aa

Please sign in to comment.