Few changes

Fixed the code, added 4 meter measure limit.
Only thing left is to replace on and off programs with motor programs and add delay needed for the robot to turn 180°.