🧠 平滑舵机控制系统
一、项目概述
本项目基于 Raspberry Pi + PCA9685 + 舵机 实现了一个平滑的舵机控制系统。
通过 adafruit_servokit.ServoKit 控制 PWM 信号输出，使舵机在指定角度范围内进行平滑往返运动。
系统支持速度调节、缓动曲线（Easing Function）和平滑控制，可用于机械臂、云台、舵机测试平台等应用场景。
二、系统组成与依赖
1. 硬件组成
-
Raspberry Pi 4 / 5
-
PCA9685 16通道舵机控制模块
-
舵机（如 SG90、MG996R 等）
-
外部电源（推荐 5V 2A 或更高）
2. 软件依赖
| 库名 | 说明 | 安装命令 |
|---|---|---|
adafruit-circuitpython-servokit |
控制 PCA9685 输出舵机信号 | pip install adafruit-circuitpython-servokit |
math、time |
Python 标准库 | 无需安装 |
三、核心类与方法
1. 类：SmoothServoController
该类封装了 PCA9685 通道的初始化与舵机运动控制。
初始化方法：
python
def __init__(self, channel=0, pulse_min=1000, pulse_max=2000)参数：
-
channel: 使用的 PCA9685 通道号 (0--15) -
pulse_min: 最小 PWM 脉宽（μs） -
pulse_max: 最大 PWM 脉宽（μs）
功能说明：
-
初始化 ServoKit 实例；
-
设置舵机脉宽范围；
-
默认将舵机角度设为 90°；
-
打印初始化状态。
2. 平滑往返运动函数
python
def smooth_pingpong(
self,
angle_range=(0, 180),
speed=1.0,
easing="ease_in_out",
duration=None,
cycles=None
)功能：
让舵机在指定角度范围内往返运动，并使用缓动算法控制加减速，使运动更加自然。
参数说明：
| 参数 | 类型 | 默认值 | 说明 |
|---|---|---|---|
angle_range |
tuple | (0, 180) | 运动角度范围 |
speed |
float | 1.0 | 运动速度（0.1-2.0） |
easing |
str | "ease_in_out" | 缓动函数类型 |
duration |
float | None | 运行时间（秒），None 表示无限循环 |
cycles |
int | None | 循环次数，None 表示无限循环 |
实现逻辑：
-
解析角度范围；
-
检查输入参数是否合法；
-
启动平滑运动；
-
通过正弦曲线函数实现"缓动效果"；
-
当达到最大角度后，自动反向回到最小角度，形成循环；
-
支持设定运行时间或循环次数。
四、缓动算法说明（Easing Function）
舵机运动采用正弦函数实现缓入缓出（ease-in-out）效果：
其中：
-
ttt 为归一化时间（0 → 1）
-
该函数在开始和结束时速度较慢，中间加速，实现自然的平滑过渡。
五、系统工作原理
-
Raspberry Pi 通过 I²C 与 PCA9685 通信；
-
ServoKit库控制 PCA9685 输出 PWM； -
PWM 信号控制舵机转动角度；
-
使用
smooth_pingpong函数循环输出角度； -
通过
time.sleep()控制更新频率，模拟连续运动。
六、运行方法
1、硬件组成
-
Raspberry Pi 4 / 5
-
PCA9685 16通道舵机控制模块
-
舵机（如 SG90、MG996R 等）
-
外部电源（推荐 5V 2A 或更高）
2、安装依赖
python
sudo pip3 install adafruit-circuitpython-servokit
python
def _smooth_move(self, start_angle, end_angle, speed, easing_type, direction):
steps = max(20, int(50 * speed)) # 动态计算步数
total_time = 1.0 / speed # 速度控制总时间
for step in range(steps + 1):
progress = step / steps # 线性进度 0.0 → 1.0
eased_progress = self._apply_easing(progress, easing_type) # 应用缓动
current_angle = start_angle + (end_angle - start_angle) * eased_progress
self._set_angle_smooth(current_angle, total_time / steps)3、动函数对比
| 缓动类型 | 数学公式 | 运动特点 | 适用场景 |
|---|---|---|---|
| linear | progress |
匀速运动 | 机械运动 |
| ease_in_out | progress²(3-2progress) |
平滑启停 | 自然运动 |
| ease_in | progress² |
缓慢启动 | 加速出场 |
| ease_out | 1-(1-progress)² |
缓慢停止 | 减速入场 |
| sine | -(cos(πprogress)-1)/2 |
正弦曲线 | 波浪运动 |
| circular | 1-√(1-progress²) |
圆形轨迹 | 特殊效果 |
4、全部代码
python
#!/usr/bin/env python3
"""
平滑舵机控制器-lianying
功能：实现舵机的平滑运动控制，支持多种缓动效果
"""
import time
import math
from adafruit_servokit import ServoKit
class SmoothServoController:
"""
平滑舵机控制器类
提供舵机的平滑运动控制，支持多种缓动效果和运动参数调节
"""
def __init__(self, channel=0, pulse_min=1000, pulse_max=2000):
"""
初始化平滑舵机控制器
Args:
channel: PCA9685通道号 (0-15)
pulse_min: 最小脉冲宽度(微秒)，默认1000
pulse_max: 最大脉冲宽度(微秒)，默认2000
"""
self.channel = channel
# 初始化PCA9685舵机控制板
self.kit = ServoKit(channels=16)
# 设置舵机脉冲宽度范围
self.kit.servo[channel].set_pulse_width_range(pulse_min, pulse_max)
# 初始化状态变量
self.current_angle = 90 # 默认中间位置
self.kit.servo[channel].angle = self.current_angle
self.is_running = False # 运行状态标志
print(f"✅ 平滑舵机控制器初始化完成")
print(f" - 通道: {channel}")
print(f" - 脉冲范围: {pulse_min}-{pulse_max}μs")
print(f" - 初始角度: {self.current_angle}°")
def smooth_pingpong(self, angle_range=(0, 180), speed=1.0,
easing="ease_in_out", duration=None, cycles=None):
"""
平滑往返运动 - 支持角度范围入参
Args:
angle_range: 角度范围元组 (min_angle, max_angle)，默认(0, 180)
speed: 运动速度系数 (0.1-2.0)，默认1.0
easing: 缓动函数类型，默认"ease_in_out"
duration: 总运行时间(秒)，None表示无限循环
cycles: 运行循环次数，None表示无限循环
"""
# 解析角度范围
min_angle, max_angle = self._parse_angle_range(angle_range)
# 验证参数合法性
self._validate_parameters(min_angle, max_angle, speed)
# 设置运行状态
self.is_running = True
start_time = time.time()
cycle_count = 0
# 显示运动参数
print(f"🚀 开始平滑往返运动")
print(f" 角度范围: {min_angle}° - {max_angle}°")
print(f" 运动速度: {speed}")
print(f" 缓动类型: {easing}")
if duration:
print(f" 持续时间: {duration}秒")
if cycles:
print(f" 循环次数: {cycles}次")
print(" 按 Ctrl+C 停止运动")
print("-" * 50)
try:
while self.is_running:
# 检查循环次数限制
if cycles and cycle_count >= cycles:
print(f"✅ 达到指定循环次数 {cycles}次，停止运动")
break
# 正向运动 (min_angle -> max_angle)
self._smooth_move(min_angle, max_angle, speed, easing, "正向")
# 反向运动 (max_angle -> min_angle)
self._smooth_move(max_angle, min_angle, speed, easing, "反向")
cycle_count += 1
# 显示进度信息
if duration or cycles:
self._display_progress(cycle_count, start_time, duration, cycles)
# 检查是否达到指定持续时间
if duration and (time.time() - start_time) >= duration:
print(f"✅ 达到指定持续时间 {duration}秒，停止运动")
break
except KeyboardInterrupt:
print("\n🛑 用户中断运动")
except Exception as e:
print(f"❌ 运动错误: {e}")
finally:
self.is_running = False
# 回到安全位置（范围中间值）
safe_angle = (min_angle + max_angle) / 2
self._set_angle_smooth(safe_angle, 0.5)
print(f"🎯 运动结束，共完成 {cycle_count} 个循环")
print(f"📌 舵机已回到安全位置: {safe_angle}°")
def _parse_angle_range(self, angle_range):
"""
解析角度范围参数
Args:
angle_range: 角度范围参数
Returns:
tuple: (min_angle, max_angle)
Raises:
ValueError: 参数格式错误时抛出
"""
if isinstance(angle_range, (list, tuple)) and len(angle_range) == 2:
min_angle, max_angle = angle_range
else:
raise ValueError("angle_range 必须是包含两个元素的元组或列表 (min_angle, max_angle)")
# 确保min_angle <= max_angle
if min_angle > max_angle:
min_angle, max_angle = max_angle, min_angle
print(f"⚠️ 角度范围已自动校正: ({min_angle}, {max_angle})")
return min_angle, max_angle
def _validate_parameters(self, min_angle, max_angle, speed):
"""
验证参数有效性
Args:
min_angle: 最小角度
max_angle: 最大角度
speed: 速度系数
Raises:
ValueError: 参数无效时抛出
"""
if not (0 <= min_angle <= 180 and 0 <= max_angle <= 180):
raise ValueError("角度必须在 0-180 度范围内")
if min_angle == max_angle:
raise ValueError("最小角度和最大角度不能相同")
if not (0.1 <= speed <= 2.0):
raise ValueError("速度系数必须在 0.1-2.0 范围内")
def _smooth_move(self, start_angle, end_angle, speed, easing_type, direction):
"""
执行单次平滑移动
Args:
start_angle: 起始角度
end_angle: 结束角度
speed: 速度系数
easing_type: 缓动类型
direction: 运动方向描述
"""
# 根据速度计算步数和总时间
steps = max(20, int(50 * speed)) # 根据速度调整步数
total_time = 1.0 / speed # 总时间随速度变化
print(f"🔁 {direction}运动: {start_angle}° → {end_angle}°")
for step in range(steps + 1):
if not self.is_running:
break
# 计算当前进度 (0.0 - 1.0)
progress = step / steps
# 应用缓动函数
eased_progress = self._apply_easing(progress, easing_type)
# 计算当前角度（线性插值）
current_angle = start_angle + (end_angle - start_angle) * eased_progress
# 设置角度并等待
self._set_angle_smooth(current_angle, total_time / steps)
def _apply_easing(self, progress, easing_type):
"""
应用不同的缓动函数
Args:
progress: 线性进度 (0.0-1.0)
easing_type: 缓动类型
Returns:
float: 缓动后的进度值
"""
easing_type = easing_type.lower()
if easing_type == "linear":
# 线性缓动：匀速运动
return progress
elif easing_type in ["ease_in_out", "easeinout"]:
# 平滑的缓入缓出：开始和结束慢，中间快
return progress * progress * (3 - 2 * progress)
elif easing_type in ["ease_out", "easeout"]:
# 缓出函数：开始快，结束慢
return 1 - (1 - progress) * (1 - progress)
elif easing_type in ["ease_in", "easein"]:
# 缓入函数：开始慢，结束快
return progress * progress
elif easing_type == "quadratic":
# 二次方缓动
if progress < 0.5:
return 2 * progress * progress
else:
return 1 - (-2 * progress + 2) * (-2 * progress + 2) / 2
elif easing_type == "sine":
# 正弦缓动：自然的波浪运动
return -(math.cos(math.pi * progress) - 1) / 2
elif easing_type == "circular":
# 圆形缓动：圆形轨迹运动
return 1 - math.sqrt(1 - math.pow(progress, 2))
else:
print(f"⚠️ 未知的缓动类型 '{easing_type}'，使用默认线性缓动")
return progress
def _set_angle_smooth(self, angle, step_duration):
"""
平滑设置角度
Args:
angle: 目标角度
step_duration: 步进持续时间
"""
self.kit.servo[self.channel].angle = angle
self.current_angle = angle
time.sleep(step_duration)
def _display_progress(self, cycle_count, start_time, duration, cycles):
"""
显示进度信息
Args:
cycle_count: 当前循环次数
start_time: 开始时间
duration: 总持续时间
cycles: 总循环次数
"""
elapsed = time.time() - start_time
if duration:
progress_percent = min(100, (elapsed / duration) * 100)
print(f"📊 进度: {progress_percent:.1f}% ({elapsed:.1f}s/{duration}s) | 循环: {cycle_count}", end='\r')
elif cycles:
progress_percent = min(100, (cycle_count / cycles) * 100)
print(f"📊 进度: {progress_percent:.1f}% ({cycle_count}/{cycles}循环) | 时间: {elapsed:.1f}s", end='\r')
def stop(self):
"""停止运动"""
self.is_running = False
print("⏹️ 停止运动指令已发送")
def get_current_angle(self):
"""获取当前角度"""
return self.current_angle
def set_angle(self, angle):
"""
直接设置角度（无平滑效果）
Args:
angle: 目标角度 (0-180)
"""
if 0 <= angle <= 180:
self.kit.servo[self.channel].angle = angle
self.current_angle = angle
print(f"📌 舵机角度设置为: {angle}°")
else:
print("❌ 角度必须在0-180范围内")
# 使用示例和演示函数
def basic_demo():
"""
基础演示函数
展示控制器的基本功能
"""
print("=" * 60)
print("🤖 平滑舵机控制器 - 基础演示")
print("=" * 60)
# 创建控制器实例
controller = SmoothServoController(channel=0)
try:
print("\n1. 小范围平滑运动演示")
controller.smooth_pingpong(
angle_range=(60, 120), # 60-120度范围
speed=0.8, # 中等速度
easing="ease_in_out", # 平滑缓动
cycles=2 # 运行2个循环
)
print("\n2. 快速往返运动演示")
controller.smooth_pingpong(
angle_range=(30, 150), # 30-150度范围
speed=1.5, # 快速
easing="linear", # 线性运动
cycles=3 # 运行3个循环
)
except KeyboardInterrupt:
print("\n演示被用户中断")
except Exception as e:
print(f"演示错误: {e}")
finally:
controller.stop()
def easing_comparison_demo():
"""
缓动函数对比演示
展示不同缓动函数的运动效果
"""
print("\n" + "=" * 60)
print("📊 缓动函数对比演示")
print("=" * 60)
controller = SmoothServoController(channel=0)
# 缓动函数列表
easing_types = ["linear", "ease_in", "ease_out", "ease_in_out", "sine"]
for easing in easing_types:
try:
print(f"\n🎯 测试缓动函数: {easing}")
controller.smooth_pingpong(
angle_range=(45, 135),
speed=1.0,
easing=easing,
cycles=1
)
time.sleep(1) # 演示间隔
except KeyboardInterrupt:
print("\n对比演示被中断")
break
except Exception as e:
print(f"缓动测试错误: {e}")
controller.stop()
def interactive_control():
"""
交互式控制模式
用户可以通过命令行实时控制舵机
"""
print("\n" + "=" * 60)
print("🎮 交互式舵机控制模式")
print("=" * 60)
controller = SmoothServoController(channel=0)
while True:
print("\n请选择操作:")
print("1. 启动平滑往返运动")
print("2. 直接设置角度")
print("3. 显示当前角度")
print("4. 停止运动")
print("5. 退出程序")
choice = input("请输入选择 (1-5): ").strip()
if choice == "1":
try:
print("\n🚀 设置运动参数:")
min_angle = float(input("最小角度 (0-180) [默认0]: ") or "0")
max_angle = float(input("最大角度 (0-180) [默认180]: ") or "180")
speed = float(input("速度 (0.1-2.0) [默认1.0]: ") or "1.0")
easing = input("缓动类型 (linear/ease_in/ease_out/ease_in_out/sine) [默认ease_in_out]: ") or "ease_in_out"
cycles = input("循环次数 [默认无限]: ")
cycles = int(cycles) if cycles else None
controller.smooth_pingpong(
angle_range=(min_angle, max_angle),
speed=speed,
easing=easing,
cycles=cycles
)
except ValueError as e:
print(f"❌ 参数错误: {e}")
except Exception as e:
print(f"❌ 错误: {e}")
elif choice == "2":
try:
angle = float(input("请输入角度 (0-180): "))
controller.set_angle(angle)
except ValueError:
print("❌ 请输入有效的角度数值")
elif choice == "3":
print(f"📊 当前角度: {controller.get_current_angle()}°")
elif choice == "4":
controller.stop()
elif choice == "5":
controller.stop()
print("👋 再见！")
break
else:
print("❌ 无效选择，请重新输入")
def educational_examples():
"""
教学示例集合
展示各种使用场景和参数组合
"""
print("\n" + "=" * 60)
print("🎓 教学示例集合")
print("=" * 60)
examples = [
{
"name": "默认参数运动",
"params": {"angle_range": (0, 180), "speed": 1.0, "easing": "ease_in_out"}
},
{
"name": "小范围精密运动",
"params": {"angle_range": (80, 100), "speed": 0.5, "easing": "ease_in_out"}
},
{
"name": "快速扫描运动",
"params": {"angle_range": (20, 160), "speed": 2.0, "easing": "linear"}
},
{
"name": "正弦波浪运动",
"params": {"angle_range": (30, 150), "speed": 0.7, "easing": "sine"}
},
]
controller = SmoothServoController(channel=0)
for i, example in enumerate(examples, 1):
print(f"\n{i}. {example['name']}")
print(f" 参数: {example['params']}")
try:
controller.smooth_pingpong(
cycles=1,
**example['params']
)
time.sleep(1)
except KeyboardInterrupt:
print("\n示例演示被中断")
break
except Exception as e:
print(f"示例错误: {e}")
controller.stop()
# 主程序入口
if __name__ == "__main__":
print("🚀 平滑舵机控制器启动")
try:
while True:
print("\n" + "=" * 60)
print("🏠 主菜单")
print("=" * 60)
print("1. 基础演示")
print("2. 缓动函数对比")
print("3. 交互式控制")
print("4. 教学示例")
print("5. 退出程序")
choice = input("请选择功能 (1-5): ").strip()
if choice == "1":
basic_demo()
elif choice == "2":
easing_comparison_demo()
elif choice == "3":
interactive_control()
elif choice == "4":
educational_examples()
elif choice == "5":
print("👋 感谢使用平滑舵机控制器！")
break
else:
print("❌ 无效选择，请重新输入")
except KeyboardInterrupt:
print("\n\n👋 程序被用户中断，再见！")
except Exception as e:
print(f"\n❌ 程序错误: {e}")七、调试与优化建议
-
⚙️ 如果舵机抖动或角度异常，检查电源供电是否稳定；
-
🔄 如果希望快速运动，可增大
speed； -
🕒 若需自动停止，可设置
duration； -
🔧 如需更高精度控制，可在 PCA9685 初始化时设置频率（默认 50Hz）。
八、扩展方向
-
支持多通道同步控制；
-
增加多种缓动函数（ease-in、ease-out、bounce 等）；
-
支持实时控制（例如通过 MQTT 或 WebSocket 动态调整角度）；
-
加入舵机状态反馈（角度、速度、电流检测）。