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