Working code. Will turn if object is in front of it, moves forward othrwise, fixed issues with multiple functions trying to control the Servos at once