forked from akkana/scripts
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathpimotors.py
More file actions
executable file
·149 lines (124 loc) · 4.04 KB
/
pimotors.py
File metadata and controls
executable file
·149 lines (124 loc) · 4.04 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
#!/usr/bin/env python
# Control 2 DC motors from a Raspberry Pi
# through a SN754410 quad half H-bridge chip.
# Copyright 2013 by Akkana Peck -- share and enjoy under the GPL v1 or later.
import RPi.GPIO as GPIO
import time
class PiMotor:
def __init__(self, pwm, c0, c1):
self.pwm_pin = pwm
self.logic_pins = [c0, c1]
self.pwm = None
# We'll set these again in init_pins, but let's be sure:
self.direc = None
self.speed = 0
def init_pins(self):
'''Call GPIO.setup on all the pins we'll be using,
and set the PWM pin to PWM mode.
'''
# Set both logic pins to output mode, initially LOW:
for pin in self.logic_pins:
print "Setting up", pin, "for output"
GPIO.setup(pin, GPIO.OUT)
GPIO.output(pin, 0)
# Set up the PWM pin:
print "Setting up", self.pwm_pin, "for PWM"
GPIO.setup(self.pwm_pin, GPIO.OUT)
self.pwm = GPIO.PWM(self.pwm_pin, 50)
self.pwm.start(0)
# Initially we have no direction and no speed.
self.direc = None
self.speed = 0
def stop(self):
for pin in self.logic_pins:
GPIO.output(pin, 0)
if not self.pwm:
return
self.pwm.ChangeDutyCycle(0)
self.pwm.stop()
self.direc = None
self.speed = 0
print self.pwm_pin, "Stopped"
def set_direction(self, direc):
'''Change the current direction.
None will set both pins to 0;
0 will set the first pin to 1 and the second to 0,
1 will do the reverse.
Users shouldn't have to call this directly;
use set_speed() with positive or negative values.
'''
if direc == self.direc:
return
self.pwm.ChangeDutyCycle(0)
for pin in self.logic_pins:
GPIO.output(pin, 0)
if direc == 0 or direc == 1:
print "Setting direction", direc
GPIO.output(self.logic_pins[direc], 1)
else:
print "Direction none"
self.direc = direc
# Don't try to set the speed back to what it was before.
# Put the responsibility on the caller to ramp up at a sane rate.
def set_speed(self, speed):
'''Set the speed, represented as a percentage of max
(a number between 0 and 100).
This is actually the duty cycle for the PWM pin.
'''
if not self.pwm:
return
self.speed = speed
if speed == 0:
self.set_direction(None)
elif speed < 0:
self.set_direction(1)
else:
self.set_direction(0)
self.pwm.ChangeDutyCycle(abs(speed))
if __name__ == '__main__':
# GPIO pins you can't use (this is from the Pi1 days, may have changed)::
# 14-15: used for serial console
# 9-10: SPI0_MOSI and SPI0_MISO
# 23, 25: not used for anything but we can't use them anyway.
# It looks like the pins we CAN use are:
# top row: 18, 23, 24, 25, 8, 7
# bottom row: 4, 17, 22
#motors = [ PiMotor(22, 9, 10), PiMotor(24, 7, 8) ]
#motors = [ PiMotor(22, 23, 25), PiMotor(24, 7, 8) ]
# Better for Pi plate, since it doesn't expose 7 and 8:
motors = [ PiMotor(25, 23, 24), PiMotor(17, 21, 22) ]
print "Cleaning up GPIO"
GPIO.cleanup()
GPIO.setmode(GPIO.BCM)
for m in motors:
m.init_pins()
# Drive straight:
print "Driving straight"
for m in motors:
m.set_speed(30)
time.sleep(2)
# circle in one direction
print "Circling"
motors[0].set_speed(-20)
motors[1].set_speed(80)
time.sleep(2)
# circle in the other direction
print "Circling the other way"
motors[0].set_speed(80)
motors[1].set_speed(-20)
time.sleep(2)
# stop
print "Stop"
for m in motors:
m.set_speed(0)
time.sleep(2)
# Back up
print "Backing up"
for m in motors:
m.set_speed(-30)
time.sleep(2)
# stop
print "Stop"
for m in motors:
m.stop()
GPIO.cleanup()