Merge pull request #12 from redphx/feature/merge_joycon_python
Improve tracking, again
This commit is contained in:
commit
0f8e7f7550
4
dance.py
4
dance.py
|
@ -11,12 +11,12 @@ from enum import Enum
|
||||||
import aiohttp
|
import aiohttp
|
||||||
import hid
|
import hid
|
||||||
from aiohttp import WSMsgType, web
|
from aiohttp import WSMsgType, web
|
||||||
from pyjoycon import ButtonEventJoyCon, JoyCon
|
|
||||||
from pyjoycon.constants import JOYCON_PRODUCT_IDS, JOYCON_VENDOR_ID
|
|
||||||
|
|
||||||
from joydance import JoyDance, PairingState
|
from joydance import JoyDance, PairingState
|
||||||
from joydance.constants import (DEFAULT_CONFIG, JOYDANCE_VERSION,
|
from joydance.constants import (DEFAULT_CONFIG, JOYDANCE_VERSION,
|
||||||
WsSubprotocolVersion)
|
WsSubprotocolVersion)
|
||||||
|
from pycon import ButtonEventJoyCon, JoyCon
|
||||||
|
from pycon.constants import JOYCON_PRODUCT_IDS, JOYCON_VENDOR_ID
|
||||||
|
|
||||||
logging.getLogger('asyncio').setLevel(logging.WARNING)
|
logging.getLogger('asyncio').setLevel(logging.WARNING)
|
||||||
|
|
||||||
|
|
|
@ -69,7 +69,6 @@ class JoyDance:
|
||||||
self.available_shortcuts = set()
|
self.available_shortcuts = set()
|
||||||
|
|
||||||
self.accel_data = []
|
self.accel_data = []
|
||||||
self.last_accel = (0, 0, 0)
|
|
||||||
|
|
||||||
self.ws = None
|
self.ws = None
|
||||||
self.disconnected = False
|
self.disconnected = False
|
||||||
|
@ -242,34 +241,46 @@ class JoyDance:
|
||||||
async for message in self.ws:
|
async for message in self.ws:
|
||||||
await self.on_message(message)
|
await self.on_message(message)
|
||||||
|
|
||||||
|
async def sleep_approx(self, target_duration):
|
||||||
|
tmp_duration = target_duration
|
||||||
|
x = 0.3
|
||||||
|
start = time.time()
|
||||||
|
while True:
|
||||||
|
tmp_duration = tmp_duration * x
|
||||||
|
await asyncio.sleep(tmp_duration)
|
||||||
|
|
||||||
|
dt = time.time() - start
|
||||||
|
if dt >= target_duration:
|
||||||
|
break
|
||||||
|
|
||||||
|
tmp_duration = target_duration - dt
|
||||||
|
|
||||||
async def tick(self):
|
async def tick(self):
|
||||||
sleep_duration = FRAME_DURATION * 0.75
|
sleep_duration = FRAME_DURATION
|
||||||
last_time = time.time()
|
|
||||||
frames = 0
|
frames = 0
|
||||||
|
|
||||||
while True:
|
while True:
|
||||||
if self.disconnected:
|
if self.disconnected:
|
||||||
break
|
break
|
||||||
|
|
||||||
# Make sure it runs at exactly 60 FPS
|
|
||||||
while True:
|
|
||||||
time_now = time.time()
|
|
||||||
dt = time_now - last_time
|
|
||||||
if dt >= FRAME_DURATION:
|
|
||||||
break
|
|
||||||
last_time = time_now
|
|
||||||
frames = frames + 1 if frames < 3 else 1
|
|
||||||
|
|
||||||
if not self.should_start_accelerometer:
|
if not self.should_start_accelerometer:
|
||||||
await asyncio.sleep(sleep_duration),
|
frames = 0
|
||||||
|
await asyncio.sleep(sleep_duration)
|
||||||
continue
|
continue
|
||||||
|
|
||||||
await asyncio.gather(
|
last_time = time.time()
|
||||||
asyncio.sleep(sleep_duration),
|
frames = frames + 1 if frames < 3 else 1
|
||||||
self.collect_accelerometer_data(frames),
|
|
||||||
)
|
|
||||||
|
|
||||||
async def collect_accelerometer_data(self, frames):
|
await asyncio.gather(
|
||||||
|
self.sleep_approx(sleep_duration),
|
||||||
|
self.collect_accelerometer_data(),
|
||||||
|
)
|
||||||
|
await self.send_accelerometer_data(frames)
|
||||||
|
|
||||||
|
dt = time.time() - last_time
|
||||||
|
sleep_duration = FRAME_DURATION - (dt - sleep_duration)
|
||||||
|
|
||||||
|
async def collect_accelerometer_data(self):
|
||||||
if self.disconnected:
|
if self.disconnected:
|
||||||
return
|
return
|
||||||
|
|
||||||
|
@ -278,23 +289,15 @@ class JoyDance:
|
||||||
return
|
return
|
||||||
|
|
||||||
try:
|
try:
|
||||||
start = time.time()
|
accels = self.joycon.get_accels() # (x, y, z)
|
||||||
max_runtime = FRAME_DURATION * 0.5
|
|
||||||
while time.time() - start < max_runtime:
|
|
||||||
# Make sure accelerometer axes are changed
|
|
||||||
accel = self.joycon.get_accels() # (x, y, z)
|
|
||||||
if accel != self.last_accel:
|
|
||||||
self.last_accel = accel
|
|
||||||
break
|
|
||||||
|
|
||||||
# Accelerator axes on phone & Joy-Con are different so we need to swap some axes
|
# Accelerator axes on phone & Joy-Con are different so we need to swap some axes
|
||||||
# https://github.com/dekuNukem/Nintendo_Switch_Reverse_Engineering/blob/master/imu_sensor_notes.md
|
# https://github.com/dekuNukem/Nintendo_Switch_Reverse_Engineering/blob/master/imu_sensor_notes.md
|
||||||
|
accel = accels[2]
|
||||||
x = accel[1] * -1
|
x = accel[1] * -1
|
||||||
y = accel[0]
|
y = accel[0]
|
||||||
z = accel[2]
|
z = accel[2]
|
||||||
|
|
||||||
self.accel_data.append([x, y, z])
|
self.accel_data.append([x, y, z])
|
||||||
await self.send_accelerometer_data(frames),
|
|
||||||
except OSError:
|
except OSError:
|
||||||
self.disconnect()
|
self.disconnect()
|
||||||
return
|
return
|
||||||
|
|
|
@ -15,6 +15,7 @@ WS_SUBPROTOCOLS[WsSubprotocolVersion.V1.value] = 'v1.phonescoring.jd.ubisoft.com
|
||||||
WS_SUBPROTOCOLS[WsSubprotocolVersion.V2.value] = 'v2.phonescoring.jd.ubisoft.com'
|
WS_SUBPROTOCOLS[WsSubprotocolVersion.V2.value] = 'v2.phonescoring.jd.ubisoft.com'
|
||||||
|
|
||||||
FRAME_DURATION = 1 / 60
|
FRAME_DURATION = 1 / 60
|
||||||
|
SEND_FREQ_MS = 0.005
|
||||||
ACCEL_ACQUISITION_FREQ_HZ = 60 # Hz
|
ACCEL_ACQUISITION_FREQ_HZ = 60 # Hz
|
||||||
ACCEL_ACQUISITION_LATENCY = 40 # ms
|
ACCEL_ACQUISITION_LATENCY = 40 # ms
|
||||||
ACCEL_MAX_RANGE = 8 # ±G
|
ACCEL_MAX_RANGE = 8 # ±G
|
||||||
|
@ -38,6 +39,7 @@ class Command(Enum):
|
||||||
|
|
||||||
BACK = 'SHORTCUT_BACK'
|
BACK = 'SHORTCUT_BACK'
|
||||||
CHANGE_DANCERCARD = 'SHORTCUT_CHANGE_DANCERCARD'
|
CHANGE_DANCERCARD = 'SHORTCUT_CHANGE_DANCERCARD'
|
||||||
|
DONT_SHOW_ANYMORE = 'SHORTCUT_DONT_SHOW_ANYMORE'
|
||||||
FAVORITE = 'SHORTCUT_FAVORITE'
|
FAVORITE = 'SHORTCUT_FAVORITE'
|
||||||
GOTO_SONGSTAB = 'SHORTCUT_GOTO_SONGSTAB'
|
GOTO_SONGSTAB = 'SHORTCUT_GOTO_SONGSTAB'
|
||||||
SKIP = 'SHORTCUT_SKIP'
|
SKIP = 'SHORTCUT_SKIP'
|
||||||
|
@ -109,6 +111,7 @@ SHORTCUT_MAPPING = {
|
||||||
Command.UPLAY,
|
Command.UPLAY,
|
||||||
],
|
],
|
||||||
JoyConButton.PLUS: [
|
JoyConButton.PLUS: [
|
||||||
|
Command.DONT_SHOW_ANYMORE,
|
||||||
Command.FAVORITE,
|
Command.FAVORITE,
|
||||||
Command.PAUSE,
|
Command.PAUSE,
|
||||||
Command.PLAYLIST_RENAME,
|
Command.PLAYLIST_RENAME,
|
||||||
|
|
|
@ -0,0 +1,3 @@
|
||||||
|
Simplified version of [tocoteron/joycon-python](https://github.com/tocoteron/joycon-python):
|
||||||
|
- Remove codes irrelevant to JoyDance.
|
||||||
|
- Fix bugs & improve stability.
|
|
@ -0,0 +1,9 @@
|
||||||
|
from .event import ButtonEventJoyCon
|
||||||
|
from .joycon import JoyCon
|
||||||
|
from .wrappers import PythonicJoyCon # as JoyCon
|
||||||
|
|
||||||
|
__all__ = [
|
||||||
|
"ButtonEventJoyCon",
|
||||||
|
"JoyCon",
|
||||||
|
"PythonicJoyCon",
|
||||||
|
]
|
|
@ -0,0 +1,4 @@
|
||||||
|
JOYCON_VENDOR_ID = 0x057E
|
||||||
|
JOYCON_L_PRODUCT_ID = 0x2006
|
||||||
|
JOYCON_R_PRODUCT_ID = 0x2007
|
||||||
|
JOYCON_PRODUCT_IDS = (JOYCON_L_PRODUCT_ID, JOYCON_R_PRODUCT_ID)
|
|
@ -0,0 +1,133 @@
|
||||||
|
from .wrappers import PythonicJoyCon
|
||||||
|
|
||||||
|
|
||||||
|
class ButtonEventJoyCon(PythonicJoyCon):
|
||||||
|
def __init__(self, *args, track_sticks=False, **kwargs):
|
||||||
|
super().__init__(*args, **kwargs)
|
||||||
|
|
||||||
|
self._events_buffer = [] # TODO: perhaps use a deque instead?
|
||||||
|
|
||||||
|
self._event_handlers = {}
|
||||||
|
self._event_track_sticks = track_sticks
|
||||||
|
|
||||||
|
self._previous_stick_l_btn = 0
|
||||||
|
self._previous_stick_r_btn = 0
|
||||||
|
self._previous_stick_r = self._previous_stick_l = (0, 0)
|
||||||
|
self._previous_r = self._previous_l = 0
|
||||||
|
self._previous_zr = self._previous_zl = 0
|
||||||
|
self._previous_plus = self._previous_minus = 0
|
||||||
|
self._previous_a = self._previous_right = 0
|
||||||
|
self._previous_b = self._previous_down = 0
|
||||||
|
self._previous_x = self._previous_up = 0
|
||||||
|
self._previous_y = self._previous_left = 0
|
||||||
|
self._previous_home = self._previous_capture = 0
|
||||||
|
self._previous_right_sr = self._previous_left_sr = 0
|
||||||
|
self._previous_right_sl = self._previous_left_sl = 0
|
||||||
|
|
||||||
|
if self.is_left():
|
||||||
|
self.register_update_hook(self._event_tracking_update_hook_left)
|
||||||
|
else:
|
||||||
|
self.register_update_hook(self._event_tracking_update_hook_right)
|
||||||
|
|
||||||
|
def joycon_button_event(self, button, state): # overridable
|
||||||
|
self._events_buffer.append((button, state))
|
||||||
|
|
||||||
|
def events(self):
|
||||||
|
while self._events_buffer:
|
||||||
|
yield self._events_buffer.pop(0)
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _event_tracking_update_hook_right(self):
|
||||||
|
if self._event_track_sticks:
|
||||||
|
pressed = self.stick_r_btn
|
||||||
|
if self._previous_stick_r_btn != pressed:
|
||||||
|
self._previous_stick_r_btn = pressed
|
||||||
|
self.joycon_button_event("stick_r_btn", pressed)
|
||||||
|
pressed = self.r
|
||||||
|
if self._previous_r != pressed:
|
||||||
|
self._previous_r = pressed
|
||||||
|
self.joycon_button_event("r", pressed)
|
||||||
|
pressed = self.zr
|
||||||
|
if self._previous_zr != pressed:
|
||||||
|
self._previous_zr = pressed
|
||||||
|
self.joycon_button_event("zr", pressed)
|
||||||
|
pressed = self.plus
|
||||||
|
if self._previous_plus != pressed:
|
||||||
|
self._previous_plus = pressed
|
||||||
|
self.joycon_button_event("plus", pressed)
|
||||||
|
pressed = self.a
|
||||||
|
if self._previous_a != pressed:
|
||||||
|
self._previous_a = pressed
|
||||||
|
self.joycon_button_event("a", pressed)
|
||||||
|
pressed = self.b
|
||||||
|
if self._previous_b != pressed:
|
||||||
|
self._previous_b = pressed
|
||||||
|
self.joycon_button_event("b", pressed)
|
||||||
|
pressed = self.x
|
||||||
|
if self._previous_x != pressed:
|
||||||
|
self._previous_x = pressed
|
||||||
|
self.joycon_button_event("x", pressed)
|
||||||
|
pressed = self.y
|
||||||
|
if self._previous_y != pressed:
|
||||||
|
self._previous_y = pressed
|
||||||
|
self.joycon_button_event("y", pressed)
|
||||||
|
pressed = self.home
|
||||||
|
if self._previous_home != pressed:
|
||||||
|
self._previous_home = pressed
|
||||||
|
self.joycon_button_event("home", pressed)
|
||||||
|
pressed = self.right_sr
|
||||||
|
if self._previous_right_sr != pressed:
|
||||||
|
self._previous_right_sr = pressed
|
||||||
|
self.joycon_button_event("right_sr", pressed)
|
||||||
|
pressed = self.right_sl
|
||||||
|
if self._previous_right_sl != pressed:
|
||||||
|
self._previous_right_sl = pressed
|
||||||
|
self.joycon_button_event("right_sl", pressed)
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _event_tracking_update_hook_left(self):
|
||||||
|
if self._event_track_sticks:
|
||||||
|
pressed = self.stick_l_btn
|
||||||
|
if self._previous_stick_l_btn != pressed:
|
||||||
|
self._previous_stick_l_btn = pressed
|
||||||
|
self.joycon_button_event("stick_l_btn", pressed)
|
||||||
|
pressed = self.l
|
||||||
|
if self._previous_l != pressed:
|
||||||
|
self._previous_l = pressed
|
||||||
|
self.joycon_button_event("l", pressed)
|
||||||
|
pressed = self.zl
|
||||||
|
if self._previous_zl != pressed:
|
||||||
|
self._previous_zl = pressed
|
||||||
|
self.joycon_button_event("zl", pressed)
|
||||||
|
pressed = self.minus
|
||||||
|
if self._previous_minus != pressed:
|
||||||
|
self._previous_minus = pressed
|
||||||
|
self.joycon_button_event("minus", pressed)
|
||||||
|
pressed = self.up
|
||||||
|
if self._previous_up != pressed:
|
||||||
|
self._previous_up = pressed
|
||||||
|
self.joycon_button_event("up", pressed)
|
||||||
|
pressed = self.down
|
||||||
|
if self._previous_down != pressed:
|
||||||
|
self._previous_down = pressed
|
||||||
|
self.joycon_button_event("down", pressed)
|
||||||
|
pressed = self.left
|
||||||
|
if self._previous_left != pressed:
|
||||||
|
self._previous_left = pressed
|
||||||
|
self.joycon_button_event("left", pressed)
|
||||||
|
pressed = self.right
|
||||||
|
if self._previous_right != pressed:
|
||||||
|
self._previous_right = pressed
|
||||||
|
self.joycon_button_event("right", pressed)
|
||||||
|
pressed = self.capture
|
||||||
|
if self._previous_capture != pressed:
|
||||||
|
self._previous_capture = pressed
|
||||||
|
self.joycon_button_event("capture", pressed)
|
||||||
|
pressed = self.left_sr
|
||||||
|
if self._previous_left_sr != pressed:
|
||||||
|
self._previous_left_sr = pressed
|
||||||
|
self.joycon_button_event("left_sr", pressed)
|
||||||
|
pressed = self.left_sl
|
||||||
|
if self._previous_left_sl != pressed:
|
||||||
|
self._previous_left_sl = pressed
|
||||||
|
self.joycon_button_event("left_sl", pressed)
|
|
@ -0,0 +1,461 @@
|
||||||
|
import time
|
||||||
|
from threading import Thread
|
||||||
|
from typing import Optional, Tuple
|
||||||
|
|
||||||
|
import hid
|
||||||
|
|
||||||
|
from .constants import (JOYCON_L_PRODUCT_ID, JOYCON_PRODUCT_IDS,
|
||||||
|
JOYCON_R_PRODUCT_ID, JOYCON_VENDOR_ID)
|
||||||
|
|
||||||
|
# TODO: disconnect, power off sequence
|
||||||
|
|
||||||
|
|
||||||
|
class JoyCon:
|
||||||
|
_INPUT_REPORT_SIZE = 49
|
||||||
|
_INPUT_REPORT_PERIOD = 0.015
|
||||||
|
_RUMBLE_DATA = b'\x00\x01\x40\x40\x00\x01\x40\x40'
|
||||||
|
|
||||||
|
vendor_id: int
|
||||||
|
product_id: int
|
||||||
|
serial: Optional[str]
|
||||||
|
simple_mode: bool
|
||||||
|
color_body: Tuple[int, int, int]
|
||||||
|
color_btn: Tuple[int, int, int]
|
||||||
|
stick_cal: Tuple[int, int, int, int, int, int, int, int]
|
||||||
|
|
||||||
|
def __init__(self, vendor_id: int, product_id: int, serial: str = None, simple_mode=False):
|
||||||
|
if vendor_id != JOYCON_VENDOR_ID:
|
||||||
|
raise ValueError(f'vendor_id is invalid: {vendor_id!r}')
|
||||||
|
|
||||||
|
if product_id not in JOYCON_PRODUCT_IDS:
|
||||||
|
raise ValueError(f'product_id is invalid: {product_id!r}')
|
||||||
|
|
||||||
|
self.vendor_id = vendor_id
|
||||||
|
self.product_id = product_id
|
||||||
|
self.serial = serial
|
||||||
|
self.simple_mode = simple_mode # TODO: It's for reporting mode 0x3f
|
||||||
|
|
||||||
|
# setup internal state
|
||||||
|
self._input_hooks = []
|
||||||
|
self._input_report = bytes(self._INPUT_REPORT_SIZE)
|
||||||
|
self._packet_number = 0
|
||||||
|
self.set_accel_calibration((0, 0, 0), (1, 1, 1))
|
||||||
|
|
||||||
|
# connect to joycon
|
||||||
|
self._joycon_device = self._open(vendor_id, product_id, serial=serial)
|
||||||
|
self._read_joycon_data()
|
||||||
|
self._setup_sensors()
|
||||||
|
|
||||||
|
# start talking with the joycon in a daemon thread
|
||||||
|
Thread(target=self._update_input_report, daemon=True).start()
|
||||||
|
|
||||||
|
def _open(self, vendor_id, product_id, serial):
|
||||||
|
try:
|
||||||
|
if hasattr(hid, "device"): # hidapi
|
||||||
|
_joycon_device = hid.device()
|
||||||
|
_joycon_device.open(vendor_id, product_id, serial)
|
||||||
|
elif hasattr(hid, "Device"): # hid
|
||||||
|
_joycon_device = hid.Device(vendor_id, product_id, serial)
|
||||||
|
else:
|
||||||
|
raise Exception("Implementation of hid is not recognized!")
|
||||||
|
except IOError as e:
|
||||||
|
raise IOError('joycon connect failed') from e
|
||||||
|
return _joycon_device
|
||||||
|
|
||||||
|
def _close(self):
|
||||||
|
if self._joycon_device:
|
||||||
|
self._joycon_device.close()
|
||||||
|
self._joycon_device = None
|
||||||
|
|
||||||
|
def _read_input_report(self) -> bytes:
|
||||||
|
if self._joycon_device:
|
||||||
|
return bytes(self._joycon_device.read(self._INPUT_REPORT_SIZE))
|
||||||
|
|
||||||
|
def _write_output_report(self, command, subcommand, argument):
|
||||||
|
if not self._joycon_device:
|
||||||
|
return
|
||||||
|
|
||||||
|
# TODO: add documentation
|
||||||
|
self._joycon_device.write(b''.join([
|
||||||
|
command,
|
||||||
|
self._packet_number.to_bytes(1, byteorder='little'),
|
||||||
|
self._RUMBLE_DATA,
|
||||||
|
subcommand,
|
||||||
|
argument,
|
||||||
|
]))
|
||||||
|
self._packet_number = (self._packet_number + 1) & 0xF
|
||||||
|
|
||||||
|
def _send_subcmd_get_response(self, subcommand, argument) -> Tuple[bool, bytes]:
|
||||||
|
# TODO: handle subcmd when daemon is running
|
||||||
|
self._write_output_report(b'\x01', subcommand, argument)
|
||||||
|
|
||||||
|
report = [0]
|
||||||
|
while report[0] != 0x21: # TODO, avoid this, await daemon instead
|
||||||
|
report = self._read_input_report()
|
||||||
|
|
||||||
|
# TODO, remove, see the todo above
|
||||||
|
assert report[1:2] != subcommand, "THREAD carefully"
|
||||||
|
|
||||||
|
# TODO: determine if the cut bytes are worth anything
|
||||||
|
|
||||||
|
return report[13] & 0x80, report[13:] # (ack, data)
|
||||||
|
|
||||||
|
def _spi_flash_read(self, address, size) -> bytes:
|
||||||
|
assert size <= 0x1d
|
||||||
|
argument = address.to_bytes(4, "little") + size.to_bytes(1, "little")
|
||||||
|
ack, report = self._send_subcmd_get_response(b'\x10', argument)
|
||||||
|
if not ack:
|
||||||
|
raise IOError("After SPI read @ {address:#06x}: got NACK")
|
||||||
|
|
||||||
|
if report[:2] != b'\x90\x10':
|
||||||
|
raise IOError("Something else than the expected ACK was recieved!")
|
||||||
|
assert report[2:7] == argument, (report[2:5], argument)
|
||||||
|
|
||||||
|
return report[7:size + 7]
|
||||||
|
|
||||||
|
def _update_input_report(self): # daemon thread
|
||||||
|
try:
|
||||||
|
while self._joycon_device:
|
||||||
|
report = [0]
|
||||||
|
# TODO, handle input reports of type 0x21 and 0x3f
|
||||||
|
while report[0] != 0x30:
|
||||||
|
report = self._read_input_report()
|
||||||
|
|
||||||
|
self._input_report = report
|
||||||
|
|
||||||
|
# Call input hooks in a different thread
|
||||||
|
Thread(target=self._input_hook_caller, daemon=True).start()
|
||||||
|
except OSError:
|
||||||
|
print('connection closed')
|
||||||
|
pass
|
||||||
|
|
||||||
|
def _input_hook_caller(self):
|
||||||
|
for callback in self._input_hooks:
|
||||||
|
callback(self)
|
||||||
|
|
||||||
|
def _read_joycon_data(self):
|
||||||
|
color_data = self._spi_flash_read(0x6050, 6)
|
||||||
|
self.color_body = tuple(color_data[:3])
|
||||||
|
self.color_btn = tuple(color_data[3:])
|
||||||
|
|
||||||
|
self._read_stick_calibration_data()
|
||||||
|
|
||||||
|
buf = self._spi_flash_read(0x6086 if self.is_left() else 0x6098, 16)
|
||||||
|
self.deadzone = (buf[4] << 8) & 0xF00 | buf[3]
|
||||||
|
|
||||||
|
# user IME data
|
||||||
|
if self._spi_flash_read(0x8026, 2) == b"\xB2\xA1":
|
||||||
|
# print(f"Calibrate {self.serial} IME with user data")
|
||||||
|
imu_cal = self._spi_flash_read(0x8028, 24)
|
||||||
|
|
||||||
|
# factory IME data
|
||||||
|
else:
|
||||||
|
# print(f"Calibrate {self.serial} IME with factory data")
|
||||||
|
imu_cal = self._spi_flash_read(0x6020, 24)
|
||||||
|
|
||||||
|
self.set_accel_calibration((
|
||||||
|
self._to_int16le_from_2bytes(imu_cal[0], imu_cal[1]),
|
||||||
|
self._to_int16le_from_2bytes(imu_cal[2], imu_cal[3]),
|
||||||
|
self._to_int16le_from_2bytes(imu_cal[4], imu_cal[5]),
|
||||||
|
), (
|
||||||
|
self._to_int16le_from_2bytes(imu_cal[6], imu_cal[7]),
|
||||||
|
self._to_int16le_from_2bytes(imu_cal[8], imu_cal[9]),
|
||||||
|
self._to_int16le_from_2bytes(imu_cal[10], imu_cal[11]),
|
||||||
|
))
|
||||||
|
|
||||||
|
def _read_stick_calibration_data(self):
|
||||||
|
user_stick_cal_addr = 0x8012 if self.is_left() else 0x801D
|
||||||
|
buf = self._spi_flash_read(user_stick_cal_addr, 9)
|
||||||
|
use_user_data = False
|
||||||
|
|
||||||
|
for b in buf:
|
||||||
|
if b != 0xFF:
|
||||||
|
use_user_data = True
|
||||||
|
break
|
||||||
|
|
||||||
|
if not use_user_data:
|
||||||
|
factory_stick_cal_addr = 0x603D if self.is_left() else 0x6046
|
||||||
|
buf = self._spi_flash_read(factory_stick_cal_addr, 9)
|
||||||
|
|
||||||
|
self.stick_cal = [0] * 6
|
||||||
|
|
||||||
|
# X Axis Max above center
|
||||||
|
self.stick_cal[0 if self.is_left() else 2] = (buf[1] << 8) & 0xF00 | buf[0]
|
||||||
|
# Y Axis Max above center
|
||||||
|
self.stick_cal[1 if self.is_left() else 3] = (buf[2] << 4) | (buf[1] >> 4)
|
||||||
|
# X Axis Center
|
||||||
|
self.stick_cal[2 if self.is_left() else 4] = (buf[4] << 8) & 0xF00 | buf[3]
|
||||||
|
# Y Axis Center
|
||||||
|
self.stick_cal[3 if self.is_left() else 5] = (buf[5] << 4) | (buf[4] >> 4)
|
||||||
|
# X Axis Min below center
|
||||||
|
self.stick_cal[4 if self.is_left() else 0] = (buf[7] << 8) & 0xF00 | buf[6]
|
||||||
|
# Y Axis Min below center
|
||||||
|
self.stick_cal[5 if self.is_left() else 1] = (buf[8] << 4) | (buf[7] >> 4)
|
||||||
|
|
||||||
|
def _setup_sensors(self):
|
||||||
|
# Enable 6 axis sensors
|
||||||
|
self._write_output_report(b'\x01', b'\x40', b'\x01')
|
||||||
|
# It needs delta time to update the setting
|
||||||
|
time.sleep(0.02)
|
||||||
|
# Change format of input report
|
||||||
|
self._write_output_report(b'\x01', b'\x03', b'\x30')
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _to_int16le_from_2bytes(hbytebe, lbytebe):
|
||||||
|
uint16le = (lbytebe << 8) | hbytebe
|
||||||
|
int16le = uint16le if uint16le < 32768 else uint16le - 65536
|
||||||
|
return int16le
|
||||||
|
|
||||||
|
def _get_nbit_from_input_report(self, offset_byte, offset_bit, nbit):
|
||||||
|
byte = self._input_report[offset_byte]
|
||||||
|
return (byte >> offset_bit) & ((1 << nbit) - 1)
|
||||||
|
|
||||||
|
def __del__(self):
|
||||||
|
self._close()
|
||||||
|
|
||||||
|
def set_accel_calibration(self, offset_xyz=None, coeff_xyz=None):
|
||||||
|
if offset_xyz and coeff_xyz:
|
||||||
|
self._ACCEL_OFFSET_X, self._ACCEL_OFFSET_Y, self._ACCEL_OFFSET_Z = offset_xyz
|
||||||
|
|
||||||
|
cx, cy, cz = coeff_xyz
|
||||||
|
self._ACCEL_COEFF_X = (1.0 / (cx - self._ACCEL_OFFSET_X)) * 4.0
|
||||||
|
self._ACCEL_COEFF_Y = (1.0 / (cy - self._ACCEL_OFFSET_Y)) * 4.0
|
||||||
|
self._ACCEL_COEFF_Z = (1.0 / (cz - self._ACCEL_OFFSET_Z)) * 4.0
|
||||||
|
|
||||||
|
def get_actual_stick_value(self, pre_cal, orientation): # X/Horizontal = 0, Y/Vertical = 1
|
||||||
|
diff = pre_cal - self.stick_cal[2 + orientation]
|
||||||
|
if (abs(diff) < self.deadzone):
|
||||||
|
return 0
|
||||||
|
elif diff > 0: # Axis is above center
|
||||||
|
return diff / self.stick_cal[orientation]
|
||||||
|
else:
|
||||||
|
return diff / self.stick_cal[4 + orientation]
|
||||||
|
|
||||||
|
def register_update_hook(self, callback):
|
||||||
|
self._input_hooks.append(callback)
|
||||||
|
return callback # this makes it so you could use it as a decorator
|
||||||
|
|
||||||
|
def is_left(self):
|
||||||
|
return self.product_id == JOYCON_L_PRODUCT_ID
|
||||||
|
|
||||||
|
def is_right(self):
|
||||||
|
return self.product_id == JOYCON_R_PRODUCT_ID
|
||||||
|
|
||||||
|
def get_battery_charging(self):
|
||||||
|
return self._get_nbit_from_input_report(2, 4, 1)
|
||||||
|
|
||||||
|
def get_battery_level(self):
|
||||||
|
return self._get_nbit_from_input_report(2, 5, 3)
|
||||||
|
|
||||||
|
def get_button_y(self):
|
||||||
|
return self._get_nbit_from_input_report(3, 0, 1)
|
||||||
|
|
||||||
|
def get_button_x(self):
|
||||||
|
return self._get_nbit_from_input_report(3, 1, 1)
|
||||||
|
|
||||||
|
def get_button_b(self):
|
||||||
|
return self._get_nbit_from_input_report(3, 2, 1)
|
||||||
|
|
||||||
|
def get_button_a(self):
|
||||||
|
return self._get_nbit_from_input_report(3, 3, 1)
|
||||||
|
|
||||||
|
def get_button_right_sr(self):
|
||||||
|
return self._get_nbit_from_input_report(3, 4, 1)
|
||||||
|
|
||||||
|
def get_button_right_sl(self):
|
||||||
|
return self._get_nbit_from_input_report(3, 5, 1)
|
||||||
|
|
||||||
|
def get_button_r(self):
|
||||||
|
return self._get_nbit_from_input_report(3, 6, 1)
|
||||||
|
|
||||||
|
def get_button_zr(self):
|
||||||
|
return self._get_nbit_from_input_report(3, 7, 1)
|
||||||
|
|
||||||
|
def get_button_minus(self):
|
||||||
|
return self._get_nbit_from_input_report(4, 0, 1)
|
||||||
|
|
||||||
|
def get_button_plus(self):
|
||||||
|
return self._get_nbit_from_input_report(4, 1, 1)
|
||||||
|
|
||||||
|
def get_button_r_stick(self):
|
||||||
|
return self._get_nbit_from_input_report(4, 2, 1)
|
||||||
|
|
||||||
|
def get_button_l_stick(self):
|
||||||
|
return self._get_nbit_from_input_report(4, 3, 1)
|
||||||
|
|
||||||
|
def get_button_home(self):
|
||||||
|
return self._get_nbit_from_input_report(4, 4, 1)
|
||||||
|
|
||||||
|
def get_button_capture(self):
|
||||||
|
return self._get_nbit_from_input_report(4, 5, 1)
|
||||||
|
|
||||||
|
def get_button_charging_grip(self):
|
||||||
|
return self._get_nbit_from_input_report(4, 7, 1)
|
||||||
|
|
||||||
|
def get_button_down(self):
|
||||||
|
return self._get_nbit_from_input_report(5, 0, 1)
|
||||||
|
|
||||||
|
def get_button_up(self):
|
||||||
|
return self._get_nbit_from_input_report(5, 1, 1)
|
||||||
|
|
||||||
|
def get_button_right(self):
|
||||||
|
return self._get_nbit_from_input_report(5, 2, 1)
|
||||||
|
|
||||||
|
def get_button_left(self):
|
||||||
|
return self._get_nbit_from_input_report(5, 3, 1)
|
||||||
|
|
||||||
|
def get_button_left_sr(self):
|
||||||
|
return self._get_nbit_from_input_report(5, 4, 1)
|
||||||
|
|
||||||
|
def get_button_left_sl(self):
|
||||||
|
return self._get_nbit_from_input_report(5, 5, 1)
|
||||||
|
|
||||||
|
def get_button_l(self):
|
||||||
|
return self._get_nbit_from_input_report(5, 6, 1)
|
||||||
|
|
||||||
|
def get_button_zl(self):
|
||||||
|
return self._get_nbit_from_input_report(5, 7, 1)
|
||||||
|
|
||||||
|
def get_stick_left_horizontal(self):
|
||||||
|
if not self.is_left():
|
||||||
|
return 0
|
||||||
|
|
||||||
|
pre_cal = self._get_nbit_from_input_report(6, 0, 8) \
|
||||||
|
| (self._get_nbit_from_input_report(7, 0, 4) << 8)
|
||||||
|
return self.get_actual_stick_value(pre_cal, 0)
|
||||||
|
|
||||||
|
def get_stick_left_vertical(self):
|
||||||
|
if not self.is_left():
|
||||||
|
return 0
|
||||||
|
|
||||||
|
pre_cal = self._get_nbit_from_input_report(7, 4, 4) \
|
||||||
|
| (self._get_nbit_from_input_report(8, 0, 8) << 4)
|
||||||
|
return self.get_actual_stick_value(pre_cal, 1)
|
||||||
|
|
||||||
|
def get_stick_right_horizontal(self):
|
||||||
|
if self.is_left():
|
||||||
|
return 0
|
||||||
|
|
||||||
|
pre_cal = self._get_nbit_from_input_report(9, 0, 8) \
|
||||||
|
| (self._get_nbit_from_input_report(10, 0, 4) << 8)
|
||||||
|
return self.get_actual_stick_value(pre_cal, 0)
|
||||||
|
|
||||||
|
def get_stick_right_vertical(self):
|
||||||
|
if self.is_left():
|
||||||
|
return 0
|
||||||
|
|
||||||
|
pre_cal = self._get_nbit_from_input_report(10, 4, 4) \
|
||||||
|
| (self._get_nbit_from_input_report(11, 0, 8) << 4)
|
||||||
|
return self.get_actual_stick_value(pre_cal, 1)
|
||||||
|
|
||||||
|
def get_accels(self):
|
||||||
|
input_report = bytes(self._input_report)
|
||||||
|
accels = []
|
||||||
|
|
||||||
|
for idx in range(3):
|
||||||
|
x = self.get_accel_x(input_report, sample_idx=idx)
|
||||||
|
y = self.get_accel_y(input_report, sample_idx=idx)
|
||||||
|
z = self.get_accel_z(input_report, sample_idx=idx)
|
||||||
|
accels.append((x, y, z))
|
||||||
|
|
||||||
|
return accels
|
||||||
|
|
||||||
|
def get_accel_x(self, input_report=None, sample_idx=0):
|
||||||
|
if not input_report:
|
||||||
|
input_report = self._input_report
|
||||||
|
|
||||||
|
if sample_idx not in (0, 1, 2):
|
||||||
|
raise IndexError('sample_idx should be between 0 and 2')
|
||||||
|
data = self._to_int16le_from_2bytes(
|
||||||
|
input_report[13 + sample_idx * 12],
|
||||||
|
input_report[14 + sample_idx * 12])
|
||||||
|
return data * self._ACCEL_COEFF_X
|
||||||
|
|
||||||
|
def get_accel_y(self, input_report=None, sample_idx=0):
|
||||||
|
if not input_report:
|
||||||
|
input_report = self._input_report
|
||||||
|
|
||||||
|
if sample_idx not in (0, 1, 2):
|
||||||
|
raise IndexError('sample_idx should be between 0 and 2')
|
||||||
|
data = self._to_int16le_from_2bytes(
|
||||||
|
input_report[15 + sample_idx * 12],
|
||||||
|
input_report[16 + sample_idx * 12])
|
||||||
|
return data * self._ACCEL_COEFF_Y * (1 if self.is_left() else -1)
|
||||||
|
|
||||||
|
def get_accel_z(self, input_report=None, sample_idx=0):
|
||||||
|
if not input_report:
|
||||||
|
input_report = self._input_report
|
||||||
|
|
||||||
|
if sample_idx not in (0, 1, 2):
|
||||||
|
raise IndexError('sample_idx should be between 0 and 2')
|
||||||
|
data = self._to_int16le_from_2bytes(
|
||||||
|
input_report[17 + sample_idx * 12],
|
||||||
|
input_report[18 + sample_idx * 12])
|
||||||
|
return data * self._ACCEL_COEFF_Z * (1 if self.is_left() else -1)
|
||||||
|
|
||||||
|
def get_status(self) -> dict:
|
||||||
|
return {
|
||||||
|
"battery": {
|
||||||
|
"charging": self.get_battery_charging(),
|
||||||
|
"level": self.get_battery_level(),
|
||||||
|
},
|
||||||
|
"buttons": {
|
||||||
|
"right": {
|
||||||
|
"y": self.get_button_y(),
|
||||||
|
"x": self.get_button_x(),
|
||||||
|
"b": self.get_button_b(),
|
||||||
|
"a": self.get_button_a(),
|
||||||
|
"sr": self.get_button_right_sr(),
|
||||||
|
"sl": self.get_button_right_sl(),
|
||||||
|
"r": self.get_button_r(),
|
||||||
|
"zr": self.get_button_zr(),
|
||||||
|
},
|
||||||
|
"shared": {
|
||||||
|
"minus": self.get_button_minus(),
|
||||||
|
"plus": self.get_button_plus(),
|
||||||
|
"r-stick": self.get_button_r_stick(),
|
||||||
|
"l-stick": self.get_button_l_stick(),
|
||||||
|
"home": self.get_button_home(),
|
||||||
|
"capture": self.get_button_capture(),
|
||||||
|
"charging-grip": self.get_button_charging_grip(),
|
||||||
|
},
|
||||||
|
"left": {
|
||||||
|
"down": self.get_button_down(),
|
||||||
|
"up": self.get_button_up(),
|
||||||
|
"right": self.get_button_right(),
|
||||||
|
"left": self.get_button_left(),
|
||||||
|
"sr": self.get_button_left_sr(),
|
||||||
|
"sl": self.get_button_left_sl(),
|
||||||
|
"l": self.get_button_l(),
|
||||||
|
"zl": self.get_button_zl(),
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"analog-sticks": {
|
||||||
|
"left": {
|
||||||
|
"horizontal": self.get_stick_left_horizontal(),
|
||||||
|
"vertical": self.get_stick_left_vertical(),
|
||||||
|
},
|
||||||
|
"right": {
|
||||||
|
"horizontal": self.get_stick_right_horizontal(),
|
||||||
|
"vertical": self.get_stick_right_vertical(),
|
||||||
|
},
|
||||||
|
},
|
||||||
|
"accel": self.get_accels(),
|
||||||
|
}
|
||||||
|
|
||||||
|
def disconnect_device(self):
|
||||||
|
self._write_output_report(b'\x01', b'\x06', b'\x00')
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
import pyjoycon.device as d
|
||||||
|
ids = d.get_L_id() if None not in d.get_L_id() else d.get_R_id()
|
||||||
|
|
||||||
|
if None not in ids:
|
||||||
|
joycon = JoyCon(*ids)
|
||||||
|
lamp_pattern = 0
|
||||||
|
while True:
|
||||||
|
print(joycon.get_status())
|
||||||
|
joycon.set_player_lamp_on(lamp_pattern)
|
||||||
|
lamp_pattern = (lamp_pattern + 1) & 0xf
|
||||||
|
time.sleep(0.2)
|
|
@ -0,0 +1,88 @@
|
||||||
|
from .joycon import JoyCon
|
||||||
|
|
||||||
|
|
||||||
|
# Preferably, this class gets merged into the
|
||||||
|
# parent class if approved by the original author
|
||||||
|
class PythonicJoyCon(JoyCon):
|
||||||
|
"""
|
||||||
|
A wrapper class for the JoyCon parent class.
|
||||||
|
This creates a more pythonic interface by
|
||||||
|
* using properties instead of requiring java-style getters and setters,
|
||||||
|
* bundles related xy/xyz data in tuples
|
||||||
|
* bundles the multiple measurements of the
|
||||||
|
gyroscope and accelerometer into a list
|
||||||
|
* Adds the option to invert the y and z axis of the left joycon
|
||||||
|
to make it match the right joycon. This is enabled by default
|
||||||
|
"""
|
||||||
|
|
||||||
|
def __init__(self, *a, invert_left_ime_yz=True, **kw):
|
||||||
|
super().__init__(*a, **kw)
|
||||||
|
self._ime_yz_coeff = -1 if invert_left_ime_yz and self.is_left() else 1
|
||||||
|
|
||||||
|
is_charging = property(JoyCon.get_battery_charging)
|
||||||
|
battery_level = property(JoyCon.get_battery_level)
|
||||||
|
|
||||||
|
r = property(JoyCon.get_button_r)
|
||||||
|
zr = property(JoyCon.get_button_zr)
|
||||||
|
plus = property(JoyCon.get_button_plus)
|
||||||
|
a = property(JoyCon.get_button_a)
|
||||||
|
b = property(JoyCon.get_button_b)
|
||||||
|
x = property(JoyCon.get_button_x)
|
||||||
|
y = property(JoyCon.get_button_y)
|
||||||
|
stick_r_btn = property(JoyCon.get_button_r_stick)
|
||||||
|
home = property(JoyCon.get_button_home)
|
||||||
|
right_sr = property(JoyCon.get_button_right_sr)
|
||||||
|
right_sl = property(JoyCon.get_button_right_sl)
|
||||||
|
|
||||||
|
l = property(JoyCon.get_button_l) # noqa: E741
|
||||||
|
zl = property(JoyCon.get_button_zl)
|
||||||
|
minus = property(JoyCon.get_button_minus)
|
||||||
|
stick_l_btn = property(JoyCon.get_button_l_stick)
|
||||||
|
up = property(JoyCon.get_button_up)
|
||||||
|
down = property(JoyCon.get_button_down)
|
||||||
|
left = property(JoyCon.get_button_left)
|
||||||
|
right = property(JoyCon.get_button_right)
|
||||||
|
capture = property(JoyCon.get_button_capture)
|
||||||
|
left_sr = property(JoyCon.get_button_left_sr)
|
||||||
|
left_sl = property(JoyCon.get_button_left_sl)
|
||||||
|
|
||||||
|
disconnect = JoyCon.disconnect_device
|
||||||
|
|
||||||
|
@property
|
||||||
|
def stick_l(self):
|
||||||
|
return (
|
||||||
|
self.get_stick_left_horizontal(),
|
||||||
|
self.get_stick_left_vertical(),
|
||||||
|
)
|
||||||
|
|
||||||
|
@property
|
||||||
|
def stick_r(self):
|
||||||
|
return (
|
||||||
|
self.get_stick_right_horizontal(),
|
||||||
|
self.get_stick_right_vertical(),
|
||||||
|
)
|
||||||
|
|
||||||
|
@property
|
||||||
|
def accel(self):
|
||||||
|
c = self._ime_yz_coeff
|
||||||
|
return [
|
||||||
|
(
|
||||||
|
self.get_accel_x(i),
|
||||||
|
self.get_accel_y(i) * c,
|
||||||
|
self.get_accel_z(i) * c,
|
||||||
|
)
|
||||||
|
for i in range(3)
|
||||||
|
]
|
||||||
|
|
||||||
|
@property
|
||||||
|
def accel_in_g(self):
|
||||||
|
c = 4.0 / 0x4000
|
||||||
|
c2 = c * self._ime_yz_coeff
|
||||||
|
return [
|
||||||
|
(
|
||||||
|
self.get_accel_x(i) * c,
|
||||||
|
self.get_accel_y(i) * c2,
|
||||||
|
self.get_accel_z(i) * c2,
|
||||||
|
)
|
||||||
|
for i in range(3)
|
||||||
|
]
|
|
@ -1,5 +1,3 @@
|
||||||
https://github.com/redphx/joycon-python/archive/refs/tags/0.3.zip#egg=joycon-python
|
|
||||||
websockets==10.2
|
websockets==10.2
|
||||||
aiohttp==3.8.1
|
aiohttp==3.8.1
|
||||||
hidapi==0.11.2
|
hidapi==0.11.2
|
||||||
pyglm==2.5.7
|
|
||||||
|
|
Loading…
Reference in New Issue