PAA5100JE motion sensor code

devices/PAA5100JE.py

@@ -0,0 +1,236 @@
+import time, gc, math
+from array import array
+from machine import Pin, SPI
+from pyControl.hardware import *
+from device.PAA5100JE_firmware import *
+
+def to_signed_16(value):
+    """Convert a 16-bit integer to a signed 16-bit integer."""
+    if value & 0x8000:  # Check if the sign bit is set
+        value -= 0x10000  # Convert to negative
+    return value
+
+class PAA5100JE():
+    """Optical tracking sensor."""
+    def __init__(self, spi_port: str, 
+                 spi_cs_gpio: str,
+                 reset: str = None, 
+                 sck: str = None, 
+                 mosi: str =None, 
+                 miso: str =None):
+
+        # Initialize SPI
+        # SPI_type = 'SPI1' or 'SPI2' or 'softSPI'
+        SPIparams = {'baudrate': 400000, 'polarity': 0, 'phase': 0,
+                     'bits': 8, 'firstbit': machine.SPI.MSB}
+        if '1' in spi_port:
+            self.spi = SPI(1, **SPIparams)
+
+        elif '2' in spi_port:
+            self.spi = SPI(2, **SPIparams)
+
+        elif 'soft' in spi_port.lower():  # Works for newer versions of micropython
+            self.spi = SoftSPI(sck=Pin(sck, mode=machine.Pin.OUT, pull=machine.Pin.PULL_DOWN),
+                                       mosi=Pin(mosi, mode=machine.Pin.OUT, pull=machine.Pin.PULL_DOWN),
+                                       miso=Pin(miso, mode=machine.Pin.IN),
+                                       **SPIparams
+                                       )
+
+        # Handle Chip Select (CS) pin
+        self.select = Digital_output(pin=spi_cs_gpio, inverted=True)
+        self.reset = Digital_output(pin=reset, inverted=True)
+        self.select.off()  # Deselect the device by setting CS high
+        self.reset.off()
+        time.sleep_ms(50)
+
+        # Reset the sensor
+        self.firmware = PAA5100JE_firmware()
+        self._write(self.firmware.REG_POWER_UP_RESET, 0x5A)
+        time.sleep_ms(20)
+        for offset in range(5):
+            self._read(self.firmware.REG_DATA_READY + offset)
+
+        # Initiate registers
+        PROGMEM = self.firmware.init_registers()
+        self._bulk_write(PROGMEM[0:10])
+        if self._read(0x67) & 0b10000000:
+            self._write(0x48, 0x04)
+        else:
+            self._write(0x48, 0x02)
+        self._bulk_write(PROGMEM[10:154])
+        time.sleep_ms(10)
+        self._bulk_write(PROGMEM[154:186])
+        time.sleep_ms(10)
+        self._bulk_write(PROGMEM[186:])
+
+    def set_rotation(self, degrees:int =0):
+        """Set orientation of PAA5100 in increments of 90 degrees."""
+        if degrees == 0:
+            self.set_orientation(invert_x=True, invert_y=True, swap_xy=True)
+        elif degrees == 90:
+            self.set_orientation(invert_x=False, invert_y=True, swap_xy=False)
+        elif degrees == 180:
+            self.set_orientation(invert_x=False, invert_y=False, swap_xy=True)
+        elif degrees == 270:
+            self.set_orientation(invert_x=True, invert_y=False, swap_xy=False)
+        else:
+            raise TypeError("Degrees must be one of 0, 90, 180 or 270")
+
+    def set_orientation(self, invert_x:bool =True, invert_y:bool =True, swap_xy:bool =True):
+        """Set orientation of PAA5100 manually."""
+        value = 0
+        if swap_xy:
+            value |= 0b10000000
+        if invert_y:
+            value |= 0b01000000
+        if invert_x:
+            value |= 0b00100000
+        self._write(self.firmware.REG_ORIENTATION, value)
+
+    def _write(self, register: bytes, value: bytes):
+        """Write into register"""
+        self.select.on()
+        self.spi.write(bytearray([register | 0x80, value]))
+        self.select.off()
+
+    def _read(self, register: bytes, length: int =1):
+        """Read register"""
+        # Create a buffer to send (with one extra byte for the register)
+        send_buf = bytearray([register]) + bytearray(length)
+        # Create a result buffer of the same length as the send_buf
+        result = bytearray(len(send_buf))
+
+        self.select.on()
+        self.spi.write_readinto(send_buf, result)
+        self.select.off()
+
+        # Return the read result, skipping the first byte (which corresponds to the register)
+        return result[1:] if length > 1 else result[1]
+
+    def _bulk_write(self, data: bytearray):
+        """Write a list of commands into registers"""
+        for x in range(0, len(data), 2):
+            register, value = data[x : x + 2]
+            self._write(register, value)
+
+    def read_registers(self, reg: bytes, buf: bytearray, len: int):
+        """Read an array of data from the registers"""
+        self.select.on()
+        self.spi.write(bytearray([reg]))
+        # Read data from the register
+        buf[:] = self.spi.read(len)
+        self.select.off()   
+
+    def shut_down(self, deinitSPI:bool =True):
+        """Shutdown the sensor"""
+        self.select.off()
+        time.sleep_ms(1)
+        self.select.on()
+        time.sleep_ms(1)
+        self.reset.on()
+        time.sleep_ms(60)
+        self.write(self.firmware.REG_SHUTDOWN, 1)
+        time.sleep_ms(1)
+        self.select.off()
+        time.sleep_ms(1)
+        if deinitSPI:
+            self.SPI.deinit()
+
+class MotionDetector(Analog_input):
+    """
+    Using the Analog_input code to interface with 2 PAA5100JE sensors
+    reading `x` (SPI2) and `y` (softSPI) separately.
+    """
+    def __init__(self, reset: str, cs1: str, cs2: str,
+                 name='MotDet', threshold=1, calib_coef=1,
+                 sampling_rate=100, event='motion'):
+
+        # Create SPI objects
+        self.motSen_x = PAA5100JE('SPI2', cs1, reset)
+        self.motSen_y = PAA5100JE('SPI2', cs2, reset)
+
+        self._threshold = threshold
+        self.calib_coef = calib_coef
+
+        # Motion sensor variables
+        self.x_buffer = bytearray(12)
+        self.y_buffer = bytearray(12)
+        self.x_buffer_mv = memoryview(self.x_buffer)
+        self.y_buffer_mv = memoryview(self.y_buffer)
+
+        self.delta_x, self.delta_y = 0, 0    # accumulated position
+        self._delta_x, self._delta_y = 0, 0  # instantaneous position
+        self.x, self.y = 0, 0  # to be accessed from the task, unit=mm
+
+        # Parent
+        Analog_input.__init__(self, pin=None, name=name + '-X', sampling_rate=int(sampling_rate),
+                              threshold=threshold, rising_event=event, falling_event=None,
+                              data_type='l')
+        self.data_chx = self.data_channel
+        self.data_chy = Data_channel(name + '-Y', sampling_rate, data_type='l')
+        self.crossing_direction = True  # to conform to the Analog_input syntax
+        self.timestamp = fw.current_time
+        self.acquiring = False
+
+        gc.collect()
+        time.sleep_ms(2)
+
+    @property
+    def threshold(self):
+        """return the value in mms"""
+        return math.sqrt(int(self._threshold**2) * self.calib_coef)
+
+    def reset_delta(self):
+        """reset the accumulated position data"""
+        self.delta_x, self.delta_y = 0, 0
+
+    def read_sample(self):
+        """read motion once"""
+        # All units are in millimeters
+        # Read motion in x direction
+        self.motSen_x.read_registers(self.firmware.REG_MOTION_BURST, self.x_buffer_mv, 12)
+        self._delta_x = to_signed_16((self.x_buffer_mv[3] << 8) | self.x_buffer_mv[2])
+
+        # Read motion in y direction
+        self.motSen_y.read_registers(self.firmware.REG_MOTION_BURST, self.y_buffer_mv, 12)
+        self._delta_y = to_signed_16((self.y_buffer_mv[5] << 8) | self.y_buffer_mv[4])
+
+        # Record accumulated motion
+        self.delta_y += self._delta_y
+        self.delta_x += self._delta_x
+
+    def _timer_ISR(self, t):
+        """Read a sample to the buffer, update write index."""
+        self.read_sample()
+        self.data_chx.put(self._delta_x)
+        self.data_chy.put(self._delta_y)
+
+        if self.delta_x**2 + self.delta_y**2 >= self._threshold:
+            self.x = self.delta_x
+            self.y = self.delta_y
+            self.reset_delta()
+            self.timestamp = fw.current_time
+            interrupt_queue.put(self.ID) 
+
+    def _stop_acquisition(self):
+        """Stop sampling analog input values."""
+        self.timer.deinit()
+        self.data_chx.stop()
+        self.data_chy.stop()
+        self.motSen_x.shut_down(deinitSPI=False)      
+        self.motSen_y.shut_down()
+        self.acquiring = False
+        self.reset_delta()
+
+    def _start_acquisition(self):
+        """Start sampling analog input values"""
+        self.timer.init(freq=self.data_chx.sampling_rate)
+        self.timer.callback(self._timer_ISR)
+        self.acquiring = True
+
+    def record(self):
+        """Start streaming data to computer."""
+        self.data_chx.record()
+        self.data_chy.record()
+        if not self.acquiring:
+            self._start_acquisition()

devices/PAA5100JE_firmware.py

@@ -0,0 +1,138 @@
+class PAA5100JE_firmware():
+    def __init__(self):
+        """
+        PAA5100JE firmware includes registers and initiation protocol for
+        the optical tracking chip
+        """
+        self.name = "firmware"
+        # PAA5100 registers definitions
+        self.REG_ID = 0x00
+        self.REG_DATA_READY = 0x02
+        self.REG_MOTION_BURST = 0x16
+        self.REG_POWER_UP_RESET = 0x3A
+        self.REG_SHUTDOWN = 0x3B
+        self.REG_ORIENTATION = 0x5B
+        self.REG_RESOLUTION = 0x4E
+
+        self.REG_RAWDATA_GRAB = 0x58
+        self.REG_RAWDATA_GRAB_STATUS = 0x59
+
+    def init_registers(self):
+        return [
+            0x7F, 0x00,
+            0x55, 0x01,
+            0x50, 0x07,
+
+            0x7F, 0x0E,
+            0x43, 0x10,
+
+            0x7F, 0x00,
+            0x51, 0x7B,
+            0x50, 0x00,
+            0x55, 0x00,
+            0x7F, 0x0E,
+
+            0x7F, 0x00,
+            0x61, 0xAD,
+
+            0x7F, 0x03,
+            0x40, 0x00,
+
+            0x7F, 0x05,
+            0x41, 0xB3,
+            0x43, 0xF1,
+            0x45, 0x14,
+
+            0x5F, 0x34,
+            0x7B, 0x08,
+            0x5E, 0x34,
+            0x5B, 0x11,
+            0x6D, 0x11,
+            0x45, 0x17,
+            0x70, 0xE5,
+            0x71, 0xE5,
+
+            0x7F, 0x06,
+            0x44, 0x1B,
+            0x40, 0xBF,
+            0x4E, 0x3F,
+
+            0x7F, 0x08,
+            0x66, 0x44,
+            0x65, 0x20,
+            0x6A, 0x3A,
+            0x61, 0x05,
+            0x62, 0x05,
+
+            0x7F, 0x09,
+            0x4F, 0xAF,
+            0x5F, 0x40,
+            0x48, 0x80,
+            0x49, 0x80,
+            0x57, 0x77,
+            0x60, 0x78,
+            0x61, 0x78,
+            0x62, 0x08,
+            0x63, 0x50,
+
+            0x7F, 0x0A,
+            0x45, 0x60,
+
+            0x7F, 0x00,
+            0x4D, 0x11,
+            0x55, 0x80,
+            0x74, 0x21,
+            0x75, 0x1F,
+            0x4A, 0x78,
+            0x4B, 0x78,
+            0x44, 0x08,
+
+            0x45, 0x50,
+            0x64, 0xFF,
+            0x65, 0x1F,
+
+            0x7F, 0x14,
+            0x65, 0x67,
+            0x66, 0x08,
+            0x63, 0x70,
+            0x6F, 0x1C,
+
+            0x7F, 0x15,
+            0x48, 0x48,
+
+            0x7F, 0x07,
+            0x41, 0x0D,
+            0x43, 0x14,
+            0x4B, 0x0E,
+            0x45, 0x0F,
+            0x44, 0x42,
+            0x4C, 0x80,
+
+            0x7F, 0x10,
+            0x5B, 0x02,
+
+            0x7F, 0x07,
+            0x40, 0x41,
+
+            0x7F, 0x00,
+            0x32, 0x00,
+
+            0x7F, 0x07,
+            0x40, 0x40,
+
+            0x7F, 0x06,
+            0x68, 0xF0,
+            0x69, 0x00,
+
+            0x7F, 0x0D,
+            0x48, 0xC0,
+            0x6F, 0xD5,
+
+            0x7F, 0x00,
+            0x5B, 0xA0,
+            0x4E, 0xA8,
+            0x5A, 0x90,
+            0x40, 0x80,
+            0x73, 0x1F,  
+
+            0x73, 0x00]