Add joycon-python source code
This commit is contained in:
redphx
parent
749a2a92fe
commit
3d00015607
|
|
@ -0,0 +1,25 @@
|
|||
from .joycon import JoyCon
|
||||
from .wrappers import PythonicJoyCon # as JoyCon
|
||||
from .gyro import GyroTrackingJoyCon
|
||||
from .event import ButtonEventJoyCon
|
||||
from .device import get_device_ids, get_ids_of_type
|
||||
from .device import is_id_L
|
||||
from .device import get_R_ids, get_L_ids
|
||||
from .device import get_R_id, get_L_id
|
||||
|
||||
|
||||
__version__ = "0.2.4"
|
||||
|
||||
__all__ = [
|
||||
"ButtonEventJoyCon",
|
||||
"GyroTrackingJoyCon",
|
||||
"JoyCon",
|
||||
"PythonicJoyCon",
|
||||
"get_L_id",
|
||||
"get_L_ids",
|
||||
"get_R_id",
|
||||
"get_R_ids",
|
||||
"get_device_ids",
|
||||
"get_ids_of_type",
|
||||
"is_id_L",
|
||||
]
|
||||
|
|
@ -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,77 @@
|
|||
import hid
|
||||
from .constants import JOYCON_VENDOR_ID, JOYCON_PRODUCT_IDS
|
||||
from .constants import JOYCON_L_PRODUCT_ID, JOYCON_R_PRODUCT_ID
|
||||
|
||||
|
||||
def get_device_ids(debug=False):
|
||||
"""
|
||||
returns a list of tuples like `(vendor_id, product_id, serial_number)`
|
||||
"""
|
||||
devices = hid.enumerate(0, 0)
|
||||
|
||||
out = []
|
||||
for device in devices:
|
||||
vendor_id = device["vendor_id"]
|
||||
product_id = device["product_id"]
|
||||
product_string = device["product_string"]
|
||||
serial = device.get('serial') or device.get("serial_number")
|
||||
|
||||
if vendor_id != JOYCON_VENDOR_ID:
|
||||
continue
|
||||
if product_id not in JOYCON_PRODUCT_IDS:
|
||||
continue
|
||||
if not product_string:
|
||||
continue
|
||||
|
||||
out.append((vendor_id, product_id, serial))
|
||||
|
||||
if debug:
|
||||
print(product_string)
|
||||
print(f"\tvendor_id is {vendor_id!r}")
|
||||
print(f"\tproduct_id is {product_id!r}")
|
||||
print(f"\tserial is {serial!r}")
|
||||
|
||||
return out
|
||||
|
||||
|
||||
def is_id_L(id):
|
||||
return id[1] == JOYCON_L_PRODUCT_ID
|
||||
|
||||
|
||||
def get_ids_of_type(lr, **kw):
|
||||
"""
|
||||
returns a list of tuples like `(vendor_id, product_id, serial_number)`
|
||||
|
||||
arg: lr : str : put `R` or `L`
|
||||
"""
|
||||
if lr.lower() == "l":
|
||||
product_id = JOYCON_L_PRODUCT_ID
|
||||
else:
|
||||
product_id = JOYCON_R_PRODUCT_ID
|
||||
return [i for i in get_device_ids(**kw) if i[1] == product_id]
|
||||
|
||||
|
||||
def get_R_ids(**kw):
|
||||
"""returns a list of tuple like `(vendor_id, product_id, serial_number)`"""
|
||||
return get_ids_of_type("R", **kw)
|
||||
|
||||
|
||||
def get_L_ids(**kw):
|
||||
"""returns a list of tuple like `(vendor_id, product_id, serial_number)`"""
|
||||
return get_ids_of_type("L", **kw)
|
||||
|
||||
|
||||
def get_R_id(**kw):
|
||||
"""returns a tuple like `(vendor_id, product_id, serial_number)`"""
|
||||
ids = get_R_ids(**kw)
|
||||
if not ids:
|
||||
return (None, None, None)
|
||||
return ids[0]
|
||||
|
||||
|
||||
def get_L_id(**kw):
|
||||
"""returns a tuple like `(vendor_id, product_id, serial_number)`"""
|
||||
ids = get_L_ids(**kw)
|
||||
if not ids:
|
||||
return (None, None, None)
|
||||
return ids[0]
|
||||
|
|
@ -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,84 @@
|
|||
from .wrappers import PythonicJoyCon
|
||||
from glm import vec2, vec3, quat, angleAxis, eulerAngles
|
||||
from typing import Optional
|
||||
import time
|
||||
|
||||
|
||||
class GyroTrackingJoyCon(PythonicJoyCon):
|
||||
"""
|
||||
A specialized class based on PythonicJoyCon which tracks the gyroscope data
|
||||
and deduces the current rotation of the JoyCon. Can be used to create a
|
||||
pointer rotate an object or pointin a direction. Comes with the need to be
|
||||
calibrated.
|
||||
"""
|
||||
def __init__(self, *args, **kwargs):
|
||||
super().__init__(*args, simple_mode=False, **kwargs)
|
||||
|
||||
# set internal state:
|
||||
self.reset_orientation()
|
||||
|
||||
# register the update callback
|
||||
self.register_update_hook(self._gyro_update_hook)
|
||||
|
||||
@property
|
||||
def pointer(self) -> Optional[vec2]:
|
||||
d = self.direction
|
||||
if d.x <= 0:
|
||||
return None
|
||||
return vec2(d.y, -d.z) / d.x
|
||||
|
||||
@property
|
||||
def direction(self) -> vec3:
|
||||
return self.direction_X
|
||||
|
||||
@property
|
||||
def rotation(self) -> vec3:
|
||||
return -eulerAngles(self.direction_Q)
|
||||
|
||||
is_calibrating = False
|
||||
|
||||
def calibrate(self, seconds=2):
|
||||
self.calibration_acumulator = vec3(0)
|
||||
self.calibration_acumulations = 0
|
||||
self.is_calibrating = time.time() + seconds
|
||||
|
||||
def _set_calibration(self, gyro_offset=None):
|
||||
if not gyro_offset:
|
||||
c = vec3(1, self._ime_yz_coeff, self._ime_yz_coeff)
|
||||
gyro_offset = self.calibration_acumulator * c
|
||||
gyro_offset /= self.calibration_acumulations
|
||||
gyro_offset += vec3(
|
||||
self._GYRO_OFFSET_X,
|
||||
self._GYRO_OFFSET_Y,
|
||||
self._GYRO_OFFSET_Z,
|
||||
)
|
||||
self.is_calibrating = False
|
||||
self.set_gyro_calibration(gyro_offset)
|
||||
|
||||
def reset_orientation(self):
|
||||
self.direction_X = vec3(1, 0, 0)
|
||||
self.direction_Y = vec3(0, 1, 0)
|
||||
self.direction_Z = vec3(0, 0, 1)
|
||||
self.direction_Q = quat()
|
||||
|
||||
@staticmethod
|
||||
def _gyro_update_hook(self):
|
||||
if self.is_calibrating:
|
||||
if self.is_calibrating < time.time():
|
||||
self._set_calibration()
|
||||
else:
|
||||
for xyz in self.gyro:
|
||||
self.calibration_acumulator += xyz
|
||||
self.calibration_acumulations += 3
|
||||
|
||||
for gx, gy, gz in self.gyro_in_rad:
|
||||
# TODO: find out why 1/86 works, and not 1/60 or 1/(60*30)
|
||||
rotation \
|
||||
= angleAxis(gx * (-1/86), self.direction_X) \
|
||||
* angleAxis(gy * (-1/86), self.direction_Y) \
|
||||
* angleAxis(gz * (-1/86), self.direction_Z)
|
||||
|
||||
self.direction_X *= rotation
|
||||
self.direction_Y *= rotation
|
||||
self.direction_Z *= rotation
|
||||
self.direction_Q *= rotation
|
||||
|
|
@ -0,0 +1,530 @@
|
|||
from .constants import JOYCON_VENDOR_ID, JOYCON_PRODUCT_IDS
|
||||
from .constants import JOYCON_L_PRODUCT_ID, JOYCON_R_PRODUCT_ID
|
||||
import hid
|
||||
import time
|
||||
import threading
|
||||
from typing import Optional
|
||||
|
||||
# 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 : (int, int, int)
|
||||
color_btn : (int, int, int)
|
||||
stick_cal : [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))
|
||||
self.set_gyro_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
|
||||
self._update_input_report_thread \
|
||||
= threading.Thread(target=self._update_input_report)
|
||||
self._update_input_report_thread.setDaemon(True)
|
||||
self._update_input_report_thread.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) -> (bool, bytes):
|
||||
# TODO: handle subcmd when daemon is running
|
||||
self._write_output_report(b'\x01', subcommand, argument)
|
||||
|
||||
report = self._read_input_report()
|
||||
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 = self._read_input_report()
|
||||
# TODO, handle input reports of type 0x21 and 0x3f
|
||||
while report[0] != 0x30:
|
||||
report = self._read_input_report()
|
||||
|
||||
self._input_report = report
|
||||
|
||||
for callback in self._input_hooks:
|
||||
callback(self)
|
||||
except OSError:
|
||||
print('connection closed')
|
||||
pass
|
||||
|
||||
def _read_joycon_data(self):
|
||||
color_data = self._spi_flash_read(0x6050, 6)
|
||||
|
||||
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.color_body = tuple(color_data[:3])
|
||||
self.color_btn = tuple(color_data[3:])
|
||||
|
||||
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]),
|
||||
)
|
||||
)
|
||||
self.set_gyro_calibration((
|
||||
self._to_int16le_from_2bytes(imu_cal[12], imu_cal[13]),
|
||||
self._to_int16le_from_2bytes(imu_cal[14], imu_cal[15]),
|
||||
self._to_int16le_from_2bytes(imu_cal[16], imu_cal[17]),
|
||||
), (
|
||||
self._to_int16le_from_2bytes(imu_cal[18], imu_cal[19]),
|
||||
self._to_int16le_from_2bytes(imu_cal[20], imu_cal[21]),
|
||||
self._to_int16le_from_2bytes(imu_cal[22], imu_cal[23]),
|
||||
)
|
||||
)
|
||||
|
||||
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_gyro_calibration(self, offset_xyz=None, coeff_xyz=None):
|
||||
if offset_xyz:
|
||||
self._GYRO_OFFSET_X, \
|
||||
self._GYRO_OFFSET_Y, \
|
||||
self._GYRO_OFFSET_Z = offset_xyz
|
||||
if coeff_xyz:
|
||||
cx, cy, cz = coeff_xyz
|
||||
self._GYRO_COEFF_X = 0x343b / cx if cx != 0x343b else 1
|
||||
self._GYRO_COEFF_Y = 0x343b / cy if cy != 0x343b else 1
|
||||
self._GYRO_COEFF_Z = 0x343b / cz if cz != 0x343b else 1
|
||||
|
||||
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)
|
||||
|
||||
x = self.get_accel_x(input_report)
|
||||
y = self.get_accel_y(input_report)
|
||||
z = self.get_accel_z(input_report)
|
||||
|
||||
return (x, y, z)
|
||||
|
||||
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_gyro_x(self, sample_idx=0):
|
||||
if sample_idx not in (0, 1, 2):
|
||||
raise IndexError('sample_idx should be between 0 and 2')
|
||||
data = self._to_int16le_from_2bytes(
|
||||
self._input_report[19 + sample_idx * 12],
|
||||
self._input_report[20 + sample_idx * 12])
|
||||
return (data - self._GYRO_OFFSET_X) * self._GYRO_COEFF_X
|
||||
|
||||
def get_gyro_y(self, sample_idx=0):
|
||||
if sample_idx not in (0, 1, 2):
|
||||
raise IndexError('sample_idx should be between 0 and 2')
|
||||
data = self._to_int16le_from_2bytes(
|
||||
self._input_report[21 + sample_idx * 12],
|
||||
self._input_report[22 + sample_idx * 12])
|
||||
return (data - self._GYRO_OFFSET_Y) * self._GYRO_COEFF_Y
|
||||
|
||||
def get_gyro_z(self, sample_idx=0):
|
||||
if sample_idx not in (0, 1, 2):
|
||||
raise IndexError('sample_idx should be between 0 and 2')
|
||||
data = self._to_int16le_from_2bytes(
|
||||
self._input_report[23 + sample_idx * 12],
|
||||
self._input_report[24 + sample_idx * 12])
|
||||
return (data - self._GYRO_OFFSET_Z) * self._GYRO_COEFF_Z
|
||||
|
||||
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": {
|
||||
"x": self.get_accel_x(),
|
||||
"y": self.get_accel_y(),
|
||||
"z": self.get_accel_z(),
|
||||
},
|
||||
"gyro": {
|
||||
"x": self.get_gyro_x(),
|
||||
"y": self.get_gyro_y(),
|
||||
"z": self.get_gyro_z(),
|
||||
},
|
||||
}
|
||||
|
||||
def set_player_lamp_on(self, on_pattern: int):
|
||||
self._write_output_report(
|
||||
b'\x01', b'\x30',
|
||||
(on_pattern & 0xF).to_bytes(1, byteorder='little'))
|
||||
|
||||
def set_player_lamp_flashing(self, flashing_pattern: int):
|
||||
self._write_output_report(
|
||||
b'\x01', b'\x30',
|
||||
((flashing_pattern & 0xF) << 4).to_bytes(1, byteorder='little'))
|
||||
|
||||
def set_player_lamp(self, pattern: int):
|
||||
self._write_output_report(
|
||||
b'\x01', b'\x30',
|
||||
pattern.to_bytes(1, byteorder='little'))
|
||||
|
||||
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,142 @@
|
|||
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)
|
||||
|
||||
set_led_on = JoyCon.set_player_lamp_on
|
||||
set_led_flashing = JoyCon.set_player_lamp_flashing
|
||||
set_led = JoyCon.set_player_lamp
|
||||
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)
|
||||
]
|
||||
|
||||
@property
|
||||
def gyro(self):
|
||||
c = self._ime_yz_coeff
|
||||
return [
|
||||
(
|
||||
self.get_gyro_x(i),
|
||||
self.get_gyro_y(i) * c,
|
||||
self.get_gyro_z(i) * c,
|
||||
)
|
||||
for i in range(3)
|
||||
]
|
||||
|
||||
@property
|
||||
def gyro_in_deg(self):
|
||||
c = 0.06103
|
||||
c2 = c * self._ime_yz_coeff
|
||||
return [
|
||||
(
|
||||
self.get_gyro_x(i) * c,
|
||||
self.get_gyro_y(i) * c2,
|
||||
self.get_gyro_z(i) * c2,
|
||||
)
|
||||
for i in range(3)
|
||||
]
|
||||
|
||||
@property
|
||||
def gyro_in_rad(self):
|
||||
c = 0.0001694 * 3.1415926536
|
||||
c2 = c * self._ime_yz_coeff
|
||||
return [
|
||||
(
|
||||
self.get_gyro_x(i) * c,
|
||||
self.get_gyro_y(i) * c2,
|
||||
self.get_gyro_z(i) * c2,
|
||||
)
|
||||
for i in range(3)
|
||||
]
|
||||
|
||||
@property
|
||||
def gyro_in_rot(self):
|
||||
c = 0.0001694
|
||||
c2 = c * self._ime_yz_coeff
|
||||
return [
|
||||
(
|
||||
self.get_gyro_x(i) * c,
|
||||
self.get_gyro_y(i) * c2,
|
||||
self.get_gyro_z(i) * c2,
|
||||
)
|
||||
for i in range(3)
|
||||
]
|
||||
Loading…
Reference in New Issue