disable_alter_motion()

# ---------------------------------------------------------------
# 실시간 경로 보정
#   예제1 누적량 모드(DR_DPOS) - 절대 오프셋을 아는 경우
#   예제2 증분량 모드(DR_DVEL) - 외부 센서로 남은 오차를 재는 경우
# ---------------------------------------------------------------
drl_report_line(0)    # 보정 주기를 코드량과 무관하게 만듭니다

DT      = 0.05        # 공급 주기 기준값 [sec]. 실제 주기는 이보다 큽니다
AMP     = 5.0         # 예제1 보정 진폭 [mm]
FREQ    = 0.25        # 예제1 보정 주파수 [Hz]
GAIN    = 0.5         # 예제2 보정 이득 (0 < GAIN <= 1)
LAT_MAX = 0.25        # 예제1 허용 보정 지연 [sec]. 지연 = 약 2 x n x 10ms
LIM_POS = [50, 90]    # limit_dPOS     : 누적 보정 한계 [mm, deg]
LIM_INC = [10, 10]    # limit_dPOS_per : 1 주기 증분 한계 [mm, deg]

g_mode  = 0           # 0 = 누적량 / 1 = 증분량
g_gain  = 0.0         # 보정 배율. 0 이면 보정이 걸리지 않습니다
g_t0    = 0.0
g_calls = 0
g_prev  = 0.0
g_max   = 0.0         # 실측 공급 주기의 최대값 [sec]

def read_path_error():
    # 외부 센서(비전 등)가 측정한 "현재 남은 경로 오차" [mm] 를 반환하도록
    # 교체하십시오. 보정이 반영된 뒤의 잔여 오차여야 합니다.
    return 0.0

# thread_run(loop=True) 는 함수가 반환할 때마다 다시 호출하므로 함수 안에
# while 루프를 두지 않습니다. while 을 두면 thread_stop 이 걸리지 않습니다.
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))       # 지금 있어야 할 오프셋
    else:
        var_e = g_gain * GAIN * read_path_error()
        alter_motion(posx(0, var_e, 0, 0, 0, 0))       # 남은 오차의 일부만큼 더
    # 호출 간격을 매 주기 재서 최대값을 남깁니다.
    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)

# 기저 모션 양단 자세 - 측정값(get_current_posx)이 아니라 지령값을 씁니다.
# 측정 자세에는 추종 잔차가 섞여 있어 목표로 되돌려 쓰면 오차가 누적됩니다.
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)

# --- 공급 주기 실측 -------------------------------------------
# 실제로 쓸 모션과 thread 구성 그대로 한 번 돌립니다.
# g_gain = 0.0 이라 보정량은 0 이지만 계산과 호출은 그대로 일어나므로,
# 로봇 궤적은 변하지 않으면서 실사용과 같은 부하로 측정됩니다.
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                                # 실측 실패 시 대비값
if g_calls > 5:
    var_supply = g_max                         # 최대 공급 주기 [sec] - n 산정 기준
tp_log("supply max = {0} ms".format(int(var_supply * 100000) / 100.0))

# 하한 : 최대 공급 주기 / 10ms 를 올림한 값
N_MIN = int(var_supply * 100)
if var_supply * 100 > N_MIN:
    N_MIN = N_MIN + 1
if N_MIN < 1:
    N_MIN = 1

# --- 예제 1. 누적량 모드 (DR_DPOS) ----------------------------
# 매 전송이 완결된 오프셋입니다. 제어기가 한 건을 받지 못해도 다음 전송이
# 그대로 복구하므로 손실이 없습니다.
# 지연 예산이 허용하는 최대로 잡습니다.
N_DPOS = int(LAT_MAX * 100 / 2)
if N_DPOS < N_MIN:
    N_DPOS = N_MIN                             # 하한이 우선입니다

movel(X1, vel=30, acc=60)
mwait(0)                                       # 정지 상태에서 보정 시작
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)                      # 보정이 중첩된 기저 모션

# 종료 순서 : 보정 정지 -> thread_stop -> disable_alter_motion
# 비활성화 후에도 thread 가 살아 있으면 alter_motion() 호출이 남아 에러가 납니다.
g_gain = 0.0
thread_stop(th_id)
disable_alter_motion()

# --- 예제 2. 증분량 모드 (DR_DVEL) ----------------------------
# 센서가 남은 오차를 계속 재는 폐루프에서만 쓰십시오. 제어기가 받지 못한
# 증분은 되돌아오지 않지만, 폐루프면 다음 측정에서 자동으로 재시도됩니다.
# n 은 하한에 붙여 잡습니다. 간헐적으로 누락되는 증분은 폐루프가 다음
# 측정에서 보정합니다.
# read_path_error() 가 폐루프가 아니라면 이 예제 대신 예제 1 을 쓰십시오.
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)                      # 센서 보정이 중첩된 기저 모션

g_gain = 0.0
thread_stop(th_id)
disable_alter_motion()