这个提交包含在:
2025-09-19 18:02:06 +08:00
父节点 7d3206cfc8
当前提交 c189022db4
共有 3 个文件被更改,包括 71 次插入 和 0 次删除

386
keyboard/main.py 普通文件
查看文件

@ -0,0 +1,386 @@
import pygame
import sys
import websockets
import asyncio
import json
from dataclasses import dataclass
from typing import Dict, Any, List, Tuple
from enum import Enum
class Joint(Enum):
GROUND = "Ground"
JOINT0 = "Joint0"
JOINT2 = "Joint2"
JOINT3 = "Joint3"
TURNER = "Turner"
CLAW = "Claw"
class ControlKey(Enum):
MOVE_UP = pygame.K_UP
MOVE_DOWN = pygame.K_DOWN
MOVE_LEFT = pygame.K_LEFT
MOVE_RIGHT = pygame.K_RIGHT
JOINT2_UP = pygame.K_w
JOINT2_DOWN = pygame.K_s
JOINT3_UP = pygame.K_e
JOINT3_DOWN = pygame.K_d
CLAW_OPEN = pygame.K_SPACE
QUIT = pygame.K_ESCAPE
RESET_ALL = pygame.K_r
@dataclass
class JointState:
id: str
value: int
min_val: int = 0
max_val: int = 180
step: int = 5 # 每帧的步进值,实现平滑移动
class RobotStatus:
def __init__(self):
self.joints = {
Joint.GROUND: JointState(id=Joint.GROUND.value, value=90),
Joint.JOINT0: JointState(id=Joint.JOINT0.value, value=90),
Joint.JOINT2: JointState(id=Joint.JOINT2.value, value=90),
Joint.JOINT3: JointState(id=Joint.JOINT3.value, value=90),
Joint.TURNER: JointState(id=Joint.TURNER.value, value=90),
Joint.CLAW: JointState(id=Joint.CLAW.value, value=90, min_val=0, max_val=120)
}
self.active_controls = {
Joint.JOINT0: 0, # -1: 减少, 0: 无操作, 1: 增加
Joint.JOINT2: 0,
Joint.JOINT3: 0,
Joint.TURNER: 0
}
def get_joint(self, joint: Joint) -> JointState:
return self.joints[joint]
def update_joint(self, joint: Joint, value: int):
joint_state = self.joints[joint]
joint_state.value = max(joint_state.min_val, min(joint_state.max_val, value))
def set_control_direction(self, joint: Joint, direction: int):
"""设置控制方向 (-1, 0, 1)"""
self.active_controls[joint] = direction
def update_continuous(self):
"""根据当前激活的控制更新关节状态"""
status_changed = False
for joint, direction in self.active_controls.items():
if direction != 0:
joint_state = self.joints[joint]
new_value = joint_state.value + direction * joint_state.step
new_value = max(joint_state.min_val, min(joint_state.max_val, new_value))
if new_value != joint_state.value:
joint_state.value = new_value
status_changed = True
return status_changed
def toggle_claw(self):
claw = self.joints[Joint.CLAW]
self.update_joint(Joint.CLAW, 0 if claw.value > 45 else 90)
return True
def reset_all(self):
for joint_state in self.joints.values():
joint_state.value = 90 if joint_state.max_val >= 90 else 0
return True
def get_updates(self) -> List[Dict[str, Any]]:
"""获取所有需要发送的更新数据"""
return [{"id": joint_state.id, "val": joint_state.value}
for joint_state in self.joints.values()]
class WebSocketClient:
def __init__(self, url: str):
self.url = url
self.connection = None
async def connect(self):
"""建立WebSocket连接"""
try:
self.connection = await websockets.connect(self.url)
print(f"已连接到 {self.url}")
return True
except Exception as e:
print(f"无法连接到WebSocket服务器: {e}")
return False
async def send_updates(self, updates: List[Dict[str, Any]]) -> bool:
"""异步发送更新数据到服务器"""
if not self.connection:
print("未建立WebSocket连接")
return False
try:
# 发送所有更新
for update in updates:
await self.connection.send(json.dumps(update))
await asyncio.sleep(0.001) # 小延迟避免发送过快
return True
except websockets.exceptions.ConnectionClosed:
print("WebSocket连接已关闭")
return False
except Exception as e:
print(f"发送数据时出错: {e}")
return False
async def close(self):
"""关闭WebSocket连接"""
if self.connection:
await self.connection.close()
class RobotController:
def __init__(self, websocket_client: WebSocketClient):
self.websocket_client = websocket_client
self.status = RobotStatus()
self.status_changed = False
self.screen:pygame.Surface
self.clock = pygame.time.Clock()
self.font:pygame.font.Font
self.last_update_time = 0
self.update_interval = 0.05 # 每秒最多更新20次
self.last_mouse_pos = None
self.mouse_sensitivity = 0.5 # 鼠标移动灵敏度
async def initialize(self):
"""初始化控制器"""
pygame.init()
self.screen = pygame.display.set_mode((1280, 960))
pygame.display.set_caption("设备控制端 - 连续控制模式")
self.font = pygame.font.SysFont("SimHei", 36)
pygame.mouse.set_visible(True)
# 连接WebSocket服务器
return await self.websocket_client.connect()
def handle_events(self):
"""处理Pygame事件"""
for event in pygame.event.get():
if event.type == pygame.QUIT:
return False
elif event.type == pygame.KEYDOWN:
if not self.handle_keydown(event):
return False
elif event.type == pygame.KEYUP:
self.handle_keyup(event)
elif event.type == pygame.MOUSEBUTTONDOWN:
if event.button == 1: # 左键
self.status.update_joint(Joint.CLAW, 0)
self.status_changed = True
elif event.button == 3: # 右键
self.status.update_joint(Joint.CLAW, 120)
self.status_changed = True
elif event.type == pygame.MOUSEBUTTONUP:
if event.button in [1, 3]: # 左右键释放
self.status.update_joint(Joint.CLAW, 120)
self.status_changed = True
# 处理鼠标移动
self.handle_mouse_movement()
return True
def handle_keydown(self, event) -> bool:
"""处理键盘按下事件"""
if event.key == ControlKey.QUIT.value:
return False
elif event.key == ControlKey.RESET_ALL.value:
self.status.reset_all()
self.status_changed = True
return True
# 关节连续控制
elif event.key == ControlKey.MOVE_UP.value:
self.status.set_control_direction(Joint.JOINT0, 1)
elif event.key == ControlKey.MOVE_DOWN.value:
self.status.set_control_direction(Joint.JOINT0, -1)
elif event.key == ControlKey.JOINT2_UP.value:
self.status.set_control_direction(Joint.JOINT2, 1)
elif event.key == ControlKey.JOINT2_DOWN.value:
self.status.set_control_direction(Joint.JOINT2, -1)
elif event.key == ControlKey.JOINT3_UP.value:
self.status.set_control_direction(Joint.JOINT3, 1)
elif event.key == ControlKey.JOINT3_DOWN.value:
self.status.set_control_direction(Joint.JOINT3, -1)
elif event.key == ControlKey.MOVE_LEFT.value:
self.status.set_control_direction(Joint.TURNER, -1)
elif event.key == ControlKey.MOVE_RIGHT.value:
self.status.set_control_direction(Joint.TURNER, 1)
elif event.key == ControlKey.CLAW_OPEN.value:
self.status.toggle_claw()
self.status_changed = True
return True
def handle_keyup(self, event):
"""处理键盘释放事件"""
if event.key in [ControlKey.MOVE_UP.value, ControlKey.MOVE_DOWN.value]:
self.status.set_control_direction(Joint.JOINT0, 0)
elif event.key in [ControlKey.JOINT2_UP.value, ControlKey.JOINT2_DOWN.value]:
self.status.set_control_direction(Joint.JOINT2, 0)
elif event.key in [ControlKey.JOINT3_UP.value, ControlKey.JOINT3_DOWN.value]:
self.status.set_control_direction(Joint.JOINT3, 0)
elif event.key in [ControlKey.MOVE_LEFT.value, ControlKey.MOVE_RIGHT.value]:
self.status.set_control_direction(Joint.TURNER, 0)
def handle_mouse_movement(self):
"""处理鼠标移动控制"""
current_mouse_pos = pygame.mouse.get_pos()
if self.last_mouse_pos is not None:
dx = current_mouse_pos[0] - self.last_mouse_pos[0]
dy = current_mouse_pos[1] - self.last_mouse_pos[1]
# 左右移动控制Turner
if abs(dx) > 2: # 死区过滤
turner_change = int(dx * self.mouse_sensitivity)
current_turner = self.status.get_joint(Joint.TURNER).value
new_turner = max(0, min(180, current_turner + turner_change))
if new_turner != current_turner:
self.status.update_joint(Joint.TURNER, new_turner)
self.status_changed = True
# 前后移动控制Joint3
if abs(dy) > 2: # 死区过滤
joint3_change = int(-dy * self.mouse_sensitivity) # 反向,因为鼠标向下移动是正方向
current_joint3 = self.status.get_joint(Joint.JOINT3).value
new_joint3 = max(0, min(180, current_joint3 + joint3_change))
if new_joint3 != current_joint3:
self.status.update_joint(Joint.JOINT3, new_joint3)
self.status_changed = True
self.last_mouse_pos = current_mouse_pos
async def update(self, current_time: float):
"""更新控制器状态"""
# 更新连续控制
continuous_changed = self.status.update_continuous()
if continuous_changed:
self.status_changed = True
if self.status_changed and current_time - self.last_update_time >= self.update_interval:
updates = self.status.get_updates()
success = await self.websocket_client.send_updates(updates)
if not success:
# 尝试重新连接
if await self.websocket_client.connect():
await self.websocket_client.send_updates(updates)
self.status_changed = False
self.last_update_time = current_time
def render(self):
"""渲染界面"""
self.screen.fill((240, 240, 240))
# 显示标题
title_font = pygame.font.SysFont(None, 48)
title = title_font.render("机器人连续控制器", True, (0, 0, 139))
self.screen.blit(title, (50, 20))
# 显示状态信息
y_offset = 80
for joint, joint_state in self.status.joints.items():
# 关节名称和值
text = self.font.render(f"{joint.value}: {joint_state.value}°", True, (0, 0, 0))
self.screen.blit(text, (50, y_offset))
# 进度条
bar_width = 200
bar_height = 20
fill_width = int((joint_state.value - joint_state.min_val) /
(joint_state.max_val - joint_state.min_val) * bar_width)
pygame.draw.rect(self.screen, (200, 200, 200),
(250, y_offset, bar_width, bar_height))
pygame.draw.rect(self.screen, (0, 128, 0),
(250, y_offset, fill_width, bar_height))
y_offset += 40
# 显示控制说明
controls = [
"控制说明:",
"↑/↓: 控制Joint0角度 (按住连续控制)",
"←/→: 控制Turner角度 (按住连续控制)",
"W/S: 控制Joint2角度 (按住连续控制)",
"E/D: 控制Joint3角度 (按住连续控制)",
"鼠标左右移动: 控制Turner",
"鼠标前后移动: 控制Joint3",
"左键点击: 抓取,右键点击: 释放",
"空格键: 切换爪子开合",
"R键: 重置所有关节",
"ESC键: 退出程序"
]
y_offset += 40
for i, text in enumerate(controls):
color = (50, 50, 150) if i == 0 else (80, 80, 80)
rendered = self.font.render(text, True, color)
self.screen.blit(rendered, (50, y_offset + i * 35))
# 显示连接状态
status_text = "连接状态: 已连接" if self.websocket_client.connection else "连接状态: 未连接"
status_color = (0, 128, 0) if self.websocket_client.connection else (255, 0, 0)
status_rendered = self.font.render(status_text, True, status_color)
self.screen.blit(status_rendered, (50, 900))
# 显示鼠标控制提示
mouse_text = "鼠标控制中..." if self.last_mouse_pos and pygame.mouse.get_focused() else "移动鼠标控制Turner和Joint3"
mouse_rendered = self.font.render(mouse_text, True, (100, 100, 100))
self.screen.blit(mouse_rendered, (50, 930))
pygame.display.flip()
async def run(self):
"""运行主循环"""
running = True
while running:
current_time = pygame.time.get_ticks() / 1000.0 # 转换为秒
running = self.handle_events()
await self.update(current_time)
self.render()
self.clock.tick(60) # 限制帧率
await self.websocket_client.close()
pygame.quit()
sys.exit()
async def main():
# 创建WebSocket客户端
websocket_client = WebSocketClient("ws://jamaccao.cn:8989/ws")
# 创建机器人控制器
controller = RobotController(websocket_client)
# 初始化并运行
if await controller.initialize():
await controller.run()
if __name__ == '__main__':
asyncio.run(main())