def drill(self, face, x, z, depth):
"""Uses the drill to drill a hole.
Args:
face (string): The face being worked on (front, top, left, right, back).
x (double): The horizontal displacement of the drill from center.
z (double): The vertical displacement from the face's bottom of the drill's center point.
depth (double): Depth of the drill into the foam.
"""
commandString = ', '.join([
'drill(' + '"' + str(face) + '"',
str(x),
str(z),
str(depth) + ')'
])
self.controller.writeToHistory(commandString)
controller = self.controller
# Align drill
self.moveTo(controller.drill, x, z, face=face)
# Drill in
controller.addCommand(
CombinedCommand([
PushCommand(controller.drill, depth, controller),
SpinCommand(controller.drill)
], 'Drill In'))
# Pull drill out
self.retractDrill(spinning=True)
def homeHandler(self):
"""Homes all the limit switches for the handler."""
self.controller.addCommand(
FlipCommand(self.controller.handler, 'down', home=True))
self.controller.addCommand(
CombinedCommand([
ShiftCommand(self.controller.drill,
self.controller.handler,
0,
inAbsolute=True,
home=True,
rapid=True),
SpinCommand(self.controller.handler, 0, home=True),
]))
def retractDrill(self, spinning=False):
"""Stops the drill spin and pulls the drill back to base position."""
if spinning:
# Retract slowly while spinning
motorStartDepthOffset = configurationMap['drill']['offsets'][
'motorStartDepthOffset']
self.controller.addCommand(
CombinedCommand([
SpinCommand(self.controller.drill),
PushCommand(self.controller.drill, -motorStartDepthOffset,
self.controller)
]))
self.controller.addCommand(
PushCommand(self.controller.drill,
0,
self.controller,
home=True,
rapid=True))
def retractMill(self, spinning=False):
"""Stops the mill spin and pulls the mill back to base position."""
commandString = 'controller.commandGenerator.retractMill(spinning=' + str(
spinning) + ')'
self.controller.writeToHistory(commandString)
if spinning:
# Retract slowly while spinning
motorStartDepthOffset = configurationMap['mill']['offsets'][
'motorStartDepthOffset']
self.controller.addCommand(
CombinedCommand([
SpinCommand(self.controller.mill),
PushCommand(self.controller.mill, -motorStartDepthOffset,
self.controller)
]))
self.controller.addCommand(
PushCommand(self.controller.mill,
0,
self.controller,
home=True,
rapid=True))
def top():
return CombinedCommand([
SpinCommand(self.handler, 0),
FlipCommand(self.handler, 'up')
])
def right():
return CombinedCommand([
SpinCommand(self.handler, 270),
FlipCommand(self.handler, 'down')
])
def left():
return CombinedCommand([
SpinCommand(self.handler, 90),
FlipCommand(self.handler, 'down')
])
def back():
return CombinedCommand([
SpinCommand(self.handler, 180),
FlipCommand(self.handler, 'down')
])
def front():
return CombinedCommand([
SpinCommand(self.handler, 0),
FlipCommand(self.handler, 'down')
])
def millPointsSequence(self, ptsList, depth, face, closedLoop=True):
"""Uses the mill to cut through a sequence of points in order.
Args:
ptsList (List(tuples)): A list of x, z coordinate tuples for the path you want the mill to follow.
depth (double): The depth of the path in the foam.
face (String): The face you want to work on.
"""
commandString = ', '.join([
'millPointSequence(' + str(ptsList),
str(depth), '"' + face + '")'
])
self.controller.writeToHistory(commandString)
syncSpeed = min(configurationMap['handler']['railSpeed'],
configurationMap['mill']['raiseSpeed'])
# Go to starting point
(x0, z0) = ptsList[0] if len(ptsList) > 0 else (0, 0)
if not closedLoop:
self.moveTo(self.controller.mill, x0, z0, face=face)
# Push in with mill to depth
self.controller.addCommand(
CombinedCommand([
PushCommand(self.controller.mill, depth, self.controller),
SpinCommand(self.controller.mill)
]))
# Make mill go through all the points in sequence
sequenceCommand = SequentialCommand([])
for (i, (x, z)) in enumerate(ptsList):
if i != 0:
x0, z0 = ptsList[i - 1]
dx = abs(x - x0)
dz = abs(z - z0)
distanceMoved = max(0.0001, math.sqrt(dx**2 + dz**2))
dt = distanceMoved / syncSpeed
sequenceCommand.addCommand(
CombinedCommand([
SpinCommand(self.controller.mill),
RaiseCommand(self.controller.mill,
z,
self.controller,
startSpeed=max(0.1, dz / dt)),
ShiftCommand(self.controller.mill,
self.controller.handler,
x,
startSpeed=max(0.1, dx / dt)),
]))
if closedLoop:
# Mill back to first command
(x, z) = ptsList[len(ptsList) - 1]
(x0, z0) = ptsList[len(ptsList) - 2]
dx = abs(x - x0)
dz = abs(z - z0)
distanceMoved = math.sqrt(dx**2 + dz**2)
dt = distanceMoved / syncSpeed
sequenceCommand.addCommand(
CombinedCommand([
SpinCommand(self.controller.mill),
RaiseCommand(self.controller.mill,
z,
self.controller,
startSpeed=min(0.1, abs(dz / dt))),
ShiftCommand(self.controller.mill,
self.controller.handler,
x,
startSpeed=min(0.1, abs(dx / dt))),
]))
self.controller.addCommand(sequenceCommand)
# Retract mill
self.retractMill(spinning=True)
def intrude(self, face, x0, x1, z0, z1, d0, d1, radius):
"""Creates an intrusion from one point to another with a set radius.
Args:
face (string): The face being worked on (front, top, left, right, back).
x0 (double): The initial horizontal displacement.
x1 (double): The final horizontal displacement.
z0 (double): The initial vertical displacement from the face's bottom of the circle's center point.
z1 (double): The initial vertical displacement from the face's bottom of the circle's center point.
d0 (double): The initial depth.
d1 (double): The final depth.
radius (double): Radius of the circle being cut out.
"""
controller = self.controller
if z0 > z1:
zLow = z1
xLow = x1
dLow = d1
zHigh = z0
xHigh = x0
dHigh = d0
else:
zLow = z0
xLow = x0
dLow = d0
zHigh = z1
xHigh = x1
dHigh = d1
millRadius = configurationMap['mill']['diameter'] / 2
speed = configurationMap['coordination']['speed']
# Calculate speed coordinations
xDiff = abs(x1 - x0)
zDiff = abs(z1 - z0)
dDiff = abs(d1 - d0)
maxDiff = max([xDiff, zDiff, dDiff])
xSpeed = speed * xDiff / maxDiff if xDiff != 0 else 1
zSpeed = speed * zDiff / maxDiff if zDiff != 0 else 1
dSpeed = speed * dDiff / maxDiff if dDiff != 0 else 1
tiltAngle = np.arctan2(zHigh - zLow, xHigh - xLow) + np.pi / 2
r = radius - millRadius
if r < 0:
print('WARNING intrude radius too small')
# Start at top left
self.moveTo(controller.mill,
xHigh - r * np.cos(tiltAngle),
zHigh - r * np.sin(tiltAngle),
0,
face=face)
# Push in
self.controller.addCommand(
CombinedCommand([
SpinCommand(controller.mill),
PushCommand(controller.mill, dHigh, controller)
]))
radiusRange = np.arange(radius - millRadius, 0, -millRadius * 2)
if float(radius) == float(millRadius):
radiusRange = np.append(radiusRange, 0)
for r in radiusRange:
# Half circle around to right hand side
self.millArcDiscrete(face, xHigh, zHigh, r, dHigh,
math.pi + tiltAngle, 2 * math.pi + tiltAngle)
# Move down to bottom right
self.controller.addCommand(
CombinedCommand([
SpinCommand(controller.mill),
RaiseCommand(controller.mill,
zLow + r * np.sin(tiltAngle),
controller,
startSpeed=zSpeed),
ShiftCommand(controller.mill,
controller.handler,
xLow + r * np.cos(tiltAngle),
startSpeed=xSpeed),
PushCommand(controller.mill,
dLow,
controller,
startSpeed=dSpeed)
]))
# Half circle around to left hand side
self.millArcDiscrete(face, xLow, zLow, r, dLow, 0 + tiltAngle,
math.pi + tiltAngle)
# Move up to top left
self.controller.addCommand(
CombinedCommand([
SpinCommand(controller.mill),
RaiseCommand(controller.mill,
zHigh - r * np.sin(tiltAngle),
controller,
startSpeed=zSpeed),
ShiftCommand(controller.mill,
controller.handler,
xHigh - r * np.cos(tiltAngle),
startSpeed=xSpeed),
PushCommand(controller.mill,
dHigh,
controller,
startSpeed=dSpeed)
]))
self.retractMill()
def millCircle(self, face, x, z, radius, depth):
"""Mills a full circle cut out from the center point of the mill using smooth curves.
Args:
face (string): The face being worked on (front, top, left, right, back).
x (double): The horizontal displacement from the face's center of the center of the circle's center point.
z (double): The vertical displacement from the face's bottom of the circle's center point.
radius (double): Radius of the circle being cut out.s
depth (double): Depth of the circle being cut out.
"""
# Set starting location
# Start with top right quadrant
actualZ = self.controller.currentFaceHeight - z
currentZ = actualZ
currentX = x + radius
self.selectFace(face)
self.controller.addCommand(
CombinedCommand([
ShiftCommand(self.controller.mill,
self.controller.handler,
currentX,
rapid=True),
RaiseCommand(self.controller.mill,
currentZ,
self.controller,
rapid=True),
]))
self.controller.addCommand(
CombinedCommand([
PushCommand(self.controller.mill, depth, self.controller),
SpinCommand(self.controller.mill),
]))
# Move to right
fastSpeed = 30
slowSpeed = 5
timeTaken = getLinearVelocityTime(fastSpeed, slowSpeed, radius)
constantSpeed = radius / timeTaken
# Curve to top
self.controller.addCommand(
CombinedCommand([
SpinCommand(self.controller.mill),
RaiseCommand(self.controller.mill,
actualZ - radius,
self.controller,
startSpeed=slowSpeed,
endSpeed=fastSpeed * 2),
ShiftCommand(self.controller.mill,
self.controller.handler,
x,
startSpeed=fastSpeed,
endSpeed=slowSpeed)
]))
# Curve to left
self.controller.addCommand(
CombinedCommand([
SpinCommand(self.controller.mill),
RaiseCommand(self.controller.mill,
actualZ,
self.controller,
startSpeed=constantSpeed),
ShiftCommand(self.controller.mill,
self.controller.handler,
x - radius,
startSpeed=slowSpeed,
endSpeed=fastSpeed)
]))
# Curve to bottom
self.controller.addCommand(
CombinedCommand([
SpinCommand(self.controller.mill),
RaiseCommand(self.controller.mill,
actualZ + radius,
self.controller,
startSpeed=constantSpeed),
ShiftCommand(self.controller.mill,
self.controller.handler,
x,
startSpeed=fastSpeed,
endSpeed=slowSpeed)
]))
# Curve back to right
self.controller.addCommand(
CombinedCommand([
SpinCommand(self.controller.mill),
RaiseCommand(self.controller.mill,
currentZ,
self.controller,
startSpeed=constantSpeed),
ShiftCommand(self.controller.mill,
self.controller.handler,
currentX,
startSpeed=slowSpeed,
endSpeed=fastSpeed)
]))
def millArcDiscrete(self,
face,
x,
z,
radius,
depth,
startAngle,
endAngle,
moveTo=False):
"""Uses the mill to cut out in an arc shape.
All variables are relative to the center of the mill cutting tool so functions that use this command will need
to keep in mind to offset by the mill cutting tool's radius if needed. The starting and ending angles are all
in degrees. The zero starts on the right axis and moves in the clockwise direction.
Args:
face (string): The surface being worked on (front, left, right, top, back).
x (double): The horizontal displacement of center point of the mill arc.
z (double): The vertical displacement of the center point of the mill arc.
radius (double): The radius that the mill is working around.
depth (double): The depth of the mill from the foam surface.
startAngle(double): The starting angle of the mill in radians.
endAngle(double): The ending angle of the mill in radians.
"""
sequentialCommand = SequentialCommand([])
# Set starting location
self.selectFace(face)
# Start with top right quadrant
actualZ = z
angle = startAngle
currentX = x + radius * math.cos(angle)
currentZ = actualZ + radius * math.sin(angle)
if moveTo:
self.moveTo(self.controller.mill, currentX, currentZ)
# Shift to correct position
self.controller.addCommand(
CombinedCommand([
ShiftCommand(self.controller.mill, self.controller.handler,
currentX),
RaiseCommand(self.controller.mill, currentZ, self.controller),
SpinCommand(self.controller.mill),
]))
# Insert mill into foam
self.controller.addCommand(
CombinedCommand([
PushCommand(self.controller.mill, depth, self.controller),
SpinCommand(self.controller.mill),
]))
maxStep = 1
minSpeed = 0.5
# Figure out synchronizing speed
moveSpeed = min(configurationMap['handler']['railSpeed'],
configurationMap['mill']['raiseSpeed'])
angleStep = 2 * math.asin(maxStep / (2 * radius))
distPerPoint = 2 * radius * math.sin(angleStep / 2)
timePerPoint = distPerPoint / moveSpeed
if radius != 0:
for i, angle in enumerate(
np.arange(startAngle, endAngle, angleStep)):
prevX = currentX
prevZ = currentZ
currentX = x + radius * math.cos(angle)
currentZ = actualZ + radius * math.sin(angle)
dx = abs(currentX - prevX)
dz = abs(currentZ - prevZ)
# Time for each movement
tx = max(dx / timePerPoint, minSpeed)
tz = max(dz / timePerPoint, minSpeed)
sequentialCommand.addCommand(
CombinedCommand([
SpinCommand(self.controller.mill),
ShiftCommand(self.controller.mill,
self.controller.handler,
currentX,
startSpeed=tx),
RaiseCommand(self.controller.mill,
currentZ,
self.controller,
startSpeed=tz),
]))
self.controller.addCommand(sequentialCommand)
def lathe(self, z0, z1, radius):
"""Uses the lathe to create a radial cut from z0 to z1.
Args:
z0 (double): The initial vertical displacement from the face's bottom of the circle's center point.
z1 (double): The final vertical displacement from the face's bottom of the circle's center point.
radius (double): Radius of the circle being cut out.
"""
commandString = ', '.join(
['lathe(' + str(z0),
str(z1), str(radius) + ')'])
self.controller.writeToHistory(commandString)
controller = self.controller
if z0 > z1:
zBot = z1
zTop = z0
else:
zBot = z0
zTop = z1
# Offset to account for lathe length
latheLength = configurationMap['lathe']['length']
zTop -= latheLength
pushIncrement = configurationMap['lathe']['pushIncrement']
handlerSpinCommand = SpinCommand(controller.handler, rapid=True)
# Set starting positions
self.moveTo(controller.lathe, 0, zTop, face='front')
# Start lathing
maxRadius = math.sqrt(controller.currentFaceDepth**2 + controller.
currentFaceWidth**2) / 2 + 2 # A little safety
sequenceCommand = SequentialCommand([])
# Ramp up speed for 5 rotations
sequenceCommand.addCommand(
SpinCommand(controller.handler, 360 * 5,
configurationMap['handler']['initialRampUpSpeed'],
configurationMap['handler']['rapidSpinSpeed']))
sequenceCommand.addCommand(handlerSpinCommand)
goingUp = True
for currentRadius in np.arange(maxRadius, radius, -pushIncrement):
# Push in
sequenceCommand.addCommand(
CombinedCommand([
PushCommand(controller.lathe,
currentRadius,
controller,
fromCenter=True), handlerSpinCommand
], 'Push Lathe in'))
if goingUp:
# Go up
sequenceCommand.addCommand(
CombinedCommand([
RaiseCommand(controller.lathe, zTop, controller),
handlerSpinCommand
], 'Lathe Up'))
else:
# Back down
sequenceCommand.addCommand(
CombinedCommand([
RaiseCommand(controller.lathe, zBot, controller),
handlerSpinCommand
], 'Lathe Down'))
goingUp = not goingUp
controller.addCommand(sequenceCommand)
# if not controller.useSimulator:
# controller.addCommand(StopCommand())
self.retractLathe()
def cutRectangle(self,
xLeft,
xRight,
zBot,
zTop,
depth,
startingSide='top',
face=None):
"""Cuts out the inside of a rectangle with a mill.
Args:
xLeft (double): The vertical value of the left side of the rectangle.
xRight (double): The vertical value of the right side of the rectangle.
zBot (double): The horizontal value of the bottom side of the rectangle.
zTop (double): The horizontal value of the top side of the rectangle.
depth (double): The depth of the rectangle into the foam.
startingSide (string): The side which we start milling ['top', 'left', 'right'].
face (string): The foam face being worked on.
"""
millDepth = configurationMap['mill']['pushIncrement']
millRadius = configurationMap['mill']['diameter'] / 2
for d in incRange(millDepth, depth, millDepth):
# Cut out rectange from starting side
if startingSide == 'top':
# Start at top left hand side and cut across and down
self.moveTo(self.controller.mill,
xLeft + millRadius,
zTop - millRadius,
face=face)
# Push mill in
self.controller.addCommand(
CombinedCommand([
SpinCommand(self.controller.mill),
PushCommand(self.controller.mill, d, self.controller),
]))
# Cut through rectangle down to bottom
moveRight = True
for z in incRange(zTop - millRadius, zBot + millRadius,
-millRadius):
x = xRight - millRadius if moveRight else xLeft + millRadius
print('god', x, z, zTop, millRadius, zBot, d, moveRight)
# Move down then across (The first down should not move anywhere)
self.controller.addCommand(
CombinedCommand([
SpinCommand(self.controller.mill),
RaiseCommand(self.controller.mill, z,
self.controller)
]))
self.controller.addCommand(
CombinedCommand([
SpinCommand(self.controller.mill),
ShiftCommand(self.controller.mill,
self.controller.handler, x)
]))
moveRight = not moveRight
self.retractMill(spinning=True)
if startingSide == 'left':
# Cut through rectangle down to bottom
for x in incRange(xLeft + millRadius, xRight - millRadius,
millRadius):
# Start at top left hand side and cut across and down
self.moveTo(self.controller.mill,
x,
zTop - millRadius,
face=face)
# Push mill in
self.controller.addCommand(
CombinedCommand([
SpinCommand(self.controller.mill),
PushCommand(self.controller.mill, d,
self.controller),
]))
# Cut down
self.controller.addCommand(
CombinedCommand([
SpinCommand(self.controller.mill),
RaiseCommand(self.controller.mill,
zBot + millRadius, self.controller)
]))
self.retractMill(spinning=True)
if startingSide == 'right':
# Cut through rectangle down to bottom
for x in incRange(xRight - millRadius, xLeft + millRadius,
-millRadius):
# Start at top right hand side and cut across and down
self.moveTo(self.controller.mill,
x,
zTop - millRadius,
face=face)
# Push mill in
self.controller.addCommand(
CombinedCommand([
SpinCommand(self.controller.mill),
PushCommand(self.controller.mill, d,
self.controller),
]))
# Cut down
self.controller.addCommand(
CombinedCommand([
SpinCommand(self.controller.mill),
RaiseCommand(self.controller.mill,
zBot + millRadius, self.controller)
]))
self.retractMill(spinning=True)
def reset(self):
self.controller.addCommand(SpinCommand(self, 0))
self.controller.addCommand(ShiftCommand(self, 0))