imrishabh18/rp2040-motor-controller

firmware/main.py

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

Commands: arm, stop, status, zero. Position is single-turn, with a volatile zero. 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, PWM, I2C
from time import ticks_ms, ticks_diff, sleep_ms
import sys
import select
from thermal_guard import ThermalGuard
from audible_alarm import AudibleAlarm
from as5600 import AS5600

Pin(22, Pin.OUT, value=0)
watchdog = WDT(timeout=1000)
guard = None
alarm = AudibleAlarm(PWM(Pin(16)))
encoder = AS5600(I2C(0, sda=Pin(0), scl=Pin(1), freq=400_000))
encoder_ok = False
try:
    guard = ThermalGuard()
    console = select.poll()
    console.register(sys.stdin, select.POLLIN)
    command = ""
    last_sample = ticks_ms()
    last_report = last_sample
    last_encoder = last_sample
    while True:
        now = ticks_ms()
        if ticks_diff(now, last_encoder) >= 10:
            try:
                encoder.sample()
                encoder_ok = True
            except (OSError, ValueError):
                encoder_ok = False
                guard.stop("encoder unavailable or invalid magnet")
            last_encoder = now
        if ticks_diff(now, last_sample) >= 100:
            guard.poll()
            alarm.update(guard.policy.temperature_c, bool(guard.alert.value()), now)
            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 encoder_ok and guard.arm() else "ARM DENIED")
                elif command == "stop":
                    guard.stop()
                elif command == "zero" and not guard.policy.enabled:
                    try:
                        encoder.zero()
                    except (OSError, ValueError):
                        encoder_ok = False
                        guard.stop("encoder zero failed")
                elif command == "status":
                    print(guard.policy.temperature_c, guard.policy.status(), "angle_deg=", encoder.angle_degrees)
                command = ""
            elif len(command) < 32:
                command += char
        if ticks_diff(now, last_report) >= 1000:
            print("temperature_C=", guard.policy.temperature_c, guard.policy.status(), "angle_deg=", encoder.angle_degrees)
            last_report = now
        # Feed only in this loop, never in an interrupt or unrelated timer.
        watchdog.feed()
        sleep_ms(10)
finally:
    alarm.close()
    Pin(22, Pin.OUT, value=0)
    if guard is not None:
        guard.stop("monitor exited")