267 lines
10 KiB
Python
267 lines
10 KiB
Python
# SPDX-FileCopyrightText: 2023 John Park for Adafruit Industries
|
|
# SPDX-License-Identifier: MIT
|
|
# PowerWash Simulator controller
|
|
"""
|
|
Hardware:
|
|
# QT Py RP2040, BNO055, Wiichuck adapter, Piezo driver on D10 ('MO' pin on silk)
|
|
User control:
|
|
nozzle heading/roll (sensor is mounted "sideways" in washer handle) = mouse x/y
|
|
nozzle tap/shake = next nozzle tip
|
|
wii C button (while level) = rotate nozzle tip
|
|
wii Z button = trigger water
|
|
wii joystick = WASD
|
|
wii roll right = change stance stand/crouch/prone
|
|
wii roll left = jump
|
|
wii pitch up + C button = set target angle offset
|
|
wii pitch down = show dirt
|
|
wii pitch down + C button = toggle aim mode
|
|
"""
|
|
|
|
import time
|
|
import math
|
|
import board
|
|
from simpleio import map_range, tone
|
|
import adafruit_bno055
|
|
import usb_hid
|
|
from adafruit_hid.mouse import Mouse
|
|
from adafruit_hid.keycode import Keycode
|
|
from adafruit_hid.keyboard import Keyboard
|
|
from adafruit_hid.keyboard_layout_us import KeyboardLayoutUS
|
|
from adafruit_nunchuk import Nunchuk
|
|
|
|
# ===========================================
|
|
# constants
|
|
DEBUG = False
|
|
CURSOR = True # use to toggle cursor movment during testing/use
|
|
SENSOR_PACKET_FACTOR = 10 # Ratio of BNo055 data packets per Wiichuck packet
|
|
HORIZONTAL_RATE = 127 # mouse x speed
|
|
VERTICAL_RATE = 63 # mouse y speed
|
|
WII_C_KEY_1 = Keycode.R # rotate nozzle
|
|
WII_C_KEY_2 = Keycode.C # aim mode
|
|
WII_PITCH_UP = 270 # value to trigger wiichuk up state
|
|
WII_PITCH_DOWN = 730 # value to trigger wiichuck down state
|
|
WII_ROLL_LEFT = 280 # value to trigger wiichuck left state
|
|
WII_ROLL_RIGHT = 740 # value to trigger wiichuck right state
|
|
TAP_THRESHOLD = 6 # Tap sensitivity threshold; depends on the physical sensor mount
|
|
TAP_DEBOUNCE = 0.3 # Time for accelerometer to settle after tap (seconds)
|
|
|
|
# ===========================================
|
|
# Instantiate I2C interface connection
|
|
# i2c = board.I2C() # For board.SCL and board.SDA
|
|
i2c = board.STEMMA_I2C() # For the built-in STEMMA QT connection
|
|
|
|
# ===========================================
|
|
# setup USB HID mouse and keyboard
|
|
mouse = Mouse(usb_hid.devices)
|
|
keyboard = Keyboard(usb_hid.devices)
|
|
layout = KeyboardLayoutUS(keyboard)
|
|
|
|
# ===========================================
|
|
# wii nunchuk setup
|
|
wiichuk = Nunchuk(i2c)
|
|
|
|
# ===========================================
|
|
# Instantiate the BNo055 sensor
|
|
sensor = adafruit_bno055.BNO055_I2C(i2c)
|
|
sensor.mode = 0x0C # Set the sensor to NDOF_MODE
|
|
|
|
# ===========================================
|
|
# beep function
|
|
def beep(freq=440, duration=0.2):
|
|
"""Play the piezo element for duration (sec) at freq (Hz).
|
|
This is a blocking method."""
|
|
tone(board.D10, freq, duration)
|
|
|
|
# ===========================================
|
|
# debug print function
|
|
def printd(line):
|
|
"""Prints a string if DEBUG is True."""
|
|
if DEBUG:
|
|
print(line)
|
|
|
|
# ===========================================
|
|
# euclidean distance function
|
|
def euclidean_distance(reference, measured):
|
|
"""Calculate the Euclidean distance between reference and measured points
|
|
in a universe. The point position tuples can be colors, compass,
|
|
accelerometer, absolute position, or almost any other multiple value data
|
|
set.
|
|
reference: A tuple or list of reference point position values.
|
|
measured: A tuple or list of measured point position values."""
|
|
# Create list of deltas using list comprehension
|
|
deltas = [(reference[idx] - count) for idx, count in enumerate(measured)]
|
|
# Resolve squared deltas to a Euclidean difference and return the result
|
|
# pylint:disable=c-extension-no-member
|
|
return math.sqrt(sum([d ** 2 for d in deltas]))
|
|
|
|
# ===========================================
|
|
# BNO055 offsets
|
|
# Preset the sensor calibration offsets
|
|
# User sets this up once for geographic location using `bno055_calibrator.py` in library examples
|
|
sensor.offsets_magnetometer = (198, 238, 465)
|
|
sensor.offsets_gyroscope = (-2, 0, -1)
|
|
sensor.offsets_accelerometer = (-28, -5, -29)
|
|
printd(f"offsets_magnetometer set to: {sensor.offsets_magnetometer}")
|
|
printd(f"offsets_gyroscope set to: {sensor.offsets_gyroscope}")
|
|
printd(f"offsets_accelerometer set to: {sensor.offsets_accelerometer}")
|
|
|
|
# ===========================================
|
|
# controller states
|
|
wii_roll_state = 1 # roll left 0, center 1, roll right 2
|
|
wii_pitch_state = 1 # pitch down 0, center 1, pitch up 2
|
|
wii_last_roll_state = 1
|
|
wii_last_pitch_state = 1
|
|
c_button_state = False
|
|
z_button_state = False
|
|
|
|
sensor_packet_count = 0 # Initialize the BNo055 packet counter
|
|
|
|
print("PowerWash controller ready, point at center of screen for initial offset:")
|
|
beep(400, 0.1)
|
|
beep(440, 0.2)
|
|
time.sleep(3)
|
|
# The target angle offset used to reorient the wand to point at the display
|
|
#pylint:disable=(unnecessary-comprehension)
|
|
target_angle_offset = [angle for angle in sensor.euler]
|
|
beep(220, 0.4)
|
|
print("......reoriented", target_angle_offset)
|
|
|
|
|
|
while True:
|
|
# ===========================================
|
|
# BNO055
|
|
# Get the Euler angle values from the sensor
|
|
# The Euler angle limits are: +180 to -180 pitch, +360 to -360 heading, +90 to -90 roll
|
|
sensor_euler = sensor.euler
|
|
sensor_packet_count += 1 # Increment the BNo055 packet counter
|
|
# Adjust the Euler angle values with the target_position_offset
|
|
heading, roll, pitch = [
|
|
position - target_angle_offset[idx] for idx,
|
|
position in enumerate(sensor_euler)
|
|
]
|
|
printd(f"heading {heading}, roll {roll}")
|
|
# Scale the heading for horizontal movement range
|
|
# horizontal_mov = map_range(heading, 220, 260, -30.0, 30.0)
|
|
horizontal_mov = int(map_range(heading, -16, 16, HORIZONTAL_RATE*-1, HORIZONTAL_RATE))
|
|
printd(f"mouse x: {horizontal_mov}")
|
|
|
|
# Scale the roll for vertical movement range
|
|
vertical_mov = int(map_range(roll, 9, -9, VERTICAL_RATE*-1, VERTICAL_RATE))
|
|
printd(f"mouse y: {vertical_mov}")
|
|
if CURSOR:
|
|
mouse.move(x=horizontal_mov)
|
|
mouse.move(y=vertical_mov)
|
|
|
|
# ===========================================
|
|
# sensor packet ratio
|
|
# Read the wiichuck every "n" times the BNo055 is read
|
|
if sensor_packet_count >= SENSOR_PACKET_FACTOR:
|
|
sensor_packet_count = 0 # Reset the BNo055 packet counter
|
|
|
|
# ===========================================
|
|
# wiichuck joystick
|
|
joy_x, joy_y = wiichuk.joystick
|
|
printd(f"joystick = {wiichuk.joystick}")
|
|
if joy_x < 25:
|
|
keyboard.press(Keycode.A)
|
|
else:
|
|
keyboard.release(Keycode.A)
|
|
|
|
if joy_x > 225:
|
|
keyboard.press(Keycode.D)
|
|
else:
|
|
keyboard.release(Keycode.D)
|
|
|
|
if joy_y > 225:
|
|
keyboard.press(Keycode.W)
|
|
else:
|
|
keyboard.release(Keycode.W)
|
|
|
|
if joy_y < 25:
|
|
keyboard.press(Keycode.S)
|
|
else:
|
|
keyboard.release(Keycode.S)
|
|
|
|
# ===========================================
|
|
# wiichuck accel
|
|
wii_roll, wii_pitch, wii_az = wiichuk.acceleration
|
|
printd(f"roll:, {wii_roll}, pitch:, {wii_pitch}")
|
|
if wii_roll <= WII_ROLL_LEFT:
|
|
wii_roll_state = 0
|
|
if wii_last_roll_state != 0:
|
|
keyboard.press(Keycode.SPACE) # jump
|
|
wii_last_roll_state = 0
|
|
elif WII_ROLL_LEFT < wii_roll < WII_ROLL_RIGHT: # centered
|
|
wii_roll_state = 1
|
|
if wii_last_roll_state != 1:
|
|
keyboard.release(Keycode.LEFT_CONTROL)
|
|
keyboard.release(Keycode.SPACE)
|
|
wii_last_roll_state = 1
|
|
else:
|
|
wii_roll_state = 2
|
|
if wii_last_roll_state != 2:
|
|
keyboard.press(Keycode.LEFT_CONTROL) # change stance
|
|
wii_last_roll_state = 2
|
|
|
|
if wii_pitch <= WII_PITCH_UP: # up used as modifier
|
|
wii_pitch_state = 0
|
|
if wii_last_pitch_state != 0:
|
|
beep(freq=660)
|
|
wii_last_pitch_state = 0
|
|
elif WII_PITCH_UP < wii_pitch < WII_PITCH_DOWN: # level
|
|
wii_pitch_state = 1
|
|
if wii_last_pitch_state != 1:
|
|
wii_last_pitch_state = 1
|
|
else:
|
|
wii_pitch_state = 2 # down sends command and is modifier
|
|
if wii_last_pitch_state != 2:
|
|
keyboard.send(Keycode.TAB)
|
|
beep(freq=110)
|
|
wii_last_pitch_state = 2
|
|
|
|
# ===========================================
|
|
# wiichuck buttons
|
|
if wii_pitch_state == 0: # button use when wiichuck is held level
|
|
if wiichuk.buttons.C and c_button_state is False:
|
|
target_angle_offset = [angle for angle in sensor_euler]
|
|
beep()
|
|
beep()
|
|
c_button_state = True
|
|
if not wiichuk.buttons.C and c_button_state is True:
|
|
c_button_state = False
|
|
|
|
elif wii_pitch_state == 1: # level
|
|
if wiichuk.buttons.C and c_button_state is False:
|
|
keyboard.press(WII_C_KEY_1)
|
|
c_button_state = True
|
|
if not wiichuk.buttons.C and c_button_state is True:
|
|
keyboard.release(WII_C_KEY_1)
|
|
c_button_state = False
|
|
|
|
elif wii_pitch_state == 2: # down
|
|
if wiichuk.buttons.C and c_button_state is False:
|
|
keyboard.press(WII_C_KEY_2)
|
|
c_button_state = True
|
|
if not wiichuk.buttons.C and c_button_state is True:
|
|
keyboard.release(WII_C_KEY_2)
|
|
c_button_state = False
|
|
|
|
if wiichuk.buttons.Z and z_button_state is False:
|
|
mouse.press(Mouse.LEFT_BUTTON)
|
|
z_button_state = True
|
|
if not wiichuk.buttons.Z and z_button_state is True:
|
|
mouse.release(Mouse.LEFT_BUTTON)
|
|
z_button_state = False
|
|
|
|
# ===========================================
|
|
# BNO055 tap detection
|
|
# Detect a single tap on any axis of the BNo055 accelerometer
|
|
accel_sample_1 = sensor.acceleration # Read one sample
|
|
accel_sample_2 = sensor.acceleration # Read the next sample
|
|
if euclidean_distance(accel_sample_1, accel_sample_2) >= TAP_THRESHOLD:
|
|
# The difference between two consecutive samples exceeded the threshold ()
|
|
# (equivalent to a high-pass filter)
|
|
mouse.move(wheel=1)
|
|
printd("SINGLE tap detected")
|
|
beep()
|
|
time.sleep(TAP_DEBOUNCE) # Debounce delay
|