方位とモーションを両方同時に調べてみよう

複数のI2Cセンサーで同時に測定する

ここでは、parallaxで販売しているHMC5883Lという方位センサーを使ってみます。
また、parallaxで販売しているL3G4200Dというジャイロセンサーを使います。
4D systemsで開発販売している小型ディスプレイに表示します。

テストメインプログラム
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();
            L3G4200D gyro= new L3G4200D();
            String str;
            while (true)
            {
                LED.Write(true);     
                double degree=compass.Read();    // 測定する
                gyro.Read();
                LED.Write(false);
                LCD.EraseScreen();
                str = Std.sprintf("X:%5d", compass.X);
                LCD.Print(0,  0, str, Color.White);     // 左上に表示させる
                str = Std.sprintf("Y:%5d", compass.Y);
                LCD.Print(0, 15, str, Color.White);
                str = Std.sprintf("Z:%5d", compass.Z);
                LCD.Print(0, 30, str, Color.White);
                str = Std.sprintf("Deg:%6.1f", degree);
                LCD.Print(0, 45, str, Color.Red);
                str = Std.sprintf("X:%5d", gyro.X);
                LCD.Print(0, 60, str, Color.Green);
                str = Std.sprintf("Y:%5d", gyro.Y);
                LCD.Print(0, 72, str, Color.Green);
                str = Std.sprintf("Z:%5d", gyro.Z);
                LCD.Print(0, 84, str, Color.Green);
                CPU.delay(100);
            }
        }

        public static void Main1()
        {
            uOLED96G1 LCD = new uOLED96G1();      // LCDオブジェクトCOM2を用いる
            int x = -123;
            double y = 45.67;
            char ch = 'A';
            String s = "String";
            int hex = 0xabcd;
            LCD.Print(Std.sprintf("test %5d", x));
            LCD.Print(Std.sprintf("%8.3f", y));
            LCD.Print(Std.sprintf("ch %c", ch));
            LCD.Print(Std.sprintf("%s", s));
            LCD.Print(Std.sprintf("hex %5x", hex));
        }
    }
}

 1 
 2 
 3 
 4 
 5 
 6 
 7 
 8 
 9 
10 
11 
12 
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 
47 
48 
49 
50 
51 
52 
53 
54 
55 
56 
57 
58 
59 
60 
61 
62 
63


I2CBusクラス


I2Cのバスチェックのメソッドが見つからないので排他的制御を用いて複数のデバイスを使えるようにします。
この記事が元です
バグがあったので訂正してありますが、これもバグがまだ残っている可能性はあります。
I2CBusクラス
using System;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using System.Threading;
using BoeBotLib;

// 複数のI2Cデバイスを同じポートを使用するためのクラス

namespace BoeBotLib
{
    public class I2CBus : IDisposable
    {
        private static I2CBus _instance = null;
        // 排他制御のためのオブジェクト
        private static readonly object LockObject = new object();

        public static I2CBus GetInstance()
        {
            lock (LockObject)
            {
                if (_instance == null) _instance = new I2CBus();
                return _instance;
            }
        }
        // 実際に用いるデバイス
        private I2CDevice _slaveDevice;

        private I2CBus()
        {
            this._slaveDevice = new I2CDevice(new I2CDevice.Configuration(0, 0));
        }

        public void Dispose()
        {
            this._slaveDevice.Dispose();
        }
        // 複数のデータを送信する場合
        public bool Write(I2CDevice.Configuration config, byte[] writeBuffer, int transactionTimeout)
        {
            // i2cデバイス設定をつくる
            _slaveDevice.Config = config;
            I2CDevice.I2CTransaction[] writeXAction =
                new I2CDevice.I2CTransaction[] { I2CDevice.CreateWriteTransaction(writeBuffer) };
            // 排他制御をする
            lock(_slaveDevice)
            {
                // データを転送する
                int transferred = _slaveDevice.Execute(writeXAction, transactionTimeout);
                // データが正しく転送されたかチェックする
                if (transferred != writeBuffer.Length)
                {
                      return true;
                 //   throw new Exception("Could not write to device.");
                }
            }
            return false;
        }
        // 複数のデータを読み込む場合
        public void Read(I2CDevice.Configuration config, byte[] readBuffer, int transactionTimeout)
        {
            // i2cデバイス設定をつくる
            _slaveDevice.Config = config;
            // 読み込み設定
            I2CDevice.I2CTransaction[] readXAction = new I2CDevice.I2CTransaction[] 
                { I2CDevice.CreateReadTransaction(readBuffer) };

            lock (_slaveDevice)
            {
                // データを転送する
                int transferred = _slaveDevice.Execute(readXAction, transactionTimeout);
                // データが正しく転送されたかチェックする
                if (transferred != readBuffer.Length)
                    throw new Exception("Could not read from device.");
            }
        }
        // レジスタを読むだけの場合
        public void ReadRegister(I2CDevice.Configuration config, byte register, byte[] readBuffer, 
            int transactionTimeout)
        {
            byte[] registerBuffer = {register};
            Write(config, registerBuffer, transactionTimeout);
            Read(config, readBuffer, transactionTimeout);
        }
        // レジスタから複数データを書き込む場合
        public bool WriteRegister(I2CDevice.Configuration config, byte register, byte[] writeBuffer, 
            int transactionTimeout)
        {
            byte[] buffer = new byte[writeBuffer.Length + 1];
            buffer[0] = register;
            int i = 0;
            foreach (byte x in writeBuffer) buffer[i++ + 1] = x;
            bool bad = Write(config, buffer, transactionTimeout);
            return bad;
        }
        // レジスタに1byteだけ書き込む場合
        public bool WriteRegister(I2CDevice.Configuration config, byte register, byte value, 
            int transactionTimeout)
        {
            byte[] writeBuffer = {register, value};
            return Write(config, writeBuffer, transactionTimeout);
        }
    }
}

 1 
 2 
 3 
 4 
 5 
 6 
 7 
 8 
 9 
10 
11 
12 
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 
47 
48 
49 
50 
51 
52 
53 
54 
55 
56 
57 
58 
59 
60 
61 
62 
63 
64 
65 
66 
67 
68 
69 
70 
71 
72 
73 
74 
75 
76 
77 
78 
79 
80 
81 
82 
83 
84 
85 
86 
87 
88 
89 
90 
91 
92 
93 
94 
95 
96 
97 
98 
99 
100 
101
102
103


HMC5883Lクラス



HMC5883L.cs
using System;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;

namespace BoeBotLib
{
    public class HMC5883L
    {
        const byte HMC5883L_ADDRESS = 0x1E;
        private I2CDevice.Configuration _slaveConfig;
        private const int TransactionTimeout = 1000; //ms
        private const byte ClockRateKHz = 100;
        // プロパティ
        public byte Address { get; private set; }
        public int X { get; private set; }
        public int Y { get; private set; }
        public int Z { get; private set; }

        public HMC5883L()
        {
            Address = HMC5883L_ADDRESS;
            _slaveConfig = new I2CDevice.Configuration(Address, ClockRateKHz);
            // 8-average,15Hz default, normal measurement mode
            while (I2CBus.GetInstance().WriteRegister(_slaveConfig, 0, 0x70, TransactionTimeout))
            {   // 転送エラーが解消されるまで頑張る
                CPU.PanicLED();
                Thread.Sleep(300);
            }
            // Gain=5
            I2CBus.GetInstance().WriteRegister(_slaveConfig, 1, 0xa0, TransactionTimeout);  
        }

        public double Read()
        {       
            I2CBus.GetInstance().WriteRegister(_slaveConfig, 0x02, 0x01, TransactionTimeout);
            Thread.Sleep(6);
            // get MSB and LSB result
            byte[] data = new byte[6];
            I2CBus.GetInstance().Read(_slaveConfig, data, TransactionTimeout);

            int y =    ((short)(data[0] << 8)) | (short)data[1];
            Y = ((short)(data[2] << 8)) | (short)data[3];
            int x = Z = ((short)(data[4] << 8)) | (short)data[5];
            X = y;

            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 dir;
        }
    }
}

 1 
 2 
 3 
 4 
 5 
 6 
 7 
 8 
 9 
10 
11 
12 
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 
47 
48 
49 
50 
51 
52 
53 
54 
55 
56 
57 
58 
59 
60 
61 
62 
63 
64 
65 
66 
67 
68 
69 
70 
71 
72


L3G4200Dクラス



L3G4200D.cs
using System;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;

namespace BoeBotLib
{
    public class L3G4200D
    {
        const byte L3G4200D_ADDRESS = 0x69;
        private I2CDevice.Configuration _slaveConfig;
        private const int TransactionTimeout = 1000; //ms
        private const byte ClockRateKHz = 100;
        short[] mean = new short[3];
        // プロパティ
        public byte Address { get; private set; }
        public int X { get; private set; }
        public int Y { get; private set; }
        public int Z { get; private set; }

        public L3G4200D()
        {
            Address = L3G4200D_ADDRESS;
            _slaveConfig = new I2CDevice.Configuration(Address, ClockRateKHz);
            // 帯域幅25Hz
            I2CBus.GetInstance().WriteRegister(_slaveConfig, 0x20, 0x1F, TransactionTimeout);
            I2CBus.GetInstance().WriteRegister(_slaveConfig, 0x21, 0x00, TransactionTimeout);
            I2CBus.GetInstance().WriteRegister(_slaveConfig, 0x22, 0x00, TransactionTimeout);
            // 出力レジスタはMSB,LSBが更新されないと最新にしない
            I2CBus.GetInstance().WriteRegister(_slaveConfig, 0x23, 0x80, TransactionTimeout);
            I2CBus.GetInstance().WriteRegister(_slaveConfig, 0x24, 0x00, TransactionTimeout);
            int[] average = { 0, 0, 0 };
            for(int i=0;i<mean.Length;i++)  mean[i]=0;
            // キャリブレーションのために平均を求める
            for(int i=0; i<30;i++) 
            {
                Read();
                average[0] += X;
                average[1] += Y;
                average[2] += Z;
            }
            for (int i = 0; i < average.Length; i++) mean[i] = (short)(average[i] / 30);
        }

        public void Read()
        {            
            // bit7は自動アドレスインクリメント。これをしないと一気読みができない
            I2CBus.GetInstance().Write(_slaveConfig, new byte[] { 0xA8 }, TransactionTimeout);
            // get MSB and LSB result
            byte[] data = new byte[6];
            I2CBus.GetInstance().Read(_slaveConfig, data, TransactionTimeout);
            X = (((short)(data[1] << 8)) | (short)data[0])-mean[0];
            Z = (((short)(data[3] << 8)) | (short)data[2])-mean[1];
            Y = (((short)(data[5] << 8)) | (short)data[4])-mean[2];
        }
    }
}

 1 
 2 
 3 
 4 
 5 
 6 
 7 
 8 
 9 
10 
11 
12 
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 
47 
48 
49 
50 
51 
52 
53 
54 
55 
56 
57