Remove code related to gyro
This commit is contained in:
parent
3d00015607
commit
a3872b1f38
5 changed files with 12 additions and 218 deletions
|
@ -1,18 +1,13 @@
|
|||
from .device import (get_device_ids, get_ids_of_type, get_L_id, get_L_ids,
|
||||
get_R_id, get_R_ids, is_id_L)
|
||||
from .event import ButtonEventJoyCon
|
||||
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",
|
||||
|
|
|
@ -1,6 +1,7 @@
|
|||
import hid
|
||||
from .constants import JOYCON_VENDOR_ID, JOYCON_PRODUCT_IDS
|
||||
from .constants import JOYCON_L_PRODUCT_ID, JOYCON_R_PRODUCT_ID
|
||||
|
||||
from .constants import (JOYCON_L_PRODUCT_ID, JOYCON_PRODUCT_IDS,
|
||||
JOYCON_R_PRODUCT_ID, JOYCON_VENDOR_ID)
|
||||
|
||||
|
||||
def get_device_ids(debug=False):
|
||||
|
|
|
@ -1,84 +0,0 @@
|
|||
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
|
|
@ -1,10 +1,12 @@
|
|||
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
|
||||
import time
|
||||
from typing import Optional
|
||||
|
||||
import hid
|
||||
|
||||
from .constants import (JOYCON_L_PRODUCT_ID, JOYCON_PRODUCT_IDS,
|
||||
JOYCON_R_PRODUCT_ID, JOYCON_VENDOR_ID)
|
||||
|
||||
# TODO: disconnect, power off sequence
|
||||
|
||||
|
||||
|
@ -38,7 +40,6 @@ class JoyCon:
|
|||
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)
|
||||
|
@ -162,16 +163,6 @@ class JoyCon:
|
|||
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
|
||||
|
@ -223,17 +214,6 @@ class JoyCon:
|
|||
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, \
|
||||
|
@ -414,30 +394,6 @@ class JoyCon:
|
|||
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": {
|
||||
|
@ -490,28 +446,8 @@ class JoyCon:
|
|||
"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')
|
||||
|
||||
|
|
|
@ -46,9 +46,6 @@ class PythonicJoyCon(JoyCon):
|
|||
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
|
||||
|
@ -89,54 +86,3 @@ class PythonicJoyCon(JoyCon):
|
|||
)
|
||||
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 a new issue