-
Notifications
You must be signed in to change notification settings - Fork 0
/
move.py
161 lines (145 loc) · 5.03 KB
/
move.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
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
#!/usr/bin/env python
# This code is written by Stephen C Phillips.
# It is in the public domain, so you can do what you like with it
# but a link to http://scphillips.com would be nice.
# It works on the Raspberry Pi computer with the standard Debian Wheezy OS and
# the 28BJY-48 stepper motor with ULN2003 control board.
from time import sleep
import RPi.GPIO as GPIO
class Motor(object):
def __init__(self, pins, mode=3):
"""Initialise the motor object.
pins -- a list of 4 integers referring to the GPIO pins that the IN1, IN2
IN3 and IN4 pins of the ULN2003 board are wired to
mode -- the stepping mode to use:
1: wave drive (not yet implemented)
2: full step drive
3: half step drive (default)
"""
self.P1 = pins[0]
self.P2 = pins[1]
self.P3 = pins[2]
self.P4 = pins[3]
self.mode = mode
self.deg_per_step = 5.625 / 64 # for half-step drive (mode 3)
self.steps_per_rev = int(360 / self.deg_per_step) # 4096
self.step_angle = 0 # Assume the way it is pointing is zero degrees
for p in pins:
GPIO.setup(p, GPIO.OUT)
GPIO.output(p, 0)
def _set_rpm(self, rpm):
"""Set the turn speed in RPM."""
self._rpm = rpm
# T is the amount of time to stop between signals
self._T = (60.0 / rpm) / self.steps_per_rev
# This means you can set "rpm" as if it is an attribute and
# behind the scenes it sets the _T attribute
rpm = property(lambda self: self._rpm, _set_rpm)
def move_to(self, angle):
"""Take the shortest route to a particular angle (degrees)."""
# Make sure there is a 1:1 mapping between angle and stepper angle
target_step_angle = 8 * (int(angle / self.deg_per_step) / 8)
steps = target_step_angle - self.step_angle
steps = (steps % self.steps_per_rev)
if steps > self.steps_per_rev / 2:
steps -= self.steps_per_rev
print "moving " + `steps` + " steps"
if self.mode == 2:
self._move_acw_2(-steps / 8)
else:
self._move_acw_3(-steps / 8)
else:
print "moving " + `steps` + " steps"
if self.mode == 2:
self._move_cw_2(steps / 8)
else:
self._move_cw_3(steps / 8)
self.step_angle = target_step_angle
def __clear(self):
GPIO.output(self.P1, 0)
GPIO.output(self.P2, 0)
GPIO.output(self.P3, 0)
GPIO.output(self.P4, 0)
def _move_acw_2(self, big_steps):
self.__clear()
for i in range(big_steps):
GPIO.output(self.P3, 0)
GPIO.output(self.P1, 1)
sleep(self._T * 2)
GPIO.output(self.P2, 0)
GPIO.output(self.P4, 1)
sleep(self._T * 2)
GPIO.output(self.P1, 0)
GPIO.output(self.P3, 1)
sleep(self._T * 2)
GPIO.output(self.P4, 0)
GPIO.output(self.P2, 1)
sleep(self._T * 2)
def _move_cw_2(self, big_steps):
self.__clear()
for i in range(big_steps):
GPIO.output(self.P4, 0)
GPIO.output(self.P2, 1)
sleep(self._T * 2)
GPIO.output(self.P1, 0)
GPIO.output(self.P3, 1)
sleep(self._T * 2)
GPIO.output(self.P2, 0)
GPIO.output(self.P4, 1)
sleep(self._T * 2)
GPIO.output(self.P3, 0)
GPIO.output(self.P1, 1)
sleep(self._T * 2)
def _move_acw_3(self, big_steps):
self.__clear()
for i in range(big_steps):
GPIO.output(self.P1, 0)
sleep(self._T)
GPIO.output(self.P3, 1)
sleep(self._T)
GPIO.output(self.P4, 0)
sleep(self._T)
GPIO.output(self.P2, 1)
sleep(self._T)
GPIO.output(self.P3, 0)
sleep(self._T)
GPIO.output(self.P1, 1)
sleep(self._T)
GPIO.output(self.P2, 0)
sleep(self._T)
GPIO.output(self.P4, 1)
sleep(self._T)
def _move_cw_3(self, big_steps):
self.__clear()
for i in range(big_steps):
GPIO.output(self.P3, 0)
sleep(self._T)
GPIO.output(self.P1, 1)
sleep(self._T)
GPIO.output(self.P4, 0)
sleep(self._T)
GPIO.output(self.P2, 1)
sleep(self._T)
GPIO.output(self.P1, 0)
sleep(self._T)
GPIO.output(self.P3, 1)
sleep(self._T)
GPIO.output(self.P2, 0)
sleep(self._T)
GPIO.output(self.P4, 1)
sleep(self._T)
if __name__ == "__main__":
GPIO.setmode(GPIO.BOARD)
m = Motor([18,22,24,26])
#m = Motor([10,9,11,25])
m.rpm = 5
print "Pause in seconds: " + `m._T`
m.move_to(0)
sleep(1)
m.move_to(90)
sleep(10)
m.mode = 2
m.move_to(0)
#sleep(1)
#m.move_to(0)
GPIO.cleanup()