91 lines
2.5 KiB
Python
Executable File
91 lines
2.5 KiB
Python
Executable File
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_angle = 90
|
||
set_ticks = 0
|
||
set_target = 0
|
||
while set_ticks < 3:
|
||
set_servo_angle(90, set_angle)
|
||
if set_target == 0:
|
||
set_angle = set_angle - 1
|
||
elif set_target == 1 and set_ticks < 2:
|
||
set_angle = set_angle + 1
|
||
else:
|
||
set_angle = set_angle + 1
|
||
if set_angle == 90:
|
||
set_ticks = 3
|
||
time.sleep(0.01)
|
||
if set_angle == 70:
|
||
set_target = 1
|
||
elif set_angle == 110:
|
||
set_target = 0
|
||
set_ticks = set_ticks + 1
|
||
target_type_key = False
|
||
print("已完成")
|