powerwash_jp_06.py

# SPDX-FileCopyrightText: 2023 John Park for Adafruit Industries
# SPDX-License-Identifier: MIT
# PowerWash Simulator controller
# to do:
#   - tame the taps
# interactions:
#   bno heading/roll is mouse x/y
#   bno double 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
from supervisor import ticks_ms
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

# ====================================
# 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}")

# Set the BNo055 tap detector parameters and initialize tap event history list
TAP_THRESHOLD = 10  # Tap sensitivity threshold; depends on the physical sensor mount
TAP_TIMEOUT = 200  # Remove tap event from history timeout (milliseconds)
tap_events = []  # Initialize the tap event history list
prev_accel = sensor.acceleration  # Get initial reading for tap detection


# 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 = 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(220, 0.3)
time.sleep(2)
target_angle_offset = [angle for angle in sensor.euler]
beep()

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
        # #
        # if 25 < joy_x < 255:
        #     printd("center")
        #     # keyboard.release(Keycode.A)
        #     # keyboard.release(Keycode.D)
        # elif joy_x < 25:
        #     printd("left")
        # #     keyboard.press(Keycode.A)
        # else:
        #     printd("right")
        #     # keyboard.press(Keycode.D)
        #
        # if 25 < joy_y < 255:
        #     pass  # just during HID impact testing
        #     # keyboard.release(Keycode.S)
        #     # keyboard.release(Keycode.W)
        # elif joy_y < 25:
        #     printd("down")
        #     # keyboard.press(Keycode.S)
        # else:
        #     printd("up")
        #     # keyboard.press(Keycode.W)
        #  ===========================================

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

        # x roll l/c/r 285/508/744
        # y pitch u/c/d 272/509/745
        # z freefall detect, not usefull here
        # get the roll and pitch of the wiichuk
        wii_roll, wii_pitch, wii_az = wiichuk.acceleration

        if wii_roll <= 400:
            wii_roll_state = 0
        elif wii_roll > 400 and wii_roll < 700:
            wii_roll_state = 1
        else:
            wii_roll_state = 2

        if wii_pitch <= 400:
            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 > 400 and wii_pitch < 700:
            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

    # BNo055 Accelerometer Tap Detector
    accel = sensor.acceleration  # Get the current acceleration tuple

# Detect a tap on any axis of the accelerometer
    if euclidean_distance(prev_accel, accel) >= TAP_THRESHOLD:
        # The difference between two consecutive samples exceeded the threshold
        # printd(f"SINGLE tap detected {ticks_ms()}")
        tap_events.append(ticks_ms() + TAP_TIMEOUT)  # save tap expiration time in event stack
        time.sleep(0.1)  # Debounce delay
    prev_accel = accel  # Update the previous with the current tuple

    # Clean up tap event history after timeout period expires
    if len(tap_events) > 0:
        # Check for expired events
        if tap_events[0] <= ticks_ms():
            # The oldest event has expired
            tap_events = tap_events[1:]  # Remove the oldest event

    # Check see if one tap is in the event history list
    if len(tap_events) == 1:
        # Single-tap: Execute nozzle change
        printd(f"SINGLE tap detected {ticks_ms()}")
        beep()
        mouse.move(wheel=1)
        tap_events = []  # Clear event history