feetech
Version:
javascript sdk for feetech servos
1,047 lines (967 loc) • 35 kB
JavaScript
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
} from "./feetech/scsservo_sdk.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,
} from "./feetech/scsservo_constants.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;
/**
* 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<{success: boolean, message: string}>} Object indicating connection status.
*/
export async function connect(options = {}) {
if (portHandler && portHandler.isOpen) {
return { success: true, message: "Already connected." };
}
const { baudRate = 1000000, protocolEnd = 0 } = options;
try {
portHandler = new PortHandler();
const portRequested = await portHandler.requestPort();
if (!portRequested) {
portHandler = null;
return { success: false, message: "Failed to select a serial port." };
}
portHandler.setBaudRate(baudRate);
const portOpened = await portHandler.openPort();
if (!portOpened) {
await portHandler.closePort(); // Ensure resources are released
portHandler = null;
return {
success: false,
message: `Failed to open port at baudrate ${baudRate}.`,
};
}
packetHandler = new PacketHandler(protocolEnd);
console.log(
`Connected to serial port at ${baudRate} baud, protocol end: ${protocolEnd}.`
);
return { success: true, message: "Connection successful." };
} catch (err) {
console.error("Error during connection:", err);
if (portHandler) {
try {
await portHandler.closePort();
} catch (closeErr) {
console.error("Error closing port after connection failure:", closeErr);
}
}
portHandler = null;
packetHandler = null;
return { success: false, message: `Connection failed: ${err.message}` };
}
}
/**
* Disconnects from the serial port.
* @returns {Promise<{success: boolean, message: string}>} Object indicating disconnection status.
*/
export async function disconnect() {
if (!portHandler || !portHandler.isOpen) {
return { success: true, message: "Already disconnected." };
}
try {
await portHandler.closePort();
portHandler = null;
packetHandler = null;
console.log("Disconnected from serial port.");
return { success: true, message: "Disconnection successful." };
} catch (err) {
console.error("Error during disconnection:", err);
// Attempt to nullify handlers even if close fails
portHandler = null;
packetHandler = null;
return { success: false, message: `Disconnection failed: ${err.message}` };
}
}
/**
* Reads the current position of a servo.
* @param {number} servoId - The ID of the servo (1-252).
* @returns {Promise<{position: number|null, result: number, error: number}>} Object containing position (0-4095) or null, communication result, and error code.
*/
export async function readPosition(servoId) {
if (!portHandler || !packetHandler) {
console.error("Not connected. Call connect() first.");
return { position: null, result: -10, error: 0 }; // Custom result code for not connected
}
try {
const [position, result, error] = await packetHandler.read2ByteTxRx(
portHandler,
servoId,
ADDR_SCS_PRESENT_POSITION
);
if (result !== COMM_SUCCESS) {
console.error(
`Error reading position from servo ${servoId}: ${packetHandler.getTxRxResult(
result
)}, Error code: ${error}`
);
return { position: null, result, error };
}
// Position is read as 2 bytes, SCS_MAKEWORD combines them correctly internally
return { position: position & 0xffff, result, error }; // Ensure it's within 16 bits
} catch (err) {
console.error(`Exception reading position from servo ${servoId}:`, err);
return { position: null, result: -11, error: 0 }; // Custom result code for exception
}
}
/**
* Reads the current baud rate index of a servo.
* @param {number} servoId - The ID of the servo (1-252).
* @returns {Promise<{baudRateIndex: number|null, result: number, error: number}>} Object containing baud rate index (0-7) or null, communication result, and error code.
*/
export async function readBaudRate(servoId) {
if (!portHandler || !packetHandler) {
console.error("Not connected. Call connect() first.");
return { baudRateIndex: null, result: -10, error: 0 }; // Custom result code for not connected
}
try {
const [baudIndex, result, error] = await packetHandler.read1ByteTxRx(
portHandler,
servoId,
ADDR_SCS_BAUD_RATE
);
if (result !== COMM_SUCCESS) {
console.error(
`Error reading baud rate from servo ${servoId}: ${packetHandler.getTxRxResult(
result
)}, Error code: ${error}`
);
return { baudRateIndex: null, result, error };
}
// Baud rate index is read as 1 byte
return { baudRateIndex: baudIndex, result, error };
} catch (err) {
console.error(`Exception reading baud rate from servo ${servoId}:`, err);
return { baudRateIndex: null, result: -11, error: 0 }; // Custom result code for exception
}
}
/**
* Reads the current operating mode of a servo.
* @param {number} servoId - The ID of the servo (1-252).
* @returns {Promise<{mode: number|null, result: number, error: number}>} Object containing mode (0 for position, 1 for wheel) or null, communication result, and error code.
*/
export async function readMode(servoId) {
if (!portHandler || !packetHandler) {
console.error("Not connected. Call connect() first.");
return { mode: null, result: -10, error: 0 }; // Custom result code for not connected
}
try {
const [modeValue, result, error] = await packetHandler.read1ByteTxRx(
portHandler,
servoId,
ADDR_SCS_MODE
);
if (result !== COMM_SUCCESS) {
console.error(
`Error reading mode from servo ${servoId}: ${packetHandler.getTxRxResult(
result
)}, Error code: ${error}`
);
return { mode: null, result, error };
}
// Mode is read as 1 byte (0 or 1 typically)
return { mode: modeValue, result, error };
} catch (err) {
console.error(`Exception reading mode from servo ${servoId}:`, err);
return { mode: null, result: -11, error: 0 }; // Custom result code for exception
}
}
/**
* Writes a target position to a servo.
* @param {number} servoId - The ID of the servo (1-252).
* @param {number} position - The target position value (0-4095).
* @returns {Promise<{result: number, error: number}>} Object containing communication result and error code.
*/
export async function writePosition(servoId, position) {
if (!portHandler || !packetHandler) {
console.error("Not connected. Call connect() first.");
return { result: -10, error: 0 };
}
try {
// Clamp position to valid range
const clampedPosition = Math.max(0, Math.min(4095, Math.round(position)));
// write2ByteTxRx handles splitting into LOBYTE and HIBYTE
const [result, error] = await packetHandler.write2ByteTxRx(
portHandler,
servoId,
ADDR_SCS_GOAL_POSITION,
clampedPosition
);
console.log(result, error);
if (result !== COMM_SUCCESS) {
console.error(
`Error writing position to servo ${servoId}: ${packetHandler.getTxRxResult(
result
)}, Error code: ${error}`
);
}
return { result, error };
} catch (err) {
console.error(`Exception writing position to servo ${servoId}:`, err);
return { result: -11, error: 0 };
}
}
/**
* Enables or disables the torque of a servo.
* @param {number} servoId - The ID of the servo (1-252).
* @param {boolean} enable - True to enable torque, false to disable.
* @returns {Promise<{result: number, error: number}>} Object containing communication result and error code.
*/
export async function writeTorqueEnable(servoId, enable) {
if (!portHandler || !packetHandler) {
console.error("Not connected. Call connect() first.");
return { result: -10, error: 0 };
}
try {
const enableValue = enable ? 1 : 0;
const [result, error] = await packetHandler.write1ByteTxRx(
portHandler,
servoId,
ADDR_SCS_TORQUE_ENABLE,
enableValue
);
if (result !== COMM_SUCCESS) {
console.error(
`Error setting torque for servo ${servoId}: ${packetHandler.getTxRxResult(
result
)}, Error code: ${error}`
);
}
return { result, error };
} catch (err) {
console.error(`Exception setting torque for servo ${servoId}:`, err);
return { result: -11, error: 0 };
}
}
/**
* Sets the acceleration profile for a servo's movement.
* @param {number} servoId - The ID of the servo (1-252).
* @param {number} acceleration - The acceleration value (0-254).
* @returns {Promise<{result: number, error: number}>} Object containing communication result and error code.
*/
export async function writeAcceleration(servoId, acceleration) {
if (!portHandler || !packetHandler) {
console.error("Not connected. Call connect() first.");
return { result: -10, error: 0 };
}
try {
// Clamp acceleration to valid range
const clampedAcceleration = Math.max(
0,
Math.min(254, Math.round(acceleration))
);
const [result, error] = await packetHandler.write1ByteTxRx(
portHandler,
servoId,
ADDR_SCS_GOAL_ACC,
clampedAcceleration
);
if (result !== COMM_SUCCESS) {
console.error(
`Error writing acceleration to servo ${servoId}: ${packetHandler.getTxRxResult(
result
)}, Error code: ${error}`
);
}
return { result, error };
} catch (err) {
console.error(`Exception writing acceleration to servo ${servoId}:`, err);
return { result: -11, error: 0 };
}
}
/**
* Sets a servo to wheel mode (continuous rotation).
* Requires unlocking, setting mode, and locking the configuration.
* @param {number} servoId - The ID of the servo (1-252).
* @returns {Promise<{result: number, error: number}>} Object containing the overall result and the first error encountered.
*/
export async function setWheelMode(servoId) {
if (!portHandler || !packetHandler) {
console.error("Not connected. Call connect() first.");
return { result: -10, error: 0 };
}
let finalResult = COMM_SUCCESS;
let firstError = 0;
try {
console.log(`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) {
console.error(
`Failed to unlock servo ${servoId}: ${packetHandler.getTxRxResult(
resUnlock
)}, Error: ${errUnlock}`
);
return { result: resUnlock, error: errUnlock };
}
// 2. Set mode to 1 (Wheel/Speed mode)
const [resMode, errMode] = await packetHandler.write1ByteTxRx(
portHandler,
servoId,
ADDR_SCS_MODE,
1
);
if (resMode !== COMM_SUCCESS) {
console.error(
`Failed to set wheel mode for servo ${servoId}: ${packetHandler.getTxRxResult(
resMode
)}, Error: ${errMode}`
);
// Attempt to re-lock even if mode setting failed
await packetHandler.write1ByteTxRx(
portHandler,
servoId,
ADDR_SCS_LOCK,
1
);
return { result: resMode, error: errMode };
}
// 3. Lock servo configuration
const [resLock, errLock] = await packetHandler.write1ByteTxRx(
portHandler,
servoId,
ADDR_SCS_LOCK,
1
);
if (resLock !== COMM_SUCCESS) {
console.error(
`Failed to lock servo ${servoId} after setting mode: ${packetHandler.getTxRxResult(
resLock
)}, Error: ${errLock}`
);
// Return the lock error as the primary issue now
return { result: resLock, error: errLock };
}
console.log(`Successfully set servo ${servoId} to wheel mode.`);
return { result: COMM_SUCCESS, error: 0 };
} catch (err) {
console.error(`Exception setting wheel mode for servo ${servoId}:`, err);
// Attempt to re-lock in case of exception after unlock
try {
await packetHandler.write1ByteTxRx(
portHandler,
servoId,
ADDR_SCS_LOCK,
1
);
} catch (lockErr) {
console.error(
`Failed to re-lock servo ${servoId} after exception:`,
lockErr
);
}
return { result: -11, error: 0 };
}
}
/**
* Sets a servo back to position control mode from wheel mode.
* Requires unlocking, setting mode, and locking the configuration.
* @param {number} servoId - The ID of the servo (1-252).
* @returns {Promise<{result: number, error: number}>} Object containing the overall result and the first error encountered.
*/
export async function setPositionMode(servoId) {
if (!portHandler || !packetHandler) {
console.error("Not connected. Call connect() first.");
return { result: -10, error: 0 };
}
let finalResult = COMM_SUCCESS;
let firstError = 0;
try {
console.log(`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) {
console.error(
`Failed to unlock servo ${servoId}: ${packetHandler.getTxRxResult(
resUnlock
)}, Error: ${errUnlock}`
);
return { result: resUnlock, error: errUnlock };
}
// 2. Set mode to 0 (Position/Servo mode)
const [resMode, errMode] = await packetHandler.write1ByteTxRx(
portHandler,
servoId,
ADDR_SCS_MODE,
0 // 0 for position mode
);
if (resMode !== COMM_SUCCESS) {
console.error(
`Failed to set position mode for servo ${servoId}: ${packetHandler.getTxRxResult(
resMode
)}, Error: ${errMode}`
);
// Attempt to re-lock even if mode setting failed
await packetHandler.write1ByteTxRx(
portHandler,
servoId,
ADDR_SCS_LOCK,
1
);
return { result: resMode, error: errMode };
}
// 3. Lock servo configuration
const [resLock, errLock] = await packetHandler.write1ByteTxRx(
portHandler,
servoId,
ADDR_SCS_LOCK,
1
);
if (resLock !== COMM_SUCCESS) {
console.error(
`Failed to lock servo ${servoId} after setting mode: ${packetHandler.getTxRxResult(
resLock
)}, Error: ${errLock}`
);
// Return the lock error as the primary issue now
return { result: resLock, error: errLock };
}
console.log(`Successfully set servo ${servoId} back to position mode.`);
return { result: COMM_SUCCESS, error: 0 };
} catch (err) {
console.error(`Exception setting position mode for servo ${servoId}:`, err);
// Attempt to re-lock in case of exception after unlock
try {
await packetHandler.write1ByteTxRx(
portHandler,
servoId,
ADDR_SCS_LOCK,
1
);
} catch (lockErr) {
console.error(
`Failed to re-lock servo ${servoId} after exception:`,
lockErr
);
}
return { result: -11, error: 0 };
}
}
/**
* 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 (-2500 to 2500). Negative values indicate reverse direction. 0 stops the wheel.
* @returns {Promise<{result: number, error: number}>} Object containing communication result and the combined error code from writing bytes.
*/
export async function writeWheelSpeed(servoId, speed) {
if (!portHandler || !packetHandler) {
console.error("Not connected. Call connect() first.");
return { result: -10, error: 0 };
}
try {
// Clamp absolute speed and determine direction bit
const absSpeed = Math.min(Math.abs(Math.round(speed)), 2500); // Max speed seems higher for wheels in example
let speedValue = absSpeed & 0x7fff; // Lower 15 bits are speed magnitude
if (speed < 0) {
speedValue |= 0x8000; // Set bit 15 for reverse direction
}
console.log(
`Setting wheel servo ${servoId} speed to ${speed} (raw: 0x${speedValue.toString(
16
)})`
);
// Write speed using two 1-byte writes as in robotControls.js
const lowByte = SCS_LOBYTE(speedValue); // Correctly gets low byte based on SCS_END
const highByte = SCS_HIBYTE(speedValue); // Correctly gets high byte based on SCS_END
// Write low byte to ADDR_SCS_GOAL_SPEED (46)
const [resultLow, errorLow] = await packetHandler.write1ByteTxRx(
portHandler,
servoId,
ADDR_SCS_GOAL_SPEED,
lowByte
);
if (resultLow !== COMM_SUCCESS) {
console.error(
`Error writing wheel speed low byte to servo ${servoId}: ${packetHandler.getTxRxResult(
resultLow
)}, Error: ${errorLow}`
);
return { result: resultLow, error: errorLow };
}
// Write high byte to ADDR_SCS_GOAL_SPEED + 1 (47)
const [resultHigh, errorHigh] = await packetHandler.write1ByteTxRx(
portHandler,
servoId,
ADDR_SCS_GOAL_SPEED + 1, // Address 47
highByte
);
if (resultHigh !== COMM_SUCCESS) {
console.error(
`Error writing wheel speed high byte to servo ${servoId}: ${packetHandler.getTxRxResult(
resultHigh
)}, Error: ${errorHigh}`
);
// Even if high byte fails, low byte write might have partially succeeded. Return high byte error.
return { result: resultHigh, error: errorHigh };
}
// Combine results/errors (report success only if both writes succeed)
const finalResult =
resultLow === COMM_SUCCESS && resultHigh === COMM_SUCCESS
? COMM_SUCCESS
: resultHigh; // Report last error status
const combinedError = errorLow | errorHigh; // Combine error flags
if (finalResult !== COMM_SUCCESS) {
console.error(
`Overall error writing wheel speed for servo ${servoId}. Result: ${finalResult}, Combined Error: ${combinedError}`
);
}
return { result: finalResult, error: combinedError };
} catch (err) {
console.error(`Exception writing wheel speed to servo ${servoId}:`, err);
return { result: -11, error: 0 };
}
}
/**
* Reads the current position of multiple servos synchronously.
* @param {number[]} servoIds - An array of servo IDs (1-252) to read from.
* @returns {Promise<{positions: Map<number, number|null>, results: Map<number, number>, errors: Map<number, number>, overallResult: number}>}
* Object containing:
* - positions: A Map where keys are servo IDs and values are positions (0-4095) or null if read failed for that servo.
* - results: A Map of communication results per servo.
* - errors: A Map of error codes per servo (Note: Per-servo hardware errors might not be directly available with sync read, errors map primarily reflects communication success/failure).
* - overallResult: The result of the sync read transmission and reception phases.
*/
export async function syncReadPositions(servoIds) {
if (!portHandler || !packetHandler) {
console.error("Not connected. Call connect() first.");
return {
positions: new Map(),
results: new Map(),
errors: new Map(),
overallResult: -10, // Custom code for not connected
};
}
if (!Array.isArray(servoIds) || servoIds.length === 0) {
return {
positions: new Map(),
results: new Map(),
errors: new Map(),
overallResult: COMM_SUCCESS, // Nothing to read is considered success
};
}
const startAddress = ADDR_SCS_PRESENT_POSITION;
const dataLength = 2; // Reading 2 bytes for position
const groupSyncRead = new GroupSyncRead(
portHandler,
packetHandler,
startAddress,
dataLength
);
const positions = new Map();
const results = new Map();
const errors = new Map();
let overallResult = COMM_TX_FAIL; // Default to failure
// 1. Add parameters for each servo ID
let validIdsAdded = 0;
servoIds.forEach((id) => {
if (id >= 1 && id <= 252) {
// Check valid ID range
const success = groupSyncRead.addParam(id);
if (success) {
positions.set(id, null); // Initialize position as null
results.set(id, COMM_RX_TIMEOUT); // Default to timeout until data received
errors.set(id, 0);
validIdsAdded++;
} else {
console.warn(
`Sync Read: Failed to add param for servo ID ${id} (maybe duplicate or invalid).`
);
// Optionally track failed adds if needed
}
} else {
console.warn(`Sync Read: Invalid servo ID ${id} skipped.`);
}
});
if (validIdsAdded === 0) {
console.log("Sync Read: No valid servo IDs to read.");
return { positions, results, errors, overallResult: COMM_NOT_AVAILABLE }; // No valid IDs added
}
try {
// 2. Send the Sync Read instruction packet
overallResult = await groupSyncRead.txPacket();
if (overallResult !== COMM_SUCCESS) {
console.error(
`Sync Read txPacket failed: ${packetHandler.getTxRxResult(
overallResult
)}`
);
// Mark all added servos as failed due to TX error
groupSyncRead.isAvailableServiceID.forEach((id) =>
results.set(id, overallResult)
);
return { positions, results, errors, overallResult };
}
// 3. Receive the response packets (GroupSyncRead handles this internally)
// This rxPacket call triggers reading and parsing for all servos in the group.
overallResult = await groupSyncRead.rxPacket();
if (overallResult !== COMM_SUCCESS) {
// This might indicate a general reception error (e.g., timeout waiting for the first byte)
// Individual servo results will still be checked below using isAvailable.
console.warn(
`Sync Read rxPacket overall result: ${packetHandler.getTxRxResult(
overallResult
)}. Checking individual servos.`
);
// Keep overallResult reflecting the rxPacket status, but proceed to check individual data.
}
// 4. Check data availability and retrieve data for each servo
groupSyncRead.isAvailableServiceID.forEach((id) => {
// Check if data for this servo ID was successfully received and parsed
const isAvailable = groupSyncRead.isAvailable(
id,
startAddress,
dataLength
);
if (isAvailable) {
// Data is available, retrieve it
const position = groupSyncRead.getData(id, startAddress, dataLength);
positions.set(id, position & 0xffff); // Ensure 16-bit value
results.set(id, COMM_SUCCESS);
errors.set(id, 0); // Assume no hardware error if data received (SDK might not provide specific error byte via getData)
// console.log(`Sync Read: Servo ${id} position: ${position}`);
} else {
// Data was not available for this servo (timeout, checksum error, etc.)
results.set(
id,
results.get(id) === COMM_RX_TIMEOUT
? COMM_RX_TIMEOUT
: COMM_RX_CORRUPT
); // Keep timeout or mark as corrupt/failed
// errors map could potentially store hardware errors if the SDK provided them, but typically not with sync read getData.
console.warn(`Sync Read: Data not available for servo ${id}.`);
}
});
// Check if any servo timed out that wasn't explicitly marked otherwise
let receivedCount = 0;
results.forEach((result) => {
if (result === COMM_SUCCESS) receivedCount++;
});
if (receivedCount < validIdsAdded) {
console.warn(
`Sync Read: Expected ${validIdsAdded} responses, received ${receivedCount}. Some servos may have timed out or failed.`
);
// The overallResult from rxPacket might already reflect timeout if nothing was received.
}
return { positions, results, errors, overallResult };
} catch (err) {
console.error("Exception during syncReadPositions:", err);
// Mark all added servos as failed due to exception
groupSyncRead.isAvailableServiceID.forEach((id) => results.set(id, -11)); // Custom code for exception
return { positions, results, errors, overallResult: -11 };
}
}
/**
* 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<{result: number}>} Object containing the communication result of the sync write transmission.
*/
export async function syncWritePositions(servoPositions) {
if (!portHandler || !packetHandler) {
console.error("Not connected. Call connect() first.");
return { result: -10 };
}
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) {
console.warn(
`Invalid servo ID "${idStr}" in syncWriteServoPositions. Skipping.`
);
continue;
}
const clampedPosition = Math.max(0, Math.min(4095, Math.round(position)));
const data = [SCS_LOBYTE(clampedPosition), SCS_HIBYTE(clampedPosition)];
const success = groupSyncWrite.addParam(servoId, data);
if (!success) {
console.warn(
`Failed to add servo ${servoId} to sync write group (possibly duplicate).`
);
} else {
paramAdded = true;
}
}
if (!paramAdded) {
console.log("Sync Write: No valid servo positions provided.");
return { result: COMM_SUCCESS }; // Nothing to write
}
try {
// Send the Sync Write instruction (no response expected)
const result = await groupSyncWrite.txPacket();
if (result !== COMM_SUCCESS) {
console.error(
`Sync Write txPacket failed: ${packetHandler.getTxRxResult(result)}`
);
}
return { result };
} catch (err) {
console.error("Exception during syncWritePositions:", err);
return { result: -11 };
}
}
/**
* 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).
* 0: 1000000, 1: 500000, 2: 250000, 3: 128000,
* 4: 115200, 5: 76800, 6: 57600, 7: 38400
* @returns {Promise<{result: number, error: number}>} Object containing the overall result and the first error encountered.
*/
export async function setBaudRate(servoId, baudRateIndex) {
// Reverted from writeBaudRate
if (!portHandler || !packetHandler) {
console.error("Not connected. Call connect() first.");
return { result: -10, error: 0 };
}
// Validate inputs
if (servoId < 1 || servoId > 252) {
console.error(
`Invalid servo ID provided: ${servoId}. Must be between 1 and 252.`
);
return { result: -12, error: 0 }; // Custom code for invalid input
}
if (baudRateIndex < 0 || baudRateIndex > 7) {
console.error(
`Invalid baudRateIndex: ${baudRateIndex}. Must be between 0 and 7.`
);
return { result: -12, error: 0 }; // Custom code for invalid input
}
try {
console.log(
`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) {
console.error(
`Failed to unlock servo ${servoId}: ${packetHandler.getTxRxResult(
resUnlock
)}, Error: ${errUnlock}`
);
return { result: resUnlock, error: errUnlock };
}
// 2. Write new Baud Rate index
const [resBaud, errBaud] = await packetHandler.write1ByteTxRx(
portHandler,
servoId,
ADDR_SCS_BAUD_RATE,
baudRateIndex
);
if (resBaud !== COMM_SUCCESS) {
console.error(
`Failed to write baud rate index ${baudRateIndex} to servo ${servoId}: ${packetHandler.getTxRxResult(
resBaud
)}, Error: ${errBaud}`
);
// Attempt to re-lock before returning
await packetHandler.write1ByteTxRx(
portHandler,
servoId,
ADDR_SCS_LOCK,
1
);
return { result: resBaud, error: errBaud };
}
// 3. Lock servo configuration
const [resLock, errLock] = await packetHandler.write1ByteTxRx(
portHandler,
servoId,
ADDR_SCS_LOCK,
1
);
if (resLock !== COMM_SUCCESS) {
console.error(
`Failed to lock servo ${servoId} after setting baud rate: ${packetHandler.getTxRxResult(
resLock
)}, Error: ${errLock}.`
);
// Return the lock error as the primary issue now
return { result: resLock, error: errLock };
}
console.log(
`Successfully set baud rate for servo ${servoId}. Index: ${baudRateIndex}. Remember to potentially reconnect with the new baud rate.`
);
return { result: COMM_SUCCESS, error: 0 };
} catch (err) {
console.error(`Exception during setBaudRate for servo ID ${servoId}:`, err); // Reverted error message context
// Attempt to re-lock in case of exception after unlock
try {
await packetHandler.write1ByteTxRx(
portHandler,
servoId,
ADDR_SCS_LOCK,
1
);
} catch (lockErr) {
console.error(
`Failed to re-lock servo ${servoId} after exception:`,
lockErr
);
}
return { result: -11, error: 0 }; // Custom code for exception
}
}
/**
* 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<{result: number, error: number}>} Object containing the overall result and the first error encountered.
*/
export async function setServoId(currentServoId, newServoId) {
// Reverted from writeId
if (!portHandler || !packetHandler) {
console.error("Not connected. Call connect() first.");
return { result: -10, error: 0 };
}
// Validate inputs
if (
currentServoId < 1 ||
currentServoId > 252 ||
newServoId < 1 ||
newServoId > 252
) {
console.error(
`Invalid servo ID provided. Current: ${currentServoId}, New: ${newServoId}. Must be between 1 and 252.`
);
return { result: -12, error: 0 }; // Custom code for invalid input
}
if (currentServoId === newServoId) {
console.log(`Servo ID is already ${newServoId}. No change needed.`);
return { result: COMM_SUCCESS, error: 0 };
}
try {
console.log(`Setting servo ID: From ${currentServoId} to ${newServoId}`);
// 1. Unlock servo configuration
const [resUnlock, errUnlock] = await packetHandler.write1ByteTxRx(
portHandler,
currentServoId,
ADDR_SCS_LOCK,
0 // 0 to unlock
);
if (resUnlock !== COMM_SUCCESS) {
console.error(
`Failed to unlock servo ${currentServoId}: ${packetHandler.getTxRxResult(
resUnlock
)}, Error: ${errUnlock}`
);
return { result: resUnlock, error: errUnlock };
}
// 2. Write new Servo ID
// IMPORTANT: Use the CURRENT ID to send this command. The ID change happens after this write.
const [resId, errId] = await packetHandler.write1ByteTxRx(
portHandler,
currentServoId,
ADDR_SCS_ID,
newServoId
);
if (resId !== COMM_SUCCESS) {
console.error(
`Failed to write new ID ${newServoId} to servo ${currentServoId}: ${packetHandler.getTxRxResult(
resId
)}, Error: ${errId}`
);
// Attempt to re-lock before returning (using current ID)
await packetHandler.write1ByteTxRx(
portHandler,
currentServoId,
ADDR_SCS_LOCK,
1
);
return { result: resId, error: errId };
}
// 3. Lock servo configuration
// IMPORTANT: Use the NEW ID to lock, as the ID change should have taken effect.
const [resLock, errLock] = await packetHandler.write1ByteTxRx(
portHandler,
newServoId, // Use NEW ID here
ADDR_SCS_LOCK,
1 // 1 to lock
);
if (resLock !== COMM_SUCCESS) {
console.error(
`Failed to lock servo with new ID ${newServoId}: ${packetHandler.getTxRxResult(
resLock
)}, Error: ${errLock}. Configuration might be incomplete.`
);
// Return the lock error as the primary issue now
return { result: resLock, error: errLock };
}
console.log(
`Successfully set servo ID from ${currentServoId} to ${newServoId}. Remember to use the new ID for future commands.`
);
return { result: COMM_SUCCESS, error: 0 };
} catch (err) {
console.error(
`Exception during setServoId for current ID ${currentServoId}:`, // Reverted error message context
err
);
// Attempt to re-lock in case of exception after unlock, using the *current* ID
// as we don't know if the ID change was successful.
try {
await packetHandler.write1ByteTxRx(
portHandler,
currentServoId,
ADDR_SCS_LOCK,
1
);
} catch (lockErr) {
console.error(
`Failed to re-lock servo ${currentServoId} after exception:`,
lockErr
);
}
return { result: -11, error: 0 }; // Custom code for exception
}
}