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

import sys
import time
import mcdrive as mc

# movement parameters
LOWSPEED_VELOCITY = 250000
LOWSPEED_ACCELERATION = 7000
HIGHSPEED_VELOCITY = 550000
HIGHPEED_ACCELERATION = 9000
HIGHSPEED_EDGE_PAUSE = 1500
# (jerk is used as predefined)

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

# 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

# Multiplex OUT signal: 
# use two different pulse lenghts to execute two different handler tasks
SHORT_PULSE_MS = 20
LONG_PULSE_MS = 50

# geometry
GRID_SIZE_MICROMETER = 40000

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(20)
    while (mc.ChkIn(HANDLER_ACK_IN) == 0):
        time.sleep_ms(1)


# 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(HANDLER_CMD_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)

# and go...
currentPosition = softLimitMin
nextPosition = currentPosition
stepSize = 40000
direction = 1
while True:    
    WaitHandlerReady()
    WaitDriveReady()
    # long pulse to Y -> go to start position
    SendPulseToHandler(LONG_PULSE_MS)

    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(nextPosition)       
    # wait until Y and X in position
    WaitHandlerReady()
    WaitDriveReady()
    currentPosition = nextPosition

    # now let the handler do its sequence
    SendPulseToHandler(SHORT_PULSE_MS)
    # and prepare next round
    nextPosition = currentPosition + direction * stepSize
    if nextPosition > softLimitMax:
        direction = -1
        nextPosition = currentPosition + direction * stepSize
        if HIGHSPEED_OVERRIDE:
            WaitDriveReady()
            time.sleep_ms(HIGHSPEED_EDGE_PAUSE)
    elif nextPosition < softLimitMin:
        direction = 1
        nextPosition = currentPosition + direction * stepSize
        if HIGHSPEED_OVERRIDE:
            WaitDriveReady()
            time.sleep_ms(HIGHSPEED_EDGE_PAUSE)