mirror of https://github.com/kidoman/embd
123 lines
2.5 KiB
Go
123 lines
2.5 KiB
Go
// Package us020 allows interfacing with the US020 ultrasonic range finder.
|
|
package us020
|
|
|
|
import (
|
|
"errors"
|
|
"sync"
|
|
"time"
|
|
|
|
"github.com/golang/glog"
|
|
"github.com/kidoman/embd"
|
|
)
|
|
|
|
const (
|
|
pulseDelay = 30000 * time.Nanosecond
|
|
defaultTemp = 25
|
|
)
|
|
|
|
type Thermometer interface {
|
|
Temperature() (float64, error)
|
|
}
|
|
|
|
type nullThermometer struct {
|
|
}
|
|
|
|
func (*nullThermometer) Temperature() (float64, error) {
|
|
return defaultTemp, nil
|
|
}
|
|
|
|
var NullThermometer = &nullThermometer{}
|
|
|
|
// US020 represents a US020 ultrasonic range finder.
|
|
type US020 struct {
|
|
EchoPin, TriggerPin embd.DigitalPin
|
|
|
|
Thermometer Thermometer
|
|
|
|
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(echoPin, triggerPin embd.DigitalPin, thermometer Thermometer) *US020 {
|
|
return &US020{EchoPin: echoPin, TriggerPin: triggerPin, Thermometer: thermometer}
|
|
}
|
|
|
|
func (d *US020) setup() error {
|
|
d.mu.RLock()
|
|
if d.initialized {
|
|
d.mu.RUnlock()
|
|
return nil
|
|
}
|
|
d.mu.RUnlock()
|
|
|
|
d.mu.Lock()
|
|
defer d.mu.Unlock()
|
|
|
|
d.TriggerPin.SetDirection(embd.Out)
|
|
d.EchoPin.SetDirection(embd.In)
|
|
|
|
if d.Thermometer == nil {
|
|
d.Thermometer = NullThermometer
|
|
}
|
|
|
|
if temp, err := d.Thermometer.Temperature(); err == nil {
|
|
d.speedSound = 331.3 + 0.606*temp
|
|
|
|
glog.V(1).Infof("us020: read a temperature of %v, so speed of sound = %v", temp, d.speedSound)
|
|
} else {
|
|
d.speedSound = 340
|
|
}
|
|
|
|
d.initialized = true
|
|
|
|
return nil
|
|
}
|
|
|
|
// Distance computes the distance of the bot from the closest obstruction.
|
|
func (d *US020) Distance() (float64, error) {
|
|
if err := d.setup(); err != nil {
|
|
return 0, err
|
|
}
|
|
|
|
// Ready the goroutine to measure return pulse.
|
|
done := make(chan time.Duration)
|
|
errChan := make(chan error)
|
|
go func() {
|
|
glog.V(2).Infof("us020: waiting for echo to go high")
|
|
duration, err := d.EchoPin.TimePulse(embd.High)
|
|
if err != nil {
|
|
errChan <- err
|
|
return
|
|
}
|
|
done <- duration
|
|
}()
|
|
|
|
// Generate a TRIGGER pulse.
|
|
glog.V(2).Infof("us020: trigerring pulse")
|
|
d.TriggerPin.Write(embd.High)
|
|
time.Sleep(pulseDelay)
|
|
d.TriggerPin.Write(embd.Low)
|
|
|
|
// Wait for data on channel or timeout.
|
|
t := time.NewTimer(time.Millisecond * 200)
|
|
select {
|
|
case <-t.C:
|
|
return 0, errors.New("timeout on ultrasonic pulse")
|
|
case duration := <-done:
|
|
distance := float64(duration.Nanoseconds()) / 10000000 * (d.speedSound / 2)
|
|
return distance, nil
|
|
case e := <-errChan:
|
|
return 0, e
|
|
}
|
|
|
|
}
|
|
|
|
// Close.
|
|
func (d *US020) Close() error {
|
|
return d.EchoPin.SetDirection(embd.Out)
|
|
}
|