robot.turnRight(90)
robot.hitObject(DESIRED_DISTANCE, KP, 0)
print("beans")
robot.moveBackwards(60)
#robot.stepToWaypoint(50, 70, sys.maxsize)
#robot.stepToWaypoint(70, 70, sys.maxsize)
#robot.stepToWaypoint(80, 70, sys.maxsize)
robot.turnRight(99)
robot.moveForward(60)
robot.turnSonar(-90)
d = robot.getDistance()
robot.turnSonar(90)
#x, y, t = robot.particles.getCurrentPosition()
#robot.EXPECTED_VALUE = 210 - x
robot.EXPECTED_VALUE = 210 - d
robot.speedVelocity(100, 100)
robot.findObjectContinuous("left")
robot.stop()
robot.moveForward(20)
robot.turnLeft(90)
robot.hitObject(DESIRED_DISTANCE, KP, 0)
print("beans2")
robot.turnSonar(-180)
robot.moveBackwards(10)
robot.turnRight(190)
d = robot.getDistance()
robot.moveForward(d - 60)
# Second object
