disable_alter_motion()

# ---------------------------------------------------------------
# Real-time path alteration
#   Example 1  accumulated mode (DR_DPOS) - when the absolute offset is known
#   Example 2  incremental mode (DR_DVEL) - when a sensor measures the remaining error
# ---------------------------------------------------------------
drl_report_line(0)    # makes the supply period independent of code size

DT      = 0.05        # base supply period [sec]. the actual period is larger
AMP     = 5.0         # example 1 alteration amplitude [mm]
FREQ    = 0.25        # example 1 alteration frequency [Hz]
GAIN    = 0.5         # example 2 correction gain (0 < GAIN <= 1)
LAT_MAX = 0.25        # example 1 allowed delay [sec] = about 2 x n x 10ms
LIM_POS = [50, 90]    # limit_dPOS     : total alteration limit [mm, deg]
LIM_INC = [10, 10]    # limit_dPOS_per : per-period increment limit [mm, deg]

g_mode  = 0           # 0 = accumulated / 1 = incremental
g_gain  = 0.0         # alteration gain. 0 applies no alteration
g_t0    = 0.0
g_calls = 0
g_prev  = 0.0
g_max   = 0.0         # maximum measured supply period [sec]

def read_path_error():
    # Replace this so it returns the current remaining path error [mm]
    # measured by an external sensor. It must be the error that remains
    # after the alteration has been applied.
    return 0.0

# thread_run(loop=True) calls the function again each time it returns, so do
# not put a while loop inside. With a while loop, thread_stop has no effect.
def alter_thread():
    global g_calls, g_prev, g_max
    if g_mode == 0:
        var_y = g_gain * AMP * sin(6.28318 * FREQ * (time.time() - g_t0))
        alter_motion(posx(0, var_y, 0, 0, 0, 0))       # the offset that should apply now
    else:
        var_e = g_gain * GAIN * read_path_error()
        alter_motion(posx(0, var_e, 0, 0, 0, 0))       # add a fraction of the remaining error
    # Measure the call interval every period and keep the maximum.
    var_now = time.time()
    if g_prev > 0.0:
        if var_now - g_prev > g_max:
            g_max = var_now - g_prev
    g_prev  = var_now
    g_calls = g_calls + 1
    wait(DT)

# End poses of the base motion - use the commanded pose (get_desired_posx),
# not the measured one. The measured pose contains tracking residual, which
# accumulates as error if fed back as a target.
movej(posj(30, 45, 45, 0, 90, 0), vel=60, acc=60)
X1 = get_desired_posx(DR_BASE)
movej(posj(-30, 45, 45, 0, 90, 0), vel=60, acc=60)
X2 = get_desired_posx(DR_BASE)

# --- Measure the supply period --------------------------------
# Run once with the motion and thread configuration you will actually use.
# g_gain = 0.0 makes the alteration zero while the computation and the call
# still run, so the robot path is unchanged and the load matches real use.
movel(X1, vel=30, acc=60)
mwait(0)
enable_alter_motion(n=20, mode=DR_DPOS, ref=DR_BASE,
                    limit_dPOS=LIM_POS, limit_dPOS_per=LIM_INC)
g_mode  = 0
g_gain  = 0.0
g_calls = 0
g_prev  = 0.0
g_max   = 0.0
g_t0    = time.time()
th_id   = thread_run(alter_thread, loop=True)

movel(X2, vel=30, acc=60)

thread_stop(th_id)
disable_alter_motion()

var_supply = DT                                # fallback if the measurement fails
if g_calls > 5:
    var_supply = g_max                         # maximum supply period [sec] - basis for n
tp_log("supply max = {0} ms".format(int(var_supply * 100000) / 100.0))

# Lower bound : maximum supply period / 10ms, rounded up
N_MIN = int(var_supply * 100)
if var_supply * 100 > N_MIN:
    N_MIN = N_MIN + 1
if N_MIN < 1:
    N_MIN = 1

# --- Example 1. accumulated mode (DR_DPOS) --------------------
# Every transfer is a complete offset. If the controller does not accept one,
# the next transfer restores it, so nothing is lost.
# Take the largest n the delay budget allows.
N_DPOS = int(LAT_MAX * 100 / 2)
if N_DPOS < N_MIN:
    N_DPOS = N_MIN                             # the lower bound wins

movel(X1, vel=30, acc=60)
mwait(0)                                       # start the alteration from standstill
enable_alter_motion(n=N_DPOS, mode=DR_DPOS, ref=DR_BASE,
                    limit_dPOS=LIM_POS, limit_dPOS_per=LIM_INC)
g_mode = 0
g_gain = 1.0
g_t0   = time.time()
th_id  = thread_run(alter_thread, loop=True)

movel(X2, vel=30, acc=60)                      # base motion with the alteration

# Shutdown order : stop the alteration -> thread_stop -> disable_alter_motion
# If the thread is still alive after disabling, its alter_motion() calls fail.
g_gain = 0.0
thread_stop(th_id)
disable_alter_motion()

# --- Example 2. incremental mode (DR_DVEL) --------------------
# Use only in a closed loop where a sensor keeps measuring the remaining error.
# Increments the controller does not accept never come back, but in a closed
# loop the next measurement retries them automatically.
# Keep n at the lower bound. Increments dropped intermittently are
# corrected by the closed loop on the next measurement.
# If read_path_error() is not a closed loop, use example 1 instead.
N_DVEL = N_MIN

movel(X1, vel=30, acc=60)
mwait(0)
enable_alter_motion(n=N_DVEL, mode=DR_DVEL, ref=DR_BASE,
                    limit_dPOS=LIM_POS, limit_dPOS_per=LIM_INC)
g_mode = 1
g_gain = 1.0
g_t0   = time.time()
th_id  = thread_run(alter_thread, loop=True)

movel(X2, vel=30, acc=60)                      # base motion with the sensor correction

g_gain = 0.0
thread_stop(th_id)
disable_alter_motion()