import threading
import time
import os

class PWMController:
    def __init__(self, chip_path):
        #print(chip_path)
        self.pwm_path = f"{chip_path}/pwm0" # PWM设备树路径
        self.period_ns = 20000000 # 20ms
        self.polarity = 'normal' # 极性normal
        self.export_pwm(chip_path, 0)
        self.set_period(self.period_ns)
        self.set_polarity(self.polarity)
        self.enable()

    # 导出PWM通道
    def export_pwm(self, chip_path, channel):
        if not os.path.exists(self.pwm_path):
            with open(f"{chip_path}/export", 'w') as f:
                f.write(str(channel))
            time.sleep(0.1)

    # 设置PWM周期，固定值
    def set_period(self, period):
        with open(f"{self.pwm_path}/period", 'w') as f:
            f.write(str(period))

    # 设置PWM极性
    def set_polarity(self, polarity):
        with open(f"{self.pwm_path}/polarity", 'w') as f:
            f.write(str(polarity))

    # 启用PWM输出
    def enable(self):
        with open(f"{self.pwm_path}/enable", 'w') as f:
            f.write('1')

    # 禁用PWM输出
    def disable(self, chip_path):
        with open(f"{self.pwm_path}/enable", 'w') as f:
            f.write('0')

    # 设置PWM占空比，随着角度变化
    def set_duty_cycle(self, duty_ns):
        duty_ns = max(500000, min(duty_ns, 2500000))  # 限制在合理范围内
        with open(f"{self.pwm_path}/duty_cycle", 'w') as f:
            f.write(str(duty_ns))

    def cleanup(self, chip_path):
        with open(f"{chip_path}/unexport", 'w') as f:
            f.write('0')

# 初始PWM设备树
pwm_x = PWMController("/sys/class/pwm/pwmchip4")  # 水平
pwm_y = PWMController("/sys/class/pwm/pwmchip3")  # 垂直

def angle_to_duty(angle):
    return int(500000 + (angle / 180.0) * 2000000)

# 设置舵机角度
def set_servo_angle(x_angle, y_angle):
    pwm_x.set_duty_cycle(angle_to_duty(x_angle))
    pwm_y.set_duty_cycle(angle_to_duty(y_angle))

# 舵机角度初始化
#angle_x = 90
#angle_y = 90
#set_servo_angle(angle_x, angle_y)

set_servo_angle(0, 90)
target_type_key = False
print("已完成")