diff --git a/curveEditor.js b/curveEditor.js index 5c8ad69..98f6659 100644 --- a/curveEditor.js +++ b/curveEditor.js @@ -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; diff --git a/feetechDefinitions.js b/feetechDefinitions.js index 07a897f..f30fd68 100644 --- a/feetechDefinitions.js +++ b/feetechDefinitions.js @@ -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'; } + } } diff --git a/index.html b/index.html index 3ffc948..b495ee1 100644 --- a/index.html +++ b/index.html @@ -38,7 +38,7 @@
-
+
@@ -154,7 +154,7 @@ -
+
diff --git a/robot.js b/robot.js new file mode 100644 index 0000000..d67d353 --- /dev/null +++ b/robot.js @@ -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; + } +} diff --git a/script.js b/script.js index 0f5fc4e..56eb20d 100644 --- a/script.js +++ b/script.js @@ -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; @@ -561,14 +598,14 @@ window.onload = () => { view.setUint16(offset, curveCount, true); offset += 2; // 🔹 Curve segments - + curveSegments.forEach(seg => { view.setUint8(offset++, seg.motorID); view.setUint16(offset, seg.startTime, true); offset += 2; 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;