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)