2018年走行体EV3way(二輪倒立)について 後編

[前編に戻る](https://qiita.com/pulmaster2/items/c3f04f1e1d9c944af3d6)
---
本記事はETロボコン[Advent Calendar 2017](https://qiita.com/advent-calendar/2017/etrobo)のエントリです。

最大の危機を迎えた二輪倒立走行体 EV3way の倒立が安定しない原因を探ります。

##タイヤパターンノイズが気になる
自宅のフローリングなどで作業をしていると、大径タイヤは手で転がしたときにタイヤのイボイボからパターンノイズが伝わってきます。
従来のタイヤはパターンはあるものの、常にタイヤが路面に設置するようになっていて、パターンノイズが発生しにくい構造になっていました。
![171008_134615.jpg](https://qiita-image-store.s3.amazonaws.com/0/123504/3f650a03-360d-9ec3-53a9-33cce3ec6c7a.jpeg)

試しにほとんどパターンノイズの感じられないフェルトの上で倒立をさせてみましたが、挙動の改善はまったくありませんでした。パターンノイズは原因では無いようです。
実際に競技で走行するスタイロフォーム上のコースではほとんどノイズが気にならないレベルになります。

##まだ組み付けが甘い？
走行体の電源をOFFにした状態でタイヤを手で動かしてみると、10°くらいの範囲でタイヤが回転します。モーター駆動からタイヤに動力を伝える間に遊びがあることが分かります。
モーターからタイヤまではいくつかのブロックを使用していますが、ブロック自体に遊びはありませんでした。

##そもそもモーターが悪いんじゃ？
となるとモーターかということで、タイヤを外してモーター単体の確認をすると、モーター自体に遊びがあることが分かります。
Lモーターが内部にギヤを使用していることが原因のようです。

Mindstorms EV3で倒立振子を行おうとしている文献をネットで調べてみると、モーター選定の段階でバックラッシュ(遊び)が大きいLモーターは除外されていたりします。

モーターのバックラッシュが問題になるのは、回転方向を逆転させたときに、ある程度までモーターを回さないと実際にタイヤが回転しない不感帯が発生することです。倒立制御では回転方向の逆転が頻繁に発生しますので、バックラッシュは大きな阻害要因になります。ある程度走行速度が上がると回転方向の逆転が発生しなくなり挙動が安定してきます。

PWM出力とモーターエンコーダー値(enc)と実際のタイヤ(wheel)の動きのイメージ(想像図)
![image.png](https://qiita-image-store.s3.amazonaws.com/0/123504/6f34c65f-77b4-5cbd-e1b7-5ff6d6f14f3f.png)

#倒立制御の見直し
倒立振子APIのパラメーター、モデルを見てもバックラッシュ対策を考慮したような記述は見当たりません。
ならばということで、対策を倒立振子APIの外で行うことにします。
##APIの外で対策
倒立振子APIに入力するエンコーダー値をfixedencになるように加工します。
![image.png](https://qiita-image-store.s3.amazonaws.com/0/123504/c2b3a22b-ae84-f3b3-a426-b514a6f430c7.png)

PWM出力の反転時以外では、現実のタイヤの角度に相当するエンコーダー値をAPIに与えるようにします。
エンコーダーの実測値からバックラッシュの半分値を遅延させます。

PWM出力の反転時は、バックラッシュの分だけモーターを一気に動かしたいので、バックラッシュの遅延分を先に与えます。
モーターをバックラッシュ分だけ動かすという制御を行いたいのですが、この制御を後付けするのは困難なので、
倒立振子APIの中で確立されているモーター制御モデルを利用することになります。
加工したエンコーダー値を与えることで、バックラッシュ分のモーター制御を倒立振子APIに行ってもらいます。

2018年走行体用のサンプルプログラムとして配布するコードには次の処理が追加されています。
PWM出力の極性に応じてバックラッシュの半分値を加減算します。
モーター制御部としてクラス化されていると、もっとスッキリしたコードになると思います。

```app.c
//*****************************************************************************
// 関数名 : backlash_cancel
// 引数 : lpwm (左モーターPWM値 ※前回の出力値)
//        rpwm (右モーターPWM値 ※前回の出力値)
//        lenc (左モーターエンコーダー値)
//        renc (右モーターエンコーダー値)
// 返り値 : なし
// 概要 : 直近のPWM値に応じてエンコーダー値にバックラッシュ分の値を追加します。
//*****************************************************************************
void backlash_cancel(signed char lpwm, signed char rpwm, int32_t *lenc, int32_t *renc)
{
    const int BACKLASHHALF = 4;   // バックラッシュの半分[deg]

    if(lpwm < 0) *lenc += BACKLASHHALF;
    else if(lpwm > 0) *lenc -= BACKLASHHALF;

    if(rpwm < 0) *renc += BACKLASHHALF;
    else if(rpwm > 0) *renc -= BACKLASHHALF;
}
```

##元々バックラッシュあったじゃん？
NXTway-ET、EV3way-ET2015～の頃から確かにありましたが、今回タイヤが大径化されたことで悪影響が拡大しています。
バックラッシュに相当する回転距離が長くなりました。

今回作成したバックラッシュキャンセル処理を2017年走行体に追加すると倒立時の挙動安定化が確認できます。
(動画) [ETロボコン2017ダミーカー EV3way 2018年走行体用の倒立安定化処理](https://youtu.be/2yOVG-6epm4)

#ETロボコン2018走行体デモ
バックラッシュキャンセル処理を追加した倒立走行のデモがこちらです。
バックラッシュキャンセルの処理を追加したことで、倒立停止から低速走行の挙動がかなり改善できました。
(動画) [ETロボコン2018用 EV3way-ET試作走行体デモ走行](https://youtu.be/nnraKF_Adnw)
プログラムの内容は2017年走行体東海地区ダミーカーとして開発したものを、2018年走行体に対応できるよう改造しています。

#まとめ
かくして、最大の危機を迎えたEV3wayは、無事2018年も採用されることが決定しました。
プライマリークラス用ですので、基本的にはサンプルプログラムの設定を使用すれば良いと思いますが、
走行体の個体差で挙動が悪かったり、さらなる性能アップを目指したりなどされる場合は、
本記事や次の記事を参考に挑戦してみてください。
[EV3way ETロボコンで使用している倒立振子APIパラメーターの再計算手順](https://qiita.com/pulmaster2/items/08edce1ab097539b2deb)

#[オマケ] サンプルプログラム 定数変更箇所の解説 ※記事内の倒立パラメーターK_Fの値は**今後updateされる**場合があります。
```balancer.h
#define EXEC_PERIOD                    0.00450000000F            /* バランス制御実行周期(秒) *//* sample_c4の処理時間考慮 */
//#define EXEC_PERIOD                  0.00400000000F            /* バランス制御実行周期(秒) *//* 周期タスクでタイミングをとる場合はこちらに変更してください */
```
回路図からクロック24MHzに誤差は無いものとします。
クリスタルのスペック CS10-24.000MABJ-UT 24MHz ±30ppm
タイマー設定は下記で、割り切れているためタイマーに誤差は無いものとします。
EV3RTは0.1msecのtickをベースに動いています。
一般的に使用されることの多い1msecではありませんので、注意してください。

EXEC_PERIODの値は、サンプルプログラムの処理時間を実測して設定しています。
tslp_tsk(4) sample_c4 4,520usec
cyc_tsk(4)=>isig_sem=>wai_sem 4,007usec
周期タスクでタイミング取る場合は、精度良く4msec周期で処理を行えますのでEXEC_PERIODに0.004を設定してください。

```hrp2\arch\arm_gcc\am1808\chip_timer.c(22)
	CLOCK cyc = TO_CLOCK(TIC_NUME, TIC_DENO);
```
```hrp2\arch\arm_gcc\am1808\am1808.h(48)
	TIC_NUME (1U)
	TIC_DENO (10U)
```
```hrp2\arch\arm_gcc\am1808\chip_timer.h(53)
	TO_CLOCK(nume, deno) (OSCIN_MHZ * 1000U * (nume) / (deno))
```
```hrp2\target\ev3_gcc\ev3.h(22)
	OSCIN_MHZ (24)
```


```balancer_param.c
float K_F[4] = {-0.86526F, -30.73965F, -1.14828F*0.70F, -2.29757F};  // m=0.05;R=0.05;M=0.79;W=0.177;D=0.08;H=0.140 20180224updt
```
コメントの設定にて算出しています。
K_F[2]の *0.70 はライントレース走行状態を見て現物合わせしています。（標準バッテリー使用）

```balancer_param.c
float K_PHIDOT = 25.0F*2.75F; /* 車体目標旋回角速度係数 */
```
トレッドの変更に合わせて係数を変えています。
2009年の走行体から車幅の変わったときに *2.5 されたため、
今回の車幅変更に合わせて換算しています。
変更しない場合、従来よりも旋回動作が鈍くなります。

```balancer_param.c
float K_THETADOT = 6.00F; /* モータ目標回転角速度係数 */
```
タイヤ径の変更に合わせて係数を変えています。
この定義は倒立演算に使用していますが、走行体の現在位置の変化、つまり車速に関わります。
変更しない場合、従来よりも走行速度が速くなります。

```app.c
//*****************************************************************************
// 関数名 : backlash_cancel
// 引数 : lpwm (左モーターPWM値 ※前回の出力値)
//        rpwm (右モーターPWM値 ※前回の出力値)
//        lenc (左モーターエンコーダー値)
//        renc (右モーターエンコーダー値)
// 返り値 : なし
// 概要 : 直近のPWM値に応じてエンコーダー値にバックラッシュ分の値を追加します。
//*****************************************************************************
void backlash_cancel(signed char lpwm, signed char rpwm, int32_t *lenc, int32_t *renc)
{
    const int BACKLASHHALF = 4;   // バックラッシュの半分[deg]

    if(lpwm < 0) *lenc += BACKLASHHALF;
    else if(lpwm > 0) *lenc -= BACKLASHHALF;

    if(rpwm < 0) *renc += BACKLASHHALF;
    else if(rpwm > 0) *renc -= BACKLASHHALF;
}
```
バックラッシュキャンセル処理を追加しています。