从激光切割到双向数字孪生:一台 ST3215 总线舵机机械臂的创客教学实战
背景:为什么在社团课上做“孪生”?
在创客社团的教学里,“数字孪生”这个概念常常停留在视频和 PPT 里——学生看到的往往是工业场景中昂贵的六轴机器人,很难亲手触摸到“虚拟模型与真实硬件同步运动”的完整闭环。这个项目最初的出发点很朴素:用一台学生自己用激光切割做出来的 2 自由度机械臂,把“孪生”从概念变成可触摸、可拖拽、可手掰的课堂演示。
选型上遇到了一个现实问题。DF 商城的 ST3215 串行总线舵机(7.4V 19.5kg 360°) 和 串行总线舵机驱动板(DRI0057) 都是非常合适的硬件:ST3215 自带 12 位磁编码器,能实时回读 0-4095 的绝对角度,天然适合做“位置反馈”;DRI0057 通过跳线帽切换 UART/USB 双模式,支持 9-12.6V 宽电压输入,单板可级联多达 253 个总线舵机。但当时这两款产品在商城页面上都没有可直接照搬的 Arduino + ESP32 使用文档,社区里也几乎找不到完整的“读角度 + 双向控制”的实例。于是这个项目也带上了第二层意义:把踩过的坑和跑通的代码整理成一份可复现的文档,填补这块空白。

硬件清单
| 部件 | 型号/规格 | 数量 | 说明 |
|---|---|---|---|
| 主控 | FireBeetle 2 ESP32-C6 | 1 | 支持 WebSerial,双硬件串口 |
| 总线舵机 | ST3215(7.4V 19.5kg 360°) | 2 | ID 分别设为 1(底座 Yaw)和 2(俯仰 Pitch) |
| 驱动板 | DRI0057 串行总线舵机驱动板 | 1 | 跳线帽置于 A(UART 控制) |
| 电源 | 9V DC(5.5×2.1mm) | 1 | 独立为舵机供电,不要从主控取电 |
| 结构材料 | 3mm 椴木板 + 3mm 亚克力 | 若干 | 激光切割 |
| 紧固件 | M2.5 螺丝/铜柱 | 若干 | DRI0057 安装孔距 37×28mm,孔径 2.5mm |

结构:激光切割一台“够用”的 2 自由度臂
结构设计的出发点不是追求自由度数量,而是让两个关节的运动干净、可观测、不干涉。
底座负责水平偏航(Yaw),中间关节负责俯仰(Pitch)。椴木板用于承力连杆和底座法兰,亚克力用于需要观察内部走线的转台侧板和舵机安装夹板。设计时特意在俯仰关节两侧留出了舵机盘的让位槽,避免螺丝头在转动时刮蹭板面。
这个项目俯仰安全行程被限制在490~3180(对应约 43°~280°) ,上下都留出了板件和底座之间的余量,防止在示教或拖拽时发生碰撞。

电子连接:那个让项目卡了一整晚的“串口直连”
这是整个项目中最值得写进文档的一条经验。
ESP32-C6 与 DRI0057(模式 A)之间,按常规思维应该 TX 接 RX、RX 接 TX。但这个板子的实际行为不是这样。在反复确认跳线帽、供电、波特率(1,000,000)和 GPIO 定义都正确之后,舵机依然纹丝不动。最后把 ESP32 的 TX 直接接到 DRI0057 的 TX,RX 直接接到 RX,通信瞬间打通。
DRI0057 内部对 UART 信号做了桥接处理,对主控侧呈现的并不是一个“标准交叉串口”的电气语义。这一点在产品的简短说明里没有展开,但对于任何想用 ESP32 / Arduino 通过 UART 控制这款板子的创客来说,它是必须优先尝试的接法。如果直连仍然不通,再回到交叉连接排查,顺序反过来会浪费大量时间。
Arduino 端的核心配置也随之明确:使用 Serial1(而不是用于 USB 调试的 Serial),在 begin() 中显式指定引脚,波特率与舵机一致:
Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
st.pSerial = &Serial1;
ST3215 的“当前位置”寄存器地址是 56(0x38) ,读取 2 个字节即可拿到 04095 的绝对角度。DRI0057 的 912.6V 输入会直接为舵机供电,ESP32 只负责信号,两者共地即可。
Web 端:用 Three.js + WebSerial 做双向孪生
选择 WebSerial 而不是原生桌面程序,是出于教学场景的考虑:社团教室里学生用 Chrome 打开一个本地 HTML 就能操作,不需要安装任何软件。Three.js 负责 3D 渲染,WebSerial API 负责浏览器与 ESP32 之间的串口通信,两者结合实现“鼠标拖拽 → 舵机转动”和“手掰舵机 → 模型跟随”的双向闭环。类似的“Three.js + WebSerial + ESP32”双向数字孪生架构在社区中已有验证。

协议设计
Arduino 与网页之间用最简单的文本行协议通信:
- 下行:
A2048\n、B2500\n分别控制底座和俯仰的目标位置;E1上锁(扭矩使能),E0卸力。 - 上行:Arduino 每 50ms 发送
A<位置>,B<位置>\n,把两个舵机的实时角度回传给网页。
动态扭矩是这个项目的手感关键:鼠标按下拖拽时发 E1,舵机上锁跟随鼠标;松开鼠标发 E0,舵机卸力,学生可以像工业示教一样直接用手掰动机械臂,网页上的 3D 模型同步跟随。这样既保留了“网页控硬件”的演示效果,又让“手掰示教”成为可能。
两个视觉坑与数学修复
过零翻转。 舵机位置 4095 和 0 在物理空间是同一个角度,但数值上相差 4095。如果直接对数值做线性插值,模型会在 4095→0 时反向“倒转一整圈”。修复方法是在渲染循环中对弧度差值做环绕归一化:
let diff = targetRad - currentRad;
diff = Math.atan2(Math.sin(diff), Math.cos(diff));
这行代码把任意差值强制映射到 ((-π, π]) 的最短路径,无论累计了多少圈都不会再出现反向跳变。
松手摆动。 卸力后舵机齿轮间隙和重力会让回传值在终点附近 ±1~3 跳动,网页如果无条件跟随,模型会像弹簧一样抖动。在接收端加一个死区:回传值与当前目标的差值小于 5 时忽略;渲染端再加一个角度死区(约 0.003 rad),模型停靠就干净了。
物理安全限位
俯仰关节的 490~3180 不仅在网页端用滑块 min/max 和拖拽 clamp 限制,Arduino 端在收到 B 指令后也会用 constrain() 做第二层拦截。即使有人绕过网页直接发 B4000,真实舵机也只会收到 3180。对于创客教学场景,物理端的硬限位是不可省略的最后一道防线。
教学用法:一节课能跑通的“孪生”闭环
在社团课上,这个项目可以拆成三个递进环节:
环节一:手掰示教。 舵机处于卸力状态,学生用手把机械臂摆到任意姿态,网页 3D 模型实时跟随。这一步最直观地展示“物理世界的角度正在被读取并映射到虚拟空间”。
环节二:鼠标反向控制。 学生用鼠标在网页上拖拽 3D 模型的底座或大臂,真实机械臂同步转动。这一步展示“虚拟指令如何回到物理世界”,同时引出扭矩使能和速度参数对运动平顺性的影响。
环节三:过零与限位的观察。 把底座位置从 4095 慢慢拖到 0,观察 3D 模型是否平滑跨过边界;再把俯仰拖到限位边缘,观察物理舵机是否在安全范围内停止。这两个小实验把“数学修复”和“工程限位”变成可看见的现象。
总结
这个项目的技术门槛并不高,但它在两个层面上有实际价值。对社团教学,它用不到两百元的硬件和一块激光切割板,把“数字孪生”从抽象名词变成了学生可以用手掰、用鼠标拖的课堂演示。对 DF 商城的产品生态,它补上了 ST3215 + DRI0057 在 ESP32 Arduino 环境下从“串口直连”到“双向 Web 孪生”的完整链路——尤其是那条反直觉的 TX-TX / RX-RX 接法,如果早一点被写进文档,项目可以少卡一个晚上。
代码和激光切割图纸整理后会在社区继续更新。如果你也在用 ST3215 做多关节结构,欢迎从“串口直连”这一步开始复现,少走一段弯路。
Web端代码
<!DOCTYPE html>
<html lang="zh">
<head>
<meta charset="UTF-8">
<title>工业风 2-DOF 机械臂 - 水平方向修正版</title>
<style>
body { margin: 0; overflow: hidden; font-family: 'Segoe UI', Tahoma, Geneva, Verdana, sans-serif; background-color: #1a1e24; user-select: none; }
#ui {
position: absolute; top: 15px; left: 15px; z-index: 10;
background: rgba(25, 30, 35, 0.85); color: #e0e0e0; padding: 18px 22px;
border-radius: 6px; font-size: 14px; box-shadow: 0 8px 16px rgba(0,0,0,0.6);
border-left: 4px solid #ffb300; width: 300px; backdrop-filter: blur(5px);
}
h2 { margin: 0 0 10px 0; font-size: 16px; color: #fff; letter-spacing: 1px; }
.btn-row { display: flex; gap: 10px; }
button {
margin-top: 10px; padding: 10px; cursor: pointer; width: 100%;
background: #333; color: #fff; border: 1px solid #555; border-radius: 4px; font-weight: bold; transition: 0.2s;
}
button:hover { background: #444; border-color: #ffb300; }
button:disabled { background: #222; color: #666; border-color: #333; cursor: not-allowed; }
.btn-stop { background: #d32f2f; border-color: #b71c1c; }
.btn-stop:hover { background: #f44336; border-color: #ffb300; }
#status { margin-top: 10px; color: #f44336; font-weight: bold; text-align: center; font-size: 13px;}
.slider-group { margin-top: 15px; }
.slider-group label { display: flex; justify-content: space-between; font-size: 12px; color: #aaa; margin-bottom: 5px; }
input[type=range] {
width: 100%; -webkit-appearance: none; background: transparent; height: 6px; border-radius: 3px; background: #333; outline: none;
}
input[type=range]::-webkit-slider-thumb {
-webkit-appearance: none; height: 16px; width: 16px; border-radius: 50%; background: #ffb300; cursor: pointer; border: 2px solid #fff;
}
.data-row { margin-top: 15px; font-family: 'Consolas', monospace; color: #b0bec5; font-size: 14px; line-height: 1.6; border-top: 1px solid #333; padding-top: 10px;}
.data-row span { color: #ffb300; font-weight: bold; }
.hint { font-size: 12px; color: #888; margin-top: 10px;}
.active-mode { color: #4CAF50; font-weight: bold; }
</style>
</head>
<body>
<div id="ui">
<h2>INDUSTRIAL ARM v3.1</h2>
<div class="btn-row">
<button id="connectBtn">连接串口</button>
<button id="stopBtn" class="btn-stop" disabled>急停</button>
</div>
<div id="status">未连接</div>
<div class="slider-group">
<label><span>底座旋转 (Yaw)</span> <span id="yawSliderVal">2048</span></label>
<input type="range" id="yawSlider" min="0" max="4095" value="2048">
</div>
<div class="slider-group">
<label><span>大臂俯仰 (Pitch)</span> <span id="pitchSliderVal">2048</span></label>
<input type="range" id="pitchSlider" min="490" max="3180" value="2048">
</div>
<div class="data-row">
物理反馈: <span id="valYaw">--</span> | <span id="valPitch">--</span>
</div>
<div class="hint">
<b>状态:</b> <span id="modeText" class="active-mode">⚪ 卸力</span><br>
• 拖拽黄色模型或使用滑块。<br>
• 松手即卸力,可用手掰动。
</div>
</div>
<script type="importmap">
{ "imports": {
"three": "https://unpkg.com/three@0.160.0/build/three.module.js",
"three/addons/": "https://unpkg.com/three@0.160.0/examples/jsm/"
} }
</script>
<script type="module">
import * as THREE from 'three';
import { OrbitControls } from 'three/addons/controls/OrbitControls.js';
import { RoomEnvironment } from 'three/addons/environments/RoomEnvironment.js';
// ================= 1. 场景初始化 =================
const scene = new THREE.Scene();
scene.background = new THREE.Color(0x2a303a);
scene.fog = new THREE.Fog(0x2a303a, 8, 20);
const camera = new THREE.PerspectiveCamera(45, innerWidth / innerHeight, 0.1, 100);
camera.position.set(4, 3, 5);
const renderer = new THREE.WebGLRenderer({ antialias: true });
renderer.setSize(innerWidth, innerHeight);
renderer.shadowMap.enabled = true;
renderer.shadowMap.type = THREE.PCFSoftShadowMap;
renderer.toneMapping = THREE.ACESFilmicToneMapping;
renderer.toneMappingExposure = 1.2;
document.body.appendChild(renderer.domElement);
const pmremGenerator = new THREE.PMREMGenerator(renderer);
const envTexture = pmremGenerator.fromScene(new RoomEnvironment(), 0.04).texture;
scene.environment = envTexture;
const controls = new OrbitControls(camera, renderer.domElement);
controls.target.set(0, 0.8, 0);
controls.enableDamping = true;
controls.update();
scene.add(new THREE.AmbientLight(0xffffff, 0.4));
const dirLight = new THREE.DirectionalLight(0xffffff, 1.8);
dirLight.position.set(5, 8, 5);
dirLight.castShadow = true;
dirLight.shadow.mapSize.width = 2048;
dirLight.shadow.mapSize.height = 2048;
scene.add(dirLight);
const rimLight = new THREE.DirectionalLight(0x00d2ff, 2.0);
rimLight.position.set(-5, 2, -5);
scene.add(rimLight);
const gridHelper = new THREE.GridHelper(10, 20, 0x555555, 0x333333);
gridHelper.position.y = -0.01;
scene.add(gridHelper);
// ================= 2. 材质与模型 =================
const matCastIron = new THREE.MeshStandardMaterial({ color: 0x3b3f44, roughness: 0.6, metalness: 0.8 });
const matIndustrialYellow = new THREE.MeshStandardMaterial({ color: 0xffb300, roughness: 0.3, metalness: 0.5 });
const matMotorBlack = new THREE.MeshStandardMaterial({ color: 0x24272b, roughness: 0.2, metalness: 0.9 });
const matSteel = new THREE.MeshStandardMaterial({ color: 0x9ba4b5, roughness: 0.15, metalness: 0.95 });
const robotRoot = new THREE.Group();
scene.add(robotRoot);
const basePlate = new THREE.Mesh(new THREE.BoxGeometry(1.2, 0.1, 1.2), matCastIron);
basePlate.position.y = 0.05; basePlate.receiveShadow = true; robotRoot.add(basePlate);
const baseFlange = new THREE.Mesh(new THREE.CylinderGeometry(0.5, 0.6, 0.15, 32), matSteel);
baseFlange.position.y = 0.15; baseFlange.castShadow = true; robotRoot.add(baseFlange);
const jointYaw = new THREE.Group(); jointYaw.position.y = 0.225; robotRoot.add(jointYaw);
const turretGroup = new THREE.Group(); turretGroup.name = "turret"; jointYaw.add(turretGroup);
const turretBase = new THREE.Mesh(new THREE.CylinderGeometry(0.45, 0.45, 0.2, 32), matIndustrialYellow);
turretBase.position.y = 0.1; turretBase.castShadow = true; turretGroup.add(turretBase);
const bracketL = new THREE.Mesh(new THREE.BoxGeometry(0.15, 0.5, 0.4), matMotorBlack);
bracketL.position.set(-0.35, 0.4, 0); bracketL.castShadow = true; turretGroup.add(bracketL);
const bracketR = bracketL.clone(); bracketR.position.x = 0.35; turretGroup.add(bracketR);
const motorYaw = new THREE.Mesh(new THREE.CylinderGeometry(0.2, 0.2, 0.9, 16), matMotorBlack);
motorYaw.rotation.z = Math.PI / 2; motorYaw.position.y = 0.5; motorYaw.castShadow = true; turretGroup.add(motorYaw);
const jointPitch = new THREE.Group(); jointPitch.position.y = 0.5; turretGroup.add(jointPitch);
const armGroup = new THREE.Group(); armGroup.name = "arm"; jointPitch.add(armGroup);
const pitchFlange = new THREE.Mesh(new THREE.CylinderGeometry(0.18, 0.18, 0.1, 24), matSteel);
pitchFlange.rotation.z = Math.PI / 2; pitchFlange.position.x = -0.4; armGroup.add(pitchFlange);
const pitchFlangeR = pitchFlange.clone(); pitchFlangeR.position.x = 0.4; armGroup.add(pitchFlangeR);
const armLinkMain = new THREE.Mesh(new THREE.BoxGeometry(0.25, 1.4, 0.25), matIndustrialYellow);
armLinkMain.position.y = 0.7; armLinkMain.castShadow = true; armGroup.add(armLinkMain);
const armRib = new THREE.Mesh(new THREE.BoxGeometry(0.1, 1.3, 0.35), matSteel);
armRib.position.y = 0.7; armRib.castShadow = true; armGroup.add(armRib);
const jointElbow = new THREE.Group(); jointElbow.position.y = 1.4; armGroup.add(jointElbow);
const motorElbow = new THREE.Mesh(new THREE.CylinderGeometry(0.15, 0.15, 0.5, 16), matMotorBlack);
motorElbow.rotation.z = Math.PI / 2; motorElbow.castShadow = true; jointElbow.add(motorElbow);
const forearm = new THREE.Mesh(new THREE.BoxGeometry(0.15, 0.15, 0.8), matIndustrialYellow);
forearm.position.z = 0.4; forearm.castShadow = true; jointElbow.add(forearm);
const endFlange = new THREE.Mesh(new THREE.CylinderGeometry(0.12, 0.12, 0.05, 16), matSteel);
endFlange.rotation.x = Math.PI / 2; endFlange.position.z = 0.8; jointElbow.add(endFlange);
const gripperL = new THREE.Mesh(new THREE.BoxGeometry(0.04, 0.2, 0.1), matMotorBlack);
gripperL.position.set(-0.06, 0, 0.85); jointElbow.add(gripperL);
const gripperR = gripperL.clone(); gripperR.position.x = 0.06; jointElbow.add(gripperR);
const PITCH_OFFSET = Math.PI / 6;
jointPitch.rotation.x = PITCH_OFFSET;
// ================= 3. 通信与状态 =================
const PITCH_MIN = 490;
const PITCH_MAX = 3180;
let targetRawYaw = 2048;
let targetRawPitch = 2048;
function rawToRad(raw) { return ((raw - 2048) / 4096) * Math.PI * 2; }
const connectBtn = document.getElementById('connectBtn');
const stopBtn = document.getElementById('stopBtn');
const statusDiv = document.getElementById('status');
const valYaw = document.getElementById('valYaw');
const valPitch = document.getElementById('valPitch');
const modeText = document.getElementById('modeText');
const yawSlider = document.getElementById('yawSlider');
const pitchSlider = document.getElementById('pitchSlider');
const yawSliderVal = document.getElementById('yawSliderVal');
const pitchSliderVal = document.getElementById('pitchSliderVal');
let port = null;
let isInteracting = false;
connectBtn.addEventListener('click', async () => {
if (port) return;
try {
port = await navigator.serial.requestPort();
await port.open({ baudRate: 115200 });
statusDiv.textContent = '✅ 串口已连接';
statusDiv.style.color = '#4CAF50';
connectBtn.disabled = true;
stopBtn.disabled = false;
readLoop();
} catch (e) {
statusDiv.textContent = '❌ 连接失败';
statusDiv.style.color = '#f44336';
}
});
stopBtn.addEventListener('click', () => {
sendCommand('E0');
isInteracting = false;
modeText.innerHTML = '⚪ 已急停 (卸力)';
modeText.style.color = '#4CAF50';
});
async function readLoop() {
const decoder = new TextDecoderStream();
port.readable.pipeTo(decoder.writable);
const reader = decoder.readable.getReader();
let buffer = '';
while (true) {
const { value, done } = await reader.read();
if (done) break;
buffer += value;
const lines = buffer.split('\n');
buffer = lines.pop();
for (const line of lines) {
parseIncoming(line.trim());
}
}
}
function parseIncoming(line) {
const match = line.match(/A(\d+),B(\d+)/);
if (match) {
let rawA = parseInt(match[1]);
let rawB = parseInt(match[2]);
valYaw.textContent = rawA;
valPitch.textContent = rawB;
if (!isInteracting) {
// 如果物理值变化微小,则忽略,防止终点抖动
if (Math.abs(rawA - targetRawYaw) > 3) {
targetRawYaw = rawA;
yawSlider.value = rawA;
yawSliderVal.textContent = rawA;
}
if (Math.abs(rawB - targetRawPitch) > 3) {
targetRawPitch = THREE.MathUtils.clamp(rawB, PITCH_MIN, PITCH_MAX);
pitchSlider.value = targetRawPitch;
pitchSliderVal.textContent = targetRawPitch;
}
}
}
}
let lastSendTime = 0;
async function sendCommand(msg) {
if (!port || !port.writable) return;
const now = Date.now();
if ((msg.startsWith('A') || msg.startsWith('B')) && now - lastSendTime < 30) return;
lastSendTime = now;
const writer = port.writable.getWriter();
await writer.write(new TextEncoder().encode(`${msg}\n`));
writer.releaseLock();
}
function setMode(isPowered) {
if (isPowered) {
sendCommand('E1');
modeText.innerHTML = '🔴 动力接通 (锁定)';
modeText.style.color = '#f44336';
} else {
sendCommand('E0');
modeText.innerHTML = '⚪ 卸力 (自由)';
modeText.style.color = '#4CAF50';
}
}
// 滑块交互
function onSliderChange() {
if (!port) return;
isInteracting = true;
setMode(true);
targetRawYaw = parseInt(yawSlider.value);
targetRawPitch = THREE.MathUtils.clamp(parseInt(pitchSlider.value), PITCH_MIN, PITCH_MAX);
yawSliderVal.textContent = targetRawYaw;
pitchSliderVal.textContent = targetRawPitch;
sendCommand(`A${targetRawYaw}`);
sendCommand(`B${targetRawPitch}`);
}
yawSlider.addEventListener('input', onSliderChange);
pitchSlider.addEventListener('input', onSliderChange);
yawSlider.addEventListener('change', () => { isInteracting = false; setMode(false); });
pitchSlider.addEventListener('change', () => { isInteracting = false; setMode(false); });
// 鼠标拖拽模型
const raycaster = new THREE.Raycaster();
const mouse = new THREE.Vector2();
let activeName = "";
let previousMousePosition = { x: 0, y: 0 };
renderer.domElement.addEventListener('pointerdown', (event) => {
mouse.x = (event.clientX / innerWidth) * 2 - 1;
mouse.y = -(event.clientY / innerHeight) * 2 + 1;
raycaster.setFromCamera(mouse, camera);
const intersects = raycaster.intersectObjects([turretGroup, armGroup], true);
if (intersects.length > 0) {
let tempObj = intersects[0].object;
let isArm = false;
while (tempObj) {
if (tempObj.name === 'arm') { isArm = true; break; }
tempObj = tempObj.parent;
}
activeName = isArm ? "arm" : "turret";
} else {
return;
}
isInteracting = true;
controls.enabled = false;
previousMousePosition = { x: event.clientX, y: event.clientY };
setMode(true);
});
renderer.domElement.addEventListener('pointermove', (event) => {
if (!isInteracting) return;
const deltaX = event.clientX - previousMousePosition.x;
const deltaY = event.clientY - previousMousePosition.y;
previousMousePosition = { x: event.clientX, y: event.clientY };
if (activeName === "turret") {
// 【核心修正】水平方向取反:减去 deltaX
targetRawYaw = THREE.MathUtils.clamp(targetRawYaw - deltaX * 10, 0, 4095);
sendCommand(`A${Math.round(targetRawYaw)}`);
yawSlider.value = targetRawYaw;
yawSliderVal.textContent = Math.round(targetRawYaw);
} else if (activeName === "arm") {
targetRawPitch = THREE.MathUtils.clamp(targetRawPitch - deltaY * 10, PITCH_MIN, PITCH_MAX);
sendCommand(`B${Math.round(targetRawPitch)}`);
pitchSlider.value = targetRawPitch;
pitchSliderVal.textContent = Math.round(targetRawPitch);
}
});
window.addEventListener('pointerup', () => {
if (isInteracting) setMode(false);
isInteracting = false;
activeName = "";
controls.enabled = true;
});
// ================= 4. 渲染算法 =================
function animate() {
requestAnimationFrame(animate);
controls.update();
const lerpFactor = isInteracting ? 0.5 : 0.2;
// 【核心修正】渲染时水平弧度取负
let targetRadYaw = -rawToRad(targetRawYaw);
let targetRadPitch = rawToRad(targetRawPitch) + PITCH_OFFSET;
let diffYaw = targetRadYaw - jointYaw.rotation.y;
diffYaw = Math.atan2(Math.sin(diffYaw), Math.cos(diffYaw));
let diffPitch = targetRadPitch - jointPitch.rotation.x;
diffPitch = Math.atan2(Math.sin(diffPitch), Math.cos(diffPitch));
if (Math.abs(diffYaw) > 0.003) {
jointYaw.rotation.y += diffYaw * lerpFactor;
}
if (Math.abs(diffPitch) > 0.003) {
jointPitch.rotation.x += diffPitch * lerpFactor;
}
renderer.render(scene, camera);
}
animate();
window.addEventListener('resize', () => {
camera.aspect = innerWidth / innerHeight;
camera.updateProjectionMatrix();
renderer.setSize(innerWidth, innerHeight);
});
</script>
</body>
</html>
Firebeetle 2 esp32 c6 硬件代码
#include <SCServo.h>
SMS_STS st;
#define S_RXD 4
#define S_TXD 5
#define SERVO_ID_1 1
#define SERVO_ID_2 2
const int PITCH_MIN = 490;
const int PITCH_MAX = 3180;
unsigned long lastSendTime = 0;
const int sendInterval = 50;
void setup() {
Serial.begin(115200);
Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
st.pSerial = &Serial1;
delay(1000);
st.EnableTorque(SERVO_ID_1, 0);
st.EnableTorque(SERVO_ID_2, 0);
}
void loop() {
if (Serial.available()) {
String line = Serial.readStringUntil('\n');
line.trim();
if (line == "E1") {
st.EnableTorque(SERVO_ID_1, 1);
st.EnableTorque(SERVO_ID_2, 1);
}
else if (line == "E0") {
st.EnableTorque(SERVO_ID_1, 0);
st.EnableTorque(SERVO_ID_2, 0);
}
else if (line.length() >= 2) {
char prefix = line.charAt(0);
int value = line.substring(1).toInt();
if (prefix == 'A') {
value = constrain(value, 0, 4095);
st.WritePosEx(SERVO_ID_1, value, 500, 20);
} else if (prefix == 'B') {
value = constrain(value, PITCH_MIN, PITCH_MAX);
st.WritePosEx(SERVO_ID_2, value, 500, 20);
}
}
}
if (millis() - lastSendTime > sendInterval) {
lastSendTime = millis();
int pos1 = st.ReadPos(SERVO_ID_1);
int pos2 = st.ReadPos(SERVO_ID_2);
if (pos1 >= 0 && pos2 >= 0) {
Serial.print("A"); Serial.print(pos1);
Serial.print(",B"); Serial.println(pos2);
}
}
}




