from PiBot import PiBot
import rospy

# Create a robot instance
robot = PiBot()

# Get distance from object using the front middle IR sensor

# Drive towards object

print("turning around now")

def rotate_and_search():
    robot.set_wheels_speed(0) #stop robot
    rospy.sleep(0.5)
    robot.set_left_wheel_speed(17)
    robot.set_right_wheel_speed(-17)

    while True:
        distance_from_object = robot.get_front_middle_ir()
        if distance_from_object < 0.5:
            robot.set_wheels_speed(0)
            print("Object detected")
            return

def move_forward_to_object_and_stop():
    rospy.sleep(0.5)
    robot.set_wheels_speed(17)
    print('Moving forward')
    while True:
        distance_from_object = robot.get_front_middle_ir()

        if distance_from_object < 0.2:
            print('Stopping near object.')
            robot.set_wheels_speed(0)
            return


rotate_and_search()
move_forward_to_object_and_stop()