added second thread for updating the robots position in testmode
machine_forward and machine_stop now work properly
current test-algorithm moves the robot in a rectangular way