TOPPERS/EV3RTでEV3のジャイロセンサーを使ってみる

TOPPERS/EV3RT環境でC言語を使って、EV3基本セット付属のジャイロセンサーを使ってみます。環境開発の記事はこちら

#EV3用ジャイロセンサーの特徴

EV3ジャイロセンサーは、一方向の角度と角速度を計測することが可能です。

コレがジャイロセンサーです。正面に書いてある矢印の方向に倒すと、初期角位置からの角度や角速度を測ることができるようです。

また、このセンサーは使用するうえで、角位置の初期化がとても大事になります。
例として、ETロボコンのプライマリークラスで使用される走行体を見てみましょう。

これが走行体です。(念のため説明しますが、手前の小瓶じゃなく奥のロボットです。手持ちの写真でいい感じのがこれしかなかったのです。)
走行中は尻尾を使わず、2輪のみでバランスを取りながら走行させる必要があります。そのために必要なAPIは公開されているので、必要な情報を渡してあげれば倒立走行は可能なのですが、APIに渡さなければいけない情報の中には、ジャイロセンサーを使って取得しなくてはならない情報も含まれています。

このAPIは、車体が前に傾いていたら倒れないように前の方に勢いよく進む、後ろに傾いていたら倒れないように後ろに勢いよく進むようにできています。車体の傾き具合はジャイロセンサーを用いて取得します。

しかし、**ジャイロセンサーの初期角位置は、ソースコード内でセンサーポートとセンサーをつなげた時点での角位置になります。**これがどういった意味を持つかというと、安定して倒立走行させるためには、アプリ起動時に毎回必ず同じ姿勢をとらせる必要があるということです。

しかし、現実的にそれは不可能な話で、アプリ起動時の微妙な角位置のずれが原因で、同じソースコードなのに前回はうまく走れて、次に動かしたときにすぐに転んでしまったといった事態が発生しかねません。走行体は一度転ぶと走行することも起き上がることは不可能で、走行不能となった時点で失格となってしまいます。

こんなことを防ぐためにも、ジャイロセンサーの角位置初期化は非常に重要になります。そして、EV3RTのAPIには、ちゃんと角位置初期化のためのメソッドも用意されています。

#ジャイロセンサーと本体の接続

上の画像のように接続しました。ジャイロセンサーをセンサーポート1に、タッチセンサーをセンサーポート2に接続しています。なぜタッチセンサーまで接続しているかというと、前述した角位置の初期化をボタン操作で行うためです。(タッチセンサーの記事はこちらを参照してください。)また、ジャイロセンサーは固定されてないと困るので、ブロックを使って本体につなぎとめておきます。私はこんな感じで固定しました。

#ソースコードを書く

今回は、角位置の初期化を行ったのちにボタン操作で無限ループに入り、現在の角位置をLCDに表示し続けるアプリ(gyro-test)を制作します。
以下がソースコードになります。

app.c

/**
 ******************************************************************************
 ** ファイル名   : app.c
 ** 説明       : 角位置をリセットし、リセット後の角位置を表示し続ける
 ******************************************************************************
 **/

#include "ev3api.h"
#include "app.h"
#include "stdlib.h"

#if defined(BUILD_MODULE)
#include "module_cfg.h"
#else
#include "kernel_cfg.h"
#endif

#define DEBUG

#ifdef DEBUG
#define _debug(x) (x)
#else
#define _debug(x)
#endif

/* 状態をマクロで定義して利用する */
#define INIT_WAIT 0
#define START_WAIT 1
#define RUN 2

/* センサーの設定 */
static const sensor_port_t
    gyro_sensor = EV3_PORT_1,
    touch_sensor = EV3_PORT_2;

/* 関数のプロトタイプ宣言 */
int button_pressed(void);

/* メインタスク */
void main_task(intptr_t unused)
{
    /* 角位置格納用の変数 */
    static int angle = 0;
    static int state = INIT_WAIT;
    /* LCD表示文字用の配列 */
    char message[30];

    /* ループに入る前にセンサーとセンサーポートを接続する */
    ev3_sensor_config (gyro_sensor, GYRO_SENSOR);
    ev3_sensor_config (touch_sensor, TOUCH_SENSOR);
    /* 接続が完了したら合図として本体LEDをオレンジ色に変える */
    ev3_led_set_color(LED_ORANGE); 

    /* ループの中に入る */
    while(1)
    {   /* stateに応じた条件分岐構造 */
        switch(state){
            case INIT_WAIT:
                /* INIT_WAITのときはタッチセンサーが押されるまで待ち、
                押されたらジャイロセンサーの角位置の初期化を行い次の状態へ */
                sprintf(message, "WAIT INIT......");
                if(button_pressed() == 1){ 
                    ev3_speaker_play_tone (NOTE_C4, 100);
                    ev3_gyro_sensor_reset(gyro_sensor);
                    state = START_WAIT;
                }
            break;
            case START_WAIT:
                /* START_WAITのときはタッチセンサーが押されるまで待ち、
                押されたら次の状態へ */
                sprintf(message, "WAIT START.....");
                if(button_pressed() == 1){
                    ev3_speaker_play_tone (NOTE_C5, 100);
                    state = RUN;
                }
            break;
            case RUN:
                /* RUNのときはジャイロセンサーから角位置を取得してLCDに表示し続ける。*/
                angle = ev3_gyro_sensor_get_angle(gyro_sensor);
                sprintf(message, "ANGLE:%d          ",angle);
            break;
        }
        /* 正面ディスプレイに文字配列の中身を描写させる */
        ev3_lcd_draw_string(message, 0,10);
    	
    }
    ext_tsk();
}

/* チャタリング対策用の関数 */
int button_pressed(){
    /* ボタンの現在と直前の状態と関数の返り値を保持する変数 */
    static int now = 0;
    static int before = 0;
    int kekka = 0;

    /* ボタンの現在値を記録する */
    now = ev3_touch_sensor_is_pressed(touch_sensor);
    /* 立ち上がりを検出したときのみ返り値を1に設定する */
    if(now == 1 && before == 0){
        kekka = 1;
    }else{
        kekka = 0;
    }
    before = now;
    return kekka;
}

#動作確認

あおむけの状態で角位置の初期化をすると上の画像のようになります。角位置が0なのでちゃんと動いているみたいですね。
で、これを起こすと...

数値が変わりました。どうやら前に倒すと角度はマイナスに、後ろに倒すと角度はプラスになるようです。