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