mirror of https://github.com/kidoman/embd
98 lines
2.0 KiB
Go
98 lines
2.0 KiB
Go
// Package us020 allows interfacing with the US020 ultrasonic range finder.
|
|
package us020
|
|
|
|
import (
|
|
"sync"
|
|
"time"
|
|
|
|
"github.com/kid0m4n/go-rpi/sensor/bmp085"
|
|
"github.com/stianeikeland/go-rpio"
|
|
)
|
|
|
|
const (
|
|
pulseDelay = 30000 * time.Nanosecond
|
|
)
|
|
|
|
// A US020 implements access to a US020 ultrasonic range finder.
|
|
type US020 interface {
|
|
// Distance computes the distance of the bot from the closest obstruction.
|
|
Distance() (float64, error)
|
|
}
|
|
|
|
type us020 struct {
|
|
echoPinNumber, triggerPinNumber int
|
|
|
|
echoPin rpio.Pin
|
|
triggerPin rpio.Pin
|
|
|
|
speedSound float64
|
|
|
|
initialized bool
|
|
mu *sync.RWMutex
|
|
}
|
|
|
|
// New creates a new US020 interface. The bus variable controls
|
|
// the I2C bus used to communicate with the device.
|
|
func New(e, t int) US020 {
|
|
return &us020{echoPinNumber: e, triggerPinNumber: t, mu: new(sync.RWMutex)}
|
|
}
|
|
|
|
func (d *us020) setup() (err error) {
|
|
d.mu.RLock()
|
|
if d.initialized {
|
|
d.mu.RUnlock()
|
|
return
|
|
}
|
|
d.mu.RUnlock()
|
|
|
|
d.mu.Lock()
|
|
defer d.mu.Unlock()
|
|
|
|
if err = rpio.Open(); err != nil {
|
|
return
|
|
}
|
|
|
|
d.echoPin = rpio.Pin(d.echoPinNumber) // ECHO port on the US020
|
|
d.triggerPin = rpio.Pin(d.triggerPinNumber) // TRIGGER port on the US020
|
|
|
|
temp, err := bmp085.Temperature()
|
|
if err != nil {
|
|
d.speedSound = 340
|
|
} else {
|
|
d.speedSound = 331.4 + 0.606*temp
|
|
}
|
|
|
|
d.initialized = true
|
|
|
|
return nil
|
|
}
|
|
|
|
// Distance computes the distance of the bot from the closest obstruction.
|
|
func (d *us020) Distance() (distance float64, err error) {
|
|
if err = d.setup(); err != nil {
|
|
return
|
|
}
|
|
|
|
// Generate a TRIGGER pulse
|
|
d.triggerPin.High()
|
|
time.Sleep(pulseDelay)
|
|
d.triggerPin.Low()
|
|
|
|
// Wait until ECHO goes high
|
|
for d.echoPin.Read() == rpio.Low {
|
|
}
|
|
|
|
startTime := time.Now() // Record time when ECHO goes high
|
|
|
|
// Wait until ECHO goes low
|
|
for d.echoPin.Read() == rpio.High {
|
|
}
|
|
|
|
duration := time.Since(startTime) // Calculate time lapsed for ECHO to transition from high to low
|
|
|
|
// Calculate the distance based on the time computed
|
|
distance = float64(duration.Nanoseconds()) / 10000000 * (d.speedSound / 2)
|
|
|
|
return
|
|
}
|