Rev 4448 Rev 4449
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"