番外 Ｃ言語でモーションを調べてみよう

モーションセンサー

ここでは、parallaxで販売しているHMC5883Lという方位センサーを使ってみます。このデバイスはI2Cプロトコルのデバイスですので「番外　C言語を用いてI2CでIO拡張する」のコントロールなどで理解しておいてください。


I2C言語でL3G4200Dをコントロールする

L3G4200D.c
#include "iodefine.h"
/*----------------------------------------------------------------
  ここからL3G4200D専用ルーチン
-----------------------------------------------------------------*/

int DeviceAddress = 0xD2;
int mean[3];

void sendByte(int Device, int address, int data)
{
    unsigned char rdt;

    i2c_waitBusFree();                      // i2c バス空き待ち
    do {
        i2c_setTransmitMode(3);             // マスタ送信モード
        i2c_sendStartCondition();           // i2c バスを開始条件にする
        rdt = i2c_sendByte(Device);         // スレーブアドレスを送信
    } while(rdt == 1);                      // デバイスからACKが返るまでガンバル
    i2c_sendByte(address);                  // デバイス内アドレス番号を送信
    i2c_sendByte(data);                     // 設定する値を送信
    IIC2.ICSR.BIT.TEND = 0;
    i2c_sendStopCondition();                // i2c バスを停止条件にする。
    i2c_setTransmitMode(0);                 // スレーブ受信モードにする
}

void write(int address, int data)
{
  sendByte(DeviceAddress, address, data);
}

unsigned char readByte(unsigned  reg) {

    unsigned char rdt;

    i2c_waitBusFree();                      // i2c バス空き待ち
    do {
        i2c_setTransmitMode(3);             // マスタ送信モード
        i2c_sendStartCondition();           // i2c バスを開始条件にする
        rdt = i2c_sendByte(DeviceAddress);  // スレーブアドレスを送信
    } while(rdt == 1);                      // デバイスからACKが返るまでガンバル
    i2c_sendByte(reg);                      // レジスタ番号を送信
    i2c_setTransmitMode(3);                 // マスタ送信モード
    i2c_sendStartCondition();               // i2c バスを開始条件にする
    i2c_sendByte(DeviceAddress|1);             // スレーブアドレスを送信
    i2c_setTransmitMode(2);                 // マスタ受信モード
    IIC2.ICSR.BIT.TDRE = 0;
    rdt = i2c_receiveByte(1, 1);            // ACK=1, RCVD = 1で受信する。
    i2c_sendStopCondition();                // i2c バスを停止条件にする。
    IIC2.ICCR1.BIT.RCVD = 0;
    i2c_setTransmitMode(0);                 // スレーブ受信モードにする
    return(rdt);
}

void waitForDataReady()
{
  unsigned char data;
  do {
    data = readByte(0x27);
  } while(!(data & 0x08));
}

int getData(unsigned func, unsigned char Data[], unsigned n)
{
  int i, n1;
  unsigned char rdt;
  n1 = n-1;
  i2c_waitBusFree();                    // i2c バス空き待ち
  i=0;
  do {
    i2c_setTransmitMode(3);             // マスタ送信モード
    i2c_sendStartCondition();           // i2c バスを開始条件にする
    rdt = i2c_sendByte(DeviceAddress);  // スレーブアドレスを送信
    if(i++ == 100) {                    // 返事が無かったらあきらめる
      i2c_sendStopCondition();          // i2c バスを停止条件にする。
      i2c_setTransmitMode(0);           // スレーブ受信モードにする
      return 1;
    }
  } while(rdt == 1);                    // デバイスからACKが返るまでガンバル
  i2c_sendByte(func);                   // レジスタ番号を送信
  i2c_sendStartCondition();             // i2c バスを開始条件にする
  i2c_sendByte(DeviceAddress|1);        // スレーブアドレスを送信
  i2c_setTransmitMode(2);               // マスタ受信モード
  for(i=0; i< n; i++) {
    IIC2.ICSR.BIT.TDRE = 0;
    if(i==n1) Data[i] = i2c_receiveByte(1, 1);    // ACK=1, RCVD = 1で受信する。
    else Data[i]  = i2c_receiveByte(0, 0); 
    IIC2.ICCR1.BIT.RCVD = 0;
  }
  i2c_sendStopCondition();                // i2c バスを停止条件にする。
  i2c_setTransmitMode(0);                 // スレーブ受信モードにする
  return 0;
}

void readData(short data[])
{
  unsigned char dt[6];
  unsigned char high, low;
  int i;
  waitForDataReady();
  getData(0xA8, dt, 6);
  for(i=0; i<3; i++) {
    low  = dt[i*2];
    high = dt[i*2+1];
    data[i] = (short)(((high) << 8) | low)-mean[i];
  }
}

void L3G4200Dinit()
{
  int i, j;
  int average[3];
  short data[3];
  i2c_init();                       // I2Cの初期設定
  DeviceAddress = 0xD2;
  write(0x22,8);    // CTRL_REG3 Set up data ready signal
  write(0x23,0x80);   // CTRL_REG4 Set up "block data update" mode
  write(0x20,0x1f);     // CTRL_REG1 Send the get continuous output command

  for(i=0; i<3; i++)  average[i]=mean[i]=0;

  for(i=0; i<30; i++)
  {
    readData(data);
    for(j=0; j<3; j++) average[j] += data[j];
  }
  for(i=0; i<3; i++)
  {
     mean[i] = average[i]/30;
  }
}
 
 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 
104 
105 
106 
107 
108 
109 
110 
111 
112 
113 
114 
115 
116 
117 
118 
119 
120 
121 
122 
123 
124 
125 
126 
127 
128 
129 
130


メインプログラム


main.c
#include "vs-wrc003.h"

void startInit(char* str)
{
  const BYTE MainCycle = 100;
  Init((BYTE)MainCycle);  //初期設定
  InitSci3(CBR_115200,non,1);
  Mtr_Run(0, 0, 0, 0);
  while(!getSW());
  printf(str);
}

void main(void)
{
  int i;
  short data[3];
  startInit("*** L3G4200D test program ***\n");
  L3G4200Dinit(); 
  while(1)
  {
    readData(data);
    for(i=0; i<3; i++) printf("%d ", data[i]);
    printf("\n");
    LED(1);
    Wait(50);
    LED(2);
    Wait(50);
  }
}

 1 
 2 
 3 
 4 
 5 
 6 
 7 
 8 
 9 
10 
11 
12 
13 
14 
15 
16 
17 
18 L3G4200Dの初期化
19 
20 
21 モーションを測定
22 表示
23 
24 
25 
26 
27 
28