motor data write to servo packets ready(untested)

2025-10-04 23:14:27 +08:00 · 2025-10-04 23:14:27 +08:00 · cf3765ce5a
parent b677dfd9a5
commit cf3765ce5a
4 changed files with 335 additions and 112 deletions
--- a/feetechDefinitions.js
+++ b/feetechDefinitions.js
@ -0,0 +1,161 @@
+export class ServoMotor {
+  constructor(payload) {
+    this.CHANNEL = payload[0];
+    this.ID = payload[1];
+
+    this.MODEL = getModelType(payload[3], payload[2]); // minor, major
+
+    this.MIN_ANGLE_LIMIT = (payload[4] << 8) | payload[5];
+    this.MAX_ANGLE_LIMIT = (payload[6] << 8) | payload[7];
+    this.POSITION = (payload[8] << 8) | payload[9];
+
+    this.CW_DEAD_ZONE = payload[10];
+    this.CCW_DEAD_ZONE = payload[11];
+    this.OFFSET = (payload[12] << 8) | payload[13];
+    this.MODE = payload[14];
+    this.TORQUE_ENABLE = payload[15];
+    this.ACCELERATION = payload[16];
+
+    this.GOAL_POSITION = (payload[17] << 8) | payload[18];
+    this.GOAL_TIME = (payload[19] << 8) | payload[20];
+    this.GOAL_SPEED = (payload[21] << 8) | payload[22];
+    this.LOCK = payload[23];
+
+    const rawSpeed = (payload[24] << 8) | payload[25];
+    this.CURRENT_SPEED = rawSpeed > 0x7FFF ? rawSpeed - 0x10000 : rawSpeed;
+
+    this.CURRENT_LOAD = (payload[26] << 8) | payload[27];
+    this.TEMPERATURE = payload[28];
+    this.MOVING = payload[29];
+    this.CURRENT_CURRENT = (payload[30] << 8) | payload[31];
+    this.VOLTAGE = payload[32];
+  }
+}
+
+
+// Takes Motor object and 
+export function writeData(motor, key) {
+    const entry = DataMap[key];
+    if (!entry) {
+        throw new Error(`Invalid data key: ${key}`);
+    }
+
+    const { address, length } = entry;
+    const value = motor[key];
+
+    if (value === undefined) {
+        throw new Error(`Motor does not contain value for key: ${key}`);
+    }
+
+    const packet = [motor.CHANNEL, motor.ID, address];
+
+    if (length === 2) {
+        packet.push((value >> 8) & 0xFF);
+        packet.push(value & 0xFF);
+    } else if (length === 1) {
+        packet.push(value);
+    } else {
+        throw new Error(`Unsupported byte length: ${length}`);
+    }
+
+    return packet;
+}
+
+
+const DataMap = Object.freeze({
+    MODEL: { address: 0x03, length: 2 },
+    ID: { address: 0x05, length: 1 },
+    BAUD_RATE: { address: 0x06, length: 1 },
+    MIN_ANGLE_LIMIT: { address: 0x09, length: 2 },
+    MAX_ANGLE_LIMIT: { address: 0x0B, length: 2 },
+    CW_DEAD_ZONE: { address: 0x1A, length: 1 },
+    CCW_DEAD_ZONE: { address: 0x1B, length: 1 },
+    OFFSET: { address: 0x1F, length: 2 },
+    MODE: { address: 0x21, length: 1 },
+    TORQUE_ENABLE: { address: 0x28, length: 1 },
+    ACCELERATION: { address: 0x29, length: 1 },
+    GOAL_POSITION: { address: 0x2A, length: 2 },
+    GOAL_TIME: { address: 0x2C, length: 2 },
+    GOAL_SPEED: { address: 0x2E, length: 2 },
+    LOCK_SCS: { address: 0x30, length: 1 },
+    LOCK_SMS_STS: { address: 0x37, length: 1 },
+    POSITION: { address: 0x38, length: 2 },
+    CURRENT_SPEED: { address: 0x3A, length: 2 },
+    CURRENT_LOAD: { address: 0x3C, length: 2 },
+    VOLTAGE: { address: 0x3E, length: 1 },
+    TEMPERATURE: { address: 0x3F, length: 1 },
+    MOVING: { address: 0x42, length: 1 },
+    CURRENT_CURRENT: { address: 0x45, length: 2 }
+});
+
+
+
+export function getModelType(major, minor) {
+    const modelList = new Map([
+        [combine(5, 0), "SCSXX"],
+        [combine(5, 4), "SCS009"],
+        [combine(5, 8), "SCS2332"],
+        [combine(5, 12), "SCS45"],
+        [combine(5, 15), "SCS15"],
+        [combine(5, 16), "SCS315"],
+        [combine(5, 25), "SCS115"],
+        [combine(5, 35), "SCS215"],
+        [combine(5, 40), "SCS40"],
+        [combine(5, 60), "SCS6560"],
+        [combine(5, 240), "SCDZZ"],
+        [combine(6, 0), "SMXX-360M"],
+        [combine(6, 3), "SM30-360M"],
+        [combine(6, 8), "SM60-360M"],
+        [combine(6, 12), "SM80-360M"],
+        [combine(6, 16), "SM100-360M"],
+        [combine(6, 20), "SM150-360M"],
+        [combine(6, 24), "SM85-360M"],
+        [combine(6, 26), "SM60-360M"],
+        [combine(8, 0), "SM30BL"],
+        [combine(8, 1), "SM30BL"],
+        [combine(8, 2), "SM30BL"],
+        [combine(8, 3), "SM30BL"],
+        [combine(8, 4), "SM30BL"],
+        [combine(8, 5), "SM30BL"],
+        [combine(8, 6), "SM30BL"],
+        [combine(8, 7), "SM30BL"],
+        [combine(8, 8), "SM30BL"],
+        [combine(8, 9), "SM30BL"],
+        [combine(8, 10), "SM30BL"],
+        [combine(8, 11), "SM30BL"],
+        [combine(8, 12), "SM30BL"],
+        [combine(8, 13), "SM30BL"],
+        [combine(8, 14), "SM30BL"],
+        [combine(8, 15), "SM30BL"],
+        [combine(8, 16), "SM30BL"],
+        [combine(8, 17), "SM30BL"],
+        [combine(8, 18), "SM30BL"],
+        [combine(8, 19), "SM30BL"],
+        [combine(8, 25), "SM29BL(LJ)"],
+        [combine(8, 29), "SM29BL(FT)"],
+        [combine(8, 30), "SM30BL(FT)"],
+        [combine(8, 20), "SM30BL(LJ)"],
+        [combine(8, 40), "SM40BLHV"],
+        [combine(8, 42), "SM45BLHV"],
+        [combine(8, 44), "SM85BLHV"],
+        [combine(8, 120), "SM120BLHV"],
+        [combine(8, 220), "SM200BLHV"],
+        [combine(9, 0), "STSXX"],
+        [combine(9, 2), "STS3032"],
+        [combine(9, 3), "STS3215"],
+        [combine(9, 4), "STS3040"],
+        [combine(9, 5), "STS3020"],
+        [combine(9, 6), "STS3046"],
+        [combine(9, 20), "SCSXX-2"],
+        [combine(9, 15), "SCS15-2"],
+        [combine(9, 35), "SCS225"],
+        [combine(9, 40), "SCS40-2"]
+    ]);
+
+    const id = combine(major, minor);
+    return modelList.get(id) || "Unknown Model";
+}
+
+function combine(major, minor) {
+    return (minor << 8) | major;
+}
--- a/index.html
+++ b/index.html
@ -45,8 +45,56 @@

        <div class="container mt-3">
          <div class="channel-section">
+
            <!-- Channel 1 -->
-            <div class="channel-box">
+            <div class="channel-box mb-5">
+              <label class="form-label">Channel 0</label>
+              <div class="row mb-2">
+                <div class="col-9">
+                  <select class="form-select">
+                    <option selected>SCS</option>
+                    <option>STS</option>
+                    <option>SM</option>
+                  </select>
+                </div>
+                <div class="col-3">
+                  <button id="btn_scan_channel_0" class="btn btn-primary w-100">Scan</button>
+                </div>
+              </div>
+              <table id="channel0-motor-table" class="table table-bordered">
+                <thead>
+                  <tr>
+                    <th data-key="MODEL">MODEL</th>
+                    <th data-key="ID">ID</th>
+                    <th data-key="MIN_ANGLE_LIMIT">MIN ANGLE</th>
+                    <th data-key="MAX_ANGLE_LIMIT">MAX ANGLE</th>
+                    <th data-key="POSITION">POSITION</th>
+                    <th data-key="CW_DEAD_ZONE">CW DEAD ZONE</th>
+                    <th data-key="CCW_DEAD_ZONE">CCW DEAD ZONE</th>
+                    <th data-key="OFFSET">OFFSET</th>
+                    <th data-key="MODE">MODE</th>
+                    <th data-key="TORQUE_ENABLE">TORQUE ENABLE</th>
+                    <th data-key="ACCELERATION">ACCELERATION</th>
+                    <th data-key="GOAL_POSITION">GOAL POSITION</th>
+                    <th data-key="GOAL_TIME">GOAL TIME</th>
+                    <th data-key="GOAL_SPEED">GOAL SPEED</th>
+                    <th data-key="LOCK">LOCK</th>
+                    <th data-key="CURRENT_SPEED">SPEED</th>
+                    <th data-key="CURRENT_LOAD">LOAD</th>
+                    <th data-key="TEMPERATURE">TEMPERATURE</th>
+                    <th data-key="MOVING">MOVING</th>
+                    <th data-key="CURRENT_CURRENT">CURRENT</th>
+                    <th data-key="VOLTAGE">VOLTAGE</th>
+                  </tr>
+
+                </thead>
+                <tbody></tbody>
+              </table>
+              <button id="btnSendChangesCh0">Send Changes</button>
+            </div>
+
+            <!-- Channel 2 -->
+            <div class="channel-box mb-5">
              <label class="form-label">Channel 1</label>
              <div class="row mb-2">
                <div class="col-9">
@ -60,69 +108,44 @@
                  <button id="btn_scan_channel_1" class="btn btn-primary w-100">Scan</button>
                </div>
              </div>
-              <!-- Channel 1 Table -->
              <table id="channel1-motor-table" class="table table-bordered">
                <thead>
                  <tr>
-                    <th>MODEL</th>
-                    <th>ID</th>
-                    <th>MINANGLE</th>
-                    <th>MAXANGLE</th>
-                    <th>POSITION</th>
+                    <th data-key="MODEL">MODEL</th>
+                    <th data-key="ID">ID</th>
+                    <th data-key="MIN_ANGLE_LIMIT">MIN ANGLE</th>
+                    <th data-key="MAX_ANGLE_LIMIT">MAX ANGLE</th>
+                    <th data-key="POSITION">POSITION</th>
+                    <th data-key="CW_DEAD_ZONE">CW DEAD ZONE</th>
+                    <th data-key="CCW_DEAD_ZONE">CCW DEAD ZONE</th>
+                    <th data-key="OFFSET">OFFSET</th>
+                    <th data-key="MODE">MODE</th>
+                    <th data-key="TORQUE_ENABLE">TORQUE ENABLE</th>
+                    <th data-key="ACCELERATION">ACCELERATION</th>
+                    <th data-key="GOAL_POSITION">GOAL POSITION</th>
+                    <th data-key="GOAL_TIME">GOAL TIME</th>
+                    <th data-key="GOAL_SPEED">GOAL SPEED</th>
+                    <th data-key="LOCK">LOCK</th>
+                    <th data-key="CURRENT_SPEED">SPEED</th>
+                    <th data-key="CURRENT_LOAD">LOAD</th>
+                    <th data-key="TEMPERATURE">TEMPERATURE</th>
+                    <th data-key="MOVING">MOVING</th>
+                    <th data-key="CURRENT_CURRENT">CURRENT</th>
+                    <th data-key="VOLTAGE">VOLTAGE</th>
                  </tr>
+
                </thead>
-                <tbody>
-                  <tr>
-                  </tr>
-                  <tr>
-                  </tr>
-                </tbody>
+                <tbody></tbody>
              </table>
+              <button id="btnSendChangesCh1">Send Changes</button>
            </div>

-            <!-- Channel 2 -->
-            <div class="channel-box">
-              <label class="form-label">Channel 2</label>
-              <div class="row mb-2">
-                <div class="col-9">
-                  <select class="form-select">
-                    <option selected>SCS</option>
-                    <option>STS</option>
-                    <option>SM</option>
-                  </select>
-                </div>
-                <div class="col-3">
-                  <button id="btn_scan_channel_2" class="btn btn-primary w-100">Scan</button>
-                </div>
-              </div>
-
-              <!-- Channel 2 Table -->
-              <table id="channel2-motor-table" class="table table-bordered">
-                <thead>
-                  <tr>
-                    <th>MODEL</th>
-                    <th>ID</th>
-                    <th>MINANGLE</th>
-                    <th>MAXANGLE</th>
-                    <th>POSITION</th>
-                  </tr>
-                </thead>
-                <tbody>
-                  <tr>
-
-                  </tr>
-                  <tr>
-
-                  </tr>
-                </tbody>
-              </table>
-            </div>
          </div>
        </div>



-              <button id="btnSendChanges">Send Changes</button>
+



--- a/script.js
+++ b/script.js
@ -1,14 +1,10 @@
 import { SerialManager } from './serial.js';
-
-const feetechModelsIDs = {
-    777: "STS3215",
-    521: "STS3012",
-    1029: "SCS0009"
-};
+import { ServoMotor, getModelType, writeData } from './feetechDefinitions.js';


 window.onload = () => {
    const serial = new SerialManager();
+    const servoMotors = [[], []]; // index 0 = channel 0, index 1 = channel 1
    const statusText = document.getElementById('statusText');
    const disconnectBtn = document.getElementById('disconnect');
    const connectBtn = document.getElementById('connect');
@ -724,23 +720,45 @@ window.onload = () => {

    // MOTOR CONTROL PANEL

-    function insertTableRow(channel, model, id, minAngle, maxAngle, position) {
-        const tableId = channel === 1 ? 'channel1-motor-table' :
-            channel === 2 ? 'channel2-motor-table' : null;
+    function insertTableRow(motor) {
+        const tableId = motor.CHANNEL === 0 ? 'channel0-motor-table' :
+            motor.CHANNEL === 1 ? 'channel1-motor-table' : null;

        if (!tableId) {
-            console.error('Invalid channel number. Use 1 or 2.');
+            console.error('Invalid channel number. Use 0 or 1.');
            return;
        }

        const tbody = document.querySelector(`#${tableId} tbody`);
        const newRow = document.createElement('tr');
-        newRow.setAttribute('data-row-id', id); // or any unique identifier
+        newRow.setAttribute('data-row-id', motor.ID);

+        const cells = [
+            motor.MODEL,
+            motor.ID,
+            motor.MIN_ANGLE_LIMIT,
+            motor.MAX_ANGLE_LIMIT,
+            motor.POSITION,
+            motor.CW_DEAD_ZONE,
+            motor.CCW_DEAD_ZONE,
+            motor.OFFSET,
+            motor.MODE,
+            motor.TORQUE_ENABLE,
+            motor.ACCELERATION,
+            motor.GOAL_POSITION,
+            motor.GOAL_TIME,
+            motor.GOAL_SPEED,
+            motor.LOCK,
+            motor.CURRENT_SPEED,
+            motor.CURRENT_LOAD,
+            motor.TEMPERATURE,
+            motor.MOVING,
+            motor.CURRENT_CURRENT,
+            motor.VOLTAGE
+        ];

-        const cells = [model, id, minAngle, maxAngle, position];
-        const modelType = model.startsWith('SCS') ? 'SCS' :
-            model.startsWith('STS') ? 'STS' : null;
+        const modelType = motor.MODEL.startsWith('SCS') ? 'SCS' :
+            motor.MODEL.startsWith('STS') ? 'STS' : null;

        const rangeMin = 0;
        const rangeMax = modelType === 'SCS' ? 1023 :
@ -751,19 +769,22 @@ window.onload = () => {
            td.textContent = value;

            if (index === 0) {
-                // Model cell: not editable
                td.classList.add('non-editable');
            } else {
                td.classList.add('editable-cell');
                td.setAttribute('contenteditable', 'true');
                td.setAttribute('data-type', 'number');

-                // ID cell: 0–255
                if (index === 1) {
                    td.setAttribute('data-min', '0');
                    td.setAttribute('data-max', '255');
+                } else if (index === 9) { // TORQUE ENABLE
+                    td.setAttribute('data-min', '0');
+                    td.setAttribute('data-max', '1');
+                } else if (index === 14) { // EEPROM LOCK
+                    td.setAttribute('data-min', '0');
+                    td.setAttribute('data-max', '1');
                } else {
-                    // Angle/Position cells: based on model
                    td.setAttribute('data-min', rangeMin.toString());
                    td.setAttribute('data-max', rangeMax.toString());
                }
@ -785,6 +806,7 @@ window.onload = () => {
                            if (num > max) num = max;
                            preserveCursor(td, num.toString());
                        }
+
                        td.classList.add('edited');
                        td.classList.add('bg-warning');
                        td.title = `Auto-corrected to ${num}`;
@ -800,9 +822,11 @@ window.onload = () => {
        tbody.appendChild(newRow);
    }

+
+
    function collectChangePackets(channel) {
-        const tableId = channel === 1 ? 'channel1-motor-table' :
-            channel === 2 ? 'channel2-motor-table' : null;
+        const tableId = channel === 0 ? 'channel0-motor-table' :
+            channel === 1 ? 'channel1-motor-table' : null;

        const editedCells = document.querySelectorAll(`#${tableId} td.edited`);
        const packets = [];
@ -824,21 +848,19 @@ window.onload = () => {
    }

    function getColumnTitle(cell) {
-        const headers = ['model', 'id', 'minAngle', 'maxAngle', 'position'];
-        const index = [...cell.parentNode.children].indexOf(cell);
-        return headers[index] || `col${index}`;
+        const table = cell.closest('table');
+        const cellIndex = cell.cellIndex;
+
+        const headerRow = table.querySelector('thead tr');
+        const headerCell = headerRow.children[cellIndex];
+
+        return headerCell.dataset.key || headerCell.textContent.trim();
    }

- 

-    document.getElementById('btnSendChanges').onclick = async () => {
-        
-        const packets = collectChangePackets(1); // or 2
-        console.log('Sending packets:', packets);

-        // Replace with actual send logic
-        // fetch('/api/send', { method: 'POST', body: JSON.stringify(packets) })
-    };
+
+

    function preserveCursor(td, newText, forceEnd = false) {
        const selection = window.getSelection();
@ -860,46 +882,74 @@ window.onload = () => {
        selection.addRange(newRange);
    }

+    function getServoMotorByID(channel, id){
+        for (var i = 0; i < servoMotors[channel].length; i++){
+            console.log(servoMotors[channel][i].ID, id);
+            if (servoMotors[channel][i].ID === id){
+                return servoMotors[channel][i];
+            }
+        }
+        return null;
+    }

-
-    document.getElementById('btn_scan_channel_1').onclick = async () => {
+    document.getElementById('btn_scan_channel_0').onclick = async () => {
        // Clear table
-        document.querySelector("#channel1-motor-table tbody").innerHTML = "";
+        document.querySelector("#channel0-motor-table tbody").innerHTML = "";
+        servoMotors[0] = [];

        await serial.requestScan(0);
    };

-    document.getElementById('btn_scan_channel_2').onclick = async () => {
+    document.getElementById('btn_scan_channel_1').onclick = async () => {
        // Clear table
-        document.querySelector("#channel2-motor-table tbody").innerHTML = "";
+        
+        servoMotors[1] = [];
+        document.querySelector("#channel1-motor-table tbody").innerHTML = "";

        await serial.requestScan(1);
    };

+    document.getElementById('btnSendChangesCh0').onclick = async () => {
+
+        const packets = collectChangePackets(0); // or 2
+        for (var i = 0; i < packets.length; i++){
+            let channel = 0
+            let servoMotor = getServoMotorByID(channel, parseInt(packets[i].id, 10));
+            let dataKey = packets[i].title;
+            let value = packets[i].value;
+            servoMotor[dataKey] = value;
+            console.log(writeData(servoMotor, dataKey));
+
+        }
+        console.log(servoMotors);
+        console.log('Sending packets:', packets);
+    };
+
+    document.getElementById('btnSendChangesCh1').onclick = async () => {
+
+        const packets = collectChangePackets(1); // or 2
+        console.log('Sending packets:', packets);
+    };
+
    function handleScanChannelResponse(payload) {
        console.log("received motor info packet: " + payload);
+
        if (payload.length == 2) {
            if (payload[1] == 255) {
                console.log("SCAN COMPLETE");
                document.getElementById('log').value += `Scan Complete\n`;
                return;
            }
-        } else if (payload.length != 10) {
-            console.log("ERROR: INCORRECT PACKET SIZE");
+        } else if (payload.length != 33) {
+            console.log("ERROR: INCORRECT PACKET SIZE: " + payload.length);
            return;
        }
-        const channel = payload[0]; // byte 0
-        const id = payload[1];      // byte 1
-        const model = (payload[2] << 8) | payload[3];
-        const minAngleLimit = (payload[4] << 8) | payload[5];
-        const maxAngleLimit = (payload[6] << 8) | payload[7];
-        const position = (payload[8] << 8) | payload[9];
-        let modelString = feetechModelsIDs[model]
-        if (!modelString) {
-            modelString = "UNKNOWN";
-        }
-        console.log(channel, id, modelString);
-        insertTableRow(channel + 1, modelString, id, minAngleLimit, maxAngleLimit, position);
+        const motor = new ServoMotor(payload);
+        console.log(motor.MODEL, motor.POSITION, motor.CURRENT_SPEED);
+        servoMotors[motor.CHANNEL].push(motor);
+
+        insertTableRow(motor);
+
    }


--- a/style.css
+++ b/style.css
@ -75,18 +75,7 @@ body {



-  .channel-section {
-    display: flex;
-    gap: 2rem;
-  }
-  .channel-box {
-    flex: 1;
-    padding-right: 1rem;
-  }
-  .channel-box + .channel-box {
-    border-left: 1px solid #ccc;
-    padding-left: 1rem;
-  }
+

 canvas {
  background-color: #f0f0f0;