69 lines
2.1 KiB
Python
69 lines
2.1 KiB
Python
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() |