packages/feetech.js/scsServoSDKUnlock.mjs

/**
 * Read-Only Servo SDK for USB Master
 * Simplified version that only reads positions without any locking
 */

import { PortHandler, PacketHandler, COMM_SUCCESS, GroupSyncRead } from "./lowLevelSDK.mjs";

import { ADDR_SCS_PRESENT_POSITION } from "./scsservo_constants.mjs";

// Module-level variables for handlers
let portHandler = null;
let packetHandler = null;

/**
 * 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) {
		console.log("🔓 Already connected to USB robot (read-only mode).");
		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);
		console.log(
			`🔓 Connected to USB robot (read-only mode) at ${baudRate} baud, protocol end: ${protocolEnd}.`
		);
		return true;
	} catch (err) {
		console.error("Error during USB robot 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(`USB robot 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) {
		console.log("Already disconnected from USB robot.");
		return true;
	}

	try {
		await portHandler.closePort();
		portHandler = null;
		packetHandler = null;
		console.log("🔓 Disconnected from USB robot (read-only mode).");
		return true;
	} catch (err) {
		console.error("Error during USB robot disconnection:", err);
		portHandler = null;
		packetHandler = null;
		throw new Error(`USB robot disconnection failed: ${err.message}`);
	}
}

/**
 * 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 USB robot. Call connect() first.");
	}
}

/**
 * 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 position of multiple servos synchronously.
 * Returns positions for all servos that respond, skipping failed ones gracefully.
 * @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) {
		console.log("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) {
		console.log("Sync Read: No valid servo IDs to read.");
		return new Map();
	}

	try {
		// Send the Sync Read instruction packet
		let txResult = await groupSyncRead.txPacket();
		if (txResult !== COMM_SUCCESS) {
			throw new Error(`Sync Read txPacket failed: ${packetHandler.getTxRxResult(txResult)}`);
		}

		// Receive the response packets
		let rxResult = await groupSyncRead.rxPacket();
		if (rxResult !== COMM_SUCCESS) {
			console.warn(
				`Sync Read rxPacket overall result: ${packetHandler.getTxRxResult(
					rxResult
				)}. Checking individual servos.`
			);
		}

		// Check data availability and retrieve data for each servo
		const failedIds = [];
		validIds.forEach((id) => {
			const isAvailable = groupSyncRead.isAvailable(id, startAddress, dataLength);
			if (isAvailable) {
				const position = groupSyncRead.getData(id, startAddress, dataLength);
				const finalPosition = position & 0xffff;
				console.log(
					`🔍 Debug Servo ${id}: raw=${position}, final=${finalPosition}, hex=${position.toString(16)}`
				);
				positions.set(id, finalPosition);
			} else {
				failedIds.push(id);
			}
		});

		// Log failed servos but don't throw error - return available data
		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}`);
	}
}