shibosoftwaredev/f1c100s-linux-nema8-stepper-controller

firmware/motor-control.py

#!/usr/bin/env python3
"""Reference Linux client for the STM32G030 NEMA8 motion 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 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
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


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)
            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")
    args = parser.parse_args()
    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 <= 100_000:
        parser.error("--hz must be between 1 and 100000")

    duration = abs(args.steps) / args.hz
    if args.dry_run:
        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)

        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"\x01")
            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()