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()