Added SPI - Testing branch and mag sense New Wheel branch. IMU calcDist and calcHead needs to be implement in different function