Preston Stephens
/
Mini_Project_Day_7_Taps_2
Compass Working
Diff: Compass.cpp
- Revision:
- 0:1acd78600f46
- Child:
- 2:ef6778c2438e
diff -r 000000000000 -r 1acd78600f46 Compass.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Compass.cpp Wed Jan 13 21:31:15 2016 +0000 @@ -0,0 +1,45 @@ +#include "Compass.h" + +I2C i2c_port(p9,p10); +char rawBytes[6]; +short data[6]; +short magComponents[3]; +double heading; +const int addr = (0x1E << 1); +char config[2]; +char init[2]; +double Compass() +{ + config[0]= 0x02; + config[1] = 0x00; + i2c_port.write(addr, config,2); + init[0]=0x03; + while(1) + { + i2c_port.write(addr,init,1); + i2c_port.read(addr,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; + } + wait(.05); + + return heading; + } +} + +