caspr/motion_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/python

from time import sleep, time
from sys import stdout, stdin, argv
from getch import getch

class Motion:
    # left and right are wheel pin numbers
    # (pin0, pin1)
    def __init__(self,gpio, left, right):

        self.sec_per_inch = 0.5
        self.sec_per_degree = 0.1
        self.left = left
        self.right = right
        self.time_start = -1

        self.gpio = gpio
        for w in left + right:
            self.gpio.setup(w,self.gpio.OUT)

        self.stop()


    def rotate_right(self):
        self.stopped = False
        self.gpio.output(self.left[0], False)
        self.gpio.output(self.left[1], True)
        self.gpio.output(self.right[0], True)
        self.gpio.output(self.right[1], False)

    def rotate_left(self):
        self.stopped = False
        self.gpio.output(self.left[0], True)
        self.gpio.output(self.left[1], False)
        self.gpio.output(self.right[0], False)
        self.gpio.output(self.right[1], True)

    def rotate(self, deg=0):
        print "turning", "right" if deg > 0 else "left"

        self.stopped = False
        for w in self.left:
            self.gpio.output(w, (deg > 0))
        for w in self.right:
            self.gpio.output(w, (deg <= 0))

        if abs(deg) > 0:
            sleep(self.sec_per_degree * abs(deg))
            self.stop()

    def forward(self):
        self.time_start = time()
        self.stopped = False
        self.gpio.output(self.left[0], False)
        self.gpio.output(self.left[1], True)
        self.gpio.output(self.right[0], False)
        self.gpio.output(self.right[1], True)

    def back(self):
        self.time_start = time()

        self.stopped = False
        self.gpio.output(self.left[0], True)
        self.gpio.output(self.left[1], False)
        self.gpio.output(self.right[0], True)
        self.gpio.output(self.right[1], False)

    def stop(self):
        self.gpio.output(self.left[0], False)
        self.gpio.output(self.left[1], False)
        self.gpio.output(self.right[0], False)
        self.gpio.output(self.right[1], False)
        self.stopped = True

        self.distance = (time() - self.time_start) * self.sec_per_inch


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

    gpio.setmode(gpio.BCM)
    motion = Motion(gpio, (13, 6), (26, 19))

    print "forward"
    motion.forward()
    sleep(10)
    while True:
        pass
    print "stop"
    motion.stop()