Positionfinding added. NOT tested!