caspr/vision_controller.py at master · DrPresident/caspr · 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
#!/usr/bin/env python

from time import time, sleep
from sys import stdout
import threading

class Vision:
    def __init__(self,gpio,trigger,echo):
        self.trigger = trigger
        self.echo    = echo

        self.dist = -1

        self.pulse_timeout  = .1
        self.pulse_interval = .5
        self.no_hit = float("inf")

        self.updated = False

        self.gpio = gpio
        self.gpio.setup(self.trigger,self.gpio.OUT)
        self.gpio.setup(self.echo,self.gpio.IN)

        self.gpio.output(self.trigger,False)

        self.on_pulse = list()
        self.on_update = list()

    # force fresh data
    def refresh(self):
        updated = False
        return self.pulse()

    def avg_pulse(self, n=3):
        return sum(self.pulse() for p in xrange(n)) / n

    def pulse(self):
        for f in self.on_pulse:
            f(self)

        self.gpio.output(self.trigger, True)
        sleep(0.0001)
        self.gpio.output(self.trigger, False)
        pulse_start = -1
        pulse_end = -1
        timeout = False

        check_start = time()

        while self.gpio.input(self.echo) == 0 and not timeout:
            pulse_start = time()
            if pulse_start - check_start > self.pulse_timeout:
                timeout = True

        while self.gpio.input(self.echo) == 1 and not timeout:
            pulse_end = time()
            if pulse_end - pulse_start > self.pulse_timeout:
                timeout = True

        if timeout:
            self.dist = -1
        else:
            self.dist = 17150 * (pulse_end - pulse_start)
        updated = True
        for f in self.on_update:
            f(self)

        return self.dist

    def run(self):
        while True:
            self.dist = self.pulse()
            sleep(self.pulse_interval)

    def start(self):
        self.daemon = threading.Thread(target=self.run)
        self.daemon.setDaemon(True)
        self.daemon.start()


if __name__ == "__main__":
    import RPi.GPIO as gpio

    gpio.setmode(gpio.BCM)
    left = Vision(gpio, 22, 27)
    center = Vision(gpio, 17, 4)
    right = Vision(gpio, 9, 10)

    while True:
        print round(left.pulse(), 2), round(center.pulse(), 2), round(right.pulse(), 2)
        sleep(1)