88 lines
2.5 KiB
JavaScript
88 lines
2.5 KiB
JavaScript
import { getModelType, 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();
|
|
this.motors = [];
|
|
}
|
|
|
|
// 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);
|
|
this.motors.push(motor);
|
|
}
|
|
|
|
getMotor(motorID) {
|
|
for (let i = 0; i < this.motors.length; i++) {
|
|
console.log(motorID, this.motors[i].ID);
|
|
if (String(this.motors[i].ID) === String(motorID)) {
|
|
return this.motors[i];
|
|
}
|
|
}
|
|
return null;
|
|
}
|
|
|
|
|
|
// Remove motor from a position
|
|
removeMotor(motorID) {
|
|
this.motors = this.motors.filter(motor => motor.ID !== motorID);
|
|
}
|
|
|
|
|
|
// 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 modelMajor = data[offset++];
|
|
const modelMinor = data[offset++];
|
|
const modelType = getModelType(modelMajor, modelMinor);
|
|
|
|
const position = (data[offset++] << 8) | data[offset++];
|
|
const motor = new ServoMotor(0, motorID, modelType, name);
|
|
robot.assignMotor(motorID, motor); // motorID used as position ID
|
|
}
|
|
|
|
return robot;
|
|
};
|
|
}
|