from utime import sleep_us, sleep_ms
from machine import Pin, PWM, ADC, time_pulse_us

class ServoMotor():
    def __init__(self, pin):
        self.pwm = PWM(Pin(pin), freq=50, duty=0)
        self.prev = 0

    def set(self, speed):
        if self.prev == speed:
            # Prevent jerkiness
            return
        self.prev = speed
        if speed == 0:
            self.pwm.duty(0)
        else:
            self.pwm.duty(int(77 + speed * 8 / 100))
        print(self.pwm, 77 + speed * 8 / 100)


class Ultrasonic():
    def __init__(self, echo=14, trigger=12):
        self.echo = Pin(pin_echo, Pin.IN)
        self.trigger = Pin(pin_trigger, Pin.OUT)

    def get(self):
        self.trigger.value(0)
        sleep_us(5)
        self.trigger.value(1)
        sleep_us(10)
        self.trigger.value(0)
        return (time_pulse_us(self.echo, 1, 30000) / 2) / 29.1

# Servo motors
left_motor = ServoMotor(13)
right_motor = ServoMotor(15)
opponent = Ultrasonic()

sleep_ms(2000)

while True:
    if opponent.get() < 30:
        left_motor.set(100)
        right_motor.set(-100)
    else:
        left_motor.set(100)
        right_motor.set(100)
    sleep_ms(250)