Math Update

IT WORKS with the new updated focal length of 340.
Testing Update

Internal Test.vi incorrectly used the camera vi's from wpilib.