# steer_calc.py — ใบงานเสริม 2.2F ตอนที่ 2
# ตรวจผลคำนวณด้วยมือ: ตำแหน่งใบหน้า target_cx -> heading_error, turn, left_us, right_us
# รันบนคอมพิวเตอร์ได้ ไม่มีคำสั่งควบคุมฮาร์ดแวร์

import math

STEER_GAIN = 0.15
MAX_TURN_SPEED = 0.25
STEER_CURVE = 0.4
CENTER_DEADBAND = 0.05


def clamp(v, lo, hi):
    return max(lo, min(hi, v))


def steer(target_cx):
    heading_error = (target_cx - 0.5) * 2.0
    if abs(heading_error) < CENTER_DEADBAND:
        heading_error = 0.0
    shaped = math.copysign(abs(heading_error) ** STEER_CURVE, heading_error)
    turn = clamp(STEER_GAIN * shaped, -MAX_TURN_SPEED, MAX_TURN_SPEED)
    left_us = round(1500 + turn * 500)
    right_us = round(1500 - turn * 500)
    return heading_error, turn, left_us, right_us


print("target_cx  heading_error  turn     left_us  right_us  right_us หลังกลับขั้ว")
for cx in (0.75, 0.52, 0.10):
    e, t, l, r = steer(cx)
    print(f"{cx:<10} {e:<14.2f} {t:<8.3f} {l:<8} {r:<9} {3000 - r}")

# ลองเปลี่ยน STEER_GAIN หรือ CENTER_DEADBAND ด้านบน แล้วรันใหม่เพื่อดูผล