From 146887d01c782e64a1acbd9c1e39352f49291c65 Mon Sep 17 00:00:00 2001
From: Akesa-24 <aleksa24@ru.is>
Date: Sat, 3 Oct 2026 22:01:30 +0000
Subject: [PATCH] added libraries

---
 balance.py | 39 ++++++++++++++++++++++++++++++---------
 1 file changed, 30 insertions(+), 9 deletions(-)

diff --git a/balance.py b/balance.py
index c585212..43cd22f 100644
--- a/balance.py
+++ b/balance.py
@@ -2,6 +2,7 @@
 """
   sudo python3 balance.py imu                   # find the balance point
   sudo python3 balance.py balance --offset X    # balance
+  sudo python3 balance.py balance --offset X --speed 200   # drive forward at 200 mm/s
 """
 import argparse, math, os, sys, threading, time
 import numpy as np
@@ -17,6 +18,7 @@ START_ANGLE = 5.0                   # deg: arms when held this close to upright
 PWM_MAX     = 2000                  # power-bank limit
 PWM_SLEW    = 300                   # max PWM change per tick (no instant reversals)
 ENC_TIMEOUT = 0.05                  # s without encoder data -> motors off, re-init driver board
+ACCEL_MAX   = 150                   # mm/s per second: how fast the speed setpoint ramps
 LEFT, RIGHT = 2, 3                  # M3 = left, M4 = right
 MOTOR_SIGN  = (-1, +1)              # +PWM on M3 rolls the robot backward, so flip it
 ENC_SIGN    = (-1, +1)              # M3 counts down when rolling forward, so flip it
@@ -167,7 +169,9 @@ def balance(imu, tilt, motors, args):
     out = target = v = 0.0
     prev = prev_t = None
     last_reinit = time.perf_counter()
-    print(f"kp={args.kp} kd={args.kd} kv={args.kv} ki={args.ki} offset={args.offset}")
+    stats = []                                     # (wheel position mm, tilt, pwm) for the stillness readout
+    print(f"kp={args.kp} kd={args.kd} kv={args.kv} ki={args.ki} offset={args.offset} "
+          f"slew={args.slew} deadband={args.deadband} speed={args.speed}")
     print("Hold it upright and still to arm. Ctrl-C to quit.", flush=True)
 
     for tick, (dt, now) in enumerate(ticker(LOOP_HZ)):
@@ -193,6 +197,8 @@ def balance(imu, tilt, motors, args):
             if tilt.ready() and abs(theta) < START_ANGLE and abs(rate) < 30:
                 armed, out, target, v = True, 0.0, 0.0, 0.0
                 speed_pid.reset()
+                speed_pid.setpoint = 0.0               # always start standing still, then ramp
+                stats.clear()
                 prev, prev_t = wheels, now
                 print("ARMED", flush=True)
             elif tick % 20 == 0:
@@ -209,27 +215,42 @@ def balance(imu, tilt, motors, args):
         if tick % 4 == 0:
             v_now = (wheels[0] - prev[0] + wheels[1] - prev[1]) / 2 * MM_PER_TICK / (now - prev_t)
             v = 0.7 * v + 0.3 * v_now                      # mm/s, smoothed
+            speed_pid.setpoint += clamp(args.speed - speed_pid.setpoint, ACCEL_MAX * (now - prev_t))
             target = speed_pid(v, dt=now - prev_t)
             prev, prev_t = wheels, now
 
         # inner loop, 200 Hz: angle PD (D straight from the gyro, no differentiating)
-        u = clamp(args.kp * (theta - target) + args.kd * rate, PWM_MAX)
-        out += clamp(u - out, PWM_SLEW)                   # ramp, never jump
+        u = args.kp * (theta - target) + args.kd * rate
+        if u:
+            u += math.copysign(args.deadband, u)          # help the wheels past static friction
+        u = clamp(u, PWM_MAX)
+        out += clamp(u - out, args.slew)                  # ramp, never jump
         motors.pwm(out, out)
 
+        # stillness readout every 5 s: lower = stiller
+        stats.append(((wheels[0] + wheels[1]) / 2 * MM_PER_TICK, theta, out))
+        if len(stats) == 5 * LOOP_HZ:
+            pos, th, pw = (np.array(c) for c in zip(*stats))
+            print(f"STILLNESS 5s: wheels p-p {np.ptp(pos):5.1f} mm  rms {pos.std():4.1f} mm  "
+                  f"tilt rms {th.std():5.2f} deg  pwm rms {pw.std():4.0f}", flush=True)
+            stats.clear()
+
         if tick % 20 == 0:
-            print(f"th {theta:+6.2f}  tgt {target:+5.2f}  rate {rate:+7.1f}  v {v:+6.0f} mm/s  pwm {out:+6.0f}",
-                  flush=True)
+            print(f"th {theta:+6.2f}  tgt {target:+5.2f}  rate {rate:+7.1f}  v {v:+6.0f} mm/s  "
+                  f"sp {speed_pid.setpoint:+4.0f}  pwm {out:+6.0f}", flush=True)
 
 
 def main():
     p = argparse.ArgumentParser()
     p.add_argument("mode", choices=["imu", "balance"])
     p.add_argument("--offset", type=float, default=0.0, help="pitch at the balance point, deg")
-    p.add_argument("--kp", type=float, default=100.0, help="PWM per deg of lean")
-    p.add_argument("--kd", type=float, default=2.5, help="PWM per deg/s of tipping")
-    p.add_argument("--kv", type=float, default=0.0, help="deg of lean per mm/s of speed")
-    p.add_argument("--ki", type=float, default=0.0, help="deg of lean per mm of drift")
+    p.add_argument("--kp", type=float, default=120.0, help="PWM per deg of lean")
+    p.add_argument("--kd", type=float, default=3.0, help="PWM per deg/s of tipping")
+    p.add_argument("--kv", type=float, default=0.02, help="deg of lean per mm/s of speed")
+    p.add_argument("--ki", type=float, default=0.003, help="deg of lean per mm of drift")
+    p.add_argument("--speed", type=float, default=0.0, help="drive speed, mm/s, + = forward")
+    p.add_argument("--slew", type=float, default=PWM_SLEW, help="max PWM change per tick")
+    p.add_argument("--deadband", type=float, default=0.0, help="PWM added to beat static friction")
     args = p.parse_args()
 
     realtime()