PD control: move a cart to a target

Script by Ordinate 2D ✓ · Machines and mechanisms

F = kp·(target − x) − kd·v moves a cart to four targets; the console gives each move's overshoot and settling time.

Use in Ordinate 2DOpens in the Script tab: it runs when you press Run. Free, no account needed.

"""PD control: move a cart to a target and stop it there.

A 2 kg cart on a smooth rail is pushed by a force the script works out every step: F = kp·(target − x) − kd·v.
kp pulls it towards the target, kd brakes it. With kd = 2√(kp·m) (critical damping) it arrives without overshooting;
try smaller kd (it overshoots and rings) or larger (it creeps in). The target jumps every 4 s; each row of the Script sheet
is a moment of the run, and the console says how far each move overshot and when it settled to within 1 cm.

Runs in the studio (Script tab → Run) and locally:  python3 pd_position_control.py
"""
import math
import studio as s

M_CART = 2.0
KP = 40.0                              # N/m
KD = 2 * math.sqrt(KP * M_CART)        # N·s/m: critical damping (try 0.3 × this, or 3 ×)
TARGETS = [2.0, -1.0, 3.0, 0.0]        # m, each held for 4 s

s.clear()
s.gravity = (0, -9.81)
rail = s.box(0, -0.15, 12, 0.1, static=True, name="Rail", color="#8b939b", friction=0)
cart = s.box(0, 0.15, 0.6, 0.3, mass=M_CART, name="Cart", color="#4f8ff7", friction=0)
s.slider(rail, cart, axis=(1, 0))
marker = s.box(TARGETS[0], 0.6, 0.04, 0.5, static=True, name="Target", color="#30a46c", collide=False)
leg = {"i": 0, "start_x": 0.0, "peak": 0.0, "settled": None}


@s.on_step
def control(t, dt):
    i = min(int(t // 4), len(TARGETS) - 1)
    if i != leg["i"]:
        report(leg)
        leg.update(i=i, start_x=cart.x, peak=0.0, settled=None)
        marker.x = TARGETS[i]
    target = TARGETS[i]
    err = target - cart.x
    force = KP * err - KD * cart.vx
    cart.force = (force, 0)
    direction = 1 if target >= leg["start_x"] else -1
    leg["peak"] = max(leg["peak"], -direction * err)          # how far past the target
    if abs(err) < 0.01 and abs(cart.vx) < 0.01:
        if leg["settled"] is None:
            leg["settled"] = t - 4 * i
    else:
        leg["settled"] = None
    s.record(t=t, x=cart.x, target=target, v=cart.vx, F=force)


def report(l):
    settle = f"settled in {l['settled']:.2f} s" if l["settled"] is not None else "not settled in 4 s"
    print(f"move {l['i'] + 1} to {TARGETS[l['i']]:+.1f} m: overshoot {max(0.0, l['peak']) * 100:.1f} cm, {settle}")


@s.on_end
def last():
    report(leg)


s.run(4 * len(TARGETS))

table control slider joint forces from a script