MusicBox/usprobe.go at master · ClausRN/MusicBox · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
package main

import (
	"time"
	"github.com/stianeikeland/go-rpio"
)

const HardStop = 1000000
const USSensitivity = 5

type USProbe struct {
	EchoPin 	rpio.Pin
	PingPin 	rpio.Pin
	TriggeredUntil 	time.Time
	Timeout		int
        Distance     	float32
	DistanceLow	float32
	DistanceHigh	float32
	DistanceRaw	float32
}

func NewUSProbe(echo    int, // Echo pin
	        ping    int, // Trigger pin
		Timeout int) (result USProbe) {
	if err := rpio.Open(); err != nil {
		panic(err.Error())
	}
	result.EchoPin = rpio.Pin(echo)
	result.PingPin = rpio.Pin(ping)
	result.TriggeredUntil = time.Now().Add(-(time.Duration(1)  * time.Microsecond))
	var first  float32
        var second float32
        var third  float32
 	result.Timeout = Timeout
	first = result.MeasureDistance()
        second = result.MeasureDistance()
        third = result.MeasureDistance()

	result.Distance = (first + second + third)/3
	result.DistanceLow = result.Distance - (result.Distance * float32(USSensitivity)) / float32(100)
        result.DistanceHigh = result.Distance + (result.Distance * float32(USSensitivity)) / float32(100)
	return
}

func (hcsr *USProbe) MeasureDistance() float32 {

	hcsr.EchoPin.Output()
	hcsr.PingPin.Output()

	hcsr.EchoPin.Low()
	hcsr.PingPin.Low()

	hcsr.EchoPin.Input()

	strobeZero := 0
	strobeOne := 0

	// strobe
	hcsr.delayUs(200)
	hcsr.PingPin.High()
	hcsr.delayUs(10)
	hcsr.PingPin.Low()

	// wait until strobe back

	for strobeZero = 0; strobeZero < HardStop && hcsr.EchoPin.Read() != rpio.High; strobeZero++ {
	}
	start := time.Now()
	for strobeOne = 0; strobeOne < HardStop && hcsr.EchoPin.Read() != rpio.Low; strobeOne++ {
		hcsr.delayUs(1)
	}
	end := time.Now()
	hcsr.DistanceRaw = float32(end.UnixNano()-start.UnixNano()) / (58.0 * 1000)
	return hcsr.DistanceRaw
}

func (hcsr *USProbe) delayUs(ms int) {
	time.Sleep(time.Duration(ms) * time.Microsecond)
}

func (hcsr *USProbe) Probe(){
        var first  float32
	first = hcsr.MeasureDistance()
	if (hcsr.DistanceRaw > hcsr.DistanceHigh) || (hcsr.DistanceRaw < hcsr.DistanceLow){
		hcsr.TriggeredUntil = time.Now().Add(time.Duration(hcsr.Timeout) * time.Second)
        	var second float32
	        var third  float32

	        second = hcsr.MeasureDistance()
        	third = hcsr.MeasureDistance()

	        hcsr.Distance = (first + second + third)/3
        	hcsr.DistanceLow = hcsr.Distance - (hcsr.Distance * float32(USSensitivity)) / float32(100)
	        hcsr.DistanceHigh = hcsr.Distance + (hcsr.Distance * float32(USSensitivity)) / float32(100)
	}
}