diff --git a/feetechDefinitions.js b/feetechDefinitions.js index a8e7aad..07a897f 100644 --- a/feetechDefinitions.js +++ b/feetechDefinitions.js @@ -1,64 +1,75 @@ export class ServoMotor { - constructor(payload) { - this.CHANNEL = payload[0]; - this.ID = payload[1]; + constructor(payload) { + this.CHANNEL = payload[0]; + this.ID = payload[1]; - this.MODEL = getModelType(payload[3], payload[2]); // minor, major + 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.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.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.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]; - } + 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 +// Takes Motor object and updates simulated value, returns packet for real device export function writeData(motor, key) { const entry = DataMap[key]; if (!entry) { throw new Error(`Invalid data key: ${key}`); } - const { address, length } = entry; + let { 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]; + + + // HANDLE DIFFERENT ADDRESS FOR LOCK ON SMS_STS + if (key == 'LOCK') { + if (motor.MODEL && (motor.MODEL.includes("SMS") || motor.MODEL.includes("STS"))) { + address = DataMap.LOCK_SMS_STS.address; + } else { + address = DataMap.LOCK.address; + } + } + + const packet = [motor.CHANNEL, motor.ID, address, length]; // channel, id, address, value if (length === 2) { - packet.push((value >> 8) & 0xFF); packet.push(value & 0xFF); + packet.push((value >> 8) & 0xFF); } else if (length === 1) { packet.push(value); } else { throw new Error(`Unsupported byte length: ${length}`); } - return packet; + return new Uint8Array(packet); } @@ -77,8 +88,8 @@ const DataMap = Object.freeze({ 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 }, + LOCK: { address: 0x30, length: 1 }, //SCS + LOCK_SMS_STS: { address: 0x37, length: 1 },//SMS_STS POSITION: { address: 0x38, length: 2 }, CURRENT_SPEED: { address: 0x3A, length: 2 }, CURRENT_LOAD: { address: 0x3C, length: 2 }, diff --git a/index.html b/index.html index 1fe8384..7d2bc12 100644 --- a/index.html +++ b/index.html @@ -175,6 +175,9 @@ + diff --git a/script.js b/script.js index b1f8a8d..8344f6d 100644 --- a/script.js +++ b/script.js @@ -9,6 +9,7 @@ window.onload = () => { const disconnectBtn = document.getElementById('disconnect'); const connectBtn = document.getElementById('connect'); const syncCheckbox = document.getElementById("syncCheckbox"); + const feebackCheckbox = document.getElementById("feebackCheckbox"); const clearBtn = document.getElementById("clearAnimation"); // Limits rate of move commands sent while sliding timeslider @@ -329,6 +330,17 @@ window.onload = () => { handleScanChannelResponse(new Uint8Array(payload)) break; + case 0x10: // CMD_SCAN_CHANNEL + console.log(new Uint8Array(payload)); + document.getElementById('log').value += `Data updated\n`; + //handleScanChannelResponse(new Uint8Array(payload)) + break; + + case 0x15: // POSITION STREAM + //console.log(new Uint8Array(payload)); + handlePositionStreamPacket(payload); + break; + // Add more cases as needed default: document.getElementById('log').value += `Unknown command ${command}\n`; @@ -350,6 +362,16 @@ window.onload = () => { } }); + function handlePositionStreamPacket(data) { + for (let i = 0; i < 5; i++) { + const high = data[i * 2]; // High byte + const low = data[i * 2 + 1]; // Low byte + const value = (high << 8) | low; // Combine into uint16_t + + dials[i].value = value; + } + } + function handleLoadedFile(data) { // Ensure data is a Uint8Array console.log(data.buffer); @@ -834,7 +856,7 @@ window.onload = () => { editedCells.forEach(cell => { const row = cell.closest('tr'); const rowId = row.getAttribute('data-row-id'); - const title = getColumnTitle(cell); // see below + const title = getColumnTitle(cell); const value = cell.textContent.trim(); packets.push({ @@ -842,11 +864,15 @@ window.onload = () => { title, value }); + + // Remove the classes + cell.classList.remove('edited', 'bg-warning'); }); return packets; } + function getColumnTitle(cell) { const table = cell.closest('table'); const cellIndex = cell.cellIndex; @@ -882,10 +908,10 @@ window.onload = () => { selection.addRange(newRange); } - function getServoMotorByID(channel, id){ - for (var i = 0; i < servoMotors[channel].length; i++){ + 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){ + if (servoMotors[channel][i].ID === id) { return servoMotors[channel][i]; } } @@ -902,7 +928,7 @@ window.onload = () => { document.getElementById('btn_scan_channel_1').onclick = async () => { // Clear table - + servoMotors[1] = []; document.querySelector("#channel1-motor-table tbody").innerHTML = ""; @@ -912,14 +938,14 @@ window.onload = () => { document.getElementById('btnSendChangesCh0').onclick = async () => { const packets = collectChangePackets(0); // or 2 - for (var i = 0; i < packets.length; i++){ + 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)); - + let dataPacket = writeData(servoMotor, dataKey); + serial.requestWriteData(dataPacket); } console.log(servoMotors); console.log('Sending packets:', packets); @@ -928,6 +954,16 @@ window.onload = () => { document.getElementById('btnSendChangesCh1').onclick = async () => { const packets = collectChangePackets(1); // or 2 + for (var i = 0; i < packets.length; i++) { + let channel = 1 + let servoMotor = getServoMotorByID(channel, parseInt(packets[i].id, 10)); + let dataKey = packets[i].title; + let value = packets[i].value; + servoMotor[dataKey] = value; + let dataPacket = writeData(servoMotor, dataKey); + serial.requestWriteData(dataPacket); + } + console.log(servoMotors); console.log('Sending packets:', packets); }; @@ -954,5 +990,23 @@ window.onload = () => { + feebackCheckbox.addEventListener('change', async function () { + if (feebackCheckbox.checked) { + console.log("Checkbox is checked!"); + serial.requestPositionStreaming(true); + } else { + console.log("Checkbox is unchecked!"); + serial.requestPositionStreaming(false); + } + }); + + function matchPositions() { + // if (feebackCheckbox.checked) { + // console.log("Running every 100ms"); + // } + } + + // Run 10 times per second + const intervalId = setInterval(matchPositions, 100); }; diff --git a/serial.js b/serial.js index 10255bc..814120d 100644 --- a/serial.js +++ b/serial.js @@ -12,6 +12,10 @@ const CMD_DELETE_FILE = 0x04; const CMD_SET_POSITION = 0x07; const CMD_PLAY_FILE = 0x08; const CMD_SCAN_CHANNEL = 0x09; +const CMD_WRITE_DATA = 0x10; +const CMD_READ_DATA = 0x11; +const CMD_START_POSITION_STREAM = 0x14; +const POSITION_STREAM = 0x15; export class SerialManager { constructor() { @@ -60,7 +64,7 @@ export class SerialManager { await this.send(CMD_LOAD_FILE, payload); // CMD_LOAD_FILE } - async requestPlayFile(payload){ + async requestPlayFile(payload) { console.log("Playing File: " + "filename"); await this.send(CMD_PLAY_FILE, payload); @@ -76,17 +80,26 @@ export class SerialManager { await this.send(CMD_DELETE_FILE, payload); } - async sendSetPositions(payload){ + async sendSetPositions(payload) { console.log("Setting positions"); await this.send(CMD_SET_POSITION, payload); } - async requestScan(channel){ - console.log("Scanning Channel " + (channel+1)); + async requestScan(channel) { + console.log("Scanning Channel " + (channel + 1)); let payload = new Uint8Array([channel]); await this.send(CMD_SCAN_CHANNEL, payload); } + async requestWriteData(payload) { + console.log(payload); + await this.send(CMD_WRITE_DATA, payload); + } + + async requestPositionStreaming(stream) { //stream true/false + await this.send(CMD_START_POSITION_STREAM, new Uint8Array([stream])); + } + startReading(onPacket) { const decoder = new TextDecoder(); let buffer = [];