PositioningSubsystem initial test. Works very well, a small amount of error but that could probably be reduced by increasing POSITION_RESOLUTION.