73 lines
2.1 KiB
Python
73 lines
2.1 KiB
Python
|
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(180, 90)
|
|||
|
|
target_type_key = False
|
|||
|
|
print("已完成")
|