Initial commit

BM/as5600l.py

@@ -0,0 +1,151 @@
+import time
+import setenv
+
+from machine import I2C, Pin
+
+ADDRESS     = 0x40
+ZMCO        = 0x00
+ZPOS        = 0x01
+MPOS        = 0x03
+MANG        = 0x05
+CONF_REG    = 0x07
+STATUS_REG  = 0x0B
+RAWANGLE    = 0x0c
+ANGLE_REG   = 0x0E
+AGC_REG     = 0x1A
+MAGNITUDE   = 0x1b
+BURN        = 0xff
+
+
+class AS5600L:
+
+    def __init__(self,
+        i2c_id = setenv.ENCODER_NUMBER,
+        sda = setenv.ENCODER_SDA_PIN, 
+        scl = setenv.ENCODER_SCL_PIN, 
+        i2c_freq = setenv.ENCODER_I2C_FREQUENCE, 
+        hyst = setenv.ENCODER_HYST, 
+        power_mode = 0, 
+        watchdog = 0,
+        fast_filter_threshold = 0, 
+        slow_filter = 0, 
+        pwm_freq = 0, 
+        output_stage = 0):
+        """Init
+        Args:
+            i2c_id (int, optional): ID of the I2C bus. Defaults to 0.
+            sda (int, optional) : gpio of the I2C bus. Defaults to 0.
+            scl (int, optional) : gpio of clock for I2C bus. Defaults to 1.
+            i2c_freq (int, optional): I2C Ferequency.  Default is max. Defaults to 1000000.
+            hyst (int, optional): Hysteresis: 0->3. Defaults to 0.
+            power_mode (int, optional): Power Mode: 0->3. Defaults to 0.
+            watchdog (int, optional): Enable the watchdog. Defaults to 0.
+            fast_filter_threshold (int, optional): Threshold value for the fast filter: 0->7. Defaults to 0.
+            slow_filter (int, optional): Slow filter mode: 0->3. Defaults to 0.
+            pwm_freq (int, optional): Output PWM frequency: 0->3. Defaults to 0.
+            output_stage (int, optional): Output mode (see datasheet): 0->2. Defaults to 0.
+        """
+        self.i2c = I2C(i2c_id, scl=Pin(scl), sda=Pin(sda), freq=i2c_freq)
+        print(self.i2c.scan())
+        time.sleep_ms(100)
+        c1 = 0x00
+        c2 = 0x00
+        # # set watchdog 0,1
+        # if watchdog in range(0,2):
+        #     c1 = c1 | (watchdog << 5)
+        # # set fast filter threshold 0->7
+        # if fast_filter_threshold in range(0,8):
+        #     c1 = c1 | (fast_filter_threshold << 2)
+        # # set the slow filter 0->3
+        # if slow_filter in range(0,4):
+        #     c1 = c1 | slow_filter
+        # # set PWM frequency 0->3
+        # if pwm_freq in range(0,4):
+        #     c2 = c2 | (pwm_freq << 6)
+        # # set output stage 0->2
+        # if output_stage in range(0,2):
+        #     c2 = c2 | (output_stage << 4)
+        # # set power_mode 0->3
+        # if power_mode in range(0, 4):
+        #     c2 = c2 | power_mode
+        # # set Hysteresis 0->3
+        # if hyst in range(0, 4):
+        #     c2 = c2 | (hyst << 2)
+
+        self.i2c.writeto(ADDRESS, bytes([CONF_REG, c1, c2]))
+
+    def get_status(self):
+        """Get the status of the ASL560L
+
+        Returns:
+            Dict: Dictionary of the different statuses
+        """
+        self.i2c.writeto(ADDRESS, bytes([STATUS_REG]))
+        status = int.from_bytes(self.i2c.readfrom(ADDRESS, 1), 'big')
+        mh = bool(status & 0b00001000)
+        ml = bool(status & 0b00010000)
+        md = bool(status & 0b00100000)
+        self.i2c.writeto(ADDRESS, bytes([AGC_REG]))
+        agc = int.from_bytes(self.i2c.readfrom(ADDRESS, 1), 'big')
+
+        return {
+            "magnetDetected": md, 
+            "magnetTooWeak": ml, 
+            "magnetTooStrong": mh,
+            "agc": agc
+        }
+
+    def is_ok(self) -> bool:
+        """Reads the status values to determine if the magnet is able to be read.
+        Returns:
+            bool: True if the magnet can be detercted accurately
+        """
+        self.i2c.writeto(ADDRESS, bytes([STATUS_REG]))
+        status = int.from_bytes(self.i2c.readfrom(ADDRESS, 1), 'big')
+        mh = bool(status & 0b00001000)
+        ml = bool(status & 0b00010000)
+        md = bool(status & 0b00100000)
+
+        if md and not mh and not ml:
+            return True
+        else:
+            return False
+
+    def get_angle(self) -> int:
+        """Get the raw 12bit angle value
+        Returns:
+            int: 0->4095
+        """
+        self.i2c.writeto(ADDRESS, bytes([ANGLE_REG]))
+        return int.from_bytes(self.i2c.readfrom(ADDRESS, 2), 'big')
+        
+        
+    def get_angle_degrees(self):
+        """Get the angle in degrees, IF the magnet can be read
+
+        Returns:
+            float: Degrees (0->359) if magnet can be read otherwise None.
+        """
+        if self.is_ok():
+            return (float(self.get_angle()))
+        else:
+            # if the magnet cannot be read then the angle is unreliable, so return None.
+            return None
+
+    def get_angle_degrees_fast(self):
+        """Get the angle in degrees
+
+        Returns:
+            float: Degrees (0->359) if magnet can be read otherwise None.
+        """
+        self.i2c.writeto(ADDRESS, b'\x0c')
+        return (float(int.from_bytes(self.i2c.readfrom(ADDRESS, 2), 'big')))
+
+    def get_conf(self):
+        """Internal healper function to read the raw config value
+
+        Returns:
+            int: raw value
+        """
+        self.i2c.writeto(ADDRESS, bytes([CONF_REG]))
+        return int.from_bytes(self.i2c.readfrom(ADDRESS, 2), 'big')