diff --git a/devices/PAA5100JE.py b/devices/PAA5100JE.py new file mode 100644 index 0000000..d0baf84 --- /dev/null +++ b/devices/PAA5100JE.py @@ -0,0 +1,300 @@ +import time, gc, math, machine +from array import array +from pyControl.hardware import * +from devices.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: + C++ code reference can be found on: https://github.com/pimoroni/pimoroni-pico.git + and on https://github.com/zic-95/PAA5100JE/blob/main/src/PAA5100JE.cpp + """ + def __init__(self, + SPI_type: str, + CS: str, + MI: str = None, + MO: str = None, + SCK: str = None): + + # Initialize SPI + # SPI_type = 'SPI1' or 'SPI2' or 'softSPI' + SPIparams = {'baudrate': 1000000, 'polarity': 1, 'phase': 1, + 'bits': 8, 'firstbit': machine.SPI.MSB} + + if '1' in SPI_type: + self.spi = machine.SPI(1, **SPIparams) + + elif '2' in SPI_type: + self.spi = machine.SPI(2, **SPIparams) + + elif 'soft' in SPI_type.lower(): # Works for newer versions of micropython + self.spi = machine.SoftSPI(sck=machine.Pin(SCK, mode=machine.Pin.OUT, pull=machine.Pin.PULL_DOWN), + mosi=machine.Pin(MO, mode=machine.Pin.OUT, pull=machine.Pin.PULL_DOWN), + miso=machine.Pin(MI, mode=machine.Pin.IN), + **SPIparams + ) + + # Define Chip Select (CS) pin (active low) + self.select = Digital_output(pin=CS, inverted=True) + + time.sleep_ms(1) + self.select.off() # Deselect the device by setting CS high + time.sleep_ms(1) + self.select.on() # Select the device by setting CS low + time.sleep_ms(1) + self.select.off() + time.sleep_ms(1) + + # Reset the sensor + self.firmware = PAA5100JE_firmware() + self._write(self.firmware.REG_POWER_UP_RESET, 0x5A) + + time.sleep_ms(20) + # Read motion registers once after reset + for offset in range(5): + self._read(self.firmware.REG_DATA_READY + offset) + time.sleep_ms(1) + + # Registers initialization protocol + PROGMEM = self.firmware.init_registers() + self._bulk_write(PROGMEM[0:10]) + res = self._read(0x67) & 0x80 + if res == 0x80: + self._write(0x48, 0x04) + else: + self._write(0x48, 0x02) + self._bulk_write(PROGMEM[10:20]) + + if self._read(0x73) == 0x00: + c1 = int(self._read(0x70)) + c2 = int(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:]) + + time.sleep_ms(10) + # Check for successful initialization + prod_ID = self._read(0x00) + assert prod_ID == 0x49 + + 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, address: int, value: int): + """Write value into register""" + address |= 0x80 # Flip MSB to 1 + address = address.to_bytes(1, 'little') # Convert the address from integer to a single byte + value = value.to_bytes(1, 'little') # Convert the value from integer to a single byte + + self.select.on() + time.sleep_us(1) + self.spi.write(address) # find specific address of the device + self.spi.write(value) # write value into the above address of the device + time.sleep_us(5) # tSWW + self.select.off() + time.sleep_us(5) # buffer time + + def _read(self, address: int): + """Read register""" + # Create a buffer to send + address &= ~0x80 # Ensure MSB is 0 + address = address.to_bytes(1, 'little') # Convert the integer to a single byte + + self.select.on() + time.sleep_us(1) + self.spi.write(address) + time.sleep_us(5) # tSRAD + tSWR + + data = self.spi.read(1) + + val = int.from_bytes(data, 'little') # converts received data back to integer for further calculations + time.sleep_us(1) # tSCLK-NCS for read operation is 120ns + self.select.off() + time.sleep_us(5) # tSRW/tSRR minus tSCLK-NCS + return val + + def _bulk_write(self, data: int): + """Write a list of commands into registers""" + for x in range(0, len(data), 2): + address, value = data[x : x + 2] + self._write(address, value) + + def read_registers(self, address: int, buf: bytearray, len: int): + """Read an array of data from the registers, used for reading motion burst""" + address &= ~0x80 # Flip MSB to 1 + address = address.to_bytes(1, 'little') # Convert the address from integer to a single byte + + self.select.on() + time.sleep_us(1) + self.spi.write(address) + time.sleep_us(5) + # Read 12 bytes of data from the motion burst register + for i in range(12): + buf[i] = self.spi.read(1) + time.sleep_us(5) + self.select.off() + time.sleep_us(50) + # Check for data being successfully read into the buffer + assert buf[10] == 0x1F, str(buf[10]) + + def shut_down(self, deinitSPI:bool =True): + """Shutdown the sensor""" + self.select.off() + time.sleep_ms(1) + self.select.on() + time.sleep_ms(60) + self._write(self.firmware.REG_SHUTDOWN, 0xB6) + time.sleep_ms(1) + self.select.off() + time.sleep_ms(1) + if deinitSPI: + self.spi.deinit() + +class MotionDetector2(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='MotSen', threshold=1, calib_coef=1, + sampling_rate=100, event='motion'): + + # Create SPI objects + self.motSen_x = PAA5100JE('SPI2', cs1) + self.motSen_y = PAA5100JE('SPI2', cs2) + + 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, 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(self._threshold) + + @threshold.setter + def threshold(self, new_threshold): + self._threshold = int((new_threshold)**2) * self.calib_coef + self.reset_delta() + + 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) # Note: no self.ID for this sensor, cannot interrupt queue so data were not sent + + 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() diff --git a/devices/PAA5100JE_firmware.py b/devices/PAA5100JE_firmware.py new file mode 100644 index 0000000..468c903 --- /dev/null +++ b/devices/PAA5100JE_firmware.py @@ -0,0 +1,139 @@ +class PAA5100JE_firmware(): + def __init__(self): + """ + PAA5100JE firmware includes registers and initiation protocol for + the optical tracking chip + """ + # PAA5100 registers definitions + self.REG_ID = 0x00 + self.REVISION = 0x01 + 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): + """Initialization protocol for sensor setup (C++ code reference can be found on: https://github.com/pimoroni/pimoroni-pico.git)""" + 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]