Added toString() Methods, and a StateMachine prototype.
Changed PositionController, added a check if beaconens for PositionCalculation is greater 250.