Update

packages/feetech.js/README.md

@@ -1,8 +1,10 @@
 # feetech.js
 
+> [bambot.org/feetech.js](https://bambot.org/feetech.js)
+
 Control feetech servos through browser
 
-## Usage
+## Quick start
 
 ```bash
 # Install the package
@@ -10,15 +12,29 @@ npm install feetech.js
 ```
 
 ```javascript
-import { scsServoSDK } from "feetech.js";
+import { ScsServoSDK } from "feetech.js";
+const scsServoSdk = new ScsServoSDK();
 
-await scsServoSDK.connect();
+// request permission to access the USB device and connect to it
+// Note: This will prompt the user to select a USB device
+await scsServoSdk.connect();
 
-const position = await scsServoSDK.readPosition(1);
+// read servo position
+const position = await scsServoSdk.readPosition(1);
 console.log(position); // 1122
 ```
 
+## Documentations
+
+https://deepwiki.com/timqian/bambot/4.1-feetech.js-sdk
+
+
 ## Example usage:
 
-- simple example: [test.html](./test.html)
-- the bambot website: [bambot.org](https://bambot.org)
+- Test and config servos: [bambot.org/feetech.js](https://bambot.org/feetech.js)
+- Simple html + js example: [test.html](https://github.com/timqian/bambot/blob/main/feetech.js/test.html)
+- Control different bots: [bambot.org](https://bambot.org)
+
+## Ref
+
+- https://github.com/Adam-Software/Feetech-Servo-SDK

packages/feetech.js/debug.mjs

@@ -1,15 +0,0 @@
-/**
- * Debug configuration for feetech.js
- * Set DEBUG_ENABLED to false to disable all console.log statements for performance
- */
-export const DEBUG_ENABLED = true; // Set to true to enable debug logging
-
-/**
- * Conditional logging function that respects the DEBUG_ENABLED flag
- * @param {...any} args - Arguments to log
- */
-export const debugLog = (...args) => {
-    if (DEBUG_ENABLED) {
-        console.log(...args);
-    }
-}; 

packages/feetech.js/index.d.ts

@@ -1,50 +1,28 @@
 export type ConnectionOptions = {
-	baudRate?: number;
-	protocolEnd?: number;
+  baudRate?: number;
+  protocolEnd?: number;
 };
 
 export type ServoPositions = Map<number, number> | Record<number, number>;
-export type ServoSpeeds = Map<number, number> | Record<number, number>;
-
-export interface ScsServoSDK {
-	// Connection management
-	connect(options?: ConnectionOptions): Promise<true>;
-	disconnect(): Promise<true>;
-	isConnected(): boolean;
-
-	// Servo locking operations
-	lockServo(servoId: number): Promise<"success">;
-	unlockServo(servoId: number): Promise<"success">;
-	lockServos(servoIds: number[]): Promise<"success">;
-	unlockServos(servoIds: number[]): Promise<"success">;
-	lockServosForProduction(servoIds: number[]): Promise<"success">;
-	unlockServosForManualMovement(servoIds: number[]): Promise<"success">;
-
-	// Read operations (no locking needed)
-	readPosition(servoId: number): Promise<number>;
-	syncReadPositions(servoIds: number[]): Promise<Map<number, number>>;
-
-	// Write operations - LOCKED MODE (respects servo locks)
-	writePosition(servoId: number, position: number): Promise<"success">;
-	writeTorqueEnable(servoId: number, enable: boolean): Promise<"success">;
-
-	// Write operations - UNLOCKED MODE (temporary unlock for operation)
-	writePositionUnlocked(servoId: number, position: number): Promise<"success">;
-	writePositionAndDisableTorque(servoId: number, position: number, waitTimeMs?: number): Promise<"success">;
-	writeTorqueEnableUnlocked(servoId: number, enable: boolean): Promise<"success">;
-
-	// Sync write operations
-	syncWritePositions(servoPositions: ServoPositions): Promise<"success">;
-
-	// Configuration functions
-	setBaudRate(servoId: number, baudRateIndex: number): Promise<"success">;
-	setServoId(currentServoId: number, newServoId: number): Promise<"success">;
-	setWheelMode(servoId: number): Promise<"success">;
-	setPositionMode(servoId: number): Promise<"success">;
+export type ServoSpeeds = Map<number, number> | Record<number, number>; // New type alias for speeds
+
+export declare class ScsServoSDK {
+  connect(options?: ConnectionOptions): Promise<true>;
+  disconnect(): Promise<true>;
+  readPosition(servoId: number): Promise<number>;
+  readBaudRate(servoId: number): Promise<number>;
+  readMode(servoId: number): Promise<number>;
+  writePosition(servoId: number, position: number): Promise<"success">;
+  writeTorqueEnable(servoId: number, enable: boolean): Promise<"success">;
+  writeAcceleration(servoId: number, acceleration: number): Promise<"success">;
+  setWheelMode(servoId: number): Promise<"success">;
+  setPositionMode(servoId: number): Promise<"success">;
+  writeWheelSpeed(servoId: number, speed: number): Promise<"success">;
+  syncReadPositions(servoIds: number[]): Promise<Map<number, number>>;
+  syncWritePositions(servoPositions: ServoPositions): Promise<"success">;
+  syncWriteWheelSpeed(servoSpeeds: ServoSpeeds): Promise<"success">;
+  setBaudRate(servoId: number, baudRateIndex: number): Promise<"success">;
+  setServoId(currentServoId: number, newServoId: number): Promise<"success">;
 }
 
-export const scsServoSDK: ScsServoSDK;
-
-// Debug exports
-export const DEBUG_ENABLED: boolean;
-export function debugLog(message: string): void;
+export declare const scsServoSDK: ScsServoSDK;

packages/feetech.js/index.mjs

@@ -1,65 +1,8 @@
-// Import all functions from the unified scsServoSDK module
-import {
-	connect,
-	disconnect,
-	isConnected,
-	lockServo,
-	unlockServo,
-	lockServos,
-	unlockServos,
-	lockServosForProduction,
-	unlockServosForManualMovement,
-	readPosition,
-	syncReadPositions,
-	writePosition,
-	writeTorqueEnable,
-	writePositionUnlocked,
-	writePositionAndDisableTorque,
-	writeTorqueEnableUnlocked,
-	syncWritePositions,
-	setBaudRate,
-	setServoId,
-	setWheelMode,
-	setPositionMode
-} from "./scsServoSDK.mjs";
+// Import all functions from the new scsServoSDK module
 
-// Create the unified SCS servo SDK object
-export const scsServoSDK = {
-	// Connection management
-	connect,
-	disconnect,
-	isConnected,
+// Export the class and singleton instance
+export { ScsServoSDK, scsServoSDK } from "./scsServoSDK.mjs";
 
-	// Servo locking operations
-	lockServo,
-	unlockServo,
-	lockServos,
-	unlockServos,
-	lockServosForProduction,
-	unlockServosForManualMovement,
-
-	// Read operations (no locking needed)
-	readPosition,
-	syncReadPositions,
-
-	// Write operations - LOCKED MODE (respects servo locks)
-	writePosition,
-	writeTorqueEnable,
-
-	// Write operations - UNLOCKED MODE (temporary unlock for operation)
-	writePositionUnlocked,
-	writePositionAndDisableTorque,
-	writeTorqueEnableUnlocked,
-
-	// Sync write operations
-	syncWritePositions,
-
-	// Configuration functions
-	setBaudRate,
-	setServoId,
-	setWheelMode,
-	setPositionMode
-};
-
-// Export debug configuration for easy access
-export { DEBUG_ENABLED, debugLog } from "./debug.mjs";
+// Future: You can add exports for other servo types here, e.g.:
+// export { stsServoSDK } from './stsServoSDK.mjs';
+// export { smsServoSDK } from './smsServoSDK.mjs';

packages/feetech.js/lowLevelSDK.mjs

@@ -1,9 +1,6 @@
-// Import debug logging function
-import { debugLog } from "./debug.mjs";
-
 // Constants
-export const BROADCAST_ID = 0xfe; // 254
-export const MAX_ID = 0xfc; // 252
+export const BROADCAST_ID = 0xFE;  // 254
+export const MAX_ID = 0xFC;  // 252
 
 // Protocol instructions
 export const INST_PING = 1;
@@ -11,19 +8,19 @@ export const INST_READ = 2;
 export const INST_WRITE = 3;
 export const INST_REG_WRITE = 4;
 export const INST_ACTION = 5;
-export const INST_SYNC_WRITE = 131; // 0x83
-export const INST_SYNC_READ = 130; // 0x82
-export const INST_STATUS = 85; // 0x55, status packet instruction (0x55)
+export const INST_SYNC_WRITE = 131;  // 0x83
+export const INST_SYNC_READ = 130;  // 0x82
+export const INST_STATUS = 85;  // 0x55, status packet instruction (0x55)
 
 // Communication results
-export const COMM_SUCCESS = 0; // tx or rx packet communication success
-export const COMM_PORT_BUSY = -1; // Port is busy (in use)
-export const COMM_TX_FAIL = -2; // Failed transmit instruction packet
-export const COMM_RX_FAIL = -3; // Failed get status packet
-export const COMM_TX_ERROR = -4; // Incorrect instruction packet
-export const COMM_RX_WAITING = -5; // Now receiving status packet
-export const COMM_RX_TIMEOUT = -6; // There is no status packet
-export const COMM_RX_CORRUPT = -7; // Incorrect status packet
+export const COMM_SUCCESS = 0;      // tx or rx packet communication success
+export const COMM_PORT_BUSY = -1;   // Port is busy (in use)
+export const COMM_TX_FAIL = -2;     // Failed transmit instruction packet
+export const COMM_RX_FAIL = -3;     // Failed get status packet
+export const COMM_TX_ERROR = -4;    // Incorrect instruction packet
+export const COMM_RX_WAITING = -5;  // Now receiving status packet
+export const COMM_RX_TIMEOUT = -6;  // There is no status packet
+export const COMM_RX_CORRUPT = -7;  // Incorrect status packet
 export const COMM_NOT_AVAILABLE = -9;
 
 // Packet constants
@@ -55,872 +52,848 @@ let SCS_END = 0; // (STS/SMS=0, SCS=1)
 
 // Utility functions for handling word operations
 export function SCS_LOWORD(l) {
-	return l & 0xffff;
+  return l & 0xFFFF;
 }
 
 export function SCS_HIWORD(l) {
-	return (l >> 16) & 0xffff;
+  return (l >> 16) & 0xFFFF;
 }
 
 export function SCS_LOBYTE(w) {
-	if (SCS_END === 0) {
-		return w & 0xff;
-	} else {
-		return (w >> 8) & 0xff;
-	}
+  if (SCS_END === 0) {
+    return w & 0xFF;
+  } else {
+    return (w >> 8) & 0xFF;
+  }
 }
 
 export function SCS_HIBYTE(w) {
-	if (SCS_END === 0) {
-		return (w >> 8) & 0xff;
-	} else {
-		return w & 0xff;
-	}
+  if (SCS_END === 0) {
+    return (w >> 8) & 0xFF;
+  } else {
+    return w & 0xFF;
+  }
 }
 
 export function SCS_MAKEWORD(a, b) {
-	if (SCS_END === 0) {
-		return (a & 0xff) | ((b & 0xff) << 8);
-	} else {
-		return (b & 0xff) | ((a & 0xff) << 8);
-	}
+  if (SCS_END === 0) {
+    return (a & 0xFF) | ((b & 0xFF) << 8);
+  } else {
+    return (b & 0xFF) | ((a & 0xFF) << 8);
+  }
 }
 
 export function SCS_MAKEDWORD(a, b) {
-	return (a & 0xffff) | ((b & 0xffff) << 16);
+  return (a & 0xFFFF) | ((b & 0xFFFF) << 16);
 }
 
 export function SCS_TOHOST(a, b) {
-	if (a & (1 << b)) {
-		return -(a & ~(1 << b));
-	} else {
-		return a;
-	}
+  if (a & (1 << b)) {
+    return -(a & ~(1 << b));
+  } else {
+    return a;
+  }
 }
 
 export class PortHandler {
-	constructor() {
-		this.port = null;
-		this.reader = null;
-		this.writer = null;
-		this.isOpen = false;
-		this.isUsing = false;
-		this.baudrate = DEFAULT_BAUDRATE;
-		this.packetStartTime = 0;
-		this.packetTimeout = 0;
-		this.txTimePerByte = 0;
-	}
-
-	async requestPort() {
-		try {
-			this.port = await navigator.serial.requestPort();
-			return true;
-		} catch (err) {
-			console.error("Error requesting serial port:", err);
-			return false;
-		}
-	}
-
-	async openPort() {
-		if (!this.port) {
-			return false;
-		}
-
-		try {
-			await this.port.open({ baudRate: this.baudrate });
-			this.reader = this.port.readable.getReader();
-			this.writer = this.port.writable.getWriter();
-			this.isOpen = true;
-			this.txTimePerByte = (1000.0 / this.baudrate) * 10.0;
-			return true;
-		} catch (err) {
-			console.error("Error opening port:", err);
-			return false;
-		}
-	}
-
-	async closePort() {
-		if (this.reader) {
-			await this.reader.releaseLock();
-			this.reader = null;
-		}
-
-		if (this.writer) {
-			await this.writer.releaseLock();
-			this.writer = null;
-		}
-
-		if (this.port && this.isOpen) {
-			await this.port.close();
-			this.isOpen = false;
-		}
-	}
-
-	async clearPort() {
-		if (this.reader) {
-			await this.reader.releaseLock();
-			this.reader = this.port.readable.getReader();
-		}
-	}
-
-	setBaudRate(baudrate) {
-		this.baudrate = baudrate;
-		this.txTimePerByte = (1000.0 / this.baudrate) * 10.0;
-		return true;
-	}
-
-	getBaudRate() {
-		return this.baudrate;
-	}
-
-	async writePort(data) {
-		if (!this.isOpen || !this.writer) {
-			return 0;
-		}
-
-		try {
-			await this.writer.write(new Uint8Array(data));
-			return data.length;
-		} catch (err) {
-			console.error("Error writing to port:", err);
-			return 0;
-		}
-	}
-
-	async readPort(length) {
-		if (!this.isOpen || !this.reader) {
-			return [];
-		}
-
-		try {
-			// Increase timeout for more reliable data reception
-			const timeoutMs = 500;
-			let totalBytes = [];
-			const startTime = performance.now();
-
-			// Continue reading until we get enough bytes or timeout
-			while (totalBytes.length < length) {
-				// Create a timeout promise
-				const timeoutPromise = new Promise((resolve) => {
-					setTimeout(() => resolve({ value: new Uint8Array(), done: false, timeout: true }), 100); // Short internal timeout
-				});
-
-				// Race between reading and timeout
-				const result = await Promise.race([this.reader.read(), timeoutPromise]);
-
-				if (result.timeout) {
-					// Internal timeout - check if we've exceeded total timeout
-					if (performance.now() - startTime > timeoutMs) {
-						debugLog(`readPort total timeout after ${timeoutMs}ms`);
-						break;
-					}
-					continue; // Try reading again
-				}
-
-				if (result.done) {
-					debugLog("Reader done, stream closed");
-					break;
-				}
-
-				if (result.value.length === 0) {
-					// If there's no data but we haven't timed out yet, wait briefly and try again
-					await new Promise((resolve) => setTimeout(resolve, 10));
-
-					// Check if we've exceeded total timeout
-					if (performance.now() - startTime > timeoutMs) {
-						debugLog(`readPort total timeout after ${timeoutMs}ms`);
-						break;
-					}
-					continue;
-				}
-
-				// Add received bytes to our total
-				const newData = Array.from(result.value);
-				totalBytes.push(...newData);
-				debugLog(
-					`Read ${newData.length} bytes:`,
-					newData.map((b) => b.toString(16).padStart(2, "0")).join(" ")
-				);
-
-				// If we've got enough data, we can stop
-				if (totalBytes.length >= length) {
-					break;
-				}
-			}
-
-			return totalBytes;
-		} catch (err) {
-			console.error("Error reading from port:", err);
-			return [];
-		}
-	}
-
-	setPacketTimeout(packetLength) {
-		this.packetStartTime = this.getCurrentTime();
-		this.packetTimeout = this.txTimePerByte * packetLength + LATENCY_TIMER * 2.0 + 2.0;
-	}
-
-	setPacketTimeoutMillis(msec) {
-		this.packetStartTime = this.getCurrentTime();
-		this.packetTimeout = msec;
-	}
-
-	isPacketTimeout() {
-		if (this.getTimeSinceStart() > this.packetTimeout) {
-			this.packetTimeout = 0;
-			return true;
-		}
-		return false;
-	}
-
-	getCurrentTime() {
-		return performance.now();
-	}
-
-	getTimeSinceStart() {
-		const timeSince = this.getCurrentTime() - this.packetStartTime;
-		if (timeSince < 0.0) {
-			this.packetStartTime = this.getCurrentTime();
-		}
-		return timeSince;
-	}
+  constructor() {
+    this.port = null;
+    this.reader = null;
+    this.writer = null;
+    this.isOpen = false;
+    this.isUsing = false;
+    this.baudrate = DEFAULT_BAUDRATE;
+    this.packetStartTime = 0;
+    this.packetTimeout = 0;
+    this.txTimePerByte = 0;
+  }
+  
+  async requestPort() {
+    try {
+      this.port = await navigator.serial.requestPort();
+      return true;
+    } catch (err) {
+      console.error('Error requesting serial port:', err);
+      return false;
+    }
+  }
+  
+  async openPort() {
+    if (!this.port) {
+      return false;
+    }
+    
+    try {
+      await this.port.open({ baudRate: this.baudrate });
+      this.reader = this.port.readable.getReader();
+      this.writer = this.port.writable.getWriter();
+      this.isOpen = true;
+      this.txTimePerByte = (1000.0 / this.baudrate) * 10.0;
+      return true;
+    } catch (err) {
+      console.error('Error opening port:', err);
+      return false;
+    }
+  }
+  
+  async closePort() {
+    if (this.reader) {
+      await this.reader.releaseLock();
+      this.reader = null;
+    }
+    
+    if (this.writer) {
+      await this.writer.releaseLock();
+      this.writer = null;
+    }
+    
+    if (this.port && this.isOpen) {
+      await this.port.close();
+      this.isOpen = false;
+    }
+  }
+  
+  async clearPort() {
+    if (this.reader) {
+      await this.reader.releaseLock();
+      this.reader = this.port.readable.getReader();
+    }
+  }
+  
+  setBaudRate(baudrate) {
+    this.baudrate = baudrate;
+    this.txTimePerByte = (1000.0 / this.baudrate) * 10.0;
+    return true;
+  }
+  
+  getBaudRate() {
+    return this.baudrate;
+  }
+  
+  async writePort(data) {
+    if (!this.isOpen || !this.writer) {
+      return 0;
+    }
+    
+    try {
+      await this.writer.write(new Uint8Array(data));
+      return data.length;
+    } catch (err) {
+      console.error('Error writing to port:', err);
+      return 0;
+    }
+  }
+  
+  async readPort(length) {
+    if (!this.isOpen || !this.reader) {
+      return [];
+    }
+    
+    try {
+      // Increase timeout for more reliable data reception
+      const timeoutMs = 500; 
+      let totalBytes = [];
+      const startTime = performance.now();
+      
+      // Continue reading until we get enough bytes or timeout
+      while (totalBytes.length < length) {
+        // Create a timeout promise
+        const timeoutPromise = new Promise(resolve => {
+          setTimeout(() => resolve({ value: new Uint8Array(), done: false, timeout: true }), 100); // Short internal timeout
+        });
+        
+        // Race between reading and timeout
+        const result = await Promise.race([
+          this.reader.read(),
+          timeoutPromise
+        ]);
+        
+        if (result.timeout) {
+          // Internal timeout - check if we've exceeded total timeout
+          if (performance.now() - startTime > timeoutMs) {
+            console.log(`readPort total timeout after ${timeoutMs}ms`);
+            break;
+          }
+          continue; // Try reading again
+        }
+        
+        if (result.done) {
+          console.log('Reader done, stream closed');
+          break;
+        }
+        
+        if (result.value.length === 0) {
+          // If there's no data but we haven't timed out yet, wait briefly and try again
+          await new Promise(resolve => setTimeout(resolve, 10));
+          
+          // Check if we've exceeded total timeout
+          if (performance.now() - startTime > timeoutMs) {
+            console.log(`readPort total timeout after ${timeoutMs}ms`);
+            break;
+          }
+          continue;
+        }
+        
+        // Add received bytes to our total
+        const newData = Array.from(result.value);
+        totalBytes.push(...newData);
+        // console.log(`Read ${newData.length} bytes:`, newData.map(b => b.toString(16).padStart(2, '0')).join(' '));
+        
+        // If we've got enough data, we can stop
+        if (totalBytes.length >= length) {
+          break;
+        }
+      }
+      
+      return totalBytes;
+    } catch (err) {
+      console.error('Error reading from port:', err);
+      return [];
+    }
+  }
+  
+  setPacketTimeout(packetLength) {
+    this.packetStartTime = this.getCurrentTime();
+    this.packetTimeout = (this.txTimePerByte * packetLength) + (LATENCY_TIMER * 2.0) + 2.0;
+  }
+  
+  setPacketTimeoutMillis(msec) {
+    this.packetStartTime = this.getCurrentTime();
+    this.packetTimeout = msec;
+  }
+  
+  isPacketTimeout() {
+    if (this.getTimeSinceStart() > this.packetTimeout) {
+      this.packetTimeout = 0;
+      return true;
+    }
+    return false;
+  }
+  
+  getCurrentTime() {
+    return performance.now();
+  }
+  
+  getTimeSinceStart() {
+    const timeSince = this.getCurrentTime() - this.packetStartTime;
+    if (timeSince < 0.0) {
+      this.packetStartTime = this.getCurrentTime();
+    }
+    return timeSince;
+  }
 }
 
 export class PacketHandler {
-	constructor(protocolEnd = 0) {
-		SCS_END = protocolEnd;
-		debugLog(`PacketHandler initialized with protocol_end=${protocolEnd} (STS/SMS=0, SCS=1)`);
-	}
-
-	getProtocolVersion() {
-		return 1.0;
-	}
-
-	// 获取当前协议端设置的方法
-	getProtocolEnd() {
-		return SCS_END;
-	}
-
-	getTxRxResult(result) {
-		if (result === COMM_SUCCESS) {
-			return "[TxRxResult] Communication success!";
-		} else if (result === COMM_PORT_BUSY) {
-			return "[TxRxResult] Port is in use!";
-		} else if (result === COMM_TX_FAIL) {
-			return "[TxRxResult] Failed transmit instruction packet!";
-		} else if (result === COMM_RX_FAIL) {
-			return "[TxRxResult] Failed get status packet from device!";
-		} else if (result === COMM_TX_ERROR) {
-			return "[TxRxResult] Incorrect instruction packet!";
-		} else if (result === COMM_RX_WAITING) {
-			return "[TxRxResult] Now receiving status packet!";
-		} else if (result === COMM_RX_TIMEOUT) {
-			return "[TxRxResult] There is no status packet!";
-		} else if (result === COMM_RX_CORRUPT) {
-			return "[TxRxResult] Incorrect status packet!";
-		} else if (result === COMM_NOT_AVAILABLE) {
-			return "[TxRxResult] Protocol does not support this function!";
-		} else {
-			return "";
-		}
-	}
-
-	getRxPacketError(error) {
-		if (error & ERRBIT_VOLTAGE) {
-			return "[RxPacketError] Input voltage error!";
-		}
-		if (error & ERRBIT_ANGLE) {
-			return "[RxPacketError] Angle sen error!";
-		}
-		if (error & ERRBIT_OVERHEAT) {
-			return "[RxPacketError] Overheat error!";
-		}
-		if (error & ERRBIT_OVERELE) {
-			return "[RxPacketError] OverEle error!";
-		}
-		if (error & ERRBIT_OVERLOAD) {
-			return "[RxPacketError] Overload error!";
-		}
-		return "";
-	}
-
-	async txPacket(port, txpacket) {
-		let checksum = 0;
-		const totalPacketLength = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH
-
-		if (port.isUsing) {
-			return COMM_PORT_BUSY;
-		}
-		port.isUsing = true;
-
-		// Check max packet length
-		if (totalPacketLength > TXPACKET_MAX_LEN) {
-			port.isUsing = false;
-			return COMM_TX_ERROR;
-		}
-
-		// Make packet header
-		txpacket[PKT_HEADER0] = 0xff;
-		txpacket[PKT_HEADER1] = 0xff;
-
-		// Add checksum to packet
-		for (let idx = 2; idx < totalPacketLength - 1; idx++) {
-			checksum += txpacket[idx];
-		}
-
-		txpacket[totalPacketLength - 1] = ~checksum & 0xff;
-
-		// TX packet
-		await port.clearPort();
-		const writtenPacketLength = await port.writePort(txpacket);
-		if (totalPacketLength !== writtenPacketLength) {
-			port.isUsing = false;
-			return COMM_TX_FAIL;
-		}
-
-		return COMM_SUCCESS;
-	}
-
-	async rxPacket(port) {
-		let rxpacket = [];
-		let result = COMM_RX_FAIL;
-
-		let waitLength = 6; // minimum length (HEADER0 HEADER1 ID LENGTH)
-
-		while (true) {
-			const data = await port.readPort(waitLength - rxpacket.length);
-			rxpacket.push(...data);
-
-			if (rxpacket.length >= waitLength) {
-				// Find packet header
-				let headerIndex = -1;
-				for (let i = 0; i < rxpacket.length - 1; i++) {
-					if (rxpacket[i] === 0xff && rxpacket[i + 1] === 0xff) {
-						headerIndex = i;
-						break;
-					}
-				}
-
-				if (headerIndex === 0) {
-					// Found at the beginning of the packet
-					if (rxpacket[PKT_ID] > 0xfd || rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN) {
-						// Invalid ID or length
-						rxpacket.shift();
-						continue;
-					}
-
-					// Recalculate expected packet length
-					if (waitLength !== rxpacket[PKT_LENGTH] + PKT_LENGTH + 1) {
-						waitLength = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1;
-						continue;
-					}
-
-					if (rxpacket.length < waitLength) {
-						// Check timeout
-						if (port.isPacketTimeout()) {
-							result = rxpacket.length === 0 ? COMM_RX_TIMEOUT : COMM_RX_CORRUPT;
-							break;
-						}
-						continue;
-					}
-
-					// Calculate checksum
-					let checksum = 0;
-					for (let i = 2; i < waitLength - 1; i++) {
-						checksum += rxpacket[i];
-					}
-					checksum = ~checksum & 0xff;
-
-					// Verify checksum
-					if (rxpacket[waitLength - 1] === checksum) {
-						result = COMM_SUCCESS;
-					} else {
-						result = COMM_RX_CORRUPT;
-					}
-					break;
-				} else if (headerIndex > 0) {
-					// Remove unnecessary bytes before header
-					rxpacket = rxpacket.slice(headerIndex);
-					continue;
-				}
-			}
-
-			// Check timeout
-			if (port.isPacketTimeout()) {
-				result = rxpacket.length === 0 ? COMM_RX_TIMEOUT : COMM_RX_CORRUPT;
-				break;
-			}
-		}
-
-		if (result !== COMM_SUCCESS) {
-			debugLog(
-				`rxPacket result: ${result}, packet: ${rxpacket.map((b) => "0x" + b.toString(16).padStart(2, "0")).join(" ")}`
-			);
-		} else {
-			console.debug(
-				`rxPacket successful: ${rxpacket.map((b) => "0x" + b.toString(16).padStart(2, "0")).join(" ")}`
-			);
-		}
-		return [rxpacket, result];
-	}
-
-	async txRxPacket(port, txpacket) {
-		let rxpacket = null;
-		let error = 0;
-		let result = COMM_TX_FAIL;
-
-		try {
-			// Check if port is already in use
-			if (port.isUsing) {
-				debugLog("Port is busy, cannot start new transaction");
-				return [rxpacket, COMM_PORT_BUSY, error];
-			}
-
-			// TX packet
-			debugLog(
-				"Sending packet:",
-				txpacket.map((b) => "0x" + b.toString(16).padStart(2, "0")).join(" ")
-			);
-
-			// Remove retry logic and just send once
-			result = await this.txPacket(port, txpacket);
-			debugLog(`TX result: ${result}`);
-
-			if (result !== COMM_SUCCESS) {
-				debugLog(`TX failed with result: ${result}`);
-				port.isUsing = false; // Important: Release the port on TX failure
-				return [rxpacket, result, error];
-			}
-
-			// If ID is broadcast, no need to wait for status packet
-			if (txpacket[PKT_ID] === BROADCAST_ID) {
-				port.isUsing = false;
-				return [rxpacket, result, error];
-			}
-
-			// Set packet timeout
-			if (txpacket[PKT_INSTRUCTION] === INST_READ) {
-				const length = txpacket[PKT_PARAMETER0 + 1];
-				// For READ instructions, we expect response to include the data
-				port.setPacketTimeout(length + 10); // Add extra buffer
-				debugLog(`Set READ packet timeout for ${length + 10} bytes`);
-			} else {
-				// For other instructions, we expect a status packet
-				port.setPacketTimeout(10); // HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM + buffer
-				debugLog(`Set standard packet timeout for 10 bytes`);
-			}
-
-			// RX packet - no retries, just attempt once
-			debugLog(`Receiving packet`);
-
-			// Clear port before receiving to ensure clean state
-			await port.clearPort();
-
-			const [rxpacketResult, resultRx] = await this.rxPacket(port);
-			rxpacket = rxpacketResult;
-
-			// Check if received packet is valid
-			if (resultRx !== COMM_SUCCESS) {
-				debugLog(`Rx failed with result: ${resultRx}`);
-				port.isUsing = false;
-				return [rxpacket, resultRx, error];
-			}
-
-			// Verify packet structure
-			if (rxpacket.length < 6) {
-				debugLog(`Received packet too short (${rxpacket.length} bytes)`);
-				port.isUsing = false;
-				return [rxpacket, COMM_RX_CORRUPT, error];
-			}
-
-			// Verify packet ID matches the sent ID
-			if (rxpacket[PKT_ID] !== txpacket[PKT_ID]) {
-				debugLog(
-					`Received packet ID (${rxpacket[PKT_ID]}) doesn't match sent ID (${txpacket[PKT_ID]})`
-				);
-				port.isUsing = false;
-				return [rxpacket, COMM_RX_CORRUPT, error];
-			}
-
-			// Packet looks valid
-			error = rxpacket[PKT_ERROR];
-			port.isUsing = false; // Release port on success
-			return [rxpacket, resultRx, error];
-		} catch (err) {
-			console.error("Exception in txRxPacket:", err);
-			port.isUsing = false; // Release port on exception
-			return [rxpacket, COMM_RX_FAIL, error];
-		}
-	}
-
-	async ping(port, scsId) {
-		let modelNumber = 0;
-		let error = 0;
-
-		try {
-			if (scsId >= BROADCAST_ID) {
-				debugLog(`Cannot ping broadcast ID ${scsId}`);
-				return [modelNumber, COMM_NOT_AVAILABLE, error];
-			}
-
-			const txpacket = new Array(6).fill(0);
-			txpacket[PKT_ID] = scsId;
-			txpacket[PKT_LENGTH] = 2;
-			txpacket[PKT_INSTRUCTION] = INST_PING;
-
-			debugLog(`Pinging servo ID ${scsId}...`);
-
-			// 发送ping指令并获取响应
-			const [rxpacket, result, err] = await this.txRxPacket(port, txpacket);
-			error = err;
-
-			// 与Python SDK保持一致：如果ping成功，尝试读取地址3的型号信息
-			if (result === COMM_SUCCESS) {
-				debugLog(`Ping to servo ID ${scsId} succeeded, reading model number from address 3`);
-				// 读取地址3的型号信息（2字节）
-				const [data, dataResult, dataError] = await this.readTxRx(port, scsId, 3, 2);
-
-				if (dataResult === COMM_SUCCESS && data && data.length >= 2) {
-					modelNumber = SCS_MAKEWORD(data[0], data[1]);
-					debugLog(`Model number read: ${modelNumber}`);
-				} else {
-					debugLog(`Could not read model number: ${this.getTxRxResult(dataResult)}`);
-				}
-			} else {
-				debugLog(`Ping failed with result: ${result}, error: ${error}`);
-			}
-
-			return [modelNumber, result, error];
-		} catch (error) {
-			console.error(`Exception in ping():`, error);
-			return [0, COMM_RX_FAIL, 0];
-		}
-	}
-
-	// Read methods
-	async readTxRx(port, scsId, address, length) {
-		if (scsId >= BROADCAST_ID) {
-			debugLog("Cannot read from broadcast ID");
-			return [[], COMM_NOT_AVAILABLE, 0];
-		}
-
-		// Create read packet
-		const txpacket = new Array(8).fill(0);
-		txpacket[PKT_ID] = scsId;
-		txpacket[PKT_LENGTH] = 4;
-		txpacket[PKT_INSTRUCTION] = INST_READ;
-		txpacket[PKT_PARAMETER0] = address;
-		txpacket[PKT_PARAMETER0 + 1] = length;
-
-		debugLog(`Reading ${length} bytes from address ${address} for servo ID ${scsId}`);
-
-		// Send packet and get response
-		const [rxpacket, result, error] = await this.txRxPacket(port, txpacket);
-
-		// Process the result
-		if (result !== COMM_SUCCESS) {
-			debugLog(`Read failed with result: ${result}, error: ${error}`);
-			return [[], result, error];
-		}
-
-		if (!rxpacket || rxpacket.length < PKT_PARAMETER0 + length) {
-			debugLog(
-				`Invalid response packet: expected at least ${PKT_PARAMETER0 + length} bytes, got ${rxpacket ? rxpacket.length : 0}`
-			);
-			return [[], COMM_RX_CORRUPT, error];
-		}
-
-		// Extract data from response
-		const data = [];
-		debugLog(
-			`Response packet length: ${rxpacket.length}, extracting ${length} bytes from offset ${PKT_PARAMETER0}`
-		);
-		debugLog(
-			`Response data bytes: ${rxpacket
-				.slice(PKT_PARAMETER0, PKT_PARAMETER0 + length)
-				.map((b) => "0x" + b.toString(16).padStart(2, "0"))
-				.join(" ")}`
-		);
-
-		for (let i = 0; i < length; i++) {
-			data.push(rxpacket[PKT_PARAMETER0 + i]);
-		}
-
-		debugLog(
-			`Successfully read ${length} bytes: ${data.map((b) => "0x" + b.toString(16).padStart(2, "0")).join(" ")}`
-		);
-		return [data, result, error];
-	}
-
-	async read1ByteTxRx(port, scsId, address) {
-		const [data, result, error] = await this.readTxRx(port, scsId, address, 1);
-		const value = data.length > 0 ? data[0] : 0;
-		return [value, result, error];
-	}
-
-	async read2ByteTxRx(port, scsId, address) {
-		const [data, result, error] = await this.readTxRx(port, scsId, address, 2);
-
-		let value = 0;
-		if (data.length >= 2) {
-			value = SCS_MAKEWORD(data[0], data[1]);
-		}
-
-		return [value, result, error];
-	}
-
-	async read4ByteTxRx(port, scsId, address) {
-		const [data, result, error] = await this.readTxRx(port, scsId, address, 4);
-
-		let value = 0;
-		if (data.length >= 4) {
-			const loword = SCS_MAKEWORD(data[0], data[1]);
-			const hiword = SCS_MAKEWORD(data[2], data[3]);
-			value = SCS_MAKEDWORD(loword, hiword);
-
-			debugLog(
-				`read4ByteTxRx: data=${data.map((b) => "0x" + b.toString(16).padStart(2, "0")).join(" ")}`
-			);
-			debugLog(
-				`  loword=${loword} (0x${loword.toString(16)}), hiword=${hiword} (0x${hiword.toString(16)})`
-			);
-			debugLog(`  value=${value} (0x${value.toString(16)})`);
-		}
-
-		return [value, result, error];
-	}
-
-	// Write methods
-	async writeTxRx(port, scsId, address, length, data) {
-		if (scsId >= BROADCAST_ID) {
-			return [COMM_NOT_AVAILABLE, 0];
-		}
-
-		// Create write packet
-		const txpacket = new Array(length + 7).fill(0);
-		txpacket[PKT_ID] = scsId;
-		txpacket[PKT_LENGTH] = length + 3;
-		txpacket[PKT_INSTRUCTION] = INST_WRITE;
-		txpacket[PKT_PARAMETER0] = address;
-
-		// Add data
-		for (let i = 0; i < length; i++) {
-			txpacket[PKT_PARAMETER0 + 1 + i] = data[i] & 0xff;
-		}
-
-		// Send packet and get response
-		const [rxpacket, result, error] = await this.txRxPacket(port, txpacket);
-
-		return [result, error];
-	}
-
-	async write1ByteTxRx(port, scsId, address, data) {
-		const dataArray = [data & 0xff];
-		return await this.writeTxRx(port, scsId, address, 1, dataArray);
-	}
-
-	async write2ByteTxRx(port, scsId, address, data) {
-		const dataArray = [SCS_LOBYTE(data), SCS_HIBYTE(data)];
-		return await this.writeTxRx(port, scsId, address, 2, dataArray);
-	}
-
-	async write4ByteTxRx(port, scsId, address, data) {
-		const dataArray = [
-			SCS_LOBYTE(SCS_LOWORD(data)),
-			SCS_HIBYTE(SCS_LOWORD(data)),
-			SCS_LOBYTE(SCS_HIWORD(data)),
-			SCS_HIBYTE(SCS_HIWORD(data))
-		];
-		return await this.writeTxRx(port, scsId, address, 4, dataArray);
-	}
-
-	// Add syncReadTx for GroupSyncRead functionality
-	async syncReadTx(port, startAddress, dataLength, param, paramLength) {
-		// Create packet: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN PARAM... CHKSUM
-		const txpacket = new Array(paramLength + 8).fill(0);
-
-		txpacket[PKT_ID] = BROADCAST_ID;
-		txpacket[PKT_LENGTH] = paramLength + 4; // 4: INST START_ADDR DATA_LEN CHKSUM
-		txpacket[PKT_INSTRUCTION] = INST_SYNC_READ;
-		txpacket[PKT_PARAMETER0] = startAddress;
-		txpacket[PKT_PARAMETER0 + 1] = dataLength;
-
-		// Add parameters
-		for (let i = 0; i < paramLength; i++) {
-			txpacket[PKT_PARAMETER0 + 2 + i] = param[i];
-		}
-
-		// Calculate checksum
-		const totalLen = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH
-
-		// Add headers
-		txpacket[PKT_HEADER0] = 0xff;
-		txpacket[PKT_HEADER1] = 0xff;
-
-		// Calculate checksum
-		let checksum = 0;
-		for (let i = 2; i < totalLen - 1; i++) {
-			checksum += txpacket[i] & 0xff;
-		}
-		txpacket[totalLen - 1] = ~checksum & 0xff;
-
-		debugLog(
-			`SyncReadTx: ${txpacket.map((b) => "0x" + b.toString(16).padStart(2, "0")).join(" ")}`
-		);
-
-		// Send packet
-		await port.clearPort();
-		const bytesWritten = await port.writePort(txpacket);
-		if (bytesWritten !== totalLen) {
-			return COMM_TX_FAIL;
-		}
-
-		// Set timeout based on expected response size
-		port.setPacketTimeout((6 + dataLength) * paramLength);
-
-		return COMM_SUCCESS;
-	}
-
-	// Add syncWriteTxOnly for GroupSyncWrite functionality
-	async syncWriteTxOnly(port, startAddress, dataLength, param, paramLength) {
-		// Create packet: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN PARAM... CHKSUM
-		const txpacket = new Array(paramLength + 8).fill(0);
-
-		txpacket[PKT_ID] = BROADCAST_ID;
-		txpacket[PKT_LENGTH] = paramLength + 4; // 4: INST START_ADDR DATA_LEN CHKSUM
-		txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE;
-		txpacket[PKT_PARAMETER0] = startAddress;
-		txpacket[PKT_PARAMETER0 + 1] = dataLength;
-
-		// Add parameters
-		for (let i = 0; i < paramLength; i++) {
-			txpacket[PKT_PARAMETER0 + 2 + i] = param[i];
-		}
-
-		// Calculate checksum
-		const totalLen = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH
-
-		// Add headers
-		txpacket[PKT_HEADER0] = 0xff;
-		txpacket[PKT_HEADER1] = 0xff;
-
-		// Calculate checksum
-		let checksum = 0;
-		for (let i = 2; i < totalLen - 1; i++) {
-			checksum += txpacket[i] & 0xff;
-		}
-		txpacket[totalLen - 1] = ~checksum & 0xff;
-
-		debugLog(
-			`SyncWriteTxOnly: ${txpacket.map((b) => "0x" + b.toString(16).padStart(2, "0")).join(" ")}`
-		);
-
-		// Send packet - for sync write, we don't need a response
-		await port.clearPort();
-		const bytesWritten = await port.writePort(txpacket);
-		if (bytesWritten !== totalLen) {
-			return COMM_TX_FAIL;
-		}
-
-		return COMM_SUCCESS;
-	}
-
-	// 辅助方法：格式化数据包结构以方便调试
-	formatPacketStructure(packet) {
-		if (!packet || packet.length < 4) {
-			return "Invalid packet (too short)";
-		}
-
-		try {
-			let result = "";
-			result += `HEADER: ${packet[0].toString(16).padStart(2, "0")} ${packet[1].toString(16).padStart(2, "0")} | `;
-			result += `ID: ${packet[2]} | `;
-			result += `LENGTH: ${packet[3]} | `;
-
-			if (packet.length >= 5) {
-				result += `ERROR/INST: ${packet[4].toString(16).padStart(2, "0")} | `;
-			}
-
-			if (packet.length >= 6) {
-				result += "PARAMS: ";
-				for (let i = 5; i < packet.length - 1; i++) {
-					result += `${packet[i].toString(16).padStart(2, "0")} `;
-				}
-				result += `| CHECKSUM: ${packet[packet.length - 1].toString(16).padStart(2, "0")}`;
-			}
-
-			return result;
-		} catch (e) {
-			return "Error formatting packet: " + e.message;
-		}
-	}
-
-	/**
-	 * 从响应包中解析舵机型号
-	 * @param {Array} rxpacket - 响应数据包
-	 * @returns {number} 舵机型号
-	 */
-	parseModelNumber(rxpacket) {
-		if (!rxpacket || rxpacket.length < 7) {
-			return 0;
-		}
-
-		// 检查是否有参数字段
-		if (rxpacket.length <= PKT_PARAMETER0 + 1) {
-			return 0;
-		}
-
-		const param1 = rxpacket[PKT_PARAMETER0];
-		const param2 = rxpacket[PKT_PARAMETER0 + 1];
-
-		if (SCS_END === 0) {
-			// STS/SMS 协议的字节顺序
-			return SCS_MAKEWORD(param1, param2);
-		} else {
-			// SCS 协议的字节顺序
-			return SCS_MAKEWORD(param2, param1);
-		}
-	}
-
-	/**
-	 * Verify packet header
-	 * @param {Array} packet - The packet to verify
-	 * @returns {Number} COMM_SUCCESS if packet is valid, error code otherwise
-	 */
-	getPacketHeader(packet) {
-		if (!packet || packet.length < 4) {
-			return COMM_RX_CORRUPT;
-		}
-
-		// Check header
-		if (packet[PKT_HEADER0] !== 0xff || packet[PKT_HEADER1] !== 0xff) {
-			return COMM_RX_CORRUPT;
-		}
-
-		// Check ID validity
-		if (packet[PKT_ID] > 0xfd) {
-			return COMM_RX_CORRUPT;
-		}
-
-		// Check length
-		if (packet.length != packet[PKT_LENGTH] + 4) {
-			return COMM_RX_CORRUPT;
-		}
-
-		// Calculate checksum
-		let checksum = 0;
-		for (let i = 2; i < packet.length - 1; i++) {
-			checksum += packet[i] & 0xff;
-		}
-		checksum = ~checksum & 0xff;
-
-		// Verify checksum
-		if (packet[packet.length - 1] !== checksum) {
-			return COMM_RX_CORRUPT;
-		}
-
-		return COMM_SUCCESS;
-	}
+  constructor(protocolEnd = 0) {
+    SCS_END = protocolEnd;
+    console.log(`PacketHandler initialized with protocol_end=${protocolEnd} (STS/SMS=0, SCS=1)`);
+  }
+  
+  getProtocolVersion() {
+    return 1.0;
+  }
+  
+  // 获取当前协议端设置的方法
+  getProtocolEnd() {
+    return SCS_END;
+  }
+  
+  getTxRxResult(result) {
+    if (result === COMM_SUCCESS) {
+      return "[TxRxResult] Communication success!";
+    } else if (result === COMM_PORT_BUSY) {
+      return "[TxRxResult] Port is in use!";
+    } else if (result === COMM_TX_FAIL) {
+      return "[TxRxResult] Failed transmit instruction packet!";
+    } else if (result === COMM_RX_FAIL) {
+      return "[TxRxResult] Failed get status packet from device!";
+    } else if (result === COMM_TX_ERROR) {
+      return "[TxRxResult] Incorrect instruction packet!";
+    } else if (result === COMM_RX_WAITING) {
+      return "[TxRxResult] Now receiving status packet!";
+    } else if (result === COMM_RX_TIMEOUT) {
+      return "[TxRxResult] There is no status packet!";
+    } else if (result === COMM_RX_CORRUPT) {
+      return "[TxRxResult] Incorrect status packet!";
+    } else if (result === COMM_NOT_AVAILABLE) {
+      return "[TxRxResult] Protocol does not support this function!";
+    } else {
+      return "";
+    }
+  }
+  
+  getRxPacketError(error) {
+    if (error & ERRBIT_VOLTAGE) {
+      return "[RxPacketError] Input voltage error!";
+    }
+    if (error & ERRBIT_ANGLE) {
+      return "[RxPacketError] Angle sen error!";
+    }
+    if (error & ERRBIT_OVERHEAT) {
+      return "[RxPacketError] Overheat error!";
+    }
+    if (error & ERRBIT_OVERELE) {
+      return "[RxPacketError] OverEle error!";
+    }
+    if (error & ERRBIT_OVERLOAD) {
+      return "[RxPacketError] Overload error!";
+    }
+    return "";
+  }
+  
+  async txPacket(port, txpacket) {
+    let checksum = 0;
+    const totalPacketLength = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH
+    
+    if (port.isUsing) {
+      return COMM_PORT_BUSY;
+    }
+    port.isUsing = true;
+    
+    // Check max packet length
+    if (totalPacketLength > TXPACKET_MAX_LEN) {
+      port.isUsing = false;
+      return COMM_TX_ERROR;
+    }
+    
+    // Make packet header
+    txpacket[PKT_HEADER0] = 0xFF;
+    txpacket[PKT_HEADER1] = 0xFF;
+    
+    // Add checksum to packet
+    for (let idx = 2; idx < totalPacketLength - 1; idx++) {
+      checksum += txpacket[idx];
+    }
+    
+    txpacket[totalPacketLength - 1] = (~checksum) & 0xFF;
+    
+    // TX packet
+    await port.clearPort();
+    const writtenPacketLength = await port.writePort(txpacket);
+    if (totalPacketLength !== writtenPacketLength) {
+      port.isUsing = false;
+      return COMM_TX_FAIL;
+    }
+    
+    return COMM_SUCCESS;
+  }
+  
+  async rxPacket(port) {
+    let rxpacket = [];
+    let result = COMM_RX_FAIL;
+    
+    let waitLength = 6; // minimum length (HEADER0 HEADER1 ID LENGTH)
+    
+    while (true) {
+      const data = await port.readPort(waitLength - rxpacket.length);
+      rxpacket.push(...data);
+      
+      if (rxpacket.length >= waitLength) {
+        // Find packet header
+        let headerIndex = -1;
+        for (let i = 0; i < rxpacket.length - 1; i++) {
+          if (rxpacket[i] === 0xFF && rxpacket[i + 1] === 0xFF) {
+            headerIndex = i;
+            break;
+          }
+        }
+        
+        if (headerIndex === 0) {
+          // Found at the beginning of the packet
+          if (rxpacket[PKT_ID] > 0xFD || rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN) {
+            // Invalid ID or length
+            rxpacket.shift();
+            continue;
+          }
+          
+          // Recalculate expected packet length
+          if (waitLength !== (rxpacket[PKT_LENGTH] + PKT_LENGTH + 1)) {
+            waitLength = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1;
+            continue;
+          }
+          
+          if (rxpacket.length < waitLength) {
+            // Check timeout
+            if (port.isPacketTimeout()) {
+              result = rxpacket.length === 0 ? COMM_RX_TIMEOUT : COMM_RX_CORRUPT;
+              break;
+            }
+            continue;
+          }
+          
+          // Calculate checksum
+          let checksum = 0;
+          for (let i = 2; i < waitLength - 1; i++) {
+            checksum += rxpacket[i];
+          }
+          checksum = (~checksum) & 0xFF;
+          
+          // Verify checksum
+          if (rxpacket[waitLength - 1] === checksum) {
+            result = COMM_SUCCESS;
+          } else {
+            result = COMM_RX_CORRUPT;
+          }
+          break;
+        } else if (headerIndex > 0) {
+          // Remove unnecessary bytes before header
+          rxpacket = rxpacket.slice(headerIndex);
+          continue;
+        }
+      }
+      
+      // Check timeout
+      if (port.isPacketTimeout()) {
+        result = rxpacket.length === 0 ? COMM_RX_TIMEOUT : COMM_RX_CORRUPT;
+        break;
+      }
+    }
+    
+    if (result !== COMM_SUCCESS) {
+      console.log(`rxPacket result: ${result}, packet: ${rxpacket.map(b => '0x' + b.toString(16).padStart(2,'0')).join(' ')}`);
+    } else {
+      // console.debug(`rxPacket successful: ${rxpacket.map(b => '0x' + b.toString(16).padStart(2,'0')).join(' ')}`);
+    }
+    return [rxpacket, result];
+  }
+  
+  async txRxPacket(port, txpacket) {
+    let rxpacket = null;
+    let error = 0;
+    let result = COMM_TX_FAIL;
+    
+    try {
+      // Check if port is already in use
+      if (port.isUsing) {
+        console.log("Port is busy, cannot start new transaction");
+        return [rxpacket, COMM_PORT_BUSY, error];
+      }
+      
+      // TX packet
+      // console.log("Sending packet:", txpacket.map(b => '0x' + b.toString(16).padStart(2,'0')).join(' '));
+      
+      // Remove retry logic and just send once
+      result = await this.txPacket(port, txpacket);
+      // console.log(`TX result: ${result}`);
+      
+      if (result !== COMM_SUCCESS) {
+        console.log(`TX failed with result: ${result}`);
+        port.isUsing = false; // Important: Release the port on TX failure
+        return [rxpacket, result, error];
+      }
+      
+      // If ID is broadcast, no need to wait for status packet
+      if (txpacket[PKT_ID] === BROADCAST_ID) {
+        port.isUsing = false;
+        return [rxpacket, result, error];
+      }
+      
+      // Set packet timeout
+      if (txpacket[PKT_INSTRUCTION] === INST_READ) {
+        const length = txpacket[PKT_PARAMETER0 + 1];
+        // For READ instructions, we expect response to include the data
+        port.setPacketTimeout(length + 10); // Add extra buffer
+        // console.log(`Set READ packet timeout for ${length + 10} bytes`);
+      } else {
+        // For other instructions, we expect a status packet
+        port.setPacketTimeout(10); // HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM + buffer
+        console.log(`Set standard packet timeout for 10 bytes`);
+      }
+      
+      // RX packet - no retries, just attempt once
+      // console.log(`Receiving packet`);
+      
+      // Clear port before receiving to ensure clean state
+      await port.clearPort();
+      
+      const [rxpacketResult, resultRx] = await this.rxPacket(port);
+      rxpacket = rxpacketResult;
+      
+      // Check if received packet is valid
+      if (resultRx !== COMM_SUCCESS) {
+        console.log(`Rx failed with result: ${resultRx}`);
+        port.isUsing = false;
+        return [rxpacket, resultRx, error];
+      }
+      
+      // Verify packet structure
+      if (rxpacket.length < 6) {
+        console.log(`Received packet too short (${rxpacket.length} bytes)`);
+        port.isUsing = false;
+        return [rxpacket, COMM_RX_CORRUPT, error];
+      }
+      
+      // Verify packet ID matches the sent ID
+      if (rxpacket[PKT_ID] !== txpacket[PKT_ID]) {
+        console.log(`Received packet ID (${rxpacket[PKT_ID]}) doesn't match sent ID (${txpacket[PKT_ID]})`);
+        port.isUsing = false;
+        return [rxpacket, COMM_RX_CORRUPT, error];
+      }
+      
+      // Packet looks valid
+      error = rxpacket[PKT_ERROR];
+      port.isUsing = false; // Release port on success
+      return [rxpacket, resultRx, error];
+      
+    } catch (err) {
+      console.error("Exception in txRxPacket:", err);
+      port.isUsing = false; // Release port on exception
+      return [rxpacket, COMM_RX_FAIL, error];
+    }
+  }
+  
+  async ping(port, scsId) {
+    let modelNumber = 0;
+    let error = 0;
+
+    try {
+      if (scsId >= BROADCAST_ID) {
+        console.log(`Cannot ping broadcast ID ${scsId}`);
+        return [modelNumber, COMM_NOT_AVAILABLE, error];
+      }
+
+      const txpacket = new Array(6).fill(0);
+      txpacket[PKT_ID] = scsId;
+      txpacket[PKT_LENGTH] = 2;
+      txpacket[PKT_INSTRUCTION] = INST_PING;
+
+      console.log(`Pinging servo ID ${scsId}...`);
+      
+      // 发送ping指令并获取响应
+      const [rxpacket, result, err] = await this.txRxPacket(port, txpacket);
+      error = err;
+      
+      // 与Python SDK保持一致：如果ping成功，尝试读取地址3的型号信息
+      if (result === COMM_SUCCESS) {
+        console.log(`Ping to servo ID ${scsId} succeeded, reading model number from address 3`);
+        // 读取地址3的型号信息（2字节）
+        const [data, dataResult, dataError] = await this.readTxRx(port, scsId, 3, 2);
+        
+        if (dataResult === COMM_SUCCESS && data && data.length >= 2) {
+          modelNumber = SCS_MAKEWORD(data[0], data[1]);
+          console.log(`Model number read: ${modelNumber}`);
+        } else {
+          console.log(`Could not read model number: ${this.getTxRxResult(dataResult)}`);
+        }
+      } else {
+        console.log(`Ping failed with result: ${result}, error: ${error}`);
+      }
+      
+      return [modelNumber, result, error];
+    } catch (error) {
+      console.error(`Exception in ping():`, error);
+      return [0, COMM_RX_FAIL, 0];
+    }
+  }
+  
+  // Read methods
+  async readTxRx(port, scsId, address, length) {
+    if (scsId >= BROADCAST_ID) {
+      console.log('Cannot read from broadcast ID');
+      return [[], COMM_NOT_AVAILABLE, 0];
+    }
+    
+    // Create read packet
+    const txpacket = new Array(8).fill(0);
+    txpacket[PKT_ID] = scsId;
+    txpacket[PKT_LENGTH] = 4;
+    txpacket[PKT_INSTRUCTION] = INST_READ;
+    txpacket[PKT_PARAMETER0] = address;
+    txpacket[PKT_PARAMETER0 + 1] = length;
+    
+    // console.log(`Reading ${length} bytes from address ${address} for servo ID ${scsId}`);
+    
+    // Send packet and get response
+    const [rxpacket, result, error] = await this.txRxPacket(port, txpacket);
+    
+    // Process the result
+    if (result !== COMM_SUCCESS) {
+      console.log(`Read failed with result: ${result}, error: ${error}`);
+      return [[], result, error];
+    }
+    
+    if (!rxpacket || rxpacket.length < PKT_PARAMETER0 + length) {
+      console.log(`Invalid response packet: expected at least ${PKT_PARAMETER0 + length} bytes, got ${rxpacket ? rxpacket.length : 0}`);
+      return [[], COMM_RX_CORRUPT, error];
+    }
+    
+    // Extract data from response
+    const data = [];
+    // console.log(`Response packet length: ${rxpacket.length}, extracting ${length} bytes from offset ${PKT_PARAMETER0}`);
+    // console.log(`Response data bytes: ${rxpacket.slice(PKT_PARAMETER0, PKT_PARAMETER0 + length).map(b => '0x' + b.toString(16).padStart(2,'0')).join(' ')}`);
+    
+    for (let i = 0; i < length; i++) {
+      data.push(rxpacket[PKT_PARAMETER0 + i]);
+    }
+    
+    // console.log(`Successfully read ${length} bytes: ${data.map(b => '0x' + b.toString(16).padStart(2,'0')).join(' ')}`);
+    return [data, result, error];
+  }
+  
+  async read1ByteTxRx(port, scsId, address) {
+    const [data, result, error] = await this.readTxRx(port, scsId, address, 1);
+    const value = (data.length > 0) ? data[0] : 0;
+    return [value, result, error];
+  }
+  
+  async read2ByteTxRx(port, scsId, address) {
+    const [data, result, error] = await this.readTxRx(port, scsId, address, 2);
+
+    let value = 0;
+    if (data.length >= 2) {
+      value = SCS_MAKEWORD(data[0], data[1]);
+    }
+    
+    return [value, result, error];
+  }
+  
+  async read4ByteTxRx(port, scsId, address) {
+    const [data, result, error] = await this.readTxRx(port, scsId, address, 4);
+    
+    let value = 0;
+    if (data.length >= 4) {
+      const loword = SCS_MAKEWORD(data[0], data[1]);
+      const hiword = SCS_MAKEWORD(data[2], data[3]);
+      value = SCS_MAKEDWORD(loword, hiword);
+      
+      console.log(`read4ByteTxRx: data=${data.map(b => '0x' + b.toString(16).padStart(2,'0')).join(' ')}`);
+      console.log(`  loword=${loword} (0x${loword.toString(16)}), hiword=${hiword} (0x${hiword.toString(16)})`);
+      console.log(`  value=${value} (0x${value.toString(16)})`);
+    }
+    
+    return [value, result, error];
+  }
+  
+  // Write methods
+  async writeTxRx(port, scsId, address, length, data) {
+    if (scsId >= BROADCAST_ID) {
+      return [COMM_NOT_AVAILABLE, 0];
+    }
+    
+    // Create write packet
+    const txpacket = new Array(length + 7).fill(0);
+    txpacket[PKT_ID] = scsId;
+    txpacket[PKT_LENGTH] = length + 3;
+    txpacket[PKT_INSTRUCTION] = INST_WRITE;
+    txpacket[PKT_PARAMETER0] = address;
+    
+    // Add data
+    for (let i = 0; i < length; i++) {
+      txpacket[PKT_PARAMETER0 + 1 + i] = data[i] & 0xFF;
+    }
+    
+    // Send packet and get response
+    const [rxpacket, result, error] = await this.txRxPacket(port, txpacket);
+    
+    return [result, error];
+  }
+  
+  async write1ByteTxRx(port, scsId, address, data) {
+    const dataArray = [data & 0xFF];
+    return await this.writeTxRx(port, scsId, address, 1, dataArray);
+  }
+  
+  async write2ByteTxRx(port, scsId, address, data) {
+    const dataArray = [
+      SCS_LOBYTE(data),
+      SCS_HIBYTE(data)
+    ];
+    return await this.writeTxRx(port, scsId, address, 2, dataArray);
+  }
+  
+  async write4ByteTxRx(port, scsId, address, data) {
+    const dataArray = [
+      SCS_LOBYTE(SCS_LOWORD(data)),
+      SCS_HIBYTE(SCS_LOWORD(data)),
+      SCS_LOBYTE(SCS_HIWORD(data)),
+      SCS_HIBYTE(SCS_HIWORD(data))
+    ];
+    return await this.writeTxRx(port, scsId, address, 4, dataArray);
+  }
+  
+  // Add syncReadTx for GroupSyncRead functionality
+  async syncReadTx(port, startAddress, dataLength, param, paramLength) {
+    // Create packet: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN PARAM... CHKSUM
+    const txpacket = new Array(paramLength + 8).fill(0);
+
+    txpacket[PKT_ID] = BROADCAST_ID;
+    txpacket[PKT_LENGTH] = paramLength + 4; // 4: INST START_ADDR DATA_LEN CHKSUM
+    txpacket[PKT_INSTRUCTION] = INST_SYNC_READ;
+    txpacket[PKT_PARAMETER0] = startAddress;
+    txpacket[PKT_PARAMETER0 + 1] = dataLength;
+
+    // Add parameters
+    for (let i = 0; i < paramLength; i++) {
+      txpacket[PKT_PARAMETER0 + 2 + i] = param[i];
+    }
+
+    // Calculate checksum
+    const totalLen = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH
+    
+    // Add headers
+    txpacket[PKT_HEADER0] = 0xFF;
+    txpacket[PKT_HEADER1] = 0xFF;
+    
+    // Calculate checksum
+    let checksum = 0;
+    for (let i = 2; i < totalLen - 1; i++) {
+      checksum += txpacket[i] & 0xFF;
+    }
+    txpacket[totalLen - 1] = (~checksum) & 0xFF;
+
+    console.log(`SyncReadTx: ${txpacket.map(b => '0x' + b.toString(16).padStart(2,'0')).join(' ')}`);
+
+    // Send packet
+    await port.clearPort();
+    const bytesWritten = await port.writePort(txpacket);
+    if (bytesWritten !== totalLen) {
+      return COMM_TX_FAIL;
+    }
+    
+    // Set timeout based on expected response size
+    port.setPacketTimeout((6 + dataLength) * paramLength);
+
+    return COMM_SUCCESS;
+  }
+
+  // Add syncWriteTxOnly for GroupSyncWrite functionality
+  async syncWriteTxOnly(port, startAddress, dataLength, param, paramLength) {
+    // Create packet: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN PARAM... CHKSUM
+    const txpacket = new Array(paramLength + 8).fill(0);
+
+    txpacket[PKT_ID] = BROADCAST_ID;
+    txpacket[PKT_LENGTH] = paramLength + 4; // 4: INST START_ADDR DATA_LEN CHKSUM
+    txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE;
+    txpacket[PKT_PARAMETER0] = startAddress;
+    txpacket[PKT_PARAMETER0 + 1] = dataLength;
+
+    // Add parameters
+    for (let i = 0; i < paramLength; i++) {
+      txpacket[PKT_PARAMETER0 + 2 + i] = param[i];
+    }
+
+    // Calculate checksum
+    const totalLen = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH
+    
+    // Add headers
+    txpacket[PKT_HEADER0] = 0xFF;
+    txpacket[PKT_HEADER1] = 0xFF;
+    
+    // Calculate checksum
+    let checksum = 0;
+    for (let i = 2; i < totalLen - 1; i++) {
+      checksum += txpacket[i] & 0xFF;
+    }
+    txpacket[totalLen - 1] = (~checksum) & 0xFF;
+
+    // console.log(`SyncWriteTxOnly: ${txpacket.map(b => '0x' + b.toString(16).padStart(2,'0')).join(' ')}`);
+
+    // Send packet - for sync write, we don't need a response
+    await port.clearPort();
+    const bytesWritten = await port.writePort(txpacket);
+    if (bytesWritten !== totalLen) {
+      return COMM_TX_FAIL;
+    }
+    
+    return COMM_SUCCESS;
+  }
+  
+  // 辅助方法：格式化数据包结构以方便调试
+  formatPacketStructure(packet) {
+    if (!packet || packet.length < 4) {
+      return "Invalid packet (too short)";
+    }
+    
+    try {
+      let result = "";
+      result += `HEADER: ${packet[0].toString(16).padStart(2,'0')} ${packet[1].toString(16).padStart(2,'0')} | `;
+      result += `ID: ${packet[2]} | `;
+      result += `LENGTH: ${packet[3]} | `;
+      
+      if (packet.length >= 5) {
+        result += `ERROR/INST: ${packet[4].toString(16).padStart(2,'0')} | `;
+      }
+      
+      if (packet.length >= 6) {
+        result += "PARAMS: ";
+        for (let i = 5; i < packet.length - 1; i++) {
+          result += `${packet[i].toString(16).padStart(2,'0')} `;
+        }
+        result += `| CHECKSUM: ${packet[packet.length-1].toString(16).padStart(2,'0')}`;
+      }
+      
+      return result;
+    } catch (e) {
+      return "Error formatting packet: " + e.message;
+    }
+  }
+  
+  /**
+   * 从响应包中解析舵机型号
+   * @param {Array} rxpacket - 响应数据包
+   * @returns {number} 舵机型号
+   */
+  parseModelNumber(rxpacket) {
+    if (!rxpacket || rxpacket.length < 7) {
+      return 0;
+    }
+    
+    // 检查是否有参数字段
+    if (rxpacket.length <= PKT_PARAMETER0 + 1) {
+      return 0;
+    }
+    
+    const param1 = rxpacket[PKT_PARAMETER0];
+    const param2 = rxpacket[PKT_PARAMETER0 + 1];
+    
+    if (SCS_END === 0) {
+      // STS/SMS 协议的字节顺序
+      return SCS_MAKEWORD(param1, param2);
+    } else {
+      // SCS 协议的字节顺序
+      return SCS_MAKEWORD(param2, param1);
+    }
+  }
+  
+  /**
+   * Verify packet header
+   * @param {Array} packet - The packet to verify
+   * @returns {Number} COMM_SUCCESS if packet is valid, error code otherwise
+   */
+  getPacketHeader(packet) {
+    if (!packet || packet.length < 4) {
+      return COMM_RX_CORRUPT;
+    }
+    
+    // Check header
+    if (packet[PKT_HEADER0] !== 0xFF || packet[PKT_HEADER1] !== 0xFF) {
+      return COMM_RX_CORRUPT;
+    }
+    
+    // Check ID validity
+    if (packet[PKT_ID] > 0xFD) {
+      return COMM_RX_CORRUPT;
+    }
+    
+    // Check length
+    if (packet.length != (packet[PKT_LENGTH] + 4)) {
+      return COMM_RX_CORRUPT;
+    }
+    
+    // Calculate checksum
+    let checksum = 0;
+    for (let i = 2; i < packet.length - 1; i++) {
+      checksum += packet[i] & 0xFF;
+    }
+    checksum = (~checksum) & 0xFF;
+    
+    // Verify checksum
+    if (packet[packet.length - 1] !== checksum) {
+      return COMM_RX_CORRUPT;
+    }
+    
+    return COMM_SUCCESS;
+  }
 }
 
 /**
@@ -928,213 +901,200 @@ export class PacketHandler {
  * - This class is used to read multiple servos with the same control table address at once
  */
 export class GroupSyncRead {
-	constructor(port, ph, startAddress, dataLength) {
-		this.port = port;
-		this.ph = ph;
-		this.startAddress = startAddress;
-		this.dataLength = dataLength;
-
-		this.isAvailableServiceID = new Set();
-		this.dataDict = new Map();
-		this.param = [];
-		this.clearParam();
-	}
-
-	makeParam() {
-		this.param = [];
-		for (const id of this.isAvailableServiceID) {
-			this.param.push(id);
-		}
-		return this.param.length;
-	}
-
-	addParam(scsId) {
-		if (this.isAvailableServiceID.has(scsId)) {
-			return false;
-		}
-
-		this.isAvailableServiceID.add(scsId);
-		this.dataDict.set(scsId, new Array(this.dataLength).fill(0));
-		return true;
-	}
-
-	removeParam(scsId) {
-		if (!this.isAvailableServiceID.has(scsId)) {
-			return false;
-		}
-
-		this.isAvailableServiceID.delete(scsId);
-		this.dataDict.delete(scsId);
-		return true;
-	}
-
-	clearParam() {
-		this.isAvailableServiceID.clear();
-		this.dataDict.clear();
-		return true;
-	}
-
-	async txPacket() {
-		if (this.isAvailableServiceID.size === 0) {
-			return COMM_NOT_AVAILABLE;
-		}
-
-		const paramLength = this.makeParam();
-		return await this.ph.syncReadTx(
-			this.port,
-			this.startAddress,
-			this.dataLength,
-			this.param,
-			paramLength
-		);
-	}
-
-	async rxPacket() {
-		let result = COMM_RX_FAIL;
-
-		if (this.isAvailableServiceID.size === 0) {
-			return COMM_NOT_AVAILABLE;
-		}
-
-		// Set all servos' data as invalid
-		for (const id of this.isAvailableServiceID) {
-			this.dataDict.set(id, new Array(this.dataLength).fill(0));
-		}
-
-		const [rxpacket, rxResult] = await this.ph.rxPacket(this.port);
-		if (rxResult !== COMM_SUCCESS || !rxpacket || rxpacket.length === 0) {
-			return rxResult;
-		}
-
-		// More tolerant of packets with unexpected values in the PKT_ERROR field
-		// Don't require INST_STATUS to be exactly 0x55
-		debugLog(
-			`GroupSyncRead rxPacket: ID=${rxpacket[PKT_ID]}, ERROR/INST field=0x${rxpacket[PKT_ERROR].toString(16)}`
-		);
-
-		// Check if the packet matches any of the available IDs
-		if (!this.isAvailableServiceID.has(rxpacket[PKT_ID])) {
-			debugLog(
-				`Received packet with ID ${rxpacket[PKT_ID]} which is not in the available IDs list`
-			);
-			return COMM_RX_CORRUPT;
-		}
-
-		// Extract data for the matching ID
-		const scsId = rxpacket[PKT_ID];
-		const data = new Array(this.dataLength).fill(0);
-
-		// Extract the parameter data, which should start at PKT_PARAMETER0
-		if (rxpacket.length < PKT_PARAMETER0 + this.dataLength) {
-			debugLog(
-				`Packet too short: expected ${PKT_PARAMETER0 + this.dataLength} bytes, got ${rxpacket.length}`
-			);
-			return COMM_RX_CORRUPT;
-		}
-
-		for (let i = 0; i < this.dataLength; i++) {
-			data[i] = rxpacket[PKT_PARAMETER0 + i];
-		}
-
-		// Update the data dict
-		this.dataDict.set(scsId, data);
-		debugLog(
-			`Updated data for servo ID ${scsId}: ${data.map((b) => "0x" + b.toString(16).padStart(2, "0")).join(" ")}`
-		);
-
-		// Continue receiving until timeout or all data is received
-		if (this.isAvailableServiceID.size > 1) {
-			result = await this.rxPacket();
-		} else {
-			result = COMM_SUCCESS;
-		}
-
-		return result;
-	}
-
-	async txRxPacket() {
-		try {
-			// First check if port is being used
-			if (this.port.isUsing) {
-				debugLog("Port is busy, cannot start sync read operation");
-				return COMM_PORT_BUSY;
-			}
-
-			// Start the transmission
-			debugLog("Starting sync read TX/RX operation...");
-			let result = await this.txPacket();
-			if (result !== COMM_SUCCESS) {
-				debugLog(`Sync read TX failed with result: ${result}`);
-				return result;
-			}
-
-			// Get a single response with a standard timeout
-			debugLog(`Attempting to receive a response...`);
-
-			// Receive a single response
-			result = await this.rxPacket();
-
-			// Release port
-			this.port.isUsing = false;
-
-			return result;
-		} catch (error) {
-			console.error("Exception in GroupSyncRead txRxPacket:", error);
-			// Make sure port is released
-			this.port.isUsing = false;
-			return COMM_RX_FAIL;
-		}
-	}
-
-	isAvailable(scsId, address, dataLength) {
-		if (!this.isAvailableServiceID.has(scsId)) {
-			return false;
-		}
-
-		const startAddr = this.startAddress;
-		const endAddr = startAddr + this.dataLength - 1;
-
-		const reqStartAddr = address;
-		const reqEndAddr = reqStartAddr + dataLength - 1;
-
-		if (reqStartAddr < startAddr || reqEndAddr > endAddr) {
-			return false;
-		}
-
-		const data = this.dataDict.get(scsId);
-		if (!data || data.length === 0) {
-			return false;
-		}
-
-		return true;
-	}
-
-	getData(scsId, address, dataLength) {
-		if (!this.isAvailable(scsId, address, dataLength)) {
-			return 0;
-		}
-
-		const startAddr = this.startAddress;
-		const data = this.dataDict.get(scsId);
-
-		// Calculate data offset
-		const dataOffset = address - startAddr;
-
-		// Combine bytes according to dataLength
-		switch (dataLength) {
-			case 1:
-				return data[dataOffset];
-			case 2:
-				return SCS_MAKEWORD(data[dataOffset], data[dataOffset + 1]);
-			case 4:
-				return SCS_MAKEDWORD(
-					SCS_MAKEWORD(data[dataOffset], data[dataOffset + 1]),
-					SCS_MAKEWORD(data[dataOffset + 2], data[dataOffset + 3])
-				);
-			default:
-				return 0;
-		}
-	}
+  constructor(port, ph, startAddress, dataLength) {
+    this.port = port;
+    this.ph = ph;
+    this.startAddress = startAddress;
+    this.dataLength = dataLength;
+    
+    this.isAvailableServiceID = new Set();
+    this.dataDict = new Map();
+    this.param = [];
+    this.clearParam();
+  }
+
+  makeParam() {
+    this.param = [];
+    for (const id of this.isAvailableServiceID) {
+      this.param.push(id);
+    }
+    return this.param.length;
+  }
+
+  addParam(scsId) {
+    if (this.isAvailableServiceID.has(scsId)) {
+      return false;
+    }
+
+    this.isAvailableServiceID.add(scsId);
+    this.dataDict.set(scsId, new Array(this.dataLength).fill(0));
+    return true;
+  }
+
+  removeParam(scsId) {
+    if (!this.isAvailableServiceID.has(scsId)) {
+      return false;
+    }
+
+    this.isAvailableServiceID.delete(scsId);
+    this.dataDict.delete(scsId);
+    return true;
+  }
+
+  clearParam() {
+    this.isAvailableServiceID.clear();
+    this.dataDict.clear();
+    return true;
+  }
+
+  async txPacket() {
+    if (this.isAvailableServiceID.size === 0) {
+      return COMM_NOT_AVAILABLE;
+    }
+
+    const paramLength = this.makeParam();
+    return await this.ph.syncReadTx(this.port, this.startAddress, this.dataLength, this.param, paramLength);
+  }
+
+  async rxPacket() {
+    let result = COMM_RX_FAIL;
+
+    if (this.isAvailableServiceID.size === 0) {
+      return COMM_NOT_AVAILABLE;
+    }
+
+    // Set all servos' data as invalid
+    for (const id of this.isAvailableServiceID) {
+      this.dataDict.set(id, new Array(this.dataLength).fill(0));
+      console.log(`Cleared data for servo ID ${id}`);
+    }
+
+    const [rxpacket, rxResult] = await this.ph.rxPacket(this.port);
+    if (rxResult !== COMM_SUCCESS || !rxpacket || rxpacket.length === 0) {
+      return rxResult;
+    }
+
+    // More tolerant of packets with unexpected values in the PKT_ERROR field
+    // Don't require INST_STATUS to be exactly 0x55
+    console.log(`GroupSyncRead rxPacket: ID=${rxpacket[PKT_ID]}, ERROR/INST field=0x${rxpacket[PKT_ERROR].toString(16)}`);
+
+    // Check if the packet matches any of the available IDs
+    if (!this.isAvailableServiceID.has(rxpacket[PKT_ID])) {
+      console.log(`Received packet with ID ${rxpacket[PKT_ID]} which is not in the available IDs list`);
+      return COMM_RX_CORRUPT;
+    }
+
+    // Extract data for the matching ID
+    const scsId = rxpacket[PKT_ID];
+    const data = new Array(this.dataLength).fill(0);
+
+    // Extract the parameter data, which should start at PKT_PARAMETER0
+    if (rxpacket.length < PKT_PARAMETER0 + this.dataLength) {
+      console.log(`Packet too short: expected ${PKT_PARAMETER0 + this.dataLength} bytes, got ${rxpacket.length}`);
+      return COMM_RX_CORRUPT;
+    }
+
+    for (let i = 0; i < this.dataLength; i++) {
+      data[i] = rxpacket[PKT_PARAMETER0 + i];
+    }
+
+    // Update the data dict
+    this.dataDict.set(scsId, data);
+    console.log(`Updated data for servo ID ${scsId}: ${data.map(b => '0x' + b.toString(16).padStart(2,'0')).join(' ')}`);
+    
+    // Continue receiving until timeout or all data is received
+    if (this.isAvailableServiceID.size > 1) {
+      result = await this.rxPacket();
+    } else {
+      result = COMM_SUCCESS;
+    }
+
+    return result;
+  }
+
+  async txRxPacket() {
+    try {
+      // First check if port is being used
+      if (this.port.isUsing) {
+        console.log("Port is busy, cannot start sync read operation");
+        return COMM_PORT_BUSY;
+      }
+      
+      // Start the transmission
+      console.log("Starting sync read TX/RX operation...");
+      let result = await this.txPacket();
+      if (result !== COMM_SUCCESS) {
+        console.log(`Sync read TX failed with result: ${result}`);
+        return result;
+      }
+
+      // Get a single response with a standard timeout
+      console.log(`Attempting to receive a response...`);
+      
+      // Receive a single response
+      result = await this.rxPacket();
+      console.log(`Sync read RX result###: ${result}`);
+      // Release port
+      this.port.isUsing = false;
+      
+      return result;
+    } catch (error) {
+      console.error("Exception in GroupSyncRead txRxPacket:", error);
+      // Make sure port is released
+      this.port.isUsing = false;
+      return COMM_RX_FAIL;
+    }
+  }
+
+  isAvailable(scsId, address, dataLength) {
+    if (!this.isAvailableServiceID.has(scsId)) {
+      return false;
+    }
+
+    const startAddr = this.startAddress;
+    const endAddr = startAddr + this.dataLength - 1;
+    
+    const reqStartAddr = address;
+    const reqEndAddr = reqStartAddr + dataLength - 1;
+    
+    if (reqStartAddr < startAddr || reqEndAddr > endAddr) {
+      return false;
+    }
+
+    const data = this.dataDict.get(scsId);
+    if (!data || data.length === 0) {
+      return false;
+    }
+
+    return true;
+  }
+
+  getData(scsId, address, dataLength) {
+    if (!this.isAvailable(scsId, address, dataLength)) {
+      return 0;
+    }
+
+    const startAddr = this.startAddress;
+    const data = this.dataDict.get(scsId);
+    
+    // Calculate data offset
+    const dataOffset = address - startAddr;
+    
+    // Combine bytes according to dataLength
+    switch (dataLength) {
+      case 1:
+        return data[dataOffset];
+      case 2:
+        return SCS_MAKEWORD(data[dataOffset], data[dataOffset + 1]);
+      case 4:
+        return SCS_MAKEDWORD(
+          SCS_MAKEWORD(data[dataOffset], data[dataOffset + 1]),
+          SCS_MAKEWORD(data[dataOffset + 2], data[dataOffset + 3])
+        );
+      default:
+        return 0;
+    }
+  }
 }
 
 /**
@@ -1142,94 +1102,84 @@ export class GroupSyncRead {
  * - This class is used to write multiple servos with the same control table address at once
  */
 export class GroupSyncWrite {
-	constructor(port, ph, startAddress, dataLength) {
-		this.port = port;
-		this.ph = ph;
-		this.startAddress = startAddress;
-		this.dataLength = dataLength;
-
-		this.isAvailableServiceID = new Set();
-		this.dataDict = new Map();
-		this.param = [];
-		this.clearParam();
-	}
-
-	makeParam() {
-		this.param = [];
-		for (const id of this.isAvailableServiceID) {
-			// Add ID to parameter
-			this.param.push(id);
-
-			// Add data to parameter
-			const data = this.dataDict.get(id);
-			for (let i = 0; i < this.dataLength; i++) {
-				this.param.push(data[i]);
-			}
-		}
-		return this.param.length;
-	}
-
-	addParam(scsId, data) {
-		if (this.isAvailableServiceID.has(scsId)) {
-			return false;
-		}
-
-		if (data.length !== this.dataLength) {
-			console.error(
-				`Data length (${data.length}) doesn't match required length (${this.dataLength})`
-			);
-			return false;
-		}
-
-		this.isAvailableServiceID.add(scsId);
-		this.dataDict.set(scsId, data);
-		return true;
-	}
-
-	removeParam(scsId) {
-		if (!this.isAvailableServiceID.has(scsId)) {
-			return false;
-		}
-
-		this.isAvailableServiceID.delete(scsId);
-		this.dataDict.delete(scsId);
-		return true;
-	}
-
-	changeParam(scsId, data) {
-		if (!this.isAvailableServiceID.has(scsId)) {
-			return false;
-		}
-
-		if (data.length !== this.dataLength) {
-			console.error(
-				`Data length (${data.length}) doesn't match required length (${this.dataLength})`
-			);
-			return false;
-		}
-
-		this.dataDict.set(scsId, data);
-		return true;
-	}
-
-	clearParam() {
-		this.isAvailableServiceID.clear();
-		this.dataDict.clear();
-		return true;
-	}
-
-	async txPacket() {
-		if (this.isAvailableServiceID.size === 0) {
-			return COMM_NOT_AVAILABLE;
-		}
-
-		const paramLength = this.makeParam();
-		return await this.ph.syncWriteTxOnly(
-			this.port,
-			this.startAddress,
-			this.dataLength,
-			this.param,
-			paramLength
-		);
-	}
+  constructor(port, ph, startAddress, dataLength) {
+    this.port = port;
+    this.ph = ph;
+    this.startAddress = startAddress;
+    this.dataLength = dataLength;
+    
+    this.isAvailableServiceID = new Set();
+    this.dataDict = new Map();
+    this.param = [];
+    this.clearParam();
+  }
+
+  makeParam() {
+    this.param = [];
+    for (const id of this.isAvailableServiceID) {
+      // Add ID to parameter
+      this.param.push(id);
+
+      // Add data to parameter
+      const data = this.dataDict.get(id);
+      for (let i = 0; i < this.dataLength; i++) {
+        this.param.push(data[i]);
+      }
+    }
+    return this.param.length;
+  }
+
+  addParam(scsId, data) {
+    if (this.isAvailableServiceID.has(scsId)) {
+      return false;
+    }
+
+    if (data.length !== this.dataLength) {
+      console.error(`Data length (${data.length}) doesn't match required length (${this.dataLength})`);
+      return false;
+    }
+
+    this.isAvailableServiceID.add(scsId);
+    this.dataDict.set(scsId, data);
+    return true;
+  }
+
+  removeParam(scsId) {
+    if (!this.isAvailableServiceID.has(scsId)) {
+      return false;
+    }
+
+    this.isAvailableServiceID.delete(scsId);
+    this.dataDict.delete(scsId);
+    return true;
+  }
+
+  changeParam(scsId, data) {
+    if (!this.isAvailableServiceID.has(scsId)) {
+      return false;
+    }
+
+    if (data.length !== this.dataLength) {
+      console.error(`Data length (${data.length}) doesn't match required length (${this.dataLength})`);
+      return false;
+    }
+
+    this.dataDict.set(scsId, data);
+    return true;
+  }
+
+  clearParam() {
+    this.isAvailableServiceID.clear();
+    this.dataDict.clear();
+    return true;
+  }
+
+  async txPacket() {
+    if (this.isAvailableServiceID.size === 0) {
+      return COMM_NOT_AVAILABLE;
+    }
+
+    const paramLength = this.makeParam();
+    return await this.ph.syncWriteTxOnly(this.port, this.startAddress, this.dataLength, this.param, paramLength);
+  }
 }

packages/feetech.js/package.json

@@ -1,38 +1,38 @@
 {
-	"name": "feetech.js",
-	"version": "0.0.11",
-	"description": "javascript sdk for feetech servos",
-	"main": "index.mjs",
-	"files": [
-		"*.mjs",
-		"*.ts"
-	],
-	"type": "module",
-	"engines": {
-		"node": ">=12.17.0"
-	},
-	"scripts": {
-		"test": "echo \"Error: no test specified\" && exit 1"
-	},
-	"repository": {
-		"type": "git",
-		"url": "git+https://github.com/julien-blanchon/robothub.git#main:frontend/packages/feetech.js"
-	},
-	"keywords": [
-		"feetech",
-		"sdk",
-		"js",
-		"javascript",
-		"sts3215",
-		"3215",
-		"scs",
-		"scs3215",
-		"st3215"
-	],
-	"author": "timqian",
-	"license": "MIT",
-	"bugs": {
-		"url": "https://github.com/julien-blanchon/robothub.git#main:frontend/packages/feetech.js"
-	},
-	"homepage": "https://github.com/julien-blanchon/robothub.git#main:frontend/packages/feetech.js"
+  "name": "feetech.js",
+  "version": "0.1.2",
+  "description": "javascript sdk for feetech servos",
+  "main": "index.mjs",
+  "files": [
+    "*.mjs",
+    "*.ts"
+  ],
+  "type": "module",
+  "engines": {
+    "node": ">=12.17.0"
+  },
+  "scripts": {
+    "test": "echo \"Error: no test specified\" && exit 1"
+  },
+  "repository": {
+    "type": "git",
+    "url": "git+https://github.com/timqian/bambot/tree/main/feetech.js"
+  },
+  "keywords": [
+    "feetech",
+    "sdk",
+    "js",
+    "javascript",
+    "sts3215",
+    "3215",
+    "scs",
+    "scs3215",
+    "st3215"
+  ],
+  "author": "timqian",
+  "license": "MIT",
+  "bugs": {
+    "url": "https://github.com/timqian/bambot/issues"
+  },
+  "homepage": "https://github.com/timqian/bambot/tree/main/feetech.js"
 }

packages/feetech.js/scsServoSDK.mjs

@@ -1,1205 +1,734 @@
 import {
-	PortHandler,
-	PacketHandler,
-	COMM_SUCCESS,
-	COMM_RX_TIMEOUT,
-	COMM_RX_CORRUPT,
-	COMM_TX_FAIL,
-	COMM_NOT_AVAILABLE,
-	SCS_LOBYTE,
-	SCS_HIBYTE,
-	SCS_MAKEWORD,
-	GroupSyncRead, // Import GroupSyncRead
-	GroupSyncWrite // Import GroupSyncWrite
+  PortHandler,
+  PacketHandler,
+  COMM_SUCCESS,
+  COMM_RX_TIMEOUT,
+  COMM_RX_CORRUPT,
+  COMM_RX_FAIL,
+  COMM_TX_FAIL,
+  COMM_NOT_AVAILABLE,
+  SCS_LOBYTE,
+  SCS_HIBYTE,
+  SCS_MAKEWORD,
+  GroupSyncRead, // Import GroupSyncRead
+  GroupSyncWrite, // Import GroupSyncWrite
 } from "./lowLevelSDK.mjs";
 
 // Import address constants from the correct file
 import {
-	ADDR_SCS_PRESENT_POSITION,
-	ADDR_SCS_GOAL_POSITION,
-	ADDR_SCS_TORQUE_ENABLE,
-	ADDR_SCS_GOAL_ACC,
-	ADDR_SCS_GOAL_SPEED
+  ADDR_SCS_PRESENT_POSITION,
+  ADDR_SCS_GOAL_POSITION,
+  ADDR_SCS_TORQUE_ENABLE,
+  ADDR_SCS_GOAL_ACC,
+  ADDR_SCS_GOAL_SPEED,
 } from "./scsservo_constants.mjs";
 
-// Import debug logging function
-import { debugLog } from "./debug.mjs";
-
 // Define constants not present in scsservo_constants.mjs
 const ADDR_SCS_MODE = 33;
 const ADDR_SCS_LOCK = 55;
 const ADDR_SCS_ID = 5; // Address for Servo ID
 const ADDR_SCS_BAUD_RATE = 6; // Address for Baud Rate
 
-// Module-level variables for handlers
-let portHandler = null;
-let packetHandler = null;
-
-/**
- * Unified Servo SDK with flexible locking control
- * Supports both locked (respects servo locks) and unlocked (temporary unlock) operations
- */
-
-/**
- * Connects to the serial port and initializes handlers.
- * @param {object} [options] - Connection options.
- * @param {number} [options.baudRate=1000000] - The baud rate for the serial connection.
- * @param {number} [options.protocolEnd=0] - The protocol end setting (0 for STS/SMS, 1 for SCS).
- * @returns {Promise<true>} Resolves with true on successful connection.
- * @throws {Error} If connection fails or port cannot be opened/selected.
- */
-export async function connect(options = {}) {
-	if (portHandler && portHandler.isOpen) {
-		debugLog("Already connected to servo system.");
-		return true;
-	}
-
-	const { baudRate = 1000000, protocolEnd = 0 } = options;
-
-	try {
-		portHandler = new PortHandler();
-		const portRequested = await portHandler.requestPort();
-		if (!portRequested) {
-			portHandler = null;
-			throw new Error("Failed to select a serial port.");
-		}
-
-		portHandler.setBaudRate(baudRate);
-		const portOpened = await portHandler.openPort();
-		if (!portOpened) {
-			await portHandler.closePort().catch(console.error);
-			portHandler = null;
-			throw new Error(`Failed to open port at baudrate ${baudRate}.`);
-		}
-
-		packetHandler = new PacketHandler(protocolEnd);
-		debugLog(`Connected to servo system at ${baudRate} baud, protocol end: ${protocolEnd}.`);
-		return true;
-	} catch (err) {
-		console.error("Error during servo connection:", err);
-		if (portHandler) {
-			try {
-				await portHandler.closePort();
-			} catch (closeErr) {
-				console.error("Error closing port after connection failure:", closeErr);
-			}
-		}
-		portHandler = null;
-		packetHandler = null;
-		throw new Error(`Servo connection failed: ${err.message}`);
-	}
-}
-
-/**
- * Disconnects from the serial port.
- * @returns {Promise<true>} Resolves with true on successful disconnection.
- * @throws {Error} If disconnection fails.
- */
-export async function disconnect() {
-	if (!portHandler || !portHandler.isOpen) {
-		debugLog("Already disconnected from servo system.");
-		return true;
-	}
-
-	try {
-		await portHandler.closePort();
-		portHandler = null;
-		packetHandler = null;
-		debugLog("Disconnected from servo system.");
-		return true;
-	} catch (err) {
-		console.error("Error during servo disconnection:", err);
-		portHandler = null;
-		packetHandler = null;
-		throw new Error(`Servo disconnection failed: ${err.message}`);
-	}
-}
-
-/**
- * Checks if the SDK is currently connected.
- * @returns {boolean} True if connected, false otherwise.
- */
-export function isConnected() {
-	return !!(portHandler && portHandler.isOpen && packetHandler);
-}
-
-/**
- * Checks if the SDK is connected. Throws an error if not.
- * @throws {Error} If not connected.
- */
-function checkConnection() {
-	if (!portHandler || !packetHandler) {
-		throw new Error("Not connected to servo system. Call connect() first.");
-	}
-}
-
-// =============================================================================
-// SERVO LOCKING OPERATIONS
-// =============================================================================
-
-/**
- * Locks a servo to prevent configuration changes.
- * @param {number} servoId - The ID of the servo (1-252).
- * @returns {Promise<"success">} Resolves with "success".
- * @throws {Error} If not connected, write fails, or an exception occurs.
- */
-export async function lockServo(servoId) {
-	checkConnection();
-	try {
-		debugLog(`🔒 Locking servo ${servoId}...`);
-		const [result, error] = await packetHandler.write1ByteTxRx(
-			portHandler,
-			servoId,
-			ADDR_SCS_LOCK,
-			1
-		);
-
-		if (result !== COMM_SUCCESS) {
-			throw new Error(
-				`Error locking servo ${servoId}: ${packetHandler.getTxRxResult(result)}, Error: ${error}`
-			);
-		}
-		debugLog(`🔒 Servo ${servoId} locked successfully`);
-		return "success";
-	} catch (err) {
-		console.error(`Exception locking servo ${servoId}:`, err);
-		throw new Error(`Failed to lock servo ${servoId}: ${err.message}`);
-	}
-}
-
-/**
- * Unlocks a servo to allow configuration changes.
- * @param {number} servoId - The ID of the servo (1-252).
- * @returns {Promise<"success">} Resolves with "success".
- * @throws {Error} If not connected, write fails, or an exception occurs.
- */
-export async function unlockServo(servoId) {
-	checkConnection();
-	try {
-		debugLog(`🔓 Unlocking servo ${servoId}...`);
-		const [result, error] = await packetHandler.write1ByteTxRx(
-			portHandler,
-			servoId,
-			ADDR_SCS_LOCK,
-			0
-		);
-
-		if (result !== COMM_SUCCESS) {
-			throw new Error(
-				`Error unlocking servo ${servoId}: ${packetHandler.getTxRxResult(result)}, Error: ${error}`
-			);
-		}
-		debugLog(`🔓 Servo ${servoId} unlocked successfully`);
-		return "success";
-	} catch (err) {
-		console.error(`Exception unlocking servo ${servoId}:`, err);
-		throw new Error(`Failed to unlock servo ${servoId}: ${err.message}`);
-	}
-}
-
-/**
- * Locks multiple servos sequentially.
- * @param {number[]} servoIds - Array of servo IDs to lock.
- * @returns {Promise<"success">} Resolves with "success".
- * @throws {Error} If any servo fails to lock.
- */
-export async function lockServos(servoIds) {
-	checkConnection();
-	debugLog(`🔒 Locking ${servoIds.length} servos: [${servoIds.join(', ')}]`);
-	
-	// Lock servos sequentially to avoid port conflicts
-	for (const servoId of servoIds) {
-		await lockServo(servoId);
-	}
-	
-	debugLog(`🔒 All ${servoIds.length} servos locked successfully`);
-	return "success";
-}
-
-/**
- * Locks servos for production use by both locking configuration and enabling torque.
- * This ensures servos are truly locked and controlled by the system.
- * @param {number[]} servoIds - Array of servo IDs to lock for production.
- * @returns {Promise<"success">} Resolves with "success".
- * @throws {Error} If any servo fails to lock or enable torque.
- */
-export async function lockServosForProduction(servoIds) {
-	checkConnection();
-	debugLog(`🔒 Locking ${servoIds.length} servos for production use: [${servoIds.join(', ')}]`);
-	
-	// Lock servos sequentially and enable torque for each
-	for (const servoId of servoIds) {
-		try {
-			debugLog(`🔒 Locking servo ${servoId} for production...`);
-			
-			// 1. Lock the servo configuration
-			const [lockResult, lockError] = await packetHandler.write1ByteTxRx(
-				portHandler,
-				servoId,
-				ADDR_SCS_LOCK,
-				1
-			);
-			
-			if (lockResult !== COMM_SUCCESS) {
-				throw new Error(`Error locking servo ${servoId}: ${packetHandler.getTxRxResult(lockResult)}, Error: ${lockError}`);
-			}
-			
-			// 2. Enable torque to make servo controllable
-			const [torqueResult, torqueError] = await packetHandler.write1ByteTxRx(
-				portHandler,
-				servoId,
-				ADDR_SCS_TORQUE_ENABLE,
-				1
-			);
-			
-			if (torqueResult !== COMM_SUCCESS) {
-				console.warn(`Warning: Failed to enable torque for servo ${servoId}: ${packetHandler.getTxRxResult(torqueResult)}, Error: ${torqueError}`);
-				// Don't throw here, locking is more important than torque enable
-			}
-			
-			debugLog(`🔒 Servo ${servoId} locked and torque enabled for production`);
-		} catch (err) {
-			console.error(`Exception locking servo ${servoId} for production:`, err);
-			throw new Error(`Failed to lock servo ${servoId} for production: ${err.message}`);
-		}
-	}
-	
-	debugLog(`🔒 All ${servoIds.length} servos locked for production successfully`);
-	return "success";
-}
-
-/**
- * Unlocks multiple servos sequentially.
- * @param {number[]} servoIds - Array of servo IDs to unlock.
- * @returns {Promise<"success">} Resolves with "success".
- * @throws {Error} If any servo fails to unlock.
- */
-export async function unlockServos(servoIds) {
-	checkConnection();
-	debugLog(`🔓 Unlocking ${servoIds.length} servos: [${servoIds.join(', ')}]`);
-	
-	// Unlock servos sequentially to avoid port conflicts
-	for (const servoId of servoIds) {
-		await unlockServo(servoId);
-	}
-	
-	debugLog(`🔓 All ${servoIds.length} servos unlocked successfully`);
-	return "success";
-}
-
-/**
- * Safely unlocks servos for manual movement by unlocking configuration and disabling torque.
- * This is the safest way to leave servos when disconnecting/cleaning up.
- * @param {number[]} servoIds - Array of servo IDs to unlock safely.
- * @returns {Promise<"success">} Resolves with "success".
- * @throws {Error} If any servo fails to unlock or disable torque.
- */
-export async function unlockServosForManualMovement(servoIds) {
-	checkConnection();
-	debugLog(`🔓 Safely unlocking ${servoIds.length} servos for manual movement: [${servoIds.join(', ')}]`);
-	
-	// Unlock servos sequentially and disable torque for each
-	for (const servoId of servoIds) {
-		try {
-			debugLog(`🔓 Safely unlocking servo ${servoId} for manual movement...`);
-			
-			// 1. Disable torque first (makes servo freely movable)
-			const [torqueResult, torqueError] = await packetHandler.write1ByteTxRx(
-				portHandler,
-				servoId,
-				ADDR_SCS_TORQUE_ENABLE,
-				0
-			);
-			
-			if (torqueResult !== COMM_SUCCESS) {
-				console.warn(`Warning: Failed to disable torque for servo ${servoId}: ${packetHandler.getTxRxResult(torqueResult)}, Error: ${torqueError}`);
-				// Continue anyway, unlocking is more important
-			}
-			
-			// 2. Unlock the servo configuration
-			const [unlockResult, unlockError] = await packetHandler.write1ByteTxRx(
-				portHandler,
-				servoId,
-				ADDR_SCS_LOCK,
-				0
-			);
-			
-			if (unlockResult !== COMM_SUCCESS) {
-				throw new Error(`Error unlocking servo ${servoId}: ${packetHandler.getTxRxResult(unlockResult)}, Error: ${unlockError}`);
-			}
-			
-			debugLog(`🔓 Servo ${servoId} safely unlocked - torque disabled and configuration unlocked`);
-		} catch (err) {
-			console.error(`Exception safely unlocking servo ${servoId}:`, err);
-			throw new Error(`Failed to safely unlock servo ${servoId}: ${err.message}`);
-		}
-	}
-	
-	debugLog(`🔓 All ${servoIds.length} servos safely unlocked for manual movement`);
-	return "success";
-}
-
-// =============================================================================
-// READ OPERATIONS (No locking needed)
-// =============================================================================
-
-/**
- * Reads the current position of a servo.
- * @param {number} servoId - The ID of the servo (1-252).
- * @returns {Promise<number>} Resolves with the position (0-4095).
- * @throws {Error} If not connected, read fails, or an exception occurs.
- */
-export async function readPosition(servoId) {
-	checkConnection();
-	try {
-		const [position, result, error] = await packetHandler.read2ByteTxRx(
-			portHandler,
-			servoId,
-			ADDR_SCS_PRESENT_POSITION
-		);
-
-		if (result !== COMM_SUCCESS) {
-			throw new Error(
-				`Error reading position from servo ${servoId}: ${packetHandler.getTxRxResult(
-					result
-				)}, Error code: ${error}`
-			);
-		}
-		return position & 0xffff;
-	} catch (err) {
-		console.error(`Exception reading position from servo ${servoId}:`, err);
-		throw new Error(`Exception reading position from servo ${servoId}: ${err.message}`);
-	}
-}
-
-/**
- * Reads the current baud rate index of a servo.
- * @param {number} servoId - The ID of the servo (1-252).
- * @returns {Promise<number>} Resolves with the baud rate index (0-7).
- * @throws {Error} If not connected, read fails, or an exception occurs.
- */
-export async function readBaudRate(servoId) {
-	checkConnection();
-	try {
-		const [baudIndex, result, error] = await packetHandler.read1ByteTxRx(
-			portHandler,
-			servoId,
-			ADDR_SCS_BAUD_RATE
-		);
-
-		if (result !== COMM_SUCCESS) {
-			throw new Error(
-				`Error reading baud rate from servo ${servoId}: ${packetHandler.getTxRxResult(
-					result
-				)}, Error code: ${error}`
-			);
-		}
-		return baudIndex;
-	} catch (err) {
-		console.error(`Exception reading baud rate from servo ${servoId}:`, err);
-		throw new Error(`Exception reading baud rate from servo ${servoId}: ${err.message}`);
-	}
+// --- Class-based multi-instance implementation ---
+export class ScsServoSDK {
+  constructor() {
+    this.portHandler = null;
+    this.packetHandler = null;
+  }
+
+  async connect(options = {}) {
+    if (this.portHandler && this.portHandler.isOpen) {
+      console.log("Already connected.");
+      return true;
+    }
+    const { baudRate = 1000000, protocolEnd = 0 } = options;
+    try {
+      this.portHandler = new PortHandler();
+      const portRequested = await this.portHandler.requestPort();
+      if (!portRequested) {
+        this.portHandler = null;
+        throw new Error("Failed to select a serial port.");
+      }
+      this.portHandler.setBaudRate(baudRate);
+      const portOpened = await this.portHandler.openPort();
+      if (!portOpened) {
+        await this.portHandler.closePort().catch(console.error);
+        this.portHandler = null;
+        throw new Error(`Failed to open port at baudrate ${baudRate}.`);
+      }
+      this.packetHandler = new PacketHandler(protocolEnd);
+      console.log(
+        `Connected to serial port at ${baudRate} baud, protocol end: ${protocolEnd}.`
+      );
+      return true;
+    } catch (err) {
+      console.error("Error during connection:", err);
+      if (this.portHandler) {
+        try {
+          await this.portHandler.closePort();
+        } catch (closeErr) {
+          console.error(
+            "Error closing port after connection failure:",
+            closeErr
+          );
+        }
+      }
+      this.portHandler = null;
+      this.packetHandler = null;
+      throw new Error(`Connection failed: ${err.message}`);
+    }
+  }
+
+  async disconnect() {
+    if (!this.portHandler || !this.portHandler.isOpen) {
+      console.log("Already disconnected.");
+      return true;
+    }
+    try {
+      await this.portHandler.closePort();
+      this.portHandler = null;
+      this.packetHandler = null;
+      console.log("Disconnected from serial port.");
+      return true;
+    } catch (err) {
+      console.error("Error during disconnection:", err);
+      this.portHandler = null;
+      this.packetHandler = null;
+      throw new Error(`Disconnection failed: ${err.message}`);
+    }
+  }
+
+  checkConnection() {
+    if (!this.portHandler || !this.packetHandler) {
+      throw new Error("Not connected. Call connect() first.");
+    }
+  }
+
+  async readPosition(servoId) {
+    this.checkConnection();
+    try {
+      const [position, result, error] = await this.packetHandler.read2ByteTxRx(
+        this.portHandler,
+        servoId,
+        ADDR_SCS_PRESENT_POSITION
+      );
+      if (result !== COMM_SUCCESS) {
+        throw new Error(
+          `Error reading position from servo ${servoId}: ${this.packetHandler.getTxRxResult(
+            result
+          )}, Error code: ${error}`
+        );
+      }
+      return position & 0xffff;
+    } catch (err) {
+      console.error(`Exception reading position from servo ${servoId}:`, err);
+      throw new Error(
+        `Exception reading position from servo ${servoId}: ${err.message}`
+      );
+    }
+  }
+
+  async readBaudRate(servoId) {
+    this.checkConnection();
+    try {
+      const [baudIndex, result, error] = await this.packetHandler.read1ByteTxRx(
+        this.portHandler,
+        servoId,
+        ADDR_SCS_BAUD_RATE
+      );
+      if (result !== COMM_SUCCESS) {
+        throw new Error(
+          `Error reading baud rate from servo ${servoId}: ${this.packetHandler.getTxRxResult(
+            result
+          )}, Error code: ${error}`
+        );
+      }
+      return baudIndex;
+    } catch (err) {
+      console.error(`Exception reading baud rate from servo ${servoId}:`, err);
+      throw new Error(
+        `Exception reading baud rate from servo ${servoId}: ${err.message}`
+      );
+    }
+  }
+
+  async readMode(servoId) {
+    this.checkConnection();
+    try {
+      const [modeValue, result, error] = await this.packetHandler.read1ByteTxRx(
+        this.portHandler,
+        servoId,
+        ADDR_SCS_MODE
+      );
+      if (result !== COMM_SUCCESS) {
+        throw new Error(
+          `Error reading mode from servo ${servoId}: ${this.packetHandler.getTxRxResult(
+            result
+          )}, Error code: ${error}`
+        );
+      }
+      return modeValue;
+    } catch (err) {
+      console.error(`Exception reading mode from servo ${servoId}:`, err);
+      throw new Error(
+        `Exception reading mode from servo ${servoId}: ${err.message}`
+      );
+    }
+  }
+
+  async writePosition(servoId, position) {
+    this.checkConnection();
+    try {
+      if (position < 0 || position > 4095) {
+        throw new Error(
+          `Invalid position value ${position} for servo ${servoId}. Must be between 0 and 4095.`
+        );
+      }
+      const targetPosition = Math.round(position);
+      const [result, error] = await this.packetHandler.write2ByteTxRx(
+        this.portHandler,
+        servoId,
+        ADDR_SCS_GOAL_POSITION,
+        targetPosition
+      );
+      if (result !== COMM_SUCCESS) {
+        throw new Error(
+          `Error writing position to servo ${servoId}: ${this.packetHandler.getTxRxResult(
+            result
+          )}, Error code: ${error}`
+        );
+      }
+      return "success";
+    } catch (err) {
+      console.error(`Exception writing position to servo ${servoId}:`, err);
+      throw new Error(
+        `Failed to write position to servo ${servoId}: ${err.message}`
+      );
+    }
+  }
+
+  async writeTorqueEnable(servoId, enable) {
+    this.checkConnection();
+    try {
+      const enableValue = enable ? 1 : 0;
+      const [result, error] = await this.packetHandler.write1ByteTxRx(
+        this.portHandler,
+        servoId,
+        ADDR_SCS_TORQUE_ENABLE,
+        enableValue
+      );
+      if (result !== COMM_SUCCESS) {
+        throw new Error(
+          `Error setting torque for servo ${servoId}: ${this.packetHandler.getTxRxResult(
+            result
+          )}, Error code: ${error}`
+        );
+      }
+      return "success";
+    } catch (err) {
+      console.error(`Exception setting torque for servo ${servoId}:`, err);
+      throw new Error(
+        `Exception setting torque for servo ${servoId}: ${err.message}`
+      );
+    }
+  }
+
+  async writeAcceleration(servoId, acceleration) {
+    this.checkConnection();
+    try {
+      const clampedAcceleration = Math.max(
+        0,
+        Math.min(254, Math.round(acceleration))
+      );
+      const [result, error] = await this.packetHandler.write1ByteTxRx(
+        this.portHandler,
+        servoId,
+        ADDR_SCS_GOAL_ACC,
+        clampedAcceleration
+      );
+      if (result !== COMM_SUCCESS) {
+        throw new Error(
+          `Error writing acceleration to servo ${servoId}: ${this.packetHandler.getTxRxResult(
+            result
+          )}, Error code: ${error}`
+        );
+      }
+      return "success";
+    } catch (err) {
+      console.error(`Exception writing acceleration to servo ${servoId}:`, err);
+      throw new Error(
+        `Exception writing acceleration to servo ${servoId}: ${err.message}`
+      );
+    }
+  }
+
+  async setWheelMode(servoId) {
+    this.checkConnection();
+    let unlocked = false;
+    try {
+      console.log(`Setting servo ${servoId} to wheel mode...`);
+      const [resUnlock, errUnlock] = await this.packetHandler.write1ByteTxRx(
+        this.portHandler,
+        servoId,
+        ADDR_SCS_LOCK,
+        0
+      );
+      if (resUnlock !== COMM_SUCCESS) {
+        throw new Error(
+          `Failed to unlock servo ${servoId}: ${this.packetHandler.getTxRxResult(
+            resUnlock
+          )}, Error: ${errUnlock}`
+        );
+      }
+      unlocked = true;
+      const [resMode, errMode] = await this.packetHandler.write1ByteTxRx(
+        this.portHandler,
+        servoId,
+        ADDR_SCS_MODE,
+        1
+      );
+      if (resMode !== COMM_SUCCESS) {
+        throw new Error(
+          `Failed to set wheel mode for servo ${servoId}: ${this.packetHandler.getTxRxResult(
+            resMode
+          )}, Error: ${errMode}`
+        );
+      }
+      const [resLock, errLock] = await this.packetHandler.write1ByteTxRx(
+        this.portHandler,
+        servoId,
+        ADDR_SCS_LOCK,
+        1
+      );
+      if (resLock !== COMM_SUCCESS) {
+        throw new Error(
+          `Failed to lock servo ${servoId} after setting mode: ${this.packetHandler.getTxRxResult(
+            resLock
+          )}, Error: ${errLock}`
+        );
+      }
+      unlocked = false;
+      console.log(`Successfully set servo ${servoId} to wheel mode.`);
+      return "success";
+    } catch (err) {
+      console.error(`Exception setting wheel mode for servo ${servoId}:`, err);
+      if (unlocked) {
+        await this.tryLockServo(servoId);
+      }
+      throw new Error(
+        `Failed to set wheel mode for servo ${servoId}: ${err.message}`
+      );
+    }
+  }
+
+  async setPositionMode(servoId) {
+    this.checkConnection();
+    let unlocked = false;
+    try {
+      console.log(`Setting servo ${servoId} back to position mode...`);
+      const [resUnlock, errUnlock] = await this.packetHandler.write1ByteTxRx(
+        this.portHandler,
+        servoId,
+        ADDR_SCS_LOCK,
+        0
+      );
+      if (resUnlock !== COMM_SUCCESS) {
+        throw new Error(
+          `Failed to unlock servo ${servoId}: ${this.packetHandler.getTxRxResult(
+            resUnlock
+          )}, Error: ${errUnlock}`
+        );
+      }
+      unlocked = true;
+      const [resMode, errMode] = await this.packetHandler.write1ByteTxRx(
+        this.portHandler,
+        servoId,
+        ADDR_SCS_MODE,
+        0
+      );
+      if (resMode !== COMM_SUCCESS) {
+        throw new Error(
+          `Failed to set position mode for servo ${servoId}: ${this.packetHandler.getTxRxResult(
+            resMode
+          )}, Error: ${errMode}`
+        );
+      }
+      const [resLock, errLock] = await this.packetHandler.write1ByteTxRx(
+        this.portHandler,
+        servoId,
+        ADDR_SCS_LOCK,
+        1
+      );
+      if (resLock !== COMM_SUCCESS) {
+        throw new Error(
+          `Failed to lock servo ${servoId} after setting mode: ${this.packetHandler.getTxRxResult(
+            resLock
+          )}, Error: ${errLock}`
+        );
+      }
+      unlocked = false;
+      console.log(`Successfully set servo ${servoId} back to position mode.`);
+      return "success";
+    } catch (err) {
+      console.error(
+        `Exception setting position mode for servo ${servoId}:`,
+        err
+      );
+      if (unlocked) {
+        await this.tryLockServo(servoId);
+      }
+      throw new Error(
+        `Failed to set position mode for servo ${servoId}: ${err.message}`
+      );
+    }
+  }
+
+  async tryLockServo(servoId) {
+    try {
+      await this.packetHandler.write1ByteTxRx(
+        this.portHandler,
+        servoId,
+        ADDR_SCS_LOCK,
+        1
+      );
+    } catch (lockErr) {
+      console.error(`Failed to re-lock servo ${servoId}:`, lockErr);
+    }
+  }
+
+  async writeWheelSpeed(servoId, speed) {
+    this.checkConnection();
+    try {
+      const clampedSpeed = Math.max(-10000, Math.min(10000, Math.round(speed)));
+      let speedValue = Math.abs(clampedSpeed) & 0x7fff;
+      if (clampedSpeed < 0) {
+        speedValue |= 0x8000;
+      }
+      const [result, error] = await this.packetHandler.write2ByteTxRx(
+        this.portHandler,
+        servoId,
+        ADDR_SCS_GOAL_SPEED,
+        speedValue
+      );
+      if (result !== COMM_SUCCESS) {
+        throw new Error(
+          `Error writing wheel speed to servo ${servoId}: ${this.packetHandler.getTxRxResult(
+            result
+          )}, Error: ${error}`
+        );
+      }
+      return "success";
+    } catch (err) {
+      console.error(`Exception writing wheel speed to servo ${servoId}:`, err);
+      throw new Error(
+        `Exception writing wheel speed to servo ${servoId}: ${err.message}`
+      );
+    }
+  }
+
+  async syncWriteWheelSpeed(servoSpeeds) {
+    this.checkConnection();
+    const groupSyncWrite = new GroupSyncWrite(
+      this.portHandler,
+      this.packetHandler,
+      ADDR_SCS_GOAL_SPEED,
+      2
+    );
+    let paramAdded = false;
+    const entries =
+      servoSpeeds instanceof Map
+        ? servoSpeeds.entries()
+        : Object.entries(servoSpeeds);
+    for (const [idStr, speed] of entries) {
+      const servoId = parseInt(idStr, 10);
+      if (isNaN(servoId) || servoId < 1 || servoId > 252) {
+        throw new Error(`Invalid servo ID "${idStr}" in syncWriteWheelSpeed.`);
+      }
+      if (speed < -10000 || speed > 10000) {
+        throw new Error(
+          `Invalid speed value ${speed} for servo ${servoId} in syncWriteWheelSpeed. Must be between -10000 and 10000.`
+        );
+      }
+      const clampedSpeed = Math.max(-10000, Math.min(10000, Math.round(speed)));
+      let speedValue = Math.abs(clampedSpeed) & 0x7fff;
+      if (clampedSpeed < 0) {
+        speedValue |= 0x8000;
+      }
+      const data = [SCS_LOBYTE(speedValue), SCS_HIBYTE(speedValue)];
+      if (groupSyncWrite.addParam(servoId, data)) {
+        paramAdded = true;
+      } else {
+        console.warn(
+          `Failed to add servo ${servoId} to sync write speed group (possibly duplicate).`
+        );
+      }
+    }
+    if (!paramAdded) {
+      console.log("Sync Write Speed: No valid servo speeds provided or added.");
+      return "success";
+    }
+    try {
+      const result = await groupSyncWrite.txPacket();
+      if (result !== COMM_SUCCESS) {
+        throw new Error(
+          `Sync Write Speed txPacket failed: ${this.packetHandler.getTxRxResult(
+            result
+          )}`
+        );
+      }
+      return "success";
+    } catch (err) {
+      console.error("Exception during syncWriteWheelSpeed:", err);
+      throw new Error(`Sync Write Speed failed: ${err.message}`);
+    }
+  }
+
+  async syncReadPositions(servoIds) {
+    this.checkConnection();
+    if (!Array.isArray(servoIds) || servoIds.length === 0) {
+      console.log("Sync Read: No servo IDs provided.");
+      return new Map();
+    }
+    const startAddress = ADDR_SCS_PRESENT_POSITION;
+    const positions = new Map();
+    for (const id of servoIds) {
+      if (id < 1 || id > 252) {
+        console.warn(`Sync Read: Invalid servo ID ${id} skipped.`);
+        continue;
+      }
+      try {
+        const [pos, result, error] = await this.packetHandler.read2ByteTxRx(
+          this.portHandler,
+          id,
+          startAddress
+        );
+        if (result === COMM_SUCCESS) {
+          positions.set(id, pos & 0xffff);
+        } else {
+          console.warn(
+            `Sync Read: Failed to read position for servo ID ${id}: ${this.packetHandler.getTxRxResult(
+              result
+            )}, Error: ${error}`
+          );
+        }
+      } catch (e) {
+        console.warn(`Sync Read: Exception reading servo ID ${id}:`, e);
+      }
+    }
+    return positions;
+  }
+
+  async syncWritePositions(servoPositions) {
+    this.checkConnection();
+    const groupSyncWrite = new GroupSyncWrite(
+      this.portHandler,
+      this.packetHandler,
+      ADDR_SCS_GOAL_POSITION,
+      2
+    );
+    let paramAdded = false;
+    const entries =
+      servoPositions instanceof Map
+        ? servoPositions.entries()
+        : Object.entries(servoPositions);
+    for (const [idStr, position] of entries) {
+      const servoId = parseInt(idStr, 10);
+      if (isNaN(servoId) || servoId < 1 || servoId > 252) {
+        throw new Error(`Invalid servo ID "${idStr}" in syncWritePositions.`);
+      }
+      if (position < 0 || position > 4095) {
+        throw new Error(
+          `Invalid position value ${position} for servo ${servoId} in syncWritePositions. Must be between 0 and 4095.`
+        );
+      }
+      const targetPosition = Math.round(position);
+      const data = [SCS_LOBYTE(targetPosition), SCS_HIBYTE(targetPosition)];
+      if (groupSyncWrite.addParam(servoId, data)) {
+        paramAdded = true;
+      } else {
+        console.warn(
+          `Failed to add servo ${servoId} to sync write group (possibly duplicate).`
+        );
+      }
+    }
+    if (!paramAdded) {
+      console.log("Sync Write: No valid servo positions provided or added.");
+      return "success";
+    }
+    try {
+      const result = await groupSyncWrite.txPacket();
+      if (result !== COMM_SUCCESS) {
+        throw new Error(
+          `Sync Write txPacket failed: ${this.packetHandler.getTxRxResult(
+            result
+          )}`
+        );
+      }
+      return "success";
+    } catch (err) {
+      console.error("Exception during syncWritePositions:", err);
+      throw new Error(`Sync Write failed: ${err.message}`);
+    }
+  }
+
+  async setBaudRate(servoId, baudRateIndex) {
+    this.checkConnection();
+    if (servoId < 1 || servoId > 252) {
+      throw new Error(
+        `Invalid servo ID provided: ${servoId}. Must be between 1 and 252.`
+      );
+    }
+    if (baudRateIndex < 0 || baudRateIndex > 7) {
+      throw new Error(
+        `Invalid baudRateIndex: ${baudRateIndex}. Must be between 0 and 7.`
+      );
+    }
+    let unlocked = false;
+    try {
+      console.log(
+        `Setting baud rate for servo ${servoId}: Index=${baudRateIndex}`
+      );
+      const [resUnlock, errUnlock] = await this.packetHandler.write1ByteTxRx(
+        this.portHandler,
+        servoId,
+        ADDR_SCS_LOCK,
+        0
+      );
+      if (resUnlock !== COMM_SUCCESS) {
+        throw new Error(
+          `Failed to unlock servo ${servoId}: ${this.packetHandler.getTxRxResult(
+            resUnlock
+          )}, Error: ${errUnlock}`
+        );
+      }
+      unlocked = true;
+      const [resBaud, errBaud] = await this.packetHandler.write1ByteTxRx(
+        this.portHandler,
+        servoId,
+        ADDR_SCS_BAUD_RATE,
+        baudRateIndex
+      );
+      if (resBaud !== COMM_SUCCESS) {
+        throw new Error(
+          `Failed to write baud rate index ${baudRateIndex} to servo ${servoId}: ${this.packetHandler.getTxRxResult(
+            resBaud
+          )}, Error: ${errBaud}`
+        );
+      }
+      const [resLock, errLock] = await this.packetHandler.write1ByteTxRx(
+        this.portHandler,
+        servoId,
+        ADDR_SCS_LOCK,
+        1
+      );
+      if (resLock !== COMM_SUCCESS) {
+        throw new Error(
+          `Failed to lock servo ${servoId} after setting baud rate: ${this.packetHandler.getTxRxResult(
+            resLock
+          )}, Error: ${errLock}.`
+        );
+      }
+      unlocked = false;
+      console.log(
+        `Successfully set baud rate for servo ${servoId}. Index: ${baudRateIndex}. Remember to potentially reconnect with the new baud rate.`
+      );
+      return "success";
+    } catch (err) {
+      console.error(
+        `Exception during setBaudRate for servo ID ${servoId}:`,
+        err
+      );
+      if (unlocked) {
+        await this.tryLockServo(servoId);
+      }
+      throw new Error(
+        `Failed to set baud rate for servo ${servoId}: ${err.message}`
+      );
+    }
+  }
+
+  async setServoId(currentServoId, newServoId) {
+    this.checkConnection();
+    if (
+      currentServoId < 1 ||
+      currentServoId > 252 ||
+      newServoId < 1 ||
+      newServoId > 252
+    ) {
+      throw new Error(
+        `Invalid servo ID provided. Current: ${currentServoId}, New: ${newServoId}. Must be between 1 and 252.`
+      );
+    }
+    if (currentServoId === newServoId) {
+      console.log(`Servo ID is already ${newServoId}. No change needed.`);
+      return "success";
+    }
+    let unlocked = false;
+    let idWritten = false;
+    try {
+      console.log(`Setting servo ID: From ${currentServoId} to ${newServoId}`);
+      const [resUnlock, errUnlock] = await this.packetHandler.write1ByteTxRx(
+        this.portHandler,
+        currentServoId,
+        ADDR_SCS_LOCK,
+        0
+      );
+      if (resUnlock !== COMM_SUCCESS) {
+        throw new Error(
+          `Failed to unlock servo ${currentServoId}: ${this.packetHandler.getTxRxResult(
+            resUnlock
+          )}, Error: ${errUnlock}`
+        );
+      }
+      unlocked = true;
+      const [resId, errId] = await this.packetHandler.write1ByteTxRx(
+        this.portHandler,
+        currentServoId,
+        ADDR_SCS_ID,
+        newServoId
+      );
+      if (resId !== COMM_SUCCESS) {
+        throw new Error(
+          `Failed to write new ID ${newServoId} to servo ${currentServoId}: ${this.packetHandler.getTxRxResult(
+            resId
+          )}, Error: ${errId}`
+        );
+      }
+      idWritten = true;
+      const [resLock, errLock] = await this.packetHandler.write1ByteTxRx(
+        this.portHandler,
+        newServoId,
+        ADDR_SCS_LOCK,
+        1
+      );
+      if (resLock !== COMM_SUCCESS) {
+        throw new Error(
+          `Failed to lock servo with new ID ${newServoId}: ${this.packetHandler.getTxRxResult(
+            resLock
+          )}, Error: ${errLock}. Configuration might be incomplete.`
+        );
+      }
+      unlocked = false;
+      console.log(
+        `Successfully set servo ID from ${currentServoId} to ${newServoId}. Remember to use the new ID for future commands.`
+      );
+      return "success";
+    } catch (err) {
+      console.error(
+        `Exception during setServoId for current ID ${currentServoId}:`,
+        err
+      );
+      if (unlocked) {
+        const idToLock = idWritten ? newServoId : currentServoId;
+        console.warn(`Attempting to re-lock servo using ID ${idToLock}...`);
+        await this.tryLockServo(idToLock);
+      }
+      throw new Error(
+        `Failed to set servo ID from ${currentServoId} to ${newServoId}: ${err.message}`
+      );
+    }
+  }
 }
 
-/**
- * Reads the current operating mode of a servo.
- * @param {number} servoId - The ID of the servo (1-252).
- * @returns {Promise<number>} Resolves with the mode (0 for position, 1 for wheel).
- * @throws {Error} If not connected, read fails, or an exception occurs.
- */
-export async function readMode(servoId) {
-	checkConnection();
-	try {
-		const [modeValue, result, error] = await packetHandler.read1ByteTxRx(
-			portHandler,
-			servoId,
-			ADDR_SCS_MODE
-		);
-
-		if (result !== COMM_SUCCESS) {
-			throw new Error(
-				`Error reading mode from servo ${servoId}: ${packetHandler.getTxRxResult(
-					result
-				)}, Error code: ${error}`
-			);
-		}
-		return modeValue;
-	} catch (err) {
-		console.error(`Exception reading mode from servo ${servoId}:`, err);
-		throw new Error(`Exception reading mode from servo ${servoId}: ${err.message}`);
-	}
-}
-
-/**
- * Reads the current position of multiple servos synchronously.
- * @param {number[]} servoIds - An array of servo IDs (1-252) to read from.
- * @returns {Promise<Map<number, number>>} Resolves with a Map where keys are servo IDs and values are positions (0-4095).
- * @throws {Error} If not connected or transmission fails completely.
- */
-export async function syncReadPositions(servoIds) {
-	checkConnection();
-	if (!Array.isArray(servoIds) || servoIds.length === 0) {
-		debugLog("Sync Read: No servo IDs provided.");
-		return new Map();
-	}
-
-	const startAddress = ADDR_SCS_PRESENT_POSITION;
-	const dataLength = 2;
-	const groupSyncRead = new GroupSyncRead(portHandler, packetHandler, startAddress, dataLength);
-	const positions = new Map();
-	const validIds = [];
-
-	// Add parameters for each valid servo ID
-	servoIds.forEach((id) => {
-		if (id >= 1 && id <= 252) {
-			if (groupSyncRead.addParam(id)) {
-				validIds.push(id);
-			} else {
-				console.warn(`Sync Read: Failed to add param for servo ID ${id} (maybe duplicate or invalid).`);
-			}
-		} else {
-			console.warn(`Sync Read: Invalid servo ID ${id} skipped.`);
-		}
-	});
-
-	if (validIds.length === 0) {
-		debugLog("Sync Read: No valid servo IDs to read.");
-		return new Map();
-	}
-
-	try {
-		let txResult = await groupSyncRead.txPacket();
-		if (txResult !== COMM_SUCCESS) {
-			throw new Error(`Sync Read txPacket failed: ${packetHandler.getTxRxResult(txResult)}`);
-		}
-
-		let rxResult = await groupSyncRead.rxPacket();
-		if (rxResult !== COMM_SUCCESS) {
-			console.warn(`Sync Read rxPacket overall result: ${packetHandler.getTxRxResult(rxResult)}. Checking individual servos.`);
-		}
-
-		const failedIds = [];
-		validIds.forEach((id) => {
-			const isAvailable = groupSyncRead.isAvailable(id, startAddress, dataLength);
-			if (isAvailable) {
-				const position = groupSyncRead.getData(id, startAddress, dataLength);
-				positions.set(id, position & 0xffff);
-			} else {
-				failedIds.push(id);
-			}
-		});
-
-		if (failedIds.length > 0) {
-			console.warn(`Sync Read: Data not available for servo IDs: ${failedIds.join(", ")}. Got ${positions.size}/${validIds.length} servos successfully.`);
-		}
-
-		return positions;
-	} catch (err) {
-		console.error("Exception during syncReadPositions:", err);
-		throw new Error(`Sync Read failed: ${err.message}`);
-	}
-}
-
-// =============================================================================
-// WRITE OPERATIONS - LOCKED MODE (Respects servo locks)
-// =============================================================================
-
-/**
- * Writes a target position to a servo (respects locks).
- * Will fail if the servo is locked.
- * @param {number} servoId - The ID of the servo (1-252).
- * @param {number} position - The target position value (0-4095).
- * @returns {Promise<"success">} Resolves with "success".
- * @throws {Error} If not connected, position is out of range, write fails, or an exception occurs.
- */
-export async function writePosition(servoId, position) {
-	checkConnection();
-	try {
-		if (position < 0 || position > 4095) {
-			throw new Error(`Invalid position value ${position} for servo ${servoId}. Must be between 0 and 4095.`);
-		}
-		const targetPosition = Math.round(position);
-
-		const [result, error] = await packetHandler.write2ByteTxRx(
-			portHandler,
-			servoId,
-			ADDR_SCS_GOAL_POSITION,
-			targetPosition
-		);
-
-		if (result !== COMM_SUCCESS) {
-			throw new Error(`Error writing position to servo ${servoId}: ${packetHandler.getTxRxResult(result)}, Error code: ${error}`);
-		}
-		return "success";
-	} catch (err) {
-		console.error(`Exception writing position to servo ${servoId}:`, err);
-		throw new Error(`Failed to write position to servo ${servoId}: ${err.message}`);
-	}
-}
-
-/**
- * Enables or disables the torque of a servo (respects locks).
- * @param {number} servoId - The ID of the servo (1-252).
- * @param {boolean} enable - True to enable torque, false to disable.
- * @returns {Promise<"success">} Resolves with "success".
- * @throws {Error} If not connected, write fails, or an exception occurs.
- */
-export async function writeTorqueEnable(servoId, enable) {
-	checkConnection();
-	try {
-		const enableValue = enable ? 1 : 0;
-		const [result, error] = await packetHandler.write1ByteTxRx(
-			portHandler,
-			servoId,
-			ADDR_SCS_TORQUE_ENABLE,
-			enableValue
-		);
-
-		if (result !== COMM_SUCCESS) {
-			throw new Error(`Error setting torque for servo ${servoId}: ${packetHandler.getTxRxResult(result)}, Error code: ${error}`);
-		}
-		return "success";
-	} catch (err) {
-		console.error(`Exception setting torque for servo ${servoId}:`, err);
-		throw new Error(`Exception setting torque for servo ${servoId}: ${err.message}`);
-	}
-}
-
-// =============================================================================
-// WRITE OPERATIONS - UNLOCKED MODE (Temporary unlock for operation)
-// =============================================================================
-
-/**
- * Helper to attempt locking a servo, logging errors without throwing.
- * @param {number} servoId
- */
-async function tryLockServo(servoId) {
-	try {
-		await packetHandler.write1ByteTxRx(portHandler, servoId, ADDR_SCS_LOCK, 1);
-	} catch (lockErr) {
-		console.error(`Failed to re-lock servo ${servoId}:`, lockErr);
-	}
-}
-
-/**
- * Writes a target position to a servo with temporary unlocking.
- * Temporarily unlocks the servo, writes the position, then locks it back.
- * @param {number} servoId - The ID of the servo (1-252).
- * @param {number} position - The target position value (0-4095).
- * @returns {Promise<"success">} Resolves with "success".
- * @throws {Error} If not connected, position is out of range, write fails, or an exception occurs.
- */
-export async function writePositionUnlocked(servoId, position) {
-	checkConnection();
-	let unlocked = false;
-	try {
-		if (position < 0 || position > 4095) {
-			throw new Error(`Invalid position value ${position} for servo ${servoId}. Must be between 0 and 4095.`);
-		}
-		const targetPosition = Math.round(position);
-
-		debugLog(`🔓 Temporarily unlocking servo ${servoId} for position write...`);
-
-		// 1. Unlock servo configuration first
-		const [resUnlock, errUnlock] = await packetHandler.write1ByteTxRx(portHandler, servoId, ADDR_SCS_LOCK, 0);
-		if (resUnlock !== COMM_SUCCESS) {
-			debugLog(`Warning: Failed to unlock servo ${servoId}, trying direct write anyway...`);
-		} else {
-		unlocked = true;
-		}
-
-		// 2. Write the position
-		const [result, error] = await packetHandler.write2ByteTxRx(portHandler, servoId, ADDR_SCS_GOAL_POSITION, targetPosition);
-		if (result !== COMM_SUCCESS) {
-			throw new Error(`Error writing position to servo ${servoId}: ${packetHandler.getTxRxResult(result)}, Error code: ${error}`);
-		}
-
-		// 3. Lock servo configuration back
-		if (unlocked) {
-			const [resLock, errLock] = await packetHandler.write1ByteTxRx(portHandler, servoId, ADDR_SCS_LOCK, 1);
-		if (resLock !== COMM_SUCCESS) {
-				console.warn(`Warning: Failed to re-lock servo ${servoId} after position write: ${packetHandler.getTxRxResult(resLock)}, Error: ${errLock}`);
-			} else {
-				unlocked = false;
-			}
-		}
-
-		return "success";
-	} catch (err) {
-		console.error(`Exception writing position to servo ${servoId}:`, err);
-		if (unlocked) {
-			await tryLockServo(servoId);
-		}
-		throw new Error(`Failed to write position to servo ${servoId}: ${err.message}`);
-	}
-}
-
-/**
- * Writes a target position and disables torque for manual movement.
- * @param {number} servoId - The ID of the servo (1-252).
- * @param {number} position - The target position value (0-4095).
- * @param {number} waitTimeMs - Time to wait for servo to reach position (milliseconds).
- * @returns {Promise<"success">} Resolves with "success".
- * @throws {Error} If not connected, position is out of range, write fails, or an exception occurs.
- */
-export async function writePositionAndDisableTorque(servoId, position, waitTimeMs = 1500) {
-	checkConnection();
-	let unlocked = false;
-	try {
-		if (position < 0 || position > 4095) {
-			throw new Error(`Invalid position value ${position} for servo ${servoId}. Must be between 0 and 4095.`);
-		}
-		const targetPosition = Math.round(position);
-
-		debugLog(`🔓 Moving servo ${servoId} to position ${targetPosition}, waiting ${waitTimeMs}ms, then disabling torque...`);
-
-		// 1. Unlock servo configuration first
-		const [resUnlock, errUnlock] = await packetHandler.write1ByteTxRx(portHandler, servoId, ADDR_SCS_LOCK, 0);
-		if (resUnlock !== COMM_SUCCESS) {
-			debugLog(`Warning: Failed to unlock servo ${servoId}, trying direct write anyway...`);
-		} else {
-		unlocked = true;
-		}
-
-		// 2. Enable torque first
-		const [torqueEnableResult, torqueEnableError] = await packetHandler.write1ByteTxRx(portHandler, servoId, ADDR_SCS_TORQUE_ENABLE, 1);
-		if (torqueEnableResult !== COMM_SUCCESS) {
-			console.warn(`Warning: Failed to enable torque for servo ${servoId}: ${packetHandler.getTxRxResult(torqueEnableResult)}, Error: ${torqueEnableError}`);
-		} else {
-			debugLog(`✅ Torque enabled for servo ${servoId}`);
-		}
-
-		// 3. Write the position
-		const [result, error] = await packetHandler.write2ByteTxRx(portHandler, servoId, ADDR_SCS_GOAL_POSITION, targetPosition);
-		if (result !== COMM_SUCCESS) {
-			throw new Error(`Error writing position to servo ${servoId}: ${packetHandler.getTxRxResult(result)}, Error code: ${error}`);
-		}
-
-		// 4. Wait for servo to reach position
-		debugLog(`⏳ Waiting ${waitTimeMs}ms for servo ${servoId} to reach position ${targetPosition}...`);
-		await new Promise(resolve => setTimeout(resolve, waitTimeMs));
-
-		// 5. Disable torque
-		const [torqueResult, torqueError] = await packetHandler.write1ByteTxRx(portHandler, servoId, ADDR_SCS_TORQUE_ENABLE, 0);
-		if (torqueResult !== COMM_SUCCESS) {
-			console.warn(`Warning: Failed to disable torque for servo ${servoId}: ${packetHandler.getTxRxResult(torqueResult)}, Error: ${torqueError}`);
-		} else {
-			debugLog(`✅ Torque disabled for servo ${servoId} - now movable by hand`);
-		}
-
-		// 6. Lock servo configuration back
-		if (unlocked) {
-			const [resLock, errLock] = await packetHandler.write1ByteTxRx(portHandler, servoId, ADDR_SCS_LOCK, 1);
-		if (resLock !== COMM_SUCCESS) {
-				console.warn(`Warning: Failed to re-lock servo ${servoId} after position write: ${packetHandler.getTxRxResult(resLock)}, Error: ${errLock}`);
-			} else {
-				unlocked = false;
-			}
-		}
-
-		return "success";
-	} catch (err) {
-		console.error(`Exception writing position and disabling torque for servo ${servoId}:`, err);
-		if (unlocked) {
-			await tryLockServo(servoId);
-		}
-		throw new Error(`Failed to write position and disable torque for servo ${servoId}: ${err.message}`);
-	}
-}
-
-/**
- * Enables or disables the torque of a servo with temporary unlocking.
- * @param {number} servoId - The ID of the servo (1-252).
- * @param {boolean} enable - True to enable torque, false to disable.
- * @returns {Promise<"success">} Resolves with "success".
- * @throws {Error} If not connected, write fails, or an exception occurs.
- */
-export async function writeTorqueEnableUnlocked(servoId, enable) {
-	checkConnection();
-	let unlocked = false;
-	try {
-		const enableValue = enable ? 1 : 0;
-		
-		debugLog(`🔓 Temporarily unlocking servo ${servoId} for torque enable write...`);
-
-		// 1. Unlock servo configuration first
-		const [resUnlock, errUnlock] = await packetHandler.write1ByteTxRx(portHandler, servoId, ADDR_SCS_LOCK, 0);
-		if (resUnlock !== COMM_SUCCESS) {
-			debugLog(`Warning: Failed to unlock servo ${servoId}, trying direct write anyway...`);
-		} else {
-			unlocked = true;
-		}
-
-		// 2. Write the torque enable
-		const [result, error] = await packetHandler.write1ByteTxRx(portHandler, servoId, ADDR_SCS_TORQUE_ENABLE, enableValue);
-		if (result !== COMM_SUCCESS) {
-			throw new Error(`Error setting torque for servo ${servoId}: ${packetHandler.getTxRxResult(result)}, Error code: ${error}`);
-		}
-
-		// 3. Lock servo configuration back
-		if (unlocked) {
-			const [resLock, errLock] = await packetHandler.write1ByteTxRx(portHandler, servoId, ADDR_SCS_LOCK, 1);
-			if (resLock !== COMM_SUCCESS) {
-				console.warn(`Warning: Failed to re-lock servo ${servoId} after torque enable write: ${packetHandler.getTxRxResult(resLock)}, Error: ${errLock}`);
-			} else {
-				unlocked = false;
-			}
-		}
-
-		return "success";
-	} catch (err) {
-		console.error(`Exception setting torque for servo ${servoId}:`, err);
-		if (unlocked) {
-			await tryLockServo(servoId);
-		}
-		throw new Error(`Exception setting torque for servo ${servoId}: ${err.message}`);
-	}
-}
-
-// =============================================================================
-// SYNC WRITE OPERATIONS
-// =============================================================================
-
-/**
- * Writes target positions to multiple servos synchronously.
- * @param {Map<number, number> | object} servoPositions - A Map or object where keys are servo IDs (1-252) and values are target positions (0-4095).
- * @returns {Promise<"success">} Resolves with "success".
- * @throws {Error} If not connected, any position is out of range, transmission fails, or an exception occurs.
- */
-export async function syncWritePositions(servoPositions) {
-	checkConnection();
-
-	const groupSyncWrite = new GroupSyncWrite(portHandler, packetHandler, ADDR_SCS_GOAL_POSITION, 2);
-	let paramAdded = false;
-
-	const entries = servoPositions instanceof Map ? servoPositions.entries() : Object.entries(servoPositions);
-
-	for (const [idStr, position] of entries) {
-		const servoId = parseInt(idStr, 10);
-		if (isNaN(servoId) || servoId < 1 || servoId > 252) {
-			throw new Error(`Invalid servo ID "${idStr}" in syncWritePositions.`);
-		}
-		if (position < 0 || position > 4095) {
-			throw new Error(`Invalid position value ${position} for servo ${servoId} in syncWritePositions. Must be between 0 and 4095.`);
-		}
-		const targetPosition = Math.round(position);
-		const data = [SCS_LOBYTE(targetPosition), SCS_HIBYTE(targetPosition)];
-
-		if (groupSyncWrite.addParam(servoId, data)) {
-			paramAdded = true;
-		} else {
-			console.warn(`Failed to add servo ${servoId} to sync write group (possibly duplicate).`);
-		}
-	}
-
-	if (!paramAdded) {
-		debugLog("Sync Write: No valid servo positions provided or added.");
-		return "success";
-	}
-
-	try {
-		const result = await groupSyncWrite.txPacket();
-		if (result !== COMM_SUCCESS) {
-			throw new Error(`Sync Write txPacket failed: ${packetHandler.getTxRxResult(result)}`);
-		}
-		return "success";
-	} catch (err) {
-		console.error("Exception during syncWritePositions:", err);
-		throw new Error(`Sync Write failed: ${err.message}`);
-	}
-}
-
-/**
- * Writes a target speed for a servo in wheel mode.
- * @param {number} servoId - The ID of the servo
- * @param {number} speed - The target speed value (-10000 to 10000). Negative values indicate reverse direction. 0 stops the wheel.
- * @returns {Promise<"success">} Resolves with "success".
- * @throws {Error} If not connected, either write fails, or an exception occurs.
- */
-export async function writeWheelSpeed(servoId, speed) {
-	checkConnection();
-	let unlocked = false;
-	try {
-		const clampedSpeed = Math.max(-10000, Math.min(10000, Math.round(speed)));
-		let speedValue = Math.abs(clampedSpeed) & 0x7fff;
-
-		if (clampedSpeed < 0) {
-			speedValue |= 0x8000;
-		}
-
-		debugLog(`Temporarily unlocking servo ${servoId} for wheel speed write...`);
-
-		// 1. Unlock servo configuration first
-		const [resUnlock, errUnlock] = await packetHandler.write1ByteTxRx(portHandler, servoId, ADDR_SCS_LOCK, 0);
-		if (resUnlock !== COMM_SUCCESS) {
-			debugLog(`Warning: Failed to unlock servo ${servoId}, trying direct write anyway...`);
-			} else {
-			unlocked = true;
-		}
-
-		// 2. Write the speed
-		const [result, error] = await packetHandler.write2ByteTxRx(portHandler, servoId, ADDR_SCS_GOAL_SPEED, speedValue);
-		if (result !== COMM_SUCCESS) {
-			throw new Error(`Error writing wheel speed to servo ${servoId}: ${packetHandler.getTxRxResult(result)}, Error: ${error}`);
-		}
-
-		// 3. Lock servo configuration back
-		if (unlocked) {
-			const [resLock, errLock] = await packetHandler.write1ByteTxRx(portHandler, servoId, ADDR_SCS_LOCK, 1);
-			if (resLock !== COMM_SUCCESS) {
-				console.warn(`Warning: Failed to re-lock servo ${servoId} after wheel speed write: ${packetHandler.getTxRxResult(resLock)}, Error: ${errLock}`);
-			} else {
-				unlocked = false;
-			}
-		}
-
-		return "success";
-	} catch (err) {
-		console.error(`Exception writing wheel speed to servo ${servoId}:`, err);
-		if (unlocked) {
-			await tryLockServo(servoId);
-		}
-		throw new Error(`Exception writing wheel speed to servo ${servoId}: ${err.message}`);
-	}
-}
-
-/**
- * Writes target speeds to multiple servos in wheel mode synchronously.
- * @param {Map<number, number> | object} servoSpeeds - A Map or object where keys are servo IDs (1-252) and values are target speeds (-10000 to 10000).
- * @returns {Promise<"success">} Resolves with "success".
- * @throws {Error} If not connected, any speed is out of range, transmission fails, or an exception occurs.
- */
-export async function syncWriteWheelSpeed(servoSpeeds) {
-	checkConnection();
-
-	const groupSyncWrite = new GroupSyncWrite(
-		portHandler,
-		packetHandler,
-		ADDR_SCS_GOAL_SPEED,
-		2 // Data length for speed (2 bytes)
-	);
-	let paramAdded = false;
-
-	const entries = servoSpeeds instanceof Map ? servoSpeeds.entries() : Object.entries(servoSpeeds);
-
-	// Second pass: Add valid parameters
-	for (const [idStr, speed] of entries) {
-		const servoId = parseInt(idStr, 10); // Already validated
-
-		if (isNaN(servoId) || servoId < 1 || servoId > 252) {
-			throw new Error(`Invalid servo ID "${idStr}" in syncWriteWheelSpeed.`);
-		}
-		if (speed < -10000 || speed > 10000) {
-			throw new Error(
-				`Invalid speed value ${speed} for servo ${servoId} in syncWriteWheelSpeed. Must be between -10000 and 10000.`
-			);
-		}
-
-		const clampedSpeed = Math.max(-10000, Math.min(10000, Math.round(speed))); // Ensure integer, already validated range
-		let speedValue = Math.abs(clampedSpeed) & 0x7fff; // Get absolute value, ensure within 15 bits
-
-		// Set the direction bit (MSB of the 16-bit value) if speed is negative
-		if (clampedSpeed < 0) {
-			speedValue |= 0x8000; // Set the 16th bit for reverse direction
-		}
-
-		const data = [SCS_LOBYTE(speedValue), SCS_HIBYTE(speedValue)];
-
-		if (groupSyncWrite.addParam(servoId, data)) {
-			paramAdded = true;
-		} else {
-			// This should ideally not happen if IDs are unique, but handle defensively
-			console.warn(
-				`Failed to add servo ${servoId} to sync write speed group (possibly duplicate).`
-			);
-		}
-	}
-
-	if (!paramAdded) {
-		debugLog("Sync Write Speed: No valid servo speeds provided or added.");
-		return "success"; // Nothing to write is considered success
-	}
-
-	try {
-		// Send the Sync Write instruction
-		const result = await groupSyncWrite.txPacket();
-		if (result !== COMM_SUCCESS) {
-			throw new Error(`Sync Write Speed txPacket failed: ${packetHandler.getTxRxResult(result)}`);
-		}
-		return "success";
-	} catch (err) {
-		console.error("Exception during syncWriteWheelSpeed:", err);
-		// Re-throw the original error or a new one wrapping it
-		throw new Error(`Sync Write Speed failed: ${err.message}`);
-	}
-}
-
-/**
- * Sets the Baud Rate of a servo.
- * NOTE: After changing the baud rate, you might need to disconnect and reconnect
- *       at the new baud rate to communicate with the servo further.
- * @param {number} servoId - The current ID of the servo to configure (1-252).
- * @param {number} baudRateIndex - The index representing the new baud rate (0-7).
- * @returns {Promise<"success">} Resolves with "success".
- * @throws {Error} If not connected, input is invalid, any step fails, or an exception occurs.
- */
-export async function setBaudRate(servoId, baudRateIndex) {
-	checkConnection();
-
-	// Validate inputs
-	if (servoId < 1 || servoId > 252) {
-		throw new Error(`Invalid servo ID provided: ${servoId}. Must be between 1 and 252.`);
-	}
-	if (baudRateIndex < 0 || baudRateIndex > 7) {
-		throw new Error(`Invalid baudRateIndex: ${baudRateIndex}. Must be between 0 and 7.`);
-	}
-
-	let unlocked = false;
-	try {
-		debugLog(`Setting baud rate for servo ${servoId}: Index=${baudRateIndex}`);
-
-		// 1. Unlock servo configuration
-		const [resUnlock, errUnlock] = await packetHandler.write1ByteTxRx(
-			portHandler,
-			servoId,
-			ADDR_SCS_LOCK,
-			0 // 0 to unlock
-		);
-		if (resUnlock !== COMM_SUCCESS) {
-			throw new Error(
-				`Failed to unlock servo ${servoId}: ${packetHandler.getTxRxResult(
-					resUnlock
-				)}, Error: ${errUnlock}`
-			);
-		}
-		unlocked = true;
-
-		// 2. Write new Baud Rate index
-		const [resBaud, errBaud] = await packetHandler.write1ByteTxRx(
-			portHandler,
-			servoId,
-			ADDR_SCS_BAUD_RATE,
-			baudRateIndex
-		);
-		if (resBaud !== COMM_SUCCESS) {
-			throw new Error(
-				`Failed to write baud rate index ${baudRateIndex} to servo ${servoId}: ${packetHandler.getTxRxResult(
-					resBaud
-				)}, Error: ${errBaud}`
-			);
-		}
-
-		// 3. Lock servo configuration
-		const [resLock, errLock] = await packetHandler.write1ByteTxRx(
-			portHandler,
-			servoId,
-			ADDR_SCS_LOCK,
-			1
-		);
-		if (resLock !== COMM_SUCCESS) {
-			throw new Error(
-				`Failed to lock servo ${servoId} after setting baud rate: ${packetHandler.getTxRxResult(
-					resLock
-				)}, Error: ${errLock}.`
-			);
-		}
-		unlocked = false; // Successfully locked
-
-		debugLog(
-			`Successfully set baud rate for servo ${servoId}. Index: ${baudRateIndex}. Remember to potentially reconnect with the new baud rate.`
-		);
-		return "success";
-	} catch (err) {
-		console.error(`Exception during setBaudRate for servo ID ${servoId}:`, err);
-		if (unlocked) {
-			await tryLockServo(servoId);
-		}
-		throw new Error(`Failed to set baud rate for servo ${servoId}: ${err.message}`);
-	}
-}
-
-/**
- * Sets the ID of a servo.
- * NOTE: Changing the ID requires using the new ID for subsequent commands.
- * @param {number} currentServoId - The current ID of the servo to configure (1-252).
- * @param {number} newServoId - The new ID to set for the servo (1-252).
- * @returns {Promise<"success">} Resolves with "success".
- * @throws {Error} If not connected, input is invalid, any step fails, or an exception occurs.
- */
-export async function setServoId(currentServoId, newServoId) {
-	checkConnection();
-
-	// Validate inputs
-	if (currentServoId < 1 || currentServoId > 252 || newServoId < 1 || newServoId > 252) {
-		throw new Error(
-			`Invalid servo ID provided. Current: ${currentServoId}, New: ${newServoId}. Must be between 1 and 252.`
-		);
-	}
-
-	if (currentServoId === newServoId) {
-		debugLog(`Servo ID is already ${newServoId}. No change needed.`);
-		return "success";
-	}
-
-	let unlocked = false;
-	let idWritten = false;
-	try {
-		debugLog(`Setting servo ID: From ${currentServoId} to ${newServoId}`);
-
-		// 1. Unlock servo configuration (using current ID)
-		const [resUnlock, errUnlock] = await packetHandler.write1ByteTxRx(
-			portHandler,
-			currentServoId,
-			ADDR_SCS_LOCK,
-			0 // 0 to unlock
-		);
-		if (resUnlock !== COMM_SUCCESS) {
-			throw new Error(
-				`Failed to unlock servo ${currentServoId}: ${packetHandler.getTxRxResult(
-					resUnlock
-				)}, Error: ${errUnlock}`
-			);
-		}
-		unlocked = true;
-
-		// 2. Write new Servo ID (using current ID)
-		const [resId, errId] = await packetHandler.write1ByteTxRx(
-			portHandler,
-			currentServoId,
-			ADDR_SCS_ID,
-			newServoId
-		);
-		if (resId !== COMM_SUCCESS) {
-			throw new Error(
-				`Failed to write new ID ${newServoId} to servo ${currentServoId}: ${packetHandler.getTxRxResult(
-					resId
-				)}, Error: ${errId}`
-			);
-		}
-		idWritten = true;
-
-		// 3. Lock servo configuration (using NEW ID)
-		const [resLock, errLock] = await packetHandler.write1ByteTxRx(
-			portHandler,
-			newServoId, // Use NEW ID here
-			ADDR_SCS_LOCK,
-			1 // 1 to lock
-		);
-		if (resLock !== COMM_SUCCESS) {
-			// ID was likely changed, but lock failed. Critical state.
-			throw new Error(
-				`Failed to lock servo with new ID ${newServoId}: ${packetHandler.getTxRxResult(
-					resLock
-				)}, Error: ${errLock}. Configuration might be incomplete.`
-			);
-		}
-		unlocked = false; // Successfully locked with new ID
-
-		debugLog(
-			`Successfully set servo ID from ${currentServoId} to ${newServoId}. Remember to use the new ID for future commands.`
-		);
-		return "success";
-	} catch (err) {
-		console.error(`Exception during setServoId for current ID ${currentServoId}:`, err);
-		if (unlocked) {
-			// If unlock succeeded but subsequent steps failed, attempt to re-lock.
-			// If ID write failed, use current ID. If ID write succeeded but lock failed, use new ID.
-			const idToLock = idWritten ? newServoId : currentServoId;
-			console.warn(`Attempting to re-lock servo using ID ${idToLock}...`);
-			await tryLockServo(idToLock);
-		}
-		throw new Error(
-			`Failed to set servo ID from ${currentServoId} to ${newServoId}: ${err.message}`
-		);
-	}
-}
-
-// =============================================================================
-// LEGACY COMPATIBILITY FUNCTIONS (for backward compatibility)
-// =============================================================================
-
-/**
- * Sets a servo to wheel mode (continuous rotation) with unlocking.
- * @param {number} servoId - The ID of the servo (1-252).
- * @returns {Promise<"success">} Resolves with "success".
- * @throws {Error} If not connected, any step fails, or an exception occurs.
- */
-export async function setWheelMode(servoId) {
-	checkConnection();
-	let unlocked = false;
-	try {
-		debugLog(`Setting servo ${servoId} to wheel mode...`);
-
-		// 1. Unlock servo configuration
-		const [resUnlock, errUnlock] = await packetHandler.write1ByteTxRx(portHandler, servoId, ADDR_SCS_LOCK, 0);
-		if (resUnlock !== COMM_SUCCESS) {
-			throw new Error(`Failed to unlock servo ${servoId}: ${packetHandler.getTxRxResult(resUnlock)}, Error: ${errUnlock}`);
-		}
-		unlocked = true;
-
-		// 2. Set mode to 1 (Wheel/Speed mode)
-		const [resMode, errMode] = await packetHandler.write1ByteTxRx(portHandler, servoId, ADDR_SCS_MODE, 1);
-		if (resMode !== COMM_SUCCESS) {
-			throw new Error(`Failed to set wheel mode for servo ${servoId}: ${packetHandler.getTxRxResult(resMode)}, Error: ${errMode}`);
-		}
-
-		// 3. Lock servo configuration
-		const [resLock, errLock] = await packetHandler.write1ByteTxRx(portHandler, servoId, ADDR_SCS_LOCK, 1);
-		if (resLock !== COMM_SUCCESS) {
-			throw new Error(`Failed to lock servo ${servoId} after setting mode: ${packetHandler.getTxRxResult(resLock)}, Error: ${errLock}`);
-		}
-		unlocked = false;
-
-		debugLog(`Successfully set servo ${servoId} to wheel mode.`);
-		return "success";
-	} catch (err) {
-		console.error(`Exception setting wheel mode for servo ${servoId}:`, err);
-		if (unlocked) {
-			await tryLockServo(servoId);
-		}
-		throw new Error(`Failed to set wheel mode for servo ${servoId}: ${err.message}`);
-	}
-}
-
-/**
- * Sets a servo back to position control mode from wheel mode.
- * @param {number} servoId - The ID of the servo (1-252).
- * @returns {Promise<"success">} Resolves with "success".
- * @throws {Error} If not connected, any step fails, or an exception occurs.
- */
-export async function setPositionMode(servoId) {
-	checkConnection();
-	let unlocked = false;
-	try {
-		debugLog(`Setting servo ${servoId} back to position mode...`);
-
-		// 1. Unlock servo configuration
-		const [resUnlock, errUnlock] = await packetHandler.write1ByteTxRx(portHandler, servoId, ADDR_SCS_LOCK, 0);
-		if (resUnlock !== COMM_SUCCESS) {
-			throw new Error(`Failed to unlock servo ${servoId}: ${packetHandler.getTxRxResult(resUnlock)}, Error: ${errUnlock}`);
-		}
-		unlocked = true;
-
-		// 2. Set mode to 0 (Position/Servo mode)
-		const [resMode, errMode] = await packetHandler.write1ByteTxRx(portHandler, servoId, ADDR_SCS_MODE, 0);
-		if (resMode !== COMM_SUCCESS) {
-			throw new Error(`Failed to set position mode for servo ${servoId}: ${packetHandler.getTxRxResult(resMode)}, Error: ${errMode}`);
-		}
-
-		// 3. Lock servo configuration
-		const [resLock, errLock] = await packetHandler.write1ByteTxRx(portHandler, servoId, ADDR_SCS_LOCK, 1);
-		if (resLock !== COMM_SUCCESS) {
-			throw new Error(`Failed to lock servo ${servoId} after setting mode: ${packetHandler.getTxRxResult(resLock)}, Error: ${errLock}`);
-		}
-		unlocked = false;
-
-		debugLog(`Successfully set servo ${servoId} back to position mode.`);
-		return "success";
-	} catch (err) {
-		console.error(`Exception setting position mode for servo ${servoId}:`, err);
-		if (unlocked) {
-			await tryLockServo(servoId);
-		}
-		throw new Error(`Failed to set position mode for servo ${servoId}: ${err.message}`);
-	}
-}
+// For backward compatibility: keep a singleton instance
+export const scsServoSDK = new ScsServoSDK();

packages/feetech.js/scsservo_constants.mjs

@@ -1,8 +1,8 @@
 // Constants for FeetTech SCS servos
 
 // Constants
-export const BROADCAST_ID = 0xfe; // 254
-export const MAX_ID = 0xfc; // 252
+export const BROADCAST_ID = 0xFE;  // 254
+export const MAX_ID = 0xFC;  // 252
 
 // Protocol instructions
 export const INST_PING = 1;
@@ -10,19 +10,19 @@ export const INST_READ = 2;
 export const INST_WRITE = 3;
 export const INST_REG_WRITE = 4;
 export const INST_ACTION = 5;
-export const INST_SYNC_WRITE = 131; // 0x83
-export const INST_SYNC_READ = 130; // 0x82
-export const INST_STATUS = 85; // 0x55, status packet instruction (0x55)
+export const INST_SYNC_WRITE = 131;  // 0x83
+export const INST_SYNC_READ = 130;  // 0x82
+export const INST_STATUS = 85;  // 0x55, status packet instruction (0x55)
 
 // Communication results
-export const COMM_SUCCESS = 0; // tx or rx packet communication success
-export const COMM_PORT_BUSY = -1; // Port is busy (in use)
-export const COMM_TX_FAIL = -2; // Failed transmit instruction packet
-export const COMM_RX_FAIL = -3; // Failed get status packet
-export const COMM_TX_ERROR = -4; // Incorrect instruction packet
-export const COMM_RX_WAITING = -5; // Now receiving status packet
-export const COMM_RX_TIMEOUT = -6; // There is no status packet
-export const COMM_RX_CORRUPT = -7; // Incorrect status packet
+export const COMM_SUCCESS = 0;      // tx or rx packet communication success
+export const COMM_PORT_BUSY = -1;   // Port is busy (in use)
+export const COMM_TX_FAIL = -2;     // Failed transmit instruction packet
+export const COMM_RX_FAIL = -3;     // Failed get status packet
+export const COMM_TX_ERROR = -4;    // Incorrect instruction packet
+export const COMM_RX_WAITING = -5;  // Now receiving status packet
+export const COMM_RX_TIMEOUT = -6;  // There is no status packet
+export const COMM_RX_CORRUPT = -7;  // Incorrect status packet
 export const COMM_NOT_AVAILABLE = -9;
 
 // Packet constants
@@ -50,4 +50,4 @@ export const ADDR_SCS_TORQUE_ENABLE = 40;
 export const ADDR_SCS_GOAL_ACC = 41;
 export const ADDR_SCS_GOAL_POSITION = 42;
 export const ADDR_SCS_GOAL_SPEED = 46;
-export const ADDR_SCS_PRESENT_POSITION = 56;
+export const ADDR_SCS_PRESENT_POSITION = 56;

packages/feetech.js/test.html

@@ -1,770 +1,620 @@
-<!doctype html>
+<!DOCTYPE html>
 <html lang="en">
-	<head>
-		<meta charset="UTF-8" />
-		<meta name="viewport" content="width=device-width, initial-scale=1.0" />
-		<title>Feetech Servo Test</title>
-		<style>
-			body {
-				font-family: sans-serif;
-				line-height: 1.6;
-				padding: 20px;
-			}
-			.container {
-				max-width: 800px;
-				margin: auto;
-			}
-			.section {
-				border: 1px solid #ccc;
-				padding: 15px;
-				margin-bottom: 20px;
-				border-radius: 5px;
-			}
-			h2 {
-				margin-top: 0;
-			}
-			label {
-				display: inline-block;
-				min-width: 100px;
-				margin-bottom: 5px;
-			}
-			input[type="number"],
-			input[type="text"] {
-				width: 100px;
-				padding: 5px;
-				margin-right: 10px;
-				margin-bottom: 10px;
-			}
-			button {
-				padding: 8px 15px;
-				margin-right: 10px;
-				cursor: pointer;
-			}
-			pre {
-				background-color: #f4f4f4;
-				padding: 10px;
-				border: 1px solid #ddd;
-				border-radius: 3px;
-				white-space: pre-wrap;
-				word-wrap: break-word;
-			}
-			.status {
-				font-weight: bold;
-			}
-			.success {
-				color: green;
-			}
-			.error {
-				color: red;
-			}
-			.log-area {
-				margin-top: 10px;
-			}
-		</style>
-	</head>
-	<body>
-		<div class="container">
-			<h1>Feetech Servo Test Page</h1>
-
-			<details class="section">
-				<summary>Key Concepts</summary>
-				<p>Understanding these parameters is crucial for controlling Feetech servos:</p>
-				<ul>
-					<li>
-						<strong>Mode:</strong> Determines the servo's primary function.
-						<ul>
-							<li>
-								<code>Mode 0</code>: Position/Servo Mode. The servo moves to and holds a specific
-								angular position.
-							</li>
-							<li>
-								<code>Mode 1</code>: Wheel/Speed Mode. The servo rotates continuously at a specified
-								speed and direction, like a motor.
-							</li>
-						</ul>
-						Changing the mode requires unlocking, writing the mode value (0 or 1), and locking the
-						configuration.
-					</li>
-					<li>
-						<strong>Position:</strong> In Position Mode (Mode 0), this value represents the target
-						or current angular position of the servo's output shaft.
-						<ul>
-							<li>
-								Range: Typically <code>0</code> to <code>4095</code> (representing a 12-bit
-								resolution).
-							</li>
-							<li>
-								Meaning: Corresponds to the servo's rotational range (e.g., 0-360 degrees or 0-270
-								degrees, depending on the specific servo model). <code>0</code> is one end of the
-								range, <code>4095</code> is the other.
-							</li>
-						</ul>
-					</li>
-					<li>
-						<strong>Speed (Wheel Mode):</strong> In Wheel Mode (Mode 1), this value controls the
-						rotational speed and direction.
-						<ul>
-							<li>
-								Range: Typically <code>-2500</code> to <code>+2500</code>. (Note: Some documentation
-								might mention -1023 to +1023, but the SDK example uses a wider range).
-							</li>
-							<li>
-								Meaning: <code>0</code> stops the wheel. Positive values rotate in one direction
-								(e.g., clockwise), negative values rotate in the opposite direction (e.g.,
-								counter-clockwise). The magnitude determines the speed (larger absolute value means
-								faster rotation).
-							</li>
-							<li>Control Address: <code>ADDR_SCS_GOAL_SPEED</code> (Register 46/47).</li>
-						</ul>
-					</li>
-					<li>
-						<strong>Acceleration:</strong> Controls how quickly the servo changes speed to reach its
-						target position (in Position Mode) or target speed (in Wheel Mode).
-						<ul>
-							<li>Range: Typically <code>0</code> to <code>254</code>.</li>
-							<li>
-								Meaning: Defines the rate of change of speed. The unit is 100 steps/s².
-								<code>0</code> usually means instantaneous acceleration (or minimal delay). Higher
-								values result in slower, smoother acceleration and deceleration. For example, a
-								value of <code>10</code> means the speed changes by 10 * 100 = 1000 steps per
-								second, per second. This helps reduce jerky movements and mechanical stress.
-							</li>
-							<li>Control Address: <code>ADDR_SCS_GOAL_ACC</code> (Register 41).</li>
-						</ul>
-					</li>
-					<li>
-						<strong>Baud Rate:</strong> The speed of communication between the controller and the
-						servo. It must match on both ends. Servos often support multiple baud rates, selectable
-						via an index:
-						<ul>
-							<li>Index 0: 1,000,000 bps</li>
-							<li>Index 1: 500,000 bps</li>
-							<li>Index 2: 250,000 bps</li>
-							<li>Index 3: 128,000 bps</li>
-							<li>Index 4: 115,200 bps</li>
-							<li>Index 5: 76,800 bps</li>
-							<li>Index 6: 57,600 bps</li>
-							<li>Index 7: 38,400 bps</li>
-						</ul>
-					</li>
-				</ul>
-			</details>
-
-			<div class="section">
-				<h2>Connection</h2>
-				<button id="connectBtn">Connect</button>
-				<button id="disconnectBtn">Disconnect</button>
-				<p>Status: <span id="connectionStatus" class="status error">Disconnected</span></p>
-				<label for="baudRate">Baud Rate:</label>
-				<input type="number" id="baudRate" value="1000000" />
-				<label for="protocolEnd">Protocol End (0=STS/SMS, 1=SCS):</label>
-				<input type="number" id="protocolEnd" value="0" min="0" max="1" />
-			</div>
-
-			<div class="section">
-				<h2>Scan Servos</h2>
-				<label for="scanStartId">Start ID:</label>
-				<input type="number" id="scanStartId" value="1" min="1" max="252" />
-				<label for="scanEndId">End ID:</label>
-				<input type="number" id="scanEndId" value="15" min="1" max="252" />
-				<button id="scanServosBtn">Scan</button>
-				<p>Scan Results:</p>
-				<pre id="scanResultsOutput" style="max-height: 200px; overflow-y: auto"></pre>
-				<!-- Added element for results -->
-			</div>
-
-			<div class="section">
-				<h2>Single Servo Control</h2>
-				<label for="servoId">Servo ID:</label>
-				<input type="number" id="servoId" value="1" min="1" max="252" /><br />
-
-				<label for="idWrite">Change servo ID:</label>
-				<input type="number" id="idWrite" value="1" min="1" max="252" />
-				<button id="writeIdBtn">Write</button><br />
-
-				<label for="baudRead">Read Baud Rate:</label>
-				<button id="readBaudBtn">Read</button>
-				<span id="readBaudResult"></span><br />
-
-				<label for="baudWrite">Write Baud Rate Index:</label>
-				<input type="number" id="baudWrite" value="6" min="0" max="7" />
-				<!-- Assuming index 0-7 -->
-				<button id="writeBaudBtn">Write</button><br />
-
-				<label for="positionRead">Read Position:</label>
-				<button id="readPosBtn">Read</button>
-				<span id="readPosResult"></span><br />
-
-				<label for="positionWrite">Write Position:</label>
-				<input type="number" id="positionWrite" value="1000" min="0" max="4095" />
-				<button id="writePosBtn">Write</button><br />
-
-				<label for="torqueEnable">Torque:</label>
-				<button id="torqueEnableBtn">Enable</button>
-				<button id="torqueDisableBtn">Disable</button><br />
-
-				<label for="accelerationWrite">Write Acceleration:</label>
-				<input type="number" id="accelerationWrite" value="50" min="0" max="254" />
-				<button id="writeAccBtn">Write</button><br />
-
-				<label for="wheelMode">Wheel Mode:</label>
-				<button id="setWheelModeBtn">Set Wheel Mode</button>
-				<button id="removeWheelModeBtn">Set Position Mode</button><br />
-
-				<label for="wheelSpeedWrite">Write Wheel Speed:</label>
-				<input type="number" id="wheelSpeedWrite" value="0" min="-2500" max="2500" />
-				<button id="writeWheelSpeedBtn">Write Speed</button>
-			</div>
-
-			<div class="section">
-				<h2>Sync Operations</h2>
-				<label for="syncReadIds">Sync Read IDs (csv):</label>
-				<input type="text" id="syncReadIds" value="1,2,3" style="width: 150px" />
-				<button id="syncReadBtn">Sync Read Positions</button><br />
-
-				<label for="syncWriteData">Sync Write (id:pos,...):</label>
-				<input type="text" id="syncWriteData" value="1:1500,2:2500" style="width: 200px" />
-				<button id="syncWriteBtn">Sync Write Positions</button><br />
-
-				<label for="syncWriteSpeedData">Sync Write Speed (id:speed,...):</label>
-				<input type="text" id="syncWriteSpeedData" value="1:500,2:-1000" style="width: 200px" />
-				<button id="syncWriteSpeedBtn">Sync Write Speeds</button>
-				<!-- New Button -->
-			</div>
-
-			<div class="section">
-				<h2>Log Output</h2>
-				<pre id="logOutput"></pre>
-			</div>
-		</div>
-
-		<script type="module">
-			// Import the scsServoSDK object from index.mjs
-			import { scsServoSDK } from "./index.mjs";
-			// No longer need COMM_SUCCESS etc. here as errors are thrown
-
-			const connectBtn = document.getElementById("connectBtn");
-			const disconnectBtn = document.getElementById("disconnectBtn");
-			const connectionStatus = document.getElementById("connectionStatus");
-			const baudRateInput = document.getElementById("baudRate");
-			const protocolEndInput = document.getElementById("protocolEnd");
-
-			const servoIdInput = document.getElementById("servoId");
-			const readIdBtn = document.getElementById("readIdBtn"); // New
-			const readIdResult = document.getElementById("readIdResult"); // New
-			const idWriteInput = document.getElementById("idWrite"); // New
-			const writeIdBtn = document.getElementById("writeIdBtn"); // New
-			const readBaudBtn = document.getElementById("readBaudBtn"); // New
-			const readBaudResult = document.getElementById("readBaudResult"); // New
-			const baudWriteInput = document.getElementById("baudWrite"); // New
-			const writeBaudBtn = document.getElementById("writeBaudBtn"); // New
-			const readPosBtn = document.getElementById("readPosBtn");
-			const readPosResult = document.getElementById("readPosResult");
-			const positionWriteInput = document.getElementById("positionWrite");
-			const writePosBtn = document.getElementById("writePosBtn");
-			const torqueEnableBtn = document.getElementById("torqueEnableBtn");
-			const torqueDisableBtn = document.getElementById("torqueDisableBtn");
-			const accelerationWriteInput = document.getElementById("accelerationWrite");
-			const writeAccBtn = document.getElementById("writeAccBtn");
-			const setWheelModeBtn = document.getElementById("setWheelModeBtn");
-			const removeWheelModeBtn = document.getElementById("removeWheelModeBtn"); // Get reference to the new button
-			const wheelSpeedWriteInput = document.getElementById("wheelSpeedWrite");
-			const writeWheelSpeedBtn = document.getElementById("writeWheelSpeedBtn");
-
-			const syncReadIdsInput = document.getElementById("syncReadIds");
-			const syncReadBtn = document.getElementById("syncReadBtn");
-			const syncWriteDataInput = document.getElementById("syncWriteData");
-			const syncWriteBtn = document.getElementById("syncWriteBtn");
-			const syncWriteSpeedDataInput = document.getElementById("syncWriteSpeedData"); // New Input
-			const syncWriteSpeedBtn = document.getElementById("syncWriteSpeedBtn"); // New Button
-			const scanServosBtn = document.getElementById("scanServosBtn"); // Get reference to the scan button
-			const scanStartIdInput = document.getElementById("scanStartId"); // Get reference to start ID input
-			const scanEndIdInput = document.getElementById("scanEndId"); // Get reference to end ID input
-			const scanResultsOutput = document.getElementById("scanResultsOutput"); // Get reference to the new results area
-
-			const logOutput = document.getElementById("logOutput");
-
-			let isConnected = false;
-
-			function log(message) {
-				console.log(message);
-				const timestamp = new Date().toLocaleTimeString();
-				logOutput.textContent = `[${timestamp}] ${message}\n` + logOutput.textContent;
-				// Limit log size
-				const lines = logOutput.textContent.split("\n"); // Use '\n' instead of literal newline
-				if (lines.length > 50) {
-					logOutput.textContent = lines.slice(0, 50).join("\n"); // Use '\n' instead of literal newline
-				}
-			}
-
-			function updateConnectionStatus(connected, message) {
-				isConnected = connected;
-				connectionStatus.textContent = message || (connected ? "Connected" : "Disconnected");
-				connectionStatus.className = `status ${connected ? "success" : "error"}`;
-				log(`Connection status: ${connectionStatus.textContent}`);
-			}
-
-			connectBtn.onclick = async () => {
-				log("Attempting to connect...");
-				try {
-					const baudRate = parseInt(baudRateInput.value, 10);
-					const protocolEnd = parseInt(protocolEndInput.value, 10);
-					// Use scsServoSDK - throws on error
-					await scsServoSDK.connect({ baudRate, protocolEnd });
-					updateConnectionStatus(true, "Connected");
-				} catch (err) {
-					updateConnectionStatus(false, `Connection error: ${err.message}`);
-					console.error(err);
-				}
-			};
-
-			disconnectBtn.onclick = async () => {
-				log("Attempting to disconnect...");
-				try {
-					// Use scsServoSDK - throws on error
-					await scsServoSDK.disconnect();
-					updateConnectionStatus(false, "Disconnected"); // Success means disconnected
-				} catch (err) {
-					// Assuming disconnect might fail if already disconnected or other issues
-					updateConnectionStatus(false, `Disconnection error: ${err.message}`);
-					console.error(err);
-				}
-			};
-
-			writeIdBtn.onclick = async () => {
-				// New handler
-				if (!isConnected) {
-					log("Error: Not connected");
-					return;
-				}
-				const currentId = parseInt(servoIdInput.value, 10);
-				const newId = parseInt(idWriteInput.value, 10);
-				if (isNaN(newId) || newId < 1 || newId > 252) {
-					log(`Error: Invalid new ID ${newId}. Must be between 1 and 252.`);
-					return;
-				}
-				log(`Writing new ID ${newId} to servo ${currentId}...`);
-				try {
-					// Use scsServoSDK - throws on error
-					await scsServoSDK.setServoId(currentId, newId);
-					log(`Successfully wrote new ID ${newId} to servo (was ${currentId}).`);
-					// IMPORTANT: Update the main ID input to reflect the change
-					servoIdInput.value = newId;
-					log(`Servo ID input field updated to ${newId}.`);
-				} catch (err) {
-					log(`Error writing ID for servo ${currentId}: ${err.message}`);
-					console.error(err);
-				}
-			};
-
-			readBaudBtn.onclick = async () => {
-				// New handler
-				if (!isConnected) {
-					log("Error: Not connected");
-					return;
-				}
-				const id = parseInt(servoIdInput.value, 10);
-				log(`Reading Baud Rate Index for servo ${id}...`);
-				readBaudResult.textContent = "Reading...";
-				try {
-					// Use scsServoSDK - returns value directly or throws
-					const baudRateIndex = await scsServoSDK.readBaudRate(id);
-					readBaudResult.textContent = `Baud Index: ${baudRateIndex}`;
-					log(`Servo ${id} Baud Rate Index: ${baudRateIndex}`);
-				} catch (err) {
-					readBaudResult.textContent = `Error: ${err.message}`;
-					log(`Error reading Baud Rate Index for servo ${id}: ${err.message}`);
-					console.error(err);
-				}
-			};
-
-			writeBaudBtn.onclick = async () => {
-				// New handler
-				if (!isConnected) {
-					log("Error: Not connected");
-					return;
-				}
-				const id = parseInt(servoIdInput.value, 10);
-				const newBaudIndex = parseInt(baudWriteInput.value, 10);
-				if (isNaN(newBaudIndex) || newBaudIndex < 0 || newBaudIndex > 7) {
-					// Adjust max index if needed
-					log(`Error: Invalid new Baud Rate Index ${newBaudIndex}. Check valid range.`);
-					return;
-				}
-				log(`Writing new Baud Rate Index ${newBaudIndex} to servo ${id}...`);
-				try {
-					// Use scsServoSDK - throws on error
-					await scsServoSDK.setBaudRate(id, newBaudIndex);
-					log(`Successfully wrote new Baud Rate Index ${newBaudIndex} to servo ${id}.`);
-					log(
-						`IMPORTANT: You may need to disconnect and reconnect with the new baud rate if it differs from the current connection baud rate.`
-					);
-				} catch (err) {
-					log(`Error writing Baud Rate Index for servo ${id}: ${err.message}`);
-					console.error(err);
-				}
-			};
-
-			readPosBtn.onclick = async () => {
-				if (!isConnected) {
-					log("Error: Not connected");
-					return;
-				}
-				const id = parseInt(servoIdInput.value, 10);
-				log(`Reading position for servo ${id}...`);
-				readPosResult.textContent = "Reading...";
-				try {
-					// Use scsServoSDK - returns value directly or throws
-					const position = await scsServoSDK.readPosition(id);
-					readPosResult.textContent = `Position: ${position}`;
-					log(`Servo ${id} position: ${position}`);
-				} catch (err) {
-					readPosResult.textContent = `Error: ${err.message}`;
-					log(`Error reading position for servo ${id}: ${err.message}`);
-					console.error(err);
-				}
-			};
-
-			writePosBtn.onclick = async () => {
-				if (!isConnected) {
-					log("Error: Not connected");
-					return;
-				}
-				const id = parseInt(servoIdInput.value, 10);
-				const pos = parseInt(positionWriteInput.value, 10);
-				log(`Writing position ${pos} to servo ${id}...`);
-				try {
-					// Use scsServoSDK - throws on error
-					await scsServoSDK.writePosition(id, pos);
-					log(`Successfully wrote position ${pos} to servo ${id}.`);
-				} catch (err) {
-					log(`Error writing position for servo ${id}: ${err.message}`);
-					console.error(err);
-				}
-			};
-
-			torqueEnableBtn.onclick = async () => {
-				if (!isConnected) {
-					log("Error: Not connected");
-					return;
-				}
-				const id = parseInt(servoIdInput.value, 10);
-				log(`Enabling torque for servo ${id}...`);
-				try {
-					// Use scsServoSDK - throws on error
-					await scsServoSDK.writeTorqueEnable(id, true);
-					log(`Successfully enabled torque for servo ${id}.`);
-				} catch (err) {
-					log(`Error enabling torque for servo ${id}: ${err.message}`);
-					console.error(err);
-				}
-			};
-
-			torqueDisableBtn.onclick = async () => {
-				if (!isConnected) {
-					log("Error: Not connected");
-					return;
-				}
-				const id = parseInt(servoIdInput.value, 10);
-				log(`Disabling torque for servo ${id}...`);
-				try {
-					// Use scsServoSDK - throws on error
-					await scsServoSDK.writeTorqueEnable(id, false);
-					log(`Successfully disabled torque for servo ${id}.`);
-				} catch (err) {
-					log(`Error disabling torque for servo ${id}: ${err.message}`);
-					console.error(err);
-				}
-			};
-
-			writeAccBtn.onclick = async () => {
-				if (!isConnected) {
-					log("Error: Not connected");
-					return;
-				}
-				const id = parseInt(servoIdInput.value, 10);
-				const acc = parseInt(accelerationWriteInput.value, 10);
-				log(`Writing acceleration ${acc} to servo ${id}...`);
-				try {
-					// Use scsServoSDK - throws on error
-					await scsServoSDK.writeAcceleration(id, acc);
-					log(`Successfully wrote acceleration ${acc} to servo ${id}.`);
-				} catch (err) {
-					log(`Error writing acceleration for servo ${id}: ${err.message}`);
-					console.error(err);
-				}
-			};
-
-			setWheelModeBtn.onclick = async () => {
-				if (!isConnected) {
-					log("Error: Not connected");
-					return;
-				}
-				const id = parseInt(servoIdInput.value, 10);
-				log(`Setting servo ${id} to wheel mode...`);
-				try {
-					// Use scsServoSDK - throws on error
-					await scsServoSDK.setWheelMode(id);
-					log(`Successfully set servo ${id} to wheel mode.`);
-				} catch (err) {
-					log(`Error setting wheel mode for servo ${id}: ${err.message}`);
-					console.error(err);
-				}
-			};
-
-			// Add event listener for the new button
-			removeWheelModeBtn.onclick = async () => {
-				if (!isConnected) {
-					log("Error: Not connected");
-					return;
-				}
-				const id = parseInt(servoIdInput.value, 10);
-				log(`Setting servo ${id} back to position mode...`);
-				try {
-					// Use scsServoSDK - throws on error
-					await scsServoSDK.setPositionMode(id);
-					log(`Successfully set servo ${id} back to position mode.`);
-				} catch (err) {
-					log(`Error setting position mode for servo ${id}: ${err.message}`);
-					console.error(err);
-				}
-			};
-
-			writeWheelSpeedBtn.onclick = async () => {
-				if (!isConnected) {
-					log("Error: Not connected");
-					return;
-				}
-				const id = parseInt(servoIdInput.value, 10);
-				const speed = parseInt(wheelSpeedWriteInput.value, 10);
-				log(`Writing wheel speed ${speed} to servo ${id}...`);
-				try {
-					// Use scsServoSDK - throws on error
-					await scsServoSDK.writeWheelSpeed(id, speed);
-					log(`Successfully wrote wheel speed ${speed} to servo ${id}.`);
-				} catch (err) {
-					log(`Error writing wheel speed for servo ${id}: ${err.message}`);
-					console.error(err);
-				}
-			};
-
-			syncReadBtn.onclick = async () => {
-				if (!isConnected) {
-					log("Error: Not connected");
-					return;
-				}
-				const idsString = syncReadIdsInput.value;
-				const ids = idsString
-					.split(",")
-					.map((s) => parseInt(s.trim(), 10))
-					.filter((id) => !isNaN(id) && id > 0 && id < 253);
-				if (ids.length === 0) {
-					log("Sync Read: No valid servo IDs provided.");
-					return;
-				}
-				log(`Sync reading positions for servos: ${ids.join(", ")}...`);
-				try {
-					// Use scsServoSDK - returns Map or throws
-					const positions = await scsServoSDK.syncReadPositions(ids);
-					let logMsg = "Sync Read Successful:\n";
-					positions.forEach((pos, id) => {
-						logMsg += `  Servo ${id}: Position=${pos}\n`;
-					});
-					log(logMsg.trim());
-				} catch (err) {
-					log(`Sync Read Failed: ${err.message}`);
-					console.error(err);
-				}
-			};
-
-			syncWriteBtn.onclick = async () => {
-				if (!isConnected) {
-					log("Error: Not connected");
-					return;
-				}
-				const dataString = syncWriteDataInput.value;
-				const positionMap = new Map();
-				const pairs = dataString.split(",");
-				let validData = false;
-
-				pairs.forEach((pair) => {
-					const parts = pair.split(":");
-					if (parts.length === 2) {
-						const id = parseInt(parts[0].trim(), 10);
-						const pos = parseInt(parts[1].trim(), 10);
-						// Position validation (0-4095)
-						if (!isNaN(id) && id > 0 && id < 253 && !isNaN(pos) && pos >= 0 && pos <= 4095) {
-							positionMap.set(id, pos);
-							validData = true;
-						} else {
-							log(
-								`Sync Write Position: Invalid data pair "${pair}". ID (1-252), Pos (0-4095). Skipping.`
-							);
-						}
-					} else {
-						log(`Sync Write Position: Invalid format "${pair}". Skipping.`);
-					}
-				});
-
-				if (!validData) {
-					log("Sync Write Position: No valid servo position data provided.");
-					return;
-				}
-
-				log(
-					`Sync writing positions: ${Array.from(positionMap.entries())
-						.map(([id, pos]) => `${id}:${pos}`)
-						.join(", ")}...`
-				);
-				try {
-					// Use scsServoSDK - throws on error
-					await scsServoSDK.syncWritePositions(positionMap);
-					log(`Sync write position command sent successfully.`);
-				} catch (err) {
-					log(`Sync Write Position Failed: ${err.message}`);
-					console.error(err);
-				}
-			};
-
-			// New handler for Sync Write Speed
-			syncWriteSpeedBtn.onclick = async () => {
-				if (!isConnected) {
-					log("Error: Not connected");
-					return;
-				}
-				const dataString = syncWriteSpeedDataInput.value;
-				const speedMap = new Map();
-				const pairs = dataString.split(",");
-				let validData = false;
-
-				pairs.forEach((pair) => {
-					const parts = pair.split(":");
-					if (parts.length === 2) {
-						const id = parseInt(parts[0].trim(), 10);
-						const speed = parseInt(parts[1].trim(), 10);
-						// Speed validation (-10000 to 10000)
-						if (
-							!isNaN(id) &&
-							id > 0 &&
-							id < 253 &&
-							!isNaN(speed) &&
-							speed >= -10000 &&
-							speed <= 10000
-						) {
-							speedMap.set(id, speed);
-							validData = true;
-						} else {
-							log(
-								`Sync Write Speed: Invalid data pair "${pair}". ID (1-252), Speed (-10000 to 10000). Skipping.`
-							);
-						}
-					} else {
-						log(`Sync Write Speed: Invalid format "${pair}". Skipping.`);
-					}
-				});
-
-				if (!validData) {
-					log("Sync Write Speed: No valid servo speed data provided.");
-					return;
-				}
-
-				log(
-					`Sync writing speeds: ${Array.from(speedMap.entries())
-						.map(([id, speed]) => `${id}:${speed}`)
-						.join(", ")}...`
-				);
-				try {
-					// Use scsServoSDK - throws on error
-					await scsServoSDK.syncWriteWheelSpeed(speedMap);
-					log(`Sync write speed command sent successfully.`);
-				} catch (err) {
-					log(`Sync Write Speed Failed: ${err.message}`);
-					console.error(err);
-				}
-			};
-
-			scanServosBtn.onclick = async () => {
-				if (!isConnected) {
-					log("Error: Not connected");
-					return;
-				}
-
-				const startId = parseInt(scanStartIdInput.value, 10);
-				const endId = parseInt(scanEndIdInput.value, 10);
-
-				if (isNaN(startId) || isNaN(endId) || startId < 1 || endId > 252 || startId > endId) {
-					const errorMsg =
-						"Error: Invalid scan ID range. Please enter values between 1 and 252, with Start ID <= End ID.";
-					log(errorMsg);
-					scanResultsOutput.textContent = errorMsg; // Show error in results area too
-					return;
-				}
-
-				const startMsg = `Starting servo scan (IDs ${startId}-${endId})...`;
-				log(startMsg);
-				scanResultsOutput.textContent = startMsg + "\n"; // Clear and start results area
-				scanServosBtn.disabled = true; // Disable button during scan
-
-				let foundCount = 0;
-
-				for (let id = startId; id <= endId; id++) {
-					let resultMsg = `Scanning ID ${id}... `;
-					try {
-						// Attempt to read position. If it succeeds, the servo exists.
-						// If it throws, the servo likely doesn't exist or there's another issue.
-						const position = await scsServoSDK.readPosition(id);
-						foundCount++;
-
-						// Servo found, now try to read mode and baud rate
-						let mode = "ReadError";
-						let baudRateIndex = "ReadError";
-						try {
-							mode = await scsServoSDK.readMode(id);
-						} catch (modeErr) {
-							log(`  Servo ${id}: Error reading mode: ${modeErr.message}`);
-						}
-						try {
-							baudRateIndex = await scsServoSDK.readBaudRate(id);
-						} catch (baudErr) {
-							log(`  Servo ${id}: Error reading baud rate: ${baudErr.message}`);
-						}
-
-						resultMsg += `FOUND: Pos=${position}, Mode=${mode}, BaudIdx=${baudRateIndex}`;
-						log(
-							`  Servo ${id} FOUND: Position=${position}, Mode=${mode}, BaudIndex=${baudRateIndex}`
-						);
-					} catch (err) {
-						// Check if the error message indicates a timeout or non-response, which is expected for non-existent IDs
-						// This check might need refinement based on the exact error messages thrown by readPosition
-						if (
-							err.message.includes("timeout") ||
-							err.message.includes("No response") ||
-							err.message.includes("failed: RX")
-						) {
-							resultMsg += `No response`;
-							// log(`  Servo ${id}: No response`); // Optional: reduce log noise
-						} else {
-							// Log other unexpected errors
-							resultMsg += `Error: ${err.message}`;
-							log(`  Servo ${id}: Error during scan: ${err.message}`);
-							console.error(`Error scanning servo ${id}:`, err);
-						}
-					}
-					scanResultsOutput.textContent += resultMsg + "\n"; // Append result to the results area
-					scanResultsOutput.scrollTop = scanResultsOutput.scrollHeight; // Auto-scroll
-					// Optional small delay between scans if needed
-					// await new Promise(resolve => setTimeout(resolve, 10));
-				}
-
-				const finishMsg = `Servo scan finished. Found ${foundCount} servo(s).`;
-				log(finishMsg);
-				scanResultsOutput.textContent += finishMsg + "\n"; // Add finish message to results area
-				scanResultsOutput.scrollTop = scanResultsOutput.scrollHeight; // Auto-scroll
-				scanServosBtn.disabled = false; // Re-enable button
-			};
-
-			// Initial log
-			log("Test page loaded. Please connect to a servo controller.");
-</script>
-	</body>
+<head>
+    <meta charset="UTF-8">
+    <meta name="viewport" content="width=device-width, initial-scale=1.0">
+    <title>Feetech Servo Test</title>
+    <style>
+        body { font-family: sans-serif; line-height: 1.6; padding: 20px; }
+        .container { max-width: 800px; margin: auto; }
+        .section { border: 1px solid #ccc; padding: 15px; margin-bottom: 20px; border-radius: 5px; }
+        h2 { margin-top: 0; }
+        label { display: inline-block; min-width: 100px; margin-bottom: 5px; }
+        input[type="number"], input[type="text"] { width: 100px; padding: 5px; margin-right: 10px; margin-bottom: 10px; }
+        button { padding: 8px 15px; margin-right: 10px; cursor: pointer; }
+        pre { background-color: #f4f4f4; padding: 10px; border: 1px solid #ddd; border-radius: 3px; white-space: pre-wrap; word-wrap: break-word; }
+        .status { font-weight: bold; }
+        .success { color: green; }
+        .error { color: red; }
+        .log-area { margin-top: 10px; }
+    </style>
+</head>
+<body>
+    <div class="container">
+        <h1>Feetech Servo Test Page</h1>
+
+        <details class="section">
+            <summary>Key Concepts</summary>
+            <p>Understanding these parameters is crucial for controlling Feetech servos:</p>
+            <ul>
+                <li>
+                    <strong>Mode:</strong> Determines the servo's primary function.
+                    <ul>
+                        <li><code>Mode 0</code>: Position/Servo Mode. The servo moves to and holds a specific angular position.</li>
+                        <li><code>Mode 1</code>: Wheel/Speed Mode. The servo rotates continuously at a specified speed and direction, like a motor.</li>
+                    </ul>
+                     Changing the mode requires unlocking, writing the mode value (0 or 1), and locking the configuration.
+                </li>
+                <li>
+                    <strong>Position:</strong> In Position Mode (Mode 0), this value represents the target or current angular position of the servo's output shaft.
+                    <ul>
+                        <li>Range: Typically <code>0</code> to <code>4095</code> (representing a 12-bit resolution).</li>
+                        <li>Meaning: Corresponds to the servo's rotational range (e.g., 0-360 degrees or 0-270 degrees, depending on the specific servo model). <code>0</code> is one end of the range, <code>4095</code> is the other.</li>
+                    </ul>
+                </li>
+                <li>
+                    <strong>Speed (Wheel Mode):</strong> In Wheel Mode (Mode 1), this value controls the rotational speed and direction.
+                    <ul>
+                        <li>Range: Typically <code>-2500</code> to <code>+2500</code>. (Note: Some documentation might mention -1023 to +1023, but the SDK example uses a wider range).</li>
+                        <li>Meaning: <code>0</code> stops the wheel. Positive values rotate in one direction (e.g., clockwise), negative values rotate in the opposite direction (e.g., counter-clockwise). The magnitude determines the speed (larger absolute value means faster rotation).</li>
+                        <li>Control Address: <code>ADDR_SCS_GOAL_SPEED</code> (Register 46/47).</li>
+                    </ul>
+                </li>
+                 <li>
+                    <strong>Acceleration:</strong> Controls how quickly the servo changes speed to reach its target position (in Position Mode) or target speed (in Wheel Mode).
+                    <ul>
+                        <li>Range: Typically <code>0</code> to <code>254</code>.</li>
+                        <li>Meaning: Defines the rate of change of speed. The unit is 100 steps/s². <code>0</code> usually means instantaneous acceleration (or minimal delay). Higher values result in slower, smoother acceleration and deceleration. For example, a value of <code>10</code> means the speed changes by 10 * 100 = 1000 steps per second, per second. This helps reduce jerky movements and mechanical stress.</li>
+                        <li>Control Address: <code>ADDR_SCS_GOAL_ACC</code> (Register 41).</li>
+                    </ul>
+                </li>
+                <li>
+                    <strong>Baud Rate:</strong> The speed of communication between the controller and the servo. It must match on both ends. Servos often support multiple baud rates, selectable via an index:
+                    <ul>
+                        <li>Index 0: 1,000,000 bps</li>
+                        <li>Index 1: 500,000 bps</li>
+                        <li>Index 2: 250,000 bps</li>
+                        <li>Index 3: 128,000 bps</li>
+                        <li>Index 4: 115,200 bps</li>
+                        <li>Index 5: 76,800 bps</li>
+                        <li>Index 6: 57,600 bps</li>
+                        <li>Index 7: 38,400 bps</li>
+                    </ul>
+                </li>
+            </ul>
+        </details>
+
+
+        <div class="section">
+            <h2>Connection</h2>
+            <button id="connectBtn">Connect</button>
+            <button id="disconnectBtn">Disconnect</button>
+            <p>Status: <span id="connectionStatus" class="status error">Disconnected</span></p>
+            <label for="baudRate">Baud Rate:</label>
+            <input type="number" id="baudRate" value="1000000">
+            <label for="protocolEnd">Protocol End (0=STS/SMS, 1=SCS):</label>
+            <input type="number" id="protocolEnd" value="0" min="0" max="1">
+        </div>
+        
+        <div class="section">
+            <h2>Scan Servos</h2>
+            <label for="scanStartId">Start ID:</label>
+            <input type="number" id="scanStartId" value="1" min="1" max="252">
+            <label for="scanEndId">End ID:</label>
+            <input type="number" id="scanEndId" value="15" min="1" max="252">
+            <button id="scanServosBtn">Scan</button>
+            <p>Scan Results:</p>
+            <pre id="scanResultsOutput" style="max-height: 200px; overflow-y: auto;"></pre> <!-- Added element for results -->
+        </div>
+
+        <div class="section">
+            <h2>Single Servo Control</h2>
+            <label for="servoId">Servo ID:</label>
+            <input type="number" id="servoId" value="1" min="1" max="252"><br>
+
+
+            <label for="idWrite">Change servo ID:</label>
+            <input type="number" id="idWrite" value="1" min="1" max="252">
+            <button id="writeIdBtn">Write</button><br>
+
+            <label for="baudRead">Read Baud Rate:</label>
+            <button id="readBaudBtn">Read</button>
+            <span id="readBaudResult"></span><br>
+
+            <label for="baudWrite">Write Baud Rate Index:</label>
+            <input type="number" id="baudWrite" value="6" min="0" max="7"> <!-- Assuming index 0-7 -->
+            <button id="writeBaudBtn">Write</button><br>
+
+            <label for="positionRead">Read Position:</label>
+            <button id="readPosBtn">Read</button>
+            <span id="readPosResult"></span><br>
+
+            <label for="positionWrite">Write Position:</label>
+            <input type="number" id="positionWrite" value="1000" min="0" max="4095">
+            <button id="writePosBtn">Write</button><br>
+
+            <label for="torqueEnable">Torque:</label>
+            <button id="torqueEnableBtn">Enable</button>
+            <button id="torqueDisableBtn">Disable</button><br>
+
+            <label for="accelerationWrite">Write Acceleration:</label>
+            <input type="number" id="accelerationWrite" value="50" min="0" max="254">
+            <button id="writeAccBtn">Write</button><br>
+
+             <label for="wheelMode">Wheel Mode:</label>
+            <button id="setWheelModeBtn">Set Wheel Mode</button>
+            <button id="removeWheelModeBtn">Set Position Mode</button><br>
+
+            <label for="wheelSpeedWrite">Write Wheel Speed:</label>
+            <input type="number" id="wheelSpeedWrite" value="0" min="-2500" max="2500">
+            <button id="writeWheelSpeedBtn">Write Speed</button>
+        </div>
+
+         <div class="section">
+            <h2>Sync Operations</h2>
+            <label for="syncReadIds">Sync Read IDs (csv):</label>
+            <input type="text" id="syncReadIds" value="1,2,3" style="width: 150px;">
+            <button id="syncReadBtn">Sync Read Positions</button><br>
+
+            <label for="syncWriteData">Sync Write (id:pos,...):</label>
+            <input type="text" id="syncWriteData" value="1:1500,2:2500" style="width: 200px;">
+            <button id="syncWriteBtn">Sync Write Positions</button><br>
+
+            <label for="syncWriteSpeedData">Sync Write Speed (id:speed,...):</label>
+            <input type="text" id="syncWriteSpeedData" value="1:500,2:-1000" style="width: 200px;">
+            <button id="syncWriteSpeedBtn">Sync Write Speeds</button> <!-- New Button -->
+        </div>
+
+
+        <div class="section">
+            <h2>Log Output</h2>
+            <pre id="logOutput"></pre>
+        </div>
+    </div>
+
+    <script type="module">
+        // Import the scsServoSDK object from index.mjs
+        import { ScsServoSDK } from './index.mjs';
+        const scsServoSDK = new ScsServoSDK();
+        // No longer need COMM_SUCCESS etc. here as errors are thrown
+
+        const connectBtn = document.getElementById('connectBtn');
+        const disconnectBtn = document.getElementById('disconnectBtn');
+        const connectionStatus = document.getElementById('connectionStatus');
+        const baudRateInput = document.getElementById('baudRate');
+        const protocolEndInput = document.getElementById('protocolEnd');
+
+        const servoIdInput = document.getElementById('servoId');
+        const readIdBtn = document.getElementById('readIdBtn'); // New
+        const readIdResult = document.getElementById('readIdResult'); // New
+        const idWriteInput = document.getElementById('idWrite'); // New
+        const writeIdBtn = document.getElementById('writeIdBtn'); // New
+        const readBaudBtn = document.getElementById('readBaudBtn'); // New
+        const readBaudResult = document.getElementById('readBaudResult'); // New
+        const baudWriteInput = document.getElementById('baudWrite'); // New
+        const writeBaudBtn = document.getElementById('writeBaudBtn'); // New
+        const readPosBtn = document.getElementById('readPosBtn');
+        const readPosResult = document.getElementById('readPosResult');
+        const positionWriteInput = document.getElementById('positionWrite');
+        const writePosBtn = document.getElementById('writePosBtn');
+        const torqueEnableBtn = document.getElementById('torqueEnableBtn');
+        const torqueDisableBtn = document.getElementById('torqueDisableBtn');
+        const accelerationWriteInput = document.getElementById('accelerationWrite');
+        const writeAccBtn = document.getElementById('writeAccBtn');
+        const setWheelModeBtn = document.getElementById('setWheelModeBtn');
+        const removeWheelModeBtn = document.getElementById('removeWheelModeBtn'); // Get reference to the new button
+        const wheelSpeedWriteInput = document.getElementById('wheelSpeedWrite');
+        const writeWheelSpeedBtn = document.getElementById('writeWheelSpeedBtn');
+
+        const syncReadIdsInput = document.getElementById('syncReadIds');
+        const syncReadBtn = document.getElementById('syncReadBtn');
+        const syncWriteDataInput = document.getElementById('syncWriteData');
+        const syncWriteBtn = document.getElementById('syncWriteBtn');
+        const syncWriteSpeedDataInput = document.getElementById('syncWriteSpeedData'); // New Input
+        const syncWriteSpeedBtn = document.getElementById('syncWriteSpeedBtn');       // New Button
+        const scanServosBtn = document.getElementById('scanServosBtn'); // Get reference to the scan button
+        const scanStartIdInput = document.getElementById('scanStartId'); // Get reference to start ID input
+        const scanEndIdInput = document.getElementById('scanEndId');   // Get reference to end ID input
+        const scanResultsOutput = document.getElementById('scanResultsOutput'); // Get reference to the new results area
+
+        const logOutput = document.getElementById('logOutput');
+
+        let isConnected = false;
+
+        function log(message) {
+            console.log(message);
+            const timestamp = new Date().toLocaleTimeString();
+            logOutput.textContent = `[${timestamp}] ${message}\n` + logOutput.textContent;
+            // Limit log size
+            const lines = logOutput.textContent.split('\n'); // Use '\n' instead of literal newline
+            if (lines.length > 50) {
+                logOutput.textContent = lines.slice(0, 50).join('\n'); // Use '\n' instead of literal newline
+            }
+        }
+
+        function updateConnectionStatus(connected, message) {
+            isConnected = connected;
+            connectionStatus.textContent = message || (connected ? 'Connected' : 'Disconnected');
+            connectionStatus.className = `status ${connected ? 'success' : 'error'}`;
+            log(`Connection status: ${connectionStatus.textContent}`);
+        }
+
+        connectBtn.onclick = async () => {
+            log('Attempting to connect...');
+            try {
+                const baudRate = parseInt(baudRateInput.value, 10);
+                const protocolEnd = parseInt(protocolEndInput.value, 10);
+                // Use scsServoSDK - throws on error
+                await scsServoSDK.connect({ baudRate, protocolEnd });
+                updateConnectionStatus(true, 'Connected');
+            } catch (err) {
+                updateConnectionStatus(false, `Connection error: ${err.message}`);
+                console.error(err);
+            }
+        };
+
+        disconnectBtn.onclick = async () => {
+            log('Attempting to disconnect...');
+            try {
+                // Use scsServoSDK - throws on error
+                await scsServoSDK.disconnect();
+                updateConnectionStatus(false, 'Disconnected'); // Success means disconnected
+            } catch (err) {
+                // Assuming disconnect might fail if already disconnected or other issues
+                updateConnectionStatus(false, `Disconnection error: ${err.message}`);
+                console.error(err);
+            }
+        };
+
+        writeIdBtn.onclick = async () => { // New handler
+            if (!isConnected) { log('Error: Not connected'); return; }
+            const currentId = parseInt(servoIdInput.value, 10);
+            const newId = parseInt(idWriteInput.value, 10);
+            if (isNaN(newId) || newId < 1 || newId > 252) {
+                log(`Error: Invalid new ID ${newId}. Must be between 1 and 252.`);
+                return;
+            }
+            log(`Writing new ID ${newId} to servo ${currentId}...`);
+            try {
+                // Use scsServoSDK - throws on error
+                await scsServoSDK.setServoId(currentId, newId);
+                log(`Successfully wrote new ID ${newId} to servo (was ${currentId}).`);
+                // IMPORTANT: Update the main ID input to reflect the change
+                servoIdInput.value = newId;
+                log(`Servo ID input field updated to ${newId}.`);
+            } catch (err) {
+                 log(`Error writing ID for servo ${currentId}: ${err.message}`);
+                 console.error(err);
+            }
+        };
+
+        readBaudBtn.onclick = async () => { // New handler
+            if (!isConnected) { log('Error: Not connected'); return; }
+            const id = parseInt(servoIdInput.value, 10);
+            log(`Reading Baud Rate Index for servo ${id}...`);
+            readBaudResult.textContent = 'Reading...';
+            try {
+                // Use scsServoSDK - returns value directly or throws
+                const baudRateIndex = await scsServoSDK.readBaudRate(id);
+                readBaudResult.textContent = `Baud Index: ${baudRateIndex}`;
+                log(`Servo ${id} Baud Rate Index: ${baudRateIndex}`);
+            } catch (err) {
+                 readBaudResult.textContent = `Error: ${err.message}`;
+                 log(`Error reading Baud Rate Index for servo ${id}: ${err.message}`);
+                 console.error(err);
+            }
+        };
+
+        writeBaudBtn.onclick = async () => { // New handler
+            if (!isConnected) { log('Error: Not connected'); return; }
+            const id = parseInt(servoIdInput.value, 10);
+            const newBaudIndex = parseInt(baudWriteInput.value, 10);
+             if (isNaN(newBaudIndex) || newBaudIndex < 0 || newBaudIndex > 7) { // Adjust max index if needed
+                log(`Error: Invalid new Baud Rate Index ${newBaudIndex}. Check valid range.`);
+                return;
+            }
+            log(`Writing new Baud Rate Index ${newBaudIndex} to servo ${id}...`);
+            try {
+                // Use scsServoSDK - throws on error
+                await scsServoSDK.setBaudRate(id, newBaudIndex);
+                log(`Successfully wrote new Baud Rate Index ${newBaudIndex} to servo ${id}.`);
+                log(`IMPORTANT: You may need to disconnect and reconnect with the new baud rate if it differs from the current connection baud rate.`);
+            } catch (err) {
+                 log(`Error writing Baud Rate Index for servo ${id}: ${err.message}`);
+                 console.error(err);
+            }
+        };
+
+
+        readPosBtn.onclick = async () => {
+            if (!isConnected) { log('Error: Not connected'); return; }
+            const id = parseInt(servoIdInput.value, 10);
+            log(`Reading position for servo ${id}...`);
+            readPosResult.textContent = 'Reading...';
+            try {
+                // Use scsServoSDK - returns value directly or throws
+                const position = await scsServoSDK.readPosition(id);
+                readPosResult.textContent = `Position: ${position}`;
+                log(`Servo ${id} position: ${position}`);
+            } catch (err) {
+                 readPosResult.textContent = `Error: ${err.message}`;
+                 log(`Error reading position for servo ${id}: ${err.message}`);
+                 console.error(err);
+            }
+        };
+
+        writePosBtn.onclick = async () => {
+            if (!isConnected) { log('Error: Not connected'); return; }
+            const id = parseInt(servoIdInput.value, 10);
+            const pos = parseInt(positionWriteInput.value, 10);
+            log(`Writing position ${pos} to servo ${id}...`);
+            try {
+                // Use scsServoSDK - throws on error
+                await scsServoSDK.writePosition(id, pos);
+                log(`Successfully wrote position ${pos} to servo ${id}.`);
+            } catch (err) {
+                 log(`Error writing position for servo ${id}: ${err.message}`);
+                 console.error(err);
+            }
+        };
+
+        torqueEnableBtn.onclick = async () => {
+             if (!isConnected) { log('Error: Not connected'); return; }
+            const id = parseInt(servoIdInput.value, 10);
+            log(`Enabling torque for servo ${id}...`);
+            try {
+                // Use scsServoSDK - throws on error
+                await scsServoSDK.writeTorqueEnable(id, true);
+                log(`Successfully enabled torque for servo ${id}.`);
+            } catch (err) {
+                 log(`Error enabling torque for servo ${id}: ${err.message}`);
+                 console.error(err);
+            }
+        };
+
+        torqueDisableBtn.onclick = async () => {
+             if (!isConnected) { log('Error: Not connected'); return; }
+            const id = parseInt(servoIdInput.value, 10);
+            log(`Disabling torque for servo ${id}...`);
+            try {
+                // Use scsServoSDK - throws on error
+                await scsServoSDK.writeTorqueEnable(id, false);
+                log(`Successfully disabled torque for servo ${id}.`);
+            } catch (err) {
+                 log(`Error disabling torque for servo ${id}: ${err.message}`);
+                 console.error(err);
+            }
+        };
+
+         writeAccBtn.onclick = async () => {
+            if (!isConnected) { log('Error: Not connected'); return; }
+            const id = parseInt(servoIdInput.value, 10);
+            const acc = parseInt(accelerationWriteInput.value, 10);
+            log(`Writing acceleration ${acc} to servo ${id}...`);
+            try {
+                // Use scsServoSDK - throws on error
+                await scsServoSDK.writeAcceleration(id, acc);
+                log(`Successfully wrote acceleration ${acc} to servo ${id}.`);
+            } catch (err) {
+                 log(`Error writing acceleration for servo ${id}: ${err.message}`);
+                 console.error(err);
+            }
+        };
+
+        setWheelModeBtn.onclick = async () => {
+            if (!isConnected) { log('Error: Not connected'); return; }
+            const id = parseInt(servoIdInput.value, 10);
+            log(`Setting servo ${id} to wheel mode...`);
+            try {
+                // Use scsServoSDK - throws on error
+                await scsServoSDK.setWheelMode(id);
+                log(`Successfully set servo ${id} to wheel mode.`);
+            } catch (err) {
+                 log(`Error setting wheel mode for servo ${id}: ${err.message}`);
+                 console.error(err);
+            }
+        };
+
+        // Add event listener for the new button
+        removeWheelModeBtn.onclick = async () => {
+            if (!isConnected) { log('Error: Not connected'); return; }
+            const id = parseInt(servoIdInput.value, 10);
+            log(`Setting servo ${id} back to position mode...`);
+            try {
+                // Use scsServoSDK - throws on error
+                await scsServoSDK.setPositionMode(id);
+                log(`Successfully set servo ${id} back to position mode.`);
+            } catch (err) {
+                 log(`Error setting position mode for servo ${id}: ${err.message}`);
+                 console.error(err);
+            }
+        };
+
+        writeWheelSpeedBtn.onclick = async () => {
+            if (!isConnected) { log('Error: Not connected'); return; }
+            const id = parseInt(servoIdInput.value, 10);
+            const speed = parseInt(wheelSpeedWriteInput.value, 10);
+            log(`Writing wheel speed ${speed} to servo ${id}...`);
+            try {
+                // Use scsServoSDK - throws on error
+                await scsServoSDK.writeWheelSpeed(id, speed);
+                log(`Successfully wrote wheel speed ${speed} to servo ${id}.`);
+            } catch (err) {
+                 log(`Error writing wheel speed for servo ${id}: ${err.message}`);
+                 console.error(err);
+            }
+        };
+
+        syncReadBtn.onclick = async () => {
+            if (!isConnected) { log('Error: Not connected'); return; }
+            const idsString = syncReadIdsInput.value;
+            const ids = idsString.split(',').map(s => parseInt(s.trim(), 10)).filter(id => !isNaN(id) && id > 0 && id < 253);
+            if (ids.length === 0) {
+                log('Sync Read: No valid servo IDs provided.');
+                return;
+            }
+            log(`Sync reading positions for servos: ${ids.join(', ')}...`);
+            try {
+                // Use scsServoSDK - returns Map or throws
+                const positions = await scsServoSDK.syncReadPositions(ids);
+                let logMsg = 'Sync Read Successful:\n';
+                positions.forEach((pos, id) => {
+                    logMsg += `  Servo ${id}: Position=${pos}\n`;
+                });
+                log(logMsg.trim());
+
+            } catch (err) {
+                 log(`Sync Read Failed: ${err.message}`);
+                 console.error(err);
+            }
+        };
+
+        syncWriteBtn.onclick = async () => {
+            if (!isConnected) { log('Error: Not connected'); return; }
+            const dataString = syncWriteDataInput.value;
+            const positionMap = new Map();
+            const pairs = dataString.split(',');
+            let validData = false;
+
+            pairs.forEach(pair => {
+                const parts = pair.split(':');
+                if (parts.length === 2) {
+                    const id = parseInt(parts[0].trim(), 10);
+                    const pos = parseInt(parts[1].trim(), 10);
+                    // Position validation (0-4095)
+                    if (!isNaN(id) && id > 0 && id < 253 && !isNaN(pos) && pos >= 0 && pos <= 4095) {
+                        positionMap.set(id, pos);
+                        validData = true;
+                    } else {
+                         log(`Sync Write Position: Invalid data pair "${pair}". ID (1-252), Pos (0-4095). Skipping.`);
+                    }
+                } else {
+                    log(`Sync Write Position: Invalid format "${pair}". Skipping.`);
+                }
+            });
+
+            if (!validData) {
+                log('Sync Write Position: No valid servo position data provided.');
+                return;
+            }
+
+            log(`Sync writing positions: ${Array.from(positionMap.entries()).map(([id, pos]) => `${id}:${pos}`).join(', ')}...`);
+            try {
+                // Use scsServoSDK - throws on error
+                await scsServoSDK.syncWritePositions(positionMap);
+                log(`Sync write position command sent successfully.`);
+            } catch (err) {
+                 log(`Sync Write Position Failed: ${err.message}`);
+                 console.error(err);
+            }
+        };
+
+        // New handler for Sync Write Speed
+        syncWriteSpeedBtn.onclick = async () => {
+            if (!isConnected) { log('Error: Not connected'); return; }
+            const dataString = syncWriteSpeedDataInput.value;
+            const speedMap = new Map();
+            const pairs = dataString.split(',');
+            let validData = false;
+
+            pairs.forEach(pair => {
+                const parts = pair.split(':');
+                if (parts.length === 2) {
+                    const id = parseInt(parts[0].trim(), 10);
+                    const speed = parseInt(parts[1].trim(), 10);
+                    // Speed validation (-10000 to 10000)
+                    if (!isNaN(id) && id > 0 && id < 253 && !isNaN(speed) && speed >= -10000 && speed <= 10000) {
+                        speedMap.set(id, speed);
+                        validData = true;
+                    } else {
+                         log(`Sync Write Speed: Invalid data pair "${pair}". ID (1-252), Speed (-10000 to 10000). Skipping.`);
+                    }
+                } else {
+                    log(`Sync Write Speed: Invalid format "${pair}". Skipping.`);
+                }
+            });
+
+            if (!validData) {
+                log('Sync Write Speed: No valid servo speed data provided.');
+                return;
+            }
+
+            log(`Sync writing speeds: ${Array.from(speedMap.entries()).map(([id, speed]) => `${id}:${speed}`).join(', ')}...`);
+            try {
+                // Use scsServoSDK - throws on error
+                await scsServoSDK.syncWriteWheelSpeed(speedMap);
+                log(`Sync write speed command sent successfully.`);
+            } catch (err) {
+                 log(`Sync Write Speed Failed: ${err.message}`);
+                 console.error(err);
+            }
+        };
+
+
+        scanServosBtn.onclick = async () => {
+            if (!isConnected) { log('Error: Not connected'); return; }
+
+            const startId = parseInt(scanStartIdInput.value, 10);
+            const endId = parseInt(scanEndIdInput.value, 10);
+
+            if (isNaN(startId) || isNaN(endId) || startId < 1 || endId > 252 || startId > endId) {
+                const errorMsg = 'Error: Invalid scan ID range. Please enter values between 1 and 252, with Start ID <= End ID.';
+                log(errorMsg);
+                scanResultsOutput.textContent = errorMsg; // Show error in results area too
+                return;
+            }
+
+            const startMsg = `Starting servo scan (IDs ${startId}-${endId})...`;
+            log(startMsg);
+            scanResultsOutput.textContent = startMsg + '\n'; // Clear and start results area
+            scanServosBtn.disabled = true; // Disable button during scan
+
+            let foundCount = 0;
+
+            for (let id = startId; id <= endId; id++) {
+                let resultMsg = `Scanning ID ${id}... `;
+                try {
+                    // Attempt to read position. If it succeeds, the servo exists.
+                    // If it throws, the servo likely doesn't exist or there's another issue.
+                    const position = await scsServoSDK.readPosition(id);
+                    foundCount++;
+
+                    // Servo found, now try to read mode and baud rate
+                    let mode = 'ReadError';
+                    let baudRateIndex = 'ReadError';
+                    try {
+                        mode = await scsServoSDK.readMode(id);
+                    } catch (modeErr) {
+                        log(`  Servo ${id}: Error reading mode: ${modeErr.message}`);
+                    }
+                    try {
+                        baudRateIndex = await scsServoSDK.readBaudRate(id);
+                    } catch (baudErr) {
+                        log(`  Servo ${id}: Error reading baud rate: ${baudErr.message}`);
+                    }
+
+                    resultMsg += `FOUND: Pos=${position}, Mode=${mode}, BaudIdx=${baudRateIndex}`;
+                    log(`  Servo ${id} FOUND: Position=${position}, Mode=${mode}, BaudIndex=${baudRateIndex}`);
+
+                } catch (err) {
+                    // Check if the error message indicates a timeout or non-response, which is expected for non-existent IDs
+                    // This check might need refinement based on the exact error messages thrown by readPosition
+                    if (err.message.includes('timeout') || err.message.includes('No response') || err.message.includes('failed: RX')) {
+                         resultMsg += `No response`;
+                         // log(`  Servo ${id}: No response`); // Optional: reduce log noise
+                    } else {
+                        // Log other unexpected errors
+                        resultMsg += `Error: ${err.message}`;
+                        log(`  Servo ${id}: Error during scan: ${err.message}`);
+                        console.error(`Error scanning servo ${id}:`, err);
+                    }
+                }
+                scanResultsOutput.textContent += resultMsg + '\n'; // Append result to the results area
+                scanResultsOutput.scrollTop = scanResultsOutput.scrollHeight; // Auto-scroll
+                 // Optional small delay between scans if needed
+                 // await new Promise(resolve => setTimeout(resolve, 10));
+            }
+
+            const finishMsg = `Servo scan finished. Found ${foundCount} servo(s).`;
+            log(finishMsg);
+            scanResultsOutput.textContent += finishMsg + '\n'; // Add finish message to results area
+            scanResultsOutput.scrollTop = scanResultsOutput.scrollHeight; // Auto-scroll
+            scanServosBtn.disabled = false; // Re-enable button
+        };
+
+        // Initial log
+        log('Test page loaded. Please connect to a servo controller.');
+
+    </script>
+</body>
 </html>

src/lib/components/3d/elements/robot/modal/InputConnectionModal.svelte

@@ -41,13 +41,6 @@
 		}
 	});
 
-	// Set up calibration completion callback
-	$effect(() => {
-		robot.calibrationManager.onCalibrationCompleteWithPositions((finalPositions) => {
-			robot.syncToCalibrationPositions(finalPositions);
-		});
-	});
-
 	async function refreshRooms() {
 		try {
 			error = null;
@@ -129,17 +122,20 @@
 			isConnecting = true;
 			error = null;
 
-			if (robot.calibrationManager.needsCalibration) {
-				pendingUSBConnection = "input";
-				showUSBCalibration = true;
-				return;
-			}
-
+			// Create USB consumer first, which will handle calibration internally
 			await robot.setConsumer({
 				type: "usb",
 				baudRate: 1000000
 			});
 
+			// Check if the new consumer needs calibration
+			const uncalibratedDrivers = robot.getUncalibratedUSBDrivers();
+			if (uncalibratedDrivers.length > 0) {
+				pendingUSBConnection = "input";
+				showUSBCalibration = true;
+				return;
+			}
+
 			toast.success("USB Input Connected", {
 				description: "Successfully connected to physical robot hardware"
 			});
@@ -174,18 +170,22 @@
 
 	async function onCalibrationComplete() {
 		showUSBCalibration = false;
-
-		if (pendingUSBConnection === "input") {
-			await connectUSBInput();
-		}
-
 		pendingUSBConnection = null;
+
+		toast.success("Calibration Complete", {
+			description: "Hardware calibrated and ready for use"
+		});
 	}
 
 	function onCalibrationCancel() {
 		showUSBCalibration = false;
 		pendingUSBConnection = null;
 		isConnecting = false;
+		
+		// Clean up the uncalibrated USB consumer
+		robot.removeConsumer().catch(err => {
+			console.error("Failed to clean up USB consumer after calibration cancel:", err);
+		});
 	}
 </script>
 
@@ -252,12 +252,19 @@
 								</Alert.Description>
 							</Alert.Root>
 
-							<USBCalibrationPanel
-								calibrationManager={robot.calibrationManager}
-								connectionType="consumer"
-								{onCalibrationComplete}
-								onCancel={onCalibrationCancel}
-							/>
+							<!-- Show calibration panels for each uncalibrated USB driver -->
+							{#each robot.getUncalibratedUSBDrivers() as usbDriver}
+								<USBCalibrationPanel
+									calibrationManager={usbDriver}
+									connectionType="consumer"
+									{onCalibrationComplete}
+									onCancel={onCalibrationCancel}
+								/>
+							{:else}
+								<div class="text-center text-slate-400">
+									No USB drivers require calibration
+								</div>
+							{/each}
 						</Card.Content>
 					</Card.Root>
 				{:else}

src/lib/components/3d/elements/robot/modal/OutputConnectionModal.svelte

@@ -50,13 +50,6 @@
 		}
 	});
 
-	// Set up calibration completion callback
-	$effect(() => {
-		robot.calibrationManager.onCalibrationCompleteWithPositions((finalPositions) => {
-			robot.syncToCalibrationPositions(finalPositions);
-		});
-	});
-
 	// Room management functions
 	async function refreshRooms() {
 		try {
@@ -138,18 +131,20 @@
 			isConnecting = true;
 			error = null;
 
-			// Check if calibration is needed
-			if (robot.calibrationManager.needsCalibration) {
-				pendingUSBConnection = "output";
-				showUSBCalibration = true;
-				return;
-			}
-
+			// Create USB producer first, which will handle calibration internally
 			await robot.addProducer({
 				type: "usb",
 				baudRate: 1000000
 			});
 
+			// Check if any USB drivers need calibration
+			const uncalibratedDrivers = robot.getUncalibratedUSBDrivers();
+			if (uncalibratedDrivers.length > 0) {
+				pendingUSBConnection = "output";
+				showUSBCalibration = true;
+				return;
+			}
+
 			toast.success("USB Output Connected", {
 				description: "Successfully connected to physical robot hardware"
 			});
@@ -185,18 +180,29 @@
 	// Handle calibration completion
 	async function onCalibrationComplete() {
 		showUSBCalibration = false;
-
-		if (pendingUSBConnection === "output") {
-			await connectUSBOutput();
-		}
-
 		pendingUSBConnection = null;
+
+		toast.success("Calibration Complete", {
+			description: "Hardware calibrated and ready for use"
+		});
 	}
 
 	function onCalibrationCancel() {
 		showUSBCalibration = false;
 		pendingUSBConnection = null;
 		isConnecting = false;
+		
+		// Clean up the uncalibrated USB producer
+		const uncalibratedDrivers = robot.getUncalibratedUSBDrivers();
+		if (uncalibratedDrivers.length > 0) {
+			// Remove the most recent producer (should be the one we just added)
+			const lastProducer = robot.producers[robot.producers.length - 1];
+			if (lastProducer) {
+				robot.removeProducer(lastProducer.id).catch(err => {
+					console.error("Failed to clean up USB producer after calibration cancel:", err);
+				});
+			}
+		}
 	}
 </script>
 
@@ -262,12 +268,19 @@
 								</Alert.Description>
 							</Alert.Root>
 
-							<USBCalibrationPanel
-								calibrationManager={robot.calibrationManager}
-								connectionType="producer"
-								{onCalibrationComplete}
-								onCancel={onCalibrationCancel}
-							/>
+							<!-- Show calibration panels for each uncalibrated USB driver -->
+							{#each robot.getUncalibratedUSBDrivers() as usbDriver}
+								<USBCalibrationPanel
+									calibrationManager={usbDriver}
+									connectionType="producer"
+									{onCalibrationComplete}
+									onCancel={onCalibrationCancel}
+								/>
+							{:else}
+								<div class="text-center text-slate-400">
+									No USB drivers require calibration
+								</div>
+							{/each}
 						</Card.Content>
 					</Card.Root>
 				{:else}

src/lib/elements/robot/Robot.svelte.ts

@@ -12,8 +12,8 @@ import { USBConsumer } from './drivers/USBConsumer.js';
 import { USBProducer } from './drivers/USBProducer.js';
 import { RemoteConsumer } from './drivers/RemoteConsumer.js';
 import { RemoteProducer } from './drivers/RemoteProducer.js';
-import { USBCalibrationManager } from './calibration/USBCalibrationManager.js';
-import type { UrdfRobotState } from '@/types/robot.js';
+import { USBServoDriver } from './drivers/USBServoDriver.js';
+
 import { ROBOT_CONFIG } from './config.js';
 import type IUrdfRobot from '@/components/3d/elements/robot/URDF/interfaces/IUrdfRobot.js';
 
@@ -46,9 +46,6 @@ export class Robot implements Positionable {
   // URDF robot state for 3D visualization - PUBLIC for reactive access
   urdfRobotState = $state<IUrdfRobot | null>(null);
   
-  // Shared USB calibration manager for this robot
-  private usbCalibrationManager: USBCalibrationManager = new USBCalibrationManager();
-  
   // Derived reactive values for components
   jointArray = $derived(Object.values(this.joints));
   hasProducers = $derived(this.producers.length > 0);
@@ -83,49 +80,53 @@ export class Robot implements Positionable {
     this.position = { ...newPosition };
   }
 
-  // Calibration access
-  get calibrationManager(): USBCalibrationManager {
-    return this.usbCalibrationManager;
-  }
-
-  // NEW: Sync virtual robot to final calibration positions
-  syncToCalibrationPositions(finalPositions: Record<string, number>): void {
-    console.log(`[Robot ${this.id}] 🔄 Syncing virtual robot to final calibration positions...`);
+  // Get all USB drivers (both consumer and producers) for calibration
+  getUSBDrivers(): USBServoDriver[] {
+    const usbDrivers: USBServoDriver[] = [];
     
-    Object.entries(finalPositions).forEach(([jointName, rawPosition]) => {
-      const joint = this.joints[jointName];
-      if (!joint) {
-        console.warn(`[Robot ${this.id}] Joint ${jointName} not found for position sync`);
-        return;
-      }
-
-      // Convert raw servo position to normalized value using calibration data
-      const normalizedValue = this.usbCalibrationManager.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));
+    // Check consumer
+    if (this.consumer && USBServoDriver.isUSBDriver(this.consumer)) {
+      usbDrivers.push(this.consumer);
+    }
+    
+    // Check producers
+    this.producers.forEach(producer => {
+      if (USBServoDriver.isUSBDriver(producer)) {
+        usbDrivers.push(producer);
       }
-      
-      console.log(`[Robot ${this.id}] ${jointName}: ${rawPosition} (raw) -> ${normalizedValue.toFixed(1)} -> ${clampedValue.toFixed(1)} (normalized)`);
-      
-      // Update joint value to match physical servo position
-      this.joints[jointName] = { ...joint, value: clampedValue };
     });
     
-    console.log(`[Robot ${this.id}] ✅ Virtual robot synced to calibration positions`);
+    return usbDrivers;
   }
 
-  // Joint value updates (normalized)
+  // Get uncalibrated USB drivers that need calibration
+  getUncalibratedUSBDrivers(): USBServoDriver[] {
+    return this.getUSBDrivers().filter(driver => driver.needsCalibration);
+  }
+
+  // Check if robot has any USB drivers
+  hasUSBDrivers(): boolean {
+    return this.getUSBDrivers().length > 0;
+  }
+
+  // Check if all USB drivers are calibrated
+  areAllUSBDriversCalibrated(): boolean {
+    const usbDrivers = this.getUSBDrivers();
+    return usbDrivers.length > 0 && usbDrivers.every(driver => driver.isCalibrated);
+  }
+
+  // Joint value updates (normalized) - for manual control
   updateJoint(name: string, normalizedValue: number): void {
     if (!this.isManualControlEnabled) {
       console.warn('Manual control is disabled');
       return;
     }
 
+    this.updateJointValue(name, normalizedValue, true);
+  }
+
+  // Internal joint value update (used by both manual control and USB calibration sync)
+  updateJointValue(name: string, normalizedValue: number, sendToProducers: boolean = false): void {
     const joint = this.joints[name];
     if (!joint) {
       console.warn(`Joint ${name} not found`);
@@ -139,13 +140,15 @@ export class Robot implements Positionable {
       normalizedValue = Math.max(-100, Math.min(100, normalizedValue));
     }
 
-    console.debug(`[Robot ${this.id}] Manual update joint ${name} to ${normalizedValue} (normalized)`);
+    console.debug(`[Robot ${this.id}] Update joint ${name} to ${normalizedValue} (normalized, sendToProducers: ${sendToProducers})`);
 
     // Create a new joint object to ensure reactivity
     this.joints[name] = { ...joint, value: normalizedValue };
 
-    // Send normalized command to producers
-    this.sendToProducers({ joints: [{ name, value: normalizedValue }] });
+    // Send normalized command to producers if requested
+    if (sendToProducers) {
+      this.sendToProducers({ joints: [{ name, value: normalizedValue }] });
+    }
   }
 
   executeCommand(command: RobotCommand): void {
@@ -183,27 +186,7 @@ export class Robot implements Positionable {
       console.debug(`[Robot ${this.id}] Executing command with ${command.joints.length} joints:`, 
         command.joints.map(j => `${j.name}=${j.value}`).join(', '));
       
-      // Check if USB calibration is in progress
-      if (this.usbCalibrationManager.calibrationState.isCalibrating) {
-        console.debug(`[Robot ${this.id}] 🚫 Blocking virtual robot updates - USB calibration in progress`);
-        // Still send to producers, but don't update virtual robot
-        this.sendToProducers(command);
-        return;
-      }
-      
-      // Check if USB calibration is needed (if we have USB consumer/producers)
-      const hasUSBDrivers = (this.consumer instanceof USBConsumer) || 
-                           this.producers.some(p => p instanceof USBProducer);
-      
-      if (hasUSBDrivers && this.usbCalibrationManager.needsCalibration) {
-        console.debug(`[Robot ${this.id}] ⏳ Blocking virtual robot updates - USB drivers need calibration`);
-        // Still send to producers, but don't update virtual robot
-        this.sendToProducers(command);
-        return;
-      }
-      
-      console.debug(`[Robot ${this.id}] ✅ Updating virtual robot - USB calibrated or no USB drivers`);
-      
+      // Update virtual robot joints with normalized values
       command.joints.forEach(jointCmd => {
         const joint = this.joints[jointCmd.name];
         if (joint) {
@@ -273,9 +256,30 @@ export class Robot implements Positionable {
 
     const consumer = this.createConsumer(config);
     
+    // Set up calibration completion callback for USB drivers
+    if (USBServoDriver.isUSBDriver(consumer)) {
+      const calibrationUnsubscribe = consumer.onCalibrationCompleteWithPositions(async (finalPositions: Record<string, number>) => {
+        console.log(`[Robot ${this.id}] Calibration complete, syncing robot to final positions`);
+        consumer.syncRobotPositions(finalPositions, (jointName: string, normalizedValue: number) => {
+          this.updateJointValue(jointName, normalizedValue, false); // Don't send to producers to avoid feedback loop
+        });
+        
+        // Start listening now that calibration is complete
+        if ('startListening' in consumer && consumer.startListening) {
+          try {
+            await consumer.startListening();
+            console.log(`[Robot ${this.id}] Started listening after calibration completion`);
+          } catch (error) {
+            console.error(`[Robot ${this.id}] Failed to start listening after calibration:`, error);
+          }
+        }
+      });
+      this.unsubscribeFns.push(calibrationUnsubscribe);
+    }
+    
     // Only pass joinExistingRoom to remote drivers
     if (config.type === 'remote') {
-      await (consumer as any).connect(joinExistingRoom);
+      await (consumer as RemoteConsumer).connect(joinExistingRoom);
     } else {
       await consumer.connect();
     }
@@ -292,9 +296,18 @@ export class Robot implements Positionable {
     });
     this.unsubscribeFns.push(statusUnsubscribe);
 
-    // Start listening for consumers with this capability
+    // Start listening for consumers with this capability (only if calibrated for USB)
     if ('startListening' in consumer && consumer.startListening) {
-      await consumer.startListening();
+      // For USB consumers, only start listening if calibrated
+      if (USBServoDriver.isUSBDriver(consumer)) {
+        if (consumer.isCalibrated) {
+          await consumer.startListening();
+        }
+        // If not calibrated, startListening will be called after calibration completion
+      } else {
+        // For non-USB consumers, start listening immediately
+        await consumer.startListening();
+      }
     }
 
     this.consumer = consumer;
@@ -319,9 +332,22 @@ export class Robot implements Positionable {
   private async _addProducer(config: USBDriverConfig | RemoteDriverConfig, joinExistingRoom: boolean): Promise<string> {
     const producer = this.createProducer(config);
     
+    // Set up calibration completion callback for USB drivers
+    if (USBServoDriver.isUSBDriver(producer)) {
+      const calibrationUnsubscribe = producer.onCalibrationCompleteWithPositions(async (finalPositions: Record<string, number>) => {
+        console.log(`[Robot ${this.id}] Calibration complete, syncing robot to final positions`);
+        producer.syncRobotPositions(finalPositions, (jointName: string, normalizedValue: number) => {
+          this.updateJointValue(jointName, normalizedValue, false); // Don't send to producers to avoid feedback loop
+        });
+        
+        console.log(`[Robot ${this.id}] USB Producer calibration completed and ready for commands`);
+      });
+      this.unsubscribeFns.push(calibrationUnsubscribe);
+    }
+    
     // Only pass joinExistingRoom to remote drivers
     if (config.type === 'remote') {
-      await (producer as any).connect(joinExistingRoom);
+      await (producer as RemoteProducer).connect(joinExistingRoom);
     } else {
       await producer.connect();
     }
@@ -366,7 +392,7 @@ export class Robot implements Positionable {
   private createConsumer(config: USBDriverConfig | RemoteDriverConfig): Consumer {
     switch (config.type) {
       case 'usb':
-        return new USBConsumer(config, this.usbCalibrationManager);
+        return new USBConsumer(config);
       case 'remote':
         return new RemoteConsumer(config);
       default:
@@ -378,7 +404,7 @@ export class Robot implements Positionable {
   private createProducer(config: USBDriverConfig | RemoteDriverConfig): Producer {
     switch (config.type) {
       case 'usb':
-        return new USBProducer(config, this.usbCalibrationManager);
+        return new USBProducer(config);
       case 'remote':
         return new RemoteProducer(config);
       default:
@@ -465,7 +491,6 @@ export class Robot implements Positionable {
       })
     );
 
-    // Clean up calibration manager
-    await this.usbCalibrationManager.destroy();
+    // Calibration cleanup is handled by individual USB drivers
   }
 } 

src/lib/elements/robot/calibration/CalibrationState.svelte.ts

@@ -1,114 +1,271 @@
-import type { USBCalibrationManager } from './USBCalibrationManager.js';
 import type { JointCalibration } from '../models.js';
 import { ROBOT_CONFIG } from '../config.js';
 
 export class CalibrationState {
-  private manager: USBCalibrationManager;
+  // Reactive calibration state
+  isCalibrating = $state(false);
+  progress = $state(0);
   
-  // Reactive state using Svelte 5 runes - direct state management
-  private _isCalibrating = $state(false);
-  private _progress = $state(0);
-  private _isCalibrated = $state(false);
-  private _needsCalibration = $state(true);
-  private _jointValues = $state<Record<string, number>>({});
-  private _jointCalibrations = $state<Record<string, JointCalibration>>({});
+  // Joint calibration data
+  private jointCalibrations = $state<Record<string, JointCalibration>>({});
+  private currentValues = $state<Record<string, number>>({});
   
-  constructor(manager: USBCalibrationManager) {
-    this.manager = manager;
-    
-    // Initialize reactive state
-    manager.jointNames_.forEach(name => {
-      this._jointValues[name] = 0;
-      this._jointCalibrations[name] = { isCalibrated: false };
+  // Callbacks for completion with final positions
+  private completionCallbacks: Array<(positions: Record<string, number>) => void> = [];
+
+  constructor() {
+    // Initialize calibration data for expected joints
+    const jointNames = ["Rotation", "Pitch", "Elbow", "Wrist_Pitch", "Wrist_Roll", "Jaw"];
+    jointNames.forEach(name => {
+      this.jointCalibrations[name] = { 
+        isCalibrated: false,
+        minServoValue: undefined,
+        maxServoValue: undefined
+      };
+      this.currentValues[name] = 0;
     });
+  }
+
+  // Computed properties
+  get needsCalibration(): boolean {
+    return Object.values(this.jointCalibrations).some(cal => !cal.isCalibrated);
+  }
+
+  get isCalibrated(): boolean {
+    return Object.values(this.jointCalibrations).every(cal => cal.isCalibrated);
+  }
+
+  // Update current servo value during calibration
+  updateCurrentValue(jointName: string, servoValue: number): void {
+    this.currentValues[jointName] = servoValue;
     
-    // Subscribe to manager changes
-    this.setupManagerSubscription();
-    
-    // Initial state sync
-    this.syncManagerState();
+    // Update calibration range if calibrating
+    if (this.isCalibrating) {
+      const calibration = this.jointCalibrations[jointName];
+      if (calibration) {
+        // Update min/max values
+        if (calibration.minServoValue === undefined || servoValue < calibration.minServoValue) {
+          calibration.minServoValue = servoValue;
+        }
+        if (calibration.maxServoValue === undefined || servoValue > calibration.maxServoValue) {
+          calibration.maxServoValue = servoValue;
+        }
+        
+        // Update progress based on range coverage
+        this.updateProgress();
+      }
+    }
   }
-  
-  // Reactive getters - now use internal reactive state
-  get isCalibrating(): boolean { return this._isCalibrating; }
-  get progress(): number { return this._progress; }
-  get isCalibrated(): boolean { return this._isCalibrated; }
-  get needsCalibration(): boolean { return this._needsCalibration; }
-  get jointValues(): Record<string, number> { return this._jointValues; }
-  get jointCalibrations(): Record<string, JointCalibration> { return this._jointCalibrations; }
-  
-  // Get current value for a specific joint
+
+  // Get current value for a joint
   getCurrentValue(jointName: string): number | undefined {
-    return this._jointValues[jointName];
+    return this.currentValues[jointName];
   }
-  
-  // Get calibration for a specific joint
+
+  // Get calibration data for a joint
   getJointCalibration(jointName: string): JointCalibration | undefined {
-    return this._jointCalibrations[jointName];
+    return this.jointCalibrations[jointName];
   }
-  
-  // Get range for a specific joint
-  getJointRange(jointName: string): number {
-    const calibration = this._jointCalibrations[jointName];
-    if (!calibration?.minServoValue || !calibration?.maxServoValue) return 0;
-    return Math.abs(calibration.maxServoValue - calibration.minServoValue);
+
+  // Get formatted range string for display
+  getJointRange(jointName: string): string {
+    const calibration = this.jointCalibrations[jointName];
+    if (!calibration || calibration.minServoValue === undefined || calibration.maxServoValue === undefined) {
+      return "Not set";
+    }
+    return `${calibration.minServoValue}-${calibration.maxServoValue}`;
   }
-  
-  private updateInterval: Timer | null = null;
-  private managerUnsubscribe: (() => void) | null = null;
-
-  private setupManagerSubscription(): void {
-    // Use centralized config for UI update frequency
-    this.updateInterval = setInterval(() => {
-      this.syncManagerState();
-    }, ROBOT_CONFIG.polling.uiUpdateRate); // Centralized UI update rate
+
+  // Format servo value for display
+  formatServoValue(value: number | undefined): string {
+    return value !== undefined ? value.toString() : "---";
+  }
+
+  // Start calibration process
+  startCalibration(): void {
+    console.log("[CalibrationState] Starting calibration...");
+    this.isCalibrating = true;
+    this.progress = 0;
     
-    // Also listen to manager calibration changes for immediate updates
-    const unsubscribe = this.manager.onCalibrationChange(() => {
-      console.debug('[CalibrationState] Manager calibration changed, syncing state');
-      this.syncManagerState();
+    // Reset calibration data
+    Object.keys(this.jointCalibrations).forEach(jointName => {
+      this.jointCalibrations[jointName] = {
+        isCalibrated: false,
+        minServoValue: undefined,
+        maxServoValue: undefined
+      };
     });
-    
-    // Store unsubscribe function for cleanup
-    this.managerUnsubscribe = unsubscribe;
   }
 
-  private syncManagerState(): void {
-    // Sync manager state to reactive state
-    this._isCalibrating = this.manager.calibrationState.isCalibrating;
-    this._progress = this.manager.calibrationState.progress;
-    this._isCalibrated = this.manager.isCalibrated;
-    this._needsCalibration = this.manager.needsCalibration;
+  // Complete calibration and mark joints as calibrated
+  completeCalibration(): Record<string, number> {
+    console.log("[CalibrationState] Completing calibration...");
+    
+    const finalPositions: Record<string, number> = {};
     
-    // Update joint values and calibrations
-    this.manager.jointNames_.forEach(name => {
-      const currentValue = this.manager.getCurrentRawValue(name);
-      if (currentValue !== undefined) {
-        this._jointValues[name] = currentValue;
+    // Mark all joints with sufficient range as calibrated
+    Object.keys(this.jointCalibrations).forEach(jointName => {
+      const calibration = this.jointCalibrations[jointName];
+      if (calibration.minServoValue !== undefined && calibration.maxServoValue !== undefined) {
+        const range = calibration.maxServoValue - calibration.minServoValue;
+        if (range >= ROBOT_CONFIG.calibration.minRangeThreshold) {
+          calibration.isCalibrated = true;
+          finalPositions[jointName] = this.currentValues[jointName] || 0;
+          console.log(`[CalibrationState] Joint ${jointName} calibrated: ${this.getJointRange(jointName)} (range: ${range})`);
+        } else {
+          console.warn(`[CalibrationState] Joint ${jointName} range too small: ${range} < ${ROBOT_CONFIG.calibration.minRangeThreshold}`);
+        }
       }
-      
-      const calibration = this.manager.getJointCalibration(name);
-      if (calibration) {
-        // Create new object to ensure reactivity
-        this._jointCalibrations[name] = { ...calibration };
+    });
+
+    this.isCalibrating = false;
+    this.progress = 100;
+    
+    // Notify completion callbacks
+    this.completionCallbacks.forEach(callback => {
+      try {
+        callback(finalPositions);
+      } catch (error) {
+        console.error("[CalibrationState] Error in completion callback:", error);
       }
-    }); 
+    });
+    
+    return finalPositions;
   }
 
-  // Cleanup method
-  destroy(): void {
-    if (this.updateInterval) {
-      clearInterval(this.updateInterval);
-      this.updateInterval = null;
+  // Cancel calibration
+  cancelCalibration(): void {
+    console.log("[CalibrationState] Calibration cancelled");
+    this.isCalibrating = false;
+    this.progress = 0;
+    
+    // Reset calibration data
+    Object.keys(this.jointCalibrations).forEach(jointName => {
+      this.jointCalibrations[jointName] = {
+        isCalibrated: false,
+        minServoValue: undefined,
+        maxServoValue: undefined
+      };
+    });
+  }
+
+  // Skip calibration (use predefined values)
+  skipCalibration(): void {
+    console.log("[CalibrationState] Skipping calibration with predefined values");
+    
+    // Set predefined calibration values for SO-100 arm
+    const predefinedCalibrations = {
+      "Rotation": { minServoValue: 500, maxServoValue: 3500, isCalibrated: true },
+      "Pitch": { minServoValue: 500, maxServoValue: 3500, isCalibrated: true },
+      "Elbow": { minServoValue: 500, maxServoValue: 3500, isCalibrated: true },
+      "Wrist_Pitch": { minServoValue: 500, maxServoValue: 3500, isCalibrated: true },
+      "Wrist_Roll": { minServoValue: 500, maxServoValue: 3500, isCalibrated: true },
+      "Jaw": { minServoValue: 1000, maxServoValue: 3000, isCalibrated: true }
+    };
+    
+    Object.entries(predefinedCalibrations).forEach(([jointName, calibration]) => {
+      this.jointCalibrations[jointName] = calibration;
+    });
+    
+    this.isCalibrating = false;
+    this.progress = 100;
+  }
+
+  // Convert raw servo value to normalized percentage (for USB INPUT - reading from servo)
+  normalizeValue(rawValue: number, jointName: string): number {
+    const calibration = this.jointCalibrations[jointName];
+    if (!calibration || !calibration.isCalibrated || 
+        calibration.minServoValue === undefined || calibration.maxServoValue === undefined) {
+      // No calibration, use appropriate default conversion
+      const isGripper = jointName.toLowerCase() === 'jaw' || jointName.toLowerCase() === 'gripper';
+      if (isGripper) {
+        return Math.max(0, Math.min(100, (rawValue / 4095) * 100));
+      } else {
+        return Math.max(-100, Math.min(100, ((rawValue - 2048) / 2048) * 100));
+      }
     }
-    if (this.managerUnsubscribe) {
-      this.managerUnsubscribe();
-      this.managerUnsubscribe = null;
+
+    const { minServoValue, maxServoValue } = calibration;
+    if (maxServoValue === minServoValue) return 0;
+
+    // Bound the input servo value to calibrated range
+    const bounded = Math.max(minServoValue, Math.min(maxServoValue, rawValue));
+    
+    const isGripper = jointName.toLowerCase() === 'jaw' || jointName.toLowerCase() === 'gripper';
+    if (isGripper) {
+      // Gripper: 0-100%
+      return ((bounded - minServoValue) / (maxServoValue - minServoValue)) * 100;
+    } else {
+      // Regular joint: -100 to +100%
+      return (((bounded - minServoValue) / (maxServoValue - minServoValue)) * 200) - 100;
     }
   }
-  
-  // Format servo value for display
-  formatServoValue(value: number | undefined): string {
-    return value !== undefined ? value.toString() : '---';
+
+  // Convert normalized percentage to raw servo value (for USB OUTPUT - writing to servo)
+  denormalizeValue(normalizedValue: number, jointName: string): number {
+    const calibration = this.jointCalibrations[jointName];
+    if (!calibration || !calibration.isCalibrated || 
+        calibration.minServoValue === undefined || calibration.maxServoValue === undefined) {
+      // No calibration, use appropriate default conversion
+      const isGripper = jointName.toLowerCase() === 'jaw' || jointName.toLowerCase() === 'gripper';
+      if (isGripper) {
+        return Math.round((normalizedValue / 100) * 4095);
+      } else {
+        return Math.round(2048 + (normalizedValue / 100) * 2048);
+      }
+    }
+
+    const { minServoValue, maxServoValue } = calibration;
+    const range = maxServoValue - minServoValue;
+
+    let normalizedRatio: number;
+    const isGripper = jointName.toLowerCase() === 'jaw' || jointName.toLowerCase() === 'gripper';
+    if (isGripper) {
+      // Gripper: 0-100% -> 0-1
+      normalizedRatio = Math.max(0, Math.min(1, normalizedValue / 100));
+    } else {
+      // Regular joint: -100 to +100% -> 0-1
+      normalizedRatio = Math.max(0, Math.min(1, (normalizedValue + 100) / 200));
+    }
+
+    return Math.round(minServoValue + normalizedRatio * range);
+  }
+
+  // Register callback for calibration completion with final positions
+  onCalibrationCompleteWithPositions(callback: (positions: Record<string, number>) => void): () => void {
+    this.completionCallbacks.push(callback);
+    
+    // Return unsubscribe function
+    return () => {
+      const index = this.completionCallbacks.indexOf(callback);
+      if (index >= 0) {
+        this.completionCallbacks.splice(index, 1);
+      }
+    };
+  }
+
+  // Update progress based on calibration coverage
+  private updateProgress(): void {
+    if (!this.isCalibrating) return;
+
+    let totalProgress = 0;
+    let jointCount = 0;
+
+    Object.values(this.jointCalibrations).forEach(calibration => {
+      jointCount++;
+      if (calibration.minServoValue !== undefined && calibration.maxServoValue !== undefined) {
+        const range = calibration.maxServoValue - calibration.minServoValue;
+        // Progress is based on range size (more range = more progress)
+        const jointProgress = Math.min(100, (range / ROBOT_CONFIG.calibration.minRangeThreshold) * 100);
+        totalProgress += jointProgress;
+      }
+    });
+
+    this.progress = jointCount > 0 ? totalProgress / jointCount : 0;
+  }
+
+  // Cleanup
+  destroy(): void {
+    this.completionCallbacks = [];
   }
 } 

src/lib/elements/robot/calibration/USBCalibrationManager.ts

@@ -1,544 +0,0 @@
-import type { JointCalibration, CalibrationState } from '../models.js';
-import { scsServoSDK } from "feetech.js";
-import { ROBOT_CONFIG } from '../config.js';
-
-export class USBCalibrationManager {
-  // Joint configuration
-  private readonly jointIds = [1, 2, 3, 4, 5, 6];
-  private readonly jointNames = ["Rotation", "Pitch", "Elbow", "Wrist_Pitch", "Wrist_Roll", "Jaw"];
-
-  // Calibration state
-  private jointCalibrations: Record<string, JointCalibration> = {};
-  private _calibrationState: CalibrationState = {
-    isCalibrating: false,
-    progress: 0
-  };
-
-  // Connection state for calibration
-  private isConnectedForCalibration = false;
-  private baudRate: number = 1000000;
-
-  // Calibration polling
-  private calibrationPollingAbortController: AbortController | null = null;
-  private lastPositions: Record<string, number> = {};
-  private calibrationCallbacks: (() => void)[] = [];
-
-  // Calibration completion callback with final positions
-  private calibrationCompleteCallback: ((finalPositions: Record<string, number>) => void) | null = null;
-
-  // Servo reading queue for calibration
-  private isReadingServos = false;
-  private readingQueue: Array<{
-    servoId: number;
-    resolve: (value: number) => void;
-    reject: (error: Error) => void;
-  }> = [];
-
-  constructor(baudRate: number = ROBOT_CONFIG.usb.baudRate) {
-    this.baudRate = baudRate;
-
-    // Initialize joint calibrations
-    this.jointNames.forEach(name => {
-      this.jointCalibrations[name] = { isCalibrated: false };
-    });
-  }
-
-  // Getters
-  get isCalibrated(): boolean {
-    return Object.values(this.jointCalibrations).every(cal => cal.isCalibrated);
-  }
-
-  get needsCalibration(): boolean {
-    return !this.isCalibrated;
-  }
-
-  get calibrationState(): CalibrationState {
-    return this._calibrationState;
-  }
-
-  get jointNames_(): string[] {
-    return [...this.jointNames];
-  }
-
-  // Connection management for calibration
-  async ensureConnectedForCalibration(): Promise<void> {
-    if (this.isConnectedForCalibration) {
-      console.log('[USBCalibrationManager] Already connected for calibration');
-      return;
-    }
-
-    try {
-      console.log('[USBCalibrationManager] Connecting SDK for calibration...');
-      await scsServoSDK.connect({ baudRate: this.baudRate });
-      this.isConnectedForCalibration = true;
-      console.log('[USBCalibrationManager] Connected successfully for calibration');
-    } catch (error) {
-      console.error('[USBCalibrationManager] Failed to connect SDK for calibration:', error);
-      throw error;
-    }
-  }
-
-  async disconnectFromCalibration(): Promise<void> {
-    if (!this.isConnectedForCalibration) return;
-
-    try {
-      await scsServoSDK.disconnect();
-      this.isConnectedForCalibration = false;
-      console.log('[USBCalibrationManager] Disconnected from calibration');
-    } catch (error) {
-      console.warn('[USBCalibrationManager] Failed to disconnect from calibration:', error);
-    }
-  }
-
-  // Check if the SDK is currently connected (for external use)
-  get isSDKConnected(): boolean {
-    return this.isConnectedForCalibration && scsServoSDK.isConnected();
-  }
-
-  // Calibration methods
-  async startCalibration(): Promise<void> {
-    if (this._calibrationState.isCalibrating) {
-      console.warn('[USBCalibrationManager] Calibration already in progress');
-      return;
-    }
-
-    console.log('[USBCalibrationManager] Starting calibration process');
-
-    // Ensure connection for calibration
-    await this.ensureConnectedForCalibration();
-
-    // Unlock all servos for calibration (allow manual movement)
-    console.log('[USBCalibrationManager] 🔓 Unlocking all servos for calibration...');
-    try {
-      await scsServoSDK.unlockServos(this.jointIds);
-      console.log('[USBCalibrationManager] ✅ All servos unlocked for manual movement during calibration');
-    } catch (error) {
-      console.warn('[USBCalibrationManager] Warning: Failed to unlock some servos for calibration:', error);
-    }
-
-    this._calibrationState = {
-      isCalibrating: true,
-      progress: 0
-    };
-
-    // Initialize calibrations with current values
-    this.jointCalibrations = {};
-    this.jointNames.forEach(name => {
-      const currentValue = this.lastPositions[name] || 2048;
-      this.jointCalibrations[name] = {
-        isCalibrated: false,
-        minServoValue: currentValue,
-        maxServoValue: currentValue
-      };
-    });
-
-    this.startCalibrationPolling();
-    this.notifyCalibrationChange();
-  }
-
-  async stopCalibration(): Promise<void> {
-    console.log('[USBCalibrationManager] Stopping calibration');
-
-    this._calibrationState = {
-      isCalibrating: false,
-      progress: 100
-    };
-
-    // Mark all joints as calibrated
-    this.jointNames.forEach(name => {
-      if (this.jointCalibrations[name]) {
-        this.jointCalibrations[name].isCalibrated = true;
-      }
-    });
-
-    this.stopCalibrationPolling();
-
-    // NEW: Read final positions and sync to virtual robot before locking
-    console.log('[USBCalibrationManager] 📍 Reading final servo positions for virtual robot sync...');
-    try {
-      const finalPositions = await this.readFinalPositionsAndSync();
-      console.log('[USBCalibrationManager] ✅ Final positions read and synced to virtual robot');
-
-      // Notify robot of calibration completion with final positions
-      if (this.calibrationCompleteCallback) {
-        this.calibrationCompleteCallback(finalPositions);
-      }
-    } catch (error) {
-      console.error('[USBCalibrationManager] Failed to read final positions:', error);
-    }
-
-    this.notifyCalibrationChange();
-
-    // Keep connection open - don't disconnect automatically
-    // The connection will be reused by USB drivers
-    console.log('[USBCalibrationManager] Calibration complete, keeping connection for drivers');
-  }
-
-  skipCalibration(): void {
-    console.log('[USBCalibrationManager] Skipping calibration, using full range');
-
-    // Set full range for all joints
-    this.jointNames.forEach(name => {
-      this.jointCalibrations[name] = {
-        isCalibrated: true,
-        minServoValue: 0,
-        maxServoValue: 4095
-      };
-    });
-
-    this._calibrationState = {
-      isCalibrating: false,
-      progress: 100
-    };
-
-    this.notifyCalibrationChange();
-  }
-
-  // NEW: Set predefined calibration values
-  async setPredefinedCalibration(): Promise<void> {
-    console.log('[USBCalibrationManager] Setting predefined calibration values');
-
-    // Ensure SDK connection for hardware access
-    await this.ensureConnectedForCalibration();
-
-    // Predefined calibration values based on known good robot configuration
-    const predefinedValues: Record<string, { min: number; max: number; current: number }> = {
-      "Rotation": { current: 2180, min: 764, max: 3388 },
-      "Pitch": { current: 1159, min: 1138, max: 3501 },
-      "Elbow": { current: 2874, min: 660, max: 2876 },
-      "Wrist_Pitch": { current: 2138, min: 762, max: 3075 },
-      "Wrist_Roll": { current: 2081, min: 154, max: 3995 },
-      "Jaw": { current: 2061, min: 2013, max: 3555 }
-    };
-
-    // Set calibration values for all joints
-    this.jointNames.forEach(name => {
-      const values = predefinedValues[name];
-      if (values) {
-        this.jointCalibrations[name] = {
-          isCalibrated: true,
-          minServoValue: values.min,
-          maxServoValue: values.max
-        };
-        // Set current position for reference
-        this.lastPositions[name] = values.current;
-      }
-    });
-
-    this._calibrationState = {
-      isCalibrating: false,
-      progress: 100
-    };
-
-    this.notifyCalibrationChange();
-    console.log('[USBCalibrationManager] Predefined calibration values applied successfully');
-  }
-
-  // NEW: Read final positions and prepare for sync
-  private async readFinalPositionsAndSync(): Promise<Record<string, number>> {
-    const finalPositions: Record<string, number> = {};
-
-    console.log('[USBCalibrationManager] Reading final positions from all servos...');
-
-    // Read all servo positions sequentially
-    for (let i = 0; i < this.jointIds.length; i++) {
-      const servoId = this.jointIds[i];
-      const jointName = this.jointNames[i];
-
-      try {
-        const position = await this.readServoPosition(servoId);
-        finalPositions[jointName] = position;
-        this.lastPositions[jointName] = position;
-
-        console.log(`[USBCalibrationManager] ${jointName} (servo ${servoId}): ${position} (raw) -> ${this.normalizeValue(position, jointName).toFixed(1)}% (normalized)`);
-      } catch (error) {
-        console.warn(`[USBCalibrationManager] Failed to read final position for ${jointName} (servo ${servoId}):`, error);
-        // Use last known position as fallback
-        finalPositions[jointName] = this.lastPositions[jointName] || 2048;
-      }
-    }
-
-    return finalPositions;
-  }
-
-  // NEW: Set callback for calibration completion with final positions
-  onCalibrationCompleteWithPositions(callback: (finalPositions: Record<string, number>) => void): () => void {
-    this.calibrationCompleteCallback = callback;
-    return () => {
-      this.calibrationCompleteCallback = null;
-    };
-  }
-
-  // Post-calibration servo locking methods
-  async lockServosForProduction(): Promise<void> {
-    if (!this.isSDKConnected) {
-      throw new Error('SDK not connected - cannot lock servos');
-    }
-
-    console.log('[USBCalibrationManager] 🔒 Locking all servos for production use (robot control)...');
-    try {
-      // Use the new lockServosForProduction function that both locks and enables torque
-      await scsServoSDK.lockServosForProduction(this.jointIds);
-      console.log('[USBCalibrationManager] ✅ All servos locked for production - robot is now controlled and cannot be moved manually');
-    } catch (error) {
-      console.error('[USBCalibrationManager] Failed to lock servos for production:', error);
-      throw error;
-    }
-  }
-
-  async keepServosUnlockedForConsumer(): Promise<void> {
-    if (!this.isSDKConnected) {
-      console.log('[USBCalibrationManager] SDK not connected - servos remain in current state');
-      return;
-    }
-
-    console.log('[USBCalibrationManager] 🔓 Keeping servos unlocked for consumer use (reading positions)...');
-    try {
-      // Ensure servos are unlocked for reading
-      await scsServoSDK.unlockServos(this.jointIds);
-      console.log('[USBCalibrationManager] ✅ All servos remain unlocked for consumer - can be moved manually and positions read');
-    } catch (error) {
-      console.warn('[USBCalibrationManager] Warning: Failed to ensure servos are unlocked for consumer:', error);
-    }
-  }
-
-  // Data access methods
-  getCurrentRawValue(jointName: string): number | undefined {
-    return this.lastPositions[jointName];
-  }
-
-  getJointCalibration(jointName: string): JointCalibration | undefined {
-    return this.jointCalibrations[jointName];
-  }
-
-  getJointRange(jointName: string): number {
-    const calibration = this.jointCalibrations[jointName];
-    if (!calibration?.minServoValue || !calibration?.maxServoValue) return 0;
-    return Math.abs(calibration.maxServoValue - calibration.minServoValue);
-  }
-
-  getAllCalibrations(): Record<string, JointCalibration> {
-    return { ...this.jointCalibrations };
-  }
-
-  // Value conversion methods
-  normalizeValue(servoValue: number, jointName: string): number {
-    const calibration = this.jointCalibrations[jointName];
-    const isGripper = jointName.toLowerCase() === 'jaw' || jointName.toLowerCase() === 'gripper';
-
-    if (!calibration?.isCalibrated || !calibration.minServoValue || !calibration.maxServoValue) {
-      if (isGripper) {
-        return (servoValue / 4095) * 100;
-      } else {
-        return ((servoValue - 2048) / 2048) * 100;
-      }
-    }
-
-    const { minServoValue, maxServoValue } = calibration;
-    if (maxServoValue === minServoValue) return 0;
-
-    const bounded = Math.max(minServoValue, Math.min(maxServoValue, servoValue));
-
-    if (isGripper) {
-      return ((bounded - minServoValue) / (maxServoValue - minServoValue)) * 100;
-    } else {
-      return (((bounded - minServoValue) / (maxServoValue - minServoValue)) * 200) - 100;
-    }
-  }
-
-  denormalizeValue(normalizedValue: number, jointName: string): number {
-    const calibration = this.jointCalibrations[jointName];
-    const isGripper = jointName.toLowerCase() === 'jaw' || jointName.toLowerCase() === 'gripper';
-
-    if (!calibration?.isCalibrated || !calibration.minServoValue || !calibration.maxServoValue) {
-      // No calibration, use appropriate default conversion
-      if (isGripper) {
-        return Math.round((normalizedValue / 100) * 4095);
-      } else {
-        return Math.round(2048 + (normalizedValue / 100) * 2048);
-      }
-    }
-
-    const { minServoValue, maxServoValue } = calibration;
-    let ratio: number;
-
-    if (isGripper) {
-      ratio = normalizedValue / 100;
-    } else {
-      ratio = (normalizedValue + 100) / 200;
-    }
-
-    const servoValue = minServoValue + ratio * (maxServoValue - minServoValue);
-    return Math.round(Math.max(minServoValue, Math.min(maxServoValue, servoValue)));
-  }
-
-  // Event handling
-  onCalibrationChange(callback: () => void): () => void {
-    this.calibrationCallbacks.push(callback);
-    return () => {
-      const index = this.calibrationCallbacks.indexOf(callback);
-      if (index >= 0) {
-        this.calibrationCallbacks.splice(index, 1);
-      }
-    };
-  }
-
-  // Format servo value for display
-  formatServoValue(value: number | undefined): string {
-    return value !== undefined ? value.toString() : '---';
-  }
-
-  // Cleanup
-  async destroy(): Promise<void> {
-    console.log('[USBCalibrationManager] 🧹 Destroying calibration manager...');
-
-    this.stopCalibrationPolling();
-
-    // Safely unlock all servos before disconnecting (best practice)
-    if (this.isSDKConnected) {
-      try {
-        console.log('[USBCalibrationManager] 🔓 Safely unlocking all servos before cleanup...');
-        await scsServoSDK.unlockServosForManualMovement(this.jointIds);
-        console.log('[USBCalibrationManager] ✅ All servos safely unlocked for manual movement');
-      } catch (error) {
-        console.warn('[USBCalibrationManager] Warning: Failed to safely unlock servos during cleanup:', error);
-      }
-    }
-
-    await this.disconnectFromCalibration();
-    this.calibrationCallbacks = [];
-    this.calibrationCompleteCallback = null;
-
-    console.log('[USBCalibrationManager] ✅ Calibration manager destroyed');
-  }
-
-  // Private methods
-  private async readServoPosition(servoId: number): Promise<number> {
-    return new Promise((resolve, reject) => {
-      this.readingQueue.push({ servoId, resolve, reject });
-      this.processReadingQueue();
-    });
-  }
-
-  private async processReadingQueue(): Promise<void> {
-    if (this.isReadingServos || this.readingQueue.length === 0) {
-      return;
-    }
-
-    this.isReadingServos = true;
-
-    try {
-      const batch = [...this.readingQueue];
-      this.readingQueue = [];
-
-      for (const { servoId, resolve, reject } of batch) {
-        try {
-          const position = await scsServoSDK.readPosition(servoId);
-          resolve(position);
-        } catch (error) {
-          reject(error instanceof Error ? error : new Error(`Failed to read servo ${servoId}`));
-        }
-
-        await new Promise(resolve => setTimeout(resolve, 5));
-      }
-    } finally {
-      this.isReadingServos = false;
-
-      if (this.readingQueue.length > 0) {
-        setTimeout(() => this.processReadingQueue(), 50);
-      }
-    }
-  }
-
-  private async startCalibrationPolling(): Promise<void> {
-    this.stopCalibrationPolling();
-
-    this.calibrationPollingAbortController = new AbortController();
-    const signal = this.calibrationPollingAbortController.signal;
-
-    console.log('[USBCalibrationManager] Starting calibration polling');
-
-    try {
-      while (!signal.aborted && this._calibrationState.isCalibrating) {
-        const readPromises = this.jointIds.map(async (servoId, i) => {
-          if (signal.aborted) return null;
-
-          const jointName = this.jointNames[i];
-
-          try {
-            const currentValue = await this.readServoPosition(servoId);
-            return { jointName, currentValue };
-          } catch (error) {
-            return null;
-          }
-        });
-
-        const results = await Promise.all(readPromises);
-        let hasUpdates = false;
-
-        results.forEach(result => {
-          if (!result) return;
-
-          const { jointName, currentValue } = result;
-          this.lastPositions[jointName] = currentValue;
-
-          const calibration = this.jointCalibrations[jointName];
-          if (calibration) {
-            if (currentValue < calibration.minServoValue!) {
-              calibration.minServoValue = currentValue;
-              hasUpdates = true;
-            }
-            if (currentValue > calibration.maxServoValue!) {
-              calibration.maxServoValue = currentValue;
-              hasUpdates = true;
-            }
-          }
-        });
-
-        if (hasUpdates) {
-          this.notifyCalibrationChange();
-        }
-
-        // Calculate progress
-        const totalRangeNeeded = 500;
-        let totalRangeDiscovered = 0;
-
-        this.jointNames.forEach(name => {
-          const calibration = this.jointCalibrations[name];
-          if (calibration?.minServoValue !== undefined && calibration?.maxServoValue !== undefined) {
-            totalRangeDiscovered += Math.abs(calibration.maxServoValue - calibration.minServoValue);
-          }
-        });
-
-        const newProgress = Math.min(100, (totalRangeDiscovered / (totalRangeNeeded * this.jointNames.length)) * 100);
-        if (Math.abs(newProgress - this._calibrationState.progress) > 1) {
-          this._calibrationState.progress = newProgress;
-          this.notifyCalibrationChange();
-        }
-
-        await new Promise(resolve => setTimeout(resolve, 10));
-      }
-    } catch (error) {
-      if (!signal.aborted) {
-        console.error('[USBCalibrationManager] Calibration polling error:', error);
-      }
-    }
-  }
-
-  private stopCalibrationPolling(): void {
-    if (this.calibrationPollingAbortController) {
-      this.calibrationPollingAbortController.abort();
-      this.calibrationPollingAbortController = null;
-    }
-  }
-
-  private notifyCalibrationChange(): void {
-    this.calibrationCallbacks.forEach(callback => {
-      try {
-        callback();
-      } catch (error) {
-        console.error('[USBCalibrationManager] Error in calibration callback:', error);
-      }
-    });
-  }
-} 

src/lib/elements/robot/calibration/USBCalibrationPanel.svelte

@@ -1,12 +1,10 @@
 <!-- USB Calibration Panel - Compact Modal Version -->
 <script lang="ts">
-	import type { USBCalibrationManager } from "./USBCalibrationManager.js";
-	import { CalibrationState } from "./CalibrationState.svelte.js";
 	import { Button } from "@/components/ui/button/index.js";
 	import { Badge } from "@/components/ui/badge/index.js";
 
 	interface Props {
-		calibrationManager: USBCalibrationManager;
+		calibrationManager: any; // USBServoDriver (Consumer or Producer)
 		connectionType?: "consumer" | "producer";
 		onCalibrationComplete?: () => void;
 		onCancel?: () => void;
@@ -22,14 +20,11 @@
 	// Joint names for reference
 	const jointNames = ["Rotation", "Pitch", "Elbow", "Wrist_Pitch", "Wrist_Roll", "Jaw"];
 
-	// Create reactive calibration state manager for high-performance updates
-	const calibrationState = new CalibrationState(calibrationManager);
-
 	// Reactive getters from the calibration state
-	const isCalibrating = $derived(calibrationState.isCalibrating);
-	const progress = $derived(calibrationState.progress);
-	const isCalibrated = $derived(calibrationState.isCalibrated);
-	const needsCalibration = $derived(calibrationState.needsCalibration);
+	const isCalibrating = $derived(calibrationManager.calibrationState.isCalibrating);
+	const progress = $derived(calibrationManager.calibrationState.progress);
+	const isCalibrated = $derived(calibrationManager.calibrationState.isCalibrated);
+	const needsCalibration = $derived(calibrationManager.calibrationState.needsCalibration);
 
 	// Connection type descriptions
 	const connectionInfo = $derived(
@@ -44,19 +39,13 @@
 				}
 	);
 
-	// Cleanup on component destruction
-	$effect(() => {
-		return () => {
-			calibrationState.destroy();
-		};
-	});
 
 	async function startCalibration() {
 		await calibrationManager.startCalibration();
 	}
 
-	async function stopCalibration() {
-		await calibrationManager.stopCalibration();
+	async function completeCalibration() {
+		await calibrationManager.completeCalibration();
 		if (onCalibrationComplete) {
 			onCalibrationComplete();
 		}
@@ -83,7 +72,7 @@
 
 	function handleCancel() {
 		if (isCalibrating) {
-			calibrationManager.stopCalibration();
+			calibrationManager.cancelCalibration();
 		}
 		if (onCancel) {
 			onCancel();
@@ -102,7 +91,7 @@
 					<Badge variant="secondary" class="text-xs">{Math.round(progress)}%</Badge>
 				</div>
 				<div class="flex gap-2">
-					<Button variant="default" size="sm" onclick={stopCalibration}>Complete</Button>
+					<Button variant="default" size="sm" onclick={completeCalibration}>Complete</Button>
 					<Button variant="outline" size="sm" onclick={handleCancel}>Cancel</Button>
 				</div>
 			</div>
@@ -119,20 +108,20 @@
 			<div class="max-h-48 overflow-y-auto">
 				<div class="grid grid-cols-2 gap-2">
 					{#each jointNames as jointName}
-						{@const currentValue = calibrationState.getCurrentValue(jointName)}
-						{@const calibration = calibrationState.getJointCalibration(jointName)}
+						{@const currentValue = calibrationManager.calibrationState.getCurrentValue(jointName)}
+						{@const calibration = calibrationManager.calibrationState.getJointCalibration(jointName)}
 
 						<div class="space-y-1 rounded bg-slate-700/50 p-2">
 							<div class="flex items-center justify-between">
 								<span class="text-xs font-medium text-slate-300">{jointName}</span>
 								<span class="font-mono text-xs text-green-400"
-									>{calibrationState.formatServoValue(currentValue)}</span
+									>{calibrationManager.calibrationState.formatServoValue(currentValue)}</span
 								>
 							</div>
 
 							<div class="flex justify-between text-xs text-slate-500">
-								<span>Min: {calibrationState.formatServoValue(calibration?.minServoValue)}</span>
-								<span>Max: {calibrationState.formatServoValue(calibration?.maxServoValue)}</span>
+								<span>Min: {calibrationManager.calibrationState.formatServoValue(calibration?.minServoValue)}</span>
+								<span>Max: {calibrationManager.calibrationState.formatServoValue(calibration?.maxServoValue)}</span>
 							</div>
 
 							{#if calibration?.minServoValue !== undefined && calibration?.maxServoValue !== undefined && currentValue !== undefined}
@@ -178,8 +167,8 @@
 			<div class="max-h-32 overflow-y-auto">
 				<div class="grid grid-cols-2 gap-1">
 					{#each jointNames as jointName}
-						{@const calibration = calibrationState.getJointCalibration(jointName)}
-						{@const range = calibrationState.getJointRange(jointName)}
+						{@const calibration = calibrationManager.calibrationState.getJointCalibration(jointName)}
+						{@const range = calibrationManager.calibrationState.getJointRange(jointName)}
 
 						<div class="flex items-center justify-between rounded bg-slate-700/30 p-2 text-xs">
 							<span class="font-medium text-slate-300">{jointName}</span>

src/lib/elements/robot/calibration/index.ts

@@ -1,4 +1,2 @@
-// Robot calibration exports
-export { USBCalibrationManager } from './USBCalibrationManager.js';
 export { CalibrationState } from './CalibrationState.svelte.js';
 export { default as USBCalibrationPanel } from './USBCalibrationPanel.svelte'; 

src/lib/elements/robot/components/ConnectionPanel.svelte

@@ -39,30 +39,34 @@
 		}
 	});
 
-	// Set up calibration completion callback with position sync
-	robot.calibrationManager.onCalibrationCompleteWithPositions((finalPositions) => {
-		console.log("[ConnectionPanel] Calibration complete, syncing robot to final positions");
-		robot.syncToCalibrationPositions(finalPositions);
-	});
+	// Find USB driver for calibration (if any)
+	function getUSBDriver(): any {
+		// Check consumer first
+		if (robot.consumer && 'calibrationState' in robot.consumer) {
+			return robot.consumer;
+		}
+		// Then check producers
+		return robot.producers.find(p => 'calibrationState' in p) || null;
+	}
 
 	async function connectUSBConsumer() {
 		try {
 			connecting = true;
 			error = null;
 
-			// Check if calibration is needed
-			if (robot.calibrationManager.needsCalibration) {
-				pendingUSBConnection = "consumer";
-				showUSBCalibration = true;
-				return; // Don't proceed with connection yet
-			}
-
+			// USB drivers handle their own calibration requirements
 			await robot.setConsumer({
 				type: "usb",
 				baudRate: 1000000
 			});
 		} catch (err) {
 			console.error("Failed to connect USB consumer:", err);
+			// Check if it's a calibration error
+			if (err instanceof Error && err.message.includes('calibration')) {
+				pendingUSBConnection = "consumer";
+				showUSBCalibration = true;
+				return;
+			}
 			error = err instanceof Error ? err.message : "Unknown error";
 		} finally {
 			connecting = false;
@@ -92,19 +96,19 @@
 			connecting = true;
 			error = null;
 
-			// Check if calibration is needed
-			if (robot.calibrationManager.needsCalibration) {
-				pendingUSBConnection = "producer";
-				showUSBCalibration = true;
-				return; // Don't proceed with connection yet
-			}
-
+			// USB drivers handle their own calibration requirements
 			await robot.addProducer({
 				type: "usb",
 				baudRate: 1000000
 			});
 		} catch (err) {
 			console.error("Failed to connect USB producer:", err);
+			// Check if it's a calibration error
+			if (err instanceof Error && err.message.includes('calibration')) {
+				pendingUSBConnection = "producer";
+				showUSBCalibration = true;
+				return;
+			}
 			error = err instanceof Error ? err.message : "Unknown error";
 		} finally {
 			connecting = false;
@@ -533,12 +537,18 @@
 					to software values.
 				</div>
 
-				<USBCalibrationPanel
-					calibrationManager={robot.calibrationManager}
-					connectionType={pendingUSBConnection || "consumer"}
-					{onCalibrationComplete}
-					onCancel={onCalibrationCancel}
-				/>
+				{#if getUSBDriver()}
+					<USBCalibrationPanel
+						calibrationManager={getUSBDriver()}
+						connectionType={pendingUSBConnection || "consumer"}
+						{onCalibrationComplete}
+						onCancel={onCalibrationCancel}
+					/>
+				{:else}
+					<div class="text-center text-slate-400">
+						No USB driver available for calibration
+					</div>
+				{/if}
 			</div>
 		</div>
 	{/if}

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

@@ -1,144 +1,70 @@
-import type { Consumer, ConnectionStatus, RobotCommand, USBDriverConfig } from '../models.js';
-import { USBCalibrationManager } from '../calibration/USBCalibrationManager.js';
-import { scsServoSDK } from "feetech.js";
+import type { Consumer, RobotCommand, ConnectionStatus } from '../models.js';
+import { USBServoDriver } from './USBServoDriver.js';
 import { ROBOT_CONFIG } from '../config.js';
 
-export class USBConsumer implements Consumer {
-  readonly id: string;
-  readonly name = 'USB Consumer';
-  readonly config: USBDriverConfig;
-  
-  private _status: ConnectionStatus = { isConnected: false };
-  private statusCallbacks: ((status: ConnectionStatus) => void)[] = [];
+export class USBConsumer extends USBServoDriver implements Consumer {
   private commandCallbacks: ((command: RobotCommand) => void)[] = [];
-  
-  // Listening state
-  private isListening = false;
-  private pollingAbortController: AbortController | null = null;
-  
-  // Joint configuration
-  private readonly jointIds = [1, 2, 3, 4, 5, 6];
-  private readonly jointNames = ["Rotation", "Pitch", "Elbow", "Wrist_Pitch", "Wrist_Roll", "Jaw"];
-  private lastPositions: Record<string, number> = {};
-  
-  // Shared calibration manager
-  private calibrationManager: USBCalibrationManager;
-  
-  // Servo reading queue
-  private isReadingServos = false;
-  private readingQueue: Array<{
-    servoId: number;
-    resolve: (value: number) => void;
-    reject: (error: Error) => void;
-  }> = [];
-  
-  // Error tracking for better backoff
-  private consecutiveErrors = 0;
-  private lastErrorTime = 0;
+  private pollingInterval: ReturnType<typeof setInterval> | null = null;
+  private lastPositions: Record<number, number> = {};
+  private errorCount = 0;
 
-  constructor(config: USBDriverConfig, calibrationManager: USBCalibrationManager) {
-    this.config = config;
-    this.calibrationManager = calibrationManager;
-    this.id = `usb-consumer-${Date.now()}`;
-  }
-
-  get status(): ConnectionStatus { 
-    return this._status; 
+  constructor(config: any) {
+    super(config, 'Consumer');
   }
 
   async connect(): Promise<void> {
-    if (this._status.isConnected) {
-      console.debug('[USBConsumer] Already connected');
-      return;
-    }
-
-    try {
-      console.debug('[USBConsumer] Connecting...');
-      
-      // Check if calibration is needed
-      if (this.calibrationManager.needsCalibration) {
-        throw new Error('USB Consumer requires calibration. Please complete calibration first.');
-      }
-
-      // Ensure the SDK is connected (reuse calibration connection if available)
-      if (!this.calibrationManager.isSDKConnected) {
-        console.debug('[USBConsumer] Establishing new SDK connection');
-        await scsServoSDK.connect({
-          baudRate: this.config.baudRate || ROBOT_CONFIG.usb.baudRate
-        });
-      } else {
-        console.debug('[USBConsumer] Reusing existing SDK connection from calibration');
-      }
-
-      // Ensure servos remain unlocked for consumer (reading positions)
-      console.debug('[USBConsumer] 🔓 Ensuring servos remain unlocked for position reading...');
-      await this.calibrationManager.keepServosUnlockedForConsumer();
-
-      this._status = { 
-        isConnected: true, 
-        lastConnected: new Date()
-      };
-      this.notifyStatusChange();
-      
-      console.debug('[USBConsumer] ✅ Connected successfully - servos unlocked for reading');
-    } catch (error) {
-      console.error('[USBConsumer] Connection failed:', error);
-      this._status = { 
-        isConnected: false, 
-        error: error instanceof Error ? error.message : 'Connection failed'
-      };
-      this.notifyStatusChange();
-      throw error;
-    }
+    // Connect to USB first (this triggers browser's device selection dialog)
+    await this.connectToUSB();
+    
+    // Unlock servos for manual movement (consumer mode)
+    await this.unlockAllServos();
+    
+    // Note: Calibration is checked when operations are actually needed
   }
 
   async disconnect(): Promise<void> {
-    if (this._status.isConnected) {
-      await this.stopListening();
-      console.debug('[USBConsumer] Disconnecting (keeping shared SDK connection)');
-      // Don't disconnect the SDK here - let calibration manager handle it
-      // This allows multiple USB drivers to share the same connection
-    }
-
-    this._status = { isConnected: false };
-    this.notifyStatusChange();
+    await this.stopListening();
+    await this.disconnectFromUSB();
   }
 
   async startListening(): Promise<void> {
-    if (this.isListening) {
-      console.warn('[USBConsumer] Already listening');
+    if (!this._status.isConnected || this.pollingInterval !== null) {
       return;
     }
 
-    this.isListening = true;
-    this.pollingAbortController = new AbortController();
+    if (!this.isCalibrated) {
+      throw new Error('Cannot start listening: not calibrated');
+    }
+
+    console.log(`[${this.name}] Starting position listening...`);
+    this.errorCount = 0;
     
-    console.debug('[USBConsumer] Starting continuous polling');
-    this.pollContinuously();
+    this.pollingInterval = setInterval(async () => {
+      try {
+        await this.pollAndBroadcastPositions();
+        this.errorCount = 0;
+      } catch (error) {
+        this.errorCount++;
+        console.warn(`[${this.name}] Polling error (${this.errorCount}):`, error);
+        
+        if (this.errorCount >= ROBOT_CONFIG.polling.maxPollingErrors) {
+          console.warn(`[${this.name}] Too many polling errors, slowing down...`);
+          await this.stopListening();
+          setTimeout(() => this.startListening(), ROBOT_CONFIG.polling.errorBackoffRate);
+        }
+      }
+    }, ROBOT_CONFIG.polling.consumerPollingRate);
   }
 
   async stopListening(): Promise<void> {
-    if (!this.isListening) return;
-    
-    this.isListening = false;
-    if (this.pollingAbortController) {
-      this.pollingAbortController.abort();
-      this.pollingAbortController = null;
+    if (this.pollingInterval !== null) {
+      clearInterval(this.pollingInterval);
+      this.pollingInterval = null;
+      console.log(`[${this.name}] Stopped position listening`);
     }
-    
-    console.debug('[USBConsumer] Stopped listening');
   }
 
-  // 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);
-      }
-    };
-  }
+  // Event handlers already in base class
 
   onCommand(callback: (command: RobotCommand) => void): () => void {
     this.commandCallbacks.push(callback);
@@ -151,122 +77,63 @@ export class USBConsumer implements Consumer {
   }
 
   // Private methods
-  private async readServoPosition(servoId: number): Promise<number> {
-    return new Promise((resolve, reject) => {
-      this.readingQueue.push({ servoId, resolve, reject });
-      this.processReadingQueue();
-    });
-  }
-
-  private async processReadingQueue(): Promise<void> {
-    if (this.isReadingServos || this.readingQueue.length === 0) {
+  private async pollAndBroadcastPositions(): Promise<void> {
+    if (!this.scsServoSDK || !this._status.isConnected) {
       return;
     }
 
-    this.isReadingServos = true;
-
     try {
-      const batch = [...this.readingQueue];
-      this.readingQueue = [];
-
-      for (const { servoId, resolve, reject } of batch) {
-        try {
-          const position = await scsServoSDK.readPosition(servoId);
-          resolve(position);
-        } catch (error) {
-          reject(error instanceof Error ? error : new Error(`Failed to read servo ${servoId}`));
-        }
-        
-        await new Promise(resolve => setTimeout(resolve, 5));
-      }
-    } finally {
-      this.isReadingServos = false;
+      // Read positions for all servos
+      const servoIds = Object.values(this.jointToServoMap);
+      const positions = await this.scsServoSDK.syncReadPositions(servoIds);
       
-      if (this.readingQueue.length > 0) {
-        setTimeout(() => this.processReadingQueue(), 50);
-      }
-    }
-  }
-
-  private async pollContinuously(): Promise<void> {
-    while (this.isListening && this._status.isConnected && !this.pollingAbortController?.signal.aborted) {
-      try {
-        const changes: { name: string; value: number }[] = [];
-
-        const readPromises = this.jointIds.map(async (servoId, i) => {
-          const jointName = this.jointNames[i];
-          try {
-            const position = await this.readServoPosition(servoId);
-            const lastPosition = this.lastPositions[jointName];
-            
-            if (position !== lastPosition) {
-              // Use calibration manager for normalization
-              const normalizedValue = this.calibrationManager.normalizeValue(position, jointName);
-              this.lastPositions[jointName] = position;
-              return { name: jointName, value: normalizedValue };
-            }
-          } catch (error) {
-            // Silent continue on read errors
-          }
-          return null;
-        });
-
-        const results = await Promise.all(readPromises);
+      const jointsWithChanges: { name: string; value: number }[] = [];
+      
+      // Check for position changes and convert to normalized values
+      Object.entries(this.jointToServoMap).forEach(([jointName, servoId]) => {
+        const currentPosition = positions.get(servoId);
+        const lastPosition = this.lastPositions[servoId];
         
-        results.forEach(result => {
-          if (result) {
-            changes.push(result);
-          }
-        });
-
-        if (changes.length > 0) {
-          const command: RobotCommand = {
-            joints: changes,
-            timestamp: Date.now()
-          };
-          this.notifyCommand(command);
+        if (currentPosition !== undefined && 
+            (lastPosition === undefined || 
+             Math.abs(currentPosition - lastPosition) > ROBOT_CONFIG.performance.jointUpdateThreshold)) {
           
-          // Reset error counter on successful read
-          this.consecutiveErrors = 0;
-        }
-
-        await new Promise(resolve => setTimeout(resolve, ROBOT_CONFIG.polling.consumerPollingRate));
-        
-      } catch (error) {
-        if (!this.pollingAbortController?.signal.aborted) {
-          console.error('[USBConsumer] Polling error:', error);
+          this.lastPositions[servoId] = currentPosition;
           
-          // Smart error backoff
-          this.consecutiveErrors++;
-          this.lastErrorTime = Date.now();
+          // Convert to normalized value using calibration (required)
+          const normalizedValue = this.normalizeValue(currentPosition, jointName);
           
-          const backoffTime = this.consecutiveErrors > ROBOT_CONFIG.polling.maxPollingErrors 
-            ? ROBOT_CONFIG.polling.errorBackoffRate * 3  // Longer backoff after many errors
-            : ROBOT_CONFIG.polling.errorBackoffRate;
-            
-          await new Promise(resolve => setTimeout(resolve, backoffTime));
+          jointsWithChanges.push({
+            name: jointName,
+            value: normalizedValue
+          });
         }
+      });
+      
+      // Broadcast changes if any
+      if (jointsWithChanges.length > 0) {
+        const command: RobotCommand = {
+          timestamp: Date.now(),
+          joints: jointsWithChanges
+        };
+        
+        this.notifyCommand(command);
       }
+      
+    } catch (error) {
+      throw error; // Re-throw for error handling in polling loop
     }
   }
 
-  private notifyStatusChange(): void {
-    this.statusCallbacks.forEach(callback => {
-      try {
-        callback(this._status);
-      } catch (error) {
-        console.error('[USBConsumer] Error in status callback:', error);
-      }
-    });
-  }
-
   private notifyCommand(command: RobotCommand): void {
     this.commandCallbacks.forEach(callback => {
       try {
         callback(command);
       } catch (error) {
-        console.error('[USBConsumer] Error in command callback:', error);
+        console.error(`[${this.name}] Error in command callback:`, error);
       }
     });
   }
+
+
 } 

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

@@ -1,204 +1,122 @@
 import type { Producer, ConnectionStatus, RobotCommand, USBDriverConfig } from '../models.js';
-import { USBCalibrationManager } from '../calibration/USBCalibrationManager.js';
-import { scsServoSDK } from "feetech.js";
 import { ROBOT_CONFIG } from '../config.js';
+import { USBServoDriver } from './USBServoDriver.js';
 
-export class USBProducer implements Producer {
-  readonly id: string;
-  readonly name = 'USB Producer';
-  readonly config: USBDriverConfig;
-  
-  private _status: ConnectionStatus = { isConnected: false };
-  private statusCallbacks: ((status: ConnectionStatus) => void)[] = [];
-  
-  // Joint configuration
-  private readonly jointIds = [1, 2, 3, 4, 5, 6];
-  private readonly jointNames = ["Rotation", "Pitch", "Elbow", "Wrist_Pitch", "Wrist_Roll", "Jaw"];
-  
-  // Shared calibration manager
-  private calibrationManager: USBCalibrationManager;
-  
-  // Serial command processing to prevent "Port is busy" errors
-  private commandQueue: Array<{ joints: Array<{ name: string; value: number }>, resolve: () => void, reject: (error: Error) => void }> = [];
+export class USBProducer extends USBServoDriver implements Producer {
+  private commandQueue: RobotCommand[] = [];
   private isProcessingCommands = false;
 
-  constructor(config: USBDriverConfig, calibrationManager: USBCalibrationManager) {
-    this.config = config;
-    this.calibrationManager = calibrationManager;
-    this.id = `usb-producer-${Date.now()}`;
-  }
-
-  get status(): ConnectionStatus { 
-    return this._status; 
+  constructor(config: any) {
+    super(config, 'Producer');
   }
 
   async connect(): Promise<void> {
-    if (this._status.isConnected) {
-      console.debug('[USBProducer] Already connected');
-      return;
-    }
-
-    try {
-      console.debug('[USBProducer] Connecting...');
-      
-      // Check if calibration is needed
-      if (this.calibrationManager.needsCalibration) {
-        throw new Error('USB Producer requires calibration. Please complete calibration first.');
-      }
-
-      // Ensure the SDK is connected (reuse calibration connection if available)
-      if (!this.calibrationManager.isSDKConnected) {
-        console.debug('[USBProducer] Establishing new SDK connection');
-        await scsServoSDK.connect({
-          baudRate: this.config.baudRate || ROBOT_CONFIG.usb.baudRate
-        });
-      } else {
-        console.debug('[USBProducer] Reusing existing SDK connection from calibration');
-      }
-
-      // Lock servos for production use (robot control)
-      console.debug('[USBProducer] 🔒 Locking servos for production use...');
-      await this.calibrationManager.lockServosForProduction();
-
-      this._status = { 
-        isConnected: true, 
-        lastConnected: new Date()
-      };
-      this.notifyStatusChange();
-      
-      console.debug('[USBProducer] ✅ Connected successfully - servos locked for robot control');
-    } catch (error) {
-      console.error('[USBProducer] Connection failed:', error);
-      this._status = { 
-        isConnected: false, 
-        error: error instanceof Error ? error.message : 'Connection failed'
-      };
-      this.notifyStatusChange();
-      throw error;
-    }
+    // 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> {
-    if (this._status.isConnected) {
-      console.debug('[USBProducer] 🔓 Disconnecting and unlocking servos...');
-      
-      try {
-        // Safely unlock servos when disconnecting (best practice)
-        if (this.calibrationManager.isSDKConnected) {
-          console.debug('[USBProducer] 🔓 Safely unlocking servos for manual movement...');
-          await scsServoSDK.unlockServosForManualMovement(this.jointIds);
-          console.debug('[USBProducer] ✅ Servos safely unlocked - can now be moved manually');
-        }
-      } catch (error) {
-        console.warn('[USBProducer] Warning: Failed to unlock servos during disconnect:', error);
-      }
-      
-      // Don't disconnect the SDK here - let calibration manager handle it
-      // This allows multiple USB drivers to share the same connection
-    }
+    // Stop command processing
+    this.isProcessingCommands = false;
+    this.commandQueue = [];
 
-    this._status = { isConnected: false };
-    this.notifyStatusChange();
-    console.debug('[USBProducer] ✅ Disconnected');
+    await this.disconnectFromUSB();
   }
 
   async sendCommand(command: RobotCommand): Promise<void> {
     if (!this._status.isConnected) {
-      throw new Error('Cannot send command: USB Producer not connected');
+      throw new Error(`[${this.name}] Cannot send command: not connected`);
+    }
+
+    if (!this.isCalibrated) {
+      throw new Error(`[${this.name}] Cannot send command: not calibrated`);
     }
 
-    console.debug(`[USBProducer] Queuing command:`, command);
+    // Add command to queue for processing
+    this.commandQueue.push(command);
     
-    // Queue command for serial processing
-    return new Promise((resolve, reject) => {
-      this.commandQueue.push({
-        joints: command.joints,
-        resolve,
-        reject
-      });
-      
-      // Start processing if not already running
+    // 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
-  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);
-      }
-    };
-  }
+  // Event handlers already in base class
 
   // Private methods
   private async processCommandQueue(): Promise<void> {
-    if (this.isProcessingCommands || this.commandQueue.length === 0) {
+    if (this.isProcessingCommands || !this._status.isConnected) {
       return;
     }
 
     this.isProcessingCommands = true;
 
-    try {
-      while (this.commandQueue.length > 0) {
-        const { joints, resolve, reject } = this.commandQueue.shift()!;
-        
+    while (this.commandQueue.length > 0 && this._status.isConnected) {
+      const command = this.commandQueue.shift();
+      if (command) {
         try {
-          // Process servos sequentially to prevent "Port is busy" errors
-          for (const jointCmd of joints) {
-            const jointIndex = this.jointNames.indexOf(jointCmd.name);
-            if (jointIndex >= 0) {
-              const servoId = this.jointIds[jointIndex];
-              const servoPosition = this.calibrationManager.denormalizeValue(jointCmd.value, jointCmd.name);
-              
-              await this.writeServoWithRetry(servoId, servoPosition, jointCmd.name);
-              
-              // Small delay between servo writes to prevent port conflicts
-              await new Promise(resolve => setTimeout(resolve, ROBOT_CONFIG.usb.servoWriteDelay));
-            }
-          }
-          
-          resolve();
+          await this.executeCommand(command);
         } catch (error) {
-          reject(error as Error);
+          console.error(`[${this.name}] Command execution failed:`, error);
+          // Continue processing other commands
         }
       }
-    } finally {
-      this.isProcessingCommands = false;
     }
+
+    this.isProcessingCommands = false;
   }
 
-  private async writeServoWithRetry(servoId: number, position: number, jointName: string): Promise<void> {
-    let lastError: Error | null = null;
-    
-    for (let attempt = 1; attempt <= ROBOT_CONFIG.usb.maxRetries; attempt++) {
-      try {
-        await scsServoSDK.writePositionUnlocked(servoId, position);
-        console.debug(`[USBProducer] ✅ ${jointName} (servo ${servoId}) -> ${position}`);
-        return; // Success!
-      } catch (error) {
-        lastError = error as Error;
-        console.warn(`[USBProducer] Attempt ${attempt}/${ROBOT_CONFIG.usb.maxRetries} failed for servo ${servoId}:`, error);
-        
-        if (attempt < ROBOT_CONFIG.usb.maxRetries) {
-          await new Promise(resolve => setTimeout(resolve, ROBOT_CONFIG.usb.retryDelay));
+  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;
     }
-    
-    // All retries failed
-    throw new Error(`Failed to write servo ${servoId} after ${ROBOT_CONFIG.usb.maxRetries} attempts: ${lastError?.message}`);
   }
 
-  private notifyStatusChange(): void {
-    this.statusCallbacks.forEach(callback => {
-      try {
-        callback(this._status);
-      } catch (error) {
-        console.error('[USBProducer] Error in status callback:', error);
-      }
-    });
-  }
+
 } 

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

@@ -1,4 +1,5 @@
 // Robot drivers exports
+export { USBServoDriver } from './USBServoDriver.js';
 export { USBConsumer } from './USBConsumer.js';
 export { USBProducer } from './USBProducer.js';
 export { RemoteConsumer } from './RemoteConsumer.js';

src/lib/elements/robot/index.ts

@@ -12,7 +12,6 @@ export * from './models.js';
 export * from './drivers/index.js';
 
 // Robot calibration (avoid naming conflicts with models)
-export { USBCalibrationManager } from './calibration/USBCalibrationManager.js';
 export { CalibrationState as CalibrationStateManager } from './calibration/CalibrationState.svelte.js';
 export { default as USBCalibrationPanel } from './calibration/USBCalibrationPanel.svelte';