from PiBot import PiBot
import rospy
# Create a robot instance
robot = PiBot()

# Get distance from object using the front middle IR sensor
distance_from_object = robot.get_front_middle_ir()

# Drive towards object
robot.set_left_wheel_speed(17)
robot.set_right_wheel_speed(-17)
print("turning around now")

while distance_from_object == 1:
    distance_from_object = robot.get_front_middle_ir()
    print(distance_from_object , "2")
"""while 0.5 < distance_from_object < 0.99:
    distance_from_object = robot.get_front_middle_ir()
    print(distance_from_object, "3")
    if 0 < distance_from_object < 0.5:
        print("i see walls 4")
    else:
        print("5")"""
while 0 < distance_from_object < 0.5:
    distance_from_object = robot.get_front_middle_ir()
    robot.set_wheels_speed(0)
    print(distance_from_object , "1")
    if 0 < distance_from_object < 0.5:
        print("i see walls 6")
        robot.set_wheels_speed(17)


if __name__ == "__main__":
    print("Hello, I started running!")
    rospy.sleep(1.0)
    print("Resumed after waiting 1 second!")