从激光切割到双向数字孪生:一台 ST3215 总线舵机机械臂的创客教学实战

2026-10-313

背景:为什么在社团课上做“孪生”?

在创客社团的教学里,“数字孪生”这个概念常常停留在视频和 PPT 里——学生看到的往往是工业场景中昂贵的六轴机器人,很难亲手触摸到“虚拟模型与真实硬件同步运动”的完整闭环。这个项目最初的出发点很朴素:用一台学生自己用激光切割做出来的 2 自由度机械臂,把“孪生”从概念变成可触摸、可拖拽、可手掰的课堂演示。

选型上遇到了一个现实问题。DF 商城的 ST3215 串行总线舵机(7.4V 19.5kg 360°) 和 串行总线舵机驱动板(DRI0057) 都是非常合适的硬件:ST3215 自带 12 位磁编码器,能实时回读 0-4095 的绝对角度,天然适合做“位置反馈”;DRI0057 通过跳线帽切换 UART/USB 双模式,支持 9-12.6V 宽电压输入,单板可级联多达 253 个总线舵机。但当时这两款产品在商城页面上都没有可直接照搬的 Arduino + ESP32 使用文档,社区里也几乎找不到完整的“读角度 + 双向控制”的实例。于是这个项目也带上了第二层意义:把踩过的坑和跑通的代码整理成一份可复现的文档,填补这块空白。
从激光切割到双向数字孪生:一台 ST3215 总线舵机机械臂的创客教学实战_image_1.webp

硬件清单

部件 型号/规格 数量 说明
主控 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

从激光切割到双向数字孪生:一台 ST3215 总线舵机机械臂的创客教学实战_image_2.webp

结构:激光切割一台“够用”的 2 自由度臂

结构设计的出发点不是追求自由度数量,而是让两个关节的运动干净、可观测、不干涉。

底座负责水平偏航(Yaw),中间关节负责俯仰(Pitch)。椴木板用于承力连杆和底座法兰,亚克力用于需要观察内部走线的转台侧板和舵机安装夹板。设计时特意在俯仰关节两侧留出了舵机盘的让位槽,避免螺丝头在转动时刮蹭板面。
这个项目俯仰安全行程被限制在490~3180(对应约 43°~280°) ,上下都留出了板件和底座之间的余量,防止在示教或拖拽时发生碰撞。
从激光切割到双向数字孪生:一台 ST3215 总线舵机机械臂的创客教学实战_image_3.webp

电子连接:那个让项目卡了一整晚的“串口直连”

这是整个项目中最值得写进文档的一条经验。

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”双向数字孪生架构在社区中已有验证。
从激光切割到双向数字孪生:一台 ST3215 总线舵机机械臂的创客教学实战_image_4.webp

协议设计

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);
    }
  }
}

创作许可协议

本项目采用 CC BY(署名) 进行许可。

评论(0)
- 没有更多了 -