Spaces:
Running
Running
File size: 3,430 Bytes
67a499d 6ce4ca6 f62f94b 67a499d |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 |
import type { Producer, RobotCommand } from "../models.js";
import { ROBOT_CONFIG } from "../config.js";
import { USBServoDriver } from "./USBServoDriver.js";
export class USBProducer extends USBServoDriver implements Producer {
private commandQueue: RobotCommand[] = [];
private isProcessingCommands = false;
constructor(config: any) {
super(config, "Producer");
}
async connect(): Promise<void> {
// Connect to USB first (this triggers browser's device selection dialog)
await this.connectToUSB();
// Lock servos for software control (producer mode)
await this.lockAllServos();
// Note: Calibration is checked when operations are actually needed
}
async disconnect(): Promise<void> {
// Stop command processing
this.isProcessingCommands = false;
this.commandQueue = [];
await this.disconnectFromUSB();
}
async sendCommand(command: RobotCommand): Promise<void> {
if (!this._status.isConnected) {
throw new Error(`[${this.name}] Cannot send command: not connected`);
}
if (!this.isCalibrated) {
throw new Error(`[${this.name}] Cannot send command: not calibrated`);
}
// Add command to queue for processing
this.commandQueue.push(command);
// Limit queue size to prevent memory issues
if (this.commandQueue.length > ROBOT_CONFIG.commands.maxQueueSize) {
this.commandQueue.shift(); // Remove oldest command
}
// Start processing if not already running
if (!this.isProcessingCommands) {
this.processCommandQueue();
}
}
// Event handlers already in base class
// Private methods
private async processCommandQueue(): Promise<void> {
if (this.isProcessingCommands || !this._status.isConnected) {
return;
}
this.isProcessingCommands = true;
while (this.commandQueue.length > 0 && this._status.isConnected) {
const command = this.commandQueue.shift();
if (command) {
try {
await this.executeCommand(command);
} catch (error) {
console.error(`[${this.name}] Command execution failed:`, error);
// Continue processing other commands
}
}
}
this.isProcessingCommands = false;
}
private async executeCommand(command: RobotCommand): Promise<void> {
if (!this.scsServoSDK || !this._status.isConnected) {
return;
}
try {
// Convert normalized values to servo positions using calibration (required)
const servoCommands = new Map<number, number>();
command.joints.forEach((joint) => {
const servoId = this.jointToServoMap[joint.name as keyof typeof this.jointToServoMap];
if (servoId) {
const servoPosition = this.denormalizeValue(joint.value, joint.name);
// Clamp to valid servo range
const clampedPosition = Math.max(0, Math.min(4095, Math.round(servoPosition)));
servoCommands.set(servoId, clampedPosition);
}
});
if (servoCommands.size > 0) {
// Use batch commands when possible for better performance
if (servoCommands.size > 1) {
await this.scsServoSDK.syncWritePositions(servoCommands);
} else {
// Single servo command
const entry = servoCommands.entries().next().value;
if (entry) {
const [servoId, position] = entry;
await this.scsServoSDK.writePosition(servoId, position);
}
}
console.debug(`[${this.name}] Sent positions to ${servoCommands.size} servos`);
}
} catch (error) {
console.error(`[${this.name}] Failed to execute command:`, error);
throw error;
}
}
}
|