import microbit


class Servo:
    """A class for controlling continuous-rotation servos."""
    
    def __init__(self, pin, trim=0):
        self.pin = pin  # The pin to which the servo is connected.
        self.trim = trim  # What value makes the servo stop.
        self.speed = 0  # Current speed.
        self.pin.set_analog_period(20)  # Servo needs 20Hz signal.

    def set_speed(self, speed):
        """Sets the rotation speed of the servo, range is -90 to +90."""
        # Value of 25 gives us around 1000us, which is the minimum duty cycle
        # for the servo. 125 is the maximum.
        self.pin.write_analog(int(25 + 100 * (90 + speed) / 180 + self.trim))
        self.speed = speed


class Robot:
    def __init__(self):
        # Initialize the servos. Adjust the trim as needed.
        self.left_servo = Servo(microbit.pin0, 2)
        self.right_servo = Servo(microbit.pin1, 1)

    def go(self, distance):
        """Make the robot go straight for a given distance."""
        microbit.display.show(microbit.Image.ARROW_S)
        self.left_servo.set_speed(-90)
        self.right_servo.set_speed(90)
        microbit.sleep(int(distance * 2000 / 17))
        self.stop()

    def turn(self, angle):
        """Make the robot turn in place by given angle in degrees."""
        if angle > 0:
            microbit.display.show(microbit.Image.ARROW_E)
            self.left_servo.set_speed(-90)
            self.right_servo.set_speed(-90)
            microbit.sleep(int(angle * 64 / 9))
        else:
            microbit.display.show(microbit.Image.ARROW_W)
            self.left_servo.set_speed(90)
            self.right_servo.set_speed(90)
            microbit.sleep(int(-angle * 64 / 9))
        self.stop()

    def stop(self):
        """Make the robot stop."""
        microbit.display.show(microbit.Image.DIAMOND)
        self.left_servo.set_speed(0)
        self.right_servo.set_speed(0)

    def get_distance(self):
        """Use the distance sensor on pin2."""
        return microbit.pin2.read_analog()


robot = Robot()  # Create the robot object.
while True:
    robot.go(5) # Go forward 5 units.
    if robot.get_distance() > 700:  # If the distance sensor returns something...
        robot.turn(20) # Turn 20 degrees one way.
        left_distance = robot.get_distance()  # Take a measurement.
        robot.turn(-40) # Turn 20 degrees the other way (40 in total)
        right_distance = robot.get_distance() # Take a measurement.
        robot.turn(20) # Turn back straight.
        if left_distance < right_distance: # Pick whatever side was further away.
            robot.turn(60)
        else:
            robot.turn(-60)