49 lines
1.1 KiB
Python
49 lines
1.1 KiB
Python
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)
|