init

robot_driver.py

@@ -0,0 +1,69 @@
+import serial
+import time
+
+class RobotDriver:
+    def __init__(self, port='COM9', baud=115200, ack=True, timeout=0.25):
+        self.ack = ack
+        self.timeout = timeout
+        self.ser = serial.Serial(port, baud, timeout=timeout, write_timeout=0.5)
+        time.sleep(2.0)
+        self.ser.reset_input_buffer()
+        print(f"[Robot] Connected on {port} @ {baud}")
+
+    def _readline(self):
+        return self.ser.readline().decode("ascii", errors="replace").strip()
+
+    def _send(self, cmd, expect_ok=True):
+        packet = f"{cmd}\r\n".encode("ascii")
+        if not self.ack or not expect_ok:
+            self.ser.write(packet)
+            self.ser.flush()
+            return None
+
+        MAX_RETRIES = 3
+        for attempt in range(MAX_RETRIES):
+            self.ser.write(packet)
+            self.ser.flush()
+            deadline = time.time() + 0.3
+            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}")
+            if attempt < MAX_RETRIES - 1:
+                time.sleep(0.1)
+        raise TimeoutError(f"No ACK for {cmd} after {MAX_RETRIES} attempts")
+
+    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()