''========================================================================================
'  Created From: C:\Perl\site\lib\MyMach3Macros\ATCDrivers\ATCDriver.m3m
'  Includes: 
'       C:\Perl\site\lib\MyMach3Macros\ATCDrivers\PDBDriver.def
'       C:\Perl\site\lib\MyMach3Macros\ATCDrivers\PDBDriver.inc
'       C:\Perl\site\lib\MyMach3Macros\ATCDrivers\ATCDriver.def
'       C:\Perl\site\lib\MyMach3Macros\ATCDrivers\ATCDriver.inc
''========================================================================================

''========================================================================================
''	 Included Definitions:
''========================================================================================
ATCMode = 0                             '' ATC mode when set to 1
ATCReturnValDRO         = 1104      '' ATC Macro return value
ATC_CMD_CLEAR_MAP       = 11    '' Clear Tool Map
ATC_CMD_GRAB_R8         = 4     '' PDB R8 Grab
ATC_CMD_GRAB_TTS        = 2     '' PDB TTS Grab
ATC_CMD_INIT_ATC        = 0     '' Initialize ATC on power-up
ATC_CMD_LIFT_DOWN       = 20    '' Lower Lift
ATC_CMD_LIFT_UP         = 21    '' Raise Lift
ATC_CMD_LOAD_TOOL       = 12    '' Load Carousel Slot, Q = slot, R = tool
ATC_CMD_MOUNT_TOOL      = 13    '' Mount Tool, Q = tool
ATC_CMD_PARK_ATC        = 10    '' Park ATC
ATC_CMD_PIVOT_TO_PARK   = 23    '' Pivot To Park
ATC_CMD_PIVOT_TO_QUILL  = 22    '' Pivot To Quill
ATC_CMD_RELEASE_R8      = 3     '' PDB R8 Release
ATC_CMD_RELEASE_TTS     = 1     '' PDB TTS Release
ATC_CMD_ROTATE_TO_SLOT  = 24    '' Rotate Carousel, Q = slot
ATC_CMD_UNMOUNT_TOOL    = 14    '' Unmount Tool
ATC_DIR                 = OUTPUT12
ATC_DIR_CCW             = 1
ATC_DIR_CW              = 0
ATC_ERR_LIFT_DOWN_FAIL  = 1
ATC_ERR_LIFT_UP_FAIL    = 2
ATC_ERR_NOERROR         = 0
ATC_ERR_PIVOT_IN_FAIL   = 3
ATC_ERR_PIVOT_OUT_FAIL  = 4
ATC_ERR_TOOL_LOAD_FAIL  = 6
ATC_ERR_TOOL_UNLOAD_FAIL = 7
ATC_LIFT                = OUTPUT9
ATC_LIFT_LOWER          = 1
ATC_LIFT_RAISE	        = 0
ATC_LIFT_SENSE          = INPUT1    '' 0 => Lift Is At End of Travel, 1 => Lift Is Mid-Travel
ATC_LIFT_TIMEOUT        = 2000      '' 2 seconds
ATC_PIVOT               = OUTPUT10
ATC_PIVOT_PARK          = 0
ATC_PIVOT_QUILL	        = 1
ATC_PIVOT_SENSE         = INPUT2    '' 0 => Pivot Is At End of Travel, 1 => Pivot Is Mid-Travel
ATC_PIVOT_TIMEOUT       = 3000      '' 3 seconds
ATC_ROTATE              = OUTPUT11
B_MACH_COORD_DRO = 87                   '' B Axis Machine Coordinate DRO
COOLANT_FLOOD       = OUTPUT6           '' Flood Coolant control output, for safety interlock
COOLANT_MIST        = OUTPUT4           '' Mist Coolant control output, for safety interlock
CurrentBPos = 0                         '' Current B axis Position, in Machine Coords
CurrentCarouselSlotDRO  = 1100	    '' Current Carousel position
CurrentLiftPosDRO       = 1102	    '' Current Lift position
CurrentMountedToolDRO   = 1103      '' Current Mounted tool
CurrentPivotPosDRO      = 1101	    '' Current Pivot position
DRAWBAR_ACTIVATE    = OUTPUT3           '' PDB Activate control output, 1 => Activate
DRAWBAR_HITORQ      = 0                 '' PDB High Torque control output, 1 => High Torque
InitialZPos = 0                         '' Current Z axis Position, in Machine Coords
PDB_ACTIVATE_DELAY = 500                '' Delay, in mSec, for PDB to activate
PDB_GRAB_R8_TURNS = 9.0                 '' # turns to tighten an R8 tool
PDB_GRAB_TTS_TURNS = 2.25               '' # turns to tighten a TTS tool
PDB_RELEASE_R8_TURNS = 8.0              '' # turns to loosen an R8 tool
PDB_RELEASE_TTS_TURNS = 2.00            '' # turns to loosen a TTS tool
PDB_Z_OFFSET = -0.05                    '' Offset, from Z Home, for PDB operation, in Machine Coords
SPINDLE_CCW         = OUTPUT2           '' Spindle CCW control output, for safety interlock
SPINDLE_CW          = OUTPUT1           '' Spindle CW control output, for safety interlock
Z_MACH_COORD_DRO = 85                   '' Z Axis Machine Coordinate DRO
''========================================================================================

''************************************************************************
''	Top-Level ATC Driver Macro
''
''	Note that LoadTool(VTOOL_NOTOOL) will unload current tool, if any, and 
''	return carousel to Home (tool 1) position.
''************************************************************************

'#filename   "M806.m1s"

'#include    "PDBDriver.def"
'#include    "PDBDriver.inc"

'#include    "ATCDriver.def"
'#include    "ATCDriver.inc"

''*************************************************
''  Stand-Alone ATC Driver Entry Point
''
''  Usage:
''      M806 Pa Qb Rc
''
''  P Parameter Defines Operation To Perform
''  Q Parameter Defines (optional) First Argument (Tool or Slot)
''  R Parameter Defines (optional) Second Argument (Tool)
''
''  See ATCDriver.def for argument details
''
''  M806 P0 must be added to Mach Init string
''*************************************************

''*************************************************
''  ATC Global Data
''*************************************************

Dim CurrentCarouselSlot As Integer
Dim CurrentPivotPos As Integer
Dim CurrentLiftPos As Integer
Dim CurrentMountedTool As Integer
Dim ATCReturnVal As Integer
Dim ATCToolMap (0 To 9) As Integer

PParam = Param1
QParam = Param2
RParam = Param3

OpenATC

DumpVars

Select Case PParam
    
    Case ATC_CMD_INIT_ATC
        InitATC
    
    Case ATC_CMD_RELEASE_TTS
        PDBReleaseTTS
        
    Case ATC_CMD_GRAB_TTS
        PDBGrabTTS
        
    Case ATC_CMD_RELEASE_R8
        PDBReleaseR8
        
    Case ATC_CMD_GRAB_R8
        PDBGrabR8
        
    Case ATC_CMD_PARK_ATC
        ret = PivotToPark(1)
        If ret <> ATC_ERR_NOERROR Then
            MachMsg("Pivot To Park Failed With Error " & ret, "Park ATC", 0)
        Else
            ret = RaiseLift(1)
            If ret <> ATC_ERR_NOERROR Then
                MachMsg("Raise Lift Failed With Error " & ret, "Park ATC", 0)
            Else
                ret = RotateToSlot(1)
                If ret <> ATC_ERR_NOERROR Then
                    MachMsg("Rotate To Slot Failed With Error " & ret, "Park ATC", 0)
                End If
            End If
        End If
        
    Case ATC_CMD_CLEAR_MAP
        ClearToolMap
        
    Case ATC_CMD_LOAD_TOOL
        If CurrentPivotPos <> 1 Then
            ret = PivotToQuill(true)
            If ret <> ATC_ERR_NOERROR Then
                MachMsg("Pivot To Quill Failed With Error " & ret, "Load Tool", 0)
            Else
                ret = RotateToSlot(QParam)
                If ret <> ATC_ERR_NOERROR Then
                    MachMsg("Rotate To Slot Failed With Error " & ret, "Load Tool", 0)
                Else
                    Call SetToolMapEntry(QParam, RParam)
                End If
            End If
        End If
        
    Case ATC_CMD_MOUNT_TOOL
        ret = MountTool(QParam)
        If ret <> ATC_ERR_NOERROR Then
            MachMsg("Mount Tool Failed With Error " & ret, "Mount Tool", 0)
        End If
        
    Case ATC_CMD_UNMOUNT_TOOL
        ret = UnMountTool
        If ret <> ATC_ERR_NOERROR Then
            MachMsg("UnMount Tool Failed With Error " & ret, "UnMount Tool", 0)
        End If
        
    Case ATC_CMD_LIFT_UP
        ret = RaiseLift(true)
        If ret <> ATC_ERR_NOERROR Then
            MachMsg("Lift Up Failed With Error " & ret, "Lift Up Test", 0)
        End If
        
    Case ATC_CMD_LIFT_DOWN       
        ret = LowerLift(true)
        If ret <> ATC_ERR_NOERROR Then
            MachMsg("Lift Down Failed With Error " & ret, "Lift Down Test", 0)
        End If
        
    Case ATC_CMD_PIVOT_TO_PARK
        ret = PivotToPark(true)
        If ret <> ATC_ERR_NOERROR Then
            MachMsg("Pivot To Park Failed With Error " & ret, "Pivot To Park Test", 0)
        End If
        
    Case ATC_CMD_PIVOT_TO_QUILL
        ret = PivotToQuill(true)
        If ret <> ATC_ERR_NOERROR Then
            MachMsg("Pivot To Quill Failed With Error " & ret, "Pivot To Quill Test", 0)
        End If
        
    Case ATC_CMD_ROTATE_TO_SLOT
        ret = RotateToSlot(QParam)
        If ret <> ATC_ERR_NOERROR Then
            MachMsg("Rotate To Slot Failed With Error " & ret, "Rotate To Slot Test", 0)
        End If
        
    Case Else
        MachMsg("Invalid ATC Command Specified: " & PParam))
        
End Select

DumpVars

CloseATC


Sub DumpVars
    ''Dbg ("Lift=" & CurrentLiftPos & " Pivot=" & CurrentPivotPos & "Slot=" & CurrentCarouselSlot & " Tool=" & CurrentMountedTool)
End Sub
    


''========================================================================================
''	 Included Subroutines:
''========================================================================================
Sub ClearToolMap
    For i = 0 To 9 Step 1
        ATCToolMap(i) = -1
        SetUserDRO(FirstToolMapDRO + i, ATCToolMap(i))
    Next
End Sub

Sub CloseATC
    SetUserDRO(CurrentCarouselSlotDRO, CurrentCarouselSlot)
    SetUserDRO(CurrentPivotPosDRO, CurrentPivotPos)
    SetUserDRO(CurrentLiftPosDRO, CurrentLiftPos)
    SetUserDRO(CurrentMountedToolDRO, CurrentMountedTool)
    SetUserDRO(ATCReturnValDRO, ATCReturnVal)
    For i = 0 To 9 Step 1
        SetUserDRO(FirstToolMapDRO + i, ATCToolMap(i))
    Next
End Sub

Sub Dbg (Msg As String)
    If 1 Then
        MachMsg(Msg, "Dbg", 0)
    End If
End Sub

Sub InitATC
    PDBInit
    DeactivateSignal(ATC_PIVOT)
    Sleep(1000)
    ActivateSignal(ATC_LIFT)
    Sleep(250)
    DeactivateSignal(ATC_LIFT)
    SetUserDRO(CurrentCarouselSlotDRO, 1)
    SetUserDRO(CurrentPivotPosPosDRO, 0)
    SetUserDRO(CurrentLiftPosDRO, 0)
    SetUserDRO(CurrentMountedToolDRO, -1)
    SetUserDRO(ATCReturnValDRO, 0)
    ClearToolMap
    OpenATC
End Sub

Sub OpenATC
    CurrentCarouselSlot = GetUserDRO(CurrentCarouselSlotDRO)
    CurrentPivotPos = GetUserDRO(CurrentPivotPosDRO)
    CurrentLiftPos = GetUserDRO(CurrentLiftPosDRO)
    CurrentMountedTool = GetUserDRO(CurrentMountedToolDRO)
    For i = 0 To 9 Step 1
        ATCToolMap(i) = GetUserDRO(FirstToolMapDRO + i)
    Next
End Sub

Sub PDBGrab (ByVal turns As Single)
    If IsMoving() Or _
    	(IsOutputActive(DRAWBAR_ACTIVATE) And Not ATCMode) Or _
    	IsOutputActive(SPINDLE_CW) Or _
    	IsOutputActive(SPINDLE_CCW) Or _
    	IsOutputActive(COOLANT_MIST) Or _
    	IsOutputActive(COOLANT_FLOOD) Then
            GoTo RETURN1
    End If

    GetInitialZPos                      '' Remember current Z position

    Code ("G53 G0 Z[ " & PDB_Z_OFFSET & "] ")   '' Move Z axis to PDB position
    WaitNotMoving

    SetLowTorque                      	'' Put PDB in Low Torque mode

    PDB_Activate                      	'' Activate the Drawbar

    Sleep(PDB_ACTIVATE_DELAY)           '' Delay a bit

    GetCurrentBPos
    Code("G53 G0 B[ " & turns & " ]")   '' Spin the drawbar to grab the tool
    WaitNotMoving

    Code("G53 G1 B[ 0.25 ] F[ " & B_MAX_VEL_COUNTS / 5 & " ]") '' Go up to 1/4 turn further, to ensure it's really tight
    WaitNotMoving

    PDB_Deactivate                      '' Deactivate Drawbar

    Code("G53 G0 Z" & InitialZPos)      '' Return Z Axis to initial position
    WaitNotMoving

RETURN1:
End Sub

Sub PDBGrabR8
    PDBGrab(PDB_GRAB_R8_TURNS)
End Sub

Sub PDBGrabTTS
    PDBGrab(PDB_GRAB_TTS_TURNS)
End Sub

Sub PDBRelease (ByVal turns As Single)
    If IsMoving() Or _
        (IsOutputActive(DRAWBAR_ACTIVATE) And Not ATCMode) Or _
        IsOutputActive(SPINDLE_CW) Or _
        IsOutputActive(SPINDLE_CCW) Or _
        IsOutputActive(COOLANT_MIST) Or _
        IsOutputActive(COOLANT_FLOOD) Then
            GoTo RETURN2
    End If

    GetInitialZPos                      '' Remember current Z position

    Code ("G53 G0 Z[ " & PDB_Z_OFFSET & "] ")   '' Move Z axis to PDB position
    WaitNotMoving

    SetHighTorque                       '' Put PDB in High Torque mode

    PDB_Activate                        '' Activate the Drawbar

    Sleep(PDB_ACTIVATE_DELAY)           '' Delay a bit

    ' Spin the drawbar to release the tool
    Code("G53 G0 B[ 0 - " & turns & " ]")   '' Spin the drawbar to release the tool
    WaitNotMoving

    SetLowTorque                        '' Force Low Torque Mode
    
    PDB_Deactivate                      '' Deactivate Drawbar

    Code("G53 G0 Z" & InitialZPos)      '' Return Z Axis to initial position
    WaitNotMoving

RETURN2:
End Sub

Sub PDBReleaseR8
    PDBRelease(PDB_RELEASE_R8_TURNS)
End Sub

Sub PDBReleaseTTS
    PDBRelease(PDB_RELEASE_TTS_TURNS)
End Sub

Sub SetToolMapEntry (Slot As Integer, Tool As Integer)
    ATCToolMap(Slot - 1) = Tool
End Sub

Sub PDBInit
    PDB_Deactivate
End Sub

Sub PDBReleaseATC
    ATCMode = 1
    PDBReleaseTTS
End Sub

Sub PDB_Activate
    ActivateSignal(DRAWBAR_ACTIVATE)    '' Activate (Drop) Drawbar
End Sub

Sub PDB_ActivateATC
    ATCMode = 1                         '' Activate (Drop) Drawbar for ATC use
    ActivateSignal(DRAWBAR_ACTIVATE)
End Sub

Sub PDB_Deactivate
    DeactivateSignal(DRAWBAR_ACTIVATE)  '' De-Activate (Raise) Drawbar
    ATCMode = 0
End Sub

Sub GetCurrentBPos
    CurrentBPos = GetOEMDRO(B_MACH_COORD_DRO)
End Sub

Sub GetInitialZPos
    InitialZPos = GetOEMDRO(Z_MACH_COORD_DRO)
End Sub

Sub SetLowTorque
    DeactivateSignal(DRAWBAR_HITORQ)    '' Set Drawbar to Low Torque mode
End Sub

Sub WaitNotMoving
    While (IsMoving())                  '' Wait for all motion to stop
        Sleep(10)
    Wend
End Sub

Sub SetHighTorque
    ActivateSignal(DRAWBAR_HITORQ)      '' Set Drawbar to High Torque mode
End Sub
''========================================================================================

''========================================================================================
''	 Included Functions:
''========================================================================================
Function LowerLift (ByVal verify As Integer) As Integer

    If IsOutputActive(ATC_LIFT) = ATC_LIFT_DOWN And IsActive(ATC_LIFT_SENSE) = 0 Then
        LowerLift = ATC_ERR_NOERROR
	GoTo EXIT4
    End If

    If verify = 0 Then
        ActivateSignal(ATC_LIFT)
        Sleep(1000)
        LowerLift = ATC_ERR_NOERROR
	GoTo EXIT4
    End If

    LowerLift = ConfirmAction(ATC_LIFT, ATC_LIFT_LOWER, ATC_LIFT_SENSE, ATC_LIFT_TIMEOUT, ATC_ERR_LIFT_DOWN_FAIL)

EXIT4:
    CurrentLiftPos = 1
End Function

Function MountTool (ByVal NextTool As Integer) As Integer
    If NextTool = CurrentMountedTool Then
        '' Already mounted, so nothing to do
        MountTool = ATC_ERR_NOERROR
        GoTo EXIT0
    End If
    
    '' Rotate carousel to slot for currently mounted tool
    MountTool = RotateToTool(CurrentATCTool)
    If (MountTool <> ATC_ERR_NOERROR) Then
        GoTo EXIT0
    End If

    '' Make sure tool length comp is off, and knee is down
    Code("G849")
    While (IsMoving())
        Sleep(10)
    Wend

    '' Move quill to toolchange position		
    Code("G53 G0 Z" & PDB_Z_OFFSET)

    While (IsMoving())
        Sleep(10)
    Wend

    If CurrentATCTool < 0 Then
        '' No tool is currently mounted, so lower lift to load new tool
        MountTool = LowerLift(1)
        If MountTool <> ATC_ERR_NOERROR Then
            GoTo EXIT0
        End If
    Else
        '' Tool is currently mounted, so raise lift to unload it
        MountTool = RaiseLift(1)
        If MountTool <> ATC_ERR_NOERROR Then
            GoTo EXIT0
        End If
    End If

    '' Swing carousel to quill
    MountTool = PivotToQuill(1)
    If MountTool <> ATC_ERR_NOERROR Then
        GoTo EXIT0
    End If

    '' If a tool is currently mounted, remove it	
    If CurrentATCTool > 0 Then
        PDBReleaseATC
        '' Lower lift to unload tool from spindle
        MountTool = LowerLift(1)
        ATCCurrentTool = -1
        If MountTool <> ATC_ERR_NOERROR Then
            MountTool = ATC_ERR_TOOL_UNLOAD_FAIL
            GoTo EXIT0
        End If
    Else
        '' No tool is currently mounted, so just activate PDB to hold collet open
        PDB_ActivateATC
        Sleep(1000)
    End If

    '' Rotate carousel to new tool to mount	
    MountTool = RotateToTool(NextTool)
    If MountTool <> ATC_ERR_NOERROR Then
        GoTo EXIT0
    End If

    If NextTool > 0 Then
        '' Mount next tool
        MountTool = RaiseLift(1)
        If MountTool <> ATC_ERR_NOERROR Then
            MountTool = ATC_ERR_TOOL_LOAD_FAIL
            GoTo EXIT0
        End If
        PDBGrabTTS
        ATCCurrentTool = NextTool
    End If

    '' Deactivate drawbar
    PDB_Deactivate

    '' Return carousel to park position
    MountTool = PivotToPark(1)
    If MountTool <> ATC_ERR_NOERROR Then
        GoTo EXIT0
    End If

    MountTool = RaiseLift(1)
    If MountTool <> ATC_ERR_NOERROR Then
        GoTo EXIT0
    End If

    LoadTool = ATC_ERR_NOERROR

EXIT0:
End Function

Function PivotToPark (ByVal verify As Integer) As Integer

    If IsOutputActive(ATC_PIVOT) = ATC_PIVOT_PARK And IsActive(ATC_PIVOT_SENSE) = 0 Then
	PivotToPark = ATC_ERR_NOERROR
	GoTo EXIT6
    End If

    If verify  = 0 Then
        DeactivateSignal(ATC_PIVOT)
        Sleep(1000)
        PivotToPark = ATC_ERR_NOERROR
	GoTo EXIT6
    End If

    PivotToPark = ConfirmAction(ATC_PIVOT, ATC_PIVOT_PARK, ATC_PIVOT_SENSE, ATC_PIVOT_TIMEOUT, ATC_ERR_PIVOT_OUT_FAIL)

EXIT6:
    CurrentPivotPos = 0
End Function

Function PivotToQuill (ByVal verify As Integer) As Integer

    If IsOutputActive(ATC_PIVOT) = ATC_PIVOT_QUILL And IsActive(ATC_PIVOT_SENSE) = 0 Then
	PivotToQuill = ATC_ERR_NOERROR
	GoTo EXIT5
    End If

    If verify = 0 Then
        ActivateSignal(ATC_PIVOT)
        Sleep(1000)
        PivotToQuill = ATC_ERR_NOERROR
    End If

    PivotToQuill = ConfirmAction(ATC_PIVOT, ATC_PIVOT_QUILL, ATC_PIVOT_SENSE, ATC_PIVOT_TIMEOUT, ATC_ERR_PIVOT_IN_FAIL)

    Sleep(500)

EXIT5:
    CurrentPivotPos = 1
End Function

Function RaiseLift (ByVal verify As Integer) As Integer

    If IsOutputActive(ATC_LIFT) = ATC_LIFT_UP And IsActive(ATC_LIFT_SENSE) = 0 Then
        RaiseLift = ATC_ERR_NOERROR
	GoTo EXIT3
    End If

    If verify = 0 Then
        DeactivateSignal(ATC_LIFT)
        Sleep(1000)
        RaiseLift = ATC_ERR_NOERROR
	GoTo EXIT3
    End If

    RaiseLift = ConfirmAction(ATC_LIFT, ATC_LIFT_RAISE, ATC_LIFT_SENSE, ATC_LIFT_TIMEOUT, ATC_ERR_LIFT_UP_FAIL)

EXIT3:
    CurrentLiftPos = 0
End Function

Function RotateToSlot (ByVal slot As Integer) As Integer
    target_pos = slot
    distance = 0
    movedir = ATC_DIR_CW

    If slot = -1 Then
        target_pos = 1
    End If

    distance = target_pos - CurrentCarouselSlot
    If distance < 0  Then
        distance = distance * -1
        movedir = ATC_DIR_CCW
    End If	
    If distance > 5 Then
        distance = 10 - distance
        if (movedir = ATC_DIR_CW) Then
            movedir = ATC_DIR_CCW
        Else
            movedir = ATC_DIR_CW
        End If
    End If
    DeactivateSignal(ATC_DIR)
    If movedir = ATC_DIR_CCW Then
        ActivateSignal(ATC_DIR)
    End If

    While CurrentCarouselSlot <> target_pos
        ActivateSignal(ATC_ROTATE)
        Sleep(500)
        DeactivateSignal(ATC_ROTATE)
	If movedir = ATC_DIR_CW Then
	    CurrentCarouselSlot = CurrentCarouselSlot + 1
	Else
	    CurrentCarouselSlot = CurrentCarouselSlot - 1
	End If
        If (CurrentCarouselSlot > 10) Then
            CurrentCarouselSlot = 1
        End If
        If (CurrentCarouselSlot < 1) Then
            CurrentCarouselSlot = 10
        End If
        Sleep(750)
    Wend
    RotateToSlot = ATC_ERR_NOERROR

EXIT2:
End Function

Function ConfirmAction (ByVal bit As Integer, _
			ByVal bitval As Integer, _
			ByVal sense As Integer, _
			ByVal timeout As Integer, _ 
			ByVal err As Integer) _
			 As Integer
                         
     '' Must not be mid-travel at start
    If IsActive(sense) Then
	ConfirmAction = err
        GoTo Exit7
    End If

    '' Set/Clear the control bit
    If bitval = 1 Then
        ActivateSignal(bit)
    Else
        DeactivateSignal(bit)
    End If

    '' Ensure it moves - first wait for sensor high
    ElapsedTime = 0
    i = 0
    While i < 10
        If IsActive(sense) = 1 Then
            i = i + 1
        Else
            i = 0
        End If
        Sleep(1)
	ElapsedTime = ElapsedTime + 1
        If ElapsedTime >= timeout Then
	    ConfirmAction = err
	    GoTo EXIT7
        End If
    Wend

    '' Now wait for sensor low
    i = 0
    While i < 10
        If IsActive(sense) = 0 Then
            i = i + 1
        Else
            i = 0
        End If
        Sleep(1)
	ElapsedTime = ElapsedTime + 1
        If (ElapsedTime >= timeout) Then
	    ConfirmAction = err
	    GoTo EXIT7
        End If
    Wend
    ConfirmAction = ATC_ERR_NOERROR

EXIT7:
End Function

Function RotateToTool (ByVal tool As Integer) As Integer
    RotateToTool = ATC_ERR_NOERROR
    slot = MapToolToSlot(tool)
    If slot > 0 Then
        RotateToTool = RotateToSlot(slot)
    End If
End Function

Function MapToolToSlot (Tool As Integer) As Integer
    MapToolToSlot = -1
    For i = 0 To 9 Step 1
        If ATCToolMap(i) = Tool Then
            MapToolToSlot = i
            Exit For
        End If
    Next
End Function
''========================================================================================