init

gamepad_remote.py

@@ -0,0 +1,173 @@
+import time
+import serial
+import pygame
+from robot_driver import RobotDriver
+
+
+
+
+class GamepadRemoteController:
+    # 默认按键映射（Xbox 常见布局）
+    # A/B/X 控制构型 0/1/2
+    # LB 打开，RB 闭合
+    # START 退出
+    BTN_A = 0
+    BTN_B = 1
+    BTN_X = 2
+    BTN_LB = 4
+    BTN_RB = 5
+    BTN_BACK = 6
+    BTN_START = 7
+
+    def __init__(self, robot):
+        self.robot = robot
+        self.joystick = None
+        self.estop_active = False
+
+        self.last_open_pressed = False
+        self.last_close_pressed = False
+
+        self.last_mode_buttons = {
+            self.BTN_A: False,
+            self.BTN_B: False,
+            self.BTN_X: False,
+            self.BTN_BACK: False,
+        }
+
+    def init_gamepad(self):
+        pygame.init()
+        pygame.joystick.init()
+
+        if pygame.joystick.get_count() < 1:
+            raise RuntimeError("No gamepad detected. Please connect controller via Bluetooth first.")
+
+        self.joystick = pygame.joystick.Joystick(0)
+        self.joystick.init()
+        print(f"[System] Gamepad connected: {self.joystick.get_name()}")
+
+    def _edge_pressed(self, btn_id, cache_dict):
+        now_pressed = bool(self.joystick.get_button(btn_id))
+        prev_pressed = cache_dict.get(btn_id, False)
+        cache_dict[btn_id] = now_pressed
+        return now_pressed and (not prev_pressed)
+
+    def _handle_mode_buttons(self):
+        if self._edge_pressed(self.BTN_A, self.last_mode_buttons):
+            self.robot.set_config(0)
+            print("[Action] Config 0")
+        if self._edge_pressed(self.BTN_B, self.last_mode_buttons):
+            self.robot.set_config(1)
+            print("[Action] Config 1")
+        if self._edge_pressed(self.BTN_X, self.last_mode_buttons):
+            self.robot.set_config(2)
+            print("[Action] Config 2")
+
+    def _handle_estop_toggle(self):
+        if not self._edge_pressed(self.BTN_BACK, self.last_mode_buttons):
+            return
+
+        self.estop_active = not self.estop_active
+        self.robot.motor_stop()
+
+        if self.estop_active:
+            print("\n" + "!" * 40)
+            print("!!! EMERGENCY STOP ACTIVE !!!")
+            print("!!! Motion locked. Press BACK again to resume.")
+            print("!" * 40)
+        else:
+            print("\n" + "=" * 40)
+            print(">>> EMERGENCY STOP RELEASED")
+            print("= " * 20)
+
+    def _handle_open_close_hold(self):
+        # 按住 LB 持续张开，按住 RB 持续闭合；松开则停止
+        open_pressed = bool(self.joystick.get_button(self.BTN_LB))
+        close_pressed = bool(self.joystick.get_button(self.BTN_RB))
+
+        if open_pressed and not close_pressed:
+            if not self.last_open_pressed or self.last_close_pressed:
+                self.robot.motor_open()
+                print("[Action] Palm OPEN")
+        elif close_pressed and not open_pressed:
+            if not self.last_close_pressed or self.last_open_pressed:
+                self.robot.motor_close()
+                print("[Action] Palm CLOSE")
+        else:
+            if self.last_open_pressed or self.last_close_pressed:
+                self.robot.motor_stop()
+                print("[Action] Palm STOP")
+
+        self.last_open_pressed = open_pressed
+        self.last_close_pressed = close_pressed
+
+    def loop(self):
+        print("\n=== Gamepad Remote Started ===")
+        print("A -> Config 0")
+        print("B -> Config 1")
+        print("X -> Config 2")
+        print("LB hold -> Palm OPEN")
+        print("RB hold -> Palm CLOSE")
+        print("Release LB/RB -> STOP")
+        print("BACK -> Global E-STOP toggle")
+        print("START -> Exit")
+
+        running = True
+        while running:
+            pygame.event.pump()
+
+            self._handle_estop_toggle()
+
+            if self.joystick.get_button(self.BTN_START):
+                print("[System] Exit requested by START")
+                running = False
+                continue
+
+            if self.estop_active:
+                # 急停锁定期间屏蔽所有动作命令
+                self.last_open_pressed = bool(self.joystick.get_button(self.BTN_LB))
+                self.last_close_pressed = bool(self.joystick.get_button(self.BTN_RB))
+                time.sleep(0.02)
+                continue
+
+            self._handle_mode_buttons()
+            self._handle_open_close_hold()
+
+            time.sleep(0.02)
+
+        self.robot.motor_stop()
+
+    def shutdown(self):
+        try:
+            if self.joystick is not None:
+                self.joystick.quit()
+        finally:
+            pygame.joystick.quit()
+            pygame.quit()
+
+
+def main():
+    # 根据实际串口修改
+    port = "COM9"
+    baud = 115200
+
+    robot = RobotDriver(port=port, baud=baud)
+    if robot.ser is None or not robot.ser.is_open:
+        return
+
+    controller = GamepadRemoteController(robot)
+
+    try:
+        controller.init_gamepad()
+        controller.loop()
+    except KeyboardInterrupt:
+        print("\n[System] Keyboard interrupt")
+    except Exception as e:
+        print(f"[Error] {e}")
+    finally:
+        controller.shutdown()
+        robot.close()
+        print("[System] Shutdown complete.")
+
+
+if __name__ == "__main__":
+    main()