ゼロから始めるロボットプログラミング入門講座

ジャイロセンサの制御方法(続き) dsPIC入門講座05

開発環境

基盤

  • SPPBoard(dsPIC30F3014使用)

コンパイラ

  • C30

ジャイロセンサ

  • IMU3000

はじめに

以前、ジャイロセンサ(IMU3000)をdsPIC上で制御するための基礎知識について解説しました。

今回はIMU3000の通信仕様について軽く触れた後にdsPIC上で制御するサンプルプログラムを紹介します。

IMU3000とは?

3.3V駆動でx, y, z方向の3軸仕様でI2Cインタフェースをサポートしたジャイロセンサです。

IMU3000の通信方法

それではIMU3000の仕様書から必要な情報を読み取りましょう。

データ送信方式

send

記号に関しては以下を参照

terms

データ受信方式

read

レジスタ解説

ジャイロの値を取得する際にアクセスする必要があるレジスタについて解説します。

terms

register

  • Register 60

FIFO Dataレジスタにはレジスタ18で有効にされたデータが16ビットで格納されています。

register60

register60

  • Register 18

FIFOレジスタに格納するデータを選択する

register18

IMU3000のFIFOは最低でも1ワード読み出しできるものがないといけないのでそれを保証するのでFIFO_FOOTERです。

  • Register 22

ジャイロのフルスケールレンジとサンプルレートを設定します。

register22

フィルターとサンプルレートの設定する

register22

register22

フルスケールレンジの設定する

register22

  • Register 21

サンプルレートを設定する

register21

register21

(注) 8ビットなので当然、SMPLRT_DIVは0 ~ 255の範囲で設定すること

  • Register 61

使用したい機能はこのレジスタでリセットする

register61

register61

FIFO_ENとFIFO_RSTとGYRO_RSTは最低限設定する必要があります。

IMU3000の制御関数を作ろう

初期化関数

I2C I2CInitFunc(void){
    I2C i2c;
    static short first = TRUE;

    if(first){
        I2CBRG = 399;
        I2CCON = 0b1001001001111101;
        IDLE_I2C;
        I2CADD = 0x09;
        first = FALSE;
    }

    i2c.close = _CloseI2C;
    i2c.write = WriteI2C;
    i2c.read  = ReadI2C;
    i2c.SendDataToIMU = SendDataToIMU;
    i2c.ReadDataFromIMU = ReadDataFromIMU;

    return i2c;
}

i2c_register_map

  • I2CBRG = 399

通信速度を設定 40MHz動作時に100kHzで通信

  • I2CCON = 0b1001001001111101

I2Cの初期設定

  • I2CADD = 0x09

スレーブとしてのアドレスを設定

データ送信用関数

static void SendDataI2C(I2CData data){

    I2CTRN = data;
    if(!I2CSTATbits.IWCOL){
        while(I2CSTATbits.TRSTAT);
        IDLE_I2C;
    }

    while(I2CSTATbits.TBF);
    while(I2CSTATbits.ACKSTAT);
    IDLE_I2C;

}
  • I2CTRN = data

データ送信

  • if(!I2CSTATbits.IWCOL)

書き込み衝突なしの場合

  • while(I2CSTATbits.TRSTAT)

送信終了待ち

  • while(I2CSTATbits.TBF)

I2CTRNが空になるまで待つ

  • while(I2CSTATbits.ACKSTAT)

アクノリッジが返って来るまで待つ

アドレス送信関数

static void SendAddressI2C(I2CData address, char AddressType){
    address = address << 1;
    if(AddressType == SEND){
        address &= 0b11111110;
    }else{
        address |= 0b00000001;
    }
    SendDataI2C(address);

}

スレーブへ送信する場合は最下位ビットを0に スレーブから受信する場合は最下位ビットを1にする。

IMU3000への送信関数

この関数は仕様書に書かれている以下の規則に従ったものです。

send

static void SendDataToIMU(I2CData I2CAddress, I2CData RegisterAddress, I2CData data){

    I2CCONbits.SEN = 1;
    IDLE_I2C;

    SendAddressI2C(I2CAddress, SEND);
    SendDataI2C(RegisterAddress);
    SendDataI2C(data);

    I2CCONbits.PEN = 1;
    IDLE_I2C;

}
  • I2CCONbits.SEN = 1

スタートコンディション発行

  • SendAddressI2C(I2CAddress, SEND)

IMUアドレス+送信要求

  • SendDataI2C(RegisterAddress)

レジスタアドレスを送信

  • SendDataI2C(data)

レジスタへの書き込み

  • I2CCONbits.PEN = 1

ストップコンディション発行

汎用送信関数

static void WriteI2C(I2CData address, I2CData data){

    I2CCONbits.SEN = 1;
    IDLE_I2C;

    SendAddressI2C(address, SEND);
    SendDataI2C(data);

    I2CCONbits.PEN = 1;
    IDEL_I2C;

}

IMU3000からの受信関数

この関数は仕様書に書かれている以下の規則に従ったものです。

read

static I2CData ReadDataFromIMU(I2CData I2CAddress, I2CData RegisterAddress){

    I2CData buffer;

    I2CCONbits.SEN = 1;
    IDLE_I2C;

    SendAddressI2C(I2CAddress, SEND);
    SendDataI2C(RegisterAddress);

    I2CCONbits.SEN = 1;
    IDLE_I2c;

    SendAddressI2C(I2CAddress, READ);

    I2CCONbits.RCEN = 1;
    while(I2CCONbits.RCEN);
    I2CSTATbits.I2COV = 0;
    buffer = I2CRCV;
    IDLE_I2C;

    I2CCONbits.ACKDT = 1;
    I2CCONbits.ACKEN = 1;
    IDLE_I2C;

    I2CCONbits.PEN = 1;
    IDLE_I2C;

    return buffer;

}

  • SendAddressI2C(I2CAddress, SEND)

    アドレス+送信要求

  • SendDataI2C(RegisterAddress)

    レジスタアドレス送信

  • SendAddressI2C(I2CAddress, READ)

    アドレス+受信要求

  • I2CCONbits.RCEN = 1

    受信モードを有効化

  • while(I2CCONbits.RCEN)

    有効化待ち

  • I2CSTATbits.I2COV = 0

    オーバーフローなしにセット

  • buffer = I2CRCV

    受信データを読み出し

  • I2CCONbits.ACKDT = 1
    I2CCONbits.ACKEN = 1

NACKを出力

汎用受信関数

static I2CData ReadI2C(I2CData address){

    unsigned char received_data;

    I2CCONbits.SEN = 1;
    IDLE_I2C;

    SendAddressI2C(address, READ);

    I2CCONbits.RCEN = 1;
    while(I2CCONbits.RCEN);
    I2CSTATbits.I2COV = 0;
    received_data = I2CRCV;
    IDLE_I2C;

    I2CCONbits.ACKDT = 1;
    I2CCONbits.ACKEN = 1;
    IDLE_I2C;

    I2CCONbits.PEN = 1;
    IDLE_I2C;

    return received_data;

}

IMU3000の制御関数を使ってみよう

int main(void){

    int buffer[8];
    int GyroX;
    int i;
    TRISA = 0x800;
    TRISB = 0x1C7;
    TRISC = 0x6000;
    TRISD = 0x200;
    TRISF = 0x0000;

    I2C i2c = I2CInitFunc();
    Lcd lcd = LcdInitFunc();

    i2c.SendDataToIMU(GYRO, 0x12, 0x71);
    i2c.SendDataToIMU(GYRO, 0x16, 0x09);
    i2c.SendDataToIMU(GYRO, 0x15, 0x07);
    i2c.SendDataToIMU(GYRO, 0x3D, 0x43);

    while(1){
        for(i=0; i < 8; i++) buffer[i] = (int)i2c.ReadDataFromIMU(GYRO, 0x3C);
        GyroX = buffer[0] << 8 | buffer[1];

        lcd.NumPuts(GyroX);
        wait_msec(1000);
        lcd.clear();
        wait_msec(1000);
    }

    return 0;
}
  • GYRO

IMU3000のアドレス

0x68であらかじめ定義している

  • i2c.SendDataToIMU(GYRO, 0x12, 0x71)

FIFO Enable (GyroX + GyroY + GyroZ + FIFO_FOOTER)

  • i2c.SendDataToIMU(GYRO, 0x16, 0x09)

DLPF, Full scale(500 deg/sec, LowPass->188Hz, ADサンプルレート1kHz)

  • i2c.SendDataToIMU(GYRO, 0x15, 0x07)

SampleRate(125Hz)

  • i2c.SendDataToIMU(GYRO, 0x3D, 0x43)

User Control (FIFO_EN = 1, FIFO_RST = 1, GYRO_RST = 1)

  • for(i=0; i < 8; i++) buffer[i] = (int)i2c.ReadDataFromIMU(GYRO, 0x3C)

FIFOレジスタからX, Y, Z, FIFO_FOOTERを取得

  • lcd.NumPuts(GyroX)

X軸の値をLCDに表示

  • lcd.clear()

LCDの表示をクリア

以上でジャイロセンサから取得したX軸の値をLCDに表示するサンプログラムの完成です。

blog comments powered by Disqus

Published

06 September 2013

Category

SPPBoard

Tags

Profile

千葉工大産のロボットナビゲーションエンジニア

ros-jpの勉強会の主催やロボカップ世界大会優勝チームのリーダをやってました。


　
badge_description about badge's

　

Follow me on Github

Follow me on Qiita

総訪問者数

アクセスカウンター