PWM Control for Dynamixel Actuators - Carleton-SRL/SPOT GitHub Wiki
Introduction
This Wiki entry will walk you through how to achieve PWM control on the Dynamixel MX-28.
PWM Control
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 relationship in 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).
Code Sample
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()