Rev 4574 Rev 4718
Line 21... Line 21...
21 sys.stderr.write("Usage: %s PORT ADDRESS SPEED MOVE_DISTANCE\n" % (sys.argv[0], )) 21 sys.stderr.write("Usage: %s PORT ADDRESS SPEED MOVE_DISTANCE\n" % (sys.argv[0], ))
22 sys.exit(1) 22 sys.exit(1)
23   23  
24 elif len(sys.argv) == 2: 24 elif len(sys.argv) == 2:
25 PORT = eval(sys.argv[1]) 25 PORT = eval(sys.argv[1])
26 SPEED = 5 26 SPEED = 50
27 DISTANCE = 50 27 DISTANCE = 5000
28   28  
29 elif len(sys.argv) == 3: 29 elif len(sys.argv) == 3:
30 SPEED = eval(sys.argv[2]) 30 SPEED = eval(sys.argv[2])
31 DISTANCE = 100 31 DISTANCE = 1000
32   32  
33 elif len(sys.argv) == 4: 33 elif len(sys.argv) == 4:
34 SPEED = eval(sys.argv[2]) 34 SPEED = eval(sys.argv[2])
35 DISTANCE = eval(sys.argv[3]) 35 DISTANCE = eval(sys.argv[3])
36   36  
37 else: 37 else:
38 PORT = 0 38 PORT = 0
39 SPEED = 10 39 SPEED = 20
40 DISTANCE = 50 40 DISTANCE = 5000
41   41  
42 class axis: 42 class axis:
43 def __init__(self, SPI_handler, Direction, StepsPerUnit, MaxSpeed): 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.maxspeed = MaxSpeed
-   49  
-   50 self.L6470_ABS_POS =0x01
-   51 self.L6470_EL_POS =0x02
-   52 self.L6470_MARK =0x03
-   53 self.L6470_SPEED =0x04
-   54 self.L6470_ACC =0x05
-   55 self.L6470_DEC =0x06
-   56 self.L6470_MAX_SPEED =0x07
-   57 self.L6470_MIN_SPEED =0x08
-   58 self.L6470_FS_SPD =0x15
-   59 self.L6470_KVAL_HOLD =0x09
-   60 self.L6470_KVAL_RUN =0x0A
-   61 self.L6470_KVAL_ACC =0x0B
-   62 self.L6470_KVAL_DEC =0x0C
-   63 self.L6470_INT_SPEED =0x0D
-   64 self.L6470_ST_SLP =0x0E
-   65 self.L6470_FN_SLP_ACC =0x0F
-   66 self.L6470_FN_SLP_DEC =0x10
-   67 self.L6470_K_THERM =0x11
-   68 self.L6470_ADC_OUT =0x12
-   69 self.L6470_OCD_TH =0x13
-   70 self.L6470_STALL_TH =0x14
-   71 self.L6470_STEP_MODE =0x16
-   72 self.L6470_ALARM_EN =0x17
-   73 self.L6470_CONFIG =0x18
-   74 self.L6470_STATUS =0x19
-   75  
49 self.Reset() 76 self.Reset()
50 self.Initialize() 77 self.Initialize()
51   78  
-   79  
-   80  
-   81  
52 def Reset(self): 82 def Reset(self):
53 'Reset the Axis' 83 'Reset the Axis'
54 self.spi.xfer([0xC0]) # reset 84 self.spi.xfer([0xC0]) # reset
55   85  
56 def Initialize(self): 86 def Initialize(self):
Line 66... Line 96...
66 self.spi.xfer([0x00]) 96 self.spi.xfer([0x00])
67 self.spi.xfer([0x10]) 97 self.spi.xfer([0x10])
68 self.spi.xfer([0x06]) # DEC 98 self.spi.xfer([0x06]) # DEC
69 self.spi.xfer([0x00]) 99 self.spi.xfer([0x00])
70 self.spi.xfer([0x10]) 100 self.spi.xfer([0x10])
71 self.spi.xfer([0x0A]) # KVAL_RUN 101 self.spi.xfer([self.L6470_KVAL_RUN]) # KVAL_RUN
72 self.spi.xfer([0x50]) 102 self.spi.xfer([0x50])
73 self.spi.xfer([0x0B]) # KVAL_ACC 103 self.spi.xfer([self.L6470_KVAL_ACC]) # KVAL_ACC
74 self.spi.xfer([0x50]) 104 self.spi.xfer([0x50])
75 self.spi.xfer([0x0C]) # KVAL_DEC 105 self.spi.xfer([self.L6470_KVAL_DEC]) # KVAL_DEC
76 self.spi.xfer([0x50]) 106 self.spi.xfer([0x50])
77 # self.spi.xfer([0x18]) # CONFIG 107 # self.spi.xfer([0x18]) # CONFIG
78 # self.spi.xfer([0b00111000]) 108 # self.spi.xfer([0b00111000])
79 # self.spi.xfer([0b00000000]) 109 # self.spi.xfer([0b00000000])
-   110 self.MaxSpeed(self.maxspeed)
80 111
81 def MaxSpeed(self, speed): 112 def MaxSpeed(self, speed):
82 'Setup of maximum speed in steps/s' 113 '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.'
83 speed_value = int(speed / 15.25) 114 speed_value = int(speed / 15.25)
84 if (speed_value == 0): 115 if (speed_value <= 0):
85 speed_value = 1 116 speed_value = 1
86 print hex(speed_value) 117 elif (speed_value >= 1023):
-   118 speed_value = 1023
87   119  
88 data = [(speed_value >> i & 0xff) for i in (16,8,0)] 120 data = [(speed_value >> i & 0xff) for i in (8,0)]
89 self.spi.xfer([data[0]]) # Max Speed setup 121 self.spi.xfer([self.L6470_MAX_SPEED]) # Max Speed setup
-   122 self.spi.xfer([data[0]])
90 self.spi.xfer([data[1]]) 123 self.spi.xfer([data[1]])
91 self.spi.xfer([data[2]]) -  
92 return (speed_value * 15.25) 124 return (speed_value * 15.25)
93   125  
94 def ReleaseSW(self): 126 def ReleaseSW(self):
95 ' Go away from Limit Switch ' 127 ' Go away from Limit Switch '
96 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ? 128 while self.ReadStatusBit(2) == 1: # is Limit Switch ON ?
Line 189... Line 221...
189 spi.max_speed_hz = 100000 221 spi.max_speed_hz = 100000
190 #spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz) 222 #spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz)
191 time.sleep(1) 223 time.sleep(1)
192   224  
193 print "Axis inicialization" 225 print "Axis inicialization"
194 X = axis(spi, 0, 641, MaxSpeed = SPEED) # set Number of Steps per axis Unit and set Direction of Rotation 226 X = axis(spi, 0, 1, MaxSpeed = SPEED) # set Number of Steps per axis Unit and set Direction of Rotation
195   227  
196 print X.MaxSpeed(SPEED) # set maximal motor speed 228 print X.MaxSpeed(SPEED) # set maximal motor speed
197   229  
198 #print X.Run(1, 200.456431) 230 #print X.Run(1, 200.456431)
199 #time.sleep(10) 231 #time.sleep(10)