PAA5100JE motion sensor code

devices/PAA5100JE.py

@@ -1,8 +1,8 @@
 import time, gc, math
 from array import array
-from machine import Pin, SPI
+import machine
 from pyControl.hardware import *
-from device.PAA5100JE_firmware import *
+from devices.PAA5100JE_firmware import *
 
 def to_signed_16(value):
     """Convert a 16-bit integer to a signed 16-bit integer."""
@@ -13,8 +13,7 @@ def to_signed_16(value):
 class PAA5100JE():
     """Optical tracking sensor."""
     def __init__(self, spi_port: str, 
-                 spi_cs_gpio: str,
-                 reset: str = None, 
+                 spi_cs_gpio: str, 
                  sck: str = None, 
                  mosi: str =None, 
                  miso: str =None):
@@ -24,23 +23,21 @@ def __init__(self, spi_port: str,
         SPIparams = {'baudrate': 400000, 'polarity': 0, 'phase': 0,
                      'bits': 8, 'firstbit': machine.SPI.MSB}
         if '1' in spi_port:
-            self.spi = SPI(1, **SPIparams)
+            self.spi = machine.SPI(1, **SPIparams)
 
         elif '2' in spi_port:
-            self.spi = SPI(2, **SPIparams)
+            self.spi = machine.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),
+            self.spi = machine.SoftSPI(sck=machine.Pin(sck, mode=machine.Pin.OUT, pull=machine.Pin.PULL_DOWN),
+                                       mosi=machine.Pin(mosi, mode=machine.Pin.OUT, pull=machine.Pin.PULL_DOWN),
+                                       miso=machine.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
@@ -57,12 +54,38 @@ def __init__(self, spi_port: str,
             self._write(0x48, 0x04)
         else:
             self._write(0x48, 0x02)
-        self._bulk_write(PROGMEM[10:154])
+        self._bulk_write(PROGMEM[10:20])
+
+        if self._read(0x73) == 0x00:
+            c1 = self._read(0x70)
+            c2 = self._read(0x71)
+            if c1 <= 28:
+                c1 += 14
+            if c1 > 28:
+                c1 += 11
+            c1 = max(0, min(0x3F, c1))
+            c2 = (c2 * 45) // 100
+
+         self._bulk_write([
+                0x7F, 0x00,
+                0x61, 0xAD,
+                0x51, 0x70,
+                0x7F, 0x0E
+            ])
+            self._write(0x70, c1)
+            self._write(0x71, c2)
+
+        self._bulk_write(PROGMEM[20:154])
         time.sleep_ms(10)
         self._bulk_write(PROGMEM[154:186])
         time.sleep_ms(10)
         self._bulk_write(PROGMEM[186:])
-
+
+        # Check if registers are initialized
+        rev_ID = self._read(0x01)
+        if rev_ID == 0:
+            print("Registers initialized successfully.")
+
     def set_rotation(self, degrees:int =0):
         """Set orientation of PAA5100 in increments of 90 degrees."""
         if degrees == 0:
@@ -91,7 +114,9 @@ def _write(self, register: bytes, value: bytes):
         """Write into register"""
         self.select.on()
         self.spi.write(bytearray([register | 0x80, value]))
+        time.sleep_us(20)
         self.select.off()
+        time.sleep_us(100)
 
     def _read(self, register: bytes, length: int =1):
         """Read register"""
@@ -102,62 +127,98 @@ def _read(self, register: bytes, length: int =1):
 
         self.select.on()
         self.spi.write_readinto(send_buf, result)
+        time.sleep_us(1)
         self.select.off()
+        time.sleep_us(19)
 
         # 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"""
+        self.select.on()
         for x in range(0, len(data), 2):
             register, value = data[x : x + 2]
             self._write(register, value)
+            time.sleep_us(20)
+        self.select.off()
+        time.sleep_us(100)
 
-    def read_registers(self, reg: bytes, buf: bytearray, len: int):
+    def read_registers(self, registers: bytes, buf: bytearray, len: int):
         """Read an array of data from the registers"""
         self.select.on()
-        self.spi.write(bytearray([reg]))
+        self.spi.write(bytearray([registers]))
+        time.sleep_us(100)
         # Read data from the register
         buf[:] = self.spi.read(len)
-        self.select.off()   
+        time.sleep_us(1)
+        self.select.off()  
+        time.sleep_us(19)
 
     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()
+
+    def read_register_buff(self, addrs: bytes, buff: bytes):
+        """
+        addrs < 128
+        """
+        self.select.on()
+        self.spi.write(addrs)
+        time.sleep_us(100)  # tSRAD
+        self.spi.readinto(buff)
+        time.sleep_us(1)  # tSCLK-NCS for read operation is 120ns
+        self.select.off()
+        time.sleep_us(19)  # tSRW/tSRR (=20us) minus tSCLK-NCS
+
+    def write_register_buff(self, addrs: bytes, data: bytes):
+        """
+        addrs < 128
+        """
+        # flip the MSB to 1:...
+        self.select.on()
+        self.spi.write(addrs)
+        self.spi.write(data)
+        time.sleep_us(20)  # tSCLK-NCS for write operation
+        self.select.off()
+        time.sleep_us(100)  # tSWW/tSWR (=120us) minus tSCLK-NCS. Could be shortened, but is looks like a safe lower bound
 
 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'):
+    def __init__(self, cs1: str, cs2: str,
+                 name='MotSen', event='motion', 
+                 calib_coef=1, threshold=1, sampling_rate=100):
 
         # Create SPI objects
-        self.motSen_x = PAA5100JE('SPI2', cs1, reset)
-        self.motSen_y = PAA5100JE('SPI2', cs2, reset)
+        self.motSen_x = PAA5100JE('SPI2', cs1)
+        self.motSen_y = PAA5100JE('SPI2', cs2)
 
-        self.threshold = threshold
         self.calib_coef = calib_coef
+        self.threshold = threshold
 
         # 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_l = self.x_buffer_mv[0:1]
+        #self.delta_x_h = self.x_buffer_mv[1:]
+        #self.delta_y_l = self.y_buffer_mv[0:1]
+        #self.delta_y_h = self.y_buffer_mv[1:]
+
         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
@@ -200,6 +261,22 @@ def read_sample(self):
         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])
 
+        #self.motSen_y.write_register_buff(b'\x82', b'\x01')  # not sure what register 0x82 is (from previous sensor)
+        #self.motSen_y.read_register_buff(b'\x02', self.delta_y_l)
+        #self.motSen_y.read_register_buff(b'\x03', self.delta_y_l)
+        #self.motSen_y.read_register_buff(b'\x04', self.delta_y_l)
+        #self.motSen_y.read_register_buff(b'\x05', self.delta_y_l)
+        #self.motSen_y.read_register_buff(b'\x06', self.delta_y_h)
+        #self._delta_y = to_signed_16(int.from_bytes(self.y_buffer_mv, 'little'))
+
+        #self.motSen_x.write_register_buff(b'\x82', b'\x01')
+        #self.motSen_x.read_register_buff(b'\x02', self.delta_x_l)
+        #self.motSen_x.read_register_buff(b'\x03', self.delta_x_l)
+        #self.motSen_x.read_register_buff(b'\x04', self.delta_x_h)
+        #self.motSen_x.read_register_buff(b'\x05', self.delta_y_l)
+        #self.motSen_x.read_register_buff(b'\x06', self.delta_y_l)
+        #self._delta_x = to_signed_16(int.from_bytes(self.x_buffer_mv, 'little'))
+
         # Record accumulated motion
         self.delta_y += self._delta_y
         self.delta_x += self._delta_x