Converted all floats to doubles because it is easier to work with WPI library which only uses doubles.