-
Notifications
You must be signed in to change notification settings - Fork 1
Expand file tree
/
Copy pathmotorControl.py
More file actions
190 lines (150 loc) · 5.93 KB
/
motorControl.py
File metadata and controls
190 lines (150 loc) · 5.93 KB
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
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
#!/usr/bin/env python
# Motor Control for Raspberry Pi with MC33886 Motor Driver Board
# Product: http://www.robotshop.com/en/mc33886-raspberry-pi-motor-driver-board-raspberry-pi.html
# Datasheet: http://www.robotshop.com/media/files/pdf2/rpi_motor_driver_board_-_waveshare_wiki.pdf
# Schematic: http://www.robotshop.com/media/files/pdf2/rpi-motor-driver-board-schematic.pdf
'''
On the Motor Driver board, M1 and M2 are connected to the right motor while
M3 and M4 are connected to the left motor. The orientation of the actual
(black or red wire) doesn't matter when connected to the motor driver board.
Motors are interally controlled by outputting signals through GPIO.
PWMA and PWMB are output enable pins. When driven high, the PWM pulse will be
outputted from the motor pins to control the speed of the motor.
Interface Pin name
-------------------------------------------------------------------------------
Right motor: M1 --> _RIGHT_MOTOR_1
M2 --> _RIGHT_MOTOR_2
PWMA --> _RIGHT_MOTOR_PWM_SPEED
Left motor: M3 --> _LEFT_MOTOR_1
M4 --> _LEFT_MOTOR_2
PWMB --> _LEFT_MOTOR_PWM_SPEED
The orientation of the driver board taken from the data sheet
Left motor Right motor
M3 M4 M1 M2
'''
#-------------------------------------------------------------------------------
#### Imports ####
import time
import RPi.GPIO as GPIO
#### Constants ####
#-------------------------------------------------------------------------------
# GPIO Mode (BOARD/BCM)
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)
# Assign GPIO pins for motors
# Right motor
_RIGHT_MOTOR_1 = 38
_RIGHT_MOTOR_2 = 40
# Left motor
_LEFT_MOTOR_1 = 31
_LEFT_MOTOR_2 = 33
# Output enable pins (active high enable)
# Right motor
_RIGHT_MOTOR_PWM_SPEED = 37
# Left motor
_LEFT_MOTOR_PWM_SPEED = 32
# Step duration is used for forward and backward commands.
_STEP_DURATION = 1000
# Controls motor strength
_MOTOR_SPEED = 1000
# Turn duration in milliseconds.
_TURN_DURATION = 1800
#-------------------------------------------------------------------------------
# Set up the motor GPIO pins
GPIO.setup(_RIGHT_MOTOR_1, GPIO.OUT)
GPIO.setup(_RIGHT_MOTOR_2, GPIO.OUT)
GPIO.setup(_LEFT_MOTOR_1, GPIO.OUT)
GPIO.setup(_LEFT_MOTOR_2, GPIO.OUT)
GPIO.setup(_RIGHT_MOTOR_PWM_SPEED, GPIO.OUT)
GPIO.setup(_LEFT_MOTOR_PWM_SPEED, GPIO.OUT)
right_motor = GPIO.PWM(_RIGHT_MOTOR_PWM_SPEED, 1400)
left_motor = GPIO.PWM(_LEFT_MOTOR_PWM_SPEED, _MOTOR_SPEED)
right_motor.start(50)
left_motor.start(50)
# Initially "Turn off" all motors
GPIO.output(_RIGHT_MOTOR_1, False)
GPIO.output(_RIGHT_MOTOR_2, False)
GPIO.output(_LEFT_MOTOR_1, False)
GPIO.output(_LEFT_MOTOR_2, False)
#### Objects ####
#-------------------------------------------------------------------------------
class MotorControl(object):
# Make sure there is only one instance of MotorControl
_instances=[]
# Initialize the object
def __init__(self):
if ( len(self._instances)>1 ):
print("ERROR: One instance of MotorControl is running already.")
exit(1)
self._instances.append(self)
#-------------------------------------------------------------------------------
# Move BACKWARD
def backward(self):
#print("Backward")
GPIO.output(_RIGHT_MOTOR_1, False)
GPIO.output(_RIGHT_MOTOR_2, True)
GPIO.output(_LEFT_MOTOR_1, True)
GPIO.output(_LEFT_MOTOR_2, False)
'''
time.sleep (_STEP_DURATION/1000)
GPIO.output(_RIGHT_MOTOR_1, False)
GPIO.output(_LEFT_MOTOR_2, False)
'''
#-------------------------------------------------------------------------------
# Move FORWARD
def forward(self):
#print("Forward")
GPIO.output(_RIGHT_MOTOR_1, True)
GPIO.output(_RIGHT_MOTOR_2, False)
GPIO.output(_LEFT_MOTOR_1, False)
GPIO.output(_LEFT_MOTOR_2, True)
'''
time.sleep (_STEP_DURATION/1000)
GPIO.output(_RIGHT_MOTOR_2, False)
GPIO.output(_LEFT_MOTOR_1, False)
'''
#-------------------------------------------------------------------------------
# Move LEFT
def left(self):
#print("left")
GPIO.output(_RIGHT_MOTOR_1, True)
GPIO.output(_RIGHT_MOTOR_2, False)
GPIO.output(_LEFT_MOTOR_1, True)
GPIO.output(_LEFT_MOTOR_2, False)
'''
time.sleep (_TURN_DURATION/1000)
GPIO.output(_RIGHT_MOTOR_1, False)
GPIO.output(_LEFT_MOTOR_1, False)
'''
#-------------------------------------------------------------------------------
# Move RIGHT
def right(self):
#print("right")
GPIO.output(_RIGHT_MOTOR_1, False)
GPIO.output(_RIGHT_MOTOR_2, True)
GPIO.output(_LEFT_MOTOR_1, False)
GPIO.output(_LEFT_MOTOR_2, True)
'''
time.sleep (_TURN_DURATION/1000)
GPIO.output(_RIGHT_MOTOR_2, False)
GPIO.output(_LEFT_MOTOR_1, False)
'''
#-------------------------------------------------------------------------------
# STOP all the motors
def stop(self):
GPIO.output(_RIGHT_MOTOR_1, False)
GPIO.output(_RIGHT_MOTOR_2, False)
GPIO.output(_LEFT_MOTOR_1, False)
GPIO.output(_LEFT_MOTOR_2, False)
#-------------------------------------------------------------------------------
# Change right motor PWM frequency in Hz
def changeRightPWM(self, frequency):
right_motor.ChangeFrequency(frequency)
#-------------------------------------------------------------------------------
# Change left motor PWM frequency in Hz
def changeLeftPWM(self, frequency):
left_motor.ChangeFrequency(frequency)
#-------------------------------------------------------------------------------
# Properly clean up GPIO
def cleanup (self):
GPIO.cleanup()