138 lines
3.7 KiB
Python
138 lines
3.7 KiB
Python
#!/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()
|