VMC tool changer questions

o<toolchange> sub

M19 R0  ;align the spindle

; only unload the tool if there is a tool in the spindle
; This assumes that the carousel is already aligned correctly. 
; It is important to unload the tool before shutting down the machine. 

O100 IF [#<tool_in_spindle> GT 0]
    G53 G0 Z -4
    
    M64 P2 ; move arm in
    M66 P2 L3 Q5 ; wait for arm-in = true
    O104 if [#5399 LT 0]
        (abort, failed to move arm in)
    O104 endif

    M64 P3 ; release tool
    M66 P3 L3 Q2 ; wait for tool-released = true
    O105 if [#5399 LT 0]
        (abort, failed to release tool)
    O105 endif

O100 ENDIF

    G53 G0 Z0

O200 IF [#<selected_tool> GT 0]


    M68 E0 Q#<selected_pocket> ;set the carousel to move to the right pocket

    M64 P0 ; start carousel
    G4 P2
    M66 P0 L3 Q60 ; wait for carousel finished
    O107 if [#5399 LT 0]
        (abort, failed to align carousel)
    O107 endif
   
    O108 endif

    M64 P2 ; move arm in (might already be in)
    M66 P2 L3 Q5 ; wait for arm-in = true
    O109 if [#5399 LT 0]
        (abort, failed to move arm in)
    O109 endif

    G53 G0 Z-4 ; pick up the tool

O200 ENDIF 

M65 P3 ; clamp tool
M66 P3 L4 Q2 ; wait for tool-locked = true
O110 if [#5399 LT 0]
    (abort, failed to lock tool)
O110 endif

M65 P2 ; move arm back out
M66 P4 L3 Q5 ; wait for arm-out = true
O111 if [#5399 LT 0]
    (abort, failed to move arm out)
O111 endif


o<toolchange> endsub [1]


M2

#24 Pocket tool changer, Pin at #24, also pin for each revolution
*loadrt carousel pockets=24 dir=1 encoding=index num_sense=4 <--????
#loadrt conv_float_s32 (Already loaded in sim_vmc.hal)
addf carousel.0 servo-thread
*addf conv-float-s32.2 servo-thread # G-code analogue outputs are float-type

*net car-enable motion.digital-out-00 carousel.0.enable  <---think I need to kill
*net car-ready carousel.0.ready motion.digital-in-00      <---think I need to kill

# Carousel has PIN on #24 pocket, and PIN on motor shaft for each revolution
# Looks like I need index mode and the Carousel will have to home to #24
*net car-pos-req motion.analog-out-00 conv-float-s32.2.in
*net car-pos-s32 conv-float-s32.2.out carousel.0.pocket-number   <----WTF ? 
*net bit0 carousel.0.sense-0
*net bit1 carousel.0.sense-1
*net bit2 carousel.0.sense-2
*net bit3 carousel.0.sense-3


# Mag ROTATE I think mine is unidirectional    SSR 9
*net car-fwd carousel.0.motor-fwd ppmc.0.dout.08.out
# Tool Caroseul rotate button DI29
*net jog-fwd carousel.0.jog-fwd ppmc.0.din.29.in

*loadusr sim_pin carousel.0.jog-fwd

# Orient Command             SSR 6
# Orient Complete            DI 08
net orient-spindle <= motion.spindle-orient ppmc.0.dout.05.out
net spindle-is-oriented => motion.spindle-is-oriented ppmc.0.din.07.in

# TOOL CHANGER
# Mag POS             DI 20
*net mag-pos carousel.0.mag-pos ppmc.0.din.19.in
# Mag REF             DI 21 
*net mag-ref carousel.0.mag-ref ppmc.0.din.20.in
# Mag Right           DI 22  SSR 11
*net arm-in carousel.0.arm-in ppmc.0.din.21.in
*net arm-in-act carousel.0.arm-in-act ppmc.0.dout.10.out
# Mag Left            DI 23  SSR 12
*net arm-out carousel.0.arm-out ppmc.0.din.22.in
*net arm-out-act carousel.0.arm-out-act ppmc.0.dout.11.out

# Release Solenoid SSR 10, Tool Locked DI10, Tool Released DI11
*net release-tool (need to find something here ) ppmc.0.dout.09.out
*net tool-released (need to find something here ) ppmc.0.din.10.in
*net tool-locked (need to find something here ) ppmc.0.din.09.in
*net spindle-inhibit motion.spindle-inhibit ppmc.0.din.09.in  
*# NEED AND2 TO ALLOW LINES 51 AND 52 ON SAME PIN, NEED SPINDLE INHIBITED MESSAGE ALSO
net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared
net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed

M66 P0 ; spindle oriented
M66 P1 ; Arm IN
M66 P2 ; Arm OUT
M66 P3 ; Tool RELEASED
M66 P????  ; Tool LOCKED

#motion controller, get name and thread periods from ini file
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[TRAJ]AXES num_dio=6 

net arm-in-act motion.ppmc.0.dout.10.out
net arm-out-act motion.ppmc.0.dout.11.out
net arm-in motion.ppmc.0.din.19.in
net arm-out motion.ppmc.0.din.20.in

net tool-release motion.ppmc.0.dout.12.out
net tool-released motion.ppmc.0.din.21.in
net tool-locked motion.ppmc.0.din.22.in