Preston Stephens
/
Mini_Project_Day_10_Timers
Final
Diff: Compass.cpp
- Revision:
- 0:5fd3f73db34f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Compass.cpp Wed Jan 20 02:15:26 2016 +0000 @@ -0,0 +1,44 @@ +#include "Compass.h" +extern I2C acc; +char rawBytes[6]; +short data[6]; +short magComponents[3]; +double heading; +const int addr1 = (0x1E << 1); +char config[2]; +char init[2]; +double Compass() +{ + config[0]= 0x02; + config[1] = 0x00; + acc.write(addr1, config,2); + init[0]=0x03; + while(1) + { + acc.write(addr1,init,1); + acc.read(addr1,rawBytes,6); + + + for (int i = 0; i<6; i++) + { + data[i] = rawBytes[i]; + } + + magComponents[0] = data[0]<<8 | data[1]; + magComponents[1] = data[4]<<8 | data[5]; + magComponents[2] = data[2]<<8 | data[3]; + + heading = atan2((double)magComponents[1],(double)magComponents[0]); // Angle in Radians + heading = (heading*180)/3.14159; // Convert to Degrees + heading = heading + 14.85; // Find True North + if(heading > 180) + { + heading -= 360; + } + + return heading; + } +} + + +