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