1 |
#!/usr/bin/python |
1 |
#!/usr/bin/python |
2 |
# ------------------------------------------- |
2 |
# ------------------------------------------- |
3 |
# HBSTEP01B Stepper Motor control test code |
3 |
# HBSTEP01B Stepper Motor control test code |
4 |
# ------------------------------------------- |
4 |
# ------------------------------------------- |
- |
|
5 |
# |
- |
|
6 |
# Program uses MLAB Python modules library from https://github.com/MLAB-project/pymlab |
- |
|
7 |
|
5 |
|
8 |
|
6 |
#uncomment for debbug purposes |
9 |
#uncomment for debbug purposes |
7 |
#import logging |
10 |
#import logging |
8 |
#logging.basicConfig(level=logging.DEBUG) |
11 |
#logging.basicConfig(level=logging.DEBUG) |
9 |
|
12 |
|
10 |
import sys |
13 |
import sys |
11 |
import time |
14 |
import time |
12 |
from pymlab import config |
15 |
from pymlab import config |
13 |
|
16 |
|
14 |
|
17 |
|
15 |
|
18 |
|
16 |
#### Script Arguments ############################################### |
19 |
#### Script Arguments ############################################### |
17 |
|
20 |
|
18 |
if len(sys.argv) < 2: |
21 |
if len(sys.argv) < 2: |
19 |
sys.stderr.write("Invalid number of arguments.\n") |
22 |
sys.stderr.write("Invalid number of arguments.\n") |
20 |
sys.stderr.write("Usage: %s PORT ADDRESS SPEED MOVE_DISTANCE\n" % (sys.argv[0], )) |
23 |
sys.stderr.write("Usage: %s PORT ADDRESS SPEED MOVE_DISTANCE\n" % (sys.argv[0], )) |
21 |
sys.exit(1) |
24 |
sys.exit(1) |
22 |
|
25 |
|
23 |
elif len(sys.argv) == 2: |
26 |
elif len(sys.argv) == 2: |
24 |
PORT = eval(sys.argv[1]) |
27 |
PORT = eval(sys.argv[1]) |
25 |
SPEED = 5 |
28 |
SPEED = 5 |
26 |
DISTANCE = 50 |
29 |
DISTANCE = 50 |
27 |
|
30 |
|
28 |
elif len(sys.argv) > 2: |
31 |
elif len(sys.argv) > 2: |
29 |
SPEED = eval(sys.argv[2]) |
32 |
SPEED = eval(sys.argv[2]) |
30 |
DISTANCE = 100 |
33 |
DISTANCE = 100 |
31 |
|
34 |
|
32 |
elif len(sys.argv) > 3: |
35 |
elif len(sys.argv) > 3: |
33 |
SPEED = eval(sys.argv[2]) |
36 |
SPEED = eval(sys.argv[2]) |
34 |
DISTANCE = eval(sys.argv[3]) |
37 |
DISTANCE = eval(sys.argv[3]) |
35 |
|
38 |
|
36 |
else: |
39 |
else: |
37 |
PORT = 0 |
40 |
PORT = 0 |
38 |
SPEED = 10 |
41 |
SPEED = 10 |
39 |
DISTANCE = 100 |
42 |
DISTANCE = 100 |
40 |
|
43 |
|
41 |
|
44 |
|
42 |
class axis: |
45 |
class axis: |
43 |
def __init__(self, SPI_CS, Direction, StepsPerUnit): |
46 |
def __init__(self, SPI_CS, Direction, StepsPerUnit): |
44 |
' One axis of robot ' |
47 |
' One axis of robot ' |
45 |
self.CS = SPI_CS |
48 |
self.CS = SPI_CS |
46 |
self.Dir = Direction |
49 |
self.Dir = Direction |
47 |
self.SPU = StepsPerUnit |
50 |
self.SPU = StepsPerUnit |
48 |
self.Reset() |
51 |
self.Reset() |
49 |
|
52 |
|
50 |
def Reset(self): |
53 |
def Reset(self): |
51 |
' Reset Axis and set default parameters for H-bridge ' |
54 |
' Reset Axis and set default parameters for H-bridge ' |
52 |
spi.SPI_write_byte(self.CS, 0xC0) # reset |
55 |
spi.SPI_write_byte(self.CS, 0xC0) # reset |
53 |
# spi.SPI_write_byte(self.CS, 0x14) # Stall Treshold setup |
56 |
# spi.SPI_write_byte(self.CS, 0x14) # Stall Treshold setup |
54 |
# spi.SPI_write_byte(self.CS, 0xFF) |
57 |
# spi.SPI_write_byte(self.CS, 0xFF) |
55 |
# spi.SPI_write_byte(self.CS, 0x13) # Over Current Treshold setup |
58 |
# spi.SPI_write_byte(self.CS, 0x13) # Over Current Treshold setup |
56 |
# spi.SPI_write_byte(self.CS, 0xFF) |
59 |
# spi.SPI_write_byte(self.CS, 0xFF) |
57 |
spi.SPI_write_byte(self.CS, 0x15) # Full Step speed |
60 |
spi.SPI_write_byte(self.CS, 0x15) # Full Step speed |
58 |
spi.SPI_write_byte(self.CS, 0xFF) |
61 |
spi.SPI_write_byte(self.CS, 0xFF) |
59 |
spi.SPI_write_byte(self.CS, 0xFF) |
62 |
spi.SPI_write_byte(self.CS, 0xFF) |
60 |
spi.SPI_write_byte(self.CS, 0x05) # ACC |
63 |
spi.SPI_write_byte(self.CS, 0x05) # ACC |
61 |
spi.SPI_write_byte(self.CS, 0x00) |
64 |
spi.SPI_write_byte(self.CS, 0x00) |
62 |
spi.SPI_write_byte(self.CS, 0x10) |
65 |
spi.SPI_write_byte(self.CS, 0x10) |
63 |
spi.SPI_write_byte(self.CS, 0x06) # DEC |
66 |
spi.SPI_write_byte(self.CS, 0x06) # DEC |
64 |
spi.SPI_write_byte(self.CS, 0x00) |
67 |
spi.SPI_write_byte(self.CS, 0x00) |
65 |
spi.SPI_write_byte(self.CS, 0x10) |
68 |
spi.SPI_write_byte(self.CS, 0x10) |
66 |
spi.SPI_write_byte(self.CS, 0x0A) # KVAL_RUN |
69 |
spi.SPI_write_byte(self.CS, 0x0A) # KVAL_RUN |
67 |
spi.SPI_write_byte(self.CS, 0x90) |
70 |
spi.SPI_write_byte(self.CS, 0xFF) |
68 |
spi.SPI_write_byte(self.CS, 0x0B) # KVAL_ACC |
71 |
spi.SPI_write_byte(self.CS, 0x0B) # KVAL_ACC |
69 |
spi.SPI_write_byte(self.CS, 0x90) |
72 |
spi.SPI_write_byte(self.CS, 0xFF) |
70 |
spi.SPI_write_byte(self.CS, 0x0C) # KVAL_DEC |
73 |
spi.SPI_write_byte(self.CS, 0x0C) # KVAL_DEC |
71 |
spi.SPI_write_byte(self.CS, 0x90) |
74 |
spi.SPI_write_byte(self.CS, 0xFF) |
72 |
spi.SPI_write_byte(self.CS, 0x18) # CONFIG |
75 |
spi.SPI_write_byte(self.CS, 0x18) # CONFIG |
73 |
spi.SPI_write_byte(self.CS, 0b00111000) |
76 |
spi.SPI_write_byte(self.CS, 0b00111000) |
74 |
spi.SPI_write_byte(self.CS, 0b00000000) |
77 |
spi.SPI_write_byte(self.CS, 0b00000000) |
75 |
|
78 |
|
76 |
def MaxSpeed(self, speed): |
79 |
def MaxSpeed(self, speed): |
77 |
' Setup of maximum speed ' |
80 |
' Setup of maximum speed ' |
78 |
spi.SPI_write_byte(self.CS, 0x07) # Max Speed setup |
81 |
spi.SPI_write_byte(self.CS, 0x07) # Max Speed setup |
79 |
spi.SPI_write_byte(self.CS, 0x00) |
82 |
spi.SPI_write_byte(self.CS, 0x00) |
80 |
spi.SPI_write_byte(self.CS, speed) |
83 |
spi.SPI_write_byte(self.CS, speed) |
81 |
|
84 |
|
82 |
def ReleaseSW(self): |
85 |
def ReleaseSW(self): |
83 |
' Go away from Limit Switch ' |
86 |
' Go away from Limit Switch ' |
84 |
while self.ReadStatusBit(2) == 1: # is Limit Switch ON ? |
87 |
while self.ReadStatusBit(2) == 1: # is Limit Switch ON ? |
85 |
spi.SPI_write_byte(self.CS, 0x92 | (~self.Dir & 1)) # release SW |
88 |
spi.SPI_write_byte(self.CS, 0x92 | (~self.Dir & 1)) # release SW |
86 |
while self.IsBusy(): |
89 |
while self.IsBusy(): |
87 |
pass |
90 |
pass |
88 |
self.MoveWait(10) # move 10 units away |
91 |
self.MoveWait(10) # move 10 units away |
89 |
|
92 |
|
90 |
def GoZero(self, speed): |
93 |
def GoZero(self, speed): |
91 |
' Go to Zero position ' |
94 |
' Go to Zero position ' |
92 |
self.ReleaseSW() |
95 |
self.ReleaseSW() |
93 |
|
96 |
|
94 |
spi.SPI_write_byte(self.CS, 0x82 | (self.Dir & 1)) # Go to Zero |
97 |
spi.SPI_write_byte(self.CS, 0x82 | (self.Dir & 1)) # Go to Zero |
95 |
spi.SPI_write_byte(self.CS, 0x00) |
98 |
spi.SPI_write_byte(self.CS, 0x00) |
96 |
spi.SPI_write_byte(self.CS, speed) |
99 |
spi.SPI_write_byte(self.CS, speed) |
97 |
while self.IsBusy(): |
100 |
while self.IsBusy(): |
98 |
pass |
101 |
pass |
99 |
time.sleep(0.3) |
102 |
time.sleep(0.3) |
100 |
self.ReleaseSW() |
103 |
self.ReleaseSW() |
101 |
|
104 |
|
102 |
def Move(self, units): |
105 |
def Move(self, units): |
103 |
' Move some distance units from current position ' |
106 |
' Move some distance units from current position ' |
104 |
steps = units * self.SPU # translate units to steps |
107 |
steps = units * self.SPU # translate units to steps |
105 |
if steps > 0: # look for direction |
108 |
if steps > 0: # look for direction |
106 |
spi.SPI_write_byte(self.CS, 0x40 | (~self.Dir & 1)) |
109 |
spi.SPI_write_byte(self.CS, 0x40 | (~self.Dir & 1)) |
107 |
else: |
110 |
else: |
108 |
spi.SPI_write_byte(self.CS, 0x40 | (self.Dir & 1)) |
111 |
spi.SPI_write_byte(self.CS, 0x40 | (self.Dir & 1)) |
109 |
steps = int(abs(steps)) |
112 |
steps = int(abs(steps)) |
110 |
spi.SPI_write_byte(self.CS, (steps >> 16) & 0xFF) |
113 |
spi.SPI_write_byte(self.CS, (steps >> 16) & 0xFF) |
111 |
spi.SPI_write_byte(self.CS, (steps >> 8) & 0xFF) |
114 |
spi.SPI_write_byte(self.CS, (steps >> 8) & 0xFF) |
112 |
spi.SPI_write_byte(self.CS, steps & 0xFF) |
115 |
spi.SPI_write_byte(self.CS, steps & 0xFF) |
113 |
|
116 |
|
114 |
def MoveWait(self, units): |
117 |
def MoveWait(self, units): |
115 |
' Move some distance units from current position and wait for execution ' |
118 |
' Move some distance units from current position and wait for execution ' |
116 |
self.Move(units) |
119 |
self.Move(units) |
117 |
while self.IsBusy(): |
120 |
while self.IsBusy(): |
118 |
pass |
121 |
pass |
119 |
|
122 |
|
120 |
def Float(self): |
123 |
def Float(self): |
121 |
' switch H-bridge to High impedance state ' |
124 |
' switch H-bridge to High impedance state ' |
122 |
spi.SPI_write_byte(self.CS, 0xA0) |
125 |
spi.SPI_write_byte(self.CS, 0xA0) |
123 |
|
126 |
|
124 |
def ReadStatusBit(self, bit): |
127 |
def ReadStatusBit(self, bit): |
125 |
' Report given status bit ' |
128 |
' Report given status bit ' |
126 |
spi.SPI_write_byte(self.CS, 0x39) # Read from address 0x19 (STATUS) |
129 |
spi.SPI_write_byte(self.CS, 0x39) # Read from address 0x19 (STATUS) |
127 |
spi.SPI_write_byte(self.CS, 0x00) |
130 |
spi.SPI_write_byte(self.CS, 0x00) |
128 |
data0 = spi.SPI_read_byte() # 1st byte |
131 |
data0 = spi.SPI_read_byte() # 1st byte |
129 |
spi.SPI_write_byte(self.CS, 0x00) |
132 |
spi.SPI_write_byte(self.CS, 0x00) |
130 |
data1 = spi.SPI_read_byte() # 2nd byte |
133 |
data1 = spi.SPI_read_byte() # 2nd byte |
131 |
#print hex(data0), hex(data1) |
134 |
#print hex(data0), hex(data1) |
132 |
if bit > 7: # extract requested bit |
135 |
if bit > 7: # extract requested bit |
133 |
OutputBit = (data0 >> (bit - 8)) & 1 |
136 |
OutputBit = (data0 >> (bit - 8)) & 1 |
134 |
else: |
137 |
else: |
135 |
OutputBit = (data1 >> bit) & 1 |
138 |
OutputBit = (data1 >> bit) & 1 |
136 |
return OutputBit |
139 |
return OutputBit |
137 |
|
140 |
|
138 |
|
141 |
|
139 |
def IsBusy(self): |
142 |
def IsBusy(self): |
140 |
""" Return True if tehre are motion """ |
143 |
""" Return True if tehre are motion """ |
141 |
if self.ReadStatusBit(1) == 1: |
144 |
if self.ReadStatusBit(1) == 1: |
142 |
return False |
145 |
return False |
143 |
else: |
146 |
else: |
144 |
return True |
147 |
return True |
145 |
|
148 |
|
146 |
# End Class axis -------------------------------------------------- |
149 |
# End Class axis -------------------------------------------------- |
147 |
|
150 |
|
148 |
|
151 |
|
149 |
|
152 |
|
150 |
cfg = config.Config( |
153 |
cfg = config.Config( |
151 |
i2c = { |
154 |
i2c = { |
152 |
"port": 1, |
155 |
"port": 1, |
153 |
}, |
156 |
}, |
154 |
|
157 |
|
155 |
bus = [ |
158 |
bus = [ |
156 |
{ |
159 |
{ |
157 |
"name":"spi", |
160 |
"name":"spi", |
158 |
"type":"i2cspi", |
161 |
"type":"i2cspi", |
159 |
"address": 0x2e, |
162 |
"address": 0x2e, |
160 |
}, |
163 |
}, |
161 |
], |
164 |
], |
162 |
) |
165 |
) |
163 |
|
166 |
|
164 |
|
167 |
|
165 |
cfg.initialize() |
168 |
cfg.initialize() |
166 |
|
169 |
|
167 |
print "Stepper motor control test started. \r\n" |
170 |
print "Stepper motor control test started. \r\n" |
168 |
|
171 |
|
169 |
spi = cfg.get_device("spi") |
172 |
spi = cfg.get_device("spi") |
170 |
|
173 |
|
171 |
spi.route() |
174 |
spi.route() |
172 |
|
175 |
|
173 |
try: |
176 |
try: |
174 |
print "SPI configuration.." |
177 |
print "SPI configuration.." |
175 |
spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz) |
178 |
spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz) |
176 |
time.sleep(1) |
179 |
time.sleep(1) |
177 |
|
180 |
|
178 |
print "Axis inicialization" |
181 |
print "Axis inicialization" |
179 |
X = axis(spi.I2CSPI_SS0, 0, 641) # set Number of Steps per axis Unit and set Direction of Rotation |
182 |
X = axis(spi.I2CSPI_SS0, 0, 641) # set Number of Steps per axis Unit and set Direction of Rotation |
180 |
X.MaxSpeed(SPEED) # set maximal motor speed |
183 |
X.MaxSpeed(SPEED) # set maximal motor speed |
181 |
|
184 |
|
182 |
print "Axis is running" |
185 |
print "Axis is running" |
183 |
|
186 |
|
184 |
for i in range(5): |
187 |
for i in range(5): |
185 |
X.MoveWait(DISTANCE) # move forward and wait for motor stop |
188 |
X.MoveWait(DISTANCE) # move forward and wait for motor stop |
186 |
X.MoveWait(-DISTANCE) # move backward and wait for motor stop |
189 |
X.MoveWait(-DISTANCE) # move backward and wait for motor stop |
187 |
|
190 |
|
188 |
X.Float() # release power |
191 |
X.Float() # release power |
189 |
|
192 |
|
190 |
|
193 |
|
191 |
finally: |
194 |
finally: |
192 |
print "stop" |
195 |
print "stop" |