当前位置：首页 > news >正文

瓦力机器人-舵机控制（基于树莓派5）

news 2025/10/13 7:12:03

🧠 平滑舵机控制系统

一、项目概述

本项目基于 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 通道的初始化与舵机运动控制。

初始化方法：

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
)

功能：
让舵机在指定角度范围内往返运动，并使用缓动算法控制加减速，使运动更加自然。

参数说明：

参数	类型	默认值	说明
`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）效果：

$angle=min+(max-min)*\frac{1-cos( \pi\-*t))}{2}$

其中：

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.0eased_progress = self._apply_easing(progress, easing_type)  # 应用缓动current_angle = start_angle + (end_angle - start_angle) * eased_progressself._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、全部代码

#!/usr/bin/env python3
"""
平滑舵机控制器-lianying
功能：实现舵机的平滑运动控制，支持多种缓动效果
"""import time
import math
from adafruit_servokit import ServoKitclass SmoothServoController:"""平滑舵机控制器类提供舵机的平滑运动控制，支持多种缓动效果和运动参数调节"""def __init__(self, channel=0, pulse_min=1000, pulse_max=2000):"""初始化平滑舵机控制器Args:channel: PCA9685通道号 (0-15)pulse_min: 最小脉冲宽度(微秒)，默认1000pulse_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_angleself.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.0easing: 缓动函数类型，默认"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 = Truestart_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}秒，停止运动")breakexcept KeyboardInterrupt:print("\n🛑 用户中断运动")except Exception as e:print(f"❌ 运动错误: {e}")finally:self.is_running = False# 回到安全位置（范围中间值）safe_angle = (min_angle + max_angle) / 2self._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_rangeelse:raise ValueError("angle_range 必须是包含两个元素的元组或列表 (min_angle, max_angle)")# 确保min_angle <= max_angleif min_angle > max_angle:min_angle, max_angle = max_angle, min_angleprint(f"⚠️  角度范围已自动校正: ({min_angle}, {max_angle})")return min_angle, max_angledef _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 progresselif 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 * progresselif easing_type == "quadratic":# 二次方缓动if progress < 0.5:return 2 * progress * progresselse:return 1 - (-2 * progress + 2) * (-2 * progress + 2) / 2elif easing_type == "sine":# 正弦缓动：自然的波浪运动return -(math.cos(math.pi * progress) - 1) / 2elif easing_type == "circular":# 圆形缓动：圆形轨迹运动return 1 - math.sqrt(1 - math.pow(progress, 2))else:print(f"⚠️  未知的缓动类型 '{easing_type}'，使用默认线性缓动")return progressdef _set_angle_smooth(self, angle, step_duration):"""平滑设置角度Args:angle: 目标角度step_duration: 步进持续时间"""self.kit.servo[self.channel].angle = angleself.current_angle = angletime.sleep(step_duration)def _display_progress(self, cycle_count, start_time, duration, cycles):"""显示进度信息Args:cycle_count: 当前循环次数start_time: 开始时间duration: 总持续时间cycles: 总循环次数"""elapsed = time.time() - start_timeif 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 = Falseprint("⏹️  停止运动指令已发送")def get_current_angle(self):"""获取当前角度"""return self.current_angledef set_angle(self, angle):"""直接设置角度（无平滑效果）Args:angle: 目标角度 (0-180)"""if 0 <= angle <= 180:self.kit.servo[self.channel].angle = angleself.current_angle = angleprint(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对比演示被中断")breakexcept 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 Nonecontroller.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("👋 再见！")breakelse: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示例演示被中断")breakexcept 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("👋 感谢使用平滑舵机控制器！")breakelse:print("❌ 无效选择，请重新输入")except KeyboardInterrupt:print("\n\n👋 程序被用户中断，再见！")except Exception as e:print(f"\n❌ 程序错误: {e}")