PWM Control for Dynamixel Actuators
This Wiki entry will walk you through how to achieve PWM control on the Dynamixel MX-28.
PWM (Pulse-Width Modulation) control is effectively equivalent to torque control at the speeds used in the research conducted using SPOT. Briefly, torque produced by a DC motor is proportional to the current flowing through it through:
Where (Kt) is the motor torque constant. PWM controls the average voltage applied to the motor through:
Where the average voltage is equal to the PWM duty cycle times the supply voltage. The Voltage-Current relational in the a motor at steady state is:
Where (V) is the applied voltage, (I) is the motor current, (R) is the motor resistance, (Ke) is the back-EMF constant, and (omega) is the motor velocity. At low-speeds, which is the case for the use case in the laboratory, where back-EMF is minimal the relationship becomes:
Therefore, we get:
Practically, what this means is that the output from a control law (say a PID) can be directly mapped to PWM values. As a bonus, using the PWM control mode sets PWM limits that prevent excessive current draw and result in smoother control (as a result of the high-frequency nature of the PWM signal).
The following Python code was written to demonstrate position-based PWM control using a single MX-28 actuator. Users can simply take this code and modify it as needed.
import time
import sys
import msvcrt
from dynamixel_sdk import * # Uses Dynamixel SDK library
import scipy.io
import numpy as np
# ====================================================================================
# CONTROL TABLE ADDRESSES FOR MX-28 (PROTOCOL 2.0)
# These addresses tell the SDK where to read/write specific motor parameters
# ====================================================================================
ADDR_PWM = 100 # Goal PWM address - controls motor torque/effort
ADDR_PRESENT_POSITION = 132 # Present Position address - reads current encoder value
ADDR_TORQUE_ENABLE = 64 # Torque Enable address - turns motor on/off
# ====================================================================================
# COMMUNICATION SETTINGS
# ====================================================================================
PROTOCOL_VERSION = 2.0 # MX-28 uses Protocol 2.0 (newer Dynamixels)
DXL_ID = 4 # Dynamixel ID number (set via Dynamixel Wizard)
BAUDRATE = 57600 # Serial communication speed
DEVICENAME = 'COM3' # Serial port (Windows: COMX, Linux: /dev/ttyUSBX)
# ====================================================================================
# PID CONTROLLER GAINS
# These values determine how aggressively the motor responds to position errors
# ====================================================================================
KP = 0.5 # Proportional gain - main response to current error
KI = 0.01 # Integral gain - corrects accumulated error over time
KD = 0.1 # Derivative gain - dampens rapid changes to prevent oscillation
# Torque enable/disable constants
TORQUE_ENABLE = 1 # Value to enable motor torque
TORQUE_DISABLE = 0 # Value to disable motor torque
# Target position for the servo (0-4095 range for 12-bit encoder)
# 2048 is center, 0 is full CCW, 4095 is full CW
desired_position = 4000 # Example desired position
# Initialize PortHandler and PacketHandler for Dynamixel communication
portHandler = PortHandler(DEVICENAME) # Handles serial port operations
packetHandler = PacketHandler(PROTOCOL_VERSION) # Handles packet protocol
# ====================================================================================
# DATA STORAGE LISTS
# These lists store all sensor data for later analysis and saving to file
# ====================================================================================
positions = [] # Actual motor positions over time
velocities = [] # Calculated velocities (position change over time)
pwm_commands = [] # PWM commands sent to motor
errors = [] # Position errors (desired - actual)
timestamps = [] # Absolute time stamps
times = [] # Relative time since start
def getch():
"""Get single character input from keyboard (Windows only)"""
return msvcrt.getch().decode()
def main():
# ====================================================================================
# ESTABLISH CONNECTION TO DYNAMIXEL
# ====================================================================================
# Try to open the serial port
if not portHandler.openPort():
print('Failed to open the port')
sys.exit()
# Set the communication baudrate
if not portHandler.setBaudRate(BAUDRATE):
print('Failed to set baudrate')
sys.exit()
print('Connected to Dynamixel on', DEVICENAME)
# ====================================================================================
# ENABLE MOTOR TORQUE
# This allows the motor to hold position and respond to commands
# ====================================================================================
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(
portHandler, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_ENABLE
)
# Check for communication errors
if dxl_comm_result != COMM_SUCCESS:
print('Comm error (torque enable):', packetHandler.getTxRxResult(dxl_comm_result))
sys.exit()
elif dxl_error != 0:
print('DXL error (torque enable):', packetHandler.getRxPacketError(dxl_error))
sys.exit()
# ====================================================================================
# INITIALIZE PID CONTROLLER VARIABLES
# ====================================================================================
integral = 0 # Accumulated error for integral term
prev_error = 0 # Previous error for derivative term
prev_position = None # Previous position for velocity calculation
prev_time = None # Previous timestamp for velocity calculation
try:
start_time = time.time() # Record start time for relative timestamps
# ====================================================================================
# MAIN CONTROL LOOP
# Runs continuously until interrupted with Ctrl+C
# ====================================================================================
while True:
now = time.time() # Current timestamp
# ====================================================================================
# STEP 1: READ CURRENT POSITION FROM ENCODER
# Position is a 32-bit value (0-4095 for MX-28's 12-bit encoder)
# ====================================================================================
pos_result, comm_result, pos_error = packetHandler.read4ByteTxRx(
portHandler, DXL_ID, ADDR_PRESENT_POSITION
)
# Skip this iteration if communication failed
if comm_result != COMM_SUCCESS:
print('Comm error:', packetHandler.getTxRxResult(comm_result))
continue
elif pos_error != 0:
print('DXL error:', packetHandler.getRxPacketError(pos_error))
continue
present_position = pos_result
# Calculate time elapsed since last iteration
if prev_time is not None:
dt = now - prev_time
else:
dt = 0.02 # Assume 50 Hz if first iteration
# ====================================================================================
# STEP 2: CALCULATE PID CONTROL OUTPUT
# PID formula: output = Kp*error + Ki*integral + Kd*derivative
# ====================================================================================
# Calculate position error (positive means we need to move forward)
error = desired_position - present_position
# Integral term: accumulated error over time (eliminates steady-state error)
integral += error * dt
# Derivative term: rate of error change (prevents overshoot/oscillation)
if dt > 0:
derivative = (error - prev_error) / dt
else:
derivative = 0
# Combine all three PID terms
pid_output = KP * error + KI * integral + KD * derivative
# Update previous error for next iteration
prev_error = error
# ====================================================================================
# STEP 3: CONVERT PID OUTPUT TO PWM COMMAND
# PWM range for MX-28 is -885 to +885
# Negative values = CCW torque, Positive = CW torque
# ====================================================================================
# Clamp PWM value to safe operating range
pwm_value = int(max(min(pid_output, 885), -885))
# Send PWM command to motor (16-bit value)
dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(
portHandler, DXL_ID, ADDR_PWM, pwm_value & 0xFFFF
)
# Report any communication errors (but don't stop)
if dxl_comm_result != COMM_SUCCESS:
print('Comm error:', packetHandler.getTxRxResult(dxl_comm_result))
elif dxl_error != 0:
print('DXL error:', packetHandler.getRxPacketError(dxl_error))
# ====================================================================================
# STEP 4: CALCULATE VELOCITY (OPTIONAL)
# Velocity = change in position / change in time
# ====================================================================================
if prev_position is not None and prev_time is not None:
# Calculate velocity in encoder units per second
velocity = (present_position - prev_position) / (now - prev_time)
else:
velocity = 0.0 # First iteration has no velocity
# Update previous values for next velocity calculation
prev_position = present_position
prev_time = now
# ====================================================================================
# STEP 5: SAVE DATA FOR ANALYSIS
# All data is stored in lists for later export to MATLAB
# ====================================================================================
positions.append(present_position)
velocities.append(velocity)
pwm_commands.append(pwm_value)
errors.append(error)
timestamps.append(now) # Absolute time
times.append(now - start_time) # Time since start
# Display current status to console
print(f'Pos: {present_position}, Vel: {velocity:.2f}, PWM: {pwm_value}, Error: {error}')
# Control loop delay (50 Hz = 0.02 seconds per iteration)
time.sleep(0.02)
except KeyboardInterrupt:
# User pressed Ctrl+C to stop
print('\nShutting down...')
finally:
# ====================================================================================
# CLEANUP AND SHUTDOWN
# Always execute this code, even if an error occurred
# ====================================================================================
# Stop the motor by setting PWM to 0
packetHandler.write2ByteTxRx(portHandler, DXL_ID, ADDR_PWM, 0)
# Disable torque (motor becomes free-moving)
packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_DISABLE)
# Close the serial port
portHandler.closePort()
print('Port closed.')
# ====================================================================================
# SAVE ALL DATA TO MATLAB FILE
# Creates a .mat file that can be loaded in MATLAB for analysis
# ====================================================================================
# Create dictionary with all logged data
data = {
'position': np.array(positions), # Motor positions
'velocity': np.array(velocities), # Calculated velocities
'pwm_command': np.array(pwm_commands), # PWM commands sent
'error': np.array(errors), # Position errors
'timestamp': np.array(timestamps), # Absolute timestamps
'time': np.array(times) # Relative time from start
}
# Save to MATLAB format file
scipy.io.savemat('dynamixel_data.mat', {'data': data})
print('Data saved to dynamixel_data.mat')
if __name__ == '__main__':
main()