🧠 平滑舵機控制系統
一、項目概述
本項目基於 Raspberry Pi + PCA9685 + 舵機 實現了一個平滑的舵機控制系統。
通過 adafruit_servokit.ServoKit 控制 PWM 信號輸出，使舵機在指定角度範圍內進行平滑往返運動。
系統支持速度調節、緩動曲線（Easing Function）和平滑控制，可用於機械臂、雲台、舵機測試平台等應用場景。
二、系統組成與依賴
1. 硬件組成
- Raspberry Pi 4 / 5
- PCA9685 16通道舵機控制模塊
- 舵機（如 SG90、MG996R 等）
- 外部電源（推薦 5V 2A 或更高）
2. 軟件依賴
|
庫名
|
説明
|
安裝命令
|
|
|
控制 PCA9685 輸出舵機信號
|
|
|
|
Python 標準庫
|
無需安裝
|
三、核心類與方法
1. 類：SmoothServoController
該類封裝了 PCA9685 通道的初始化與舵機運動控制。
初始化方法：
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. 平滑往返運動函數
def smooth_pingpong(
self,
angle_range=(0, 180),
speed=1.0,
easing="ease_in_out",
duration=None,
cycles=None
)功能：
讓舵機在指定角度範圍內往返運動，並使用緩動算法控制加減速，使運動更加自然。
參數説明：
|
參數
|
類型
|
默認值
|
説明
|
|
|
tuple
|
(0, 180)
|
運動角度範圍
|
|
|
float
|
1.0
|
運動速度（0.1-2.0）
|
|
|
str
|
"ease_in_out"
|
緩動函數類型
|
|
|
float
|
None
|
運行時間（秒），None 表示無限循環
|
|
|
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、安裝依賴
sudo pip3 install adafruit-circuitpython-servokit
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
|
|
勻速運動
|
機械運動
|
|
ease_in_out
|
|
平滑啓停
|
自然運動
|
|
ease_in
|
|
緩慢啓動
|
加速出場
|
|
ease_out
|
|
緩慢停止
|
減速入場
|
|
sine
|
|
正弦曲線
|
波浪運動
|
|
circular
|
|
圓形軌跡
|
特殊效果
|
4、全部代碼
#!/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 動態調整角度）；
- 加入舵機狀態反饋（角度、速度、電流檢測）。
本文章為轉載內容，我們尊重原作者對文章享有的著作權。如有內容錯誤或侵權問題，歡迎原作者聯繫我們進行內容更正或刪除文章。