Rev 4570 Rev 4573
Line 38... Line 38...
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, MaxSpeed):
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.maxseed = MaxSpeed
48 self.Reset() 49 self.Reset()
-   50 self.Initialize()
49   51  
50 def Reset(self): 52 def Reset(self):
51 ' Reset Axis and set default parameters for H-bridge ' 53 'Reset the Axis'
52 self.spi.xfer([0xC0]) # reset 54 self.spi.xfer([0xC0]) # reset
-   55  
-   56 def Initialize(self):
-   57 'set default parameters for H-bridge '
53 # self.spi.xfer( 0x14) # Stall Treshold setup 58 # self.spi.xfer( 0x14) # Stall Treshold setup
54 # self.spi.xfer( 0xFF) 59 # self.spi.xfer( 0xFF)
55 # self.spi.xfer( 0x13) # Over Current Treshold setup 60 # self.spi.xfer( 0x13) # Over Current Treshold setup
56 # self.spi.xfer( 0xFF) 61 # self.spi.xfer( 0xFF)
57 self.spi.xfer([0x15]) # Full Step speed 62 self.spi.xfer([0x15]) # Full Step speed
Line 62... Line 67...
62 self.spi.xfer([0x10]) 67 self.spi.xfer([0x10])
63 self.spi.xfer([0x06]) # DEC 68 self.spi.xfer([0x06]) # DEC
64 self.spi.xfer([0x00]) 69 self.spi.xfer([0x00])
65 self.spi.xfer([0x10]) 70 self.spi.xfer([0x10])
66 self.spi.xfer([0x0A]) # KVAL_RUN 71 self.spi.xfer([0x0A]) # KVAL_RUN
67 self.spi.xfer([0xFF]) 72 self.spi.xfer([0x50])
68 self.spi.xfer([0x0B]) # KVAL_ACC 73 self.spi.xfer([0x0B]) # KVAL_ACC
69 self.spi.xfer([0xFF]) 74 self.spi.xfer([0x50])
70 self.spi.xfer([0x0C]) # KVAL_DEC 75 self.spi.xfer([0x0C]) # KVAL_DEC
71 self.spi.xfer([0xFF]) 76 self.spi.xfer([0x05])
72 self.spi.xfer([0x18]) # CONFIG 77 # self.spi.xfer([0x18]) # CONFIG
73 self.spi.xfer([0b00111000]) 78 # self.spi.xfer([0b00111000])
74 self.spi.xfer([0b00000000]) 79 # self.spi.xfer([0b00000000])
75 80
76 def MaxSpeed(self, speed): 81 def MaxSpeed(self, speed):
77 ' Setup of maximum speed ' 82 'Setup of maximum speed in steps/s'
-   83 speed_value = int(speed / 15.25)
-   84 if (speed_value == 0):
-   85 speed_value = 1
-   86 print hex(speed_value)
-   87  
-   88 data = [(speed_value >> i & 0xff) for i in (16,8,0)]
78 self.spi.xfer([0x07]) # Max Speed setup 89 self.spi.xfer([data[0]]) # Max Speed setup
79 self.spi.xfer([0x00]) 90 self.spi.xfer([data[1]])
80 self.spi.xfer([speed]) 91 self.spi.xfer([data[2]])
-   92 return (speed_value * 15.25)
81   93  
82 def ReleaseSW(self): 94 def ReleaseSW(self):
83 ' Go away from Limit Switch ' 95 ' Go away from Limit Switch '
84 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ? 96 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ?
85 self.spi.xfer([0x92 | (~self.Dir & 1)]) # release SW 97 self.spi.xfer([0x92 | (~self.Dir & 1)]) # release SW
86 while self.IsBusy(): 98 while self.GetStatus()['BUSY']:
87 pass 99 pass
88 self.MoveWait(10) # move 10 units away 100 self.MoveWait(10) # move 10 units away
89 101
90 def GoZero(self, speed): 102 def GoZero(self, speed):
91 ' Go to Zero position ' 103 ' Go to Zero position '
92 self.ReleaseSW() 104 self.ReleaseSW()
93   -  
94 self.spi.xfer([0x82 | (self.Dir & 1)]) # Go to Zero 105 self.spi.xfer([0x82 | (self.Dir & 1)]) # Go to Zero
95 self.spi.xfer([0x00]) 106 self.spi.xfer([0x00])
96 self.spi.xfer([speed]) 107 self.spi.xfer([speed])
97 while self.IsBusy(): 108 while self.GetStatus()['BUSY']:
98 pass 109 pass
99 time.sleep(0.3) 110 time.sleep(0.3)
100 self.ReleaseSW() 111 self.ReleaseSW()
101   112  
-   113 def GetStatus(self):
-   114 #self.spi.xfer([0b11010000]) # Get status command from datasheet - does not work for uknown rasons
-   115 self.spi.xfer([0x39]) # Gotparam command on status register
-   116 data = self.spi.readbytes(1)
-   117 data = data + self.spi.readbytes(1)
-   118  
-   119 status = dict([('SCK_MOD',data[0] & 0x80 == 0x80), #The SCK_MOD bit is an active high flag indicating that the device is working in Step-clock mode. In this case the step-clock signal should be provided through the STCK input pin. The DIR bit indicates the current motor direction
-   120 ('STEP_LOSS_B',data[0] & 0x40 == 0x40),
-   121 ('STEP_LOSS_A',data[0] & 0x20 == 0x20),
-   122 ('OCD',data[0] & 0x10 == 0x10),
-   123 ('TH_SD',data[0] & 0x08 == 0x08),
-   124 ('TH_WRN',data[0] & 0x04 == 0x04),
-   125 ('UVLO',data[0] & 0x02 == 0x02),
-   126 ('WRONG_CMD',data[0] & 0x01 == 0x01), #The NOTPERF_CMD and WRONG_CMD flags are active high and indicate, respectively, that the command received by SPI cannot be performed or does not exist at all.
-   127 ('NOTPERF_CMD',data[1] & 0x80 == 0x80),
-   128 ('MOT_STATUS',data[1] & 0x60),
-   129 ('DIR',data[1] & 0x10 == 0x10),
-   130 ('SW_EVN',data[1] & 0x08 == 0x08),
-   131 ('SW_F',data[1] & 0x04 == 0x04), #The SW_F flag reports the SW input status (low for open and high for closed).
-   132 ('BUSY',data[1] & 0x02 != 0x02),
-   133 ('HIZ',data[1] & 0x01 == 0x01)])
-   134 return status
-   135  
102 def Move(self, units): 136 def Move(self, units):
103 ' Move some distance units from current position ' 137 ' Move some distance units from current position '
104 steps = units * self.SPU # translate units to steps 138 steps = units * self.SPU # translate units to steps
105 if steps > 0: # look for direction 139 if steps > 0: # look for direction
106 self.spi.xfer([0x40 | (~self.Dir & 1)]) 140 self.spi.xfer([0x40 | (~self.Dir & 1)])
Line 109... Line 143...
109 steps = int(abs(steps)) 143 steps = int(abs(steps))
110 self.spi.xfer([(steps >> 16) & 0xFF]) 144 self.spi.xfer([(steps >> 16) & 0xFF])
111 self.spi.xfer([(steps >> 8) & 0xFF]) 145 self.spi.xfer([(steps >> 8) & 0xFF])
112 self.spi.xfer([steps & 0xFF]) 146 self.spi.xfer([steps & 0xFF])
113   147  
-   148 def Run(self, direction, speed):
-   149 speed_value = int(speed / 0.015)
-   150 print hex(speed_value)
-   151  
-   152 data = [0b01010000 + direction]
-   153 data = data +[(speed_value >> i & 0xff) for i in (16,8,0)]
-   154 self.spi.xfer([data[0]]) # Max Speed setup
-   155 self.spi.xfer([data[1]])
-   156 self.spi.xfer([data[2]])
-   157 self.spi.xfer([data[3]])
-   158 return (speed_value * 0.015)
-   159  
114 def MoveWait(self, units): 160 def MoveWait(self, units):
115 ' Move some distance units from current position and wait for execution ' 161 ' Move some distance units from current position and wait for execution '
116 self.Move(units) 162 self.Move(units)
117 while self.IsBusy(): 163 while self.GetStatus()['BUSY']:
118 pass 164 pass
119 time.sleep(0.8) 165 time.sleep(0.8)
120   166  
121 def Float(self): 167 def Float(self, hard = False):
122 ' switch H-bridge to High impedance state ' 168 ' switch H-bridge to High impedance state '
123 self.spi.xfer([0xA0]) -  
124   -  
125 def ReadStatusBit(self, bit): -  
126 ' Report given status bit ' 169 if (hard == False):
127 self.spi.xfer([0x39]) # Get status command 170 self.spi.xfer([0xA0])
128 data = self.spi.readbytes(1) # 1st byte -  
129 data = data + (self.spi.readbytes(1)) # 1st byte -  
130 print data -  
131 if bit > 7: # extract requested bit -  
132 OutputBit = (data[0] >> (bit - 8)) & 1 -  
133 else: 171 else:
134 OutputBit = (data[1] >> bit) & 1 -  
135 return OutputBit 172 self.spi.xfer([0xA8])
136   173  
137   174  
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 -------------------------------------------------- 175 # End Class axis --------------------------------------------------
147   176  
148 print "Stepper motor control test started. \r\n" 177 print "Stepper motor control test started. \r\n"
149 print "Max motor speed: %d " % SPEED 178 print "Max motor speed: %f " % SPEED
150 print "Distance to run: %d " % DISTANCE 179 print "Distance to run: %f " % DISTANCE
151   180  
152 try: 181 try:
153 print "SPI configuration.." 182 print "SPI configuration.."
154 spi = spidev.SpiDev() # create a spi object 183 spi = spidev.SpiDev() # create a spi object
155 spi.open(0, 0) # open spi port 0, device (CS) 0 184 spi.open(0, 0) # open spi port 0, device (CS) 0
156 spi.mode = 0b01 185 spi.mode = 0b01
157 spi.lsbfirst = False 186 spi.lsbfirst = False
158 spi.bits_per_word = 8 187 spi.bits_per_word = 8
159 spi.cshigh = False 188 spi.cshigh = False
-   189 spi.max_speed_hz = 100000
160 #spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz) 190 #spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz)
161 time.sleep(1) 191 time.sleep(1)
162   192  
163 print "Axis inicialization" 193 print "Axis inicialization"
164 X = axis(spi, 0, 641) # set Number of Steps per axis Unit and set Direction of Rotation 194 X = axis(spi, 0, 641, MaxSpeed = SPEED) # set Number of Steps per axis Unit and set Direction of Rotation
165 X.MaxSpeed(SPEED) # set maximal motor speed -  
166   195  
167 print "Axis is running" 196 print X.MaxSpeed(SPEED) # set maximal motor speed
168   197  
-   198 print X.Run(1, 200.456431)
-   199 time.sleep(10)
-   200 '''
-   201 print "Axis is running"
169 for i in range(5): 202 for i in range(5):
170 print i 203 print i
171 X.MoveWait(DISTANCE) # move forward and wait for motor stop 204 X.MoveWait(DISTANCE) # move forward and wait for motor stop
172 print "Changing direction of rotation.." 205 print "Changing direction of rotation.."
173 time.sleep(1.1) 206 time.sleep(1.1)
174 X.MoveWait(-DISTANCE) # move backward and wait for motor stop 207 X.MoveWait(-DISTANCE) # move backward and wait for motor stop
175 print "Changing direction of rotation.." 208 print "Changing direction of rotation.."
176 time.sleep(1.1) 209 time.sleep(1.1)
-   210 '''
177 X.Float() # release power 211 X.Float(hard=False) # release power
178   -  
179   212  
180 finally: 213 finally:
181 print "stop" 214 print "stop"