139 lines
4.5 KiB
Python
139 lines
4.5 KiB
Python
#!/usr/bin/env python
|
||
# -*- coding: utf-8 -*-
|
||
'''
|
||
# @Time : 2025/9/24 16:55
|
||
# @Author : reenrr
|
||
# @File : servo_motor_PWM.py
|
||
'''
|
||
#!/usr/bin/env python
|
||
# -*- coding: utf-8 -*-
|
||
'''
|
||
# @Time : 2025/9/24 10:30
|
||
# @Author : reenrr
|
||
# @File : servo_motor_pwm.py
|
||
# 功能描述：RK3506通过硬件PWM控制DS3235舵机（模拟持续旋转）
|
||
# 参考文档：DS3235 Coreless datasheet.pdf、瑞芯微RK3506 PWM控制器文档
|
||
'''
|
||
import time
|
||
import signal
|
||
import os
|
||
|
||
# -------------------------- 1. 硬件配置（必须根据实际接线调整） --------------------------
|
||
# 1.1 舵机PWM通道（RK3506的PWM通道，需与硬件接线对应）
|
||
# 瑞芯微RK3506通常有多个PWM通道，如pwm0, pwm1等，根据实际使用的通道修改
|
||
PWM_CHIP = 0 # PWM芯片编号
|
||
PWM_CHANNEL = 0 # PWM通道编号
|
||
PWM_PATH = f"/sys/class/pwm/pwmchip{PWM_CHIP}/pwm{PWM_CHANNEL}" # 已经通过设备树绑定到了相关的引脚上了
|
||
|
||
# 1.2 DS3235舵机核心参数（来自DS3235规格书）
|
||
PWM_PERIOD_MS = 20 # PWM周期：20ms（规格书4-6条“控制频率50Hz”，1/50Hz=20ms）
|
||
MIN_PULSE_US = 500 # 最小脉宽：500μsec（对应0°，规格书4-2条）
|
||
MAX_PULSE_US = 2500 # 最大脉宽：2500μsec（对应180°，规格书4-2条）
|
||
ROTATE_DELAY = 0.1 # 角度切换间隔（越小转动越“连续”，建议0.1-0.5秒）
|
||
|
||
# 1.3 舵机角度模式选择
|
||
MODE = "180" # 180°模式
|
||
|
||
# -------------------------- 2. 全局变量初始化 --------------------------
|
||
is_running = True # 程序运行标志
|
||
|
||
|
||
# -------------------------- 3. 硬件PWM控制函数 --------------------------
|
||
def export_pwm():
|
||
"""导出PWM通道（使能PWM功能）"""
|
||
if not os.path.exists(PWM_PATH):
|
||
with open(f"/sys/class/pwm/pwmchip{PWM_CHIP}/export", "w") as f:
|
||
f.write(str(PWM_CHANNEL))
|
||
# 等待文件系统创建完成
|
||
time.sleep(0.1)
|
||
|
||
|
||
def unexport_pwm():
|
||
"""取消导出PWM通道（禁用PWM功能）"""
|
||
if os.path.exists(PWM_PATH):
|
||
with open(f"/sys/class/pwm/pwmchip{PWM_CHIP}/unexport", "w") as f:
|
||
f.write(str(PWM_CHANNEL))
|
||
|
||
|
||
def set_pwm_period(period_ns):
|
||
"""设置PWM周期（单位：纳秒）"""
|
||
with open(f"{PWM_PATH}/period", "w") as f:
|
||
f.write(str(period_ns))
|
||
|
||
|
||
def set_pwm_duty_cycle(duty_ns):
|
||
"""设置PWM占空比（单位：纳秒）"""
|
||
# 确保占空比在有效范围内
|
||
duty_ns = max(MIN_PULSE_US, min(MAX_PULSE_US, duty_ns))
|
||
with open(f"{PWM_PATH}/duty_cycle", "w") as f:
|
||
f.write(str(duty_ns))
|
||
|
||
|
||
def enable_pwm(enable=True):
|
||
"""启用或禁用PWM输出"""
|
||
with open(f"{PWM_PATH}/enable", "w") as f:
|
||
f.write("1" if enable else "0")
|
||
|
||
|
||
def set_servo_angle(pulse_ns):
|
||
"""通过设置脉宽控制舵机角度"""
|
||
set_pwm_duty_cycle(pulse_ns)
|
||
|
||
|
||
# -------------------------- 4. 模拟持续旋转函数 --------------------------
|
||
def continuous_rotate_sim():
|
||
"""模拟持续旋转：在最小和最大脉宽之间交替切换"""
|
||
global is_running
|
||
print(f"DS3235舵机已启动（{MODE}°模式，硬件PWM控制，按Ctrl+C停止）...")
|
||
|
||
# 初始化PWM
|
||
export_pwm()
|
||
set_pwm_period(PWM_PERIOD_MS)
|
||
enable_pwm(True)
|
||
|
||
try:
|
||
while is_running:
|
||
# 输出最小脉宽（对应0°）
|
||
set_servo_angle(MIN_PULSE_US)
|
||
print(f"当前脉宽：{MIN_PULSE_US}ns（0°）")
|
||
time.sleep(ROTATE_DELAY)
|
||
|
||
# 输出最大脉宽（对应180°）
|
||
set_servo_angle(MAX_PULSE_US)
|
||
if MODE == "180":
|
||
print(f"当前脉宽：{MAX_PULSE_US}ns（180°）")
|
||
else:
|
||
print(f"当前脉宽：{MAX_PULSE_US}ns（270°）")
|
||
time.sleep(ROTATE_DELAY)
|
||
finally:
|
||
# 关闭PWM输出
|
||
enable_pwm(False)
|
||
|
||
|
||
# -------------------------- 5. 信号处理（Ctrl+C优雅退出） --------------------------
|
||
def signal_handler(signum, frame):
|
||
"""捕获Ctrl+C信号，设置退出标志"""
|
||
global is_running
|
||
print("\n收到停止指令，正在清理资源...")
|
||
is_running = False
|
||
|
||
|
||
# -------------------------- 6. 主函数 --------------------------
|
||
if __name__ == "__main__":
|
||
# 注册信号处理
|
||
signal.signal(signal.SIGINT, signal_handler)
|
||
signal.signal(signal.SIGTERM, signal_handler)
|
||
|
||
try:
|
||
# 启动模拟持续旋转
|
||
continuous_rotate_sim()
|
||
|
||
except Exception as e:
|
||
print(f"程序异常：{str(e)}")
|
||
|
||
finally:
|
||
# 资源清理
|
||
unexport_pwm()
|
||
print("舵机PWM通道已关闭")
|
||
print("程序退出完成")
|