Rev 4450 Rev 4718
Line 23... Line 23...
23 sys.stderr.write("Usage: %s PORT ADDRESS SPEED MOVE_DISTANCE\n" % (sys.argv[0], )) 23 sys.stderr.write("Usage: %s PORT ADDRESS SPEED MOVE_DISTANCE\n" % (sys.argv[0], ))
24 sys.exit(1) 24 sys.exit(1)
25   25  
26 elif len(sys.argv) == 2: 26 elif len(sys.argv) == 2:
27 PORT = eval(sys.argv[1]) 27 PORT = eval(sys.argv[1])
28 SPEED = 5 28 SPEED = 50
29 DISTANCE = 50 29 DISTANCE = 5000
30   30  
31 elif len(sys.argv) == 3: 31 elif len(sys.argv) == 3:
32 SPEED = eval(sys.argv[2]) 32 SPEED = eval(sys.argv[2])
33 DISTANCE = 100 33 DISTANCE = 1000
34   34  
35 elif len(sys.argv) == 4: 35 elif len(sys.argv) == 4:
36 SPEED = eval(sys.argv[2]) 36 SPEED = eval(sys.argv[2])
37 DISTANCE = eval(sys.argv[3]) 37 DISTANCE = eval(sys.argv[3])
38   38  
39 else: 39 else:
40 PORT = 0 40 PORT = 0
41 SPEED = 10 41 SPEED = 20
42 DISTANCE = 50 42 DISTANCE = 5000
43   43  
44   44  
45 class axis: 45 class axis:
46 def __init__(self, SPI_CS, Direction, StepsPerUnit): 46 def __init__(self, SPI_CS, Direction, StepsPerUnit, MaxSpeed):
47 ' One axis of robot ' 47 ' One axis of robot '
48 self.CS = SPI_CS 48 self.CS = SPI_CS
49 self.Dir = Direction 49 self.Dir = Direction
50 self.SPU = StepsPerUnit 50 self.SPU = StepsPerUnit
-   51 self.maxspeed = MaxSpeed
-   52  
-   53 self.L6470_ABS_POS =0x01
-   54 self.L6470_EL_POS =0x02
-   55 self.L6470_MARK =0x03
-   56 self.L6470_SPEED =0x04
-   57 self.L6470_ACC =0x05
-   58 self.L6470_DEC =0x06
-   59 self.L6470_MAX_SPEED =0x07
-   60 self.L6470_MIN_SPEED =0x08
-   61 self.L6470_FS_SPD =0x15
-   62 self.L6470_KVAL_HOLD =0x09
-   63 self.L6470_KVAL_RUN =0x0A
-   64 self.L6470_KVAL_ACC =0x0B
-   65 self.L6470_KVAL_DEC =0x0C
-   66 self.L6470_INT_SPEED =0x0D
-   67 self.L6470_ST_SLP =0x0E
-   68 self.L6470_FN_SLP_ACC =0x0F
-   69 self.L6470_FN_SLP_DEC =0x10
-   70 self.L6470_K_THERM =0x11
-   71 self.L6470_ADC_OUT =0x12
-   72 self.L6470_OCD_TH =0x13
-   73 self.L6470_STALL_TH =0x14
-   74 self.L6470_STEP_MODE =0x16
-   75 self.L6470_ALARM_EN =0x17
-   76 self.L6470_CONFIG =0x18
-   77 self.L6470_STATUS =0x19
-   78  
51 self.Reset() 79 self.Reset()
-   80 self.Initialize()
52   81  
53 def Reset(self): 82 def Reset(self):
54 ' Reset Axis and set default parameters for H-bridge ' 83 'Reset the Axis'
55 spi.SPI_write_byte(self.CS, 0xC0) # reset 84 spi.SPI_write_byte(self.CS, 0xC0) # reset
-   85  
-   86 def Initialize(self):
-   87 'set default parameters for H-bridge '
56 # spi.SPI_write_byte(self.CS, 0x14) # Stall Treshold setup 88 # spi.SPI_write_byte(self.CS, 0x14) # Stall Treshold setup
57 # spi.SPI_write_byte(self.CS, 0xFF) 89 # spi.SPI_write_byte(self.CS, 0xFF)
58 # spi.SPI_write_byte(self.CS, 0x13) # Over Current Treshold setup 90 # spi.SPI_write_byte(self.CS, 0x13) # Over Current Treshold setup
59 # spi.SPI_write_byte(self.CS, 0xFF) 91 # spi.SPI_write_byte(self.CS, 0xFF)
60 spi.SPI_write_byte(self.CS, 0x15) # Full Step speed 92 spi.SPI_write_byte(self.CS, 0x15) # Full Step speed
Line 64... Line 96...
64 spi.SPI_write_byte(self.CS, 0x00) 96 spi.SPI_write_byte(self.CS, 0x00)
65 spi.SPI_write_byte(self.CS, 0x10) 97 spi.SPI_write_byte(self.CS, 0x10)
66 spi.SPI_write_byte(self.CS, 0x06) # DEC 98 spi.SPI_write_byte(self.CS, 0x06) # DEC
67 spi.SPI_write_byte(self.CS, 0x00) 99 spi.SPI_write_byte(self.CS, 0x00)
68 spi.SPI_write_byte(self.CS, 0x10) 100 spi.SPI_write_byte(self.CS, 0x10)
69 spi.SPI_write_byte(self.CS, 0x0A) # KVAL_RUN 101 spi.SPI_write_byte(self.CS, self.L6470_KVAL_RUN) # KVAL_RUN
70 spi.SPI_write_byte(self.CS, 0xFF) 102 spi.SPI_write_byte(self.CS, 0x28)
71 spi.SPI_write_byte(self.CS, 0x0B) # KVAL_ACC 103 spi.SPI_write_byte(self.CS, self.L6470_KVAL_ACC) # KVAL_ACC
72 spi.SPI_write_byte(self.CS, 0xFF) 104 spi.SPI_write_byte(self.CS, 0x28)
73 spi.SPI_write_byte(self.CS, 0x0C) # KVAL_DEC 105 spi.SPI_write_byte(self.CS, self.L6470_KVAL_DEC) # KVAL_DEC
74 spi.SPI_write_byte(self.CS, 0xFF) 106 spi.SPI_write_byte(self.CS, 0x28)
75 spi.SPI_write_byte(self.CS, 0x18) # CONFIG 107 # spi.SPI_write_byte(self.CS, 0x18) # CONFIG
76 spi.SPI_write_byte(self.CS, 0b00111000) 108 # spi.SPI_write_byte(self.CS, 0b00111000)
77 spi.SPI_write_byte(self.CS, 0b00000000) 109 # spi.SPI_write_byte(self.CS, 0b00000000)
-   110 self.MaxSpeed(self.maxspeed)
-   111  
-   112 def setKVAL(self, hold = 0.5, run = 0.5, acc = 0.5, dec = 0.5):
-   113 """ The available range is from 0 to 0.996 x VS with a resolution of 0.004 x VS """
-   114  
-   115 def setOverCurrentTH(self, hold = 0.5, run = 0.5, acc = 0.5, dec = 0.5):
-   116 """ The available range is from 375 mA to 6 A, in steps of 375 mA """
-   117  
-   118  
78 119
79 def MaxSpeed(self, speed): 120 def MaxSpeed(self, speed):
-   121 'Setup of maximum speed in steps/s. The available range is from 15.25 to 15610 step/s with a resolution of 15.25 step/s.'
-   122 speed_value = int(speed / 15.25)
-   123 if (speed_value <= 0):
80 ' Setup of maximum speed ' 124 speed_value = 1
-   125 elif (speed_value >= 1023):
-   126 speed_value = 1023
-   127  
-   128 data = [(speed_value >> i & 0xff) for i in (8,0)]
-   129 print data
81 spi.SPI_write_byte(self.CS, 0x07) # Max Speed setup 130 spi.SPI_write_byte(self.CS, self.L6470_MAX_SPEED) # Max Speed setup
82 spi.SPI_write_byte(self.CS, 0x00) 131 spi.SPI_write_byte(self.CS, data[0])
83 spi.SPI_write_byte(self.CS, speed) 132 spi.SPI_write_byte(self.CS, data[1])
-   133 return (speed_value * 15.25)
84   134  
85 def ReleaseSW(self): 135 def ReleaseSW(self):
86 ' Go away from Limit Switch ' 136 ' Go away from Limit Switch '
87 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ? 137 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ?
88 spi.SPI_write_byte(self.CS, 0x92 | (~self.Dir & 1)) # release SW 138 spi.SPI_write_byte(self.CS, 0x92 | (~self.Dir & 1)) # release SW
Line 112... Line 162...
112 steps = int(abs(steps)) 162 steps = int(abs(steps))
113 spi.SPI_write_byte(self.CS, (steps >> 16) & 0xFF) 163 spi.SPI_write_byte(self.CS, (steps >> 16) & 0xFF)
114 spi.SPI_write_byte(self.CS, (steps >> 8) & 0xFF) 164 spi.SPI_write_byte(self.CS, (steps >> 8) & 0xFF)
115 spi.SPI_write_byte(self.CS, steps & 0xFF) 165 spi.SPI_write_byte(self.CS, steps & 0xFF)
116   166  
-   167 def Run(self, direction, speed):
-   168 speed_value = int(speed / 0.015)
-   169 print hex(speed_value)
-   170  
-   171 command = 0b01010000 + int(direction)
-   172 data = [(speed_value >> i & 0xff) for i in (16,8,0)]
-   173 spi.SPI_write_byte(self.CS, command) # Max Speed setup
-   174 spi.SPI_write_byte(self.CS, data[0])
-   175 spi.SPI_write_byte(self.CS, data[1])
-   176 spi.SPI_write_byte(self.CS, data[2])
-   177 return (speed_value * 0.015)
-   178  
117 def MoveWait(self, units): 179 def MoveWait(self, units):
118 ' Move some distance units from current position and wait for execution ' 180 ' Move some distance units from current position and wait for execution '
119 self.Move(units) 181 self.Move(units)
120 while self.IsBusy(): 182 while self.IsBusy():
121 pass 183 pass
122   184  
123 def Float(self): 185 def Float(self, hard = False):
124 ' switch H-bridge to High impedance state ' 186 ' switch H-bridge to High impedance state '
-   187 if (hard == False):
125 spi.SPI_write_byte(self.CS, 0xA0) 188 spi.SPI_write_byte(self.CS, 0xA0)
-   189 else:
-   190 spi.SPI_write_byte(self.CS, 0xA8)
126   191  
127 def ReadStatusBit(self, bit): 192 def ReadStatusBit(self, bit):
128 ' Report given status bit ' 193 ' Report given status bit '
129 spi.SPI_write_byte(self.CS, 0x39) # Read from address 0x19 (STATUS) 194 spi.SPI_write_byte(self.CS, 0x39) # Read from address 0x19 (STATUS)
130 spi.SPI_write_byte(self.CS, 0x00) 195 spi.SPI_write_byte(self.CS, 0x00)
Line 179... Line 244...
179 print "SPI configuration.." 244 print "SPI configuration.."
180 spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz) 245 spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz)
181 time.sleep(1) 246 time.sleep(1)
182   247  
183 print "Axis inicialization" 248 print "Axis inicialization"
184 X = axis(spi.I2CSPI_SS0, 0, 641) # set Number of Steps per axis Unit and set Direction of Rotation 249 X = axis(spi.I2CSPI_SS0, 0, 1, MaxSpeed = SPEED) # set Number of Steps per axis Unit and set Direction of Rotation
185 X.MaxSpeed(SPEED) # set maximal motor speed 250 X.MaxSpeed(SPEED) # set maximal motor speed
186   251  
187 print "Axis is running" 252 print "Axis is running"
188   253  
189 for i in range(5): 254 for i in range(5):
Line 191... Line 256...
191 X.MoveWait(DISTANCE) # move forward and wait for motor stop 256 X.MoveWait(DISTANCE) # move forward and wait for motor stop
192 print "Changing direction of rotation.." 257 print "Changing direction of rotation.."
193 X.MoveWait(-DISTANCE) # move backward and wait for motor stop 258 X.MoveWait(-DISTANCE) # move backward and wait for motor stop
194 print "Changing direction of rotation.." 259 print "Changing direction of rotation.."
195   260  
-   261 time.sleep(1)
-   262  
-   263 X.Run(1, SPEED/2)
-   264  
-   265 time.sleep(3)
-   266  
196 X.Float() # release power 267 X.Float(hard = False) # release power
197   268  
198   269  
199 finally: 270 finally:
200 print "stop" 271 print "stop"