-
Notifications
You must be signed in to change notification settings - Fork 10
/
motor.py
145 lines (128 loc) · 4.02 KB
/
motor.py
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
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
#!/usr/bin/env python
import RPi.GPIO as GPIO
import PCA9685 as p
import time # Import necessary modules
# ===========================================================================
# Raspberry Pi pin11, 12, 13 and 15 to realize the clockwise/counterclockwise
# rotation and forward and backward movements
# ===========================================================================
Motor0_A = 11 # pin11
Motor0_B = 12 # pin12
Motor1_A = 13 # pin13
Motor1_B = 15 # pin15
# ===========================================================================
# Set channel 4 and 5 of the servo driver IC to generate PWM, thus
# controlling the speed of the car
# ===========================================================================
EN_M0 = 4 # servo driver IC CH4
EN_M1 = 5 # servo driver IC CH5
pins = [Motor0_A, Motor0_B, Motor1_A, Motor1_B]
# ===========================================================================
# Adjust the duty cycle of the square waves output from channel 4 and 5 of
# the servo driver IC, so as to control the speed of the car.
# ===========================================================================
def setSpeed(speed):
speed *= 40
print 'speed is: ', speed
pwm.write(EN_M0, 0, speed)
pwm.write(EN_M1, 0, speed)
def setup(busnum=None):
global forward0, forward1, backward1, backward0
global pwm
if busnum == None:
pwm = p.PWM() # Initialize the servo controller.
else:
pwm = p.PWM(bus_number=busnum) # Initialize the servo controller.
pwm.frequency = 60
forward0 = 'True'
forward1 = 'True'
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD) # Number GPIOs by its physical location
try:
for line in open("config"):
if line[0:8] == "forward0":
forward0 = line[11:-1]
if line[0:8] == "forward1":
forward1 = line[11:-1]
except:
pass
if forward0 == 'True':
backward0 = 'False'
elif forward0 == 'False':
backward0 = 'True'
if forward1 == 'True':
backward1 = 'False'
elif forward1 == 'False':
backward1 = 'True'
for pin in pins:
GPIO.setup(pin, GPIO.OUT) # Set all pins' mode as output
# ===========================================================================
# Control the DC motor to make it rotate clockwise, so the car will
# move forward.
# ===========================================================================
def motor0(x):
if x == 'True':
GPIO.output(Motor0_A, GPIO.LOW)
GPIO.output(Motor0_B, GPIO.HIGH)
elif x == 'False':
GPIO.output(Motor0_A, GPIO.HIGH)
GPIO.output(Motor0_B, GPIO.LOW)
else:
print 'Config Error'
def motor1(x):
if x == 'True':
GPIO.output(Motor1_A, GPIO.LOW)
GPIO.output(Motor1_B, GPIO.HIGH)
elif x == 'False':
GPIO.output(Motor1_A, GPIO.HIGH)
GPIO.output(Motor1_B, GPIO.LOW)
def forward():
motor0(forward0)
motor1(forward1)
def backward():
motor0(backward0)
motor1(backward1)
def forwardWithSpeed(spd = 50):
setSpeed(spd)
motor0(forward0)
motor1(forward1)
def backwardWithSpeed(spd = 50):
setSpeed(spd)
motor0(backward0)
motor1(backward1)
def stop():
for pin in pins:
GPIO.output(pin, GPIO.LOW)
# ===========================================================================
# The first parameter(status) is to control the state of the car, to make it
# stop or run. The parameter(direction) is to control the car's direction
# (move forward or backward).
# ===========================================================================
def ctrl(status, direction=1):
if status == 1: # Run
if direction == 1: # Forward
forward()
elif direction == -1: # Backward
backward()
else:
print 'Argument error! direction must be 1 or -1.'
elif status == 0: # Stop
stop()
else:
print 'Argument error! status must be 0 or 1.'
def test():
while True:
setup()
ctrl(1)
time.sleep(3)
setSpeed(10)
time.sleep(3)
setSpeed(100)
time.sleep(3)
ctrl(0)
if __name__ == '__main__':
setup()
setSpeed(50)
#forward()
#backward()
stop()