C言語でEV3開発(28)-EV3の移動距離を計算できるようにしてみた

公開日： 2018/04/15 : 最終更新日：2019/06/08 C言語, マインドストーム/EV3, 開発

どもです。
今回は、Ev3の移動距離の算出についてです。

1.算出方法について
移動距離は、積分により算出します。
即ち、規定時間内での変化量から、その間での移動距離を算出、その値の積算値を移動距離とします。
具体的な計算の順番は、以下の通りです。

(1).各時間ごとのモーターの角度を算出する。
(2).各時間内でのモーターの角度の変化量を算出する。
(3).角度の変化量に対するタイヤの円周を算出する。
(4).算出した円周を積算する。

2.算出方法の実装
上記計算の実装は、下記の通りです。

static int32_t calc_distance(int32_t count, int32_t count_prev);

/**
 * @brief   Calcultate travel distance.
 */
void calc_trav_distance(void) {
    int32_t distance = 0;

    //left_motor
    distance = calc_distance(left_motor_count, left_motor_count_prev);
    left_motor_trav_dist_hi_acc += distance;
    left_motor_trav_dist = left_motor_trav_dist_hi_acc / 100;    //Convert LSB from 0.01 to 1.
    if (distance &lt; 0) {
        left_motor_trav_dist_abs -= (distance / 100);
    } else {
        left_motor_trav_dist_abs += (distance / 100);
    }

    //right_motor
    distance = calc_distance(right_motor_count, right_motor_count_prev);
    right_motor_trav_dist_hi_acc += distance;
    right_motor_trav_dist = right_motor_trav_dist_hi_acc / 100;   //Convert LSB from 0.01 to 1.
    if (distance &lt; 0) {
        right_motor_trav_dista_abs -= (distance / 100);
    } else {
        right_motor_trav_dista_abs += (distance / 100);
    }

    left_motor_count_prev = left_motor_count;
    right_motor_count_prev = right_motor_count;
}

/**
 * @brief   Calc travel distance from count and that of previous cycle.
 *          The calculated value has high accuracy.
 */
static int32_t calc_distance(int32_t count, int32_t count_prev) {
    int32_t dcount = 0;
    int32_t distance = 0;

    dcount = count - count_prev;
    distance = PI * RADIUS * dcount / 180;

    return distance;
}

static int32_t calc_distance(int32_t count, int32_t count_prev);

/**

* @brief Calcultate travel distance.

void calc_trav_distance(void) {

int32_t distance = 0;

//left_motor

distance = calc_distance(left_motor_count, left_motor_count_prev);

left_motor_trav_dist_hi_acc += distance;

left_motor_trav_dist = left_motor_trav_dist_hi_acc / 100; //Convert LSB from 0.01 to 1.

if (distance < 0) {

left_motor_trav_dist_abs -= (distance / 100);

} else {

left_motor_trav_dist_abs += (distance / 100);

}

//right_motor

distance = calc_distance(right_motor_count, right_motor_count_prev);

right_motor_trav_dist_hi_acc += distance;

right_motor_trav_dist = right_motor_trav_dist_hi_acc / 100; //Convert LSB from 0.01 to 1.

if (distance < 0) {

right_motor_trav_dista_abs -= (distance / 100);

} else {

right_motor_trav_dista_abs += (distance / 100);

}

left_motor_count_prev = left_motor_count;

right_motor_count_prev = right_motor_count;

}

/**

* @brief Calc travel distance from count and that of previous cycle.

* The calculated value has high accuracy.

static int32_t calc_distance(int32_t count, int32_t count_prev) {

int32_t dcount = 0;

int32_t distance = 0;

dcount = count - count_prev;

distance = PI * RADIUS * dcount / 180;

return distance;

}

上記実装では、calc_distance()で角変化量から、対応するタイヤの円周を算出しています。
円周を計算しているのが、

    distance = PI * RADIUS * dcount / 180;

1	distance = PI * RADIUS * dcount / 180;

の部分です。
この式は、「2 x π x R x (θ / 360) = π x R x (θ / 180)」です。
この「θ」の部分が、変化量となっています。
また、円周の算出の際に使用するパラメータですが、具体的な値と精度(LSB)は下記のように設定しています。

static const int RADIUS = 28;   //Unit:mm LSB:1
static const int PI = 314;      //Unit:- LSB:0.01

1 2	static const int RADIUS = 28; //Unit:mm LSB:1 static const int PI = 314; //Unit:- LSB:0.01

なお、モーターの角度については、EV3RTのAPIを用いて取得しています。
具体的な実装を、下記に示します。

/**
 * Read motor count in degree unit.
 */
void motor_get_count(void) {
    left_motor_count = ev3_motor_get_counts(left_motor_port);
    right_motor_count = ev3_motor_get_counts(right_motor_port);
}

/**

* Read motor count in degree unit.

void motor_get_count(void) {

left_motor_count = ev3_motor_get_counts(left_motor_port);

right_motor_count = ev3_motor_get_counts(right_motor_port);

}

この設定値から、calc_distance()で算出される円周の精度は0.01(mm)と、少し高精度の値となります。
その後、少し精度を下げて移動方向を考慮した値と、移動方向を考慮していない値を算出しています。

3.実行/実測結果
上記の実装を用いた測定の結果のグラフを、以下に示します。

グラフを見ると、移動距離の測定値は、おおよそ170cm(1700mm)となっています。
では、実際の様子を動画で録画したので、見てみましょう。

えー、こちらでもおおよそ171～2cm(1710～20mm)となっており、上記の測定結果と一致しています。
これにより、Ev3の移動距離が算出できていることが確認できました。

まとめ
以上、今回のエントリでは、モータの回転角度とEV3に同梱されているタイヤのサイズから移動距離を算出する方法を記載してみました。
また、その方法で算出される移動距離が、実際の測定距離と一致していることを確認しました。
コレにより、積分を用いて測定された移動距離が、実際の値と一致することが証明できました。

以上！
ではっ！！