switched from usb hid to cdc

1 files changed, 52 insertions, 124 deletions

diff --git a/src/pico/code.py b/src/pico/code.py
index eb90c9e..3211d43 100755
--- a/src/pico/code.py
+++ b/src/pico/code.py
@@ -1,160 +1,88 @@
 import time

 import board

 import busio

-import usb_hid

-import struct

+import usb_cdc

 import digitalio

 import analogio

 import adafruit_mpu6050

 import math

 

-# https://forums.adafruit.com/viewtopic.php?t=194490

-if True:        # optionally wiggle SCL to clear I2C slave devices

-  print("***** Clear I2C bus *****")

-  scl = digitalio.DigitalInOut(board.GP1)

-  scl.direction = digitalio.Direction.OUTPUT  # want open drain scl.drive_mode = digitalio.DriveMode.OPEN_DRAIN

-  for n in range(1,20):

-    scl.value = 0

-    time.sleep(0.0001)

-    scl.value = 1

-    time.sleep(0.0001)

-  scl.direction = digitalio.Direction.INPUT

-  scl.deinit()

-

-# Initialize I2C and MPU6050

+# Setup I2C for MPU6050

 i2c = busio.I2C(board.GP1, board.GP0)

 mpu = adafruit_mpu6050.MPU6050(i2c)

 

-# Configure MPU6050

+# MPU6050 config

 mpu.gyro_range = adafruit_mpu6050.GyroRange.RANGE_2000_DPS

 mpu.filter_bandwidth = adafruit_mpu6050.Bandwidth.BAND_44_HZ

-mpu.sample_rate_divisor = 9  # Set sample rate to 100Hz

-

-# Create a custom HID device

-gamepad = usb_hid.devices[0]  # Assuming the first device is the gamepad

-

-# Button setup (digital inputs)

-buttons = {

-    1: digitalio.DigitalInOut(board.GP2),

-    2: digitalio.DigitalInOut(board.GP3),

-    3: digitalio.DigitalInOut(board.GP4),

-    4: digitalio.DigitalInOut(board.GP5),

-    5: digitalio.DigitalInOut(board.GP6),

-    6: digitalio.DigitalInOut(board.GP7),

-    7: digitalio.DigitalInOut(board.GP8),

-}

-

-for button in buttons.values():

-    button.direction = digitalio.Direction.INPUT

-    button.pull = digitalio.Pull.UP

-

-# Joystick setup (analog inputs)

+mpu.sample_rate_divisor = 9  # ~100Hz

+

+# Joystick analog inputs

 joy_x_axis = analogio.AnalogIn(board.GP26)

 joy_y_axis = analogio.AnalogIn(board.GP27)

 

-# Calibration (replace with actual calibration values)

-gyro_x_offset = 0.0

-gyro_y_offset = 0.0

-gyro_z_offset = 0.0

+# Buttons

+button_pins = [board.GP2, board.GP3, board.GP4, board.GP5, board.GP6, board.GP7, board.GP8, board.GP9, board.GP10]

+buttons = []

 

-# Complementary filter parameters

-ALPHA = 0.98  # Weight for gyroscope data (0 < ALPHA < 1)

-DT = 0.005  # Sampling time (200Hz)

+for pin in button_pins:

+    btn = digitalio.DigitalInOut(pin)

+    btn.direction = digitalio.Direction.INPUT

+    btn.pull = digitalio.Pull.UP

+    buttons.append(btn)

 

-# Initialize angles

-pitch = 0.0

-roll = 0.0

-yaw = 0.0

+# Complementary filter vars

+pitch, roll, yaw = 0.0, 0.0, 0.0

+ALPHA = 0.98

+DT = 0.01  # 100Hz

 

-# Function to calculate angles from accelerometer data

-def calculate_angles(accel_x, accel_y, accel_z):

-    pitch = math.atan2(accel_y, math.sqrt(accel_x**2 + accel_z**2)) * (180.0 / math.pi)

-    roll = math.atan2(-accel_x, accel_z) * (180.0 / math.pi)

+def calculate_angles(ax, ay, az):

+    pitch = math.atan2(ay, math.sqrt(ax**2 + az**2)) * 180.0 / math.pi

+    roll = math.atan2(-ax, az) * 180.0 / math.pi

     return pitch, roll

 

-# Function to apply complementary filter

-def complementary_filter(gyro_data, accel_data, prev_angles, alpha, dt):

-    # Calculate angles from accelerometer

-    accel_pitch, accel_roll = calculate_angles(*accel_data)

-    

-    # Integrate gyroscope data to get angles

-    gyro_pitch = prev_angles[0] + gyro_data[0] * dt

-    gyro_roll = prev_angles[1] + gyro_data[1] * dt

-    gyro_yaw = prev_angles[2] + gyro_data[2] * dt  # Integrate Z-axis for yaw

-    

-    # Combine gyroscope and accelerometer angles using complementary filter

-    pitch = alpha * gyro_pitch + (1 - alpha) * accel_pitch

-    roll = alpha * gyro_roll + (1 - alpha) * accel_roll

-    yaw = gyro_yaw  # Yaw is purely from gyroscope

-    

-    return pitch, roll, yaw

+def complementary_filter(gyro, accel, prev_angles):

+    accel_pitch, accel_roll = calculate_angles(*accel)

+    gyro_pitch = prev_angles[0] + gyro[0] * DT

+    gyro_roll = prev_angles[1] + gyro[1] * DT

+    gyro_yaw = prev_angles[2] + gyro[2] * DT

 

-def read_buttons():

-    """Reads button states and updates the bitmask for HID report."""

-    buttons_state = 0

-    for button_num, button in buttons.items():

-        if not button.value:  # Button pressed (active low)

-            buttons_state |= 1 << (button_num - 1)

-    return buttons_state

+    pitch = ALPHA * gyro_pitch + (1 - ALPHA) * accel_pitch

+    roll = ALPHA * gyro_roll + (1 - ALPHA) * accel_roll

+    yaw = gyro_yaw

+

+    return pitch, roll, yaw

 

 def read_joysticks():

-    """Reads analog joystick values and scales them to -127 to 127."""

-    joy_x = max(-127, min(127, ((joy_x_axis.value - 32768) * 127) // 32768))

-    joy_y = max(-127, min(127, ((joy_y_axis.value - 32768) * 127) // 32768))

-    return joy_x, joy_y

+    jx = ((joy_x_axis.value - 32768) * 127) // 32768

+    jy = ((joy_y_axis.value - 32768) * 127) // 32768

+    return max(-127, min(127, jx)), max(-127, min(127, jy))

 

-print("MPU6050 Game Controller Running!")

+def read_buttons():

+    return [int(not b.value) for b in buttons]  # 1 = pressed

+

+print("Streaming controller data over USB CDC...")

 

 while True:

-    while not i2c.try_lock():

-        pass

-    

-    print("I2C addresses found:", [hex(x) for x in i2c.scan()])

-    i2c.unlock()

-    

     try:

-        gyro_x, gyro_y, gyro_z = mpu.gyro

-        accel_x, accel_y, accel_z = mpu.acceleration

-    except OSError as e:

-        print(f"Sensor read failed: {e}")

-        time.sleep(0.1)

-        

+        gx, gy, gz = mpu.gyro

+        ax, ay, az = mpu.acceleration

+    except Exception as e:

+        print(f"Sensor error: {e}")

         continue

-    

-    # Read and calibrate gyro data

-    # gyro_x, gyro_y, gyro_z = (0, 0, 0)

-    # gyro_x, gyro_y, gyro_z = mpu.gyro

-    gyro_x -= gyro_x_offset

-    gyro_y -= gyro_y_offset

-    gyro_z -= gyro_z_offset

-    # accel_x, accel_y, accel_z = (0, 0, 0) 

-    # accel_x, accel_y, accel_z = mpu.acceleration

-

-    # Apply complementary filter to calculate pitch, roll, and yaw

-    pitch, roll, yaw = complementary_filter(

-        (gyro_x, gyro_y, gyro_z),  # Gyroscope data (angular velocity in rad/s)

-        (accel_x, accel_y, accel_z),  # Accelerometer data

-        (pitch, roll, yaw),  # Previous angles

-        ALPHA,  # Filter parameter

-        DT  # Sampling time

-    )

-

-    # Scale angles to -127 to 127

+

+    pitch, roll, yaw = complementary_filter((gx, gy, gz), (ax, ay, az), (pitch, roll, yaw))

+

+    # Scale angles

     pitch_scaled = max(-127, min(127, int(pitch)))

     roll_scaled = max(-127, min(127, int(roll)))

-    yaw_scaled = max(-127, min(127, int(yaw * 100)))

-

-    # Read button and joystick states

-    buttons_state = read_buttons()

-    joy_x, joy_y = read_joysticks()

+    yaw_scaled = max(-127, min(127, int(yaw * 10)))  # Scale for readability

 

-    # Send HID report

-    report = bytearray(8)

-    struct.pack_into("<H", report, 0, buttons_state)  # Buttons (2 bytes)

-    struct.pack_into("<bb", report, 2, joy_x, joy_y)  # Joystick X, Y (X and Y axes)

-    struct.pack_into("<bbb", report, 4, pitch_scaled, roll_scaled, yaw_scaled)  # Pitch, Roll, Yaw (scaled to -127 to 127)

+    jx, jy = read_joysticks()

+    button_states = read_buttons()

 

-    gamepad.send_report(report)

+    # Format: x,y,pitch,roll,yaw,b1,b2,...,b9

+    msg = f"{jx},{jy},{pitch_scaled},{roll_scaled},{yaw_scaled}," + ",".join(map(str, button_states)) + "\n"

+    usb_cdc.data.write(msg.encode('utf-8'))

 

     time.sleep(DT)