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