From a57635e6d0939c070a379fd25a433dae1d6b7e9d Mon Sep 17 00:00:00 2001
From: Akesa-24 <aleksa24@ru.is>
Date: Sat, 3 Oct 2026 13:33:21 +0000
Subject: [PATCH] adding motor test

---
 motor_test.py | 141 ++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 141 insertions(+)
 create mode 100644 motor_test.py

diff --git a/motor_test.py b/motor_test.py
new file mode 100644
index 0000000..3084a50
--- /dev/null
+++ b/motor_test.py
@@ -0,0 +1,141 @@
+#!/usr/bin/env python3
+"""
+motor_test.py - Yahboom 4-ch driver over UART, motors on M3 + M4.
+
+  python3 motor_test.py hand   # motors stay OFF. Turn wheels by hand, watch the counts.
+  python3 motor_test.py spin   # WHEELS OFF THE GROUND. Low PWM steps, one motor at a time.
+
+Ctrl-C always stops the motors.
+"""
+import sys, time, threading
+import serial
+
+PORT = "/dev/serial0"
+BAUD = 115200
+CHANNELS = {"M3": 2, "M4": 3}      # index in the 4-value commands
+TICKS_PER_REV = 30 * 11 * 4         # 520 motor: 1:30 gearbox, 11 pulses/rev, x4 edges
+SPIN_STEPS = [800, 1200, 1600, 2000, 2500, -1600]
+
+
+class Driver:
+    def __init__(self):
+        self.ser = serial.Serial(PORT, BAUD, timeout=0.05)
+        self.lock = threading.Lock()
+        self.totals = None      # latest cumulative counts [M1, M2, M3, M4]
+        self.msgs = 0
+        self.other = []         # last few lines that weren't $MAll, for debugging
+        self.alive = True
+        threading.Thread(target=self._rx, daemon=True).start()
+        self.send("$pwm:0,0,0,0#")
+        self.send("$mtype:1#")          # 1 = Yahboom 520 motor
+        self.send("$upload:1,0,0#")     # stream total encoder counts as $MAll:a,b,c,d#
+
+    def send(self, s):
+        self.ser.write(s.encode())
+        time.sleep(0.1)
+
+    def pwm(self, ch, value):
+        v = [0, 0, 0, 0]
+        v[ch] = int(value)
+        self.ser.write(f"$pwm:{v[0]},{v[1]},{v[2]},{v[3]}#".encode())
+
+    def stop(self):
+        for _ in range(3):
+            self.ser.write(b"$pwm:0,0,0,0#")
+            time.sleep(0.02)
+
+    def get(self):
+        with self.lock:
+            return self.totals, self.msgs, list(self.other)
+
+    def _rx(self):
+        buf = b""
+        while self.alive:
+            buf += self.ser.read(self.ser.in_waiting or 1)
+            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")
+                with self.lock:
+                    if msg.startswith("MAll:"):
+                        try:
+                            self.totals = [int(x) for x in msg[5:].split(",")]
+                            self.msgs += 1
+                        except ValueError:
+                            pass
+                    else:
+                        self.other = (self.other + [msg])[-5:]
+
+    def close(self):
+        self.stop()
+        self.alive = False
+        self.ser.close()
+
+
+def hold(d, ch, value, seconds):
+    """Keep re-sending the PWM so it holds even if the board has a command timeout."""
+    end = time.perf_counter() + seconds
+    while time.perf_counter() < end:
+        d.pwm(ch, value)
+        time.sleep(0.1)
+
+
+def hand(d):
+    base, _, _ = d.get()
+    print("Motors are OFF.")
+    print("Turn ONE wheel by hand, one full turn FORWARD (as if the robot drives toward the tape).")
+    print("Then the other wheel. Ctrl-C when done.\n")
+    while True:
+        t, n, _ = d.get()
+        m3 = t[2] - base[2]
+        m4 = t[3] - base[3]
+        print(f"M3 {m3:+7d} ({m3 / TICKS_PER_REV:+5.2f} rev)    "
+              f"M4 {m4:+7d} ({m4 / TICKS_PER_REV:+5.2f} rev)    [{n} msgs]   ", end="\r", flush=True)
+        time.sleep(0.1)
+
+
+def spin(d):
+    input("Are the wheels OFF the ground? Press Enter to start, Ctrl-C to abort... ")
+    for name, ch in CHANNELS.items():
+        print(f"\n--- {name} ---")
+        for p in SPIN_STEPS:
+            hold(d, ch, p, 0.5)                     # let it spin up
+            t0, _, _ = d.get(); s0 = time.perf_counter()
+            hold(d, ch, p, 1.0)                     # measure over 1 s
+            t1, _, _ = d.get(); s1 = time.perf_counter()
+            cps = (t1[ch] - t0[ch]) / (s1 - s0)
+            rpm = cps / TICKS_PER_REV * 60
+            print(f"pwm {p:+5d}  ->  {cps:+8.0f} counts/s   {rpm:+6.1f} wheel rpm")
+        d.stop()
+        time.sleep(1.0)
+    print("\nDone.")
+
+
+def main():
+    if len(sys.argv) != 2 or sys.argv[1] not in ("hand", "spin"):
+        sys.exit(__doc__)
+    d = Driver()
+    try:
+        time.sleep(1.0)
+        t, n, other = d.get()
+        if t is None:
+            print("No $MAll messages from the driver board after 1 s.")
+            print("Other lines received:", other if other else "nothing at all")
+            print("Check: board powered, Pi TX -> board RX, Pi RX -> board TX, GND shared, serial enabled.")
+            return
+        print(f"Driver board is talking ({n} encoder messages so far).\n")
+        if sys.argv[1] == "hand":
+            hand(d)
+        else:
+            spin(d)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        d.close()
+        print("\nmotors stopped")
+
+
+if __name__ == "__main__":
+    main()
\ No newline at end of file