meiqi/tienkung-szu
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

新增多设备控制

Browse Source
This commit is contained in:
meiqi
2026-03-30 14:22:53 +08:00
parent 2988d4174b
commit 32702e25f3
7 changed files with 234 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
Deploy_Tienkung/udp_loopback/udp_fsm_controller.py
View File

@@ -72,6 +72,7 @@ class UDPFSMController:
self.udp_flag = UDPFSMFlag()
self.udp_flag.height_cmd = self.initial_lift
self.last_seq_id = -1
self.last_fsm_command_time = 0.0
def start(self) -> None:
self.rx_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
@@ -132,6 +133,14 @@ class UDPFSMController:
flag_copy.__dict__.update(self.udp_flag.__dict__)
return flag_copy
def get_last_input_time(self) -> float:
with self.data_mutex:
return self.motion_frame.last_rx_time
def get_last_fsm_command_time(self) -> float:
with self.data_mutex:
return self.last_fsm_command_time
def init(self) -> int:
print("UDP FSM controller initialized")
return 0
@@ -145,14 +154,19 @@ class UDPFSMController:
event_code = packet.event_code
if event_code == "pose_home":
self.motion_frame.mode_tag = "pose_home"
self.last_fsm_command_time = packet.sent_at
elif event_code == "pose_hold":
self.motion_frame.mode_tag = "pose_hold"
self.last_fsm_command_time = packet.sent_at
elif event_code == "mode_stride":
self.motion_frame.mode_tag = "mode_stride"
self.last_fsm_command_time = packet.sent_at
elif event_code == "mode_dash":
self.motion_frame.mode_tag = "mode_dash"
self.last_fsm_command_time = packet.sent_at
elif event_code == "mode_xrun":
self.motion_frame.mode_tag = "mode_xrun"
self.last_fsm_command_time = packet.sent_at
elif event_code == "surge_up":
self.motion_frame.surge_goal = min(
self.max_surge, self.motion_frame.surge_goal + self.surge_step