aleksa24/aleksa24-balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

added libraries

Browse Source
This commit is contained in:
aleksa24
2026-10-03 16:04:23 +00:00
parent d18ea4fbe0
commit 8620fde64b
1 changed files with 152 additions and 291 deletions
Download Patch File Download Diff File Expand all files Collapse all files
balance.py
+152 -291
View File
@@ -1,390 +1,251 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
""" """
balance.py - two-wheel balancing robot sudo python3 balance.py imu # find the balance point
Pi Zero 2 W sudo python3 balance.py balance --offset X # balance
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 argparse, math, os, sys, threading, time
import numpy as np
import imufusion
import serial import serial
from simple_pid import PID
from smbus2 import SMBus from smbus2 import SMBus
# ============================ CONFIG ============================ # ---------------- config ----------------
I2C_BUS, IMU_ADDR = 1, 0x68 LOOP_HZ = 200
UART_PORT, UART_BAUD = "/dev/serial0", 115200 FALL_ANGLE = 45.0 # deg: motors off beyond this
START_ANGLE = 5.0 # deg: arms when held this close to upright and still
LOOP_HZ = 200 PWM_MAX = 2000 # power-bank limit
RT_CPU = 3 # core for the control loop (run Doom with: taskset -c 0-2 ...) PWM_SLEW = 300 # max PWM change per tick (no instant reversals)
BETA = 0.05 # Madgwick gain (rad/s): how hard the accelerometer pulls out gyro drift ENC_TIMEOUT = 0.05 # s without encoder data -> motors off, re-init driver board
ANGLE_OFFSET = 0.0 # deg: pitch reading at the true balance point (find it in 'imu' mode) LEFT, RIGHT = 2, 3 # M3 = left, M4 = right
MOTOR_SIGN = (-1, +1) # +PWM on M3 rolls the robot backward, so flip it
# motors / encoders (measured with motor_test.py) ENC_SIGN = (-1, +1) # M3 counts down when rolling forward, so flip it
LEFT_CH, RIGHT_CH = 2, 3 # M3 = left, M4 = right MM_PER_TICK = math.pi * 67.0 / 1320 # 67 mm wheel, 1320 counts per wheel turn
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): def clamp(x, limit):
return lo if x < lo else hi if x > hi else x return max(-limit, min(limit, x))
class IMU: class IMU:
"""MPU-6500 family. read() returns BODY-frame data: x forward, y left, z up.""" """MPU-6500. read() gives body-frame data: x forward, y left, z up."""
def __init__(self, bus): def __init__(self):
self.bus = bus self.bus = SMBus(1)
who = bus.read_byte_data(IMU_ADDR, 0x75) # reset, wake, gyro filter 92 Hz, gyro +-500 deg/s, accel +-2 g, accel filter 41 Hz
if who not in (0x70, 0x71, 0x73, 0x68): for reg, val in [(0x6B, 0x80), (0x6B, 0x01), (0x1A, 0x02), (0x1B, 0x08), (0x1C, 0x00), (0x1D, 0x03)]:
sys.exit(f"Unexpected WHO_AM_I 0x{who:02X} at 0x{IMU_ADDR:02X}") self.bus.write_byte_data(0x68, reg, val)
w = lambda reg, val: bus.write_byte_data(IMU_ADDR, reg, val) time.sleep(0.1)
w(0x6B, 0x80); time.sleep(0.1) # reset print("Calibrating gyro - keep the robot STILL...", flush=True)
w(0x6B, 0x01); time.sleep(0.05) # wake, PLL clock self.bias = np.zeros(3)
w(0x1A, 0x02) # gyro low-pass ~92 Hz (less delay than 41 Hz) samples = []
w(0x1B, 0x08) # gyro +-500 deg/s for _ in range(300):
w(0x1C, 0x00) # accel +-2 g samples.append(self.read()[1])
w(0x1D, 0x03) # accel low-pass ~41 Hz time.sleep(0.003)
time.sleep(0.05) self.bias = np.mean(samples, axis=0)
self.bias = (0.0, 0.0, 0.0) # gyro bias, sensor frame, deg/s print("gyro bias (deg/s):", np.round(self.bias, 2))
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): def read(self):
"""(accel in g, gyro in deg/s) in body frame. d = self.bus.read_i2c_block_data(0x68, 0x3B, 14)
The sensor sits rotated 180 deg about the axle (y), as measured: raw = np.frombuffer(bytes(d), dtype=">i2").astype(float) # ax ay az temp gx gy gz
upright az = -1 g, forward tilt makes ax + and gy +. # sensor is rotated 180 deg about the axle: body = (-x, y, -z)
So body x = -sensor x, body y = sensor y, body z = -sensor z.""" acc = imufusion.remap(raw[0:3] / 16384.0, imufusion.ALIGNMENT_NXPYNZ) # g
(ax, ay, az), (gx, gy, gz) = self._raw() gyr = imufusion.remap(raw[4:7] / 65.5, imufusion.ALIGNMENT_NXPYNZ) - self.bias # deg/s
bx, by, bz = self.bias return acc, gyr
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: class Tilt:
"""6-axis Madgwick filter (gyro + accel), quaternion q = [w, x, y, z].""" def __init__(self):
self.ahrs = imufusion.Ahrs()
self.ahrs.set_settings(imufusion.AhrsSettings(
sample_rate=LOOP_HZ,
convention=imufusion.CONVENTION_NWU, # x forward, y left, z up
gain=0.5,
gyroscope_range=500,
acceleration_rejection=10, # ignore accel while it's 10+ deg off (wheel pushes)
rejection_timeout=5))
def __init__(self, beta): def update(self, acc, gyr, dt):
self.beta = beta """Returns pitch in degrees, + = leaning forward."""
self.q = [1.0, 0.0, 0.0, 0.0] self.ahrs.set_sample_period(dt)
self.ahrs.update_no_magnetometer(gyr, acc)
return imufusion.quaternion_to_euler(self.ahrs.get_quaternion())[1]
def init_from_accel(self, a): def ready(self):
ax, ay, az = a return not self.ahrs.get_flags().startup # Fusion converges for ~3 s at startup
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: class Motors:
def __init__(self): def __init__(self):
self.ser = serial.Serial(UART_PORT, UART_BAUD, timeout=0.02) self.ser = serial.Serial("/dev/serial0", 115200, timeout=0.02)
self.lock = threading.Lock() self.counts, self.stamp, self.error = None, 0.0, None
self.totals = None
self.last_rx = 0.0
self.error = None
self.running = True
threading.Thread(target=self._reader, daemon=True).start() threading.Thread(target=self._reader, daemon=True).start()
self.configure() self.configure()
def configure(self): def configure(self):
"""(Re)send settings. Also used after the board resets from a power dip.""" """Stop, set motor type (1 = 520 motor), stream encoder totals. Also used after a board reset."""
self.ser.write(b"$pwm:0,0,0,0#") for cmd in (b"$pwm:0,0,0,0#", b"$mtype:1#", b"$upload:1,0,0#"):
self.ser.write(b"$mtype:1#"); time.sleep(0.05) # 1 = Yahboom 520 motor self.ser.write(cmd)
self.ser.write(b"$upload:1,0,0#"); time.sleep(0.05) # stream totals as $MAll time.sleep(0.05)
def pwm(self, left, right): def pwm(self, left, right):
v = [0, 0, 0, 0] v = [0, 0, 0, 0]
v[LEFT_CH] = int(LEFT_DIR * left) v[LEFT], v[RIGHT] = int(MOTOR_SIGN[0] * left), int(MOTOR_SIGN[1] * right)
v[RIGHT_CH] = int(RIGHT_DIR * right) self.ser.write(b"$pwm:%d,%d,%d,%d#" % tuple(v))
self.ser.write(f"$pwm:{v[0]},{v[1]},{v[2]},{v[3]}#".encode())
def wheels(self): def wheels(self, now):
"""((left, right) total counts with forward = +, time of last update).""" """((left, right) total counts with forward = +, seconds since last update)."""
with self.lock: c = self.counts
t, ts = self.totals, self.last_rx if c is None:
if t is None: return None, 1e9
return None, ts return (ENC_SIGN[0] * c[LEFT], ENC_SIGN[1] * c[RIGHT]), now - self.stamp
return (ENC_LEFT_DIR * t[LEFT_CH], ENC_RIGHT_DIR * t[RIGHT_CH]), ts
def _reader(self): def _reader(self):
buf = b"" buf = b""
while self.running: while True:
try: try:
buf += self.ser.read(self.ser.in_waiting or 1) buf += self.ser.read(self.ser.in_waiting or 1)
except serial.SerialException as e: except serial.SerialException as e:
self.error = str(e) self.error = e
return return
while b"#" in buf: *lines, buf = buf.split(b"#")
line, buf = buf.split(b"#", 1) for line in lines:
i = line.rfind(b"$") if b"$MAll:" in line:
if i < 0:
continue
msg = line[i + 1:].decode(errors="ignore")
if msg.startswith("MAll:"):
try: try:
vals = [int(x) for x in msg[5:].split(",")] self.counts = [int(x) for x in line.split(b"$MAll:")[-1].split(b",")]
self.stamp = time.perf_counter()
except ValueError: except ValueError:
continue pass
with self.lock:
self.totals = vals
self.last_rx = time.perf_counter()
def stop(self): def stop(self):
for _ in range(3): for _ in range(3):
try: self.ser.write(b"$pwm:0,0,0,0#")
self.ser.write(b"$pwm:0,0,0,0#")
except serial.SerialException:
pass
time.sleep(0.02) time.sleep(0.02)
self.running = False
self.ser.close()
def go_realtime(): def realtime():
try: """Pin to core 3 with real-time priority (needs sudo). Run Doom with: taskset -c 0-2 ..."""
os.sched_setaffinity(0, {RT_CPU})
except OSError as e:
print("affinity failed:", e)
try: try:
os.sched_setaffinity(0, {3})
os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(50)) os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(50))
except PermissionError: except OSError as e:
print("Not root: no real-time priority, expect more timing jitter.") print(f"No real-time priority ({e}) - run with sudo. Expect more timing jitter.")
def ticker(hz): def ticker(hz):
"""Yields the real dt of each tick while holding the loop rate.""" """Yields (dt, now) at a steady rate."""
period = 1.0 / hz period = 1.0 / hz
nxt = last = time.perf_counter() nxt = last = time.perf_counter()
while True: while True:
nxt += period nxt += period
delay = nxt - time.perf_counter() time.sleep(max(0.0, nxt - time.perf_counter()))
if delay > 0:
time.sleep(delay)
else:
nxt = time.perf_counter()
now = time.perf_counter() now = time.perf_counter()
yield now - last nxt = max(nxt, now - period) # don't try to catch up after a stall
yield now - last, now
last = now last = now
def run_imu(imu, args): def show_imu(imu, tilt, args):
f = Madgwick(args.beta) worst = 0.0
a, _ = imu.read() for i, (dt, now) in enumerate(ticker(LOOP_HZ)):
f.init_from_accel(a) acc, gyr = imu.read()
comp = accel_pitch(a) pitch = tilt.update(acc, gyr, dt)
worst, n = 0.0, 0 worst = max(worst, dt)
print("\n accel compl. MADGWICK rel.offset rate worst dt") if i % 20 == 0:
for dt in ticker(LOOP_HZ): acc_pitch = math.degrees(math.atan2(-acc[0], acc[2]))
a, g = imu.read() print(f"accel-only {acc_pitch:+7.2f} FUSION {pitch:+7.2f} minus offset {pitch - args.offset:+7.2f} "
f.update((math.radians(g[0]), math.radians(g[1]), math.radians(g[2])), a, dt) f"rate {gyr[1]:+7.1f} {'ready' if tilt.ready() else 'settling'} worst dt {worst * 1000:4.1f} ms",
acc = accel_pitch(a) flush=True)
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 worst = 0.0
def run_balance(imu, motors, args): def balance(imu, tilt, motors, args):
f = Madgwick(args.beta) # outer loop: wheel speed -> lean target. Error = 0 - speed, so P leans back against speed
a, _ = imu.read() # and I (integral of speed = distance) leans back toward the start position.
f.init_from_accel(a) speed_pid = PID(args.kv, args.ki, 0, setpoint=0, sample_time=None, output_limits=(-8, 8))
mm_per_tick = math.pi * WHEEL_D_MM / TICKS_PER_REV
armed = False armed = False
out = 0.0 # PWM actually sent, after the slew limit out = target = v = 0.0
tick = 0 prev = prev_t = None
target = v_filt = dist = 0.0 last_reinit = time.perf_counter()
prev_w, prev_t = None, 0.0 print(f"kp={args.kp} kd={args.kd} kv={args.kv} ki={args.ki} offset={args.offset}")
last_reinit = 0.0 print("Hold it upright and still to arm. Ctrl-C to quit.", flush=True)
print(f"\nkp={args.kp} kd={args.kd} kv={args.kv} ki={args.ki} offset={args.offset} " for tick, (dt, now) in enumerate(ticker(LOOP_HZ)):
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: if motors.error:
sys.exit(f"Serial error: {motors.error}") sys.exit(f"Serial error: {motors.error}")
acc, gyr = imu.read()
theta = tilt.update(acc, gyr, dt) - args.offset # deg, + = leaning forward
rate = gyr[1] # deg/s, + = tipping forward
a, g = imu.read() # watchdog: the driver board went quiet (power dip resets it)
f.update((math.radians(g[0]), math.radians(g[1]), math.radians(g[2])), a, dt) wheels, age = motors.wheels(now)
theta = f.pitch() - args.offset # deg, + = leaning forward if wheels is None or age > ENC_TIMEOUT:
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: if armed:
armed = False print("ENCODER DATA LOST - motors off, re-initialising driver board", flush=True)
out = 0.0 armed, out = False, 0.0
motors.pwm(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: if now - last_reinit > 0.5:
motors.configure() motors.configure()
last_reinit = time.perf_counter() last_reinit = time.perf_counter()
continue continue
# --- waiting to arm ---
if not armed: if not armed:
if tick % 20 == 0: if tilt.ready() and abs(theta) < START_ANGLE and abs(rate) < 30:
motors.pwm(0, 0) armed, out, target, v = True, 0.0, 0.0, 0.0
if abs(theta) < START_ANGLE and abs(rate) < 30: speed_pid.reset()
armed = True prev, prev_t = wheels, now
out = 0.0
target = v_filt = dist = 0.0
prev_w, prev_t = w, now
print("ARMED", flush=True) print("ARMED", flush=True)
elif tick % 20 == 0:
motors.pwm(0, 0)
continue continue
# --- fall cut-off ---
if abs(theta) > FALL_ANGLE: if abs(theta) > FALL_ANGLE:
armed = False armed = False
out = 0.0
motors.pwm(0, 0) motors.pwm(0, 0)
print(f"FELL ({theta:+.1f} deg) - motors off", flush=True) print(f"FELL ({theta:+.1f} deg) - motors off", flush=True)
continue continue
# --- outer loop, 50 Hz: wheel speed -> lean target --- # outer loop, 50 Hz: speed PI -> target lean angle
if tick % SPEED_DIV == 0: if tick % 4 == 0:
dts = now - prev_t v_now = (wheels[0] - prev[0] + wheels[1] - prev[1]) / 2 * MM_PER_TICK / (now - prev_t)
v = ((w[0] - prev_w[0]) + (w[1] - prev_w[1])) * 0.5 * mm_per_tick / dts # mm/s v = 0.7 * v + 0.3 * v_now # mm/s, smoothed
prev_w, prev_t = w, now target = speed_pid(v, dt=now - prev_t)
v_filt = 0.7 * v_filt + 0.3 * v prev, prev_t = wheels, now
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 --- # inner loop, 200 Hz: angle PD (D straight from the gyro, no differentiating)
u = args.kp * (theta - target) + args.kd * rate u = clamp(args.kp * (theta - target) + args.kd * rate, PWM_MAX)
if u > 0: out += clamp(u - out, PWM_SLEW) # ramp, never jump
u += args.deadband motors.pwm(out, out)
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: if tick % 20 == 0:
print(f"th {theta:+6.2f} tgt {target:+5.2f} rate {rate:+7.1f} " print(f"th {theta:+6.2f} tgt {target:+5.2f} rate {rate:+7.1f} v {v:+6.0f} mm/s pwm {out:+6.0f}",
f"v {v_filt:+6.0f} mm/s pwm {out:+6.0f}", flush=True) flush=True)
def main(): def main():
ap = argparse.ArgumentParser() p = argparse.ArgumentParser()
ap.add_argument("mode", choices=["imu", "balance"]) p.add_argument("mode", choices=["imu", "balance"])
ap.add_argument("--kp", type=float, default=100.0) # PWM per deg p.add_argument("--offset", type=float, default=0.0, help="pitch at the balance point, deg")
ap.add_argument("--kd", type=float, default=2.5) # PWM per deg/s p.add_argument("--kp", type=float, default=100.0, help="PWM per deg of lean")
ap.add_argument("--kv", type=float, default=0.0) # deg of lean per mm/s p.add_argument("--kd", type=float, default=2.5, help="PWM per deg/s of tipping")
ap.add_argument("--ki", type=float, default=0.0) # deg of lean per mm p.add_argument("--kv", type=float, default=0.0, help="deg of lean per mm/s of speed")
ap.add_argument("--offset", type=float, default=ANGLE_OFFSET) p.add_argument("--ki", type=float, default=0.0, help="deg of lean per mm of drift")
ap.add_argument("--beta", type=float, default=BETA) args = p.parse_args()
ap.add_argument("--deadband", type=float, default=DEADBAND)
args = ap.parse_args()
go_realtime() realtime()
gc.collect(); gc.disable() imu, tilt = IMU(), Tilt()
bus = SMBus(I2C_BUS)
imu = IMU(bus)
imu.calibrate()
motors = Motors() if args.mode == "balance" else None motors = Motors() if args.mode == "balance" else None
try: try:
if args.mode == "imu": if motors:
run_imu(imu, args) balance(imu, tilt, motors, args)
else: else:
run_balance(imu, motors, args) show_imu(imu, tilt, args)
except KeyboardInterrupt: except KeyboardInterrupt:
pass pass
finally: finally:
if motors: if motors:
motors.stop() motors.stop()
bus.close() print("\nmotors stopped")
print("\nmotors stopped")
if __name__ == "__main__": if __name__ == "__main__":