: STS Servo Command Implementation More...

#include "sts_servo_cmd.h"
#include "sts_servo.h"
#include "sts_registers.h"

Functions
sts_result_t	STS_SetTorqueEnable (sts_servo_t *servo, uint8_t enable)
	Enables or disables motor torque output.

sts_result_t	STS_SetOperatingMode (sts_servo_t *servo, sts_operating_mode_t mode)
	Sets the operating mode of the servo.

sts_result_t	STS_SetTargetPosition (sts_servo_t *servo, uint16_t position)
	Sets the absolute target position for the servo.

sts_result_t	STS_GetPresentPosition (sts_servo_t servo, uint16_t position_out)
	Reads the current actual position from the servo.

sts_result_t	STS_SetTargetSpeed (sts_servo_t *servo, uint16_t speed, sts_direction_t dir)
	Sets the target speed and direction of the servo.

sts_result_t	STS_GetPresentSpeed (sts_servo_t servo, int16_t speed_out)
	Reads the current rotational speed of the servo.

sts_result_t	STS_SetTargetAcceleration (sts_servo_t *servo, uint8_t acceleration)
	Sets the acceleration profile for servo movements.

sts_result_t	STS_SetTargetPWM (sts_servo_t *servo, uint16_t pwm, sts_direction_t dir)
	Sets the open-loop PWM duty cycle (effort) applied to motor coils.

sts_result_t	STS_SetTargetStep (sts_servo_t *servo, uint16_t steps, sts_direction_t dir)
	Commands the servo to move a relative number of steps.

sts_result_t	STS_SetTarget (sts_servo_t *servo, int32_t target)
	Universal target wrapper that routes the command based on current mode.

sts_result_t	STS_SetTorqueLimit (sts_servo_t *servo, uint16_t limit)
	Sets the dynamic (RAM) maximum torque limit.

sts_result_t	STS_GetPresentLoad (sts_servo_t servo, int16_t load_out)
	Reads the current physical load/effort of the motor.

sts_result_t	STS_GetPresentVoltage (sts_servo_t servo, uint8_t voltage_out)
	Reads the current input voltage at the servo's power terminals.

sts_result_t	STS_GetPresentTemperature (sts_servo_t servo, uint8_t temp_out)
	Reads the internal temperature of the servo's MCU/Motor driver.

sts_result_t	STS_GetMovingStatus (sts_servo_t servo, uint8_t status_out)
	Reads the moving status flag to determine if the motor is currently in motion.

sts_result_t	STS_SetEEPROMLock (sts_servo_t *servo, uint8_t lock)
	Toggles the EEPROM write protection.

sts_result_t	STS_SetID (sts_servo_t *servo, uint8_t new_id)
	Assigns a new permanent ID to the servo.

Detailed Description

: STS Servo Command Implementation

Author: : Grisham Balloo

Implements the high-level command set for Feetech STS servo control, built on top of the STS_Write8, STS_Write16, STS_Read8, and STS_Read16 register access primitives.

All commands are stateless and route through the service layer command engine no direct protocol framing occurs at this level.

Attention: Copyright (c) 2026 Grisham Balloo. All rights reserved.

Function Documentation

◆ STS_GetMovingStatus()

sts_result_t STS_GetMovingStatus	(	sts_servo_t *	servo,
		uint8_t *	status_out
	)

Reads the moving status flag to determine if the motor is currently in motion.

Parameters

	servo	Pointer to an initialized servo handle.
[out]	status_out	Pointer to store the moving flag (1: Moving, 0: Target reached).

Returns: sts_result_t STS_OK on success, or speciifc error code.

◆ STS_GetPresentLoad()

sts_result_t STS_GetPresentLoad	(	sts_servo_t *	servo,
		int16_t *	load_out
	)

Reads the current physical load/effort of the motor.

Parameters

	servo	Pointer to an initialized servo handle.
[out]	load_out	Pointer to store the signed load value.

Returns: sts_result_t STS_OK on success, or specific error code.

◆ STS_GetPresentPosition()

sts_result_t STS_GetPresentPosition	(	sts_servo_t *	servo,
		uint16_t *	position_out
	)

Reads the current actual position from the servo.

Parameters

	servo	Pointer to an initialised servo handle.
[out]	position_out	Pointer to store the 16-bit position.

Returns: sts_result_t STS_OK on success, or specific error code.

◆ STS_GetPresentSpeed()

sts_result_t STS_GetPresentSpeed	(	sts_servo_t *	servo,
		int16_t *	speed_out
	)

Reads the current rotational speed of the servo.

Parameters

	servo	Pointer to an initialised servo handle.
[out]	speed_out	Pointer to store the 16-bit speed value.

Returns: sts_result_t STS_OK on success, or specific error code.

◆ STS_GetPresentTemperature()

sts_result_t STS_GetPresentTemperature	(	sts_servo_t *	servo,
		uint8_t *	temp_out
	)

Reads the internal temperature of the servo's MCU/Motor driver.

Parameters

	servo	Pointer to an initialized servo handle.
[out]	temp_out	Pointer to store the temperature in degrees Celsius.

Returns: sts_result_t STS_OK on success, or specific error code.

◆ STS_GetPresentVoltage()

sts_result_t STS_GetPresentVoltage	(	sts_servo_t *	servo,
		uint8_t *	voltage_out
	)

Reads the current input voltage at the servo's power terminals.

Parameters

	servo	Pointer to an initialized servo handle.
[out]	voltage_out	Pointer to store the voltage (in 0.1V units, e.g., 120 = 12.0V).

Returns: sts_result_t STS_OK on success, or speciifc error code.

◆ STS_SetEEPROMLock()

sts_result_t STS_SetEEPROMLock	(	sts_servo_t *	servo,
		uint8_t	lock
	)

Toggles the EEPROM write protection.

Note: Must be unlocked (0) before modifying registers 0x00 to 0x20, and immediately locked (1) afterward to prevent flash corruption.

Parameters

servo	Pointer to an initialized servo handle.
lock	EEPROM_LOCK to protect, EEPROM_UNLOCK to unprotect.

Returns: sts_result_t STS_OK on success, or specific error code.

◆ STS_SetID()

sts_result_t STS_SetID	(	sts_servo_t *	servo,
		uint8_t	new_id
	)

Assigns a new permanent ID to the servo.

Note: EEPROM must be unlocked prior to calling. Changes take effect immediately.

Warning: DANGER: Unlocks EEPROM. Repeated writes will PERMANENTLY DAMAGE the servo hardware.

Parameters

servo	Pointer to an initialized servo handle.
new_id	The new ID (0 to STS_ID_BROADCAST_SYNC).

Returns: sts_result_t STS_OK on success, STS_ERR_INVALID_PARAM if ID is out of bounds.

◆ STS_SetOperatingMode()

sts_result_t STS_SetOperatingMode	(	sts_servo_t *	servo,
		sts_operating_mode_t	mode
	)

Sets the operating mode of the servo.

Parameters

servo	Pointer to an initialised servo handle.
mode	The desired operating mode (Position, Speed, PWM, Step).

Returns: sts_result_t STS_OK on success, or specific error code.

◆ STS_SetTarget()

sts_result_t STS_SetTarget	(	sts_servo_t *	servo,
		int32_t	target
	)

Universal target wrapper that routes the command based on current mode.

Note: In Position mode, negative targets are automatically clamped to 0.

Parameters

servo	Pointer to an initialized servo handle.
target	The desired target value. Bounds depend on active mode.

Returns: sts_result_t STS_OK on success, or specific error code.

◆ STS_SetTargetAcceleration()

sts_result_t STS_SetTargetAcceleration	(	sts_servo_t *	servo,
		uint8_t	acceleration
	)

Sets the acceleration profile for servo movements.

Parameters

servo	Pointer to an initialized servo handle.
acceleration	The acceleration rate (0 to STS_MAX_ACCELERATION).

Returns: sts_result_t STS_OK on success, or specific error code.

◆ STS_SetTargetPosition()

sts_result_t STS_SetTargetPosition	(	sts_servo_t *	servo,
		uint16_t	position
	)

Sets the absolute target position for the servo.

Parameters

servo	Pointer to an initialised servo handle.
position	0 to STS_MAX_POSITION.

Returns: sts_result_t STS_OK on success, or specific error code.

◆ STS_SetTargetPWM()

sts_result_t STS_SetTargetPWM	(	sts_servo_t *	servo,
		uint16_t	pwm,
		sts_direction_t	dir
	)

Sets the open-loop PWM duty cycle (effort) applied to motor coils.

Note: Servo must be in PWM Mode (Mode 2).

Parameters

servo	Pointer to an initialized servo handle.
pwm	Raw PWM effort value (0 to STS_MAX_PWM).
dir	The direction to apply force (STS_DIR_CW or STS_DIR_CCW).

Returns: sts_result_t STS_OK on success, or specific error code.

◆ STS_SetTargetSpeed()

sts_result_t STS_SetTargetSpeed	(	sts_servo_t *	servo,
		uint16_t	speed,
		sts_direction_t	dir
	)

Sets the target speed and direction of the servo.

Note: In Mode 1 (Speed) and 3 (Step), dictates movement vector. In Mode 0 (Position), acts as a speed limit.

Parameters

servo	Pointer to an initialized servo handle.
speed	The target speed magnitude (0 to STS_MAX_SPEED).
dir	The desired rotation direction (STS_DIR_CCW or STS_DIR_CW).

Returns: sts_result_t STS_OK on success, or speciifc error code.

◆ STS_SetTargetStep()

sts_result_t STS_SetTargetStep	(	sts_servo_t *	servo,
		uint16_t	steps,
		sts_direction_t	dir
	)

Commands the servo to move a relative number of steps.

Note: Servo must be in Step Mode (Mode 3).

Parameters

servo	Pointer to an initialized servo handle.
steps	Number of relative steps to execute (0 to STS_MAX_STEP).
dir	The direction of rotation (STS_DIR_CW or STS_DIR_CCW).

Returns: sts_result_t STS_OK on success, or specific error code.

◆ STS_SetTorqueEnable()

sts_result_t STS_SetTorqueEnable	(	sts_servo_t *	servo,
		uint8_t	enable
	)

Enables or disables motor torque output.

Parameters

servo	Pointer to an initialised servo handle.
enable	Non-zero to enable torque, zero to disable (free spin).

Returns: sts_result_t STS_OK on success, or specific error code.

◆ STS_SetTorqueLimit()

sts_result_t STS_SetTorqueLimit	(	sts_servo_t *	servo,
		uint16_t	limit
	)

Sets the dynamic (RAM) maximum torque limit.

Note: Safe for continuous dynamic updates. Reverts to EEPROM default on power cycle.

Parameters

servo	Pointer to an initialized servo handle.
limit	Max torque value (0 to STS_MAX_TORQUE). 0 disables torque.

Returns: sts_result_t STS_OK on success, or specifc error code.

Functions

Detailed Description

Function Documentation

◆ STS_GetMovingStatus()

◆ STS_GetPresentLoad()

◆ STS_GetPresentPosition()

◆ STS_GetPresentSpeed()

◆ STS_GetPresentTemperature()

◆ STS_GetPresentVoltage()

◆ STS_SetEEPROMLock()

◆ STS_SetID()

◆ STS_SetOperatingMode()

◆ STS_SetTarget()

◆ STS_SetTargetAcceleration()

◆ STS_SetTargetPosition()

◆ STS_SetTargetPWM()

◆ STS_SetTargetSpeed()

◆ STS_SetTargetStep()

◆ STS_SetTorqueEnable()

◆ STS_SetTorqueLimit()