ETロボコンプライマリクラス向け転倒安全装置

ETロボコンプライマリークラスにおける転倒安全装置を作ってみようと思います。

ETロボコンプライマリークラスでは、走行体が転倒してリタイア宣言がされると、係員の方がロボットを回収してくれます。
しかし、回収の際に虫捕り網が使用される場合があるのですが、タイヤが動いていると回収される間に網がタイヤに絡まってしまいます。
これは車体にいらない負荷をかけるだけでなく、係員の方にも迷惑になってしまいます。

そのため、多くのチームが、転倒を検知したらモーターを止める、という安全装置を実装しています。gyro-testで使ったものにモーターを繋げてあげればできることなので、作り方をざっくりと解説します。(モーターとジャイロセンセンサーをつかうので、モーターの記事とジャイロセンサーの記事をそれぞれ参照してください)

こんな感じでモーターポートAにLモーターを接続します。
以下ソースコードとなります。

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
#define STOP 3

/* マジックナンバーの使用を避けるため、転倒閾値をマクロ定義する */
#define FALL_DOWN /* 計測して決めてください */

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

/* モーターの設定 */
static const motor_port_t
    motor = EV3_PORT_A;

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

/* メインタスク */
void main_task(intptr_t unused)
{
    /* 状態管理用の変数 */
    static int state = INIT_WAIT;
    /* LCD表示文字用の配列 */
    char message[30];

    /* ループに入る前にセンサーとセンサーポートを、モーターとモーターポート接続する */
    ev3_sensor_config (gyro_sensor, GYRO_SENSOR);
    ev3_sensor_config (touch_sensor, TOUCH_SENSOR);
    ev3_motor_config (motor, LARGE_MOTOR);
    /* 接続が完了したら合図として本体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のときはモーターを回し続ける。ただし、転倒閾値を超えたらSTOPへ*/
                if(ev3_gyro_sensor_get_rate(gyro_sensor) > FALL_DOWN){
                    state = STOP;
                }else{
                    ev3_motor_set_power(motor, 30);
                    sprintf(message, "RUNNING......");
                }
            break;
            case STOP:
                /* STOPのときはモーターを止める*/
                ev3_motor_stop(motor,true);   
                sprintf(message, "I FALL DOWN. STOP THE MOTOR.");
            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;
}

センサーポートへの接続が完了したら、状態をいくつか設けて管理し、無限ループでボタン入力を待って(初期化待機状態)、ボタン入力があったところで角位置を初期化しています。初期化が完了したら再びボタン入力を待って(スタート待機状態)、ボタン入力を確認したらモーターを回転させます。(駆動状態)モーター動作中は転倒安全装置メソッドを呼び続け、転倒を検知するとモーターを停止させ、停止した旨の表示をします。(停止状態)実際に動かすとこんな感じになります↓↓↓

ETロボコンプライマリークラス向けの転倒安全装置を作ってみました pic.twitter.com/iAk1LMvh5j

— こーひーのき (@tiny_cafe_tree) 2017年3月4日

転倒安全装置に渡す引数(角速度)は、gyro-testを少し書き換えて本体が転倒したときの角速度を計測しておくとやりやすいです。(取得して変数に格納する値を角速度にすればいいので、呼び出すAPIを変えればいいのです。)
ETロボコン本番では、走行体が転倒するとだいたい±hogeくらいになるのですが、ここで数値書いちゃうといろいろマズいとおもうので、参加する方々はぜひ計測してみてください。(おすすめの計測方法はそのうち別な記事で紹介すると思うので、分らない方は気長に待ってていただければと...)