collab/pb-hal

git clone https://git.julia.blue/collab/pb-hal

Rahul ButaniMiscf140848

imu
3.7 KiB129 linesraw
1import RPIO
2from RPIO import PWM
3from math import copysign
4
5TOP_PIN_1 = 23
6TOP_PIN_2 = 24
7TOP_PIN_PWM = 3
8TOP_ENC_A = 0
9TOP_ENC_B = 0
10
11LEFT_PIN_1 = 7
12LEFT_PIN_2 = 8
13LEFT_PIN_PWM = 2
14LEFT_ENC_A = 0
15LEFT_ENC_B = 0
16
17BOTTOM_PIN_1 = 11
18BOTTOM_PIN_2 = 25
19BOTTOM_PIN_PWM = 18
20BOTTOM_ENC_A = 0
21BOTTOM_ENC_B = 0
22
23RIGHT_PIN_1 = 27
24RIGHT_PIN_2 = 14
25RIGHT_PIN_PWM = 17
26RIGHT_ENC_A = 0
27RIGHT_ENC_B = 0
28
29RPIO.cleanup()
30PWM.cleanup()
31
32class Motor:
33    def __init__(self, pin1, pin2, pwm_pin, encA, encB, dma_channel=0):
34        self.dma_channel = dma_channel
35        self.pwm_pin = pwm_pin
36        self.pin1 = pin1
37        self.pin2 = pin2
38        self.encA = encA
39        self.encB = encB
40        self.speed = 0
41        self.ticks = 0
42        # Set up gpio pins, encoder interupts, and dma
43        PWM.init_channel(dma_channel, subcycle_time_us=8000)
44        RPIO.setup(pwm_pin, RPIO.OUT)
45        RPIO.setup(pin1, RPIO.OUT)
46        RPIO.setup(pin2, RPIO.OUT)
47        RPIO.setup(encA, RPIO.IN)
48        RPIO.setup(encB, RPIO.IN)
49        #  RPIO.add_interrupt_callback(
50        #      encA,
51        #      self.encCallback,
52        #      edge='rising',
53        #      threaded_callback=False)  # todo: see if this makes a difference
54
55    def encCallback(self, gpioID, val):
56        temp = RPIO.input(self.encB)
57        if temp:
58            self.ticks += 1
59        else:
60            self.ticks += 1
61        #  print(self.ticks)
62
63    def set(self, speed):
64        """manipulates hardware to turn at given speed in range [-1,1] (positive
65        should be clockwise)"""
66        # todo: verify that positive is clockwise for each motor
67        mysign = copysign(1, self.speed)
68        newsign = copysign(1, speed)
69        self.speed = speed
70        if mysign != newsign:
71            if newsign == 0:
72                RPIO.output(self.pin1, False)
73                RPIO.output(self.pin2, False)
74            elif newsign == -1:
75                RPIO.output(self.pin1, False)
76                RPIO.output(self.pin2, True)
77            elif newsign == 1:
78                RPIO.output(self.pin2, False)
79                RPIO.output(self.pin1, True)
80        self.set_speed(abs(self.speed))
81
82    def set_speed(self, speed):
83        """adds a pwm pulse for the given speed in range [-1,1]"""
84        PWM.add_channel_pulse(self.dma_channel, self.pwm_pin, start=0, width=int(
85            speed * PWM.get_channel_subcycle_time_us(self.dma_channel) / 10))
86
87    def __str__(self):
88        return "Speed: " + str(self.speed) + "\t\tTicks: " + str(self.ticks)
89
90
91class Base:
92
93    def __init__(self):
94        #  self.disable()   # is this needed?
95        PWM.setup()
96        self.top = Motor(TOP_PIN_1, TOP_PIN_2, TOP_PIN_PWM, TOP_ENC_A, TOP_ENC_B, 0)
97        self.bottom = Motor(BOTTOM_PIN_1, BOTTOM_PIN_2, BOTTOM_ENC_A, BOTTOM_ENC_B,
98                BOTTOM_PIN_PWM, 1)
99        self.left = Motor(LEFT_PIN_1, LEFT_PIN_2, LEFT_PIN_PWM, LEFT_ENC_A,
100                LEFT_ENC_B, 2)
101        self.right = Motor(RIGHT_PIN_1, RIGHT_PIN_2, RIGHT_PIN_PWM, RIGHT_ENC_A,
102                RIGHT_ENC_B, 3)
103
104    def move(self, x, y):
105        """sets motors to move linearly with x and y in range [-1,1]"""
106        self.top.set(x)
107        self.bottom.set(-x)
108        self.left.set(y)
109        self.right.set(-y)
110
111    def rotate(self, speed):
112        """turns at given speed in range [-1,1]"""
113        self.top.set(speed)
114        self.bottom.set(speed)
115        self.left.set(speed)
116        self.right.set(speed)
117
118    def stop(self):
119        self.move(0, 0)
120
121    def __str__(self):
122        return "Top: " + str(self.top) + "\nBottom: " + str(self.bottom) \
123            + "\nLeft: " + str(self.left) + "\nRight: " + str(self.right)
124
125    def __del__(self):
126        self.stop()
127        PWM.cleanup()
128        RPIO.cleanup()
129