Marketplace › From Ordinate 2D
PID: balance a pole on a cart
An inverted pendulum on a cart held up by a PID controller written in the script.
Use in Ordinate 2DOpens in the Script tab: it runs when you press Run. Free, no account needed.
"""Balance an inverted pendulum on a cart with a PID controller.
The cart slides on a frictionless rail; a 1 m pole is pinned on top of it, tilted 5° at the start. Each step the
controller reads the pole's angle and pushes the cart: an inner PID loop holds the pole upright, and a slow outer loop
leans it a little towards the middle of the rail so the cart does not run away.
Runs in the studio (Script tab → Run) and locally: python3 cartpole_pid.py
"""
import math
import studio as s
s.clear()
s.gravity = (0, -9.81)
rail = s.ground(0, width=12, name="Rail")
cart = s.box(0, 0.3, 0.5, 0.2, mass=1.0, name="Cart", color="#4f8ff7")
pole = s.box(-0.5 * math.sin(math.radians(5)), 0.4 + 0.5 * math.cos(math.radians(5)), 0.04, 1.0, mass=0.2,
angle=math.radians(5), name="Pole", color="#e5484d", collide=False)
s.slider(rail, cart, axis=(1, 0))
hinge = s.pin(cart, pole, at=(0, 0.4), name="Hinge")
KP, KI, KD = 40.0, 2.0, 8.0 # the pole: N per rad, N per rad·s, N per rad/s
KX, KV = 0.06, 0.12 # the cart: lean (rad) per m off-centre and per m/s
integral = 0.0
@s.on_start
def start():
global integral
integral = 0.0
@s.on_step
def control(t, dt):
global integral
lean = KX * cart.x + KV * cart.vx # lean towards the middle: the cart follows
error = pole.angle - lean # rad, counter-clockwise = leaning left
integral += error * dt
push = KP * error + KI * integral + KD * pole.omega
cart.force = (-push, 0) # leaning left: move the cart left, under the pole
if round(t / dt) % 24 == 0: # ten rows a second
s.record(t=t, angle=math.degrees(pole.angle), x=cart.x, force=-push)
@s.on_end
def report():
print(f"after {s.time:.1f} s: pole {math.degrees(pole.angle):+.2f}°, cart at {cart.x:+.3f} m")
s.run(10)