108 lines
4.2 KiB
Python
108 lines
4.2 KiB
Python
|
#!/usr/bin/env python
|
|||
|
|
# -*- coding: utf-8 -*-
|
|||
|
|
'''
|
|||
|
|
# @Time : 2025/9/23 18:21
|
|||
|
|
# @Author : reenrr
|
|||
|
|
# @File : servo_motor.py
|
|||
|
|
# 功能描述：RK3506通过GPIO模拟PWM，控制DS3235舵机（模拟持续旋转）
|
|||
|
|
# 参考文档：DS3235 Coreless datasheet.pdf、接线图.docx
|
|||
|
|
'''
|
|||
|
|
import time
|
|||
|
|
import signal
|
|||
|
|
from periphery import GPIO
|
|||
|
|
|
|||
|
|
# -------------------------- 1. 硬件配置（必须根据实际接线调整） --------------------------
|
|||
|
|
# 1.1 舵机PWM信号输出引脚（RK3506的GPIO编号，需与硬件接线对应）
|
|||
|
|
# 例：若舵机信号线接GPIO3_B2_d，对应Linux GPIO编号为10（参考你提供的vibration_control-test.py）
|
|||
|
|
SERVO_GPIO_NUM = 10 # 替换为你的实际舵机信号引脚GPIO编号
|
|||
|
|
|
|||
|
|
# 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 舵机角度模式选择（根据需求切换，参考接线图.docx）
|
|||
|
|
MODE = "180" # 180°模式：500μsec=0°，2500μsec=180°
|
|||
|
|
|
|||
|
|
# -------------------------- 2. 全局变量初始化 --------------------------
|
|||
|
|
gpio = None # 舵机GPIO引脚对象
|
|||
|
|
is_running = True # 程序运行标志
|
|||
|
|
|
|||
|
|
|
|||
|
|
# -------------------------- 3. PWM信号生成函数 --------------------------
|
|||
|
|
def set_servo_pulse(pulse_us):
|
|||
|
|
'''
|
|||
|
|
生成指定脉宽的PWM信号（通过GPIO高低电平模拟）
|
|||
|
|
参数：pulse_us - 高电平脉宽（单位：微秒）
|
|||
|
|
'''
|
|||
|
|
global gpio
|
|||
|
|
# 脉宽范围限制（避免舵机过载，严格遵循规格书）
|
|||
|
|
pulse_us = max(MIN_PULSE_US, min(MAX_PULSE_US, pulse_us))
|
|||
|
|
|
|||
|
|
# 1. 输出高电平（持续pulse_us微秒）
|
|||
|
|
gpio.write(True)
|
|||
|
|
time.sleep(pulse_us / 1000000) # 转换为秒单位
|
|||
|
|
|
|||
|
|
# 2. 输出低电平（补全PWM周期，20ms - 高电平时间）
|
|||
|
|
low_time_ms = PWM_PERIOD_MS - (pulse_us / 1000) # 低电平时间（毫秒）
|
|||
|
|
gpio.write(False)
|
|||
|
|
time.sleep(low_time_ms / 1000) # 转换为秒单位
|
|||
|
|
|
|||
|
|
|
|||
|
|
# -------------------------- 4. 模拟持续旋转函数 --------------------------
|
|||
|
|
def continuous_rotate_sim():
|
|||
|
|
'''
|
|||
|
|
模拟持续旋转：交替输出“最小脉宽（0°）”和“最大脉宽（180°）”
|
|||
|
|
'''
|
|||
|
|
global is_running
|
|||
|
|
print(f"DS3235舵机已启动（{MODE}°模式，按Ctrl+C停止）...")
|
|||
|
|
|
|||
|
|
while is_running:
|
|||
|
|
# 第一步：输出最小脉宽（对应0°）
|
|||
|
|
set_servo_pulse(MIN_PULSE_US)
|
|||
|
|
print(f"当前脉宽：{MIN_PULSE_US}μsec（0°）")
|
|||
|
|
time.sleep(ROTATE_DELAY)
|
|||
|
|
|
|||
|
|
# 第二步：输出最大脉宽（对应180°/270°）
|
|||
|
|
set_servo_pulse(MAX_PULSE_US)
|
|||
|
|
if MODE == "180":
|
|||
|
|
print(f"当前脉宽：{MAX_PULSE_US}μsec（180°）")
|
|||
|
|
else:
|
|||
|
|
print(f"当前脉宽：{MAX_PULSE_US}μsec（270°）")
|
|||
|
|
time.sleep(ROTATE_DELAY)
|
|||
|
|
|
|||
|
|
|
|||
|
|
# -------------------------- 5. 信号处理（Ctrl+C优雅退出） --------------------------
|
|||
|
|
def signal_handler(signum, frame):
|
|||
|
|
'''捕获Ctrl+C信号，设置退出标志'''
|
|||
|
|
global is_running
|
|||
|
|
print("\n收到停止指令，正在清理资源...")
|
|||
|
|
is_running = False
|
|||
|
|
|
|||
|
|
|
|||
|
|
# -------------------------- 6. 主函数 --------------------------
|
|||
|
|
if __name__ == "__main__":
|
|||
|
|
# 1. 注册信号处理（确保Ctrl+C能正常退出）
|
|||
|
|
signal.signal(signal.SIGINT, signal_handler)
|
|||
|
|
signal.signal(signal.SIGTERM, signal_handler)
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
# 2. 初始化舵机GPIO引脚（输出模式，参考你的vibration_control-test.py风格）
|
|||
|
|
gpio = GPIO(SERVO_GPIO_NUM, "out")
|
|||
|
|
print(f"舵机GPIO引脚（编号：{SERVO_GPIO_NUM}）初始化完成")
|
|||
|
|
|
|||
|
|
# 3. 启动模拟持续旋转
|
|||
|
|
continuous_rotate_sim()
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
# 异常处理（如GPIO初始化失败）
|
|||
|
|
print(f"程序异常：{str(e)}")
|
|||
|
|
|
|||
|
|
finally:
|
|||
|
|
# 4. 资源清理（必做！避免GPIO引脚异常）
|
|||
|
|
if gpio is not None:
|
|||
|
|
gpio.write(False) # 程序结束前拉低GPIO，避免舵机持续受力
|
|||
|
|
gpio.close()
|
|||
|
|
print("舵机GPIO引脚已关闭")
|
|||
|
|
print("程序退出完成")
|