Naoya Muramatsu / Mbed 2 deprecated ROBOT_v01

ロボカップのブロック大会2014で使用したプログラムです。 ロボットには、mbedを2台使ってI2C通信しています。 これはMaster側です。 ※独自規格を使用しています。

Dependencies:   ACM1602NI Ping mbed-rtos mbed

HMC6352.h

Committer:
denden
Date:
2014-03-10
Revision:
0:27bf77b6ec71

File content as of revision 0:27bf77b6ec71:





// 基準角度を求める
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;
}