shibosoftwaredev/f1c100s-linux-nema17-stepper-controller

firmware/motor-control.py

#!/usr/bin/env python3
"""Reference Linux client for the STM32G030 NEMA17 stepper controller.

The STM32, not Linux, generates STEP pulses and enforces the watchdog. Install
pyserial, program matching safety-reviewed MCU firmware, and keep the motor
mechanically unloaded during first bring-up.
"""

import argparse
import json
import struct
import threading
import time


SYNC = b"\xA5\x5A"
VERSION = 1
CMD_HEARTBEAT = 0x01
CMD_ARM = 0x02
CMD_MOVE = 0x10
CMD_STOP = 0x11
CMD_DISABLE = 0x12
CMD_STATUS = 0x20
CMD_CURRENT = 0x21
CMD_SET_LIMIT = 0x22
CMD_LIMIT = 0x23
STATUS_TEXT = {
    0: "ok", 1: "bad command", 2: "bad argument", 3: "not armed",
    4: "busy", 5: "driver fault", 6: "over-temperature",
}


def crc16_ccitt(data: bytes) -> int:
    crc = 0xFFFF
    for byte in data:
        crc ^= byte << 8
        for _ in range(8):
            crc = ((crc << 1) ^ 0x1021) & 0xFFFF if crc & 0x8000 else (crc << 1) & 0xFFFF
    return crc


def decode_current(payload: bytes) -> dict:
    """Decode the 17 bytes after command status; invalid/clipped values are None."""
    if len(payload) != 17:
        raise ValueError("unexpected coil-current payload length")
    flags, a_ma, b_ma, raw_a, raw_b, vdda_mv, age_ms = struct.unpack("<BiiHHHH", payload)
    if flags & ~0x0F:
        raise ValueError("unknown coil-current flags")
    if (flags & 1 and flags & 4) or (flags & 2 and flags & 8):
        raise ValueError("current cannot be both valid and clipped")
    return {"phase_a_mA": a_ma if flags & 1 else None,
            "phase_b_mA": b_ma if flags & 2 else None,
            "phase_a_clipped": bool(flags & 4), "phase_b_clipped": bool(flags & 8),
            "raw_a": raw_a, "raw_b": raw_b, "vdda_mV": vdda_mv, "sample_age_ms": age_ms}


def decode_limit(payload: bytes) -> dict:
    if len(payload) != 9:
        raise ValueError("unexpected current-limit payload length")
    flags, requested, code, nominal, vdda = struct.unpack("<BHHHH", payload)
    if (flags & ~3 or (flags & 1 and not flags & 2) or requested > 396
            or (code > 4095 and not (code == 0xFFFF and not flags & 1)) or nominal > 433):
        raise ValueError("invalid current-limit response")
    return {"ready": bool(flags & 1), "dac_ok": bool(flags & 2),
            "requested_mA": requested, "dac_code": None if code == 0xFFFF else code,
            "nominal_applied_mA": nominal, "dac_vdda_mV": vdda}


def wait_limit(link, requested=None):
    deadline = time.monotonic() + 0.3
    while time.monotonic() < deadline:
        value = decode_limit(link.request(CMD_LIMIT))
        if not value["dac_ok"]:
            raise RuntimeError("current-limit DAC is not healthy; motor remains disabled")
        if value["ready"] and (requested is None or value["requested_mA"] == requested):
            return value
        time.sleep(0.005)
    raise TimeoutError("current-limit write/readback or settling did not finish")


class MotionLink:
    def __init__(self, serial_port):
        self.serial = serial_port
        self.sequence = 0
        self.lock = threading.Lock()

    def _read_exact(self, length: int) -> bytes:
        data = self.serial.read(length)
        if len(data) != length:
            raise TimeoutError(f"motion MCU timeout ({len(data)}/{length} bytes)")
        return data

    def request(self, command: int, payload: bytes = b"") -> bytes:
        with self.lock:
            sequence = self.sequence
            self.sequence = (self.sequence + 1) & 0xFF
            body = struct.pack("<BBBH", VERSION, sequence, command, len(payload)) + payload
            self.serial.write(SYNC + body + struct.pack("<H", crc16_ccitt(body)))
            self.serial.flush()

            # Re-synchronise without accepting unbounded input.
            window = bytearray()
            for _ in range(64):
                window += self._read_exact(1)
                if window[-2:] == SYNC:
                    break
            else:
                raise RuntimeError("motion MCU response has no sync word")

            header = self._read_exact(5)
            version, response_sequence, response_command, length = struct.unpack("<BBBH", header)
            if length > 32:
                raise RuntimeError("motion MCU response exceeds the protocol size limit")
            response_payload = self._read_exact(length)
            received_crc, = struct.unpack("<H", self._read_exact(2))
            if version != VERSION or response_sequence != sequence or response_command != (command | 0x80):
                raise RuntimeError("motion MCU response header mismatch")
            if received_crc != crc16_ccitt(header + response_payload):
                raise RuntimeError("motion MCU response CRC mismatch")
            if not response_payload:
                raise RuntimeError("motion MCU response has no status byte")
            status = response_payload[0]
            if status:
                raise RuntimeError(f"motion MCU rejected command: {STATUS_TEXT.get(status, status)}")
            return response_payload[1:]


def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument("--port", default="/dev/ttyS0", help="F1C100S UART0 device")
    parser.add_argument("--steps", type=int, default=3200,
                        help="signed microsteps; 3200 is one revolution at fixed 1/16 mode")
    parser.add_argument("--hz", type=int, default=200, help="microsteps per second")
    parser.add_argument("--timeout", type=float, default=0.2, help="UART response timeout")
    parser.add_argument("--dry-run", action="store_true")
    parser.add_argument("--current", action="store_true",
                        help="read signed winding telemetry without arming, disabling, or moving the motor")
    parser.add_argument("--limit", action="store_true", help="read current-limit state without motion")
    parser.add_argument("--set-current-ma", type=int,
                        help="set 0–396 nominal peak mA while disarmed; does NOT arm or move")
    parser.add_argument("--current-ma", type=int, default=396,
                        help="nominal peak mA for the requested move (default 396)")
    args = parser.parse_args()
    if sum([args.current, args.limit, args.set_current_ma is not None]) > 1:
        parser.error("choose only one of --current, --limit or --set-current-ma")
    for value in (args.current_ma, args.set_current_ma):
        if value is not None and not 0 <= value <= 396:
            parser.error("current limit must be between 0 and 396 nominal peak mA")
    if not -2_147_483_648 <= args.steps <= 2_147_483_647:
        parser.error("--steps is outside signed 32-bit range")
    if not 1 <= args.hz <= 20_000:
        parser.error("--hz must be between 1 and 20000")

    duration = abs(args.steps) / args.hz
    if args.dry_run:
        if args.current:
            print(f"query winding currents via {args.port}; no motion command")
            return
        if args.limit or args.set_current_ma is not None:
            print(f"current-limit {'read' if args.limit else args.set_current_ma} via {args.port}; no motion command")
            return
        print(f"{args.steps} microsteps at {args.hz} microsteps/s ({duration:.3f} s), via {args.port}")
        return

    try:
        import serial
    except ImportError as error:
        raise SystemExit("pyserial is required: python3 -m pip install pyserial") from error

    stop_heartbeat = threading.Event()
    heartbeat_error = []
    with serial.Serial(args.port, 115200, timeout=args.timeout, write_timeout=args.timeout) as port:
        link = MotionLink(port)
        if args.current:
            print(json.dumps(decode_current(link.request(CMD_CURRENT)), indent=2))
            return
        if args.limit:
            print(json.dumps(decode_limit(link.request(CMD_LIMIT)), indent=2))
            return
        if args.set_current_ma is not None:
            link.request(CMD_SET_LIMIT, struct.pack("<H", args.set_current_ma))
            print(json.dumps(wait_limit(link, args.set_current_ma), indent=2))
            return

        def heartbeat():
            while not stop_heartbeat.wait(0.08):
                try:
                    link.request(CMD_HEARTBEAT)
                except Exception as error:  # surfaced by the main loop
                    heartbeat_error.append(error)
                    stop_heartbeat.set()

        thread = threading.Thread(target=heartbeat, name="motion-heartbeat", daemon=True)
        try:
            link.request(CMD_DISABLE)
            link.request(CMD_STATUS)
            link.request(CMD_ARM, b"\x00")
            link.request(CMD_SET_LIMIT, struct.pack("<H", args.current_ma))
            wait_limit(link, args.current_ma)
            link.request(CMD_HEARTBEAT)
            link.request(CMD_ARM, b"\x01")
            time.sleep(0.005)  # DRV8825 wake delay; MCU also enforces >= 3 ticks.
            thread.start()
            link.request(CMD_MOVE, struct.pack("<iI", args.steps, args.hz))
            deadline = time.monotonic() + duration + 0.5
            while time.monotonic() < deadline and not stop_heartbeat.wait(0.05):
                pass
            if heartbeat_error:
                raise heartbeat_error[0]
            link.request(CMD_STATUS)
        except KeyboardInterrupt:
            link.request(CMD_STOP)
            raise
        finally:
            stop_heartbeat.set()
            if thread.is_alive():
                thread.join(timeout=0.2)
            try:
                link.request(CMD_DISABLE)
            except Exception:
                pass


if __name__ == "__main__":
    main()