89 lines
1.3 KiB
Go
89 lines
1.3 KiB
Go
package hhio
|
|
|
|
import "context"
|
|
import "errors"
|
|
import "time"
|
|
|
|
import "github.com/stianeikeland/go-rpio/v4"
|
|
|
|
type Ultrasonic struct {
|
|
C chan float64
|
|
|
|
trig rpio.Pin
|
|
echo rpio.Pin
|
|
}
|
|
|
|
var timeout = errors.New("timed out waiting for device")
|
|
|
|
func NewUltrasonic(ctx context.Context, trig, echo int) *Ultrasonic {
|
|
us := &Ultrasonic{
|
|
C: make(chan float64),
|
|
trig: rpio.Pin(trig),
|
|
echo: rpio.Pin(echo),
|
|
}
|
|
|
|
us.trig.Output()
|
|
us.trig.Low()
|
|
|
|
us.echo.Input()
|
|
|
|
go us.loop(ctx)
|
|
|
|
return us
|
|
}
|
|
|
|
func (us *Ultrasonic) loop(ctx context.Context) {
|
|
for {
|
|
select {
|
|
case <-ctx.Done():
|
|
close(us.C)
|
|
return
|
|
|
|
default:
|
|
us.measure()
|
|
}
|
|
}
|
|
}
|
|
|
|
func (us *Ultrasonic) measure() {
|
|
deadline := time.NewTimer(200 * time.Millisecond)
|
|
|
|
us.trigger()
|
|
|
|
start := us.wait(rpio.High, deadline)
|
|
end := us.wait(rpio.Low, deadline)
|
|
|
|
if start.IsZero() || end.IsZero() {
|
|
return
|
|
}
|
|
|
|
duration := end.Sub(start)
|
|
cm := duration.Seconds() * 17150
|
|
|
|
if cm > 400 {
|
|
return
|
|
}
|
|
|
|
us.C <- cm
|
|
}
|
|
|
|
func (us *Ultrasonic) trigger() {
|
|
us.trig.High()
|
|
time.Sleep(10 * time.Microsecond)
|
|
us.trig.Low()
|
|
}
|
|
|
|
func (us *Ultrasonic) wait(goal rpio.State, deadline *time.Timer) time.Time {
|
|
for {
|
|
select {
|
|
case <-deadline.C:
|
|
return time.Time{}
|
|
|
|
default:
|
|
if us.echo.Read() == goal {
|
|
return time.Now()
|
|
}
|
|
}
|
|
}
|
|
}
|