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)
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
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)