import argparse import sys import time import serial class RobotDriver: def __init__(self, port="COM9", baud=115200, ack=True): self.ack = ack self.ser = serial.Serial(port, baud, timeout=0.25, write_timeout=0.5) time.sleep(2.0) self.ser.reset_input_buffer() print(f"[System] Robot connected on {port} @ {baud}") def _readline(self): line = self.ser.readline().decode("ascii", errors="replace").strip() return line def _send(self, cmd, expect_ok=True): packet = f"{cmd}\r\n".encode("ascii") self.ser.write(packet) self.ser.flush() if not self.ack or not expect_ok: return None deadline = time.time() + 0.5 last_line = "" while time.time() < deadline: line = self._readline() if not line: continue last_line = line if line.startswith("OK:") or line.startswith("READY:"): return line if line.startswith("ERR:"): raise RuntimeError(f"MCU rejected {cmd}: {line}") raise TimeoutError(f"No ACK for {cmd}; last line={last_line!r}") def ping(self): return self._send("PING") def motor_open(self): return self._send("M:OPEN") def motor_close(self): return self._send("M:CLOSE") def motor_stop(self): return self._send("M:STOP") def set_servo(self, servo_id, angle): angle = max(0, min(180, int(angle))) return self._send(f"S{servo_id}:{angle}") def set_config(self, mode): return self._send(f"CFG:{int(mode)}") def close(self): try: if self.ser and self.ser.is_open: self.motor_stop() except Exception: pass finally: if self.ser: self.ser.close() class GamepadRemoteController: 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_buttons = { self.BTN_A: False, self.BTN_B: False, self.BTN_X: False, self.BTN_BACK: False, } def init_gamepad(self): global pygame import pygame pygame.init() pygame.joystick.init() if pygame.joystick.get_count() < 1: raise RuntimeError("No gamepad detected") self.joystick = pygame.joystick.Joystick(0) self.joystick.init() print(f"[System] Gamepad connected: {self.joystick.get_name()}") def _edge_pressed(self, btn_id): now_pressed = bool(self.joystick.get_button(btn_id)) prev_pressed = self.last_buttons.get(btn_id, False) self.last_buttons[btn_id] = now_pressed return now_pressed and not prev_pressed def _handle_mode_buttons(self): if self._edge_pressed(self.BTN_A): self.robot.set_config(0) print("[Action] Config 0") if self._edge_pressed(self.BTN_B): self.robot.set_config(1) print("[Action] Config 1") if self._edge_pressed(self.BTN_X): self.robot.set_config(2) print("[Action] Config 2") def _handle_estop_toggle(self): if not self._edge_pressed(self.BTN_BACK): return self.estop_active = not self.estop_active self.robot.motor_stop() print("[Action] E-STOP ON" if self.estop_active else "[Action] E-STOP OFF") def _handle_open_close_hold(self): 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] Linear motor 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] Linear motor CLOSE") else: if self.last_open_pressed or self.last_close_pressed: self.robot.motor_stop() print("[Action] Linear motor STOP") self.last_open_pressed = open_pressed self.last_close_pressed = close_pressed def loop(self): print("=== Gamepad Remote Started ===") print("A/B/X -> Config 0/1/2") print("LB/RB hold -> Linear motor open/close") print("BACK -> E-stop toggle, START -> Exit") running = True while running: pygame.event.pump() self._handle_estop_toggle() if self.joystick.get_button(self.BTN_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): if self.joystick is not None: self.joystick.quit() pygame.joystick.quit() pygame.quit() def run_self_test(robot): print("[Test] PING:", robot.ping()) print("[Test] Servo 1: 90") robot.set_servo(1, 90) time.sleep(0.5) print("[Test] Servo 2: 90") robot.set_servo(2, 90) time.sleep(0.5) print("[Test] Linear motor open for 0.5 s") robot.motor_open() time.sleep(0.5) robot.motor_stop() time.sleep(0.3) print("[Test] Linear motor close for 0.5 s") robot.motor_close() time.sleep(0.5) robot.motor_stop() print("[Test] Done") def build_parser(): parser = argparse.ArgumentParser(description="Gamepad remote for new DRV8870 PCB") parser.add_argument("--port", default="COM6") parser.add_argument("--baud", type=int, default=115200) parser.add_argument("--no-ack", action="store_true", help="Do not wait for MCU ACK") parser.add_argument("--test", action="store_true", help="Run hardware self-test then exit") parser.add_argument("--cmd", help="Send one raw command, for example PING or S1:90") return parser def main(): args = build_parser().parse_args() robot = None controller = None try: robot = RobotDriver(port=args.port, baud=args.baud, ack=not args.no_ack) if args.cmd: print(robot._send(args.cmd)) return if args.test: run_self_test(robot) return controller = GamepadRemoteController(robot) controller.init_gamepad() controller.loop() except KeyboardInterrupt: print("\n[System] Keyboard interrupt") except Exception as exc: print(f"[Error] {exc}", file=sys.stderr) raise finally: if controller is not None: controller.shutdown() if robot is not None: robot.close() print("[System] Shutdown complete") if __name__ == "__main__": main()