方位を調べてみよう

方位センサー

ここでは、parallaxで販売しているHMC5883Lという方位センサーを使ってみます。このデバイスはI2CプロトコルのデバイスですのでI2CのLCDに表示させるのコントロールなどを先に理解してください。


I2Cの基本クラス

I2C.cs
using System;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using GHI.OSHW.Hardware;        // FEZCerberus使用のため追加、参照設定にも追加
using BoeBotLib;

namespace BoeBotLib
{
    class I2C
    {
        // Set Constants for Address and Clock Frequency
        byte address;                   // set to 7-bit address
        const int CLOCK_FREQ = 100;
        const int TIMEOUT = 1000;
        public I2CDevice i2c;

        // fez_cerb40はI2Cが一つしかないからピン設定は無い
        public I2C(byte Address7)
        {
            address = Address7;
            //Init I2C Device Config
            I2CDevice.Configuration config =
                new I2CDevice.Configuration(address, CLOCK_FREQ);
            i2c = new I2CDevice(config);
        }

        public void Write(byte control, byte data)
        {
            i2c.Execute(new I2CDevice.I2CTransaction[1] {
                I2CDevice.CreateWriteTransaction(new Byte[] { control, data }) }, TIMEOUT);
        }

        public int Read(byte Address)
        {
            byte[] Data = new byte[2];
            i2c.Execute(new I2CDevice.I2CTransaction[1] { I2CDevice.CreateWriteTransaction(new Byte[] { Address }) }, TIMEOUT);
            i2c.Execute(new I2CDevice.I2CTransaction[1] { I2CDevice.CreateReadTransaction(Data) }, TIMEOUT);
            return Data[0] << 8 + Data[1];
        }
        public void Read(byte Address, byte[] Data)
        {
            i2c.Execute(new I2CDevice.I2CTransaction[1] { I2CDevice.CreateWriteTransaction(new Byte[] { Address }) }, TIMEOUT);
            i2c.Execute(new I2CDevice.I2CTransaction[1] { I2CDevice.CreateReadTransaction(Data) }, TIMEOUT);
        }
    }
}


HMC5883Lクラスファイル



HMC5883L.cs
using System;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using GHI.OSHW.Hardware;        // FEZCerberus使用のため追加、参照設定にも追加
using BoeBotLib;

namespace BoeBotLib
{
    class HMC5883L : I2C
    {
        const byte HMC5883L_ADDRESS = 0x1E;
        public HMC5883L()
            : base(HMC5883L_ADDRESS)
        {
            Write(0,0x70);  // 8-average,15Hz default, normal measurement mode
            Write(1,0xa0);  // Gain=5
        }
        
        public void readData(short[] data)
        {
            Write(2,1);     // 計測開始
            CPU.delay(6);   // 終了を待つ
            byte[] Data = new byte[6];
            Read(3, Data);  // X MSB から読み出し開始
            byte high, low;
            for(int i=0; i<3; i++) {
                high = Data[i*2];
                low  = Data[i*2+1];
                data[i] = (short)(((high) << 8) | low);
            }
        }

// 北を0度として反時計回りに角度を求める

        public int readData()
        {
            short[] raw = new short[3];
            readData(raw);
            int x = raw[2];
            int y = raw[0];
            double dir=-1;
            if(x>y && x!=0) dir = System.Math.Atan((double)y/x);
            else if (y != 0) dir = System.Math.Atan((double)x / y);
            double degree=dir*180/System.Math.PI;
            if(x>=0)
            {
                if(y>=0){
                    if(x>y) dir = degree;
                    else    dir = 90-degree;
                }
                else        dir = 360+degree;
            }
            else
            {
                if(y<0)
                {   
                    if(x>y) dir = 180+degree;
                    else    dir = 270-degree;
                }
                else        dir = 90-degree;
            }
            return (int)dir;
        }
    }
}

 1 
 2 
 3 
 4 
 5 
 6 
 7 
 8 
 9 
10 I2Cクラスから派生される
11 
12 7bitアドレスを指定する
13 コンストラクタ
14 
15 
16 初期化
17 
18 
19 
20 
21 
22 
23 
24 
25 
26 
27 
28 
29 
30 
31 
32 
33 
34 
35 
36 
37 
38 
39 
40 
41 
42 
43 
44 
45 
46 0～359度に変換する
47 
48 
49 
50 
51 
52 
53 
54 
55 
56 
57 
58 
59 
60 
61 
62 
63 
64 
65 
66


メインプログラム


ここでは、uOLED(有機LEDディスプレイ)に表示するようにしています。

main.cs
using System;
using Microsoft.SPOT;
using System.Threading;         // 追加
using Microsoft.SPOT.Hardware;  // 追加、参照設定にも追加
using GHI.OSHW.Hardware;        // 追加、参照設定にも追加
using System.IO.Ports;
using BoeBotLib;
// com1--p36
// com2--p28
// com3--p32

namespace AzimusMotion
{
    public class Program
    {
        static DigitalOut LED = new DigitalOut(FEZCerberus.Pin.PC1, false);
        public static void Main()
        {
            uOLED96G1 LCD = new uOLED96G1();      // LCDオブジェクトCOM2を用いる
            HMC5883L compass = new HMC5883L();    // 方位センサーオブジェクト
            LCD.EraseScreen();
            while (true)
            {
                int degree = compass.readData();    // 測定する
                LED.Write(true);
                LCD.EraseScreen();
                LCD.Print(0,0,""+degree,Color.White);       // 左上に表示させる
                CPU.delay(200);
                LED.Write(false);
                CPU.delay(200);
            }
        }
    }
}