# Forked from DFUnitVM Oct 2013 # set portname # set location of hex file for bootloader # #------IMPORTS------- from pygestalt import nodes from pygestalt import interfaces from pygestalt import machines from pygestalt import functions from pygestalt.machines import elements from pygestalt.machines import kinematics from pygestalt.machines import state from pygestalt.utilities import notice from pygestalt.publish import rpc #remote procedure call dispatcher import time import io import serial #------VIRTUAL MACHINE------ class virtualMachine(machines.virtualMachine): def initInterfaces(self): if self.providedInterface: self.fabnet = self.providedInterface #providedInterface is defined in the virtualMachine class. else: self.fabnet = interfaces.gestaltInterface('FABNET', interfaces.serialInterface(baudRate = 115200, interfaceType = 'ftdi', portName = '/dev/tty.usbserial-FTZ56N8T')) def initControllers(self): self.xAxisNode = nodes.networkedGestaltNode('X Axis', self.fabnet, filename = '086-005a.py', persistence = self.persistence) self.yAxisNode = nodes.networkedGestaltNode('Y Axis', self.fabnet, filename = '086-005a.py', persistence = self.persistence) self.xyzNode = nodes.compoundNode(self.xAxisNode, self.yAxisNode, self.zAxisNode) def initCoordinates(self): self.position = state.coordinate(['mm', 'mm', 'mm']) def initKinematics(self): self.xAxis = elements.elementChain.forward([elements.microstep.forward(4), elements.stepper.forward(1.8), elements.leadscrew.forward(8), elements.invert.forward(True)]) self.yAxis = elements.elementChain.forward([elements.microstep.forward(4), elements.stepper.forward(1.8), elements.leadscrew.forward(8), elements.invert.forward(True)]) self.stageKinematics = kinematics.direct(2) #direct drive on all axes def initFunctions(self): self.move = functions.move(virtualMachine = self, virtualNode = self.xyNode, axes = [self.xAxis, self.yAxis], kinematics = self.stageKinematics, machinePosition = self.position,planner = 'null') self.jog = functions.jog(self.move) #an incremental wrapper for the move function pass def initLast(self): # self.machineControl.setMotorCurrents(aCurrent = 0.8, bCurrent = 0.8, cCurrent = 0.8) # self.xyNode.setVelocityRequest(0) #clear velocity on nodes. Eventually this will be put in the motion planner on initialization to match state. pass def publish(self): # self.publisher.addNodes(self.machineControl) pass def getPosition(self): return {'position':self.position.future()} def setPosition(self, position = [None]): self.position.future.set(position) def setSpindleSpeed(self, speedFraction): # self.machineControl.pwmRequest(speedFraction) pass #------Servo Class------ class Servo: def moveUp(self): myserial.write('1') time.sleep(2) #serialwrite 1 pass def moveDown(self): myserial.write('0') time.sleep(2) #serialwrite 0 pass class Dc: def pick(self): myserial.write('3') time.sleep(2) #serialwrite 3 pass def place(self): myserial.write('2') time.sleep(2) #serialwrite 2 pass #------IF RUN DIRECTLY FROM TERMINAL------ if __name__ == '__main__': ser = serial.Serial('/dev/cu.usbserial-FTFPCJUB', 115200) # time.sleep(2) stage = virtualMachine(persistenceFile = "test.vmp") #stage.xNode.loadProgram('../../../086-005/086-005a.hex') #stage.xNode.setMotorCurrent(1) servoDown = False Dcleft =False stage.xyNode.setVelocityRequest(10) servo = Servo() dc = Dc() csvfile = open('pick_place3.csv', 'r') commands = [] for line in csvfile.readlines(): array = line.split(';') command = array[0] parameters = array[1] directions = parameters.split(',') xMove = directions[0] yMove = directions[1] if(array[0] == "Start"): print " --- MOVE TO --- " servo.moveUp() servoDown = False stage.move([xMove,yMove], 0) xStatus = stage.xAxisNode.spinStatusRequest() # This checks to see if the move is done. while xStatus['stepsRemaining'] > 0: time.sleep(0.001) xStatus = stage.xAxisNode.spinStatusRequest() elif(array[0] == "go"): print " --- MOVE BACK --- " servoDown = False stage.move([xMove,yMove], 0) xStatus = stage.xAxisNode.spinStatusRequest() # This checks to see if the move is done. while xStatus['stepsRemaining'] > 0: time.sleep(0.001) xStatus = stage.xAxisNode.spinStatusRequest() elif(array[0] == "moveBack"): print " --- MOVE BACK --- " servoDown = False stage.move([xMove,yMove], 0) xStatus = stage.xAxisNode.spinStatusRequest() # This checks to see if the move is done. while xStatus['stepsRemaining'] > 0: time.sleep(0.001) xStatus = stage.xAxisNode.spinStatusRequest() elif (array[0] == "MoveDown"): print " --- MOVE DOWN --- " if(servoDown == False): servo.moveDown() servoDown = True else: pass print "move to " + xMove + " - " + yMove stage.move([xMove,yMove], 0) xStatus = stage.xAxisNode.spinStatusRequest() # This checks to see if the move is done. while xStatus['stepsRemaining'] > 0: time.sleep(0.001) xStatus = stage.xAxisNode.spinStatusRequest() elif (array[0] == "Pick"): print " --- Pick --- " if(Dcleft == False): Dc.pick() DcLeft = True else: pass print "pick " + xMove + " - " + yMove stage.move([xMove,yMove], 0) xStatus = stage.xAxisNode.spinStatusRequest() # This checks to see if the move is done. while xStatus['stepsRemaining'] > 0: time.sleep(0.001) xStatus = stage.xAxisNode.spinStatusRequest() elif (array[0] == "MoveUp"): print " --- MOVE Up --- " servo.moveUp() servoDown = False else: pass print "move Up " + xMove + " - " + yMove stage.move([xMove,yMove], 0) xStatus = stage.xAxisNode.spinStatusRequest() # This checks to see if the move is done. while xStatus['stepsRemaining'] > 0: time.sleep(0.001) xStatus = stage.xAxisNode.spinStatusRequest() elif (array[0] == "Place"): print " --- Place --- " if(servoDown == False): servo.moveDown() servoDown = True else: pass Dc.place() Dcleft = False print "place " + xMove + " - " + yMove stage.move([xMove,yMove], 0) xStatus = stage.xAxisNode.spinStatusRequest() # This checks to see if the move is done. while xStatus['stepsRemaining'] > 0: time.sleep(0.001) xStatus = stage.xAxisNode.spinStatusRequest() elif (array[0] == "pause"): print " --- PAUSE --- " print "move to origins" servo.moveUp() stage.move([0,0],0) xStatus = stage.xAxisNode.spinStatusRequest() # This checks to see if the move is done. while xStatus['stepsRemaining'] > 0: time.sleep(0.001) xStatus = stage.xAxisNode.spinStatusRequest() Moves = [[0,0],[0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [-2,18], [-2,18], [-2,18], [-2,18], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [-2,14], [-2,14], [-2,14], [2,14], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [2,14], [2,14], [2,14], [2,14],[0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [6,14], [6,14], [6,14], [6,14], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [-6,10], [-6,10], [-6,10], [-6,10], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [-2,10], [-2,10], [-2,10], [-2,10], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [2,10], [2,10], [2,10], [2,10], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [6,10], [6,10], [6,10], [6,10], [0,0], [0,-4], [0,-4], [0,-4], [0,-4] [0,0], [10,10], [10,10], [10,10], [10,10], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [14,10], [14,10], [14,10], [14,10], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [18,10], [18,10], [18,10], [18,10], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [-6,6], [-6,6], [-6,6], [-6,6], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [-2,6], [-2,6], [-2,6], [-2,6], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [2,6], [2,6], [2,6], [2,6], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [6,6], [6,6], [6,6], [6,6], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [10,6], [10,6], [10,6], [10,6], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [14,6], [14,6], [14,6], [14,6], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [-10,2], [-10,2], [-10,2], [-10,2], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [-6,2], [-6,2], [-6,2], [-6,2], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [-2,2], [-2,2], [-2,2], [-2,2], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [2,2], [2,2], [2,2], [2,2], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [6,2], [6,2], [6,2], [6,2], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [10,2], [10,2], [10,2], [10,2], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [-14,-2], [-14,-2], [-14,-2], [-14,-2], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [-10,-2], [-10,-2], [-10,-2], [-10,-2], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [-6,-2], [-6,-2], [-6,-2], [-6,-2], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [-2,-2], [-2,-2], [-2,-2], [-2,-2], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [2,-2], [2,-2], [2,-2], [2,-2], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [6,-2], [6,-2], [6,-2], [6,-2], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [10,-2], [10,-2], [10,-2], [10,-2], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [-18,-6], [-18,-6], [-18,-6], [-18,-6], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [-10,-6], [-10,-6], [-10,-6], [-10,6], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [-6,-6], [-6,-6], [-6,-6], [-6,-6], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [-2,-6], [-2,-6], [-2,-6], [-2,-6], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [2,-6], [2,-6], [2,-6], [2,-6], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [-6,-6], [-6,-6], [-6,-6], [-6,-6], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [10,-6], [10,-6], [10,-6], [10,-6], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [-6,-10], [-6,-10], [-6,-10], [-6,-10], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [-2,-10], [-2,-10], [-2,-10], [-2,-10], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [6,-10], [6,-10], [6,-10], [6,-10], [0,0], [0,-4], [0,-4], [0,-4], [0,-4], [0,0], [10,-10], [10,-10], [10,-10], [10,-10], [0,0], [0,0]] for move in moves: stages.move(move, 0) xStatus = stage.xAxisNode.spinStatusRequest() # This checks to see if the move is done. while xStatus['stepsRemaining']: time.sleep(0.00001) xStatus = stage.xAxisNode.spinStatusRequest() """