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