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