added nav6 code, put in real constants for robot, changed code and variable names to match mechanical and electrical coordinate system which has wheels at north south east and west, added SwerveModule tester for tomorrow