From 6445575b12ab9a61519e5bce11c3a2dec4365b17 Mon Sep 17 00:00:00 2001 From: David Basabe Date: Tue, 19 May 2026 12:27:56 +0200 Subject: [PATCH 1/4] Updating retrieval and setting of joint angles according to motor steps and the joint limits in the SO101's new calib URDF --- examples/CalibrationOfs.cpp | 0 examples/robots/SO101_Calibration.cpp | 0 examples/robots/SO101_Home.cpp | 0 include/lerobot_cpp/robots/SO101.h | 20 ++++++++++++++++---- src/robots/RobotUtils.cpp | 6 ++++-- src/robots/SO101.cpp | 24 ++++++++++++------------ 6 files changed, 32 insertions(+), 18 deletions(-) mode change 100644 => 100755 examples/CalibrationOfs.cpp mode change 100644 => 100755 examples/robots/SO101_Calibration.cpp mode change 100644 => 100755 examples/robots/SO101_Home.cpp diff --git a/examples/CalibrationOfs.cpp b/examples/CalibrationOfs.cpp old mode 100644 new mode 100755 diff --git a/examples/robots/SO101_Calibration.cpp b/examples/robots/SO101_Calibration.cpp old mode 100644 new mode 100755 diff --git a/examples/robots/SO101_Home.cpp b/examples/robots/SO101_Home.cpp old mode 100644 new mode 100755 diff --git a/include/lerobot_cpp/robots/SO101.h b/include/lerobot_cpp/robots/SO101.h index bfc53d6..e72fca8 100644 --- a/include/lerobot_cpp/robots/SO101.h +++ b/include/lerobot_cpp/robots/SO101.h @@ -43,6 +43,18 @@ */ class SO101 { public: + + // Nominal limits from `so101_new_calib.urdf` (radians), used for logging and safe clamping. + // URDF mentions some 5° Offset in the joint between lower and upper arms (joint 3) + static constexpr std::array, 6> JOINT_LIMITS = {{ + {-1.91986f, 1.91986f}, + {-1.74533f, 1.74533f}, + {-1.69f, 1.69f}, //Joint 3 + {-1.65806f, 1.65806f}, + {-2.74385f, 2.84121f}, + {-0.174533f, 1.74533f} + }}; + /** * @brief Initialize SO101 robot controller * @param servoInstance Reference to an initialized STS3215 instance @@ -97,10 +109,10 @@ class SO101 { */ float getJointAngle(u8 jointIndex); - /** - * @brief Check if any joint of the robot is moving - * @return true if moving, false otherwise - */ + /** + * @brief Check if any joint of the robot is moving + * @return true if moving, false otherwise + */ bool isMoving(); /** diff --git a/src/robots/RobotUtils.cpp b/src/robots/RobotUtils.cpp index 752a6d8..d7dcfa0 100644 --- a/src/robots/RobotUtils.cpp +++ b/src/robots/RobotUtils.cpp @@ -7,12 +7,14 @@ namespace RobotUtils { s16 radToSteps(float rad) { - float steps = rad * (STEPS_PER_REV / (2.0f * M_PI)); + // Add pi offset (in motor steps) for proper midpoint of motor + float steps = rad * (STEPS_PER_REV / (2.0f * M_PI)) + 2047; return (s16)round(steps); } float stepsToRad(s16 steps) { - return (float)steps * RAD_PER_STEP; + // subtract pi offset (in motor steps) for proper midpoint of motor + return ((float)steps - 2047) * RAD_PER_STEP; } u16 radPerSToStepsPerS(float radPerS) { diff --git a/src/robots/SO101.cpp b/src/robots/SO101.cpp index 40c58d1..539df75 100644 --- a/src/robots/SO101.cpp +++ b/src/robots/SO101.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #ifndef M_PI #define M_PI 3.14159265358979323846 // Circular constant for calculations when not in math.h @@ -54,13 +55,12 @@ bool SO101::storeLimits(const std::array& minPositions, const std::array } int SO101::setJointAngle(u8 jointIndex, float angleRad, float speedRadPerS, float accRadPerS2) { + if (jointIndex >= servoIDs.size()) return 0; - - // Calculate steps relative to the midpoint of limits - // 0 rad = (min + max) / 2 - s16 midpoint = (minLimits[jointIndex] + maxLimits[jointIndex]) / 2; - s16 steps = midpoint + RobotUtils::radToSteps(angleRad); - + // Clamping to given URDF angles to avoid invalid movements + std::pair jointLimits = JOINT_LIMITS[jointIndex]; + angleRad = std::clamp(angleRad, jointLimits.first, jointLimits.second); + s16 steps = RobotUtils::radToSteps(angleRad); u16 speedSteps = RobotUtils::radPerSToStepsPerS(speedRadPerS); u8 accUnits = RobotUtils::radPerS2ToAccUnits(accRadPerS2); @@ -82,9 +82,10 @@ void SO101::setAllJointAngles(const std::array& anglesRad, // In the vector version, it checked sizes. for (size_t i = 0; i < 6; ++i) { - s16 midpoint = (minLimits[i] + maxLimits[i]) / 2; - positions[i] = midpoint + RobotUtils::radToSteps(anglesRad[i]); - + // Clamping to given URDF angles to avoid invalid movements + std::pair jointLimits = JOINT_LIMITS[i]; + positions[i] = RobotUtils::radToSteps(std::clamp(anglesRad[i], jointLimits.first, jointLimits.second)); + // If speed is 0, we might want to use a default speed, // but let's just convert what's provided. speeds[i] = RobotUtils::radPerSToStepsPerS(speedsRadPerS[i]); @@ -102,9 +103,8 @@ float SO101::getJointAngle(u8 jointIndex) { int pos = sm_st.ReadPos(servoIDs[jointIndex]); if (pos == -1) return NAN; - - s16 midpoint = (minLimits[jointIndex] + maxLimits[jointIndex]) / 2; - return RobotUtils::stepsToRad((s16)pos - midpoint); + + return RobotUtils::stepsToRad((s16)pos); } bool SO101::isMoving() { From 6ea92fd2445ae64616f4ff8d4d9203537465230b Mon Sep 17 00:00:00 2001 From: David Basabe Date: Tue, 19 May 2026 12:31:24 +0200 Subject: [PATCH 2/4] Adding comment about the URDF taken for SO101, including the URL of the URDF and the corresponding commit --- include/lerobot_cpp/robots/SO101.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/lerobot_cpp/robots/SO101.h b/include/lerobot_cpp/robots/SO101.h index e72fca8..8ab65fb 100644 --- a/include/lerobot_cpp/robots/SO101.h +++ b/include/lerobot_cpp/robots/SO101.h @@ -46,6 +46,8 @@ class SO101 { // Nominal limits from `so101_new_calib.urdf` (radians), used for logging and safe clamping. // URDF mentions some 5° Offset in the joint between lower and upper arms (joint 3) + // URDF and joint limits taken from https://github.com/TheRobotStudio/SO-ARM100/blob/main/Simulation/SO101/so101_new_calib.urdf + // Commit 385e8d7 static constexpr std::array, 6> JOINT_LIMITS = {{ {-1.91986f, 1.91986f}, {-1.74533f, 1.74533f}, From 94a3e2ecc2b74941b550c19be75c8d84ac919287 Mon Sep 17 00:00:00 2001 From: David Basabe Date: Wed, 20 May 2026 09:45:17 +0200 Subject: [PATCH 3/4] Removing custom calibration option from repo, using directly given values from motors and URDF --- README.md | 10 +-- examples/CalibrationOfs.cpp | 89 -------------------- examples/robots/SO101_Calibration.cpp | 113 -------------------------- include/lerobot_cpp/robots/SO101.h | 16 ---- src/robots/SO101.cpp | 26 ------ 5 files changed, 1 insertion(+), 253 deletions(-) delete mode 100755 examples/CalibrationOfs.cpp delete mode 100755 examples/robots/SO101_Calibration.cpp diff --git a/README.md b/README.md index 93821d4..fc34f3a 100644 --- a/README.md +++ b/README.md @@ -29,14 +29,7 @@ mkdir build && cd build cmake .. && make -j4 ``` -### 2. Calibrate the robot (Optional) -The joint limits and home positions can be set interactively using the calibration tool. -```bash -./lerobot_cpp_calibration /dev/ttyACM0 -``` - - -### 3. Run the Wave Example +### 2. Run the Wave Example To build and run the provided wave example: @@ -55,7 +48,6 @@ sudo chmod 666 /dev/ttyACM0 | `SO101_Home` | Moves the robot to the calibrated 0 radian (midpoint) position. | | `SO101_Wave` | Performs a soft "hello" waving animation. | | `SO101_Record` | Logs current joint positions in real-time for manual teaching. | -| `SO101_Calibration` | Interactive tool to set EEPROM offsets and mechanical limits. | ## Documentation diff --git a/examples/CalibrationOfs.cpp b/examples/CalibrationOfs.cpp deleted file mode 100755 index dea5aa3..0000000 --- a/examples/CalibrationOfs.cpp +++ /dev/null @@ -1,89 +0,0 @@ -/** - * @file CalibrationOfs.cpp - * @brief Mechanical center position calibration for STS3215 protocol servos - * - * @details - * This example demonstrates the center position offset calibration procedure for - * Feetech STS3215 servos. This calibration compensates for mechanical assembly - * tolerances by setting the current physical position as the logical zero point - * (center position). This is essential for applications requiring precise absolute - * positioning or when replacing/rebuilding servo mechanisms. - * - * Hardware Requirements: - * - Feetech STS3215 protocol servo (ID: 1) - * - USB-to-Serial adapter or direct serial port - * - Power supply appropriate for servo model - * - Serial connection at 115200 baud - * - Servo must be positioned at desired center/zero before running - * - * Key Features Demonstrated: - * - CalibrationOfs(): Set current position as logical zero (center) - * - EEPROM write for persistent calibration storage - * - Factory reset alternative using offset adjustment - * - * Usage: - * @code - * # 1. Manually position servo to desired center/zero point - * # 2. Run calibration - * ./CalibrationOfs /dev/ttyUSB0 - * # 3. New center position is now saved to EEPROM - * @endcode - * - * Calibration Procedure: - * 1. Physically position servo shaft to desired center/zero point - * 2. Run this program - * 3. CalibrationOfs() reads current position and calculates offset - * 4. Offset is written to EEPROM (persists across power cycles) - * 5. All future position commands are relative to this new center - * - * Use Cases: - * - Compensating for gear backlash or mechanical tolerances - * - Setting custom zero points for joint mechanisms - * - Correcting position drift after mechanical repairs - * - Aligning multiple servos to common reference frame - * - Factory-style calibration for production systems - * - * @note This command writes to EEPROM. Excessive use (>100,000 cycles) may wear - * out EEPROM memory. Use sparingly for calibration, not in control loops. - * - * @warning Ensure servo is at the desired center position BEFORE running this command. - * The current physical position becomes the new logical zero. - * - * @warning After calibration, all previous position values may be offset. Recalibrate - * or adjust your control software accordingly. - * - * @warning Servo must be powered and holding position during calibration. Unpowered - * or free-moving servos will result in incorrect calibration. - * - * @see STS3215::CalibrationOfs() - * @see STS3215::writeByte() for manual offset adjustment - */ -#include -#include - -STS3215 sm_st; - -int main(int argc, char **argv) -{ - if(argc<3){ - std::cout<<"argc error!"< -#include -#include -#include -#include -#include -#include -#include -#include "ExampleUtils.h" - -int main(int argc, char **argv) { - if (argc < 2) { - std::cout << "Usage: " << argv[0] << " [baud_rate]" << std::endl; - std::cout << "Default baud rate: 1000000" << std::endl; - return 0; - } - - const char* serialPort = argv[1]; - int baudRate = (argc > 2) ? std::stoi(argv[2]) : 1000000; - - STS3215 sm_st; - if (!sm_st.begin(baudRate, serialPort)) { - std::cerr << "Failed to initialize serial port: " << serialPort << std::endl; - return 1; - } - - SO101 robot(sm_st); - std::array ids = {1, 2, 3, 4, 5, 6}; - if (!robot.init(ids)) { - std::cerr << "Failed to initialize SO101 robot." << std::endl; - return 1; - } - - // Step 1: Manual positioning for Home - std::cout << "\n--- STEP 1: SET HOME POSITION ---" << std::endl; - std::cout << "Disabling torque to allow manual positioning..." << std::endl; - robot.enableTorque(false); - std::cout << "Please manually move all robot joints to their ZERO/HOME positions." << std::endl; - std::cout << "Press ENTER when finished to save Home positions..." << std::endl; - ExampleUtils::waitForEnter(); - - std::cout << "Saving Home positions (setting current physical position as logical 0)..." << std::endl; - for (u8 id : ids) { - if (sm_st.CalibrationOfs(id) == -1) { - std::cerr << "Warning: Failed to calibrate offset for Servo ID " << (int)id << std::endl; - } - } - std::cout << "Home positions saved." << std::endl; - - // Step 2: Record Limits - std::cout << "\n--- STEP 2: RECORD JOINT LIMITS ---" << std::endl; - std::cout << "Now move the robot arm through its full range of motion for each joint." << std::endl; - std::cout << "The program will track the minimum and maximum positions reached." << std::endl; - std::cout << "Press ENTER when you are done recording limits..." << std::endl; - - std::array minPos; - minPos.fill(4096); - std::array maxPos; - maxPos.fill(-4096); - - std::cout << "Recording... (Press ENTER to stop)" << std::endl; - while (!ExampleUtils::isEnterPressed()) - { - // No input, poll servos - for (size_t i = 0; i < ids.size(); ++i) { - int pos = sm_st.ReadPos(ids[i]); - if (pos != -1) { - if (pos < minPos[i]) minPos[i] = pos; - if (pos > maxPos[i]) maxPos[i] = pos; - } - } - - std::cout << "\rCurrent Limits [ID: min/max]: "; - for (size_t i = 0; i < ids.size(); ++i) { - std::cout << (int)ids[i] << ": " << minPos[i] << "/" << maxPos[i] << " "; - } - std::cout << std::flush; - } - std::cout << "\nRecording stopped." << std::endl; - - // Step 3: Save Limits to EEPROM - std::cout << "\n--- STEP 3: SAVE LIMITS TO EEPROM ---" << std::endl; - std::cout << "Saving recorded limits to servos..." << std::endl; - - if (robot.storeLimits(minPos, maxPos)) { - std::cout << "\nCalibration complete! Limits saved to EEPROM." << std::endl; - } else { - std::cerr << "\nError: Failed to save limits to EEPROM." << std::endl; - } - - std::cout << "Enabling torque and moving to Home (Midpoint)..." << std::endl; - robot.enableTorque(true); - std::array home; - home.fill(0.0f); - robot.setAllJointAngles(home); - robot.waitMovementFinished(); - - sm_st.end(); - return 0; -} diff --git a/include/lerobot_cpp/robots/SO101.h b/include/lerobot_cpp/robots/SO101.h index 8ab65fb..8f815d5 100644 --- a/include/lerobot_cpp/robots/SO101.h +++ b/include/lerobot_cpp/robots/SO101.h @@ -70,20 +70,6 @@ class SO101 { */ bool init(const std::array& ids = {1, 2, 3, 4, 5, 6}); - /** - * @brief Reload joint limits from servos - * @return true if successful - */ - bool reloadLimits(); - - /** - * @brief Save joint limits to servos' EEPROM and reload internal cache - * @param minPositions Array of 6 minimum joint positions (steps, 0-4095) - * @param maxPositions Array of 6 maximum joint positions (steps, 0-4095) - * @return true if successful - */ - bool storeLimits(const std::array& minPositions, const std::array& maxPositions); - /** * @brief Move a specific joint to an angle in radians * @param jointIndex Joint index (0-5) @@ -134,8 +120,6 @@ class SO101 { private: STS3215& sm_st; std::array servoIDs{}; - std::array minLimits{}; - std::array maxLimits{}; }; #endif // _SO101_H diff --git a/src/robots/SO101.cpp b/src/robots/SO101.cpp index 539df75..0eae08c 100644 --- a/src/robots/SO101.cpp +++ b/src/robots/SO101.cpp @@ -15,8 +15,6 @@ SO101::SO101(STS3215& servoInstance) : sm_st(servoInstance) { bool SO101::init(const std::array& ids) { servoIDs = ids; - minLimits.fill(0); - maxLimits.fill(4095); for (u8 id : servoIDs) { // Initialize motor to servo mode (0) @@ -27,33 +25,9 @@ bool SO101::init(const std::array& ids) { } } } - return reloadLimits(); -} - -bool SO101::reloadLimits() { - for (size_t i = 0; i < servoIDs.size(); ++i) { - int minL = sm_st.readWord(servoIDs[i], STS3215_MIN_ANGLE_LIMIT_L); - int maxL = sm_st.readWord(servoIDs[i], STS3215_MAX_ANGLE_LIMIT_L); - if (minL == -1 || maxL == -1) { - return false; - } - minLimits[i] = (s16)minL; - maxLimits[i] = (s16)maxL; - } return true; } -bool SO101::storeLimits(const std::array& minPositions, const std::array& maxPositions) { - for (size_t i = 0; i < servoIDs.size(); ++i) { - u8 id = servoIDs[i]; - sm_st.unLockEeprom(id); - sm_st.writeWord(id, STS3215_MIN_ANGLE_LIMIT_L, (u16)minPositions[i]); - sm_st.writeWord(id, STS3215_MAX_ANGLE_LIMIT_L, (u16)maxPositions[i]); - sm_st.LockEeprom(id); - } - return reloadLimits(); -} - int SO101::setJointAngle(u8 jointIndex, float angleRad, float speedRadPerS, float accRadPerS2) { if (jointIndex >= servoIDs.size()) return 0; From 28669ad32266fb748341bd5da11bba3b81bbc6d3 Mon Sep 17 00:00:00 2001 From: David Basabe Date: Wed, 17 Jun 2026 12:38:04 +0200 Subject: [PATCH 4/4] Adding a synchronised position and speed read from the servos functionality for faster querying --- include/lerobot_cpp/STS3215.h | 520 +++++----- include/lerobot_cpp/robots/RobotUtils.h | 7 + include/lerobot_cpp/robots/SO101.h | 18 +- src/SCS.cpp | 9 + src/STS3215.cpp | 1251 ++++++++++++----------- src/robots/RobotUtils.cpp | 4 + src/robots/SO101.cpp | 35 + 7 files changed, 995 insertions(+), 849 deletions(-) diff --git a/include/lerobot_cpp/STS3215.h b/include/lerobot_cpp/STS3215.h index 489ef58..8c783b1 100644 --- a/include/lerobot_cpp/STS3215.h +++ b/include/lerobot_cpp/STS3215.h @@ -1,259 +1,263 @@ -/** - * @file STS3215.h - * @brief Feetech STS3215 Series Serial Servo Application Layer - * - * @details This file provides the application programming interface for - * controlling Feetech STS3215 series serial bus servo motors. - * Supports three operating modes: - * - Mode 0: Servo (position control) - * - Mode 1: Closed-loop wheel (velocity control with feedback) - * - Mode 2: Open-loop wheel (PWM control without feedback) - */ - -#ifndef _STS3215_H -#define _STS3215_H - -// Baud rate definition -#define STS3215_1M 0 // 1Mbps -#define STS3215_0_5M 1 // 0.5Mbps -#define STS3215_250K 2 // 250kbps -#define STS3215_128K 3 // 128kbps -#define STS3215_115200 4 // 115200bps -#define STS3215_76800 5 // 76800bps -#define STS3215_57600 6 // 57600bps -#define STS3215_38400 7 // 38400bps - -//Memory table definition -//-------EEPROM (Read only)-------- -#define STS3215_MODEL_L 3 // Model number (Low byte) -#define STS3215_MODEL_H 4 // Model number (High byte) - -//-------EEPROM (Read and Write)-------- -#define STS3215_ID 5 // Servo ID (0-253) -#define STS3215_BAUD_RATE 6 // Baud rate selector -#define STS3215_MIN_ANGLE_LIMIT_L 9 // Minimum angle limit (Low byte) -#define STS3215_MIN_ANGLE_LIMIT_H 10 // Minimum angle limit (High byte) -#define STS3215_MAX_ANGLE_LIMIT_L 11 // Maximum angle limit (Low byte) -#define STS3215_MAX_ANGLE_LIMIT_H 12 // Maximum angle limit (High byte) -#define STS3215_CW_DEAD 26 // Clockwise dead zone -#define STS3215_CCW_DEAD 27 // Counter-clockwise dead zone -#define STS3215_OFS_L 31 // Offset calibration (Low byte) -#define STS3215_OFS_H 32 // Offset calibration (High byte) -#define STS3215_MODE 33 // Operating mode (Servo/Wheel) - -//-------SRAM (Read and Write)-------- -#define STS3215_TORQUE_ENABLE 40 // Torque enable (0: disable, 1: enable) -#define STS3215_ACC 41 // Acceleration -#define STS3215_GOAL_POSITION_L 42 // Target position (Low byte) -#define STS3215_GOAL_POSITION_H 43 // Target position (High byte) -#define STS3215_GOAL_TIME_L 44 // Target time (Low byte) -#define STS3215_GOAL_TIME_H 45 // Target time (High byte) -#define STS3215_GOAL_SPEED_L 46 // Target speed (Low byte) -#define STS3215_GOAL_SPEED_H 47 // Target speed (High byte) -#define STS3215_LOCK 55 // EEPROM lock (0: unlocked, 1: locked) - -//-------SRAM (Read only)-------- -#define STS3215_PRESENT_POSITION_L 56 // Current position (Low byte) -#define STS3215_PRESENT_POSITION_H 57 // Current position (High byte) -#define STS3215_PRESENT_SPEED_L 58 // Current speed (Low byte) -#define STS3215_PRESENT_SPEED_H 59 // Current speed (High byte) -#define STS3215_PRESENT_LOAD_L 60 // Current load (Low byte) -#define STS3215_PRESENT_LOAD_H 61 // Current load (High byte) -#define STS3215_PRESENT_VOLTAGE 62 // Current voltage -#define STS3215_PRESENT_TEMPERATURE 63 // Current temperature -#define STS3215_MOVING 66 // Movement status (0: stopped, 1: moving) -#define STS3215_PRESENT_CURRENT_L 69 // Current current (Low byte) -#define STS3215_PRESENT_CURRENT_H 70 // Current current (High byte) - -// Bit position constants for data encoding -#define STS3215_DIRECTION_BIT_POS 15 // Bit position for direction flag in position/speed -#define STS3215_LOAD_DIRECTION_BIT_POS 10 // Bit position for direction flag in load/PWM - -// Operating mode values -#define STS3215_MODE_SERVO 0 // Servo mode (position control) -#define STS3215_MODE_WHEEL_CLOSED 1 // Wheel mode - closed loop (velocity control) -#define STS3215_MODE_WHEEL_OPEN 2 // Wheel mode - open loop (PWM control) -#define STS3215_MODE_STEPPER 3 // Stepper mode (not implemented in SDK) - -// Special servo IDs -#define STS3215_BROADCAST_ID 0xFE // Broadcast ID for all servos - -// Calibration command -#define STS3215_CALIBRATION_CMD 128 // Command value for midpoint calibration - -#include -#include -#include -#include - -/** - * @class STS3215 - * @brief Application layer interface for STS3215 series serial servos - * - * @details Provides high-level control functions for Feetech STS3215 series - * servo motors. Supports three operating modes with complete read/write functionality. - * - * **Operating Modes:** - * - Mode 0: Servo mode (position control) - precise positioning - * - Mode 1: Wheel mode closed-loop (velocity control) - speed feedback - * - Mode 2: Wheel mode open-loop (PWM control) - direct motor power - * - * **Key Features:** - * - Write operations: immediate, asynchronous (Reg), and synchronized (Sync) - * - Comprehensive feedback: position, speed, load, voltage, temperature, current - * - EEPROM management: lock/unlock for persistent configuration - * - LSP compliant: uniform InitMotor() and Mode() methods - * - * **Usage Example:** - * @code - * STS3215 servo; - * servo.begin(1000000, "/dev/ttyUSB0"); - * servo.InitMotor(1, 0, 1); // ID=1, Mode=0 (servo), Enable torque - * servo.WritePosEx(1, 2048, 1000, 50); // Move to center position - * @endcode - * - * @note Remember to call begin() before using any servo methods - * @see SCSerial for serial communication layer methods - */ -class STS3215 : public SCSerial -{ -public: - /** @brief Default constructor */ - STS3215(); - /** @brief Constructor with protocol end byte - * @param End Protocol end byte (0 or 1) */ - STS3215(u8 End); - /** @brief Constructor with protocol end byte and response level - * @param End Protocol end byte (0 or 1) - * @param Level Response level (0=no response, 1=response enabled) */ - STS3215(u8 End, u8 Level); - /** @brief Write position to single servo (Mode 0) - * @param ID Servo ID (0-253, 254=broadcast) - * @param Position Target position (0-4095 steps) - * @param Speed Movement speed (0-3400 steps/s) - * @param ACC Acceleration (0-254, units of 100 steps/s²) - * @return 1 on success, 0 on failure */ - virtual int WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0); - - /** @brief Async write position (execute with RegWriteAction) - * @param ID Servo ID - * @param Position Target position - * @param Speed Movement speed - * @param ACC Acceleration - * @return 1 on success, 0 on failure */ - virtual int RegWritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0); - - /** @brief Sync write position to multiple servos - * @param ID Array of servo IDs - * @param IDN Number of servos - * @param Position Array of target positions - * @param Speed Array of speeds (NULL for 0) - * @param ACC Array of accelerations (NULL for 0) */ - virtual void SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[]); - - /** @brief Set operating mode - * @param ID Servo ID - * @param mode 0=servo, 1=wheel closed-loop, 2=wheel open-loop - * @return 1 on success, 0 on failure */ - virtual int Mode(u8 ID, u8 mode); - - /** @brief Initialize motor (unlock EEPROM → set mode → lock EEPROM → enable torque) - * @param ID Servo ID - * @param mode Operating mode (0/1/2) - * @param enableTorque 1=enable, 0=disable (default: 1) - * @return 1 on success, 0 on failure - * @note LSP compliant - available on all servo classes */ - virtual int InitMotor(u8 ID, u8 mode, u8 enableTorque = 1); - - /** @brief Write speed to single servo (Mode 1) - * @param ID Servo ID - * @param Speed Target speed (-3400 to +3400 steps/s) - * @param ACC Acceleration - * @return 1 on success, 0 on failure */ - virtual int WriteSpe(u8 ID, s16 Speed, u8 ACC = 0); - - /** @brief Async write speed */ - virtual int RegWriteSpe(u8 ID, s16 Speed, u8 ACC = 0); - - /** @brief Sync write speed to multiple servos */ - virtual void SyncWriteSpe(u8 ID[], u8 IDN, s16 Speed[], u8 ACC[]); - - /** @brief Write PWM to single servo (Mode 2) - * @param ID Servo ID - * @param Pwm PWM duty cycle (±1000 = ±100%) - * @return 1 on success, 0 on failure */ - virtual int WritePwm(u8 ID, s16 Pwm); - - /** @brief Async write PWM */ - virtual int RegWritePwm(u8 ID, s16 Pwm); - - /** @brief Sync write PWM to multiple servos */ - virtual void SyncWritePwm(u8 ID[], u8 IDN, s16 Pwm[]); - - /** @brief Enable/disable motor torque - * @param ID Servo ID - * @param Enable 1=enable, 0=disable (free-moving) - * @return 1 on success, 0 on failure */ - virtual int EnableTorque(u8 ID, u8 Enable); - - /** @brief Unlock EEPROM for writing configuration - * @param ID Servo ID - * @return 1 on success, 0 on failure */ - virtual int unLockEeprom(u8 ID); - - /** @brief Lock EEPROM to protect configuration - * @param ID Servo ID - * @return 1 on success, 0 on failure */ - virtual int LockEeprom(u8 ID); - - /** @brief Calibrate servo midpoint position - * @param ID Servo ID - * @return 1 on success, 0 on failure - * @note Sets offset register to 128 (different from InitMotor which sets operating mode) */ - virtual int CalibrationOfs(u8 ID); - - /** @brief Read all feedback data into internal buffer - * @param ID Servo ID - * @return 1 on success, 0 on failure - * @note Call before using Read* methods with ID=-1 for cached reads */ - virtual int FeedBack(int ID); - - /** @brief Read current position - * @param ID Servo ID, or -1 to read from cache (after FeedBack) - * @return Position (0-4095), -1 on error */ - virtual int ReadPos(int ID); - - /** @brief Read current speed - * @param ID Servo ID, or -1 for cached read - * @return Speed (±3400 steps/s), -1 on error */ - virtual int ReadSpeed(int ID); - - /** @brief Read motor load - * @param ID Servo ID, or -1 for cached read - * @return Load (±1000 = ±100% PWM), -1 on error */ - virtual int ReadLoad(int ID); - - /** @brief Read supply voltage - * @param ID Servo ID, or -1 for cached read - * @return Voltage in 0.1V units, -1 on error */ - virtual int ReadVoltage(int ID); - - /** @brief Read internal temperature - * @param ID Servo ID, or -1 for cached read - * @return Temperature in °C, -1 on error */ - virtual int ReadTemper(int ID); - - /** @brief Read movement status - * @param ID Servo ID, or -1 for cached read - * @return 1=moving, 0=stopped, -1=error */ - virtual int ReadMove(int ID); - - /** @brief Read motor current - * @param ID Servo ID, or -1 for cached read - * @return Current in milliamps, -1 on error */ - virtual int ReadCurrent(int ID); -private: - u8 Mem[STS3215_PRESENT_CURRENT_H-STS3215_PRESENT_POSITION_L+1]; -}; - +/** + * @file STS3215.h + * @brief Feetech STS3215 Series Serial Servo Application Layer + * + * @details This file provides the application programming interface for + * controlling Feetech STS3215 series serial bus servo motors. + * Supports three operating modes: + * - Mode 0: Servo (position control) + * - Mode 1: Closed-loop wheel (velocity control with feedback) + * - Mode 2: Open-loop wheel (PWM control without feedback) + */ + +#ifndef _STS3215_H +#define _STS3215_H + +// Baud rate definition +#define STS3215_1M 0 // 1Mbps +#define STS3215_0_5M 1 // 0.5Mbps +#define STS3215_250K 2 // 250kbps +#define STS3215_128K 3 // 128kbps +#define STS3215_115200 4 // 115200bps +#define STS3215_76800 5 // 76800bps +#define STS3215_57600 6 // 57600bps +#define STS3215_38400 7 // 38400bps + +//Memory table definition +//-------EEPROM (Read only)-------- +#define STS3215_MODEL_L 3 // Model number (Low byte) +#define STS3215_MODEL_H 4 // Model number (High byte) + +//-------EEPROM (Read and Write)-------- +#define STS3215_ID 5 // Servo ID (0-253) +#define STS3215_BAUD_RATE 6 // Baud rate selector +#define STS3215_MIN_ANGLE_LIMIT_L 9 // Minimum angle limit (Low byte) +#define STS3215_MIN_ANGLE_LIMIT_H 10 // Minimum angle limit (High byte) +#define STS3215_MAX_ANGLE_LIMIT_L 11 // Maximum angle limit (Low byte) +#define STS3215_MAX_ANGLE_LIMIT_H 12 // Maximum angle limit (High byte) +#define STS3215_CW_DEAD 26 // Clockwise dead zone +#define STS3215_CCW_DEAD 27 // Counter-clockwise dead zone +#define STS3215_OFS_L 31 // Offset calibration (Low byte) +#define STS3215_OFS_H 32 // Offset calibration (High byte) +#define STS3215_MODE 33 // Operating mode (Servo/Wheel) + +//-------SRAM (Read and Write)-------- +#define STS3215_TORQUE_ENABLE 40 // Torque enable (0: disable, 1: enable) +#define STS3215_ACC 41 // Acceleration +#define STS3215_GOAL_POSITION_L 42 // Target position (Low byte) +#define STS3215_GOAL_POSITION_H 43 // Target position (High byte) +#define STS3215_GOAL_TIME_L 44 // Target time (Low byte) +#define STS3215_GOAL_TIME_H 45 // Target time (High byte) +#define STS3215_GOAL_SPEED_L 46 // Target speed (Low byte) +#define STS3215_GOAL_SPEED_H 47 // Target speed (High byte) +#define STS3215_LOCK 55 // EEPROM lock (0: unlocked, 1: locked) + +//-------SRAM (Read only)-------- +#define STS3215_PRESENT_POSITION_L 56 // Current position (Low byte) +#define STS3215_PRESENT_POSITION_H 57 // Current position (High byte) +#define STS3215_PRESENT_SPEED_L 58 // Current speed (Low byte) +#define STS3215_PRESENT_SPEED_H 59 // Current speed (High byte) +#define STS3215_PRESENT_LOAD_L 60 // Current load (Low byte) +#define STS3215_PRESENT_LOAD_H 61 // Current load (High byte) +#define STS3215_PRESENT_VOLTAGE 62 // Current voltage +#define STS3215_PRESENT_TEMPERATURE 63 // Current temperature +#define STS3215_MOVING 66 // Movement status (0: stopped, 1: moving) +#define STS3215_PRESENT_CURRENT_L 69 // Current current (Low byte) +#define STS3215_PRESENT_CURRENT_H 70 // Current current (High byte) + +// Bit position constants for data encoding +#define STS3215_DIRECTION_BIT_POS 15 // Bit position for direction flag in position/speed +#define STS3215_LOAD_DIRECTION_BIT_POS 10 // Bit position for direction flag in load/PWM + +// Operating mode values +#define STS3215_MODE_SERVO 0 // Servo mode (position control) +#define STS3215_MODE_WHEEL_CLOSED 1 // Wheel mode - closed loop (velocity control) +#define STS3215_MODE_WHEEL_OPEN 2 // Wheel mode - open loop (PWM control) +#define STS3215_MODE_STEPPER 3 // Stepper mode (not implemented in SDK) + +// Special servo IDs +#define STS3215_BROADCAST_ID 0xFE // Broadcast ID for all servos + +// Calibration command +#define STS3215_CALIBRATION_CMD 128 // Command value for midpoint calibration + +#include +#include +#include +#include + +/** + * @class STS3215 + * @brief Application layer interface for STS3215 series serial servos + * + * @details Provides high-level control functions for Feetech STS3215 series + * servo motors. Supports three operating modes with complete read/write functionality. + * + * **Operating Modes:** + * - Mode 0: Servo mode (position control) - precise positioning + * - Mode 1: Wheel mode closed-loop (velocity control) - speed feedback + * - Mode 2: Wheel mode open-loop (PWM control) - direct motor power + * + * **Key Features:** + * - Write operations: immediate, asynchronous (Reg), and synchronized (Sync) + * - Comprehensive feedback: position, speed, load, voltage, temperature, current + * - EEPROM management: lock/unlock for persistent configuration + * - LSP compliant: uniform InitMotor() and Mode() methods + * + * **Usage Example:** + * @code + * STS3215 servo; + * servo.begin(1000000, "/dev/ttyUSB0"); + * servo.InitMotor(1, 0, 1); // ID=1, Mode=0 (servo), Enable torque + * servo.WritePosEx(1, 2048, 1000, 50); // Move to center position + * @endcode + * + * @note Remember to call begin() before using any servo methods + * @see SCSerial for serial communication layer methods + */ +class STS3215 : public SCSerial +{ +public: + /** @brief Default constructor */ + STS3215(); + /** @brief Constructor with protocol end byte + * @param End Protocol end byte (0 or 1) */ + STS3215(u8 End); + /** @brief Constructor with protocol end byte and response level + * @param End Protocol end byte (0 or 1) + * @param Level Response level (0=no response, 1=response enabled) */ + STS3215(u8 End, u8 Level); + /** @brief Write position to single servo (Mode 0) + * @param ID Servo ID (0-253, 254=broadcast) + * @param Position Target position (0-4095 steps) + * @param Speed Movement speed (0-3400 steps/s) + * @param ACC Acceleration (0-254, units of 100 steps/s²) + * @return 1 on success, 0 on failure */ + virtual int WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0); + + /** @brief Async write position (execute with RegWriteAction) + * @param ID Servo ID + * @param Position Target position + * @param Speed Movement speed + * @param ACC Acceleration + * @return 1 on success, 0 on failure */ + virtual int RegWritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0); + + /** @brief Sync write position to multiple servos + * @param ID Array of servo IDs + * @param IDN Number of servos + * @param Position Array of target positions + * @param Speed Array of speeds (NULL for 0) + * @param ACC Array of accelerations (NULL for 0) */ + virtual void SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[]); + + /** @brief Set operating mode + * @param ID Servo ID + * @param mode 0=servo, 1=wheel closed-loop, 2=wheel open-loop + * @return 1 on success, 0 on failure */ + virtual int Mode(u8 ID, u8 mode); + + /** @brief Initialize motor (unlock EEPROM → set mode → lock EEPROM → enable torque) + * @param ID Servo ID + * @param mode Operating mode (0/1/2) + * @param enableTorque 1=enable, 0=disable (default: 1) + * @return 1 on success, 0 on failure + * @note LSP compliant - available on all servo classes */ + virtual int InitMotor(u8 ID, u8 mode, u8 enableTorque = 1); + + /** @brief Write speed to single servo (Mode 1) + * @param ID Servo ID + * @param Speed Target speed (-3400 to +3400 steps/s) + * @param ACC Acceleration + * @return 1 on success, 0 on failure */ + virtual int WriteSpe(u8 ID, s16 Speed, u8 ACC = 0); + + /** @brief Async write speed */ + virtual int RegWriteSpe(u8 ID, s16 Speed, u8 ACC = 0); + + /** @brief Sync write speed to multiple servos */ + virtual void SyncWriteSpe(u8 ID[], u8 IDN, s16 Speed[], u8 ACC[]); + + /** @brief Write PWM to single servo (Mode 2) + * @param ID Servo ID + * @param Pwm PWM duty cycle (±1000 = ±100%) + * @return 1 on success, 0 on failure */ + virtual int WritePwm(u8 ID, s16 Pwm); + + /** @brief Async write PWM */ + virtual int RegWritePwm(u8 ID, s16 Pwm); + + /** @brief Sync write PWM to multiple servos */ + virtual void SyncWritePwm(u8 ID[], u8 IDN, s16 Pwm[]); + + /** @brief Enable/disable motor torque + * @param ID Servo ID + * @param Enable 1=enable, 0=disable (free-moving) + * @return 1 on success, 0 on failure */ + virtual int EnableTorque(u8 ID, u8 Enable); + + /** @brief Unlock EEPROM for writing configuration + * @param ID Servo ID + * @return 1 on success, 0 on failure */ + virtual int unLockEeprom(u8 ID); + + /** @brief Lock EEPROM to protect configuration + * @param ID Servo ID + * @return 1 on success, 0 on failure */ + virtual int LockEeprom(u8 ID); + + /** @brief Calibrate servo midpoint position + * @param ID Servo ID + * @return 1 on success, 0 on failure + * @note Sets offset register to 128 (different from InitMotor which sets operating mode) */ + virtual int CalibrationOfs(u8 ID); + + /** @brief Read all feedback data into internal buffer + * @param ID Servo ID + * @return 1 on success, 0 on failure + * @note Call before using Read* methods with ID=-1 for cached reads */ + virtual int FeedBack(int ID); + + /** @brief Read current position + * @param ID Servo ID, or -1 to read from cache (after FeedBack) + * @return Position (0-4095), -1 on error */ + virtual int ReadPos(int ID); + + virtual int SyncReadPos(u8 ID[], u8 IDN, s16 *Position); + + virtual int SyncReadSpeed(u8 ID[], u8 IDN, s16 *Speed); + + /** @brief Read current speed + * @param ID Servo ID, or -1 for cached read + * @return Speed (±3400 steps/s), -1 on error */ + virtual int ReadSpeed(int ID); + + /** @brief Read motor load + * @param ID Servo ID, or -1 for cached read + * @return Load (±1000 = ±100% PWM), -1 on error */ + virtual int ReadLoad(int ID); + + /** @brief Read supply voltage + * @param ID Servo ID, or -1 for cached read + * @return Voltage in 0.1V units, -1 on error */ + virtual int ReadVoltage(int ID); + + /** @brief Read internal temperature + * @param ID Servo ID, or -1 for cached read + * @return Temperature in °C, -1 on error */ + virtual int ReadTemper(int ID); + + /** @brief Read movement status + * @param ID Servo ID, or -1 for cached read + * @return 1=moving, 0=stopped, -1=error */ + virtual int ReadMove(int ID); + + /** @brief Read motor current + * @param ID Servo ID, or -1 for cached read + * @return Current in milliamps, -1 on error */ + virtual int ReadCurrent(int ID); +private: + u8 Mem[STS3215_PRESENT_CURRENT_H-STS3215_PRESENT_POSITION_L+1]; +}; + #endif \ No newline at end of file diff --git a/include/lerobot_cpp/robots/RobotUtils.h b/include/lerobot_cpp/robots/RobotUtils.h index 7fc4d5f..dce33ca 100644 --- a/include/lerobot_cpp/robots/RobotUtils.h +++ b/include/lerobot_cpp/robots/RobotUtils.h @@ -36,6 +36,13 @@ namespace RobotUtils { */ u16 radPerSToStepsPerS(float radPerS); + /** + * @brief Convert raw signed servo speed (steps/s) to rad/s + * @param stepsPerS Signed speed from feedback (ReadSpeed / SyncReadSpeed) + * @return Angular velocity in radians per second + */ + float stepsPerSToRadPerS(s16 stepsPerS); + /** * @brief Convert angular acceleration (rad/s^2) to raw servo acceleration units * @param radPerS2 Angular acceleration in radians per second squared diff --git a/include/lerobot_cpp/robots/SO101.h b/include/lerobot_cpp/robots/SO101.h index 8f815d5..f697e18 100644 --- a/include/lerobot_cpp/robots/SO101.h +++ b/include/lerobot_cpp/robots/SO101.h @@ -32,10 +32,11 @@ #define _SO101_H #include -#include #include #include #include +#include +#include /** * @class SO101 @@ -97,6 +98,21 @@ class SO101 { */ float getJointAngle(u8 jointIndex); + std::optional> getAllJointAngles(); + + /** + * @brief Get current joint speed in radians per second + * @param jointIndex Joint index (0-5) + * @return Speed in rad/s, or NaN on error + */ + float getJointSpeed(u8 jointIndex); + + /** + * @brief Get all joint speeds via synchronized bus read + * @return Array of 6 speeds in rad/s, or nullopt if sync read failed + */ + std::optional> getAllJointSpeeds(); + /** * @brief Check if any joint of the robot is moving * @return true if moving, false otherwise diff --git a/src/SCS.cpp b/src/SCS.cpp index 13c8756..66813d3 100644 --- a/src/SCS.cpp +++ b/src/SCS.cpp @@ -32,6 +32,9 @@ SCS::SCS() { Level = 1; // All instructions except broadcast return acknowledgement Error = 0; + syncReadRxBuff = nullptr; + syncReadRxBuffLen = 0; + syncReadRxBuffMax = 0; } /** @@ -44,6 +47,9 @@ SCS::SCS(u8 End) Level = 1; this->End = End; Error = 0; + syncReadRxBuff = nullptr; + syncReadRxBuffLen = 0; + syncReadRxBuffMax = 0; } /** @@ -57,6 +63,9 @@ SCS::SCS(u8 End, u8 Level) this->Level = Level; this->End = End; Error = 0; + syncReadRxBuff = nullptr; + syncReadRxBuffLen = 0; + syncReadRxBuffMax = 0; } /** diff --git a/src/STS3215.cpp b/src/STS3215.cpp index 81eb3ea..7e3a105 100644 --- a/src/STS3215.cpp +++ b/src/STS3215.cpp @@ -1,590 +1,661 @@ -/** - * @file STS3215.cpp - * @brief Feetech STS3215 series serial servo application layer implementation - * - * @details This file implements high-level control functions for Feetech STS3215 - * and STS series servo motors. It provides three operating modes with complete - * read/write functionality and LSP-compliant initialization. - * - * **Implemented Features:** - * - Mode 0: Position control with speed and acceleration - * - Mode 1: Velocity control (closed-loop wheel mode) - * - Mode 2: PWM control (open-loop wheel mode) - * - Synchronized writes for multi-motor coordination - * - Asynchronous writes with RegWriteAction - * - Comprehensive feedback reading (position, speed, load, voltage, temp, current) - * - EEPROM configuration management - * - * **Refactoring Improvements:** - * - Uses ServoUtils for direction bit encoding/decoding (DRY principle) - * - Uses SyncWriteBuffer for automatic memory management (RAII) - * - Standardized error handling with ServoErrors - * - * @note All sync write operations use RAII-based buffer management - * @see STS3215.h for class interface and usage examples - */ - -#include -#include -#include - -/** - * @brief Default constructor - * - * Initializes STS3215 servo controller with default End byte (0). - */ -STS3215::STS3215() -{ - End = 0; -} - -/** - * @brief Constructor with custom End byte - * - * @param End Protocol end byte (0 or 1) - */ -STS3215::STS3215(u8 End):SCSerial(End) -{ -} - -/** - * @brief Constructor with End byte and response level - * - * @param End Protocol end byte (0 or 1) - * @param Level Response level (0=no response, 1=response for read/write commands) - */ -STS3215::STS3215(u8 End, u8 Level):SCSerial(End, Level) -{ -} - -/** - * @brief Write position, speed, and acceleration to servo (extended command) - * - * Sends single-servo position command with acceleration and speed control. - * Negative positions are converted to absolute value with direction bit set. - * - * @param ID Servo ID - * @param Position Target position (0-4095 steps) - * @param Speed Moving speed (0-3400 steps/s) - * @param ACC Acceleration value (0-254) - * @return 1 on success, 0 on failure - */ -int STS3215::WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC) -{ - u16 encodedPosition = ServoUtils::encodeSignedValue(Position, STS3215_DIRECTION_BIT_POS); - - u8 bBuf[7]; - bBuf[0] = ACC; - Host2SCS(bBuf+1, bBuf+2, encodedPosition); - Host2SCS(bBuf+3, bBuf+4, 0); - Host2SCS(bBuf+5, bBuf+6, Speed); - - return genWrite(ID, STS3215_ACC, bBuf, 7); -} - -/** - * @brief Register write position command (executes on RegWriteAction) - * - * Queues position/speed/acceleration command for later execution. - * Use with RegWriteAction for synchronized multi-servo motion. - * - * @param ID Servo ID - * @param Position Target position (0-4095 steps) - * @param Speed Moving speed (0-3400 steps/s) - * @param ACC Acceleration value (0-254) - * @return 1 on success, 0 on failure - */ -int STS3215::RegWritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC) -{ - u16 encodedPosition = ServoUtils::encodeSignedValue(Position, STS3215_DIRECTION_BIT_POS); - - u8 bBuf[7]; - bBuf[0] = ACC; - Host2SCS(bBuf+1, bBuf+2, encodedPosition); - Host2SCS(bBuf+3, bBuf+4, 0); - Host2SCS(bBuf+5, bBuf+6, Speed); - - return regWrite(ID, STS3215_ACC, bBuf, 7); -} - -/** - * @brief Synchronized position write for multiple servos - * - * Sends position/speed/acceleration commands to multiple servos simultaneously. - * All servos receive commands in single transmission for coordinated motion. - * - * @param ID Array of servo IDs - * @param IDN Number of servos - * @param Position Array of target positions - * @param Speed Array of speeds (NULL for 0 speed) - * @param ACC Array of accelerations (NULL for 0 acceleration) - */ -void STS3215::SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[]) -{ - SyncWriteBuffer buffer(IDN, 7); - if(!buffer.isValid()){ - return; // Allocation failed - } - for(u8 i = 0; i +#include +#include + +/** + * @brief Default constructor + * + * Initializes STS3215 servo controller with default End byte (0). + */ +STS3215::STS3215() +{ + End = 0; +} + +/** + * @brief Constructor with custom End byte + * + * @param End Protocol end byte (0 or 1) + */ +STS3215::STS3215(u8 End):SCSerial(End) +{ +} + +/** + * @brief Constructor with End byte and response level + * + * @param End Protocol end byte (0 or 1) + * @param Level Response level (0=no response, 1=response for read/write commands) + */ +STS3215::STS3215(u8 End, u8 Level):SCSerial(End, Level) +{ +} + +/** + * @brief Write position, speed, and acceleration to servo (extended command) + * + * Sends single-servo position command with acceleration and speed control. + * Negative positions are converted to absolute value with direction bit set. + * + * @param ID Servo ID + * @param Position Target position (0-4095 steps) + * @param Speed Moving speed (0-3400 steps/s) + * @param ACC Acceleration value (0-254) + * @return 1 on success, 0 on failure + */ +int STS3215::WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC) +{ + u16 encodedPosition = ServoUtils::encodeSignedValue(Position, STS3215_DIRECTION_BIT_POS); + + u8 bBuf[7]; + bBuf[0] = ACC; + Host2SCS(bBuf+1, bBuf+2, encodedPosition); + Host2SCS(bBuf+3, bBuf+4, 0); + Host2SCS(bBuf+5, bBuf+6, Speed); + + return genWrite(ID, STS3215_ACC, bBuf, 7); +} + +/** + * @brief Register write position command (executes on RegWriteAction) + * + * Queues position/speed/acceleration command for later execution. + * Use with RegWriteAction for synchronized multi-servo motion. + * + * @param ID Servo ID + * @param Position Target position (0-4095 steps) + * @param Speed Moving speed (0-3400 steps/s) + * @param ACC Acceleration value (0-254) + * @return 1 on success, 0 on failure + */ +int STS3215::RegWritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC) +{ + u16 encodedPosition = ServoUtils::encodeSignedValue(Position, STS3215_DIRECTION_BIT_POS); + + u8 bBuf[7]; + bBuf[0] = ACC; + Host2SCS(bBuf+1, bBuf+2, encodedPosition); + Host2SCS(bBuf+3, bBuf+4, 0); + Host2SCS(bBuf+5, bBuf+6, Speed); + + return regWrite(ID, STS3215_ACC, bBuf, 7); +} + +/** + * @brief Synchronized position write for multiple servos + * + * Sends position/speed/acceleration commands to multiple servos simultaneously. + * All servos receive commands in single transmission for coordinated motion. + * + * @param ID Array of servo IDs + * @param IDN Number of servos + * @param Position Array of target positions + * @param Speed Array of speeds (NULL for 0 speed) + * @param ACC Array of accelerations (NULL for 0 acceleration) + */ +void STS3215::SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[]) +{ + SyncWriteBuffer buffer(IDN, 7); + if(!buffer.isValid()){ + return; // Allocation failed + } + for(u8 i = 0; i(stepsPerS) * RAD_PER_STEP; + } + u8 radPerS2ToAccUnits(float radPerS2) { float stepsPerS2 = radPerS2 / RAD_PER_STEP; float accUnits = stepsPerS2 / 100.0f; diff --git a/src/robots/SO101.cpp b/src/robots/SO101.cpp index 0eae08c..3c16174 100644 --- a/src/robots/SO101.cpp +++ b/src/robots/SO101.cpp @@ -81,6 +81,41 @@ float SO101::getJointAngle(u8 jointIndex) { return RobotUtils::stepsToRad((s16)pos); } +std::optional> SO101::getAllJointAngles() +{ + std::array positions{}; + if (sm_st.SyncReadPos(servoIDs.data(), 6, positions.data()) == 0) + return std::nullopt; + + std::array anglesRad{}; + for (size_t i = 0; i < 6; i++) + anglesRad[i] = RobotUtils::stepsToRad(positions[i]); + + return anglesRad; +} + +float SO101::getJointSpeed(u8 jointIndex) { + if (jointIndex >= servoIDs.size()) return NAN; + + int speedSteps = sm_st.ReadSpeed(servoIDs[jointIndex]); + if (speedSteps == -1) return NAN; + + return RobotUtils::stepsPerSToRadPerS((s16)speedSteps); +} + +std::optional> SO101::getAllJointSpeeds() +{ + std::array speedsSteps{}; + if (sm_st.SyncReadSpeed(servoIDs.data(), 6, speedsSteps.data()) == 0) + return std::nullopt; + + std::array speedsRad{}; + for (size_t i = 0; i < 6; i++) + speedsRad[i] = RobotUtils::stepsPerSToRadPerS(speedsSteps[i]); + + return speedsRad; +} + bool SO101::isMoving() { for (u8 id : servoIDs) { if (sm_st.ReadMove(id) == 1) {