I don't know if anyone shared this before, as discord channel is hard for me to access.
I changed communication between esp32(device) and upstream(mac) in websocket and update a math model impls in python here for anyone interested with control figure with math model or algorithm.
Let (A_1) be a point on (\omega_A), and (B_1) a point on (\omega_B). There exist two distinct points (C_1, C_2) on (\omega_C) such that the segment (\overline{C_1C_2}) is a diameter of (\omega_C) (hence passes through (C)), and
[
|A_1C_1| = |B_1C_2| = a,
]
where (a>0) is a given constant.
Given the polar angle (\theta) of point (C_2) with respect to the center (C) (measured from the positive (x)-axis), determine the polar angles of (A_1) with respect to (A), and of (B_1) with respect to (B). That is, find (\alpha) and (\beta) such that
[
A_1 = A + r_1(\cos\alpha,\sin\alpha), \qquad
B_1 = B + r_1(\cos\beta,\sin\beta).
]
after rounds of testing ... the final like:
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.widgets import TextBox
import asyncio
import threading
import queue
import json
import logging
import time
import websockets
from channel import ServoChannel # 确保 channel 模块可用
logging.basicConfig(level=logging.INFO, format="%(asctime)s [%(levelname)s] %(message)s")
logger = logging.getLogger(__name__)
# ================= 几何求解与过滤（不变） =================
def solve_angles(x, y, r1, r2, a, theta_rad, enforce_side=True, apply_angle_limit=True):
if enforce_side and np.cos(theta_rad) <= 0:
return [], [], None, None
A = np.array([x, 0.0])
B = np.array([-x, 0.0])
C = np.array([0.0, y])
C2 = C + r2 * np.array([np.cos(theta_rad), np.sin(theta_rad)])
C1 = C - r2 * np.array([np.cos(theta_rad), np.sin(theta_rad)])
def find_angles(center, radius, target, a):
d_vec = center - target
d = np.linalg.norm(d_vec)
cos_val = (a**2 - radius**2 - d**2) / (2 * radius * d)
if abs(cos_val) > 1.0:
return []
alpha = np.arctan2(d_vec[1], d_vec[0])
delta = np.arccos(cos_val)
angles = [alpha + delta, alpha - delta]
angles = [ang % (2*np.pi) for ang in angles]
if np.isclose(delta, 0):
return [angles[0]]
return angles
phi_A = find_angles(A, r1, C2, a)
phi_B = find_angles(B, r1, C1, a)
if apply_angle_limit:
phi_A = filter_A1(phi_A, x, r1)
phi_B = filter_B1(phi_B, x, r1)
return phi_A, phi_B, C1, C2
def filter_A1(angles, x, r1):
valid = []
for phi in angles:
A1 = np.array([x + r1*np.cos(phi), r1*np.sin(phi)])
if A1[0] < x:
vec_A1A = np.array([x, 0.0]) - A1
vec_A1O = np.array([0.0, 0.0]) - A1
cos_angle = np.dot(vec_A1A, vec_A1O) / (np.linalg.norm(vec_A1A)*np.linalg.norm(vec_A1O))
if cos_angle <= np.cos(np.deg2rad(30)):
continue
valid.append(phi)
return valid
def filter_B1(angles, x, r1):
valid = []
for phi in angles:
B1 = np.array([-x + r1*np.cos(phi), r1*np.sin(phi)])
if B1[0] > -x:
vec_B1B = np.array([-x, 0.0]) - B1
vec_B1O = np.array([0.0, 0.0]) - B1
cos_angle = np.dot(vec_B1B, vec_B1O) / (np.linalg.norm(vec_B1B)*np.linalg.norm(vec_B1O))
if cos_angle <= np.cos(np.deg2rad(30)):
continue
valid.append(phi)
return valid
# ================= WebSocket 服务器管理（不变） =================
class ServoManager:
def __init__(self, host="0.0.0.0", port=8080):
self.host = host
self.port = port
self.queue = queue.Queue()
self.connected = False
async def handler(self, websocket):
try:
msg = await asyncio.wait_for(websocket.recv(), timeout=10)
data = json.loads(msg)
if data.get("type") != "register":
logger.warning("非法的注册消息，断开连接")
return
servos_str = data.get("servos", "")
channels = [int(x) for x in servos_str.split(":") if x]
logger.info(f"设备已注册，通道: {channels}")
ch92 = ServoChannel(websocket, channel_id=92, rate=24, stats_collector=None)
ch93 = ServoChannel(websocket, channel_id=93, rate=24, stats_collector=None)
self.connected = True
loop = asyncio.get_running_loop()
while True:
angle_92, angle_93 = await loop.run_in_executor(None, self.queue.get)
if not self.connected:
break
try:
await ch92.send_angle(angle_92, record=False)
time.sleep(1)
await ch93.send_angle(angle_93, record=False)
logger.info(f"已发送 → 92:{angle_92:.1f}° 93:{angle_93:.1f}°")
except Exception as e:
logger.error(f"发送失败: {e}")
break
except asyncio.TimeoutError:
logger.warning("注册超时，断开连接")
except Exception:
logger.exception("连接处理异常")
finally:
self.connected = False
logger.info("设备连接已断开")
async def server_main(self):
async with websockets.serve(self.handler, self.host, self.port):
logger.info(f"WebSocket 服务器已启动 ws://{self.host}:{self.port}")
await asyncio.Future()
def start(self):
def run():
asyncio.run(self.server_main())
t = threading.Thread(target=run, daemon=True)
t.start()
import time
time.sleep(0.5)
def send_angles(self, a92, a93):
self.queue.put((a92, a93))
# ================= 交互式绘图（文本框输入） =================
def main():
servo_mgr = ServoManager()
servo_mgr.start()
x = 0.6
y = 3.5
r1 = 0.5
r2 = 0.5
a = 3.5
OFFSET = 150.0
fig, ax = plt.subplots(figsize=(9, 8))
plt.subplots_adjust(bottom=0.15) # 为文本框留空间
# 创建文本输入框
axbox = plt.axes([0.2, 0.02, 0.3, 0.05])
text_box = TextBox(axbox, 'θ (deg) ', initial='0')
A = np.array([x, 0.0])
B = np.array([-x, 0.0])
C = np.array([0.0, y])
ax.add_patch(plt.Circle(A, r1, fill=False, color='red', alpha=0.7))
ax.add_patch(plt.Circle(B, r1, fill=False, color='blue', alpha=0.7))
ax.add_patch(plt.Circle(C, r2, fill=False, color='green', alpha=0.7))
ax.plot(A[0], A[1], 'ro')
ax.plot(B[0], B[1], 'bo')
ax.plot(C[0], C[1], 'go')
c1_pt, = ax.plot([], [], 'mx', markersize=10)
c2_pt, = ax.plot([], [], 'cx', markersize=10)
diam_line, = ax.plot([], [], 'k--', alpha=0.5)
text_handle = ax.text(0.02, 0.95, '', transform=ax.transAxes,
fontsize=10, verticalalignment='top',
bbox=dict(boxstyle='round', facecolor='wheat', alpha=0.8))
a1_artists = []
b1_artists = []
margin = max(r1, r2, a) + 0.5
ax.set_aspect('equal')
ax.axvline(0, color='gray', linestyle='--', alpha=0.5)
ax.set_xlim(-x - r1 - margin, x + r1 + margin)
ax.set_ylim(-r1 - margin, y + r2 + margin)
ax.grid(True)
def update_from_text(text):
nonlocal a1_artists, b1_artists
try:
theta_deg = float(text)
except ValueError:
logger.warning(f"无效输入: {text}")
return
if theta_deg < -60 or theta_deg > 60:
logger.warning("角度必须在 -60 ~ 60 之间")
return
theta_rad = np.radians(theta_deg)
phi_As, phi_Bs, C1, C2 = solve_angles(x, y, r1, r2, a, theta_rad)
c1_pt.set_data([C1[0]], [C1[1]])
c2_pt.set_data([C2[0]], [C2[1]])
diam_line.set_data([C1[0], C2[0]], [C1[1], C2[1]])
for art in a1_artists + b1_artists:
art.remove()
a1_artists.clear()
b1_artists.clear()
angle_text = ""
send_93 = None # ∠(A→A1, +x)+150° → 93
send_92 = None # ∠(B→B1, -x)+150° → 92
if phi_As:
angle_text += f"∠(A→A1, +x)+{OFFSET}°: "
for i, phi in enumerate(phi_As):
A1 = A + r1 * np.array([np.cos(phi), np.sin(phi)])
abs_angle = np.arccos(np.clip(np.cos(phi), -1, 1))
signed = -abs_angle if A1[1] > 0 else abs_angle
display_angle = signed + np.deg2rad(OFFSET)
deg_val = np.degrees(display_angle)
if np.isclose(deg_val, 0):
deg_val = 0.0
angle_text += f"{deg_val:.1f}°"
if i < len(phi_As)-1:
angle_text += ", "
if send_93 is None:
send_93 = deg_val
angle_text += "\n"
else:
angle_text += "A1 无解\n"
if phi_Bs:
angle_text += f"∠(B→B1, -x)+{OFFSET}°: "
for i, phi in enumerate(phi_Bs):
B1 = B + r1 * np.array([np.cos(phi), np.sin(phi)])
abs_angle = np.arccos(np.clip(-np.cos(phi), -1, 1))
signed = -abs_angle if B1[1] > 0 else abs_angle
display_angle = np.deg2rad(OFFSET) - signed
deg_val = np.degrees(display_angle)
if np.isclose(deg_val, 0):
deg_val = 0.0
angle_text += f"{deg_val:.1f}°"
if i < len(phi_Bs)-1:
angle_text += ", "
if send_92 is None:
send_92 = deg_val
else:
angle_text += "B1 无解"
if servo_mgr.connected:
angle_text += "\n设备已连接"
else:
angle_text += "\n等待设备连接..."
text_handle.set_text(angle_text)
# 发送角度
if send_92 is not None and send_93 is not None:
send_92 = max(0, min(300, send_92))
send_93 = max(0, min(300, send_93))
servo_mgr.send_angles(send_92, send_93)
# 绘制 A1 和 B1
for phi_A in phi_As:
A1 = A + r1 * np.array([np.cos(phi_A), np.sin(phi_A)])
pt, = ax.plot(A1[0], A1[1], 'rs', markersize=6)
line, = ax.plot([A1[0], C2[0]], [A1[1], C2[1]], 'r--', alpha=0.7)
a1_artists.extend([pt, line])
for phi_B in phi_Bs:
B1 = B + r1 * np.array([np.cos(phi_B), np.sin(phi_B)])
pt, = ax.plot(B1[0], B1[1], 'bs', markersize=6)
line, = ax.plot([B1[0], C1[0]], [B1[1], C1[1]], 'b--', alpha=0.7)
b1_artists.extend([pt, line])
fig.canvas.draw_idle()
text_box.on_submit(update_from_text)
# 初始显示0度
update_from_text('0')
plt.show()
if __name__ == "__main__":
main()
I don't know if anyone shared this before, as discord channel is hard for me to access.
I changed communication between esp32(device) and upstream(mac) in websocket and update a math model impls in python here for anyone interested with control figure with math model or algorithm.
You can starts with this math model:
Problem Statement
In the (xOy) plane, let three circles be defined as follows:
Let (A_1) be a point on (\omega_A), and (B_1) a point on (\omega_B). There exist two distinct points (C_1, C_2) on (\omega_C) such that the segment (\overline{C_1C_2}) is a diameter of (\omega_C) (hence passes through (C)), and
[
|A_1C_1| = |B_1C_2| = a,
]
where (a>0) is a given constant.
Given the polar angle (\theta) of point (C_2) with respect to the center (C) (measured from the positive (x)-axis), determine the polar angles of (A_1) with respect to (A), and of (B_1) with respect to (B). That is, find (\alpha) and (\beta) such that
[
A_1 = A + r_1(\cos\alpha,\sin\alpha), \qquad
B_1 = B + r_1(\cos\beta,\sin\beta).
]
after rounds of testing ... the final like: