from time import sleep
import robot

DRIVE_A = 5.3 # 10 degrees turn at power 1
TURN_A = 16 # 10 centimeter drive at power 1

RIGHT_WHEEL_OFFSET = 4

def square(arlo, power, turn_t, drive_t):
    while True:
        # Driving forward
        arlo.go_diff(power, power + RIGHT_WHEEL_OFFSET, 1, 1)
        sleep(drive_t * 10)
        arlo.stop()

        sleep(1)

        # Turning 90 degrees
        arlo.go_diff(power, power + RIGHT_WHEEL_OFFSET, 0, 1)
        sleep(turn_t * 9)
        arlo.stop()

        sleep(1)

def main():
    # Gets the power for the robot
    power = input("Power (64):")
    if power == "":
        power = 64
    else:
        power = int(power)

    # Calculates sleep times for the robot
    turn_t = DRIVE_A / power # 10 degrees turn at given power
    drive_t = TURN_A / power # 10 centimeter drive at given power

    # Initializes the robot and runs the
    arlo = robot.Robot()

    try:
        square(arlo, power, turn_t, drive_t)

    except KeyboardInterrupt:
        arlo.stop()


if __name__ == "__main__":

    main()