9 Degrees of Freedom センサスティック for UNO

9 Degrees of Freedom　センサスティック for Uno

http://www.usmv.sys.okayama-u.ac.jp/~in/sampleprg/sen-10724.html を参考に
（ほとんどそのままに） Unoように　9Degrees for Freedom センサスティック用の
スケッチを作ってみた。　I.N.先生に感謝します。

---

//ITG-3200 & ADXL345 & HMC5883L 読み出しサンプル
//2012.12.14-17 I.N.
//2014 Nov. 14 　H.I.  for Arudino UNO

//#include <rxduino.h>　for GR-SAKURA
#include <Wire.h>

#define delayus(t) delayMicroseconds(t)
#define delayms(t) delay(t)

//ジャイロセンサ(ITG-3200)
#define G_ADR           0x68
#define G_SMPLRT_DEV    0x15
#define G_DLPF_FS       0x16
#define G_XOUT_H        0x1D
#define G_XOUT_L        0x1E
#define G_YOUT_H        0x1F
#define G_YOUT_L        0x20
#define G_ZOUT_H        0x21
#define G_ZOUT_L        0x22
//加速度センサ(ADXL345)
#define A_ADR           0x53
#define A_POWER_CTL     0x2D
#define A_DATAX0        0x32
#define A_DATAX1        0x33
#define A_DATAY0        0x34
#define A_DATAY1        0x35
#define A_DATAZ0        0x36
#define A_DATAZ1        0x37
//地磁気センサ(HMC5883L)
#define M_ADR           0x1E
#define M_CONFIG_A      0x00
#define M_CONFIG_B      0x01
#define M_MODE          0x02
#define M_DATAX_MSB     0x03
#define M_DATAX_LSB     0x04
#define M_DATAZ_MSB     0x05
#define M_DATAZ_LSB     0x06
#define M_DATAY_MSB     0x07
#define M_DATAY_LSB     0x08

// shortを"-##### "～"+##### "へ変換
void short2dec5(short n, char *s)
{
    unsigned char a;
    int d;
    if(n<0){
        d=-n;
        s[0]='-';
    }else{
        d=n;
        s[0]='+';
    }
    a=(unsigned int)d/10000u;
    s[1]='0'+a;
    d-=(unsigned int)a*10000u;
    a=(unsigned int)d/1000u;
    s[2]='0'+a;
    d-=(unsigned int)a*1000u;
    a=(unsigned int)d/100u;
    s[3]='0'+a;
    d-=(unsigned int)a*100u;
    a=(unsigned int)d/10u;
    s[4]='0'+a;
    d-=(unsigned int)a*10u;
    a=d;
    s[5]='0'+a;
    s[6]=' ';
    s[7]=0;
}
#define PIN_SW  5
#define PIN_LED0 A2
#define PIN_LED1 A3
#define PIN_LED2 5
#define PIN_LED3 6


void setup() {
    pinMode(PIN_SW,INPUT);       //青ボタン使用
    pinMode(PIN_LED0,OUTPUT);    //LED0使用
    pinMode(PIN_LED1,OUTPUT);    //LED1使用
    pinMode(PIN_LED2,OUTPUT);    //LED2使用
    pinMode(PIN_LED3,OUTPUT);    //LED3使用
    digitalWrite(PIN_LED0,0);
    digitalWrite(PIN_LED1,0);
    digitalWrite(PIN_LED2,0);
    digitalWrite(PIN_LED3,0);

    //---Serialは外部へのデータ送信---
//    Serial.begin(38400,SCI_USB0);      // 仮想USB(baudrateはダミー)
    //Serial.begin(115200,SCI_SCI0P2x);   //IO0=RXD0, IO1=TXD0
    //Serial.begin(187500,SCI_SCI0P2x);   //IO0=RXD0, IO1=TXD0
    Serial.begin(9600);  //for Uno
  //  sci_str *sci;
  //  sci=Serial.get_handle();
  //  sci->crlf_rx=CRLF_NONE;                //受信：改行変換なし
 //   sci->crlf_tx=CRLF_NONE;                //送信：改行変換なし

    //I2C開始
    Wire.begin();

    //ジャイロセンサ初期設定
    Wire.beginTransmission(G_ADR);
        Wire.write(G_SMPLRT_DEV);
        Wire.write(0x09);        //1k/(9+1)=100Hz
        Wire.write(0x18|0x01);    //2000deg/s, lpf=188Hz
        Wire.endTransmission();  

    //加速度センサ初期設定
    Wire.beginTransmission(A_ADR);
        Wire.write(A_POWER_CTL);
        Wire.write(0x00);
        Wire.endTransmission();  
    Wire.beginTransmission(A_ADR);
        Wire.write(A_POWER_CTL);
        Wire.write(0x10);
        Wire.endTransmission();  
    Wire.beginTransmission(A_ADR);
        Wire.write(A_POWER_CTL);
        Wire.write(0x08);
        Wire.endTransmission();  

    //地磁気センサ初期設定
    Wire.beginTransmission(M_ADR);
        Wire.write(M_CONFIG_A);
        Wire.write(0x18);    //average=1, rate=75[Hz]
        Wire.write(0x20);    //gain=1.3Ga
        Wire.endTransmission();  
    Wire.beginTransmission(M_ADR);
        Wire.write(M_MODE);
        Wire.write(0x01);    //mode=single
        Wire.endTransmission();  

    delayms(100);
    digitalWrite(PIN_LED0,1);
}

unsigned char dat[18];
short d[9];
unsigned int dt,t0;

void loop() {
    char ss[8];
    int i;

    digitalWrite(PIN_LED1, 1);
    t0=micros();

    //ジャイロ読み出し
    Wire.beginTransmission(G_ADR);
    Wire.write(G_XOUT_H);
    Wire.endTransmission();  
    Wire.requestFrom(G_ADR,6);
    while(Wire.available()<6);
    for(i=0;i<6;i++){
        dat[i]=Wire.read();
    }
    d[0]=(dat[0]*0x100)|dat[1];
    d[1]=(dat[2]*0x100)|dat[3];
    d[2]=(dat[4]*0x100)|dat[5];

    //加速度読み出し
    Wire.beginTransmission(A_ADR);
    Wire.write(A_DATAX0);
    Wire.endTransmission();  
    Wire.requestFrom(A_ADR,6);
    while(Wire.available()<6);
    for(i=6;i<12;i++){
        dat[i]=Wire.read();
    }
    d[3]=(dat[ 7]*0x100)|dat[ 6];
    d[4]=(dat[ 9]*0x100)|dat[ 8];
    d[5]=(dat[11]*0x100)|dat[10];

    //地磁気読み出し
    Wire.beginTransmission(M_ADR);
        Wire.write(M_DATAX_MSB);
        Wire.endTransmission();  
    Wire.requestFrom(M_ADR,6);
    while(Wire.available()<6);
    for(i=12;i<18;i++) {
        dat[i]=Wire.read();
    }
    d[6]=(dat[12]*0x100)|dat[13];
    d[7]=(dat[14]*0x100)|dat[15];
    d[8]=(dat[16]*0x100)|dat[17];
    Wire.beginTransmission(M_ADR);
        Wire.write(M_MODE);
        Wire.write(0x01);    //mode=single
        Wire.endTransmission();  

    //データ出力
    for(int i=0;i<9;i++){
        short2dec5(d[i],ss);
        Serial.print(ss);
        if(i==2 || i==5) Serial.print("|");
    }
    Serial.print("\r\n");

    digitalWrite(PIN_LED1,0);
    //ここまでで約4ms on GR-SAKURA
    //時間待ち
    do{
        dt=micros()-t0;
    }while(dt<10000);
    t0+=10000;
}