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