import Raspbot_Lib
from McLumk_Wheel_Sports import *
# McLumk_Wheel_Sports has already initialized a Raspbot() object called "bot"


def get_distance(bot):

    desired_block_length = 1
    low_bit_distance = bot.read_data_array(0x1A , 1)[0]
    high_bit_distance = bot.read_data_array(0x1B, 1)[0]
    return (high_bit_distance << 8)|low_bit_distance


if __name__ == "__main__":
    speed = 75
rotation_multiplier = 0.1
bot.Ctrl_Ulatist_Switch(1)
time.sleep(0.1) # polling rate of 10Hz
try:
    while True:
        if get_distance(bot) < 750: # 75 centimeters
            rotate_left(speed*rotation_multiplier)
        else:
            move_forward(speed)
except:
    stop_robot()
    bot.Ctrl_Ulatist_Switch(0)
    print("exiting the program...")   



speed = 75
rotation_multiplier = 0.05
bot.Ctrl_Ulatist_Switch(1)
time.sleep(0.1) # polling rate of 10Hz
try:
    while True:
        if get_distance(bot) < 750: # 75 centimeters
            move_backward(rotation_multiplier*speed)
            rotate_left(speed*rotation_multiplier)
        else:
            move_forward(speed)
except:
    stop_robot()
    bot.Ctrl_Ulatist_Switch(0)
    print("exiting the program...")