imrishabh18/rp2040-motor-controller

firmware/as5600.py

"""Read-only AS5600 single-turn angle feedback. Never burns OTP settings."""
ADDRESS = 0x36


class AS5600:
    def __init__(self, bus):
        self.bus = bus
        self.angle_degrees = None
        self.zero_raw = 0
        self.raw = None

    def sample(self):
        self.angle_degrees = None
        self.raw = None
        data = self.bus.readfrom_mem(ADDRESS, 0x0B, 3)
        if len(data) != 3:
            raise OSError("short encoder response")
        status = data[0]
        if not status & 0x20 or status & 0x18:
            raise ValueError("encoder magnet missing, too weak, or too strong")
        self.raw = ((data[1] & 0x0F) << 8) | data[2]
        self.angle_degrees = ((self.raw - self.zero_raw) & 0x0FFF) * 360 / 4096
        return self.angle_degrees

    def zero(self):
        self.sample()
        self.zero_raw = self.raw
        self.angle_degrees = 0.0