#!/usr/bin/env python3 """ balance.py - two-wheel balancing robot Pi Zero 2 W MPU-6500 (GY-6500 / GY-9250 board) on I2C 0x68, mounted upside down Yahboom 4-ch driver over UART (/dev/serial0), 520 motors: left = M3, right = M4 sudo python3 balance.py imu # watch tilt, find the balance point (--offset) sudo python3 balance.py balance --offset X # balance (tune kp/kd first, kv/ki = 0) sudo python3 balance.py balance --offset X --kp 120 --kd 3 --kv 0.02 --ki 0.003 Tilt comes from a 6-axis Madgwick filter. Pitch is + when leaning FORWARD (toward the tape). """ import argparse, gc, math, os, sys, threading, time import serial from smbus2 import SMBus # ============================ CONFIG ============================ I2C_BUS, IMU_ADDR = 1, 0x68 UART_PORT, UART_BAUD = "/dev/serial0", 115200 LOOP_HZ = 200 RT_CPU = 3 # core for the control loop (run Doom with: taskset -c 0-2 ...) BETA = 0.05 # Madgwick gain (rad/s): how hard the accelerometer pulls out gyro drift ANGLE_OFFSET = 0.0 # deg: pitch reading at the true balance point (find it in 'imu' mode) # motors / encoders (measured with motor_test.py) LEFT_CH, RIGHT_CH = 2, 3 # M3 = left, M4 = right LEFT_DIR, RIGHT_DIR = -1, +1 # +PWM on M3 rolls the robot backward, so flip it ENC_LEFT_DIR, ENC_RIGHT_DIR = -1, +1 # M3 counts down when rolling forward, so flip it TICKS_PER_REV = 30 * 11 * 4 # 1320 (measured ~1300 by hand) WHEEL_D_MM = 67.0 # safety PWM_MAX = 2000 # power-bank limit; barely any extra speed above this anyway PWM_SLEW = 150 # max PWM change per tick: full reverse takes ~27 ms instead of 0 DEADBAND = 0 # PWM added to beat static friction (wheels moved at 800 in the air) FALL_ANGLE = 45.0 # deg: motors off beyond this START_ANGLE = 5.0 # deg: arms when held this close to upright and still ENC_TIMEOUT = 0.05 # s without encoder data -> motors off, re-init the driver board TARGET_LIMIT = 8.0 # deg: max lean the speed loop may ask for SPEED_DIV = 4 # speed loop every 4th tick = 50 Hz # ================================================================ def clamp(x, lo, hi): return lo if x < lo else hi if x > hi else x class IMU: """MPU-6500 family. read() returns BODY-frame data: x forward, y left, z up.""" def __init__(self, bus): self.bus = bus who = bus.read_byte_data(IMU_ADDR, 0x75) if who not in (0x70, 0x71, 0x73, 0x68): sys.exit(f"Unexpected WHO_AM_I 0x{who:02X} at 0x{IMU_ADDR:02X}") w = lambda reg, val: bus.write_byte_data(IMU_ADDR, reg, val) w(0x6B, 0x80); time.sleep(0.1) # reset w(0x6B, 0x01); time.sleep(0.05) # wake, PLL clock w(0x1A, 0x02) # gyro low-pass ~92 Hz (less delay than 41 Hz) w(0x1B, 0x08) # gyro +-500 deg/s w(0x1C, 0x00) # accel +-2 g w(0x1D, 0x03) # accel low-pass ~41 Hz time.sleep(0.05) self.bias = (0.0, 0.0, 0.0) # gyro bias, sensor frame, deg/s def _raw(self): d = self.bus.read_i2c_block_data(IMU_ADDR, 0x3B, 14) s = lambda i: (((d[i] << 8) | d[i + 1]) ^ 0x8000) - 0x8000 acc = (s(0) / 16384.0, s(2) / 16384.0, s(4) / 16384.0) gyr = (s(8) / 65.5, s(10) / 65.5, s(12) / 65.5) return acc, gyr def read(self): """(accel in g, gyro in deg/s) in body frame. The sensor sits rotated 180 deg about the axle (y), as measured: upright az = -1 g, forward tilt makes ax + and gy +. So body x = -sensor x, body y = sensor y, body z = -sensor z.""" (ax, ay, az), (gx, gy, gz) = self._raw() bx, by, bz = self.bias return (-ax, ay, -az), (-(gx - bx), gy - by, -(gz - bz)) def calibrate(self, seconds=1.0): for attempt in range(5): print("Calibrating gyro - keep the robot STILL...", flush=True) samples = [] end = time.perf_counter() + seconds while time.perf_counter() < end: samples.append(self._raw()[1]) time.sleep(0.003) spread = max(max(s[k] for s in samples) - min(s[k] for s in samples) for k in range(3)) if spread < 4.0: n = len(samples) self.bias = tuple(sum(s[k] for s in samples) / n for k in range(3)) print("Gyro bias (deg/s): " + ", ".join(f"{b:+.2f}" for b in self.bias)) return print(f" moved during calibration (spread {spread:.1f} deg/s), retrying") sys.exit("Could not calibrate - robot kept moving.") class Madgwick: """6-axis Madgwick filter (gyro + accel), quaternion q = [w, x, y, z].""" def __init__(self, beta): self.beta = beta self.q = [1.0, 0.0, 0.0, 0.0] def init_from_accel(self, a): ax, ay, az = a roll = math.atan2(ay, az) pitch = math.atan2(-ax, math.hypot(ay, az)) cr, sr = math.cos(roll / 2), math.sin(roll / 2) cp, sp = math.cos(pitch / 2), math.sin(pitch / 2) self.q = [cr * cp, sr * cp, cr * sp, -sr * sp] def update(self, g, a, dt): """g: body gyro in rad/s, a: body accel (any unit), dt in s.""" q0, q1, q2, q3 = self.q gx, gy, gz = g ax, ay, az = a # rate of change of the quaternion from the gyro qd0 = 0.5 * (-q1 * gx - q2 * gy - q3 * gz) qd1 = 0.5 * ( q0 * gx + q2 * gz - q3 * gy) qd2 = 0.5 * ( q0 * gy - q1 * gz + q3 * gx) qd3 = 0.5 * ( q0 * gz + q1 * gy - q2 * gx) # accelerometer correction: step toward the orientation that matches gravity n = math.sqrt(ax * ax + ay * ay + az * az) if n > 0: ax /= n; ay /= n; az /= n _2q0, _2q1, _2q2, _2q3 = 2 * q0, 2 * q1, 2 * q2, 2 * q3 _4q0, _4q1, _4q2 = 4 * q0, 4 * q1, 4 * q2 _8q1, _8q2 = 8 * q1, 8 * q2 q0q0, q1q1, q2q2, q3q3 = q0 * q0, q1 * q1, q2 * q2, q3 * q3 s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay s1 = (_4q1 * q3q3 - _2q3 * ax + 4 * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az) s2 = (4 * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az) s3 = 4 * q1q1 * q3 - _2q1 * ax + 4 * q2q2 * q3 - _2q2 * ay sn = math.sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3) if sn > 0: b = self.beta / sn qd0 -= b * s0; qd1 -= b * s1; qd2 -= b * s2; qd3 -= b * s3 q0 += qd0 * dt; q1 += qd1 * dt; q2 += qd2 * dt; q3 += qd3 * dt n = math.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3) self.q = [q0 / n, q1 / n, q2 / n, q3 / n] def pitch(self): """Forward lean in degrees, measured in the plane the robot falls in.""" q0, q1, q2, q3 = self.q vx = 2 * (q1 * q3 - q0 * q2) # 'up' direction seen from the body vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3 return math.degrees(math.atan2(-vx, vz)) def accel_pitch(a): return math.degrees(math.atan2(-a[0], a[2])) class Motors: def __init__(self): self.ser = serial.Serial(UART_PORT, UART_BAUD, timeout=0.02) self.lock = threading.Lock() self.totals = None self.last_rx = 0.0 self.error = None self.running = True threading.Thread(target=self._reader, daemon=True).start() self.configure() def configure(self): """(Re)send settings. Also used after the board resets from a power dip.""" self.ser.write(b"$pwm:0,0,0,0#") self.ser.write(b"$mtype:1#"); time.sleep(0.05) # 1 = Yahboom 520 motor self.ser.write(b"$upload:1,0,0#"); time.sleep(0.05) # stream totals as $MAll def pwm(self, left, right): v = [0, 0, 0, 0] v[LEFT_CH] = int(LEFT_DIR * left) v[RIGHT_CH] = int(RIGHT_DIR * right) self.ser.write(f"$pwm:{v[0]},{v[1]},{v[2]},{v[3]}#".encode()) def wheels(self): """((left, right) total counts with forward = +, time of last update).""" with self.lock: t, ts = self.totals, self.last_rx if t is None: return None, ts return (ENC_LEFT_DIR * t[LEFT_CH], ENC_RIGHT_DIR * t[RIGHT_CH]), ts def _reader(self): buf = b"" while self.running: try: buf += self.ser.read(self.ser.in_waiting or 1) except serial.SerialException as e: self.error = str(e) return while b"#" in buf: line, buf = buf.split(b"#", 1) i = line.rfind(b"$") if i < 0: continue msg = line[i + 1:].decode(errors="ignore") if msg.startswith("MAll:"): try: vals = [int(x) for x in msg[5:].split(",")] except ValueError: continue with self.lock: self.totals = vals self.last_rx = time.perf_counter() def stop(self): for _ in range(3): try: self.ser.write(b"$pwm:0,0,0,0#") except serial.SerialException: pass time.sleep(0.02) self.running = False self.ser.close() def go_realtime(): try: os.sched_setaffinity(0, {RT_CPU}) except OSError as e: print("affinity failed:", e) try: os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(50)) except PermissionError: print("Not root: no real-time priority, expect more timing jitter.") def ticker(hz): """Yields the real dt of each tick while holding the loop rate.""" period = 1.0 / hz nxt = last = time.perf_counter() while True: nxt += period delay = nxt - time.perf_counter() if delay > 0: time.sleep(delay) else: nxt = time.perf_counter() now = time.perf_counter() yield now - last last = now def run_imu(imu, args): f = Madgwick(args.beta) a, _ = imu.read() f.init_from_accel(a) comp = accel_pitch(a) worst, n = 0.0, 0 print("\n accel compl. MADGWICK rel.offset rate worst dt") for dt in ticker(LOOP_HZ): a, g = imu.read() f.update((math.radians(g[0]), math.radians(g[1]), math.radians(g[2])), a, dt) acc = accel_pitch(a) comp = 0.98 * (comp + g[1] * dt) + 0.02 * acc mad = f.pitch() worst = max(worst, dt); n += 1 if n % (LOOP_HZ // 10) == 0: print(f"{acc:+7.2f} {comp:+7.2f} {mad:+8.2f} {mad - args.offset:+8.2f} " f"{g[1]:+7.1f} {worst * 1000:5.1f} ms", flush=True) worst = 0.0 def run_balance(imu, motors, args): f = Madgwick(args.beta) a, _ = imu.read() f.init_from_accel(a) mm_per_tick = math.pi * WHEEL_D_MM / TICKS_PER_REV armed = False out = 0.0 # PWM actually sent, after the slew limit tick = 0 target = v_filt = dist = 0.0 prev_w, prev_t = None, 0.0 last_reinit = 0.0 print(f"\nkp={args.kp} kd={args.kd} kv={args.kv} ki={args.ki} offset={args.offset} " f"beta={args.beta}\nHold it upright and still to arm. Ctrl-C to quit.\n") for dt in ticker(LOOP_HZ): if motors.error: sys.exit(f"Serial error: {motors.error}") a, g = imu.read() f.update((math.radians(g[0]), math.radians(g[1]), math.radians(g[2])), a, dt) theta = f.pitch() - args.offset # deg, + = leaning forward rate = g[1] # deg/s, + = tipping forward tick += 1 now = time.perf_counter() # --- watchdog: encoder data must keep flowing --- w, w_time = motors.wheels() if w is None or now - w_time > ENC_TIMEOUT: if armed: armed = False out = 0.0 motors.pwm(0, 0) print("ENCODER DATA LOST - motors off, re-initialising the driver board", flush=True) if now - last_reinit > 0.5: motors.configure() last_reinit = time.perf_counter() continue # --- waiting to arm --- if not armed: if tick % 20 == 0: motors.pwm(0, 0) if abs(theta) < START_ANGLE and abs(rate) < 30: armed = True out = 0.0 target = v_filt = dist = 0.0 prev_w, prev_t = w, now print("ARMED", flush=True) continue # --- fall cut-off --- if abs(theta) > FALL_ANGLE: armed = False out = 0.0 motors.pwm(0, 0) print(f"FELL ({theta:+.1f} deg) - motors off", flush=True) continue # --- outer loop, 50 Hz: wheel speed -> lean target --- if tick % SPEED_DIV == 0: dts = now - prev_t v = ((w[0] - prev_w[0]) + (w[1] - prev_w[1])) * 0.5 * mm_per_tick / dts # mm/s prev_w, prev_t = w, now v_filt = 0.7 * v_filt + 0.3 * v dist = clamp(dist + v_filt * dts, -500.0, 500.0) # mm target = clamp(-(args.kv * v_filt + args.ki * dist), -TARGET_LIMIT, TARGET_LIMIT) # --- inner loop, 200 Hz: angle PD --- u = args.kp * (theta - target) + args.kd * rate if u > 0: u += args.deadband elif u < 0: u -= args.deadband u = clamp(u, -PWM_MAX, PWM_MAX) out += clamp(u - out, -PWM_SLEW, PWM_SLEW) # never jump, ramp turn = 0.0 # hook for steering later motors.pwm(out + turn, out - turn) if tick % (LOOP_HZ // 10) == 0: print(f"th {theta:+6.2f} tgt {target:+5.2f} rate {rate:+7.1f} " f"v {v_filt:+6.0f} mm/s pwm {out:+6.0f}", flush=True) def main(): ap = argparse.ArgumentParser() ap.add_argument("mode", choices=["imu", "balance"]) ap.add_argument("--kp", type=float, default=100.0) # PWM per deg ap.add_argument("--kd", type=float, default=2.5) # PWM per deg/s ap.add_argument("--kv", type=float, default=0.0) # deg of lean per mm/s ap.add_argument("--ki", type=float, default=0.0) # deg of lean per mm ap.add_argument("--offset", type=float, default=ANGLE_OFFSET) ap.add_argument("--beta", type=float, default=BETA) ap.add_argument("--deadband", type=float, default=DEADBAND) args = ap.parse_args() go_realtime() gc.collect(); gc.disable() bus = SMBus(I2C_BUS) imu = IMU(bus) imu.calibrate() motors = Motors() if args.mode == "balance" else None try: if args.mode == "imu": run_imu(imu, args) else: run_balance(imu, motors, args) except KeyboardInterrupt: pass finally: if motors: motors.stop() bus.close() print("\nmotors stopped") if __name__ == "__main__": main()