MTM : Programming

# The Picker Machine Code was developed by Javier Burón
García and Pilu Caballero during Machine design Assignment.
Based on Nadya Peek xy_plotter example.Two stage example Virtual
Machine file moves get set in Main.
# Dec 2014

# The Picker Machine Team are : Juan García, Daniel Amigo, Alvaro la Roja, Pilu Caballero and Javier Burón.
# FabLab CEU Madrid
# FabAcademy 2017
# The code is under Picker Machine License

# THE PICKER MACHINE : Our machine need to Pick and Cut A musicSheet based in 30 notes with 70 mm for works.
# The distance beetwen each notes are 2mm so the X Axis have to move there
# The punchHeader going DOWN/UP for make holes. Y Axis Moves.
# Finally cuts done the Z Axis, roll the MusicSheet every 4mm.

#------IMPORTS------- pyGestalt main Functions
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

# Variables : We defined the main variables as you see below
# X Axis : Has 30 Notes with a space between them : 2mm
# Y Axis : Does the perforation. It needs to move a full turn to do a perforation.
# Z Axis : Is the roller motor, there is a 4mm space between each line of the song.
# Our first attended we made an array based in the stepper moves 8, 16, 24... for x axis and Initialize Notes and Songs with the idea to change later for 30 notes.
# We was coding without the machine so was difficult for test the codes made but an array is an array with machine or not and we know each axis moves commented below.
# Javi had the stepper used for header design so gently could to try the first things and modify and advance.

# JAVIBG INITIALIZE NOTES AND SONG ARRAYS
#  x axis UNITS:
 MM

x_notes = {
'C3': 0, 'D3': 2, 'G3': 4, 'A3': 6, 'B3': 8,
'C4': 10, 'D4': 12, 'E4': 14, 'F4': 16, 'Gb4': 18, 'G4': 20, 'Ab4': 22, 'A4': 24,
'Bb4': 26, 'B4': 28,
'C5': 30, 'Db5': 32, 'D5': 34, 'Eb5': 36, 'E5': 38, 'F5': 40, 'Gb5': 42, 'G5': 44,
'Ab5': 46, 'A5': 48, 'Bb5': 50, 'B5': 52,
'C6': 54, 'D6': 56, 'E6': 58
}

# y axis UNITS: rev
y_perforation = 1

# z axis  UNITS: MM
z_roller = 4

#  SECOND DEFINE AN ARRAY TO WRITE THE SONG, EACH VECTOR IS ONE LINE, THIS SONG IS C MAJOR SCALE AND C MAJOR CHORD
song = [['Bb5', 'Bb4', 'F5'], [], [], [], [], [], ['Bb4'], [], [], [], [], [], ['Bb4', 'F5', 'D5'], [], [], [], [], [], ['Bb4'], [], [], [], [], [], ['Eb5', 'Bb4'], [], ['D5'], [], ['C5'], [], ['Bb5', 'Bb4', 'F5'], [], [], [], [], [], ['Bb4'], [], [], [], [], [], ['Bb4', 'D5', 'F5'], [], [], [], [], [], ['Eb5', 'Bb4'], [], ['D5'], [], ['C5'], [], ['Eb5', 'Bb5', 'Bb4'], [], [], [], [], [], ['Bb4'], [], [], [], [], [], ['Bb4', 'D5', 'F5'], [], [], [], [], [], ['Eb5', 'C5'], [], ['B4', 'D5'], [], ['Eb5', 'C5'], [], ['A4', 'C5'], [], [], [], [], [], ['A4'], [], [], [], [], [], ['F4'], [], [], [], ['F4'], [], ['G4'], [], [], [], [], [], ['Bb4'], [], [], ['G4'], [], [], ['Eb5', 'Bb4'], [], [], ['D5'], [], [], ['C5'], [], [], ['Bb4'], [], [], ['Bb4'], [], ['C5'], [], ['D5'], [], ['C5'], [], [], [], ['G4'], [], ['A4'], [], [], [], [], [], ['F4'], [], [], [], ['F4'], [], ['G4'], [], [], [], [], [], ['Bb4'], [], [], ['G4'], [], [], ['Eb5', 'Bb4'], [], [], ['D5'], [], [], ['C5'], [], [], ['Bb4'], [], [], ['Db5', 'F5'], [], [], [], [], [], ['A4', 'C5'], [], [], [], [], [], ['F4'], [], [], [], [], [], ['C4'], [], [], ['F4'], [], [], [], ['G4'], [], [], [], [], [], ['Bb4'], [], [], ['G4'], [], [], ['Eb5', 'Bb4'], [], [], ['D5'], [], [], ['C5'], [], [], ['Bb4'], [], [], ['Bb4'], [], ['C5'], [], ['D5'], [], ['C5'], [], [], [], ['G4'], [], ['F4', 'A4'], [], [], [], [], [], ['F4', 'F5'], [], [], [], ['F5'], [], ['Bb5', 'Gb5'], [], [], [], ['F5', 'Ab5'], [], ['Eb5', 'Gb5'], [], [], [], ['Db5', 'F5'], [], ['Eb5', 'C5'], [], [], [], ['Bb4', 'Db5'], [], ['Ab4', 'C5'], [], [], [], ['Gb4', 'Bb4'], [], ['F4', 'F5']]


#------VIRTUAL MACHINE------ Main functions
class virtualMachine(machines.virtualMachine):

#------Interfaces functions for RS 485 Serial Port Communication.

def initInterfaces(self):
# We need to change the name of our FabNet usbPort here. In my our case was : '/dev/tty.usbserial-FTZ29L92'
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-FTZ29L92'))

#------Nodes/drivers Function for Gestalt Nodes. We modified the basic xy_plotter example adding zAxisNode and   compounds
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.zAxisNode = nodes.networkedGestaltNode('Z Axis', self.fabnet, filename = '086-005a.py', persistence = self.persistence)
self.xyzNode = nodes.compoundNode(self.xAxisNode, self.yAxisNode, self.zAxisNode )

#------Function for state and position coordinates. We modified adding our apropiated Y axis variable in revolutions : ´rev´
def initCoordinates(self):

# JAVIBG Y MOTOR COORDINATE IN REVOLUTIONS
 self.position = state.coordinate(['mm', 'rev' , 'mm' ])

#------Kinematics function for storage the initial arguments for each motor . This function must to adjust testing each step for each axis .
# We added the z Kinematics and the number for direct drive all axes to 3
 def initKinematics(self):

self.xAxis = elements.elementChain.forward([elements.microstep.forward(4), elements.stepper.forward(1.8), elements.leadscrew.forward(7), elements.invert.forward(False)])
self.yAxis = elements.elementChain.forward([elements.microstep.forward(4), elements.stepper.forward(1.8), elements.leadscrew.forward(8), elements.invert.forward(False)])
self.zAxis = elements.elementChain.forward([elements.microstep.forward(4), elements.stepper.forward(1.8), elements.leadscrew.forward(8), elements.invert.forward(False)])
self.stageKinematics = kinematics.direct(3) #direct drive on all axes

 def initFunctions(self):

self.move = functions.move(virtualMachine = self, virtualNode = self.xyzNode, axes = [self.xAxis, self.yAxis, self.zAxis ] , 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.xNode.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

#------IF RUN DIRECTLY FROM TERMINAL------
if __name__ == '__main__':
	# The persistence file remembers the node you set. It'll generate the first time you run the
	# file. If you are hooking up a new node, delete the previous persistence file.
	stages = virtualMachine(persistenceFile = "test.vmp")

	# You can load a new program onto the nodes if you are so inclined. This is currently set to
	# the path to the 086-005 repository on Nadya's machine.
	#stages.xyNode.loadProgram('../../../086-005/086-005a.hex')

	# This is a widget for setting the potentiometer to set the motor current limit on the nodes.
	# The A4982 has max 2A of current, running the widget will interactively help you set.
	#stages.xyNode.setMotorCurrent(0.7)

	# This is for how fast the stepper moves, we can change for precision more slow.
	stages.xyzNode.setVelocityRequest(8)
	# Some random moves to test with moves = [[10,10,10], [20,20,20], [0,0,0]]
    # Move!
    

    # JAVIBG
    # Movement is defined
    # 0. for each line in song
    # 1. position the z_roller
    # 2. if not empty then for each note
    # 3. move the first x_note in the row
    # 4. make the perforation with y_perforation one revolution
    # 5. go back to the x_note 0
    # 6. end for each note
    # 7. end for each line

    moves = [[0,0,0]] # Iniatilize moves Variable
    current_z = 0 # Variable z value for initialize
        
    # conditional about move in lines
  
        if line:
            for note in line:
                aux = [x_notes[note],0, current_z]
                moves.append(aux)
                aux2 = [x_notes[note], y_perforation, current_z]
                moves.append(aux2)
            aux3 = [0,0,current_z]
            moves.append(aux3)
        current_z += z_roller
        moves.append([0,0,current_z])

        # JAVIBG this is the moves array for the current song. moves = [[0, 0, 0], [10, 0, 0], [10, 1, 0], [0, 0, 0], [0, 0, 4], [0, 0, 8], [12, 0, 8], [12, 1, 8], [0, 0, 8], [0, 0, 12], [0, 0, 16], [14, 0, 16], [14, 1, 16], [0, 0, 16], [0, 0, 20], [0, 0, 24], [16, 0, 24], [16, 1, 24], [0, 0, 24], [0, 0, 28], [0, 0, 32], [20, 0, 32], [20, 1, 32], [0, 0, 32], [0, 0, 36], [0, 0, 40], [24, 0, 40], [24, 1, 40], [0, 0, 40], [0, 0, 44], [0, 0, 48], [28, 0, 48], [28, 1, 48], [0, 0, 48], [0, 0, 52], [0, 0, 56], [0, 0, 60], [0, 0, 64], [0, 0, 68], [10, 0, 68], [10, 1, 68], [14, 0, 68], [14, 1, 68], [20, 0, 68], [20, 1, 68], [0, 0, 68], [0, 0, 72]]
    
	for move in moves:
                stages.move(move, 0)
		status = stages.xAxisNode.spinStatusRequest()
		# This checks to see if the move is done.
        while status['stepsRemaining'] > 0:
			time.sleep(0.001)
			status = stages.xAxisNode.spinStatusRequest()