imrishabh18/rp2040-motor-controller

firmware/main.py

"""Reference USB temperature monitor. Copy all firmware/*.py onto MicroPython.

Commands: arm, stop, status. This monitor never generates motion commands.
Integrate the existing motor/PWM code only while guard.policy.enabled is true.
The main loop must call guard.poll() and feed the watchdog at least every 100 ms.
"""
from machine import Pin, WDT
from time import ticks_ms, ticks_diff, sleep_ms
import sys
import select
from thermal_guard import ThermalGuard

Pin(22, Pin.OUT, value=0)
watchdog = WDT(timeout=1000)
guard = None
try:
    guard = ThermalGuard()
    console = select.poll()
    console.register(sys.stdin, select.POLLIN)
    command = ""
    last_sample = ticks_ms()
    last_report = last_sample
    while True:
        now = ticks_ms()
        if ticks_diff(now, last_sample) >= 100:
            guard.poll()
            last_sample = now
        # Bound the input workload; an incomplete line cannot block monitoring.
        for _ in range(16):
            if not console.poll(0):
                break
            char = sys.stdin.read(1)
            if char in ("\n", "\r"):
                if command == "arm":
                    print("ARMED" if guard.arm() else "ARM DENIED")
                elif command == "stop":
                    guard.stop()
                elif command == "status":
                    print(guard.policy.temperature_c, guard.policy.status())
                command = ""
            elif len(command) < 32:
                command += char
        if ticks_diff(now, last_report) >= 1000:
            print("temperature_C=", guard.policy.temperature_c, guard.policy.status())
            last_report = now
        # Feed only in this loop, never in an interrupt or unrelated timer.
        watchdog.feed()
        sleep_ms(10)
finally:
    Pin(22, Pin.OUT, value=0)
    if guard is not None:
        guard.stop("monitor exited")