此更改更改了imu数据处理方式，使得静止状态yaw不会发生偏移，后续应优化陀螺仪数据处理：低通滤波会导致相位延迟、滑动窗口会导致数据有微量误差。

无法避免运动过程中的yaw偏移。后续应在运动时也加上零漂偏移
2026-06-23 22:34:15 +08:00
parent 630cb18277
commit b14befd1cf
29 changed files with 9433 additions and 391 deletions
--- a/scripts/windows_keyboard_control.py
+++ b/scripts/windows_keyboard_control.py
@@ -0,0 +1,137 @@
+#!/usr/bin/env python3
+"""
+Windows keyboard control → UDP → RDKx5
+Simultaneous keys supported (W+A = forward-left, etc.)
+
+Usage:
+    1. pip install keyboard
+    2. Run this script on Windows
+    3. RDKx5 must be running udp_to_cmdvel node
+
+Controls:
+    W/S      : forward / backward
+    A/D      : turn left / right
+    Q/E      : left-forward / right-forward (single-key diagonal)
+    Z/C      : left-backward / right-backward
+    Space    : emergency stop
+    I/K      : linear  speed +/-
+    J/L      : angular speed +/-
+    Esc      : quit
+"""
+
+import keyboard
+import socket
+import struct
+import time
+import sys
+
+# ========== CONFIG ==========
+ROBOT_IP = "192.168.10.210"
+UDP_PORT = 9999
+# ============================
+
+HELP = """
+========================================
+  Windows Keyboard Control → RDKx5
+========================================
+  W/S     : forward / backward
+  A/D     : turn left / right
+  Q/E     : left-forward / right-forward
+  Z/C     : left-backward / right-backward
+  Space   : STOP
+  I/K     : linear  speed +/- (step 0.05)
+  J/L     : angular speed +/- (step 0.1)
+  Esc     : quit
+========================================
+  Current: lin=%.2f  ang=%.2f
+"""
+
+
+def main():
+    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+
+    lin_speed = 0.2
+    ang_speed = 0.5
+    lin_step = 0.05
+    ang_step = 0.1
+
+    def print_help():
+        print(HELP % (lin_speed, ang_speed))
+
+    print("Connecting to %s:%d ..." % (ROBOT_IP, UDP_PORT))
+    print_help()
+
+    while True:
+        if keyboard.is_pressed('esc'):
+            sock.sendto(struct.pack('dd', 0.0, 0.0), (ROBOT_IP, UDP_PORT))
+            print("\nQuit.")
+            break
+
+        # Speed adjustments (one-shot, debounced by keyboard library)
+        if keyboard.is_pressed('i'):
+            lin_speed = round(lin_speed + lin_step, 2)
+            print_help()
+            time.sleep(0.15)
+            continue
+        if keyboard.is_pressed('k'):
+            lin_speed = round(max(0.0, lin_speed - lin_step), 2)
+            print_help()
+            time.sleep(0.15)
+            continue
+        if keyboard.is_pressed('j'):
+            ang_speed = round(ang_speed + ang_step, 2)
+            print_help()
+            time.sleep(0.15)
+            continue
+        if keyboard.is_pressed('l'):
+            ang_speed = round(max(0.0, ang_speed - ang_step), 2)
+            print_help()
+            time.sleep(0.15)
+            continue
+
+        # Emergency stop
+        if keyboard.is_pressed('space'):
+            sock.sendto(struct.pack('dd', 0.0, 0.0), (ROBOT_IP, UDP_PORT))
+            print("*** STOP ***")
+            time.sleep(0.05)
+            continue
+
+        # Combined movement: multi-key support
+        lin = 0.0
+        ang = 0.0
+
+        if keyboard.is_pressed('w'):
+            lin += lin_speed
+        if keyboard.is_pressed('s'):
+            lin -= lin_speed
+        if keyboard.is_pressed('a'):
+            ang += ang_speed
+        if keyboard.is_pressed('d'):
+            ang -= ang_speed
+
+        # Single-key diagonals (override if used)
+        if keyboard.is_pressed('q'):
+            lin, ang = lin_speed, ang_speed
+        if keyboard.is_pressed('e'):
+            lin, ang = lin_speed, -ang_speed
+        if keyboard.is_pressed('z'):
+            lin, ang = -lin_speed, ang_speed
+        if keyboard.is_pressed('c'):
+            lin, ang = -lin_speed, -ang_speed
+
+        sock.sendto(struct.pack('dd', round(lin, 3), round(ang, 3)),
+                    (ROBOT_IP, UDP_PORT))
+
+        time.sleep(0.05)  # 20Hz
+
+
+if __name__ == "__main__":
+    print("Installing keyboard library if needed...")
+    try:
+        import keyboard
+    except ImportError:
+        print("ERROR: 'keyboard' library not installed.")
+        print("Run: pip install keyboard")
+        sys.exit(1)
+
+    main()