62 lines
2.7 KiB
Python
62 lines
2.7 KiB
Python
import pygame
|
|
from .config import FlightStickConfig
|
|
from ..xbox import Xbox
|
|
"""
|
|
FlightStick类用于检测和监控飞行手柄。
|
|
"""
|
|
class FlightStick(Xbox):
|
|
config_class = FlightStickConfig
|
|
name = "flight_stick"
|
|
|
|
def get_action(self):
|
|
pygame.event.pump()
|
|
pose_speeds = [0.0] * 6
|
|
"""更新末端位姿控制"""
|
|
# 根据控制模式调整步长
|
|
current_linear_step = self.linear_step * (0.1 if self.fine_control_mode else 1.0)
|
|
current_angular_step = self.angular_step * (0.1 if self.fine_control_mode else 1.0)
|
|
|
|
# print(f"步长设置 - 线性: {current_linear_step}, 角度: {current_angular_step}")
|
|
print(f"精细控制模式: {self.fine_control_mode}")
|
|
# print(f"{self.joystick.get_axis(0)},{self.joystick.get_axis(1)},{self.joystick.get_axis(2)},{self.joystick.get_axis(3)}")
|
|
# 方向键控制XY
|
|
hat = self.joystick.get_hat(0)
|
|
hat_up = hat[1] == 1 # Y+
|
|
hat_down = hat[1] == -1 # Y-
|
|
hat_left = hat[0] == -1 # X-
|
|
hat_right = hat[0] == 1 # X+
|
|
# 计算各轴速度
|
|
pose_speeds['x.vel'] = -current_linear_step if hat_left else (current_linear_step if hat_right else 0.0) # X
|
|
pose_speeds['y.vel'] = current_linear_step if hat_up else (-current_linear_step if hat_down else 0.0) # Y
|
|
|
|
# print(f"方向键状态: up={hat_up}, down={hat_down}, left={hat_left}, right={hat_right}")
|
|
# 油门控制Z轴
|
|
gas_axis = -self.joystick.get_axis(2)
|
|
|
|
# 设置Z速度
|
|
z_mapping = self.apply_nonlinear_mapping(gas_axis)
|
|
# print(f"Z轴非线性映射: {gas_axis} -> {z_mapping}")
|
|
pose_speeds['z.vel'] = z_mapping * current_linear_step # Z
|
|
|
|
|
|
# 主摇杆X轴控制RX旋转
|
|
# 设置RX旋转速度
|
|
rx_mapping = self.apply_nonlinear_mapping(-self.joystick.get_axis(1))
|
|
# print(f"RX轴非线性映射: {x_axis} -> {rx_mapping}")
|
|
pose_speeds['rx.vel'] = rx_mapping * current_angular_step * 2 # RX
|
|
|
|
# 主摇杆Y轴控制RY旋转
|
|
# 设置RY旋转速度
|
|
ry_mapping = -self.apply_nonlinear_mapping(-self.joystick.get_axis(0))
|
|
# print(f"RY轴非线性映射: {y_axis} -> {ry_mapping}")
|
|
pose_speeds['ry.vel'] = ry_mapping * current_angular_step * 2 # RY
|
|
|
|
# 主摇杆左右旋转轴 控制RZ旋转
|
|
# 设置RZ旋转速度
|
|
rz_mapping = self.apply_nonlinear_mapping(self.joystick.get_axis(3))
|
|
# print(f"RZ轴非线性映射: {z_axis} -> {rz_mapping}")
|
|
pose_speeds['rz.vel'] = rz_mapping * current_angular_step * 2 # RZ
|
|
|
|
pose_speeds['gripper.vel'] = 0
|
|
return pose_speeds
|