diff --git a/feetechDefinitions.js b/feetechDefinitions.js index 38aa760..c8a8bef 100644 --- a/feetechDefinitions.js +++ b/feetechDefinitions.js @@ -1,45 +1,45 @@ export class ServoMotor { - constructor(arg1, arg2, arg3, arg4) { - if (Array.isArray(arg1)) { - // Full payload constructor - const payload = arg1; + constructor(arg1, arg2, arg3, arg4) { + if (Array.isArray(arg1)) { + // Full payload constructor + const payload = arg1; - this.CHANNEL = payload[0]; - this.ID = payload[1]; - this.MODEL = getModelType(payload[3], payload[2]); + this.CHANNEL = payload[0]; + this.ID = payload[1]; + this.MODEL = getModelType(payload[3], payload[2]); - 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]; + 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; + 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]; - } else { - // Simplified constructor - this.CHANNEL = arg1; - this.ID = arg2; - - this.MODEL = arg3 || 'Unknown Model'; + 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]; + } else { + // Simplified constructor + this.CHANNEL = arg1; + this.ID = arg2; - this.NAME = arg4 || "UNKNOWN"; + this.MODEL = arg3 || 'Unknown Model'; + + this.NAME = arg4 || "UNKNOWN"; + } } - } } @@ -57,11 +57,11 @@ export function writeData(motor, key) { throw new Error(`Motor does not contain value for key: ${key}`); } - + // HANDLE DIFFERENT ADDRESS FOR LOCK ON SMS_STS if (key == 'LOCK') { - if (motor.MODEL && (motor.MODEL.includes("SMS") || motor.MODEL.includes("STS"))) { + if (motor.MODEL && (motor.MODEL.includes("SMS") || motor.MODEL.includes("STS"))) { address = DataMap.LOCK_SMS_STS.address; } else { address = DataMap.LOCK.address; @@ -112,71 +112,80 @@ const DataMap = Object.freeze({ 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"; } +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"] +]); + +export const reverseModelMap = new Map(); +for (const [combined, model] of modelList.entries()) { + const minor = combined >> 8; + const major = combined & 0xFF; + reverseModelMap.set(model, { major, minor }); +} + function combine(major, minor) { return (minor << 8) | major; } diff --git a/index.html b/index.html index 4417a95..fd1188c 100644 --- a/index.html +++ b/index.html @@ -50,16 +50,19 @@
-
+
+
+ +
@@ -85,6 +88,7 @@ + @@ -97,16 +101,19 @@
-
+
+
+ +
MOVING CURRENT VOLTAGENAME
@@ -132,6 +139,7 @@ + diff --git a/script.js b/script.js index 403896a..4135df1 100644 --- a/script.js +++ b/script.js @@ -1,5 +1,5 @@ import { SerialManager } from './serial.js'; -import { ServoMotor, getModelType, writeData } from './feetechDefinitions.js'; +import { ServoMotor, getModelType, reverseModelMap, writeData } from './feetechDefinitions.js'; import { CurveEditor } from './curveEditor.js'; import { Robot } from './robot.js'; import { NodeEditor } from './nodeeditor/NodeEditor.js'; @@ -249,7 +249,7 @@ window.onload = () => { }; - function clearDials(){ + function clearDials() { const dialArea = document.getElementById('dialArea'); dialArea.innerHTML = ''; // Remove all child elements dials = []; @@ -315,7 +315,7 @@ window.onload = () => { dials[ch].value = curveEditor.getMotorPositionAtTime(dials[ch].motorID, currentFrame); } - + } @@ -330,7 +330,7 @@ window.onload = () => { for (let ch = 0; ch < dials.length; ch++) { const value = dials[ch].value; - motorPayloads.push({ motorId: ch, position: value }); + motorPayloads.push({ motorId: dials[ch].motorID, position: value }); } const buffer = new ArrayBuffer(motorPayloads.length * 3); @@ -343,6 +343,8 @@ window.onload = () => { }); const payload = new Uint8Array(buffer); + console.log("SENDING POSITIONTS"); + console.log(payload); serial.sendSetPositions(payload); } } @@ -732,6 +734,92 @@ window.onload = () => { + document.getElementById('btn_apply_config_channel_0').onclick = async () => { + const table = document.querySelector('#channel0-motor-table tbody'); + const rows = table.querySelectorAll('tr'); + const motors = []; + + rows.forEach(row => { + const cells = row.querySelectorAll('td'); + const motor = { + MODEL: reverseModelMap.get(cells[0].textContent.trim()), + ID: parseInt(cells[1].textContent.trim()), + // MIN_ANGLE_LIMIT: parseInt(cells[2].textContent.trim()), + // MAX_ANGLE_LIMIT: parseInt(cells[3].textContent.trim()), + // POSITION: parseInt(cells[4].textContent.trim()), + // CW_DEAD_ZONE: parseInt(cells[5].textContent.trim()), + // CCW_DEAD_ZONE: parseInt(cells[6].textContent.trim()), + // OFFSET: parseInt(cells[7].textContent.trim()), + // MODE: parseInt(cells[8].textContent.trim()), + // TORQUE_ENABLE: parseInt(cells[9].textContent.trim()), + // ACCELERATION: parseInt(cells[10].textContent.trim()), + // GOAL_POSITION: parseInt(cells[11].textContent.trim()), + // GOAL_TIME: parseInt(cells[12].textContent.trim()), + // GOAL_SPEED: parseInt(cells[13].textContent.trim()), + // LOCK: parseInt(cells[14].textContent.trim()), + // CURRENT_SPEED: parseInt(cells[15].textContent.trim()), + // CURRENT_LOAD: parseInt(cells[16].textContent.trim()), + // TEMPERATURE: parseInt(cells[17].textContent.trim()), + // MOVING: parseInt(cells[18].textContent.trim()), + // CURRENT_CURRENT: parseInt(cells[19].textContent.trim()), + // VOLTAGE: parseInt(cells[20].textContent.trim()), + NAME: cells[21].textContent.trim() + }; + + motors.push(motor); + }); + + console.log("Compiled motor list:", motors); + + await serial.sendConfigUpdate(encodeMotorConfig(motors)); + // You can now use this list for saving, sending, or applying config + }; + + function encodeMotorConfig(motors) { + const robotName = "Mr Roboto"; + const firmwareVersion = 1; + + const bufferSize = 1024; // adjust as needed + const buffer = new ArrayBuffer(bufferSize); + const view = new DataView(buffer); + let offset = 0; + + const encoder = new TextEncoder(); + const nameBytes = encoder.encode(robotName); + const nameLength = Math.min(nameBytes.length, 255); // max 255 bytes + + // 🔹 Encode robotName (length + bytes) + view.setUint8(offset++, nameLength); + for (let i = 0; i < nameLength; i++) { + view.setUint8(offset++, nameBytes[i]); + } + + // 🔹 Encode firmwareVersion (2 bytes) + view.setUint16(offset, firmwareVersion, true); offset += 2; + + // 🔹 Encode motor count (1 byte) + view.setUint8(offset++, motors.length); + + // 🔹 Encode motor entries + motors.forEach(motor => { + const { major, minor } = motor.MODEL; + const modelValue = (minor << 8) | major; + + view.setUint16(offset, modelValue, true); offset += 2; // MODEL + view.setUint16(offset, motor.ID, true); offset += 2; // ID + + const motorNameBytes = encoder.encode(motor.NAME); + const motorNameLength = Math.min(motorNameBytes.length, 255); + + view.setUint8(offset++, motorNameLength); + for (let i = 0; i < motorNameLength; i++) { + view.setUint8(offset++, motorNameBytes[i]); + } + }); + + return new Uint8Array(buffer.slice(0, offset)); + } + @@ -771,7 +859,8 @@ window.onload = () => { motor.TEMPERATURE, motor.MOVING, motor.CURRENT_CURRENT, - motor.VOLTAGE + motor.VOLTAGE, + motor.NAME ]; const modelType = motor.MODEL.startsWith('SCS') ? 'SCS' : @@ -792,18 +881,23 @@ window.onload = () => { td.setAttribute('contenteditable', 'true'); td.setAttribute('data-type', 'number'); - 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'); + if (index === 21) { + td.setAttribute('data-type', 'text'); } else { + td.setAttribute('data-type', 'number'); td.setAttribute('data-min', rangeMin.toString()); td.setAttribute('data-max', rangeMax.toString()); + + if (index === 1) { + td.setAttribute('data-min', '0'); + td.setAttribute('data-max', '255'); + } else if (index === 9) { + td.setAttribute('data-min', '0'); + td.setAttribute('data-max', '1'); + } else if (index === 14) { + td.setAttribute('data-min', '0'); + td.setAttribute('data-max', '1'); + } } td.addEventListener('input', function () { diff --git a/serial.js b/serial.js index 814120d..fa9603d 100644 --- a/serial.js +++ b/serial.js @@ -14,6 +14,7 @@ const CMD_PLAY_FILE = 0x08; const CMD_SCAN_CHANNEL = 0x09; const CMD_WRITE_DATA = 0x10; const CMD_READ_DATA = 0x11; +const CMD_WRITE_CONFIG_UPDATE = 0x12; const CMD_START_POSITION_STREAM = 0x14; const POSITION_STREAM = 0x15; @@ -96,6 +97,11 @@ export class SerialManager { await this.send(CMD_WRITE_DATA, payload); } + async sendConfigUpdate(payload) { + console.log(payload); + await this.send(CMD_WRITE_CONFIG_UPDATE, payload); + } + async requestPositionStreaming(stream) { //stream true/false await this.send(CMD_START_POSITION_STREAM, new Uint8Array([stream])); }
MOVING CURRENT VOLTAGENAME