/
086-005a.py
340 lines (288 loc) · 15.1 KB
/
086-005a.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
from pygestalt import nodes
from pygestalt import utilities
from pygestalt.utilities import notice
from pygestalt import functions as functions
from pygestalt import packets as packets
from pygestalt import core
import time
import math
class virtualNode(nodes.baseStandardGestaltNode):
def init(self, **kwargs):
pass
def initParameters(self):
#ADC parameters
self.ADCRefVoltage = 5.0 #voltage divider to ref pin is 5V -> 10K -> REF -> 5K -> GND
self.motorSenseResistor = 0.1 #ohms
#stepping parameters
self.maxSteps = 255 #one byte
self.clockFrequency = 18432000.0 #Hz
self.timeBaseTicks = 921.0 #clock cycles, this works out to around 20KHz
self.timeBasePeriod = self.timeBaseTicks / self.clockFrequency #seconds
self.uStepsPerStep = 1048576 #uSteps
#axes parameters
self.numberOfAxes = 1 #three axis driver
def initFunctions(self):
pass
def initPackets(self):
self.getReferenceVoltageResponsePacket = packets.packet(template = [packets.pInteger('voltage',2)])
self.spinRequestPacket = packets.packet(template = [packets.pInteger('majorSteps',1),
packets.pInteger('directions',1),
packets.pInteger('steps', 1),
packets.pInteger('accel',1),
packets.pInteger('accelSteps',1),
packets.pInteger('decelSteps',1),
packets.pInteger('sync', 1)]) #if >0, indicates that move is synchronous
self.spinStatusPacket = packets.packet(template = [packets.pInteger('statusCode',1),
packets.pInteger('currentKey',1),
packets.pInteger('stepsRemaining',1),
packets.pInteger('readPosition',1),
packets.pInteger('writePosition',1)])
self.setVelocityPacket = packets.packet(template = [packets.pInteger('velocity',2)])
def initPorts(self):
#get reference voltage
self.bindPort(port = 20, outboundFunction = self.getReferenceVoltageRequest, inboundPacket = self.getReferenceVoltageResponsePacket)
#enable drivers
self.bindPort(port = 21, outboundFunction = self.enableRequest)
#disable drivers
self.bindPort(port = 22, outboundFunction = self.disableRequest)
#move
self.bindPort(port = 23, outboundFunction = self.spinRequest, outboundPacket = self.spinRequestPacket,
inboundPacket = self.spinStatusPacket)
#set velocity
self.bindPort(port = 24, outboundFunction = self.setVelocityRequest, outboundPacket = self.setVelocityPacket)
#spin status
self.bindPort(port = 26, outboundFunction = self.spinStatusRequest, inboundPacket = self.spinStatusPacket)
#sync
self.bindPort(port = 30, outboundFunction = self.syncRequest)
#----- API FUNCTIONS --------------------
def setMotorCurrent(self, setCurrent):
if setCurrent < 0 or setCurrent > 2.0:
notice(self, "Motor current must be between 0 and 2.0 amps. " + str(setCurrent) + " was requested.")
setCurrent = round(float(setCurrent), 1)
while True:
motorVoltage = self.getReferenceVoltage()
motorCurrent = round(motorVoltage /(8.0*self.motorSenseResistor),2) #amps
if round(motorCurrent,1) > setCurrent:
notice(self, "Motor current: " + str(motorCurrent) + "A / " + "Desired: " + str(setCurrent) + "A. Turn trimmer CW.")
elif round(motorCurrent,1) < setCurrent:
notice(self, "Motor current: " + str(motorCurrent) + "A / " + "Desired: " + str(setCurrent) + "A. Turn trimmer CCW.")
else:
notice(self, "Motor current set to: " + str(motorCurrent) + "A")
break;
time.sleep(0.5)
def getReferenceVoltage(self):
ADCReading = self.getReferenceVoltageRequest()
if ADCReading:
return self.ADCRefVoltage * ADCReading / 1024.0
else:
return False
def enableMotorsRequest(self):
return self.enableRequest()
def disableMotorsRequest(self):
return self.disableRequest()
#----- SERVICE ROUTINES -----------------
class enableRequest(functions.serviceRoutine):
class actionObject(core.actionObject):
def init(self):
self.setPacket({})
self.commitAndRelease()
self.waitForChannelAccess()
if self.transmitPersistent():
notice(self.virtualNode, "Stepper motor enabled.")
return True
else:
notice(self.virtualNode, "Stepper motor could not be enabled.")
return False
class disableRequest(functions.serviceRoutine):
class actionObject(core.actionObject):
def init(self):
self.setPacket({})
self.commitAndRelease()
self.waitForChannelAccess()
if self.transmitPersistent():
notice(self.virtualNode, "Stepper motor disabled.")
return True
else:
notice(self.virtualNode, "Stepper motor could not be disabled.")
return False
class getReferenceVoltageRequest(functions.serviceRoutine):
class actionObject(core.actionObject):
def init(self):
self.setPacket({})
self.commitAndRelease()
self.waitForChannelAccess()
if self.transmitPersistent(): return self.getPacket()['voltage']
else: return False
class spinStatusRequest(functions.serviceRoutine):
class actionObject(core.actionObject):
def init(self):
self.setPacket({})
self.commitAndRelease()
self.waitForChannelAccess()
if self.transmitPersistent():
return self.getPacket()
else:
notice(self.virtualNode, 'unable to get status from node.')
class setVelocityRequest(functions.serviceRoutine):
class actionObject(core.actionObject):
def init(self, velocity):
#velocity is in steps/sec
velocity = int(round((velocity * self.virtualNode.uStepsPerStep * self.virtualNode.timeBasePeriod)/16.0,0)) #convert into 16*uSteps/timebase
print velocity
self.setPacket({'velocity': velocity})
self.commitAndRelease()
self.waitForChannelAccess()
if self.transmitPersistent():
return True
else:
notice(self.virtualNode, 'unable to set velocity.')
return False
class spinRequest(functions.serviceRoutine):
class actionObject(core.actionObject):
def init(self, axesSteps, accelSteps = 0, decelSteps = 0, accelRate = 0, external = False, sync = False, majorSteps = None):
# axesSteps: a list containing the number of steps which each axis of the node should take in synchrony.
# accelSteps: number of virtual major axis steps during which acceleration should occur.
# deccelSteps: number of virtual major axis steps during which deceleration should occur.
# accelRate: accel/decel rate in steps/sec^2
# external: indicates whether the actionObject will be commited to its interface externally.
# When False, the actionObject will self commit and release.
# When True, the actionObject will prepare a packet but will need to be commited and release externally.
# sync: indicates that this actionObject will be synchronized externally and that parameters might change. This will prevent it from spawning.
# majorSteps: this parameter is only called internally when a request calls for too many steps for one actionObject and the actionObject needs to spawn.
if type(axesSteps) != list and type(axesSteps) != tuple: axesSteps = [axesSteps]
self.axesSteps = [int(axis) for axis in axesSteps] #list of steps to take in each axis e.g. [x, y, z]. May be provided as a tuple.
self.accelSteps = accelSteps
self.decelSteps = decelSteps
self.accelRate = accelRate
self.external = external
self.sync = sync
self.actionSequence = False #start out with no action sequence
self.sequenceMajorSteps = None #if this actionObject becomes an actionSequence parent, this will store the list of major axes
#calculate virtual major axis
if majorSteps: #if provided externally, use that.
self.majorSteps = majorSteps
else: #generate majorSteps from provided axes.
self.majorSteps = max([abs(axis) for axis in self.axesSteps])
if self.majorSteps > self.virtualNode.maxSteps: #check if step request exceeds maximum length
if not sync: #no other anticipated recalculations, so go ahead and generate actionSequence
#need to generate an actionSequence with multiple new actionObjects
self.actionSequence = self.actionSequenceGen()
return self.actionSequence
else: #the majorSteps has not yet been synchronized, so cannot generate an action sequence yet.
return self
else: #step request is executable by this action object. Either initial request met this criteria, or this actionObject was initialized by actionSequence().
#calculate directions
directions = [1 if axis>0 else 0 for axis in self.axesSteps]
directionMultiplexer = [2**b for b in range(self.virtualNode.numberOfAxes-1, -1, -1)] #generates [2^n, 2^n-1,...,2^0]
self.directionByte = sum(map(lambda x,y: x*y, directionMultiplexer, directions)) #directionByte is each term of directionMultiplexer multiplied by the directions and then summed.
if external or sync: #actionObject is being managed by an external function and shouldn't auto-commit
return self
else:
#set packet
accelRate = self.tbAccelRate(self.accelRate) #convert accel rate into timebase
self.setPacket({'majorSteps':self.majorSteps, 'directions':self.directionByte, 'steps':abs(self.axesSteps[0]), 'accel':accelRate,
'accelSteps':accelSteps, 'decelSteps':decelSteps})
self.commitAndRelease()
self.waitForChannelAccess()
moveQueued = False
while not moveQueued:
if self.transmitPersistent():
responsePacket = self.getPacket()
moveQueued = bool(responsePacket['statusCode']) #False if move was not queued
time.sleep(0.02)
else:
notice(self.virtualNode, "got no response to spin request")
return False
return responsePacket
def syncPush(self):
'''Stores this actionObject's sync tokens to the provided syncToken.'''
if self.sync:
self.sync.push('majorSteps', self.majorSteps)
def syncPull(self):
'''Actually does the synchronization based on the tokens stored in self.sync.'''
if self.sync:
self.majorSteps = int(max(self.sync.pull('majorSteps')))
if self.majorSteps > self.virtualNode.maxSteps:
self.actionSequence = self.actionSequenceGen()
return self.actionSequence
else:
return self
def channelAccess(self):
'''This gets called when channel access is granted.
To prevent double-transmission, only transmit if external is True.'''
if self.external:
#set packet
accelRate = self.tbAccelRate(self.accelRate) #convert accel rate into timebase
self.setPacket({'majorSteps':self.majorSteps, 'directions':self.directionByte, 'steps':abs(self.axesSteps[0]), 'accel':accelRate,
'accelSteps':self.accelSteps, 'decelSteps':self.decelSteps, 'sync':int(bool(self.sync))})
#since this node was instantiated under external control, it did not auto-transmit.
moveQueued = False
while not moveQueued:
if self.transmitPersistent():
responsePacket = self.getPacket()
print responsePacket
moveQueued = bool(responsePacket['statusCode']) #False if move was not queued, meaning buffer is full
if not moveQueued: time.sleep(0.02) #wait before retransmitting
else:
notice(self.virtualNode, "got no response to spin request")
def splitNumber(self, value, segments):
'''Returns a list of integers (length segments) whose sum is value.
This algorithm should produce similar results to the bresenham algorithm without needing to iterate.'''
#e.g. splitNumber(800, 7)
integers = [int(value/segments) for i in range(segments)] #[114, 114, 114, 114, 114, 114, 114]
remainder = value - sum(integers) #2
distributedRemainder = remainder/float(segments) #0.285...
remainderSequence = [0] + [distributedRemainder*i - round(distributedRemainder*i,0) for i in range(1, 1+segments)] #[0.285, -0.428, -0.142, 0.142, 0.428, -0.285, 0.0]
extraSteps = map(lambda x,y: 1 if x>y else 0, remainderSequence[:-1], remainderSequence[1:]) #[0, 1, 0, 0, 0, 1, 0] passes negative steps
return map(lambda x,y: x+y, integers, extraSteps) #[114, 115, 114, 114, 114, 115, 114]
def fillFront(self, fillNumber, binSizes):
'''Fills a series of bins whose size are specified by fillList, starting with the first bin.'''
filledBins = [0 for bin in binSizes]
for binIndex, binSize in enumerate(binSizes):
if fillNumber > binSize:
filledBins[binIndex] = binSize
fillNumber -= binSize
else:
filledBins[binIndex] = fillNumber
fillNumber = 0
return filledBins
def fillBack(self, fillNumber, binSizes):
'''Fills a series of bins whose size are specified by fillList, starting with the last bin.'''
reversedBinSizes = list(binSizes) #makes a copy
reversedBinSizes.reverse()
filledBins = self.fillFront(fillNumber, reversedBinSizes)
filledBins.reverse()
return filledBins
def actionSequenceGen(self): #need to generate an action sequence to accommodate the request
'''Returns an actionSequence which contains a sequential set of actionObjects.'''
segments = int(math.ceil(self.majorSteps/float(self.virtualNode.maxSteps))) #number of actionObjects needed to address requested number of steps
axesSteps = zip(*[self.splitNumber(axisSteps, segments) for axisSteps in self.axesSteps]) #a list of axesSteps lists which are divided into the necessary number of segments
majorSteps = self.splitNumber(self.majorSteps, segments) #a list of majorSteps which are divided into the necessary number of segments
self.sequenceMajorSteps = majorSteps #store at the actionObject level for when parameters need to be updated later.
accelSteps = self.fillFront(self.accelSteps, majorSteps) #a list of accelSteps which fills bins of maximum size majorSteps starting from the front
decelSteps = self.fillBack(self.decelSteps, majorSteps) #a list of decelSteps which fills bins of maximum size majorSteps starting from the back
accelRate = [self.accelRate for segment in range(segments)]
external = [self.external for segment in range(segments)]
sync = [self.sync for segment in range(segments)]
return self.__actionSequence__(axesSteps, accelSteps, decelSteps, accelRate, external, sync, majorSteps)
def update(self, accelSteps = 0, decelSteps = 0, accelRate = 0):
'''Updates the acceleration and deceleration parameters for the spin actionObject.'''
if self.actionSequence: #this actionObject is the parent of an actionSequence. Update the whole sequence
accelSteps = self.fillFront(accelSteps, self.sequenceMajorSteps)
decelSteps = self.fillBack(decelSteps, self.sequenceMajorSteps)
accelRates = [accelRate for majorSteps in self.sequenceMajorSteps]
for actionObject, args in zip(self.actionSequence.actionObjects, zip(accelSteps, decelSteps, accelRates)):
actionObject.update(*args)
else:
self.accelSteps = accelSteps
self.decelSteps = decelSteps
self.accelRate = accelRate
def tbAccelRate(self, accelRate):
'''Converts acceleration from steps/sec^2 to uSteps/timeBase^2.'''
return int(round(accelRate * self.virtualNode.uStepsPerStep * self.virtualNode.timeBasePeriod * self.virtualNode.timeBasePeriod,0)) #uSteps/timePeriod^2
class syncRequest(functions.serviceRoutine):
class actionObject(core.actionObject):
def init(self):
self.mode = 'multicast'
def channelAccess(self):
self.transmit()