Example #1
0
    def moveTo(self, cutMachine, x, z, d=None, face=None, retractFirst=True):
        """Moves the handler towards the desired location.

        Args:
            cutMachine (CutMachine): The cut machine that is in focus.
            x (double): The horizontal displacement of the cutting tool from the center of the foam.
            z (double): The vertical displacement of the cutting tool from the bottom of the foam.
            d (double): The depth of the cutting tool into the surface of the foam.
            face (string): The face that is being worked on. This can be left None to use current face.

        """
        # commandString = ', '.join(['controller.commandGenerator.moveTo(controller.' + cutMachine.name.lower(), str(x),
        #                            str(z), str(d), '"'+face+'"', str(retractFirst) + ')'])
        # self.controller.writeToHistory(commandString)

        d = -configurationMap[cutMachine.name.lower(
        )]['offsets']['motorStartDepthOffset'] if d is None else d
        # Ensure cutmachine is retracted
        if retractFirst:
            self.retractCutMachine(cutMachine)
        # Select the right face
        if face is not None:
            self.selectFace(face)
        # Move to specific position
        moveCommand = CombinedCommand([
            ShiftCommand(cutMachine, self.controller.handler, x, rapid=True),
            RaiseCommand(cutMachine, z, self.controller, rapid=True),
        ])
        # Move to spot and push cutmachine in
        self.controller.addCommand(moveCommand)
        self.controller.addCommand(
            PushCommand(cutMachine, d, self.controller, rapid=True))
Example #2
0
 def reset(self):
     self.controller.addCommand(PushCommand(self, 0))
     self.controller.addCommand(RaiseCommand(self, 0))
Example #3
0
    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)
Example #4
0
    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()
Example #5
0
 def homeCutmachine(self, cutmachine):
     self.controller.addCommand(
         PushCommand(cutmachine, 0, self.controller, home=True, rapid=True))
     self.controller.addCommand(
         RaiseCommand(cutmachine, 0, self.controller, home=True,
                      rapid=True))
Example #6
0
    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)
            ]))
Example #7
0
    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)
Example #8
0
    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()
Example #9
0
    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)