mirror of https://github.com/kidoman/embd
Merge 0d0e327444
into d3d8c0c5c6
This commit is contained in:
commit
768d583c1d
|
@ -0,0 +1,41 @@
|
|||
// +build ignore
|
||||
|
||||
package main
|
||||
|
||||
import (
|
||||
"flag"
|
||||
"log"
|
||||
"time"
|
||||
|
||||
"github.com/kidoman/embd"
|
||||
|
||||
_ "github.com/kidoman/embd/host/chip"
|
||||
"github.com/kidoman/embd/sensor/hcsr501"
|
||||
)
|
||||
|
||||
func main() {
|
||||
flag.Parse()
|
||||
|
||||
if err := embd.InitGPIO(); err != nil {
|
||||
panic(err)
|
||||
}
|
||||
defer embd.CloseGPIO()
|
||||
|
||||
trig, err := embd.NewDigitalPin(132)
|
||||
if err != nil {
|
||||
panic(err)
|
||||
}
|
||||
defer trig.Close()
|
||||
|
||||
pir := hcsr501.New(trig)
|
||||
|
||||
for {
|
||||
time.Sleep(3 * time.Second)
|
||||
p, err := pir.Detect()
|
||||
if err != nil {
|
||||
log.Printf("error %v", err)
|
||||
continue
|
||||
}
|
||||
log.Printf("PIR Detect %v", p)
|
||||
}
|
||||
}
|
|
@ -0,0 +1,77 @@
|
|||
// Package hcsr501 allows interfacing with the HC-SR501 PIR Sensor.
|
||||
package hcsr501
|
||||
|
||||
import (
|
||||
"errors"
|
||||
"sync"
|
||||
"time"
|
||||
|
||||
"github.com/kidoman/embd"
|
||||
)
|
||||
|
||||
// HCSR501 represents a HCSR501 ultrasonic range finder.
|
||||
type HCSR501 struct {
|
||||
TriggerPin embd.DigitalPin
|
||||
initialized bool
|
||||
ready bool // Allows the sensor time to settle in.
|
||||
mu sync.RWMutex
|
||||
}
|
||||
|
||||
// New creates a new HCSR501 interface.
|
||||
func New(triggerPin embd.DigitalPin) *HCSR501 {
|
||||
return &HCSR501{TriggerPin: triggerPin}
|
||||
}
|
||||
|
||||
// setup initializes the GPIO and sensor.
|
||||
func (d *HCSR501) setup() error {
|
||||
d.mu.RLock()
|
||||
if d.initialized {
|
||||
d.mu.RUnlock()
|
||||
return nil
|
||||
}
|
||||
d.mu.RUnlock()
|
||||
|
||||
d.mu.Lock()
|
||||
defer d.mu.Unlock()
|
||||
|
||||
if err := d.TriggerPin.SetDirection(embd.In); err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
// Wait 60 sec for sensor to settle down.
|
||||
time.AfterFunc(60*time.Second, func() {
|
||||
d.ready = true
|
||||
})
|
||||
d.initialized = true
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
// Detect returns true if motion was detected.
|
||||
func (d *HCSR501) Detect() (bool, error) {
|
||||
if err := d.setup(); err != nil {
|
||||
return false, err
|
||||
}
|
||||
|
||||
if !d.ready {
|
||||
return false, errors.New("Sensor not ready")
|
||||
}
|
||||
|
||||
// Check 3 times to be sure.
|
||||
for i := 0; i < 3; i++ {
|
||||
v, err := d.TriggerPin.Read()
|
||||
if err != nil {
|
||||
return false, err
|
||||
}
|
||||
if v == embd.Low {
|
||||
return false, nil
|
||||
}
|
||||
time.Sleep(10 * time.Millisecond)
|
||||
}
|
||||
return true, nil
|
||||
}
|
||||
|
||||
// Close.
|
||||
func (d *HCSR501) Close() error {
|
||||
return d.TriggerPin.SetDirection(embd.Out)
|
||||
}
|
|
@ -0,0 +1,211 @@
|
|||
// Package hmc5883l allows interfacing with the HMC5883L magnetometer.
|
||||
package hmc5883l
|
||||
|
||||
import (
|
||||
"math"
|
||||
"sync"
|
||||
"time"
|
||||
|
||||
"github.com/golang/glog"
|
||||
"github.com/kidoman/embd"
|
||||
)
|
||||
|
||||
const (
|
||||
magAddress = 0x1E
|
||||
|
||||
// Register addresses.
|
||||
magConfigRegA = 0x00
|
||||
magConfigRegB = 0x01
|
||||
magModeReg = 0x02
|
||||
magMSBx = 0x03
|
||||
magLSBx = 0x04
|
||||
magMSBz = 0x05
|
||||
magLSBz = 0x06
|
||||
magMSBy = 0x07
|
||||
magLSBy = 0x08
|
||||
magStatusReg = 0x09
|
||||
|
||||
// ConfigA Params.
|
||||
MagHz75 = 0x00 // ODR = 0.75 Hz
|
||||
Mag1Hz5 = 0x04 // ODR = 1.5 Hz
|
||||
Mag3Hz = 0x08 // ODR = 3 Hz
|
||||
Mag7Hz5 = 0x0C // ODR = 7.5 Hz
|
||||
Mag15Hz = 0x10 // ODR = 15 Hz
|
||||
Mag30Hz = 0x14 // ODR = 30 Hz
|
||||
Mag75Hz = 0x18 // ODR = 75 Hz
|
||||
MagNormal = 0x00 // Normal mode
|
||||
MagPositiveBias = 0x01 // Positive bias mode
|
||||
MagNegativeBias = 0x02 // Negative bias mode
|
||||
|
||||
MagCRADefault = Mag15Hz | MagNormal // 15 Hz and normal mode is the default
|
||||
|
||||
// ConfigB Params.
|
||||
MagCRBDefault = 0x20 // Gain 1090
|
||||
|
||||
// Mode Reg Params.
|
||||
MagContinuous = 0x00 // Continuous conversion mode
|
||||
MagSingle = 0x01 // Single conversion mode
|
||||
MagSleep = 0x03 // Sleep mode
|
||||
|
||||
MagMRDefault = MagContinuous // Continuous conversion is the default
|
||||
|
||||
pollDelay = 250 // Delay before reading from mag. (ms)
|
||||
)
|
||||
|
||||
type calib struct {
|
||||
minX int16
|
||||
maxX int16
|
||||
minY int16
|
||||
maxY int16
|
||||
}
|
||||
|
||||
// HMC5883L represents a HMC5883L magnetometer.
|
||||
type HMC5883L struct {
|
||||
Bus embd.I2CBus
|
||||
Poll int
|
||||
initialized bool
|
||||
mu sync.RWMutex
|
||||
headings chan float64
|
||||
quit chan struct{}
|
||||
calibData calib
|
||||
}
|
||||
|
||||
// New creates a new HMC5883L interface. The bus variable controls
|
||||
// the I2C bus used to communicate with the device.
|
||||
func New(bus embd.I2CBus) *HMC5883L {
|
||||
return &HMC5883L{Bus: bus, Poll: pollDelay}
|
||||
}
|
||||
|
||||
// Initialize the device
|
||||
func (d *HMC5883L) setup() error {
|
||||
d.mu.RLock()
|
||||
if d.initialized {
|
||||
d.mu.RUnlock()
|
||||
return nil
|
||||
}
|
||||
d.mu.RUnlock()
|
||||
|
||||
d.mu.Lock()
|
||||
defer d.mu.Unlock()
|
||||
|
||||
if err := d.Bus.WriteByteToReg(magAddress, magConfigRegA, MagCRADefault); err != nil {
|
||||
return err
|
||||
}
|
||||
if err := d.Bus.WriteByteToReg(magAddress, magConfigRegB, MagCRBDefault); err != nil {
|
||||
return err
|
||||
}
|
||||
if err := d.Bus.WriteByteToReg(magAddress, magModeReg, MagMRDefault); err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
d.initialized = true
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
func (d *HMC5883L) measureHeading() (float64, error) {
|
||||
if err := d.setup(); err != nil {
|
||||
return 0, err
|
||||
}
|
||||
|
||||
data := make([]byte, 6)
|
||||
if err := d.Bus.ReadFromReg(magAddress, magMSBx, data); err != nil {
|
||||
return 0, err
|
||||
}
|
||||
|
||||
x := int16(data[0])<<8 | int16(data[1])
|
||||
z := int16(data[2])<<8 | int16(data[3])
|
||||
y := int16(data[4])<<8 | int16(data[5])
|
||||
|
||||
/*Note on Calibration:
|
||||
In order to compensate for tilt of compass, it has to be calibrated. To calibrate
|
||||
rotate the compass a full 360'. Then calculate the X and Y offsets as
|
||||
Xoffset = (minX + maxX)/2 ; Yoffset = (minY +maxY)/2
|
||||
when reading the raw values update them by offset
|
||||
Xadj = Xraw - Xoffset
|
||||
Yadj = Yraw - Yoffset
|
||||
*/
|
||||
if x < d.calibData.minX {
|
||||
d.calibData.minX = x
|
||||
}
|
||||
|
||||
if x > d.calibData.maxX {
|
||||
d.calibData.maxX = x
|
||||
}
|
||||
|
||||
if y < d.calibData.minY {
|
||||
d.calibData.minY = y
|
||||
}
|
||||
|
||||
if y > d.calibData.maxY {
|
||||
d.calibData.maxY = y
|
||||
}
|
||||
|
||||
x -= 274
|
||||
y -= 56
|
||||
|
||||
heading := math.Atan2(float64(y), float64(x))
|
||||
heading += 233.9 / 1000
|
||||
if heading < 0 {
|
||||
heading += 2 * math.Pi
|
||||
}
|
||||
|
||||
if heading > 2*math.Pi {
|
||||
heading -= 2 * math.Pi
|
||||
}
|
||||
|
||||
head := heading * 180 / math.Pi
|
||||
|
||||
glog.V(3).Infof("Mag X=%v Y=%v Z=%v HEAD=%v CalibData=%v", x, y, z, head, d.calibData)
|
||||
return head, nil
|
||||
}
|
||||
|
||||
// Heading returns the current heading [0, 360).
|
||||
func (d *HMC5883L) Heading() (float64, error) {
|
||||
select {
|
||||
case heading := <-d.headings:
|
||||
return heading, nil
|
||||
default:
|
||||
glog.V(3).Infof("lsm303: no headings available... measuring")
|
||||
return d.measureHeading()
|
||||
}
|
||||
}
|
||||
|
||||
// Run starts the sensor data acquisition loop.
|
||||
func (d *HMC5883L) Run() error {
|
||||
go func() {
|
||||
d.quit = make(chan struct{})
|
||||
|
||||
timer := time.Tick(time.Duration(d.Poll) * time.Millisecond)
|
||||
|
||||
var heading float64
|
||||
|
||||
for {
|
||||
select {
|
||||
case <-timer:
|
||||
h, err := d.measureHeading()
|
||||
if err == nil {
|
||||
heading = h
|
||||
}
|
||||
if err == nil && d.headings == nil {
|
||||
d.headings = make(chan float64)
|
||||
}
|
||||
case d.headings <- heading:
|
||||
case <-d.quit:
|
||||
d.headings = nil
|
||||
return
|
||||
}
|
||||
|
||||
}
|
||||
}()
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
// Close the sensor data acquisition loop and put the HMC5883L into sleep mode.
|
||||
func (d *HMC5883L) Close() error {
|
||||
if d.quit != nil {
|
||||
d.quit <- struct{}{}
|
||||
}
|
||||
return d.Bus.WriteByteToReg(magAddress, magModeReg, MagSleep)
|
||||
}
|
|
@ -2,6 +2,7 @@
|
|||
package us020
|
||||
|
||||
import (
|
||||
"errors"
|
||||
"sync"
|
||||
"time"
|
||||
|
||||
|
@ -82,24 +83,37 @@ func (d *US020) Distance() (float64, error) {
|
|||
return 0, err
|
||||
}
|
||||
|
||||
glog.V(2).Infof("us020: trigerring pulse")
|
||||
// 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
|
||||
// Generate a TRIGGER pulse.
|
||||
glog.V(2).Infof("us020: trigerring pulse")
|
||||
d.TriggerPin.Write(embd.High)
|
||||
time.Sleep(pulseDelay)
|
||||
d.TriggerPin.Write(embd.Low)
|
||||
|
||||
glog.V(2).Infof("us020: waiting for echo to go high")
|
||||
|
||||
duration, err := d.EchoPin.TimePulse(embd.High)
|
||||
if err != nil {
|
||||
return 0, err
|
||||
// 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
|
||||
}
|
||||
|
||||
// Calculate the distance based on the time computed
|
||||
distance := float64(duration.Nanoseconds()) / 10000000 * (d.speedSound / 2)
|
||||
|
||||
return distance, nil
|
||||
}
|
||||
|
||||
// Close.
|
||||
|
|
Loading…
Reference in New Issue