L3G4200Dでロボット方向制御

以前地磁気センサーHMC5883Lを使ってロボットの方向制御を試しましたが結果はいまいちでした。
今回はジャイロセンサーを利用してみます。素子はPololu.comのimu01aという基板に載っているL3G4200Dというものです。I2Cでデジタル出力が得られますし、Pololuがmbed用にライブラリを用意してくれていますので取扱いは比較的楽です。(Murataジャイロに比べると値段もそれなりにします。)

測定レンジは３種類(250/500/2000 dps[degrees per second])から選べます。車輪ロボットへの搭載ですのでそれほど速い動きにはなりませんから最も狭い250を選びました。これはライブラリ(L3G4200D.cpp)の中身を変更します。出力はshortのフルレンジで±250dpsなので角速度を得るときに係数(250*2/2^16)=0.00763を掛けます。ただしデータシートには250dpsのときの係数は0.00875と書かれていましたので、一応こちらを採用しました。実際には0.00875を使うと積算した角度が多めになりました(図の左側)ので補正して0.00827を使いました(右側)。

角度の計算は台形方式で、20msecごとに測定しています。

地磁気センサーのときと比較すると格段によくなりました。

#include "mbed.h"
#include "USBSerial.h"
#include "L3G4200D.h"

USBSerial serial;
L3G4200D gyro(P0_5, P0_4);

Ticker flipper;

//gyro
int gyroBias;
int prevGyro;
double heading;

void flip() { 
    int g[3];
    
    gyro.read(g);
    heading += (double)(g[2] + prevGyro - 2*gyroBias) * 0.00827 * 0.02 * 0.5;
    prevGyro = g[2];
}

int main() {
    int sum = 0;
    
    for(int i=0; i<5000; i++){
        int g[3];
        gyro.read(g);
        sum+=g[2];
    }
    gyroBias = sum/5000;
    prevGyro = gyroBias;
    heading=0.0;
    
    flipper.attach_us(&flip, 20000);
    
    while(1) {
        serial.printf("angle=%f\r\n",heading);
        wait(0.2);
    }
}

MENU

Categories