#id 3axis_flattrack_axis2_y_middle.py 2026-04-07 oh
# Fullmo MovingCap CODE - micropython example.
# original file name: 3axis_flattrack_axis2_y_middle.py

import sys
import time
import mcdrive as mc

# movement parameters
LOWSPEED_VELOCITY = 250000
LOWSPEED_ACCELERATION = 15000
HIGHSPEED_VELOCITY = 500000
HIGHPEED_ACCELERATION = 20000
# (jerk is used as predefined)

# IN signal for (temporary) high speed operation
HIGHSPEED_IN = 7
# optional override: always use highspeed
HIGHSPEED_OVERRIDE = False

# IN that controls this axis
CONTROLLER_CMD_IN = 8
# OUT that returns status of this axis
CONTROLLER_ACK_OUT = 1

# Multiplex IN signal: 
# supports two different pulse lengths to perform two different tasks
SHORT_PULSE_MS = 20
LONG_PULSE_MS = 50
PULSE_WINDOW_MS = 9

# OUT that handles the secondary handler axis
HANDLER_CMD_OUT = 2
# IN that receives the status of the secondary handler axis
HANDLER_ACK_IN = 10

def WaitDriveReady():
    """
    This function checks the status of the drive to ensure it is ready for operation.
    It continuously polls the drive's statusword for the "target reached" bit and waits until it is set.
    Additionally, it checks for any errors by continuously polling the drive's statusword for error bits.
    If an error is detected, it waits until the error is cleared.

    Parameters:
    None

    Returns:
    None
    """
    # make sure movement has started
    time.sleep_ms(10)
    # Check statusword for "target reached"
    while(mc.ChkReady() == 0):
        time.sleep_ms(1)
    # only continue if no error
    while(mc.ChkError() != 0):
        time.sleep_ms(1)


def SendPulseToHandler(duration):
    mc.SetOut(HANDLER_CMD_OUT)
    time.sleep_ms(duration)
    mc.ClearOut(HANDLER_CMD_OUT)

def WaitHandlerReady():
    # make sure the handler had time to acknowledge the new command
    time.sleep_ms(10)
    while (mc.ChkIn(HANDLER_ACK_IN) == 0):
        time.sleep_ms(1)

def WaitForPulseFromController():
    # make sure the input is low first
    while (mc.ChkIn(CONTROLLER_CMD_IN) == 1):
        time.sleep_ms(1)
    # wait for input high
    while (mc.ChkIn(CONTROLLER_CMD_IN) == 0):
        time.sleep_ms(1)
    # note start time and wait until low again
    startTime = time.ticks_ms() 
    while (mc.ChkIn(CONTROLLER_CMD_IN) == 1):
        time.sleep_ms(1)
    endTime = time.ticks_ms()
    # return pulse duration in milliseconds
    pulseDuration = time.ticks_diff(endTime, startTime)
    if (pulseDuration >= SHORT_PULSE_MS - PULSE_WINDOW_MS) and (pulseDuration <= SHORT_PULSE_MS + PULSE_WINDOW_MS):
       return SHORT_PULSE_MS
    elif (pulseDuration >= LONG_PULSE_MS - PULSE_WINDOW_MS) and  (pulseDuration <= LONG_PULSE_MS + PULSE_WINDOW_MS):
       return LONG_PULSE_MS
    else:
       return 0


# Determine soft limit positions (minimal and maximum positions) at startup
# end program, if there is no valid soft limit setup
softLimitMin = mc.ReadObject(0x607d, 1)
softLimitMax = mc.ReadObject(0x607d, 2)
if ((softLimitMin == 0 and softLimitMax == 0) or softLimitMax <= (softLimitMin + 1000)):
    print("flatTRACK demo abort. Check softlimit objects 607Dh.1h and 607D.2h!")
    sys.exit()

mc.ClearOut(CONTROLLER_ACK_OUT)

# initial wait, don't surprise me with immediate movement
time.sleep_ms(5000)
# general init
mc.EnableDrive()
# go to startup position            
mc.SetPosVel(LOWSPEED_VELOCITY)
mc.SetAcc(LOWSPEED_ACCELERATION)
mc.GoPosAbs(softLimitMin)

while True:
    WaitHandlerReady()
    WaitDriveReady()
    mc.SetOut(CONTROLLER_ACK_OUT)
    pulseDuration = WaitForPulseFromController()
    mc.ClearOut(CONTROLLER_ACK_OUT)
    if pulseDuration == LONG_PULSE_MS:
        # go to start position for the sequence
        if HIGHSPEED_OVERRIDE or mc.ChkIn(HIGHSPEED_IN):
            mc.SetPosVel(HIGHSPEED_VELOCITY)
            mc.SetAcc(HIGHPEED_ACCELERATION)
        else:
            mc.SetPosVel(LOWSPEED_VELOCITY)
            mc.SetAcc(LOWSPEED_ACCELERATION)
        mc.GoPosAbs(softLimitMin)
    elif pulseDuration == SHORT_PULSE_MS:
        # do the sequence with 2x z axis movement
        SendPulseToHandler(SHORT_PULSE_MS)
        WaitHandlerReady()
        if HIGHSPEED_OVERRIDE or mc.ChkIn(HIGHSPEED_IN):
            mc.SetPosVel(HIGHSPEED_VELOCITY)
            mc.SetAcc(HIGHPEED_ACCELERATION)
        else:
            mc.SetPosVel(LOWSPEED_VELOCITY)
            mc.SetAcc(LOWSPEED_ACCELERATION)
        mc.GoPosAbs(softLimitMax)
        WaitDriveReady()
        SendPulseToHandler(SHORT_PULSE_MS)