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

basic robot config class holds motors and assignments, curves are generated on load (all simulated only so far)

node_mode
realrobots 2025-10-13 20:15:12 +08:00
parent 55b2c178e6
commit 94daeafb79
5 changed files with 144 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
curveEditor.js
View File

@ -42,15 +42,15 @@ export class CurveEditor {
this.panStart = { x: 0, y: 0 };
// Default curve
this.setCurves([
{
startPoint: { x: this.valueToX(0), y: this.valueToY(0) },
startPointHandle: { x: this.valueToX(timelineLength / 2), y: this.valueToY(0.5) },
endPointHandle: { x: this.valueToX(timelineLength / 2), y: this.valueToY(-0.5) },
endPoint: { x: this.valueToX(timelineLength), y: this.valueToY(0) }
}
]);
this.curveSets[this.selectedMotorID] = this.curves;
// this.setCurves([
// {
// startPoint: { x: this.valueToX(0), y: this.valueToY(0) },
// startPointHandle: { x: this.valueToX(timelineLength / 2), y: this.valueToY(0.5) },
// endPointHandle: { x: this.valueToX(timelineLength / 2), y: this.valueToY(-0.5) },
// endPoint: { x: this.valueToX(timelineLength), y: this.valueToY(0) }
// }
// ]);
// this.curveSets[this.selectedMotorID] = this.curves;
// let othercurves = [
// {
@ -73,13 +73,25 @@ export class CurveEditor {
// this.curveSets[7] = othercurves;
this.setSelectedMotor(4);
//this.setSelectedMotor(4);
this.initEvents();
this.draw();
console.log(this.curveSets);
}
addChannel(motorID){
this.setCurves([
{
startPoint: { x: this.valueToX(0), y: this.valueToY(0) },
startPointHandle: { x: this.valueToX(this.timelineLength / 2), y: this.valueToY(0.5) },
endPointHandle: { x: this.valueToX(this.timelineLength / 2), y: this.valueToY(-0.5) },
endPoint: { x: this.valueToX(this.timelineLength), y: this.valueToY(0) }
}
]);
this.curveSets[motorID] = this.curves;
}
setLength(endTime) {
this.timelineLength = endTime / this.pixelsPerSecond;
console.log("new endtime: " + endTime);
@ -685,7 +697,7 @@ export class CurveEditor {
this.dragControlPoint(curve, 'endPointHandle', curve.endPointHandle.x, curve.endPointHandle.y, index);
const nextCurve = this.curves[index + 1];
console.log(curve.endPoint.y);
//console.log(curve.endPoint.y);
if (nextCurve) {
nextCurve.startPoint.x = curve.endPoint.x;
nextCurve.startPoint.y = curve.endPoint.y;

59
feetechDefinitions.js
View File

@ -1,35 +1,42 @@
export class ServoMotor {
constructor(payload) {
this.CHANNEL = payload[0];
this.ID = payload[1];
constructor(arg1, arg2, arg3) {
if (Array.isArray(arg1)) {
// Full payload constructor
const payload = arg1;
this.MODEL = getModelType(payload[3], payload[2]); // minor, major
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.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.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];
const rawSpeed = (payload[24] << 8) | payload[25];
this.CURRENT_SPEED = rawSpeed > 0x7FFF ? rawSpeed - 0x10000 : rawSpeed;
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];
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';
}
}
}

4
index.html
View File

@ -38,7 +38,7 @@
<!-- Tab content -->
<div class="tab-content" id="myTabContent">
<div class="tab-pane fade show active" id="motors" role="tabpanel" aria-labelledby="motors-tab">
<div class="tab-pane fade" id="motors" role="tabpanel" aria-labelledby="motors-tab">
@ -154,7 +154,7 @@
<div class="tab-pane fade" id="animation" role="tabpanel" aria-labelledby="animation-tab">
<div class="tab-pane fade show active" id="animation" role="tabpanel" aria-labelledby="animation-tab">
<canvas id="curveCanvas" width="900" height="600"></canvas>
<div style="margin-top: 10px; text-align: center;">
<input type="range" id="timeSlider" min="0" step="1" style="width: 80%;">

46
robot.js Normal file
View File

@ -0,0 +1,46 @@
import { ServoMotor } from './feetechDefinitions.js';
export class Robot {
constructor(name, firmwareVersionId) {
this.name = name;
this.firmwareVersionId = firmwareVersionId;
// Map of position ID → ServoMotor
this.positionMap = new Map();
}
// Assign a motor to a position
assignMotor(positionId, motor) {
if (!(motor instanceof ServoMotor)) {
throw new Error('Assigned motor must be a ServoMotor instance');
}
this.positionMap.set(positionId, motor);
}
// Get motor assigned to a position
getMotor(positionId) {
return this.positionMap.get(positionId);
}
// Remove motor from a position
removeMotor(positionId) {
this.positionMap.delete(positionId);
}
// Get all position assignments
getAllAssignments() {
const assignments = [];
for (const [position, motor] of this.positionMap.entries()) {
assignments.push({ position, motor });
}
return assignments;
}
// Optional: Find position by motor ID
findPositionByMotorId(id) {
for (const [position, motor] of this.positionMap.entries()) {
if (motor.ID === id) return position;
}
return null;
}
}

39
script.js
View File

@ -1,10 +1,14 @@
import { SerialManager } from './serial.js';
import { ServoMotor, getModelType, writeData } from './feetechDefinitions.js';
import { CurveEditor } from './curveEditor.js';
import { Robot } from './robot.js';
window.onload = () => {
const serial = new SerialManager();
const servoMotors = [[], []]; // index 0 = channel 0, index 1 = channel 1
const statusText = document.getElementById('statusText');
@ -25,6 +29,9 @@ window.onload = () => {
let selectedDial = null;
let draggingKeyframe = null; // { dialIndex, originalFrame }
let isDragging = false;
const dialColors = ['red', 'green', 'blue', 'orange', 'purple'];
const dials = [];
@ -47,6 +54,36 @@ window.onload = () => {
let selectedFile = null;
let connectedRobot = GenerateTestRobot();
console.log(connectedRobot);
for (const [position, motor] of connectedRobot.positionMap.entries()) {
console.log(`Assigning ${position} motor with ID ${motor.ID}`);
curveEditor.addChannel(motor.ID);
}
curveEditor.setSelectedMotor(10);
// TODO: Info should all be loaded on connect from handshake packet
function GenerateTestRobot() {
const robot = new Robot('Atlas', 'FW-2.0.1');
// Create motors manually
let testMotors = [];
let positions = ["eyelids", "headtilt", "neckrotate", "rightshoulder", "rightforearm"];
for (let i = 0; i < 5; i++) {
let motor = new ServoMotor(0, 10 + i, "SCS009")
robot.assignMotor(positions[i], motor);
}
// Retrieve motor by position
//console.log(robot.getMotor('eyelids'));
// List all assignments
console.log(robot.getAllAssignments());
return robot;
}
function clearFileList() {
fileListElement.innerHTML = '';
selectedFile = null;
@ -568,7 +605,7 @@ window.onload = () => {
view.setUint16(offset, seg.endTime, true); offset += 2;
view.setInt16(offset, curveEditor.yToExportRange(seg.startPointY), true); offset += 2;
console.log(curveEditor.yToExportRange(seg.startPointY));
console.log(curveEditor.yToExportRange(seg.startPointY));
view.setUint16(offset, seg.startHandleX, true); offset += 2;
view.setInt16(offset, curveEditor.yToExportRange(seg.startHandleY), true); offset += 2;
view.setUint16(offset, seg.endHandleX, true); offset += 2;