2012年9月9日日曜日

Arduino、GR-SAKURA、Raspberry Pi でI2C Compassと6DOFを試す


 前回からしばらく更新の間があきましたが、GR-SAKURAのI2Cがうまく行かず悩んでいました。結論としては、GR-SAKURAは諦める方向で検討中です。
 さて、今回はまず、ストリナのI2CディジタルコンパスをArduino と GR-SAKURA と Raspberry Pi に接続して、方位データの取得を試してみました。Arduino と Raspberry Pi では方位データが取得できましたが、GR-SAKURAではダメ(おかしな値が取得される)でした。ロジアナでみると最後にNACKが返っているんですがコンパス側なのかGR-SAKURA側なのか問題の切り分けはできていません。
 同様にもう一つ別のデバイスも試しました。sparkufunの6DOF(三軸ジャイロと三軸加速度センサの組み合わせボード)ですが、これも Arduino と Raspberry Pi は動作して、GR-SAKURAはだめでした。



これまでの各種I2Cデバイスとボードとの組み合わせの勝敗表です。


I2C デバイス
Arduino
GR-SAKURA
Raspberry Pi
秋月RTCOKOKOK
秋月LCDOKNGNG
ストリナLCDOKOK(ただし、NACK)OK
ストリナCompassOKNGOK
Sparkfun 6DOFOKNGOK


 現状、GR-SAKURAが劣勢というかダメダメです。ただし、上記の結果は、自分の作成したプログラムや使用方法に問題がある可能性が大です。というか、きっとそうなんでしょう。たしか、GR-SAKURA(のライブラリ)はRXマイコンのRIICモジュールを使わずにソフトウェア処理でI2C通信を行なっていると思うのですが、この辺りが関係するのでしょうか。Arduinoとは違い、自分のような素人には GR-SAKURAは向かないようです。一度、ライブラリを使わずにHEWで RIICモジュールを使って試したいところですが、そこまでして GR-SAKURAを使わなくても、Arduinoでいいと思えてきました。日本製マイコンだし、処理能力も高いのでArduinoから乗り換えを考えていましたが、やっぱり簡単に使える方が便利だし。重い処理が必要なら、Arduino + Raspberry Pi ってのもありだし。

 で、以下お試しのコード。単純に値を取り出しているだけで使える状態の値には加工してません。実際に使うにはオフセットやらリニアリティ補正やら積分やら結構面倒。機会があればロボットに載せてみたいけど。


Arduinoのコード(スケッチ)
#include <Adruino.h>

#include <Wire.h>

// コンパスに関する定義。
#define COMPASS_ADDRESS (0b0100001)

// ジャイロに関する定義。
#define GYRO_ADDRESS (0xd0 >> 1)
#define GYRO_WHO_AM_I 0x00
#define GYRO_SMPLRT_DEV 0x15
#define GYRO_DLPF_FS 0x16
#define GYRO_INT_CFG 0x17
#define GYRO_INT_STATUS 0x1A
#define GYRO_TEMP_OUT_H 0x1B
#define GYRO_TEMP_OUT_L 0x1C
#define GYRO_XOUT_H 0x1D
#define GYRO_XOUT_L 0x1E
#define GYRO_YOUT_H 0x1F
#define GYRO_YOUT_L 0x20
#define GYRO_ZOUT_H 0x21
#define GYRO_ZOUT_L 0x22
#define GYRO_PWR_MGM 0x3E

// 加速度センサに関する定義。
#define ACC_ADDRESS (0xa6 >> 1)
#define ACC_POWER_CTL 0x2D
#define ACC_DATA_X0 0x32
#define ACC_DATA_X1 0x33
#define ACC_DATA_Y0 0x34
#define ACC_DATA_Y1 0x35
#define ACC_DATA_Z0 0x36
#define ACC_DATA_Z1 0x37

// 関数プロトタイプ。
int getCompass(void);
void getGyro(short *x, short *y, short *z);
void initAcc(void);
void getAcc(short *x, short *y, short *z);

// 
unsigned char buf[8];

void setup()
{
 // シリアルポートを9600bpsで開始。
    Serial.begin(9600);
    // 受信バッファにデータが入るまで待機。
    while (Serial.available() == 0);        
    Serial.println("Initializing...");
    // 一旦、受信バッファにデータがあれば、
    if (Serial.available() > 0) {
        // 読み込んで、廃棄。
        char c = Serial.read();
    }
    Serial.println("Initializing...Please wait...");
    delay(500);
    
    // Bus-MasterとしてI2Cを開始。
    Wire.begin();
 
 // 加速度センサの初期化。
 initAcc();
}

void loop()
{
 int cmps;
 short gyroX, gyroY, gyroZ;
 short accX, accY, accZ;
 
 cmps = getCompass();
 getGyro(&gyroX, &gyroY, &gyroZ);
 getAcc(&accX, &accY, &accZ);
 
 Serial.print("COMPASS:");
 Serial.println(cmps, DEC);
 Serial.print("GYRO X:");
 Serial.print(gyroX, DEC);
 Serial.print(" Y:");
 Serial.print(gyroY, DEC);
 Serial.print(" Z:");
 Serial.print(gyroZ, DEC);
 Serial.println();
 Serial.print("ACC X:");
 Serial.print(accX, DEC);
 Serial.print(" Y:");
 Serial.print(accY, DEC);
 Serial.print(" Z:");
 Serial.print(accZ, DEC);
 Serial.println();
  delay(200);
}

int getCompass(void)
{
 int data;
 
 // I2C通信開始。
    Wire.beginTransmission(COMPASS_ADDRESS);
    // Get dataコマンド送信。
    Wire.write('A');
    //Wire.write(0x41);
    // キューの送信。
    Wire.endTransmission();
    
    // データ要求。
    Wire.requestFrom(COMPASS_ADDRESS, 2);
    // データ受信。
    for (int i = 0; i < 2; i++) {
        while (Wire.available() == 0);
        buf[i] = Wire.read();
    }
    
    // データ変換。
    data = (unsigned short)buf[0];
    data = (data << 8) + (unsigned short)buf[1];
 
 return data;
}

void getGyro(short *x, short *y, short *z)
{
    // I2C通信開始。
    Wire.beginTransmission(GYRO_ADDRESS);
    // レジスタアドレスGYRO_TEMP_OUT_Hを指定。
    Wire.write(GYRO_TEMP_OUT_H);
    // キューの送信。
    Wire.endTransmission();
    
    delay(1);
    
    // I2C通信開始。
    Wire.beginTransmission(GYRO_ADDRESS);
    // データ要求。
    Wire.requestFrom(GYRO_ADDRESS, 8);
    // データ受信。
    for (int i = 0; i < 8; i++) {
        while (Wire.available() == 0);
        buf[i] = Wire.read();
    }
    Wire.endTransmission();
    
    *x = (((short)buf[2]) << 8) | buf[3];
    *y = (((short)buf[4]) << 8) | buf[5];
    *z = (((short)buf[6]) << 8) | buf[7];
}

void initAcc(void)
{
 Wire.beginTransmission(ACC_ADDRESS);
    Wire.write(ACC_POWER_CTL);
    Wire.write(0b00001000);
    Wire.endTransmission();
 Serial.println("Accelerometer Initialized.");
}

void getAcc(short *x, short *y, short *z)
{
 // I2C通信開始。
    Wire.beginTransmission(ACC_ADDRESS);
    // レジスタアドレスACC_DATA_X0 を指定。
    Wire.write(ACC_DATA_X0);
    // キューの送信。
    Wire.endTransmission();
    
    delay(1);
    
    // I2C通信開始。
    Wire.beginTransmission(ACC_ADDRESS);
    // データ要求。
    Wire.requestFrom(ACC_ADDRESS, 6);
    // データ受信。
    for (int i = 0; i < 6; i++) {
        while (Wire.available() == 0);
        buf[i] = Wire.read();
    }
    Wire.endTransmission();
    
    *x = (((short)buf[1]) << 8) | buf[0];
    *y = (((short)buf[3]) << 8) | buf[2];
    *z = (((short)buf[5]) << 8) | buf[4];
}


Raspberry Pi のコード(例によってWiringPiのdelay使用)
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <unistd.h>

#include <linux/i2c-dev.h>
#include <fcntl.h>
#include <sys/ioctl.h>

#include <wiringPi.h>


#define COMPASS_ADDRESS (0b0100001)
#define GYRO_ADDRESS (0xd0 >> 1)
#define ACCELEROMETER_ADDRESS (0xa6 >> 1)

// 関数プロトタイプ。
int getCompass(int addr);
void getGyro(int addr, short *x, short *y, short *z);
void initAcc(int addr);
void getAcc(int adr, short *x, short *y, short *z);

// 
char *i2cFileName = "/dev/i2c-0"; // I2Cドライバファイル名。

int main(int argc, char **argv)
{       
 
 int cmpsAddr = COMPASS_ADDRESS; // I2C Compass のアドレス。
 int gyroAddr = GYRO_ADDRESS;  // I2C Gyro のアドレス。
 int accAddr = ACCELEROMETER_ADDRESS; // 加速度センサのアドレス。
 unsigned short cmps;
 short gyroX, gyroY, gyroZ;
 short accX, accY, accZ;
 
 printf("***** start test program *****\n");
 
 initAcc(accAddr);
 
 while(1)
 {
  // コンパスの値取得。
  cmps = getCompass(cmpsAddr);
  // ジャイロの値取得。
  getGyro(gyroAddr, &gyroX, &gyroY, &gyroZ);
  // 加速度センサの値取得。
  getAcc(accAddr, &accX, &accY, &accZ);
  
  printf("COMPASS:%d\n", cmps);
  printf("GYRO X:%d, Y:%d, Z:%d\n", gyroX, gyroY, gyroZ);
  printf("ACC X:%d, Y:%d, Z:%d\n", accX, accY, accZ);
  
  delay(200);
 }
 
 return 0;
}

int getCompass(int addr)
{
 int cmps; // ファイルディスクリプタ。
 unsigned char buf[2];
 int deg;
 
 // I2CポートをRead/Write属性でオープン。
 if ((cmps = open(i2cFileName, O_RDWR)) < 0)
 {
  printf("Faild to open i2c port\n");
  exit(1);
 }
 
 // 通信先アドレスの設定。
 if (ioctl(cmps, I2C_SLAVE, addr) < 0)
 {
  printf("Unable to get bus access to talk to slave\n");
  exit(1);
 }
 
 buf[0] = 'A';
 if ((write(cmps, buf, 1)) != 1) {
  printf("Error writing to Compass\n");
  exit(1);
 }
 
 delay(1);
 
 // データ読み込み。
 if (read(cmps, buf, 2) != 2)
 {
  printf("Uneble to read from Compass\n");
  exit(1);
 }
 
 // クローズ。
 close(cmps);
 
 deg = (unsigned short)buf[0];
 deg = (deg << 8) + (unsigned short)buf[1];
 
 return deg;
}

void getGyro(int addr, short *x, short *y, short *z)
{
 int gyro; // ファイルディスクリプタ。
 unsigned char buf[8];
 
 // I2CポートをRead/Write属性でオープン。
 if ((gyro = open(i2cFileName, O_RDWR)) < 0)
 {
  printf("Faild to open i2c port\n");
  exit(1);
 }
 
 // 通信先アドレスの設定。
 if (ioctl(gyro, I2C_SLAVE, addr) < 0)
 {
  printf("Unable to get bus access to talk to slave\n");
  exit(1);
 }
 
 buf[0] = 0x1b;
 if ((write(gyro, buf, 1)) != 1) {
  printf("Error writing to Gyro\n");
  exit(1);
 }
 
 delay(1);
 
 // データ読み込み。
 if (read(gyro, buf, 8) != 8)
 {
  printf("Uneble to read from Gyro\n");
  exit(1);
 }
 
 // クローズ。
 close(gyro);
 
 *x = (((short)buf[2]) << 8) | buf[3];
    *y = (((short)buf[4]) << 8) | buf[5];
    *z = (((short)buf[6]) << 8) | buf[7];
}


void initAcc(int addr)
{
 int acc; // ファイルディスクリプタ。
 unsigned char buf[2];
 
 // I2CポートをRead/Write属性でオープン。
 if ((acc = open(i2cFileName, O_RDWR)) < 0)
 {
  printf("Faild to open i2c port\n");
  exit(1);
 }
 
 // 通信先アドレスの設定。
 if (ioctl(acc, I2C_SLAVE, addr) < 0)
 {
  printf("Unable to get bus access to talk to slave\n");
  exit(1);
 }
 
 buf[0] = 0x2d;
 buf[1] = 0b00001000;
 if ((write(acc, buf, 2)) != 2) {
  printf("Error writing to Accelerometer\n");
  exit(1);
 }
 
 delay(1);
 
 // クローズ。
 close(acc);
}

void getAcc(int addr, short *x, short *y, short *z)
{
 int acc; // ファイルディスクリプタ。
 unsigned char buf[6];
 
 // I2CポートをRead/Write属性でオープン。
 if ((acc = open(i2cFileName, O_RDWR)) < 0)
 {
  printf("Faild to open i2c port\n");
  exit(1);
 }
 
 // 通信先アドレスの設定。
 if (ioctl(acc, I2C_SLAVE, addr) < 0)
 {
  printf("Unable to get bus access to talk to slave\n");
  exit(1);
 }
 
 buf[0] = 0x32;
 if ((write(acc, buf, 1)) != 1) {
  printf("Error writing to Accelerometer\n");
  exit(1);
 }
 
 delay(1);
 
 // データ読み込み。
 if (read(acc, buf, 6) != 6)
 {
  printf("Uneble to read from Accelereometer\n");
  exit(1);
 }
 
 // クローズ。
 close(acc);
 
 *x = (((short)buf[1]) << 8) | buf[0];
    *y = (((short)buf[3]) << 8) | buf[2];
    *z = (((short)buf[5]) << 8) | buf[4];
}