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

implemented minimal config (motors) updating

node_mode
Jake 2025-10-31 01:43:11 +08:00
parent 0fa8e04493
commit 9036014e21
4 changed files with 232 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
feetechDefinitions.js
View File

@ -112,6 +112,12 @@ const DataMap = Object.freeze({
export function getModelType(major, minor) {
const id = combine(major, minor);
return modelList.get(id) || "Unknown Model";
}
const modelList = new Map([
[combine(5, 0), "SCSXX"],
[combine(5, 4), "SCS009"],
@ -173,8 +179,11 @@ export function getModelType(major, minor) {
[combine(9, 40), "SCS40-2"]
]);
const id = combine(major, minor);
return modelList.get(id) || "Unknown Model";
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) {

16
index.html
View File

@ -50,16 +50,19 @@
<div class="channel-box mb-5">
<label class="form-label">Channel 0</label>
<div class="row mb-2">
<div class="col-9">
<!-- <div class="col-9">
<select class="form-select">
<option selected>SCS</option>
<option>STS</option>
<option>SM</option>
</select>
</div>
</div> -->
<div class="col-3">
<button id="btn_scan_channel_0" class="btn btn-primary w-100">Scan</button>
</div>
<div class="col-3">
<button id="btn_apply_config_channel_0" class="btn btn-primary w-100">Apply To Config</button>
</div>
</div>
<table id="channel0-motor-table" class="table table-bordered">
<thead>
@ -85,6 +88,7 @@
<th data-key="MOVING">MOVING</th>
<th data-key="CURRENT_CURRENT">CURRENT</th>
<th data-key="VOLTAGE">VOLTAGE</th>
<th data-key="NAME">NAME</th>
</tr>
</thead>
@ -97,16 +101,19 @@
<div class="channel-box mb-5">
<label class="form-label">Channel 1</label>
<div class="row mb-2">
<div class="col-9">
<!-- <div class="col-9">
<select class="form-select">
<option selected>SCS</option>
<option>STS</option>
<option>SM</option>
</select>
</div>
</div> -->
<div class="col-3">
<button id="btn_scan_channel_1" class="btn btn-primary w-100">Scan</button>
</div>
<div class="col-3">
<button id="btn_apply_config_channel_1" class="btn btn-primary w-100">Apply</button>
</div>
</div>
<table id="channel1-motor-table" class="table table-bordered">
<thead>
@ -132,6 +139,7 @@
<th data-key="MOVING">MOVING</th>
<th data-key="CURRENT_CURRENT">CURRENT</th>
<th data-key="VOLTAGE">VOLTAGE</th>
<th data-key="NAME">NAME</th>
</tr>
</thead>

110
script.js
View File

@ -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';
@ -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 === 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) { // TORQUE ENABLE
} else if (index === 9) {
td.setAttribute('data-min', '0');
td.setAttribute('data-max', '1');
} else if (index === 14) { // EEPROM LOCK
} else if (index === 14) {
td.setAttribute('data-min', '0');
td.setAttribute('data-max', '1');
} else {
td.setAttribute('data-min', rangeMin.toString());
td.setAttribute('data-max', rangeMax.toString());
}
}
td.addEventListener('input', function () {

6
serial.js
View File

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