Naoya Muramatsu
ロボカップのブロック大会2014で使用したプログラムです。 ロボットには、mbedを2台使ってI2C通信しています。 これはMaster側です。 ※独自規格を使用しています。
Dependencies: ACM1602NI Ping mbed-rtos mbed
Diff: HMC6352.h
- Revision:
- 0:27bf77b6ec71
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC6352.h Mon Mar 10 07:56:41 2014 +0000
@@ -0,0 +1,42 @@
+
+
+
+
+// 基準角度を求める
+void COMPASS_RESET(void) {
+ Compass_mode[0] = 0x47;
+ Compass_mode[1] = 0x74;
+ Compass_mode[2] = 0x72;
+ I2C_mbed.write(compass_addr, Compass_mode, 3);
+
+ float compass = 0.0;
+ for (int i=0; i<10; i++) {
+ val = I2C_mbed.read(compass_addr, Compass_mode, 2);
+ if(!val) led4 = 1;
+ compass += 0.1 * ((Compass_mode[0] << 8) + Compass_mode[1]);
+ }
+ Compass_Base = compass / 10.0;
+ lcd.locate(0, 1); lcd.printf("%5.1f", Compass_Base);
+}
+
+
+
+
+
+// 現在の向いている方向を求める
+void COMPASS(void) {
+ Compass = 0.0;
+ val = I2C_mbed.read(compass_addr, Compass_mode, 2);
+ Compass = 0.1 * ((Compass_mode[0] << 8) + Compass_mode[1]);
+ Compass -= Compass_Base;
+
+ if (Compass >= 180.0) Compass -= 360.0;
+ if (Compass < -180.0) Compass += 360.0;
+
+ if(!val)led1 = 1; else led1 = 0;
+}
+
+
+
+
+