Update

SPEC.md

@@ -0,0 +1,99 @@
+# Robot USB Connection System - Summary
+
+## Core Components
+
+### USBProducer (Output)
+**Purpose**: Sends software commands to control physical robot hardware
+- Receives normalized joint commands from software
+- Converts normalized values to raw servo positions
+- Sends position commands to physical servos
+- Locks servos for precise software control
+
+### USBConsumer (Input) 
+**Purpose**: Reads physical robot movements and translates to software commands
+- Continuously monitors physical servo positions
+- Converts raw servo values to normalized percentages
+- Detects movement changes and broadcasts updates
+- Keeps servos unlocked for manual manipulation
+
+## Key Features
+
+### Calibration System (`USBCalibrationPanel.svelte`)
+- **Requirement**: All USB connections must be calibrated before use
+- **Process**: Records min/max physical range for each servo by manual movement
+- **Result**: Establishes mapping between raw servo values (0-4095) and normalized values
+
+### Normalized Communication Protocol
+- **Standard Joints**: -100% to +100% range (bipolar)
+- **Gripper/Jaw**: 0% to +100% range (unipolar) 
+- **Benefits**: Consistent software interface across different robots
+- **Conversion**: Automatic normalization/denormalization based on calibration data
+
+### Multi-Port Support
+- **Capability**: Multiple independent USB connections simultaneously
+- **Independence**: Each connection has its own calibration and configuration
+- **Use Case**: Control multiple robots or multiple connections to same robot
+
+### Batch Operations
+- **Sync Read**: Read multiple servo positions simultaneously
+- **Sync Write**: Send commands to multiple servos in batched operations
+- **Performance**: Reduces USB communication overhead and latency
+
+## Key Constraints
+
+### Connection Exclusivity
+- **Input**: Only one consumer (input source) active per robot at a time
+- **Output**: Multiple producers (output destinations) can be active simultaneously
+- **Rationale**: Prevents conflicting input commands while allowing broadcast to multiple destinations
+
+### Servo Locking Strategy
+- **Consumer Mode**: Servos unlocked → manual movement possible + position reading
+- **Producer Mode**: Servos locked → software control only, no manual movement
+- **Safety**: Prevents mechanical conflicts between manual and software control
+
+### Calibration Dependency
+- **Mandatory**: USB connections cannot establish without valid calibration
+- **Per-Connection**: Each USB port requires independent calibration
+- **Safety**: Prevents commanding impossible positions or damaging hardware
+
+## Additional System Requirements (from SPEC.md analysis)
+
+### Connection Management
+- **Auto-Detection**: System should detect available USB ports automatically
+- **Status Monitoring**: Real-time connection health and error reporting
+- **Graceful Disconnection**: Safe servo unlocking on disconnect/error
+
+### Error Handling & Recovery
+- **Port Conflicts**: Queue management to prevent "Port is busy" errors
+- **Retry Logic**: Automatic retry with backoff for failed servo commands
+- **Connection Recovery**: Automatic reconnection attempts after USB disconnect
+
+### User Interface Integration
+- **Modal Management**: Unified connection setup through calibration panels
+- **Status Display**: Visual indicators for connection state and calibration status
+- **Manual Control**: Direct joint manipulation when no input consumer active
+
+### Performance Optimizations
+- **Polling Rates**: Configurable update frequencies for different use cases
+- **Change Detection**: Only broadcast updates when values actually change
+- **Queueing**: Serial command processing to prevent USB port conflicts
+
+## System Architecture Concepts
+
+### Bidirectional Data Flow
+```
+Physical Robot ←→ USB Hardware ←→ Calibration Layer ←→ Normalized Interface ←→ Software
+```
+
+### Connection Patterns
+- **Teaching Mode**: USB Consumer only (read robot movements)
+- **Control Mode**: USB Producer only (software controls robot)  
+- **Bidirectional**: Both consumer and producer (full interaction)
+- **Broadcasting**: Multiple producers (send to hardware + remote systems)
+
+### Value Transformation Pipeline
+```
+Raw Servo (0-4095) ←→ Calibrated Range ←→ Normalized (-100/+100) ←→ Software Commands
+```
+
+This system provides a robust, safe, and flexible interface for robot hardware control while maintaining consistency across different robot configurations and use cases.

packages/feetech.js/.npmignore

@@ -0,0 +1,3 @@
+*.pdf
+*.xls
+*.html

src/lib/elements/robot/drivers/USBServoDriver.ts

@@ -0,0 +1,415 @@
+import type { ConnectionStatus, USBDriverConfig } from '../models.js';
+import { CalibrationState } from '../calibration/CalibrationState.svelte.js';
+import { ScsServoSDK } from 'feetech.js';
+import { ROBOT_CONFIG } from '../config.js';
+
+export abstract class USBServoDriver {
+  readonly id: string;
+  readonly name: string;
+  readonly config: USBDriverConfig;
+  
+  protected _status: ConnectionStatus = { isConnected: false };
+  protected statusCallbacks: ((status: ConnectionStatus) => void)[] = [];
+  
+  protected scsServoSDK: ScsServoSDK | null = null;
+  
+  // Calibration state - directly embedded
+  readonly calibrationState: CalibrationState;
+  
+  // Calibration polling
+  private calibrationPollingInterval: ReturnType<typeof setInterval> | null = null;
+  
+  // Joint to servo ID mapping for SO-100 arm
+  protected readonly jointToServoMap = {
+    "Rotation": 1,
+    "Pitch": 2,
+    "Elbow": 3,
+    "Wrist_Pitch": 4,
+    "Wrist_Roll": 5,
+    "Jaw": 6
+  };
+
+  constructor(config: USBDriverConfig, driverType: string) {
+    this.config = config;
+    this.id = `usb-${driverType}-${Date.now()}`;
+    this.name = `USB ${driverType}`;
+    this.calibrationState = new CalibrationState();
+  }
+
+  get status(): ConnectionStatus {
+    return this._status;
+  }
+
+  get needsCalibration(): boolean {
+    return this.calibrationState.needsCalibration;
+  }
+
+  get isCalibrating(): boolean {
+    return this.calibrationState.isCalibrating;
+  }
+
+  get isCalibrated(): boolean {
+    return this.calibrationState.isCalibrated;
+  }
+
+  // Type guard to check if a driver is a USB driver
+  static isUSBDriver(driver: any): driver is USBServoDriver {
+    return driver && typeof driver.calibrationState === 'object' && 
+           typeof driver.needsCalibration === 'boolean' &&
+           typeof driver.isCalibrated === 'boolean' &&
+           typeof driver.startCalibration === 'function';
+  }
+
+  // Type-safe method to get calibration interface
+  getCalibrationInterface(): {
+    needsCalibration: boolean;
+    isCalibrating: boolean;
+    isCalibrated: boolean;
+    startCalibration: () => Promise<void>;
+    completeCalibration: () => Promise<Record<string, number>>;
+    skipCalibration: () => void;
+    cancelCalibration: () => void;
+    onCalibrationCompleteWithPositions: (callback: (positions: Record<string, number>) => void) => () => void;
+  } {
+    return {
+      needsCalibration: this.needsCalibration,
+      isCalibrating: this.isCalibrating,
+      isCalibrated: this.isCalibrated,
+      startCalibration: () => this.startCalibration(),
+      completeCalibration: () => this.completeCalibration(),
+      skipCalibration: () => this.skipCalibration(),
+      cancelCalibration: () => this.cancelCalibration(),
+      onCalibrationCompleteWithPositions: (callback) => this.onCalibrationCompleteWithPositions(callback)
+    };
+  }
+
+  // Abstract methods that subclasses must implement
+  abstract connect(): Promise<void>;
+  abstract disconnect(): Promise<void>;
+
+  // Common connection logic
+  protected async connectToUSB(): Promise<void> {
+    if (this._status.isConnected) {
+      console.log(`[${this.name}] Already connected`);
+      return;
+    }
+
+    try {
+      console.log(`[${this.name}] Connecting...`);
+      
+      // Create a new SDK instance for this driver instead of using the singleton
+      // This allows multiple drivers to connect to different ports simultaneously
+      this.scsServoSDK = new ScsServoSDK();
+      
+      await this.scsServoSDK.connect({
+        baudRate: this.config.baudRate || ROBOT_CONFIG.usb.baudRate,
+        protocolEnd: 0 // STS/SMS protocol
+      });
+
+      this._status = { isConnected: true, lastConnected: new Date() };
+      this.notifyStatusChange();
+      
+      console.log(`[${this.name}] Connected successfully`);
+      
+      // Debug: Log SDK instance methods to identify the issue
+      console.log(`[${this.name}] SDK instance methods:`, Object.getOwnPropertyNames(this.scsServoSDK));
+      console.log(`[${this.name}] SDK prototype methods:`, Object.getOwnPropertyNames(Object.getPrototypeOf(this.scsServoSDK)));
+      console.log(`[${this.name}] writeTorqueEnable available:`, typeof this.scsServoSDK.writeTorqueEnable);
+      console.log(`[${this.name}] syncReadPositions available:`, typeof this.scsServoSDK.syncReadPositions);
+      
+    } catch (error) {
+      console.error(`[${this.name}] Connection failed:`, error);
+      this._status = { isConnected: false, error: `Connection failed: ${error}` };
+      this.notifyStatusChange();
+      throw error;
+    }
+  }
+
+  protected async disconnectFromUSB(): Promise<void> {
+    if (this.scsServoSDK) {
+      try {
+        await this.unlockAllServos();
+        await this.scsServoSDK.disconnect();
+      } catch (error) {
+        console.warn(`[${this.name}] Error during disconnect:`, error);
+      }
+      this.scsServoSDK = null;
+    }
+
+    this._status = { isConnected: false };
+    this.notifyStatusChange();
+    
+    console.log(`[${this.name}] Disconnected`);
+  }
+
+  // Calibration methods
+  async startCalibration(): Promise<void> {
+    if (!this._status.isConnected) {
+      await this.connectToUSB();
+    }
+
+    if (!this._status.isConnected) {
+      throw new Error('Cannot start calibration: not connected');
+    }
+
+    console.log(`[${this.name}] Starting calibration...`);
+    this.calibrationState.startCalibration();
+    
+    // Unlock servos for manual movement during calibration
+    await this.unlockAllServos();
+    
+    // Start polling positions during calibration
+    await this.startCalibrationPolling();
+  }
+
+  async completeCalibration(): Promise<Record<string, number>> {
+    if (!this.isCalibrating) {
+      throw new Error('Not currently calibrating');
+    }
+
+    // Stop polling
+    this.stopCalibrationPolling();
+    
+    // Read final positions
+    const finalPositions = await this.readCurrentPositions();
+    
+    // Complete calibration state
+    this.calibrationState.completeCalibration();
+    
+    console.log(`[${this.name}] Calibration completed`);
+    return finalPositions;
+  }
+
+  skipCalibration(): void {
+    // Stop polling if active
+    this.stopCalibrationPolling();
+    this.calibrationState.skipCalibration();
+  }
+
+  async setPredefinedCalibration(): Promise<void> {
+    // Stop polling if active
+    this.stopCalibrationPolling();
+    this.skipCalibration();
+  }
+
+  // Cancel calibration
+  cancelCalibration(): void {
+    // Stop polling if active
+    this.stopCalibrationPolling();
+    this.calibrationState.cancelCalibration();
+  }
+
+  // Start polling servo positions during calibration
+  private async startCalibrationPolling(): Promise<void> {
+    if (this.calibrationPollingInterval !== null) {
+      return; // Already polling
+    }
+
+    console.log(`[${this.name}] Starting calibration position polling...`);
+    
+    // Poll positions every 100ms during calibration
+    this.calibrationPollingInterval = setInterval(async () => {
+      if (!this.isCalibrating || !this._status.isConnected || !this.scsServoSDK) {
+        this.stopCalibrationPolling();
+        return;
+      }
+
+      try {
+        // Read positions for all servos
+        const servoIds = Object.values(this.jointToServoMap);
+        const positions = await this.scsServoSDK.syncReadPositions(servoIds);
+        
+        // Update calibration state with current positions
+        Object.entries(this.jointToServoMap).forEach(([jointName, servoId]) => {
+          const position = positions.get(servoId);
+          if (position !== undefined) {
+            this.calibrationState.updateCurrentValue(jointName, position);
+            console.debug(`[${this.name}] ${jointName} (servo ${servoId}): ${position}`);
+          }
+        });
+        
+      } catch (error) {
+        console.warn(`[${this.name}] Calibration polling error:`, error);
+        // Continue polling despite errors - user might be moving servos rapidly
+      }
+    }, 100); // Poll every 100ms
+  }
+
+  // Stop polling servo positions
+  private stopCalibrationPolling(): void {
+    if (this.calibrationPollingInterval !== null) {
+      clearInterval(this.calibrationPollingInterval);
+      this.calibrationPollingInterval = null;
+      console.log(`[${this.name}] Stopped calibration position polling`);
+    }
+  }
+
+  // Servo position reading (for calibration)
+  async readCurrentPositions(): Promise<Record<string, number>> {
+    if (!this.scsServoSDK || !this._status.isConnected) {
+      throw new Error('Cannot read positions: not connected');
+    }
+
+    const positions: Record<string, number> = {};
+    
+    try {
+      const servoIds = Object.values(this.jointToServoMap);
+      const servoPositions = await this.scsServoSDK.syncReadPositions(servoIds);
+      
+      Object.entries(this.jointToServoMap).forEach(([jointName, servoId]) => {
+        const position = servoPositions.get(servoId);
+        if (position !== undefined) {
+          positions[jointName] = position;
+          // Update calibration state with current position
+          this.calibrationState.updateCurrentValue(jointName, position);
+        }
+      });
+      
+    } catch (error) {
+      console.error(`[${this.name}] Error reading positions:`, error);
+      throw error;
+    }
+    
+    return positions;
+  }
+
+  // Value conversion methods
+  normalizeValue(rawValue: number, jointName: string): number {
+    if (!this.isCalibrated) {
+      throw new Error('Cannot normalize value: not calibrated');
+    }
+    return this.calibrationState.normalizeValue(rawValue, jointName);
+  }
+
+  denormalizeValue(normalizedValue: number, jointName: string): number {
+    if (!this.isCalibrated) {
+      throw new Error('Cannot denormalize value: not calibrated');
+    }
+    return this.calibrationState.denormalizeValue(normalizedValue, jointName);
+  }
+
+  // Servo control methods
+  protected async lockAllServos(): Promise<void> {
+    if (!this.scsServoSDK) return;
+
+    try {
+      console.log(`[${this.name}] Locking all servos...`);
+      
+      const servoIds = Object.values(this.jointToServoMap);
+      
+      for (const servoId of servoIds) {
+        try {
+          // Check if writeTorqueEnable method exists before calling
+          if (typeof this.scsServoSDK.writeTorqueEnable !== 'function') {
+            console.warn(`[${this.name}] writeTorqueEnable method not available on SDK instance for servo ${servoId}`);
+            continue;
+          }
+          
+          await this.scsServoSDK.writeTorqueEnable(servoId, true);
+          await new Promise(resolve => setTimeout(resolve, ROBOT_CONFIG.usb.servoWriteDelay));
+        } catch (error) {
+          console.warn(`[${this.name}] Failed to lock servo ${servoId}:`, error);
+        }
+      }
+      
+      console.log(`[${this.name}] All servos locked`);
+      
+    } catch (error) {
+      console.error(`[${this.name}] Error locking servos:`, error);
+    }
+  }
+
+  protected async unlockAllServos(): Promise<void> {
+    if (!this.scsServoSDK) return;
+
+    try {
+      console.log(`[${this.name}] Unlocking all servos...`);
+      
+      const servoIds = Object.values(this.jointToServoMap);
+      
+      for (const servoId of servoIds) {
+        try {
+          // Check if writeTorqueEnable method exists before calling
+          if (typeof this.scsServoSDK.writeTorqueEnable !== 'function') {
+            console.warn(`[${this.name}] writeTorqueEnable method not available on SDK instance for servo ${servoId}`);
+            continue;
+          }
+          
+          await this.scsServoSDK.writeTorqueEnable(servoId, false);
+          await new Promise(resolve => setTimeout(resolve, ROBOT_CONFIG.usb.servoWriteDelay));
+        } catch (error) {
+          console.warn(`[${this.name}] Failed to unlock servo ${servoId}:`, error);
+        }
+      }
+      
+      console.log(`[${this.name}] All servos unlocked`);
+      
+    } catch (error) {
+      console.error(`[${this.name}] Error unlocking servos:`, error);
+    }
+  }
+
+  // Event handlers
+  onStatusChange(callback: (status: ConnectionStatus) => void): () => void {
+    this.statusCallbacks.push(callback);
+    return () => {
+      const index = this.statusCallbacks.indexOf(callback);
+      if (index >= 0) {
+        this.statusCallbacks.splice(index, 1);
+      }
+    };
+  }
+
+  protected notifyStatusChange(): void {
+    this.statusCallbacks.forEach(callback => {
+      try {
+        callback(this._status);
+      } catch (error) {
+        console.error(`[${this.name}] Error in status callback:`, error);
+      }
+    });
+  }
+
+  // Register callback for calibration completion with positions
+  onCalibrationCompleteWithPositions(callback: (positions: Record<string, number>) => void): () => void {
+    return this.calibrationState.onCalibrationCompleteWithPositions(callback);
+  }
+
+  // Sync robot joint positions using normalized values from calibration
+  syncRobotPositions(finalPositions: Record<string, number>, updateRobotCallback?: (jointName: string, normalizedValue: number) => void): void {
+    if (!updateRobotCallback) return;
+
+    console.log(`[${this.name}] 🔄 Syncing robot to final calibration positions...`);
+    
+    Object.entries(finalPositions).forEach(([jointName, rawPosition]) => {
+      try {
+        // Convert raw servo position to normalized value using calibration
+        const normalizedValue = this.normalizeValue(rawPosition, jointName);
+        
+        // Clamp to appropriate normalized range based on joint type
+        let clampedValue: number;
+        if (jointName.toLowerCase() === 'jaw' || jointName.toLowerCase() === 'gripper') {
+          clampedValue = Math.max(0, Math.min(100, normalizedValue));
+        } else {
+          clampedValue = Math.max(-100, Math.min(100, normalizedValue));
+        }
+        
+        console.log(`[${this.name}] ${jointName}: ${rawPosition} (raw) -> ${normalizedValue.toFixed(1)} -> ${clampedValue.toFixed(1)} (normalized)`);
+        
+        // Update robot joint through callback
+        updateRobotCallback(jointName, clampedValue);
+      } catch (error) {
+        console.warn(`[${this.name}] Failed to sync position for joint ${jointName}:`, error);
+      }
+    });
+    
+    console.log(`[${this.name}] ✅ Robot synced to calibration positions`);
+  }
+
+  // Cleanup
+  async destroy(): Promise<void> {
+    this.stopCalibrationPolling();
+    await this.disconnect();
+    this.calibrationState.destroy();
+  }
+}