class SensorsProcess: def __init__(self): # Initialise the IPC classes self.sensorsIPC = SensorsSharedIPC() self.sensorsIPC.create() self.joystick = None def initialiseQuadratureCounters(self, address): # Set up quadrature interface board i2cBus = smbus2.SMBus(1) encoder = i2cEncoderLibV2.i2cEncoderLibV2(i2cBus, address) encconfig = (i2cEncoderLibV2.INT_DATA | i2cEncoderLibV2.WRAP_ENABLE | i2cEncoderLibV2.DIRE_RIGHT | i2cEncoderLibV2.IPUP_ENABLE | i2cEncoderLibV2.RMOD_X1 | i2cEncoderLibV2.RGB_ENCODER) encoder.begin(encconfig) encoder.writeCounter(0) encoder.writeMax(32767) encoder.writeMin(-32767) encoder.writeStep(1) encoder.writeInterruptConfig(0xff) return encoder def getQuadratureCounter(self, encoder, lastQuadratureCount): encoder.updateStatus() if encoder.readStatus(i2cEncoderLibV2.RINC) == True: #last = encoder.readCounter32() count = encoder.readCounter16() print('Increment: %8d=0x%08x %8d' % (count, count & 0xffffffff, count - lastQuadratureCount)) elif encoder.readStatus(i2cEncoderLibV2.RDEC) == True: #last = encoder.readCounter32() count = encoder.readCounter16() print('Decrement: %8d=0x%08x %8d' % (count, count & 0xffffffff, count - lastQuadratureCount)) else: count = lastQuadratureCount return count def initialiseJoystick(self): # Ensure we have a joystick connection if self.joystick is None or pygame.joystick.get_count() == 0: pygame.joystick.quit() pygame.joystick.init() while pygame.joystick.get_count() == 0: print("Please connect joystick...") pygame.time.wait(500) # mS #time.sleep(2.0) # Re-initialise the joystick pygame.joystick.quit() pygame.joystick.init() self.joystick = pygame.joystick.Joystick(0) self.joystick.init() def run(self): # Initialise the Joystick connection self.initialiseJoystick() # Initialise the quadrature interface board self.encoderL = self.initialiseQuadratureCounters(0x45) lastQuadratureCountL = 0 self.encoderR = self.initialiseQuadratureCounters(0x46) lastQuadratureCountR = 0 done = False while not done: # Check for quit for event in pygame.event.get(): # User did something. if event.type == pygame.QUIT: # If user clicked close. done = True # Flag that we are done so we exit this loop. # Get the Joystick axes for axis in range(6): axisValue = self.joystick.get_axis(axis) self.sensorsIPC.setAnalogValue(axis, axisValue) # Get the Joystick buttons for button in range(17): buttonValue = self.joystick.get_button(button) self.sensorsIPC.setDigitalValue(button, buttonValue) # Read the motor speed sensors newCount = self.getQuadratureCounter(self.encoderL, lastQuadratureCountL) timestamp = time.perf_counter() self.sensorsIPC.setCounterValue(0, newCount, timestamp=timestamp) lastQuadratureCountL = newCount newCount = self.getQuadratureCounter(self.encoderR, lastQuadratureCountR) timestamp = time.perf_counter() self.sensorsIPC.setCounterValue(1, newCount, timestamp=timestamp) lastQuadratureCountR = newCount # Report sensors alive self.sensorsIPC.resetWatchdog() pygame.time.wait(50) # mS
def __init__(self): # Initialise the IPC classes self.sensorsIPC = SensorsSharedIPC() self.sensorsIPC.create() self.joystick = None
def __init__(self): # Initialise the IPC classes self.mpu = MotionSensorSharedIPC() self.sensorsIPC = SensorsSharedIPC() self.sensorsIPC.open()
class SensorAccessFactory: def __init__(self): # Initialise the IPC classes self.mpu = MotionSensorSharedIPC() self.sensorsIPC = SensorsSharedIPC() self.sensorsIPC.open() __instance = None @classmethod def getSingleton(cls): if cls.__instance == None: cls.__instance = SensorAccessFactory() return cls.__instance def process(self): # Do common processing of state #for event in pygame.event.get(): # # Processes necessary events to update current joystick state # pass self.mpu.updateReading() def resetWatchdog(self, count=100): self.sensorsIPC.resetWatchdog(count) def checkWatchdog(self): return self.sensorsIPC.checkWatchdog() ## Raw MPU accessor methods def getAcceleration(self): return self.mpu.get_accel() def getGyro(self): return self.mpu.get_gyro() def getMpuSampleNumber(): return self.mpu.get_sample_number() def getMpuTimestampSeconds(): return self.mpu.get_timestamp() # Compass reading - this is pretty unstable! def getMpuTimestampSeconds(): return self.mpu.get_compassDegrees() # MPU tap and orientation detector def getTapCount(): return self.mpu.get_tap_count(self) def getOrientation(): return self.mpu.get_orientation() #################################################################### # Factory methods to access the sensor interfacte # These are the primary methods used to access the sensor IPC values def joystickAxis(self, axis): return JoystickAxis(self.sensorsIPC, axis) def button(self, button): return Button(self.sensorsIPC, button) def upDownButton(self, buttonUp, buttonDown): return UpDownButton(self.sensorsIPC, buttonUp, buttonDown) def analog(self, sensor): return AnalogSensor(self.sensorsIPC, sensor) def counter(self, sensor): return CounterSensor(self.sensorsIPC, sensor) def rateCounter(self, sensor): return CounterChangeSensor(self.sensorsIPC, sensor) # Get current orientation positions as Euler angles def roll(self): return Roll(self.mpu) def pitch(self): return Pitch(self.mpu) def yaw(self): return Yaw(self.mpu)
class SensorsProcess: def __init__(self): # Initialise the IPC classes self.sensorsIPC = SensorsSharedIPC() self.sensorsIPC.create() def initialiseVL53L0XSensors(self, muxAddress): # Create a VL53L0X object for device on TCA9548A bus 2 self.tof1 = VL53L0X(TCA9548A_Num=2, TCA9548A_Addr=muxAddress) # Create a VL53L0X object for device on TCA9548A bus 4 self.tof2 = VL53L0X(TCA9548A_Num=4, TCA9548A_Addr=muxAddress) # Create a VL53L0X object for device on TCA9548A bus 7 self.tof3 = VL53L0X(TCA9548A_Num=7, TCA9548A_Addr=muxAddress) # Start ranging on TCA9548A bus 1 self.tof1.start_ranging( VL53L0X_LONG_RANGE_MODE ) #VL53L0X_GOOD_ACCURACY_MODE) #VL53L0X_HIGH_SPEED_MODE # Start ranging on TCA9548A bus 4 self.tof2.start_ranging(VL53L0X_LONG_RANGE_MODE) # Start ranging on TCA9548A bus 7 self.tof3.start_ranging(VL53L0X_LONG_RANGE_MODE) # Work out how fast to poll self.timing = self.tof1.get_timing() / 1000000.00 if (self.timing < 0.02): self.timing = 0.02 print("Poll rate: %.3fs" % self.timing) def run(self): # Initialise the sensors self.initialiseVL53L0XSensors(0x70) count = 0 while True: # Left distance = self.tof1.get_distance() if (distance > 0): print(f"{count} Left(1): {distance}mm") timestamp = time.perf_counter() self.sensorsIPC.setAnalogValue(16, distance, timestamp=timestamp) else: self.sensorsIPC.setAnalogValue(16, -1, status=0) # Front distance = self.tof2.get_distance() if (distance > 0): print(f"{count} Front(4): {distance}mm") timestamp = time.perf_counter() self.sensorsIPC.setAnalogValue(17, distance, timestamp=timestamp) else: self.sensorsIPC.setAnalogValue(17, -1, status=0) # Right distance = self.tof3.get_distance() if (distance > 0): print(f"{count} Right(7): {distance}mm") timestamp = time.perf_counter() self.sensorsIPC.setAnalogValue(18, distance, timestamp=timestamp) else: self.sensorsIPC.setAnalogValue(18, -1, status=0) count += 1 time.sleep(self.timing)
class SensorsProcess: def __init__(self): # Initialise the IPC classes self.sensorsIPC = SensorsSharedIPC() self.sensorsIPC.create() def initialiseQuadratureCounters(self, address): # Set up quadrature interface board i2cBus = smbus2.SMBus(1) encoder = i2cEncoderLibV2.i2cEncoderLibV2(i2cBus, address) encconfig = (i2cEncoderLibV2.INT_DATA | i2cEncoderLibV2.WRAP_ENABLE | i2cEncoderLibV2.DIRE_RIGHT | i2cEncoderLibV2.IPUP_ENABLE | i2cEncoderLibV2.RMOD_X1 | i2cEncoderLibV2.RGB_ENCODER) encoder.begin(encconfig) encoder.writeCounter(0) encoder.writeMax(32767) encoder.writeMin(-32767) encoder.writeStep(1) encoder.writeInterruptConfig(0xff) return encoder def getQuadratureCounter(self, encoder, lastQuadratureCount): encoder.updateStatus() if encoder.readStatus(i2cEncoderLibV2.RINC) == True: #last = encoder.readCounter32() count = encoder.readCounter16() print('Increment: %8d=0x%08x %8d' % (count, count & 0xffffffff, count - lastQuadratureCount)) elif encoder.readStatus(i2cEncoderLibV2.RDEC) == True: #last = encoder.readCounter32() count = encoder.readCounter16() print('Decrement: %8d=0x%08x %8d' % (count, count & 0xffffffff, count - lastQuadratureCount)) else: count = lastQuadratureCount return count def run(self): # Initialise the quadrature interface board self.encoderL = self.initialiseQuadratureCounters(0x45) lastQuadratureCountL = 0 self.encoderR = self.initialiseQuadratureCounters(0x46) lastQuadratureCountR = 0 done = False while not done: # Read the motor speed sensors newCount = self.getQuadratureCounter(self.encoderL, lastQuadratureCountL) timestamp = time.perf_counter() self.sensorsIPC.setCounterValue(0, newCount, timestamp=timestamp) lastQuadratureCountL = newCount #print(f"L: {newCount}") newCount = self.getQuadratureCounter(self.encoderR, lastQuadratureCountR) timestamp = time.perf_counter() self.sensorsIPC.setCounterValue(1, -newCount, timestamp=timestamp) # Inverted lastQuadratureCountR = newCount #print(f"R: {newCount}") # Report sensors alive #self.sensorsIPC.resetWatchdog() time.sleep(0.02) # seconds
class SensorsProcess: # Mapping of ApproxEng named axes to PyGame equivalent axes = [("lx", 0), ("ly", 1), ("lt", 2), ("rx", 3), ("ry", 4), ("rt", 5), ("pitch", 6), ("roll", 7)] # Buttons: buttons = [("cross", 0), ("circle", 1), ("triangle", 2), ("square", 3), ("l1", 4), ("r1", 5), ("l2", 6), ("r2", 7), ("select", 8), ("start", 9), ("home", 10), ("ls", 11), ("rs", 12), ("dup", 13), ("ddown", 14), ("dleft", 15), ("dright", 16)] def __init__(self): # Initialise the IPC classes self.sensorsIPC = SensorsSharedIPC() self.sensorsIPC.create() def clearAll(self): # Clear all of the buttons, to ensure a clear stop of any robot movements for axis in SensorsProcess.axes: self.sensorsIPC.setAnalogValue(axis[1], 0.0) # Copy over the "held" status of buttons for button in SensorsProcess.buttons: self.sensorsIPC.setDigitalValue(button[1], 0.0) def run(self): while True: try: with ControllerResource() as joystick: print('Found a joystick and connected') while joystick.connected: # Copy over axes values for axis in SensorsProcess.axes: axisValue = joystick[axis[0]] #print(f"axis: {axis} = {axisValue}") self.sensorsIPC.setAnalogValue(axis[1], axisValue) # Copy over the "held" status of buttons for button in SensorsProcess.buttons: held = joystick[button[0]] #print(f"button: {button} = {held}") self.sensorsIPC.setDigitalValue( button[1], 1.0 if (held is not None) else 0.0) # Report sensors alive self.sensorsIPC.resetWatchdog() # Poll delay time.sleep(0.05) # Joystick diconnected self.clearAll() except IOError: # No joystick found, wait for a bit before trying again print("Please connect joystick...") # Joystick diconnected self.clearAll() time.sleep(1.0)