84 lines
2.3 KiB
JavaScript
84 lines
2.3 KiB
JavaScript
import { ServoMotor } from './feetechDefinitions.js';
|
|
|
|
export class Robot {
|
|
constructor(name, firmwareVersionId) {
|
|
this.deviceName = name;
|
|
this.firmwareVersion = { major: 0, minor: 0 };
|
|
|
|
// 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;
|
|
}
|
|
|
|
static fromBytes(bytes) {
|
|
console.log("Loading Robot from:");
|
|
console.log(bytes);
|
|
const data = bytes instanceof Uint8Array ? bytes : Uint8Array.from(bytes);
|
|
let offset = 0;
|
|
|
|
// Device name
|
|
const nameLen = data[offset++];
|
|
const deviceName = String.fromCharCode(...data.slice(offset, offset + nameLen));
|
|
offset += nameLen;
|
|
|
|
// Firmware version
|
|
const firmwareMajor = data[offset++];
|
|
const firmwareMinor = data[offset++];
|
|
|
|
// Create Robot instance
|
|
const robot = new Robot(deviceName, `${firmwareMajor}.${firmwareMinor}`);
|
|
robot.firmwareVersion.major = firmwareMajor;
|
|
robot.firmwareVersion.minor = firmwareMinor;
|
|
|
|
// Motor count
|
|
const motorCount = data[offset++];
|
|
|
|
for (let i = 0; i < motorCount; i++) {
|
|
const motorID = data[offset++];
|
|
const nameLen = data[offset++];
|
|
const name = String.fromCharCode(...data.slice(offset, offset + nameLen));
|
|
offset += nameLen;
|
|
|
|
const position = (data[offset++] << 8) | data[offset++];
|
|
const motor = new ServoMotor(0, motorID, null, name);
|
|
robot.assignMotor(motorID, motor); // motorID used as position ID
|
|
}
|
|
|
|
return robot;
|
|
};
|
|
}
|