Scruff AI
137 lines
4.2 KiB
Python
137 lines
4.2 KiB
Python
"""dog_bridge.py — TCP bridge between Navigator and Freenove Robot Dog.
|
|
|
|
HARDWARE STATUS: NOT YET WIRED
|
|
This file is a ready-to-use client for when the Freenove Robot Dog Kit
|
|
is assembled and running its Server.py on the Raspberry Pi. It requires:
|
|
|
|
Pi-side changes (NOT yet applied):
|
|
- Command.py: add CMD_FOOTPAD = "CMD_FOOTPAD"
|
|
- Server.py: add measuring_footpad() method + elif handler
|
|
|
|
Hardware wiring (NOT yet done):
|
|
- 4x 500g FSR sensors on footpads, each with 10kOhm pull-down
|
|
- FSR front-left -> ADS7830 channel 1
|
|
- FSR front-right -> ADS7830 channel 2
|
|
- FSR rear-left -> ADS7830 channel 3
|
|
- FSR rear-right -> ADS7830 channel 4
|
|
|
|
Connection: WiFi TCP to Pi IP, port 5001 (commands), 8001 (video).
|
|
"""
|
|
import socket
|
|
import threading
|
|
|
|
|
|
class DogBridge:
|
|
"""TCP client for commanding the Freenove Robot Dog and reading sensors."""
|
|
|
|
def __init__(self, host, cmd_port=5001):
|
|
self.host = host
|
|
self.cmd_port = cmd_port
|
|
self.sock = None
|
|
self.lock = threading.Lock()
|
|
self._buffer = ""
|
|
|
|
def connect(self):
|
|
self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
|
self.sock.connect((self.host, self.cmd_port))
|
|
self.sock.settimeout(2.0)
|
|
|
|
def disconnect(self):
|
|
if self.sock:
|
|
self.sock.close()
|
|
self.sock = None
|
|
|
|
def send_cmd(self, cmd_str):
|
|
"""Send a command string (e.g. 'CMD_MOVE_FORWARD#8')."""
|
|
with self.lock:
|
|
self.sock.sendall((cmd_str + "\n").encode("utf-8"))
|
|
|
|
def recv_response(self):
|
|
"""Read one newline-delimited response from the dog."""
|
|
with self.lock:
|
|
while "\n" not in self._buffer:
|
|
chunk = self.sock.recv(1024).decode("utf-8")
|
|
if not chunk:
|
|
raise ConnectionError("Dog disconnected")
|
|
self._buffer += chunk
|
|
line, self._buffer = self._buffer.split("\n", 1)
|
|
return line
|
|
|
|
# --- Movement ---
|
|
|
|
def move_forward(self, speed=8):
|
|
self.send_cmd(f"CMD_MOVE_FORWARD#{speed}")
|
|
|
|
def move_backward(self, speed=8):
|
|
self.send_cmd(f"CMD_MOVE_BACKWARD#{speed}")
|
|
|
|
def turn_left(self, speed=8):
|
|
self.send_cmd(f"CMD_TURN_LEFT#{speed}")
|
|
|
|
def turn_right(self, speed=8):
|
|
self.send_cmd(f"CMD_TURN_RIGHT#{speed}")
|
|
|
|
def stop(self):
|
|
self.send_cmd("CMD_MOVE_STOP#")
|
|
|
|
# --- Sensors ---
|
|
|
|
def get_distance(self):
|
|
"""Ultrasonic distance in cm."""
|
|
self.send_cmd("CMD_SONIC#")
|
|
resp = self.recv_response() # CMD_SONIC#<cm>
|
|
return float(resp.split("#")[1])
|
|
|
|
def get_battery(self):
|
|
"""Battery voltage (2S LiPo, ~6.4-8.4V range)."""
|
|
self.send_cmd("CMD_POWER#")
|
|
resp = self.recv_response() # CMD_POWER#<volts>
|
|
return float(resp.split("#")[1])
|
|
|
|
def get_footpads(self):
|
|
"""Per-foot pressure readings (0-255 each, 500g FSR sensors).
|
|
|
|
Returns dict with keys: front_left, front_right, rear_left, rear_right.
|
|
Requires Pi-side CMD_FOOTPAD handler (see module docstring).
|
|
"""
|
|
self.send_cmd("CMD_FOOTPAD#")
|
|
resp = self.recv_response() # CMD_FOOTPAD#FL#FR#RL#RR
|
|
parts = resp.split("#")
|
|
return {
|
|
"front_left": int(parts[1]),
|
|
"front_right": int(parts[2]),
|
|
"rear_left": int(parts[3]),
|
|
"rear_right": int(parts[4]),
|
|
}
|
|
|
|
# --- Posture & Head ---
|
|
|
|
def set_head(self, angle):
|
|
"""Set head servo angle."""
|
|
self.send_cmd(f"CMD_HEAD#{angle}")
|
|
|
|
def relax(self):
|
|
"""Disengage all servos."""
|
|
self.send_cmd("CMD_RELAX#")
|
|
|
|
def balance_on(self):
|
|
"""Enable IMU-based self-balancing."""
|
|
self.send_cmd("CMD_BALANCE#1")
|
|
|
|
def balance_off(self):
|
|
self.send_cmd("CMD_BALANCE#0")
|
|
|
|
def set_height(self, height):
|
|
"""Adjust standing height."""
|
|
self.send_cmd(f"CMD_HEIGHT#{height}")
|
|
|
|
# --- LED & Buzzer ---
|
|
|
|
def buzzer(self, state):
|
|
"""state: '1' on, '0' off."""
|
|
self.send_cmd(f"CMD_BUZZER#{state}")
|
|
|
|
def led(self, index, r, g, b):
|
|
"""Set LED color. index: 0-based LED number."""
|
|
self.send_cmd(f"CMD_LED#{index}#{r}#{g}#{b}")
|