SelfDrivingPi/motor_test.py at master · luguenth/SelfDrivingPi · 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
import RPi.GPIO as gpio


class HBridge:
    PIN_PWM1 = 12
    PIN_PWM2 = 18

    PIN_MOTOR1P = 23
    PIN_MOTOR1N = 24
    PIN_MOTOR2P = 17
    PIN_MOTOR2N = 22

    def __init__(self):
        gpio.setmode(gpio.BCM)
        self.setupMotors()
        self.setupPWM()
        self.startPWM()

    def drive(self, steer, velo):
        if velo > 0:
            self.motor1_for()
            self.motor2_for()
        elif velo < 0:
            self.motor1_rev()
            self.motor2_rev()
        velo = abs(velo)*100
        steer_l = min(1, 1 - steer)
        steer_r = min(1, 1 + steer)
        print(steer_l*velo, steer_r*velo)
        self.pwml.ChangeDutyCycle(steer_l*velo)
        self.pwmr.ChangeDutyCycle(steer_r*velo)

    def setupMotors(self):
        gpio.setup(self.PIN_MOTOR2P, gpio.OUT)
        gpio.setup(self.PIN_MOTOR2N, gpio.OUT)
        gpio.setup(self.PIN_MOTOR1P, gpio.OUT)
        gpio.setup(self.PIN_MOTOR1N, gpio.OUT)

    def setupPWM(self):
        gpio.setup(self.PIN_PWM2, gpio.OUT)  # right motor
        gpio.setup(self.PIN_PWM1, gpio.OUT)  # left motor
        self.pwml = gpio.PWM(self.PIN_PWM1, 100)
        self.pwmr = gpio.PWM(self.PIN_PWM2, 100)

    def startPWM(self):
        self.pwml.start(0)
        self.pwmr.start(0)

    def stopPWM(self):
        self.pwml.stop(0)
        self.pwmr.stop(0)

    def defineDutyCycle(self, x=100, y=100):
        dc = 70
        if x:
            self.pwml.ChangeDutyCycle(dc)
        if y:
            self.pwmr.ChangeDutyCycle(dc)

    def motor1_rev(self):
        gpio.output(self.PIN_MOTOR1P, True)
        gpio.output(self.PIN_MOTOR1N, False)

    def motor1_for(self):
        gpio.output(self.PIN_MOTOR1P, False)
        gpio.output(self.PIN_MOTOR1N, True)

    def motor2_rev(self):
        gpio.output(self.PIN_MOTOR2P, True)
        gpio.output(self.PIN_MOTOR2N, False)

    def motor2_for(self):
        gpio.output(self.PIN_MOTOR2P, False)
        gpio.output(self.PIN_MOTOR2N, True)

    def shutdown(self):
        self.stopPWM()
        gpio.cleanup()