powerwash_07_jp.py

# SPDX-FileCopyrightText: 2023 John Park for Adafruit Industries
# SPDX-License-Identifier: MIT
# PowerWash Simulator controller
# to do:
#   -
# interactions:
#   bno heading/roll is mouse x/y
#   bno tap is next nozzle
#   wii c while level is nozzle rotate
#   wii c while up is re-center nozzle offsets
#   wii c while down is show dirt
#   wii z button triggers water
#   wii joystick is WASD
#   wii roll right soap??
#   wii roll left jump?
#   wii pitch up  + C button to set target angle offset
#   wii pitch down stance (Ctrl)
#   wii pitch down + C button show dirt


import time
import board
import math
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

CURSOR = False  # use to toggle cursor movment during coding
DEBUG = False
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.TAB  # show dirt
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)
keys = (Keycode.W, Keycode.A, Keycode.S, Keycode.D)

# ====================================
# 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


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)


def printd(line):
    """Prints a string if DEBUG is True."""
    if DEBUG:
        print(line)


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.
    :param list reference: A tuple of reference point position values.
    :param list measured: A tuple 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
    return math.sqrt(sum([d ** 2 for d in deltas]))

# Preset the sensor component offsets
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}")

# The target angle offset used to reorient the wand to point at the display screen
# (heading, roll, pitch)
target_angle_offset = (0, 0, 0)

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 = 0
wii_last_pitch_state = 0
c_button_state = False
z_button_state = False

sensor_packet_count = 0  # Initialize the BNo055 packet counter
prev_accel = sensor.acceleration  # Get initial reading for tap detection

print("PowerWash controller ready, point at center of screen for initial offset")
beep(400, 0.1)
beep(440, 0.2)
time.sleep(3)
target_angle_offset = [angle for angle in sensor.euler]
beep(220, 0.3)

while True:
    # 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
    # printd(f"Euler angle: {sensor_euler}")
    # 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)]
    # print("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, -20, 20, HORIZONTAL_RATE*-1, HORIZONTAL_RATE))
    printd(f"mouse x: {horizontal_mov}")

    # Scale the roll for vertical movement range
    vertical_mov = int(map_range(roll, 10, -10, VERTICAL_RATE*-1, VERTICAL_RATE))
    printd(f"mouse y: {vertical_mov}")
    if CURSOR:
        mouse.move(x=horizontal_mov)
        mouse.move(y=vertical_mov)

    # 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

        #  ===========================================
        # new joystick code
        #
        joy_x, joy_y = wiichuk.joystick
        printd(f"joystick = {wiichuk.joystick}")  # use for debugging

        #  ===========================================
        #   joystick code
        if joy_x < 25:
            printd("left")
            keyboard.press(Keycode.A)
        else:
            keyboard.release(Keycode.A)

        if joy_x > 225:
            printd("right")
            keyboard.press(Keycode.D)
        else:
            keyboard.release(Keycode.D)

        if joy_y > 225:
            printd("up")
            keyboard.press(Keycode.W)
        else:
            keyboard.release(Keycode.W)

        if joy_y < 25:
            printd("down")
            keyboard.press(Keycode.S)
        else:
            keyboard.release(Keycode.S)

        #  ===========================================
        # get the roll and pitch of the wiichuk
        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 is not 0:
                keyboard.press(Keycode.SPACE)  # jump
                print("press space")
                wii_last_roll_state = 0
        elif wii_roll > WII_ROLL_LEFT and wii_roll < WII_ROLL_RIGHT:
            wii_roll_state = 1
            if wii_last_roll_state is not 1:
                keyboard.release(Keycode.LEFT_CONTROL)
                keyboard.release(Keycode.SPACE)
                print("release control, space")
                wii_last_roll_state = 1
        else:
            wii_roll_state = 2
            if wii_last_roll_state is not 2:
                keyboard.press(Keycode.LEFT_CONTROL)  # change stance
                print("press control")
                wii_last_roll_state = 2


        if wii_pitch <= WII_PITCH_UP:
            wii_pitch_state = 0
            if wii_last_pitch_state is not 0:
                printd("wii up")
                beep(freq=660)
                wii_last_pitch_state = 0
        elif wii_pitch > WII_PITCH_UP and wii_pitch < WII_PITCH_DOWN:
            wii_pitch_state = 1
            if wii_last_pitch_state is not 1:
                printd("wii level")
                wii_last_pitch_state = 1
        else:
            wii_pitch_state = 2
            if wii_last_pitch_state is not 2:
                printd("wii down")
                beep(freq=110)
                wii_last_pitch_state = 2

        # button is normal use when wiichuck is held level
        if wii_pitch_state is 0:
            if wiichuk.buttons.C and c_button_state is False:
                printd("button C")
                target_angle_offset = [angle for angle in sensor_euler]
                beep()
                c_button_state = True
            if not wiichuk.buttons.C and c_button_state is True:
                c_button_state = False

        elif wii_pitch_state is 1:
            if wiichuk.buttons.C and c_button_state is False:
                printd("button C")
                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 is 2:
            if wiichuk.buttons.C and c_button_state is False:
                printd("button C")
                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:
            printd("button Z")
            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

    # Detect a tap on any axis of the BNo055 accelerometer
    prev_accel = sensor.acceleration  # Read one sample
    accel = sensor.acceleration  # Read the next sample
    if euclidean_distance(prev_accel, accel) >= TAP_THRESHOLD:
        # The difference between two consecutive samples exceeded the threshold
        # (equivalent to a high-pass filter)
        printd(f"SINGLE tap detected")
        beep()
        mouse.move(wheel=1)
        time.sleep(TAP_DEBOUNCE)  # Debounce delay