M0601 Hub Moto Interfacing RS485

"""
m0601_scan.py — Discover all M0601 motors on the RS485 bus.

Stage 1: Broadcast ID query (fast, ~0.3s)
Stage 2: Full poll of all IDs 0x01-0xFE (thorough, ~40s at 0.15s timeout)

Usage:
    pip install pyserial
    python m0601_scan.py
"""

import serial
import time
import sys

COM_PORT  = "COM13"       # ← Change to your port (e.g. /dev/ttyUSB0 on Linux)
BAUD_RATE = 115200
TIMEOUT   = 0.15          # Seconds to wait per ID. Increase to 0.25 on noisy buses.


def crc8_maxim(data):
    """CRC-8/MAXIM: polynomial x^8 + x^5 + x^4 + 1, reflected 0x8C."""
    crc = 0
    for byte in data:
        crc ^= byte
        for _ in range(8):
            crc = (crc >> 1) ^ 0x8C if (crc & 0x01) else (crc >> 1)
    return crc


# --- Verified frames ---
FRAME_ID_QUERY = bytes([0xC8, 0x64, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xDE])

def frame_feedback_request(motor_id):
    """Build a feedback query frame for a specific motor ID."""
    f = [motor_id, 0x74, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]
    f.append(crc8_maxim(f))
    return bytes(f)


def stage1_broadcast(ser):
    """
    Send the broadcast ID query and parse the response.
    The motor replies with its ID in DATA[0] of the response frame.
    Returns detected ID (int) or None.
    """
    print("\n[Stage 1] Broadcast ID query...")
    ser.reset_input_buffer()
    ser.write(FRAME_ID_QUERY)
    time.sleep(0.3)

    resp = ser.read_all()
    if not resp:
        print("  No response.")
        return None

    print(f"  Raw response: {resp.hex(' ').upper()}")

    # If response is identical to our query, it's just the RS485 echo — not a motor reply.
    if bytes(resp) == FRAME_ID_QUERY:
        print("  (Echo only — motor did not respond separately)")
        return None

    # Motor reply starts with its own ID byte (0x01–0xFE)
    for byte in resp:
        if 0x01 <= byte <= 0xFE:
            print(f"  ✓ Motor found via broadcast — ID: 0x{byte:02X} ({byte})")
            return byte

    return None


def stage2_full_poll(ser):
    """
    Poll each ID from 0x01 to 0xFE individually.
    Returns a list of all responding IDs.
    """
    print("\n[Stage 2] Full ID poll (0x01 → 0xFE)...")
    print(f"  Estimated time: {254 * TIMEOUT:.0f}s — please wait.\n")
    found = []

    for motor_id in range(0x01, 0xFF):
        # Print progress bar
        pct   = motor_id / 254
        bar   = "█" * int(30 * pct) + "░" * (30 - int(30 * pct))
        print(f"\r  [{bar}] 0x{motor_id:02X} ({motor_id}/254)", end="", flush=True)

        ser.reset_input_buffer()
        ser.write(frame_feedback_request(motor_id))
        time.sleep(TIMEOUT)

        resp = ser.read_all()
        # A valid reply is 10 bytes starting with the motor's own ID
        if resp and len(resp) >= 3 and resp[0] == motor_id:
            found.append(motor_id)
            print(f"\n  ✓ Motor at ID 0x{motor_id:02X} ({motor_id}) replied!")

    print(f"\r  [{'█' * 30}] Done!                                  ")
    return found


def main():
    print("=" * 52)
    print("  M0601 Motor Scanner")
    print(f"  Port: {COM_PORT}  |  Baud: {BAUD_RATE}")
    print("=" * 52)
    print("\n  ⚠  For Stage 1, ensure only ONE motor is on the bus.")
    print("  Press Enter to start scan, Ctrl+C to cancel...")
    input()

    try:
        ser = serial.Serial(
            port=COM_PORT, baudrate=BAUD_RATE,
            bytesize=8, parity='N', stopbits=1,
            timeout=TIMEOUT
        )
        print(f"[✓] Opened {COM_PORT}")
    except serial.SerialException as e:
        print(f"[✗] Cannot open port: {e}")
        sys.exit(1)

    try:
        broadcast_id = stage1_broadcast(ser)
        polled_ids   = stage2_full_poll(ser)

        # Combine results
        all_ids = sorted(set(polled_ids) | ({broadcast_id} if broadcast_id else set()))

        print("\n" + "=" * 52)
        print("  SCAN COMPLETE")
        print("=" * 52)
        if all_ids:
            print(f"\n  {len(all_ids)} motor(s) found:")
            for mid in all_ids:
                print(f"    • ID 0x{mid:02X}  (decimal {mid})")
            if len(all_ids) == 1:
                print(f"\n  → In other scripts, set:  MOTOR_ID = 0x{all_ids[0]:02X}")
        else:
            print("\n  ✗ No motors detected.")
            print("  Checklist:")
            print("    1. Is the 18V power adapter ON?")
            print("    2. Is the Brown wire connected to GND?")
            print("    3. Try swapping Orange ↔ White (A/B polarity)")
            print(f"    4. Try increasing TIMEOUT (currently {TIMEOUT}s)")
        print("=" * 52)

    except KeyboardInterrupt:
        print("\n[!] Scan cancelled.")
    finally:
        ser.close()
        print("[✓] Port closed.")


if __name__ == "__main__":
    main()

"""
m0601_operations.py — Interactive motor control in all three modes.

Controls:
  F / B  — Forward / Backward at SPIN_RPM (velocity mode)
  1..5   — Velocity presets: 50, 100, 150, 200, 250 RPM
  S      — Stop (velocity = 0)
  K      — Brake (electric brake)
  V      — Switch to Velocity Loop mode
  C      — Switch to Current Loop mode
  P      — Switch to Position Loop mode
  I      — Query feedback (print once)
  Q      — Quit

Usage:
    pip install pyserial
    python m0601_operations.py
"""

import serial
import time
import sys
import os
import threading

COM_PORT   = "COM13"
BAUD_RATE  = 115200
MOTOR_ID   = 0x01
SPIN_RPM   = 100        # Default speed for F/B keys
POLL_HZ    = 50         # Polling frequency in Hz


def crc8_maxim(data):
    crc = 0
    for byte in data:
        crc ^= byte
        for _ in range(8):
            crc = (crc >> 1) ^ 0x8C if (crc & 0x01) else (crc >> 1)
    return crc


def build(motor_id, b1, data7):
    """Build a standard 10-byte frame: [ID, B1, 7 data bytes, CRC]."""
    f = [motor_id, b1] + list(data7)
    f.append(crc8_maxim(f))
    return bytes(f)


# ── Verified frames ───────────────────────────────────────────────────────────

def frame_velocity_mode(motor_id):
    """Switch to velocity loop (mode 0x02). No CRC on last byte — use exact bytes."""
    return bytes([motor_id, 0xA0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02])

def frame_current_mode(motor_id):
    """Switch to current loop (mode 0x01)."""
    return bytes([motor_id, 0xA0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01])

def frame_position_mode(motor_id):
    """Switch to position loop (mode 0x03). Motor must be <10 RPM first."""
    return bytes([motor_id, 0xA0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03])

def frame_velocity(motor_id, rpm, accel=1):
    """Velocity command. rpm: -330 to +330."""
    rpm  = max(-330, min(330, rpm))
    v    = rpm.to_bytes(2, 'big', signed=True)
    return build(motor_id, 0x64, [v[0], v[1], accel, 0x00, 0x00, 0x00, 0x00])

def frame_current(motor_id, value):
    """Current command. value: -32767 to +32767 (= -8A to +8A)."""
    value = max(-32767, min(32767, value))
    v     = value.to_bytes(2, 'big', signed=True)
    return build(motor_id, 0x64, [v[0], v[1], 0x00, 0x00, 0x00, 0x00, 0x00])

def frame_position(motor_id, pos):
    """Position command. pos: 0 to 32767 (= 0° to 360°)."""
    pos = max(0, min(32767, pos))
    v   = pos.to_bytes(2, 'big', signed=False)
    return build(motor_id, 0x64, [v[0], v[1], 0x00, 0x00, 0x00, 0x00, 0x00])

def frame_brake(motor_id):
    """Electric brake. Valid in velocity mode only."""
    return build(motor_id, 0x64, [0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0x00])

def frame_feedback(motor_id):
    """Request full feedback (speed, current, position, temp, error)."""
    return build(motor_id, 0x74, [0x00] * 7)

def frame_id_query():
    return bytes([0xC8, 0x64, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xDE])


def decode_feedback(data):
    """Parse a 10-byte Protocol-2 feedback frame into a readable dict."""
    if len(data) < 10:
        return None
    mode_map = {0x01: "Current", 0x02: "Velocity", 0x03: "Position"}
    raw_cur  = int.from_bytes(data[2:4], 'big', signed=True)
    raw_vel  = int.from_bytes(data[4:6], 'big', signed=True)
    temp_c   = data[6]
    u8_pos   = data[7]
    err      = data[8]
    return {
        "id":       data[0],
        "mode":     mode_map.get(data[1], f"0x{data[1]:02X}"),
        "current_a": round(raw_cur * 8.0 / 32767, 3),
        "speed_rpm": raw_vel,
        "temp_c":    temp_c,
        "position_deg": round(u8_pos * 360.0 / 255, 1),
        "error":    f"0x{err:02X}" + (" (OK)" if err == 0 else " ⚠ FAULT"),
    }


def getch():
    """Read a single keypress without pressing Enter (cross-platform)."""
    if os.name == 'nt':
        import msvcrt
        return msvcrt.getch().decode('utf-8', errors='ignore').upper()
    else:
        import tty, termios
        fd  = sys.stdin.fileno()
        old = termios.tcgetattr(fd)
        try:
            tty.setraw(fd)
            return sys.stdin.read(1).upper()
        finally:
            termios.tcsetattr(fd, termios.TCSADRAIN, old)


def main():
    print("=" * 54)
    print("  M0601 Motor Operations — Interactive Control")
    print(f"  Port: {COM_PORT}  ID: 0x{MOTOR_ID:02X}  Default: {SPIN_RPM} RPM")
    print("=" * 54)

    try:
        ser = serial.Serial(
            port=COM_PORT, baudrate=BAUD_RATE,
            bytesize=8, parity='N', stopbits=1, timeout=0.3
        )
        print(f"[✓] Opened {COM_PORT}")
    except serial.SerialException as e:
        print(f"[✗] {e}")
        sys.exit(1)

    # Confirm motor is online
    print("[*] Checking for motor...")
    ser.reset_input_buffer()
    ser.write(frame_id_query())
    time.sleep(0.3)
    resp = ser.read_all()
    if resp:
        print(f"[✓] Motor responded: {resp.hex(' ').upper()}")
    else:
        print("[!] No motor response — check wiring and 18V power.")

    # Switch to velocity mode and initialise polling state
    for _ in range(5):
        ser.write(frame_velocity_mode(MOTOR_ID))
        time.sleep(0.02)
    print("[✓] Velocity loop mode set.\n")

    # Shared state between main thread (keyboard) and poll thread (serial writes)
    state = {
        "frame":   frame_velocity(MOTOR_ID, 0),   # Current frame to poll
        "polling": True,                            # Polling active flag
        "running": True,                            # Application running flag
        "mode":    "velocity",
    }
    lock = threading.Lock()

    def poll_loop():
        """Background thread: sends state['frame'] at POLL_HZ."""
        interval = 1.0 / POLL_HZ
        while state["running"]:
            with lock:
                if state["polling"]:
                    try:
                        ser.write(state["frame"])
                    except Exception:
                        pass
            time.sleep(interval)

    poll_thread = threading.Thread(target=poll_loop, daemon=True)
    poll_thread.start()

    print("Controls:")
    print("  F/B     Forward / Backward")
    print("  1-5     Speed presets (50/100/150/200/250 RPM)")
    print("  S       Stop   K  Brake")
    print("  V/C/P   Switch mode: Velocity / Current / Position")
    print("  I       Print live feedback")
    print("  Q       Quit\n")

    try:
        while True:
            print("Key: ", end="", flush=True)
            key = getch()
            print(key)

            with lock:
                if key == 'F':
                    state["frame"]   = frame_velocity(MOTOR_ID, SPIN_RPM)
                    state["polling"] = True
                    print(f"  ► Forward {SPIN_RPM} RPM")

                elif key == 'B':
                    state["frame"]   = frame_velocity(MOTOR_ID, -SPIN_RPM)
                    state["polling"] = True
                    print(f"  ◄ Backward {SPIN_RPM} RPM")

                elif key in ('1', '2', '3', '4', '5'):
                    rpm = int(key) * 50
                    state["frame"]   = frame_velocity(MOTOR_ID, rpm)
                    state["polling"] = True
                    print(f"  ► {rpm} RPM")

                elif key == 'S':
                    state["frame"]   = frame_velocity(MOTOR_ID, 0)
                    state["polling"] = True
                    print("  ■ Stopped (velocity = 0)")

                elif key == 'K':
                    state["frame"]   = frame_brake(MOTOR_ID)
                    state["polling"] = True
                    print("  ■ Brake applied")

                elif key == 'V':
                    state["mode"] = "velocity"
                    state["polling"] = False
                for _ in range(5):
                    ser.write(frame_velocity_mode(MOTOR_ID))
                    time.sleep(0.02)
                print("  ✓ Velocity Loop mode")

                elif key == 'C':
                    state["mode"] = "current"
                    state["polling"] = False
                    for _ in range(5):
                        ser.write(frame_current_mode(MOTOR_ID))
                        time.sleep(0.02)
                    state["frame"]   = frame_current(MOTOR_ID, 0)
                    state["polling"] = True
                    print("  ✓ Current Loop mode — sending 0A")

                elif key == 'P':
                    state["mode"] = "position"
                    state["polling"] = False
                    for _ in range(5):
                        ser.write(frame_position_mode(MOTOR_ID))
                        time.sleep(0.02)
                    pos = int(input("  Enter position (0–32767): "))
                    state["frame"]   = frame_position(MOTOR_ID, pos)
                    state["polling"] = True
                    print(f"  ✓ Position Loop — targeting {pos} ({pos*360/32767:.1f}°)")

                elif key == 'I':
                    state["polling"] = False
                    time.sleep(0.05)
                    ser.reset_input_buffer()
                    ser.write(frame_feedback(MOTOR_ID))
                    time.sleep(0.2)
                    resp = ser.read_all()
                    if resp and len(resp) >= 10:
                        fb = decode_feedback(resp[:10])
                        if fb:
                            print(f"  Mode:     {fb['mode']}")
                            print(f"  Speed:    {fb['speed_rpm']} RPM")
                            print(f"  Current:  {fb['current_a']} A")
                            print(f"  Position: {fb['position_deg']}°")
                            print(f"  Temp:     {fb['temp_c']} °C")
                            print(f"  Error:    {fb['error']}")
                    else:
                        print("  ✗ No feedback received")
                    state["polling"] = True

                elif key == 'Q':
                    state["frame"]   = frame_velocity(MOTOR_ID, 0)
                    time.sleep(0.1)
                    for _ in range(5):
                        ser.write(frame_brake(MOTOR_ID))
                        time.sleep(0.02)
                    state["running"] = False
                    break

    except KeyboardInterrupt:
        print("\n[!] Interrupted")
    finally:
        state["running"] = False
        time.sleep(0.1)
        ser.close()
        print("[✓] Port closed. Goodbye!")


if __name__ == "__main__":
    main()

"""
m0601_set_id.py — Change the RS485 ID of one M0601 motor.

⚠  Connect only ONE motor to the bus before running.
⚠  The motor saves the ID when powered off (non-volatile).

Usage:
    pip install pyserial
    python m0601_set_id.py
"""

import serial
import time
import sys

COM_PORT     = "COM13"       # ← Change to your port
BAUD_RATE    = 115200
NEW_MOTOR_ID = 0x01          # ← Change to the ID you want to assign (0x01–0xFE)


def crc8_maxim(data):
    crc = 0
    for byte in data:
        crc ^= byte
        for _ in range(8):
            crc = (crc >> 1) ^ 0x8C if (crc & 0x01) else (crc >> 1)
    return crc


# Verified broadcast ID query frame
FRAME_ID_QUERY = bytes([0xC8, 0x64, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xDE])


def build_id_set_frame(new_id):
    """
    ID set frame: AA 55 53 [ID] 00 00 00 00 00 00
    Note: This frame has NO CRC — the last byte is always 0x00.
    Must be sent exactly 5 times in a row.
    """
    assert 0x01 <= new_id <= 0xFE, "ID must be 0x01 to 0xFE"
    return bytes([0xAA, 0x55, 0x53, new_id, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00])


def query_current_id(ser):
    """Send broadcast query and return the detected motor ID, or None."""
    ser.reset_input_buffer()
    ser.write(FRAME_ID_QUERY)
    time.sleep(0.3)
    resp = ser.read_all()
    if not resp or bytes(resp) == FRAME_ID_QUERY:
        return None
    # Motor ID is the first non-echo byte in the 0x01–0xFE range
    for byte in resp:
        if 0x01 <= byte <= 0xFE:
            return byte
    return None


def main():
    print("=" * 52)
    print("  M0601 Motor ID Changer")
    print(f"  Port: {COM_PORT}  →  New ID: 0x{NEW_MOTOR_ID:02X} ({NEW_MOTOR_ID})")
    print("=" * 52)
    print()
    print("  ⚠  ONLY ONE MOTOR MUST BE CONNECTED TO THE BUS.")
    print("  ⚠  The new ID is saved permanently (survives power-off).")
    print()

    try:
        ser = serial.Serial(
            port=COM_PORT, baudrate=BAUD_RATE,
            bytesize=8, parity='N', stopbits=1, timeout=0.3
        )
        print(f"[✓] Opened {COM_PORT}")
    except serial.SerialException as e:
        print(f"[✗] Cannot open port: {e}")
        sys.exit(1)

    # Step 1: Confirm one motor is present and read current ID
    print("\n[Step 1] Scanning for motor...")
    current_id = query_current_id(ser)
    if current_id is None:
        print("[✗] No motor detected.")
        print("    Check: 18V power ON? Wiring correct? Brown wire to GND?")
        ser.close()
        sys.exit(1)

    print(f"[✓] Motor detected — current ID: 0x{current_id:02X} ({current_id})")

    if current_id == NEW_MOTOR_ID:
        print(f"[!] Motor already has ID 0x{NEW_MOTOR_ID:02X}. Nothing to do.")
        ser.close()
        sys.exit(0)

    # Step 2: Confirm with user
    print(f"\n[Step 2] Ready to change ID: 0x{current_id:02X} → 0x{NEW_MOTOR_ID:02X}")
    confirm = input("  Type 'yes' to confirm: ").strip().lower()
    if confirm != 'yes':
        print("[!] Cancelled.")
        ser.close()
        sys.exit(0)

    # Step 3: Send ID set frame exactly 5 times
    print("\n[Step 3] Sending ID set frame ×5...")
    id_frame = build_id_set_frame(NEW_MOTOR_ID)
    print(f"  Frame: {id_frame.hex(' ').upper()}")

    for i in range(1, 6):
        ser.write(id_frame)
        print(f"  Sent {i}/5")
        time.sleep(0.05)   # Small gap between frames

    print("\n[Step 4] Waiting for motor to save ID (power-cycle not needed)...")
    time.sleep(0.5)

    # Step 4: Verify new ID
    print("[Step 4] Verifying new ID...")
    verified_id = query_current_id(ser)

    if verified_id == NEW_MOTOR_ID:
        print(f"\n[✓] SUCCESS — Motor ID is now 0x{NEW_MOTOR_ID:02X} ({NEW_MOTOR_ID})")
        print(f"    Update MOTOR_ID = 0x{NEW_MOTOR_ID:02X} in your other scripts.")
    elif verified_id is not None:
        print(f"\n[✗] Motor responded with ID 0x{verified_id:02X} — ID change may have failed.")
        print("    Try power-cycling the motor and running this script again.")
    else:
        print("\n[?] No response after ID change.")
        print("    Try power-cycling the motor — the new ID may have been saved.")
        print(f"    Run m0601_scan.py after power-cycling to confirm.")

    ser.close()
    print("[✓] Port closed.")


if __name__ == "__main__":
    main()

"""
m0601_monitor.py — Live parameter monitoring with CSV logging.

Polls the motor feedback at configurable interval and displays:
  Mode | Speed (RPM) | Current (A) | Position (°) | Temp (°C) | Errors

Also writes all readings to motor_log.csv for offline analysis.

Usage:
    pip install pyserial
    python m0601_monitor.py
    Press Ctrl+C to stop.
"""

import serial
import time
import sys
import csv
import os
from datetime import datetime

COM_PORT      = "COM13"     # ← Change to your port
BAUD_RATE     = 115200
MOTOR_ID      = 0x01
POLL_INTERVAL = 0.2         # Seconds between feedback queries (5Hz default)
LOG_FILE      = "motor_log.csv"


def crc8_maxim(data):
    crc = 0
    for byte in data:
        crc ^= byte
        for _ in range(8):
            crc = (crc >> 1) ^ 0x8C if (crc & 0x01) else (crc >> 1)
    return crc


def frame_feedback_query(motor_id):
    """Protocol 2 feedback request: returns speed, current, temp, position, error."""
    f = [motor_id, 0x74, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]
    f.append(crc8_maxim(f))
    return bytes(f)


def decode_error(code):
    """Decode error bitmask into a human-readable string."""
    if code == 0:
        return "OK"
    errors = []
    if code & 0x01: errors.append("SensorErr")
    if code & 0x02: errors.append("Overcurrent")
    if code & 0x04: errors.append("PhaseOvercurrent")
    if code & 0x08: errors.append("Stall")
    if code & 0x10: errors.append("Troubleshoot")
    return " | ".join(errors)


def parse_feedback(data):
    """
    Parse a 10-byte Protocol-2 feedback frame.
    Returns a dict with all motor parameters, or None on invalid data.
    """
    if len(data) < 10:
        return None

    motor_id = data[0]
    mode_raw = data[1]
    mode_map = {0x01: "Current ", 0x02: "Velocity", 0x03: "Position"}
    mode_str = mode_map.get(mode_raw, f"Unk(0x{mode_raw:02X})")

    # Torque current: signed INT16, big-endian, bytes 2-3
    raw_current = int.from_bytes(data[2:4], byteorder='big', signed=True)
    current_a   = round(raw_current * 8.0 / 32767.0, 3)

    # Velocity: signed INT16, big-endian, bytes 4-5
    speed_rpm = int.from_bytes(data[4:6], byteorder='big', signed=True)

    # Winding temperature: uint8, byte 6
    temp_c = data[6]

    # U8 position: uint8, byte 7 — 0-255 maps to 0°-360°
    u8_pos       = data[7]
    position_deg = round(u8_pos * 360.0 / 255.0, 1)

    # Error code bitmask: byte 8
    error_raw = data[8]
    error_str = decode_error(error_raw)

    return {
        "timestamp":    datetime.now().strftime("%H:%M:%S.%f")[:-3],
        "motor_id":     motor_id,
        "mode":         mode_str,
        "speed_rpm":    speed_rpm,
        "current_a":    current_a,
        "temp_c":       temp_c,
        "position_deg": position_deg,
        "error_code":   f"0x{error_raw:02X}",
        "error_str":    error_str,
        "raw_hex":      data[:10].hex(' ').upper(),
    }


def print_dashboard(fb, count):
    """Print a compact, self-overwriting dashboard line."""
    fault_indicator = "🔴 FAULT" if fb['error_str'] != "OK" else "🟢 OK   "
    line = (
        f"\r[{fb['timestamp']}] #{count:5d} | "
        f"Mode: {fb['mode']} | "
        f"Speed: {fb['speed_rpm']:+5d} RPM | "
        f"Current: {fb['current_a']:+6.3f} A | "
        f"Pos: {fb['position_deg']:6.1f}° | "
        f"Temp: {fb['temp_c']:3d}°C | "
        f"{fault_indicator}"
        + (f" ({fb['error_str']})" if fb['error_str'] != "OK" else "")
    )
    print(line, end="", flush=True)


def main():
    print("=" * 70)
    print("  M0601 Motor Parameter Monitor")
    print(f"  Port: {COM_PORT}  |  Motor ID: 0x{MOTOR_ID:02X}  |  Poll: {POLL_INTERVAL*1000:.0f}ms")
    print(f"  Logging to: {os.path.abspath(LOG_FILE)}")
    print("=" * 70)
    print("  Press Ctrl+C to stop.\n")

    try:
        ser = serial.Serial(
            port=COM_PORT, baudrate=BAUD_RATE,
            bytesize=8, parity='N', stopbits=1, timeout=0.3
        )
        print(f"[✓] Opened {COM_PORT}")
    except serial.SerialException as e:
        print(f"[✗] Cannot open port: {e}")
        sys.exit(1)

    query_frame = frame_feedback_query(MOTOR_ID)
    count        = 0
    no_resp_count = 0

    # Open CSV log file
    csv_fields = [
        "timestamp", "motor_id", "mode", "speed_rpm",
        "current_a", "temp_c", "position_deg",
        "error_code", "error_str", "raw_hex"
    ]
    log_file = open(LOG_FILE, 'w', newline='')
    writer   = csv.DictWriter(log_file, fieldnames=csv_fields)
    writer.writeheader()

    print(f"\n{'Timestamp':14s} {'#':>6s}  Mode      Speed    Current  Position  Temp  Status")
    print("-" * 80)

    try:
        while True:
            ser.reset_input_buffer()
            ser.write(query_frame)
            time.sleep(POLL_INTERVAL)

            resp = ser.read_all()
            if not resp or len(resp) < 10:
                no_resp_count += 1
                if no_resp_count >= 5:
                    print(f"\r[!] No response for {no_resp_count} consecutive polls — check motor power.", end="")
                continue

            no_resp_count = 0
            count += 1
            fb = parse_feedback(resp[:10])
            if not fb:
                continue

            print_dashboard(fb, count)
            writer.writerow({k: fb[k] for k in csv_fields})
            log_file.flush()

    except KeyboardInterrupt:
        print("\n\n[✓] Monitoring stopped.")
    finally:
        log_file.close()
        ser.close()
        print(f"[✓] Log saved to {LOG_FILE}")
        print(f"[✓] {count} readings recorded.")
        print("[✓] Port closed.")


if __name__ == "__main__":
    main()