Epson
*

How to rotate about tool center point

Purpose
This document contains a code example that allows you to rotate a tool about one point. 
Usage
Follow these steps:
1.  Define your tool offsets using TLSet.
2.  Set the current tool number.
3.  Move to starting position
4.  Call RotateTool with desired degrees and zOffset.
 
For example:
  Accels 5000, 5000
  Speeds 500
  TLSet 1, 25, 20, 0, 0
  Tool 1
  Go startPosition
  RotateTool 360, -50
 
Notes:
1.  The radius of the tool must be greater than 15 mm.
2.  Make sure that the U axis range is set high enough to acommodate rotation.
3.  If you call this function more than once, the robot will decelerate to a stop after each call.
4.  You can use large rotation values, such as 1080 (3 revs).

SPEL Code Example
 
' ***********************************************************
' * Function RotateTool
' * Inputs:
' *   degrees    Amount of rotation.  Can be plus or minus.
' *              Must be greater than 10 degrees.
' *              Range is limited by U axis range.
' *   zOffset    Z distance. Can be 0, plus, or minus
' * Note: Speed is determined by Accels and Speeds
' ***********************************************************

Function RotateTool(degrees As Real, zOffset As Real)
 
  Integer i, toolNum, numOfMoves
  Real tx, ty, tu, thyp
  Real a, xOrg, yOrg
  Real x1, y1, x2, y2, z2, u2, x3, y3, z3, u3
  Real tAngle, tDegrees, totalDegrees
  Real zInc
 
  toolNum = Tool
  If toolNum = 0 Then
     Print "Error: Invalid tool for rotate."
     Quit All
  EndIf
 
  xOrg = CX(P*)
  yOrg = CY(P*)

  u3 = CU(P*)
  z3 = CZ(P*)
 
  Tool 0
 
  x1 = CX(P*)
  y1 = CY(P*)
 
  numOfMoves = Abs(degrees) / 45
  If numOfMoves = 0 Then
    numOfMoves = 1
  EndIf
  zInc = zOffset / numOfMoves / 2
 
  Do
    If Abs(degrees - tDegrees) > 45 Then
      tDegrees = 45 * Sgn(degrees)
    Else
      tDegrees = degrees - tDegrees
    EndIf
    totalDegrees = totalDegrees + tDegrees
 
    ' Calc tool offsets
    tx = xOrg + x1
    ty = yOrg + y1
 
    ' Calc tool angle
    tAngle = RadToDeg(Atan2(tx, ty))
 
    ' Calc tool vector
    thyp = Sqr(tx ** 2 + ty ** 2)
 
    ' Calc u of arc endpoint
    u3 = u3 + tDegrees
 
    ' Calc xy of arc endpoint
    a = DegToRad(tAngle + tDegrees)
    x3 = xOrg + Cos(a) * thyp
    y3 = yOrg + Sin(a) * thyp
 
    ' Calc u of arc midpoint
    u2 = u3 - tDegrees / 2
 
    ' Calc xy of arc midpoint
    a = DegToRad(tAngle + tDegrees / 2)
    x2 = xOrg + Cos(a) * thyp
    y2 = yOrg + Sin(a) * thyp
 
    ' Calc z increments for arc midpoint and endpoint
    z2 = z3 + zInc
    z3 = z2 + zInc
 
    ' Move using arc in CP
    Arc XY(x2, y2, z2, u2), XY(x3, y3, z3, u3) CP
 
    ' Prepare for next iteration
    x1 = x3
    y1 = y3
  Loop Until Abs(totalDegrees) >= Abs(degrees)
 
  ' Restore tool
  Tool toolNum
Fend