realrobotshk
/
sophia_controller
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

full spectrum of data read/write (needs testing), feedback checkbox activates position streaming for the 5 test motors

node_mode
Jake 2025-10-05 20:00:18 +08:00
parent cf3765ce5a
commit 2780ad2859
4 changed files with 125 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
feetechDefinitions.js
View File

@ -33,32 +33,43 @@ export class ServoMotor {
}
// 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 },

3
index.html
View File

@ -175,6 +175,9 @@
<label>
<input type="checkbox" id="syncCheckbox"> Sync
</label>
<label>
<input type="checkbox" id="feebackCheckbox"> Feedback
</label>
<canvas id="timelineCanvas" width="800" height="30"></canvas>

60
script.js
View File

@ -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;
@ -918,8 +944,8 @@ window.onload = () => {
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);
};

13
serial.js
View File

@ -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() {
@ -87,6 +91,15 @@ export class SerialManager {
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 = [];