Mechanical design, Machine design
Assignment for 2 weeks
- Group assignment:
- Make a machine
- Including the end effector
- Build the passive parts and operate it manually
- Automate your machine
- Document the group project and your individual contribution
- Link to this week’s homeworkpages: Mechanical Design and Machine Design
This page focusses mainly on my contribution. See the group page for a more global vision of the group work.
Automation of the machine
In the group assignment, I was personnally more involved in the automation of the machine using the received MTM kit i.e RS485 cable, 4 gestalt node boards and motors. The goal was to build a network of gestalt nodes that could be reconfigured (thanks to a push button on the board that is used to identify the node on the network) to drive x, y, z , e axis motors.
I read and followed the gestalt node tutorial. As the RS485 cable came with a USB connector at one end and unsoldered wires at the other end, it was necessary to make the bridge board and solder the wires to it.
Making the bridge board
The bridge board connects the host computer via the RS485 cable, the power supply that provides the power required by the motors and the first gestalt node of the network.
I downloaded the design Eagle .sch and .brd files from the fabnet page that is part of the Fab in a box project website
and made the board.
Installing the software
To build the network and talk to the fabnets, it is necessary to install python (in my case it was already installed on my MacBook Pro), the pySerial libarary (download then tar.gz file, untar/unzip using tar -xzf pyserial-3.0.1.tar.gz
and install using sudo python setup.py install
in the created folder.) and pygestalt from Nadya's github page (download, unzip into pygestalt-master). Other ressources can be found here.
In the pygestalt-master folder, install using sudo python ./setup.py install
.
running install_lib
creating /Users/vjmdupuis/Library/Enthought/Canopy_64bit/User/lib/python2.7/site-packages/pygestalt
copying build/lib/pygestalt/__init__.py -> /Users/vjmdupuis/Library/Enthought/Canopy_64bit/User/lib/python2.7/site-packages/pygestalt
Testing
The pygestalt-master folder contains many files and subfolders. It's not obvious to know what is necessary and what is not. To test my system I went to subfolder ./examples/machines/htmaa/
. There one finds several files (086-005a.py, hbot.py but also single_node.py and xy_plotter.py). I edited the single_node.py:
# 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
#------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/ttyUSB0'))
def initControllers(self):
self.xAxisNode = nodes.networkedGestaltNode('X Axis', self.fabnet, filename = '086-005a.py', persistence = self.persistence)
self.xNode = nodes.compoundNode(self.xAxisNode)
def initCoordinates(self):
self.position = state.coordinate(['mm'])
def initKinematics(self):
self.xAxis = elements.elementChain.forward([elements.microstep.forward(4), elements.stepper.forward(1.8), elements.leadscrew.forward(6.096), elements.invert.forward(True)])
self.stageKinematics = kinematics.direct(1) #direct drive on all axes
def initFunctions(self):
self.move = functions.move(virtualMachine = self, virtualNode = self.xNode, axes = [self.xAxis], 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.xyzNode.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__':
stage = virtualMachine(persistenceFile = "test.vmp")
#stage.xNode.loadProgram('../../../086-005/086-005a.hex')
#stage.xNode.setMotorCurrent(1)
stage.xNode.setVelocityRequest(8)
#f = open('path.csv','r')
#supercoords = []
#for line in f.readlines():
# supernum = float(line)
# supercoords.append([supernum])
supercoords = [[10],[20],[10],[0]]
for coords in supercoords:
stage.move(coords, 0)
status = stage.xAxisNode.spinStatusRequest()
while status['stepsRemaining'] > 0:
time.sleep(0.001)
status = stage.xAxisNode.spinStatusRequest()
The code starts with import commands. It then defines a virtualMachine
object with different methods: initInterfaces
, initControllers
, ... The initInterfaces
method initializes the serial communication interfaces.serialInterface(baudRate = 115200, interfaceType = 'ftdi', portName = '/dev/ttyUSB0')
and has to be modifed to match the specific hardware on your machine, in particular the port name here '/dev/ttyUSB0'
must be changed. To know the name attributed to the RS485 interface after it has been plugged, one can use the following commands in a terminal:
$ ls /dev/tty.usb*
In my case, the RS485 interface appeared as '/dev/tty.usbserial-FTZ565GG'
. So I made the changed, turned the power supply (12V, 1A) on and typed
$ python single_node.py
and pressed the button on the gestalt node to identify it as an X axis.
single_node.py: Warning: setting persistence without providing a name to the virtual machine can result in a conflict in multi-machine persistence files.
FABNET: port /dev/tty.usbserial-FTZ565GG connected succesfully.
X Axis: please identify me on the network.
X Axis: Could not reach virtual node. Retrying (#2)
X Axis: Could not reach virtual node. Retrying (#3)
X Axis: Could not reach virtual node. Retrying (#4)
X Axis: Could not reach virtual node. Retrying (#5)
X Axis: http://www.fabunit.com/vn/086-005a.py
X Axis: RUNNING IN APPLICATION MODE
X Axis: loaded node from: 086-005a.py
26
{'writePosition': 1, 'stepsRemaining': 0, 'readPosition': 0, 'currentKey': 0, 'statusCode': 1}
{'writePosition': 2, 'stepsRemaining': 216, 'readPosition': 1, 'currentKey': 0, 'statusCode': 1}
{'writePosition': 3, 'stepsRemaining': 178, 'readPosition': 1, 'currentKey': 0, 'statusCode': 1}
{'writePosition': 4, 'stepsRemaining': 159, 'readPosition': 1, 'currentKey': 0, 'statusCode': 1}
{'writePosition': 5, 'stepsRemaining': 119, 'readPosition': 1, 'currentKey': 0, 'statusCode': 1}
{'writePosition': 6, 'stepsRemaining': 82, 'readPosition': 1, 'currentKey': 0, 'statusCode': 1}
{'writePosition': 7, 'stepsRemaining': 0, 'readPosition': 6, 'currentKey': 5, 'statusCode': 1}
{'writePosition': 8, 'stepsRemaining': 216, 'readPosition': 7, 'currentKey': 6, 'statusCode': 1}
{'writePosition': 9, 'stepsRemaining': 176, 'readPosition': 7, 'currentKey': 6, 'statusCode': 1}
{'writePosition': 10, 'stepsRemaining': 138, 'readPosition': 7, 'currentKey': 6, 'statusCode': 1}
{'writePosition': 11, 'stepsRemaining': 118, 'readPosition': 7, 'currentKey': 6, 'statusCode': 1}
{'writePosition': 12, 'stepsRemaining': 80, 'readPosition': 7, 'currentKey': 6, 'statusCode': 1}
{'writePosition': 13, 'stepsRemaining': 0, 'readPosition': 12, 'currentKey': 11, 'statusCode': 1}
{'writePosition': 14, 'stepsRemaining': 216, 'readPosition': 13, 'currentKey': 12, 'statusCode': 1}
{'writePosition': 15, 'stepsRemaining': 177, 'readPosition': 13, 'currentKey': 12, 'statusCode': 1}
{'writePosition': 16, 'stepsRemaining': 139, 'readPosition': 13, 'currentKey': 12, 'statusCode': 1}
{'writePosition': 17, 'stepsRemaining': 107, 'readPosition': 13, 'currentKey': 12, 'statusCode': 1}
{'writePosition': 18, 'stepsRemaining': 86, 'readPosition': 13, 'currentKey': 12, 'statusCode': 1}
{'writePosition': 19, 'stepsRemaining': 0, 'readPosition': 18, 'currentKey': 17, 'statusCode': 1}
{'writePosition': 20, 'stepsRemaining': 216, 'readPosition': 19, 'currentKey': 18, 'statusCode': 1}
{'writePosition': 21, 'stepsRemaining': 196, 'readPosition': 19, 'currentKey': 18, 'statusCode': 1}
{'writePosition': 22, 'stepsRemaining': 161, 'readPosition': 19, 'currentKey': 18, 'statusCode': 1}
{'writePosition': 23, 'stepsRemaining': 121, 'readPosition': 19, 'currentKey': 18, 'statusCode': 1}
{'writePosition': 24, 'stepsRemaining': 99, 'readPosition': 19, 'currentKey': 18, 'statusCode': 1}
and the motor moved back and forth. Doing so, the program created a persistence file named test.vmp
that has to be removed if one wants to run another python script to control the motors and another file 086-005a.pyc
. I noticed that when I tried to run the second example xy_plotter.py
.
# Two stage example Virtual Machine file
# moves get set in Main
# usb port needs to be set in initInterfaces
# Nadya Peek Dec 2014
#------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
#------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-FTZ565GG'))
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.xyNode = nodes.compoundNode(self.xAxisNode, self.yAxisNode)
def initCoordinates(self):
self.position = state.coordinate(['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(False)])
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.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
stages.xyNode.setVelocityRequest(8)
# Some random moves to test with
moves = [[10,10],[20,20],[10,10],[0,0]]
# Move!
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()
Running this script, I could see the two motors move. At this stage, I showed the results to the group and explained in particular everything I did and learned to Christian Simon so that we could go further while Andreas Feron and Patxi Arruabarrena were experimenting with a RepRap based system with gcode to control the motors of the machine.
With Christian, we decided to go beyond the examples and modify the xy_plotter.py
file to add a third motor. The code xyz_machine.py
is the following and can be downloaded here.
# Two stage example Virtual Machine file
# moves get set in Main
# usb port needs to be set in initInterfaces
# Nadya Peek Dec 2014
#------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
#------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-FTZ565GG'))
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)
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(False)])
self.zAxis = elements.elementChain.forward([elements.microstep.forward(4), elements.stepper.forward(1.8), elements.leadscrew.forward(8), elements.invert.forward(False)])
# CS check this:
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
stages.xyzNode.setVelocityRequest(8)
# Some random moves to test with
moves = [[10,10,10],[20,20,20],[10,20,20],[10,10,20],[10,10,10]]
# Move!
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()
Using those files it is possible to operate the machine by changing the sequence of moves. For one motor supercoords = [[10],[20],[10],[0]]
, for two moves = [[10,10],[20,20],[10,10],[0,0]]
and for three motors moves = [[10,10,10],[20,20,20],[10,20,20],[10,10,20],[10,10,10]]
. We would need a program to convert paths in a svg file for instance in a series of moves.