from time import sleep_us,ticks_us,sleep
from machine import Pin,PWM
import time
class HCSR04():
    def __init__(self,trig,echo):
        self.trig=trig
        self.echo=echo

    def getDistance(self):
        distance=0
        self.trig.value(1)
        sleep_us(20)
        self.trig.value(0)
        while self.echo.value() == 0:
            pass
        if self.echo.value() == 1:
            ts=ticks_us()                   #开始时间
            while self.echo.value() == 1:   #等待脉冲高电平结束
                pass
            te=ticks_us()                   #结束时间
            tc=te-ts                        #回响时间（单位us，1us=1*10^(-6)s）
            distance=(tc*170)/10000         #距离计算 （单位为:cm）
        return distance

class Servo:
    '''
    这里用静态是为了分离PWM管脚的定义与占空比的输入,节省不必要的代码复用开支
    使用时先用servo_pin方法定义舵机引脚，然后再用servo_goto运行引脚
    '''
    @staticmethod
    def servo_pin(pin):
        S_pin= PWM(Pin(pin), freq=50, duty=0) # Servo X的引脚是pin
        return S_pin
    @staticmethod
    def servo_goto(s_pin,angle):
        try:
            if isinstance(s_pin,int):
                S_pin=PWM(Pin(s_pin),freq=50,duty=0)
            else:
                S_pin=s_pin
        except Exception as e:
            S_pin=s_pin 
        S_pin.duty(int(((angle+90)*2/180+0.5)/20*1023))
        time.sleep(1)

#初始化接口 trig=6,echo=7
from LC import *
import time
HC=HCSR04(2,4)

while True:
    Distance = HC.getDistance() #测量距离
    print(str(Distance)+' CM')
    if Distance <=5:
        Servo.servo_goto(16,-45)
    time.sleep(1)