MustafaMulla29/rp2040-bldc-motor-controller-new

firmware/select_pd_voltage.py

"""Guarded runtime selection. No NVM writes and never enables motor operation.

Hardware adapter is power_manager.py. SW4 must be OFF and the motor bus below
1 V before renegotiation. This prevents changing voltage on an energized motor
or on the charged 470 uF motor bus. A new ARM action is required afterwards.
"""
from pd_profile import configure_runtime, verify_runtime, profiles
from power_policy import DEFAULT_MV, valid_motor_contract

class VoltageSelector:
    def __init__(self, bus, disarm, sample, clock, address=0x28, service=lambda: None):
        self.bus, self.disarm, self.sample, self.clock = bus, disarm, sample, clock
        self.address, self.service = address, service
        self.selected_mv = DEFAULT_MV
        self.ready = False

    def select(self, millivolts, timeout_ms=5000):
        # Disarm even for an invalid command or failed prerequisite.
        self.disarm()
        self.ready = False
        profiles(millivolts)  # Reject unsupported settings before any I2C write.
        s = self.sample()
        if not 0 <= s['vmotor_mv'] < 1000:
            raise OSError("Set SW4 OFF and wait for VMOTOR below 1 V before selection")
        configure_runtime(self.bus, self.address, millivolts)
        start = self.clock.ticks_ms()
        good_since = None
        while self.clock.ticks_diff(self.clock.ticks_ms(), start) < timeout_ms:
            self.disarm()
            self.service()
            verify_runtime(self.bus, self.address, millivolts)
            s = self.sample()
            if not 0 <= s['vmotor_mv'] < 1000:
                raise OSError("Keep SW4 OFF until voltage selection completes")
            rdo = int.from_bytes(self.bus.readfrom_mem(self.address, 0x91, 4), 'little')
            pe = self.bus.readfrom_mem(self.address, 0x29, 1)[0]
            good = valid_motor_contract(s['vbus_mv'], rdo, pe, s['pd_ok2_low'], millivolts)
            now = self.clock.ticks_ms()
            if good:
                if good_since is None: good_since = now
                if self.clock.ticks_diff(now, good_since) >= 250:
                    self.selected_mv = millivolts
                    self.ready = True
                    return millivolts
            else:
                good_since = None
            self.clock.sleep_ms(25)
        raise OSError("Selected PD contract unavailable or unstable; motor remains OFF")