ElecScalesMeasur/servo_motor_PWM.py

#!/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("程序退出完成")