はじめに
リコーの @KA-2 です。
弊社ではRICOH THETAという全周囲360度撮れるカメラを出しています。
RICOH THETA VやRICOH THETA Z1は、OSにAndroidを採用しています。Androidアプリを作る感覚でTHETAをカスタマイズすることもでき、そのカスタマイズ機能を「プラグイン」と呼んでいます（詳細は本記事の末尾を参照）。
この記事は「THETAプラグイン×M5BALA Part1：ラジコンを作る」の続編です。
前回記事は、ただの組み立て動作確認みたいなもの。ここからが本番デスヨ。
（ほぼ）同じハードウェア構成のままにライントレーサーを作りました。
"THETA内部で車体下方周囲の黒い線を認識し、黒い線と車体前方のズレをなくすようM5BALAへモーター動作指示を出す"というサイクルを繰り返すことで、THETAが自律動作をします。
仕上がりは「こんな感じになりました」という動画をご覧ください。
＜2019/07/25 ボトルキャップチャレンジ動画も含むリンクへ差し替えました＞
チャレンジしてみました！
— THETAプラグイン開発者コミュニティ (@thetaplugin) July 26, 2019
PID制御パラメータが良ければ繰り返せるはず？
作成者は「当たらなければどうということはない」などと訳のわからないことを･･･#M5Stack #M5Bala #thetaplugin #BottleCapChallenge #ボトルキャップチャレンジ pic.twitter.com/pbEKaDI3YL
左右のモーターへ同じ数値の回転指示をだしても右曲がりしてたこの子が、滑らかに真っ直ぐ走るだけでなく、加速減速や操舵まで自動で行っているぅぅぅぅぅ（感涙）。
これでM5BALAに付いているエンコーダーを利用してないんだぜ？（THETAの映像品質が良いということ）なかなかの動きでしょ？
デバッグ画像の描画と無線LANからの出力しながらに、ほぼ15fpsのサイクルを実現しました(ワイプをご確認ください)。
〆切優先で「デモンストレーションに問題ないレベルに仕上げる」ことを目標に作業していました。修正したほうが良い点がいくらか残っていますが、技術解説をしてきたいと思います。
「〆切」？「デモンストレーション」？いつ？どこで？というわけで告知があります。
Maker Faire Tokyo 2019に一般参加します
私たちは「THETAプラグイン開発者コミュニティ」としてMFT2019に一般参加します。
スペース番号は「I/03-08」、出展内容は「THETA plug-in of Things (ToT)」
THETAプラグイン と ナ・ニ・カ (ToT)
複数のナニカの１つとしてこのライントレーサーをテーブルの上で走らせます！
他のナニカは MFT2019の出展者ページに文字だけ書いてあります。今後の記事で各担当が紹介しますのでご注目ください！
MFTに応募した春の構想段階では、THETAとM5BALAの繋ぎ目が長かったんだなぁ･･･（遠い目）
余談ですが、動画の最初のほーに「6月末時点、ステップ駆動版を、某所へ公開」というくだりがあります。
Maker Faire Tokyo 2019にはM5BALA(2輪車部)などを製造・販売しているM5Stack社もスポンサー出展するのですが、某所とは、そのブースへ作品展示権を得るためのコンテストです。
応募〆切が6/30だったので必死でひとまずのステップ駆動版を作成しました。
結果がどーなるか、しばし待とうと思います。(M5BALAの能力を引き出せている、今回の滑らか駆動・加減速版動画のほーをみて欲しいなぁ)
というわけで、本題へと戻ります。
ハードウェアの改善
現時点のハードウェア構成は以下のようになっています。
前回の状態はこちらをご参照ください。
改善点を以下に示します。
THETA固定
本来M5BALAの主人であるべきM5Stack FIREに付属する「M5GO CHG BASE」という充電用ドックを改造したものです。
改造といっても、中の基盤を取り出し、ケース天面中央に穴あけをし、エクステンションアダプター TE-1をネジ止めしただけです。
なぜ、補助輪寄りではなく、中央に穴あけしたのかですって？
それは、Part3以降で補助輪なしの倒立振子制御をしたいためです！
あと、次の章の対策により前後どちらにも転倒の心配が減ったというのもあります。
後方転倒防止バンパー
針金ハンガーを切断してペンチで曲げただけの代物です。
姿勢変化はこんなかんじ。
THETAの映像への写り込みに注意して足を広げないようにしました（黒線認識エリアより内側に納めました）。
これが、効果絶大！動画後半で机の段差を最高速で乗り越えても大丈夫です。
（とはいえ、過信は禁物ですよ）
軽くボディへテープ貼り付けしてあると後方へ倒れこみすぎず、バランスウェイトの具合によってはウィリー走行中にラッキーな衝撃があると姿勢が元に戻れます。
バランスウェイト
MFT2019 M5Stackブース展示権獲得コンテスト用には単三電池３本としていました。
前方への押さえつけの具合は良いものの、高速で動けるようになると、僅かな段差を拾ってウィリーしてしまったり、前方転倒してしまったりも発生したため1本にしました。
THETA固定具とM5BALAには磁石がついており、電池もよい力加減で固定してくれるので貼り付けなどはしていません。磁力固定です。
USBケーブル
短いものにかえました。（見ればわかるけど、念のために記しておきます）
目標としたこと
画像認識の目標
- 車体周囲のできるだけ狭い円形範囲で黒線を探す
- 見つからない場合、徐々に探索範囲を遠方にする
- それでもみつからない時は停止しておく
- 破線、汚れ、下地の模様に対してできるだけロバストにする
- 十字路は前方優先とする
制御の目標
- 前進と回転の組み合わせというシンプル動作とする
- 前進の範疇はトレッド幅の半径のコーナーを回れること
- 直進（≠前進より幅が狭い）で加減速すること
- 制御サイクルを10～15fps程度は出す
画像処理フロー
「はじめに」の章に掲載した全体図の画像処理に関する処理を順番に説明します。
- Equirectangularのおさらい
- 二値化
- オープニング（収縮→膨張処理）による抽象化
- 黒線の位置認識（独自処理）
Equirectangularのおさらい
THETA本体周囲と、得られるEquirectangular形式の画像の関係は以下のようになっています。
今回、CameraAPI + OpenCVを使って 1フレームづつ処理する際の映像もこの形式を指定しています。
THETA Vとしては 1920×960 30fpsまでの指定が行えますが、ライントレーサーにそれほど高解像度な情報は不要ですので「画像サイズを 横×縦 640×320 pixel 」「フレームレートを 15fps」と指定しています。
それでも映像の角度分解能は 360°/640pixel = 0.5625[°/pixel]あります。
二値化
明るい下地にある黒い線をトレースできればよいので、処理を軽くするため二値化をしています。
THETAの中でOpenCVを動かす【プレビューフレーム取得編】を参考としてください。
上記記事では、二値化する際の輝度閾値を輝度がとりうる範囲の真ん中（127/255）としています。
Imgproc.threshold(gray, mOutputFrame, 127.0, 255.0, Imgproc.THRESH_BINARY);
今回の輝度閾値はちょっと暗めの値、輝度がとりうる範囲の1/4程度(64/255)としています。
Imgproc.threshold(gray, mOutputFrame, 64.0, 255.0, Imgproc.THRESH_BINARY);
暗めにすると黒線の下地が白でなくても黒線認識がしやすくなります。
しかし、暗くしすぎてもちょっとしたライティングの変化で黒線が途切れて認識されたりもしますので映像を見ながら調節したほうが良いと思います。
オープニング（収縮→膨張処理）による抽象化
私はあまり詳しくないので、THETAの中でOpenCVを動かす【プレビューフレーム応用編】のリンクから参考資料をさぐっていくと良いと思います。
黒線の中に細かな白い点のノイズがあることを嫌って、モルフォロジーの中でもオープニングの処理を採用しました。
ついでに、できるだけ極端なノイズ除去をすることでシンプルな映像にすることも狙っています。このため章のタイトルを「抽象化」としています。
ソースコードからの処理の呼び出し方は以下のようになります。
// Morphology (Opening)
Imgproc.morphologyEx(mOutputFrame.clone(), mOutputFrame, Imgproc.MORPH_OPEN, mStructuringElement);
この中の「mStructuringElement」というオブジェクトが「構成要素」と呼ばれている入力値です。今回は9×9を指定しました。
mStructuringElement = Imgproc.getStructuringElement(Imgproc.CV_SHAPE_ELLIPSE, new Size(9,9)); //15fps OK
この値を「なし」→「3×3」→「9×9」→「15×15」と変化させたときの映像の違いを以下に掲載しておきます。
「抽象化」の目的からすると気持ちとしては 15×15を指定したかったのですが、フレームレートが6.7fps程度までしか出ませんでした。9×9まで構成要素を小さくしたところで15fpsが出せたので 9×9を指定しています。
この処理が今回の処理でもっとも処理時間を要しています。
黒線の位置認識（独自処理）
前進優先で探索を早めに打ち切る簡易処理（意図的手抜き）処理です。
この段階で処理する画像は 白=255, 黒=0の二値になっています。
図中矢印の方向へ輝度値をチェックして「白→黒と変化した位置」「黒→白と変化した位置」の中央を、「黒線の中央」とみなしています。
回転方向の探索の場合、右側左側とどちらも中央寄りから探索して両側に黒線が見つかった場合には中央に近いほうを採用しています。
黒線幅のチェックを基本してません。これは、十字路や鋭角コーナーなど、線が交わる箇所では線が太くなることと、一本線であっても断面を斜めにとると幅が太いと認識するためです。
以下のような目の粗い破線のコーナー（車体の動きが滑らかではない段階のアニメGIFです）で、稀に認識を誤ることがあったため局部的に最小値チェックをしている箇所はあります。対症療法的コードですので処理を見直したほうが良いと思います。
このような簡易処理でも、事例用の黒線幅(26mm)を なんの問題もなく追えていますし、机と机を接して並べた時の繋ぎ目など、わりと細い線も追えて(いいのか？)しまいます。
(1)から(3)の、より詳細な処理は説明を省きます。説明を省く処理の中でも（意図的に）手を抜いたために僅かな弊害を起こしている点を記載しておきます。
もし似たようなことをする方がいらっしゃった場合は、判定処理を改善して使用してください。
簡略化した処理とその弊害
■前進判定閾値をちょっと広げる必要アリ
動画のコースの2箇所で、“稀に”前進判定でなく回転判定になってしまう箇所があります。前進判定する幅を少し広げると解決します。
■(1)の特殊ケースで、十字路と鋭角コーナーに対応していない
「15fpsで通り過ぎるときの1フレーム」などレアなケースなのですが･･･
前進認識判定をする境界で（左端=白、右端=黒）（左端=黒、右端=白）のとき、中央優先としていない判定処理となっていました。十字路などは減速しなくてもよいのに減速判定になるなどの症状がでています。
■後ろの繋ぎ目をまたぐ黒線があった場合、処理を省略している
Equirectangularの 右端→左端 や 左端→右端 を跨ぐ黒線があっても端で処理を打ち切っています。回転処理をするときの微妙な回転不足になりますが、わずかなので問題としてはあまり見えません。気になる方は修正してください。
制御量の算出
「はじめに」の章に掲載した全体図の制御処理に関する処理を順番に説明します。全体図では省略していた処理についても説明します。
操舵（PID制御）
制御工学において古典制御と呼ばれる基礎です。多様な物事で今なお用いられている王道のような手法ともいえます。
制御量 = Kp×ズレ量 + Ki×ズレ量の積分 + Kd×ズレ量の微分
ズレ量の単位は[pixel]です。積分と微分で扱う時間の単位は[秒]です。
pixel∝角度であり、1pixel=0.5625度 です。[度]と[秒]の単位系でPID制御をする他のライントレーサー事例と比べKpの値は1.78倍程度大きな数値となります。
Kpの項は、ズレ量に比例しただけ制御量を加えますという主成分です。
Kiは小さな値です。この項は、継続する小さなズレ（オフセット）を取り除くよう働きます。
Kdも小さな値です。この項は、ズレに大きな変化があったときにズレを早めに打ち消すよう働きます。
Kp、Ki、Kdの各数値を決定するところがPID制御のカナメです。
決定方法については、後半の「パラメータ調整」の章で説明します。
1つの演算結果を、どのように左右のモーターへ振り分けたかは下図のとおり。
等速前提です。基準モーター出力はゼロからいきなり出力を加えても姿勢を崩さない程度の数値です（えいやで80としました）。
簡略化した処理とその弊害
前進を前提としています。バックや左右逆回転までは考慮していない設計です。
M5BALAに搭載されているギヤードモーター（ギヤ付きDCモーター）は、例えば出力値を20と指示しても、無負荷状態ですら回りません。このような下限チェックも手抜きしています。
このため、トレッド幅のコーナーを回れるものの、コーナーリング中に内側のモーターが回ってないことありすぎます。
THETA Vなど諸々搭載した状態で回転できる最小出力値で下限チェックを入れたほうが好ましい動作になると思われます。
加減速（台形制御）
上の図の基準モーター出力を下限として数値を上下させてるだけです。また、この段階でも左右差は考慮していません。
理想は台形のグラフですが、実際はデジタル信号処理（離散化した処理）ですので直線ではなく階段状になります。
加減速の勾配
今回、加速勾配と減速勾配を
加速勾配：ウィリーしない程度緩め
（1サンプリング周期あたり12加算）
減速勾配：最高速から減速しても前に倒れたりしない範囲でキツめ
（1サンプリング周期あたり32.5減算）
としました。
このあたりは目的に応じてお好みで決めてください。
加速の上限値は、次章で説明する「左右差」を考慮した値としてあります。
基準モーター出力に左右差の補正係数を考慮した値（の大きいほう）が、モーター出力上限値の255を越えないように値を決定してください。超えてしまうと直進を維持できなくなるためです。直進を維持するため幾らかのPID制御量も加わることを考慮してマージンを持っておくこともお忘れなく。
加減速の条件
今回の加速条件は複数設定してありますが、大切なのは以下２点です。
(1) PID制御の微分成分が少ないこと
(2) 黒線探索範囲よりちょっと遠方のチェック
その他の条件は、「加減速モードがON」「停止処理中ではない」「前進処理の1回目ではない」「黒線探索で(3)の遠方検索した結果ではない」と細かなものです。
加速条件を満たさないときは減速処理を行うようにしてあります。
少々話しが逸れますが、「加減速モード」のON/OFFを設けているのはPID制御パラメータや左右差の補正係数を決定するときのためです。
加速条件の(1)については、わかり易いと思いますので説明を割愛します。
加速条件の(2)については、「画像処理」の章の「(1)中央付近 進行方向探索」の処理を黒線認識幅よりちょっと遠方位置に対して行って、その結果、進行方向探索より幅の狭い範囲に黒線がいるときは加速するようにしています（ここも「意図的手抜き」をしています）。
この条件は、加速のためというより「最高速度でカーブに突入しても適切に減速させるため」の条件です。
簡略化した処理とその弊害
「(1)中央付近 進行方向探索」の処理は、前方とする範囲の左端から探索をはじめ、白→黒→白となるエッジを１セットみつけてしまうと処理を中断するような簡略処理としています。
この処理を加速判定に利用しため、車体の左側に机の谷（外側）が近くなる直線を走行させると「直線が前にあるにも関わらず加速しない」という現象が起こります（机の谷を検出した段階で処理を打ち切って、本来の黒線を探していない）。
動画の以下シーンは、机のフチ付近で加速を続けられる直線の限界くらいです。
安全といえば安全かもしれませんが当初意図してなかった動作でした。このようなことが問題になる場合には、処理を修正してください。
（あとで黒線認識処理も含めて見直そうかな･･･）
左右差の緩和
前回記事の動画でも伝えているとおり、左右に同量のモーター出力を指示しても右曲がり傾向があります。実は、もう一台別のM5BALAを動かしているのですが、同じ設定でもっと右に曲がります。
これをできるだけ緩和しようという策です。
基本的には、
左輪＝基準モーター出力
右輪＝基準モーター出力×補正係数
という出力の仕方で、できるだけ真っ直ぐ走れる補正係数を探ります。
「パラメータ調整」の章で調整用のwebUIの画面を説明しますが、そこで、Kp, Ki, Kdを全てゼロ、加速制御なしとすると、この動作をさせることができます。
補正係数を変えては動作確認する作業を繰り返して適切な値を探るわけですが、モーター出力は整数ですので、あらかじめ試したい補正係数をExcelなどで計算しておくと無駄な数値を試すことなく作業をおこなうことできます。
このようにして決定した補正係数はモーター出力を決定する一番最後に演算しています（PID制御量などにも、まるっと左右差が考慮される設計としています）。
回転
回転動作
黒線の位置認識をした結果、回転動作をさせるときがあります。
このとき、左右のモーターには「絶対値は同じ」「符号違い」の出力を以下の時間だけ駆動させています。左右差は考慮していません。
回転駆動時間 ＝ 180°回転する時間 ×（中心からのズレ量[pixel]／320[pixel]）
今回、回転させるときのモーター出力絶対値は128としています。このとき180度回転する時間は745msとしています。
個体によって異なる値ですので実験して確かめてください。
どちらも「パラメータ調整」の章で説明する調整用のwebUIから変更することができます。
なお、停止した状態から、モーターに指示を出し、実際に回転動作を始めて等速に至るまでには、いくらか時間を要します。
どの角度でも比例の関係が成り立っているわけではありませんが、１回の回転動作後、次の制御をおこなうときには前進判定になればよいので厳密な時間を計る必要はありません。
回転動作前後の動作安定待ち時間
回転動作の前後では、停止してから車体の姿勢が安定するまでの待ち時間を設けています。
この待ち時間は、安全めに見積もって220msとしていますが、もっと短くすることも可能です。
その際の限界値は、 Camera APIでステッチされたフレームを取得したときのレイテンシよりは長くしなければなりません。フレームレートとの関連もあります。
概ね 15fpsとしたときの 2フレーム分の時間 = 1000/15 × 2 ≒ 133.3 msとしてもレイテンシは感じられず、1フレーム = 1000/15 ≒66.6 ms とするとレイテンシの影響で回転不足が生じました(2回の回転動作をするようになります)。
だいたいの勘所として「100msよりは長く 130msよりは短いレイテンシ」だと思われます。ご参考までに掲載しておきます。
シリアル通信まわりの改善点
前回と異なりコマンドを極端に減らしました。
実質使っているのはNo3とNo4の2つだけです。
| No |
コマンド |
説明 |
|---|---|---|
| 1 | go | テスト用コマンド 両輪へ+80のモーター出力をし、1000ms経過後、モーターを停止させ、acceptを返します |
| 2 | set 左モーター出力 右モーター出力 | 前回記事と同じです。動作テスト用に残しています。 |
| 3 | mov_s 左モーター出力 右モーター出力 時間[ms] | 指定したモーター出力をし、指定した時間[ms]経過後、モーターを停止させ、acceptを返します |
| 4 | mov_w 左モーター出力 右モーター出力 時間[ms] | 指定したモーター出力をし、指定した時間[ms]経過後、acceptを返します |
| 5 | その他の入力 | 空白以外の入力で1～4に該当しないものは全てモーターを停止させ、acceptを返します |
コマンドの数を減らしただけでなく、振る舞いも変わっています。
変更点は以下のとおりです。
- ProMicro側はコマンド実行後に必ずacceptを返すようにしました。THETA側はコマンド送信後accept受信を待つようにしています。前回のようなTHETA側でコマンド送信したあとwaitをするだけの2者調停なし状態は、高頻度な通信を不安定にさせるためです。
- モーターの連続駆動に対応しました。mov_wコマンドが該当します。これにより滑らかな動作ができています。
- 上記の連続駆動に対応したので、安全装置としてProMicro側にタイムアウトを設けました。1000msの期間THETA側からなにもコマンドを受信しない場合強制的にモーターを停止します。
パラメータ調整
M5BALAの個体差は大きく、私と同じパラメータで動作させても、同じような滑らかなライントレースは行われないと思われます。（直線でもうねうねと曲がったり、途中止まったりしながらのライントレースはすると思いますが･･･）
そこで、webUIを設け、個体差を吸収するために必要な最小限の項目を変更できるようにしました。PID制御のパラメータ決定作業については10倍くらい作業効率上がります。
余談となりますが、「左右差の補正係数」は 1.1625、「Kd」は0.195としているのですが、妙に多い小数点下位桁まで表示されています。
これは、HTMLの数値入力フィールドのデータ型がfloat、内部処理ではdoubleで数値を扱っていることの差異が見えてしまっているためです。
ご容赦くださいませ（汗
画像処理の前進回転判定の境界線や加減速の勾配や上限値などなど、もう少し弄れるよーにしたほうが良い項目もあると思います。ご自身で拡張して下さい。
以下、パラメータ調整の作業順に詳細を記します。
回転動作のパラメータ決定
すでに紹介済みなので割愛します。
左右差の補正係数決定
すでに紹介済みなので割愛します。
限界感度法によるPID制御係数（暫定値）決定
限界感度法は、PID制御の3つのパラメータを決める"知見"の一つです。
ライントレーサーでは以下の手順を行います。
- P制御だけ（Ki,Kdを0として）で直線で振動動作をするKuを探り、振動周期Pu[秒]を測定しておく。
- Kp=0.6Ku, Ki=0.5Pu, Kd=0.125*Puの式にあてはめる
真面目に「限界感度法」を調べると、ライントレーサーにおける「ステップ応答」とはなんぞや？という点で悩みがでるのかと思います。
今回は、50cm程度の直線コースを作り、わざと端にきたときの回転不足を生じるようにしておき、繰り返し往復動作をさせて振動したままになるKpを探る手順としました。
Kpが小さければ振幅が収束していきますし、Kpが多すぎれば度々回転動作をするようになります。
Kp=3.0, Ki=0.0, Kd=0.0の例はこんな感じです。
このときの振動周期 Pu はおおよそ 1.1秒程度でしたので
Kp = 0.6 × 3.0 = 1.8
Ki = 0.5 × 1.1 = 0.55
Kd = 0.125 × 1.1 = 0.1375
と Kp, Ki, Kd を仮決めすることができました。
時間の測定はビデオ撮影をするのが一番楽で確実だと思います。30fps程度の動画の時間分解能で問題ありません。
最終調整
限界感度法で決めた数値は、固定の目標値に対して外乱があると"できる限り短い時間で目標値に戻る"ような設定です。"できる限り短い時間で目標値に戻る"ときにはオーバーシュートも厭わないという設定のため、ライントレーサーに適用すると、直線を維持するには良い設定なのですが、コーナーを曲がる際には車体の挙動が怪しくなります。
ライントレーサー向きの数値と比較すると、 Kp少なめ、Kiが多めになっていますので、コーナーのあるコースを走らせながら程よい値に調節していくのが最終調整となります。
限界感度法や最終調整については、以下のリンクの記事や動画がわかり易いと思います。
https://monoist.atmarkit.co.jp/mn/articles/1007/26/news083.html
私の車体では
| 項目名称 | 最終調整結果 |
|---|---|
| Kp | 2.25 |
| Ki | 0.25 |
| Kd | 0.195 |
としました。
もう少し追い込めたかもしれせんが、だいたいのコースにそこそこ適用できるあたりで調整を打ち切っています。
ソースコード
THETAプラグイン側
ベースとしたのは、THETAの中でOpenCVを動かす【プレビューフレーム取得編】の記事のプロジェクトファイル一式です。
USBシリアル通信やwebUIを使うための諸設定については、前回記事の「THETAプラグイン」の章から各リンクを辿ってください。
全体構成
ファイル構成はベースと変えていません。
MainActivity.javaの以下メソッドの構成も変えていません。
追加はしていますが以降の章にてメソッドと処理の対応を説明します。
| メソッド名称 |
概要 |
|---|---|
| onManagerConnected() | 手を加えていません |
| onCreate() | 末尾でwebUIのサーバーを起動させています |
| onKeyDown() | ライントレース開始／停止、加速のあり／なしの操作を追加 |
| onKeyUp() | 手を加えていません |
| onKeyLongPress() | 手を加えていません |
| onResume() | 幾らかのプラグイン開始処理を追加しています |
| onPause() | 幾らかのプラグイン終了処理を追加しています |
| onCameraViewStarted() | モルフォロジーの指定を変えています |
| onCameraViewStopped() | 手を加えていません |
| onCameraFrame() | 画像処理フローの章で説明した内容のメイン処理です。 黒線位置を求めたあと制御のメイン処理も呼び出しています。 |
| closeCamera() | 手を加えていません |
ThetaView.javaは「画像サイズ」と「フレームレート」を指定する所だけ変更しました。以下の通りです。
「FPS」と名前をつけた固定値は、PID制御でサンプリングレートを扱う部分からも参照しています。
ThetaView.java
public class ThetaView extends CameraBridgeViewBase implements PreviewCallback {
～省略～
private static final int PREVIEW_SIZE_WIDTH = 640;
private static final int PREVIEW_SIZE_HEIGHT = 320;
private static final String PREVIEW_SIZE_PARAM = "RicMoviePreview640";
public static final int FPS = 15;
～省略～
protected boolean initializeCamera(int width, int height) {
～省略～
params.setPreviewFpsRange(FPS * 1000, FPS * 1000);
～省略～
}
}
画像処理フローに該当するメソッド
先に説明した「onCameraFrame()」が「画像処理フロー」のメイン処理です。
そこから呼ばれる部品となる処理は以下となります。
| メソッド名称 |
概要 |
|---|---|
| drawColorResult() | カラー画像にデバッグ情報を描画する処理 |
| setPwmColor() | カラー画像に左右のモータ出力を描画する処理 |
| drawBWResult() | 二値画像にデバッグ情報を描画する処理 |
| searchForwardArea() | 「(1) 中央付近 進行方向探索」や 「(3)-1:進行方向探索を1pixelづつ遠方へ」の処理です |
| searchRotationalDirection() | 「(2)中央付近以外 回転方向探索」や 「(3)-2:回転方向探索を1pixelづつ遠方へ」の処理です |
| searcLeftDownUpEdge() | 画像の左方向へ 白→黒→白 と変化する位置を探し、 黒線中心値を返します |
| searcRightDownUpEdge() | 画像の右方向へ 白→黒→白 と変化する位置を探し、 黒線中心値を返します |
| sarchLeftUpEdge() | 画像の左方向へ 黒→白 と変化する位置を探し、 エッジの位置を返します |
| sarchRightUpEdge() | 画像の右方向へ 黒→白 と変化する位置を探し、 エッジの位置を返します |
制御量の算出に該当するメソッド
「制御量の算出」の章で説明した事項に対応するメソッドは以下のとおりです。
| メソッド名称 |
概要 |
|---|---|
| resetControlParam() | PID制御パラメータを初期化する処理です |
| controlBala() | 制御に関する処理のメインです |
| controlForward() | 前進に関する処理がまとまっています |
| controlRotation() | 回転に関する処理がまとまっています |
| controlWait() | モータ出力をゼロ指定したときのmov_wコマンドの振る舞いを利用し、 1ms単位の通信waitをするための処理です |
シリアル通信関連のメソッド
シリアル通信関連のメソッドは以下のとおりです。
| メソッド名称 |
概要 |
|---|---|
| usbSerialOpen() | USBシリアル通信の開始処理です |
| usbSerialColse() | USBシリアル通信の終了処理です |
| start_read_thread() | シリアル通信スレッド開始用です |
| run() | シリアル通信スレッドのメイン処理です |
| readWait() | 「accept」を受信するまで待つ処理です |
パラメータ調整関連のメソッド
パラメータ調整関連のメソッドは以下のとおりです。
| メソッド名称 |
概要 |
|---|---|
| restoreControlParam() | sharedPreferencesを利用して保存したパラメータを復帰します。 onResume()から呼ばれます。 |
| saveControlParam() | sharedPreferencesを利用してパラメータを保存します。 webUIの「SAVE」ボタンを操作したときに呼ばれます。 |
あとは、以下コメントからファイル末尾までが web UI の処理です。
MainActivity.java
//=====================================================
//<<< web server processings >>>
//=====================================================
MainActivity.java全文
以下に「MainActivity.java」を全文を畳んで掲載しておきます。
「MainActivity.java」全文
MainActivity.java
/**
* Copyright 2018 Ricoh Company, Ltd.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package com.theta360.opencvpreview;
import android.content.Context;
import android.content.Intent;
import android.hardware.usb.UsbDeviceConnection;
import android.os.Bundle;
import android.util.Log;
import android.view.KeyEvent;
import android.view.SurfaceView;
import com.theta360.pluginlibrary.activity.PluginActivity;
import com.theta360.pluginlibrary.callback.KeyCallback;
import com.theta360.pluginlibrary.receiver.KeyReceiver;
import org.opencv.android.BaseLoaderCallback;
import org.opencv.android.CameraBridgeViewBase.CvCameraViewListener2;
import org.opencv.android.CameraBridgeViewBase.CvCameraViewFrame;
import org.opencv.android.LoaderCallbackInterface;
import org.opencv.android.OpenCVLoader;
import org.opencv.core.Core;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.opencv.core.Scalar;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
import org.opencv.android.Utils;
//シリアル通信まわりで使用
import android.app.PendingIntent;
import android.hardware.usb.UsbManager;
import com.hoho.android.usbserial.driver.UsbSerialDriver;
import com.hoho.android.usbserial.driver.UsbSerialPort;
import com.hoho.android.usbserial.driver.UsbSerialProber;
import com.theta360.pluginlibrary.values.LedColor;
import com.theta360.pluginlibrary.values.LedTarget;
import java.io.IOException;
import java.util.List;
import android.content.SharedPreferences;
import android.preference.PreferenceManager;
import java.util.HashMap;
import java.util.Map;
import fi.iki.elonen.NanoHTTPD;
import fi.iki.elonen.NanoHTTPD.Response.Status;
public class MainActivity extends PluginActivity implements CvCameraViewListener2 {
private static final String TAG = "Plug-in::MainActivity";
//シリアル通信関連
private boolean mFinished;
private UsbSerialPort port ;
//USBデバイスへのパーミッション付与関連
PendingIntent mPermissionIntent;
private static final String ACTION_USB_PERMISSION = "com.android.example.USB_PERMISSION";
private ThetaView mOpenCvCameraView;
private boolean isEnded = false;
private Mat mOutputFrame;
private Mat mStructuringElement;
private BaseLoaderCallback mLoaderCallback = new BaseLoaderCallback(this) {
@Override
public void onManagerConnected(int status) {
switch (status) {
case LoaderCallbackInterface.SUCCESS:
mOpenCvCameraView.enableView();
break;
default:
super.onManagerConnected(status);
break;
}
}
};
@Override
protected void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
setContentView(R.layout.activity_main);
Log.d(TAG, "OpenCV version: " + Core.VERSION);
// Set a callback when a button operation event is acquired.
setKeyCallback(new KeyCallback() {
@Override
public void onKeyDown(int keyCode, KeyEvent keyEvent) {
if (keyCode == KeyReceiver.KEYCODE_CAMERA) {
if (linetraceEnable) {
linetraceEnable = false;
notificationLedHide(LedTarget.LED3);
notificationLedHide(LedTarget.LED7); //REC
} else {
//制御変数リセット
resetControlParam();
//操作から3秒後に動かし始める
controlWait(3000);
linetraceEnable = true;
notificationLedBlink(LedTarget.LED3, LedColor.CYAN, 1000);
}
} else if (keyCode == KeyReceiver.KEYCODE_MEDIA_RECORD) {
if (accelerationEnable) {
accelerationEnable = false;
notificationLedHide(LedTarget.LED6); //Live
} else {
accelerationEnable = true;
notificationLedShow(LedTarget.LED6); //Live
}
}
}
@Override
public void onKeyUp(int keyCode, KeyEvent keyEvent) {
}
@Override
public void onKeyLongPress(int keyCode, KeyEvent keyEvent) {
if (keyCode == KeyReceiver.KEYCODE_MEDIA_RECORD) {
Log.d(TAG, "Do end process.");
closeCamera();
}
}
});
//webUI用のサーバー開始処理
this.context = getApplicationContext();
this.webServer = new WebServer(this.context);
try {
this.webServer.start();
} catch (IOException e) {
e.printStackTrace();
Log.d(TAG,"web server error : [" + e + "]" );
}
notificationCameraClose();
setContentView(R.layout.activity_main);
mOpenCvCameraView = (ThetaView) findViewById(R.id.opencv_surface_view);
mOpenCvCameraView.setVisibility(SurfaceView.VISIBLE);
mOpenCvCameraView.setCvCameraViewListener(this);
//Camera
}
@Override
protected void onResume() {
super.onResume();
if (!OpenCVLoader.initDebug()) {
Log.d(TAG, "Internal OpenCV library not found.");
} else {
Log.d(TAG, "OpenCV library found inside package.");
mLoaderCallback.onManagerConnected(LoaderCallbackInterface.SUCCESS);
}
//--------------- added code ---------------
notificationLedHide(LedTarget.LED3);
notificationLedHide(LedTarget.LED4); //Still
notificationLedHide(LedTarget.LED5); //Move
notificationLedHide(LedTarget.LED6); //Live
notificationLedHide(LedTarget.LED7); //REC
notificationLedHide(LedTarget.LED8); //Card Error
restoreControlParam();
mFinished = true;
usbSerialOpen();
//-----------------------------------------
}
@Override
protected void onPause() {
closeCamera();
//--------------- added code ---------------
//スレッドを終わらせる指示。終了待ちしていません。
mFinished = true;
//シリアル通信の後片付け ポート開けてない場合にはCloseしないこと
usbSerialColse();
//-----------------------------------------
super.onPause();
}
public void onCameraViewStarted(int width, int height) {
mOutputFrame = new Mat(height, width, CvType.CV_8UC1);
mStructuringElement = Imgproc.getStructuringElement(Imgproc.CV_SHAPE_ELLIPSE, new Size(9,9)); //15fps OK
}
public void onCameraViewStopped() {
mOutputFrame.release();
mStructuringElement.release();
}
//=======================================================================================
void usbSerialOpen() {
// Find all available drivers from attached devices.
UsbManager manager = (UsbManager) getSystemService(Context.USB_SERVICE);
List<UsbSerialDriver> usb = UsbSerialProber.getDefaultProber().findAllDrivers(manager);
//定義済みのArduinoを利用するため、新たな定義を追加する必要がない
//final ProbeTable probeTable = UsbSerialProber.getDefaultProbeTable();
//probeTable.addProduct(0x2341,0x8036,CdcAcmSerialDriver.class);
//List<UsbSerialDriver> usb = new UsbSerialProber(probeTable).findAllDrivers(manager);
if (usb.isEmpty()) {
int usb_num = usb.size();
Log.d(TAG,"usb num =" + usb_num );
Log.d(TAG,"usb device is not connect." );
//return;
//port = null;
} else {
// デバッグのため認識したデバイス数をしらべておく
int usb_num = usb.size();
Log.d(TAG,"usb num =" + usb_num );
// Open a connection to the first available driver.
UsbSerialDriver driver = usb.get(0);
//USBデバイスへのパーミッション付与用（機器を刺したときスルーしてもアプリ起動時にチャンスを与えるだけ。なくても良い。）
mPermissionIntent = PendingIntent.getBroadcast(this, 0, new Intent(ACTION_USB_PERMISSION), 0);
manager.requestPermission( driver.getDevice() , mPermissionIntent);
UsbDeviceConnection connection = manager.openDevice(driver.getDevice());
if (connection == null) {
// You probably need to call UsbManager.requestPermission(driver.getDevice(), ..)
// パーミッションを与えた後でも、USB機器が接続されたままの電源Off->On だとnullになる... 刺しなおせばOK
Log.d(TAG,"M:Can't open usb device.\n");
port = null;
} else {
port = driver.getPorts().get(0);
try {
port.open(connection);
port.setParameters(115200, 8, UsbSerialPort.STOPBITS_1, UsbSerialPort.PARITY_NONE);
port.setDTR(true); // for arduino(ATmega32U4)
port.setRTS(true); // for arduino(ATmega32U4)
port.purgeHwBuffers(true,true);//念のため
mFinished = false;
start_read_thread();
} catch (IOException e) {
// Deal with error.
e.printStackTrace();
Log.d(TAG, "M:IOException");
//return;
}
}
}
}
void usbSerialColse() {
if (port != null) {
try {
port.close();
Log.d(TAG, "M:onDestroy() port.close()");
} catch (IOException e) {
Log.d(TAG, "M:onDestroy() IOException");
}
} else {
Log.d(TAG, "M:port=null\n");
}
}
//=======================================================================================
//=======================================================================================
// <<< 黒線認識処理用 固定値群 >>>
// 基礎固定値
final static int NOT_FOUND = -1;
final static double BIN_WHITE = 255.0;
final static double BIN_BLACK = 0.0;
//高さ(上下)方向
final static int PITCH_SEARCH_HIGHT = 40 ;
final static int PITCH_SEARCH_START = 280 ;
final static int PITCH_SEARCH_END = PITCH_SEARCH_START - PITCH_SEARCH_HIGHT ;
//横(左右)方向
final static int FRONT_CENTER = 320 ;
final static int FRONT_LIMIT_HALF_WIDTH = 50 ;
final static int FRONT_LIMIT_LEFT = (FRONT_CENTER - FRONT_LIMIT_HALF_WIDTH) ;
final static int FRONT_LIMIT_RIGHT = (FRONT_CENTER + FRONT_LIMIT_HALF_WIDTH) ;
//黒線ノイズ除去用
final static int LINE_WIDTH_LIMIT = 8;
//=======================================================================================
public Mat onCameraFrame(CvCameraViewFrame inputFrame) {
// get a frame by rgba() or gray()onCameraFrame
Mat gray = inputFrame.gray();
Mat rgba = inputFrame.rgba();
// do some image processing
Imgproc.threshold(gray, mOutputFrame, 64.0, 255.0, Imgproc.THRESH_BINARY);
// Morphology (Opening)
Imgproc.morphologyEx(mOutputFrame.clone(), mOutputFrame, Imgproc.MORPH_OPEN, mStructuringElement);
//--------------------------
// 黒線認識処理
//--------------------------
//全般メモ：急な折り返しとか交差は太くなるケースあるので、今回は黒線幅をあまり気にしない。
int forwardPos = NOT_FOUND;
int rotationPos = NOT_FOUND ;
int detectHight = PITCH_SEARCH_START ;
forwardPos = searchForwardArea(PITCH_SEARCH_START, mOutputFrame); // (1) 中央付近 進行方向探索
if ( forwardPos == NOT_FOUND ) {
rotationPos = searchRotationalDirection(PITCH_SEARCH_START, mOutputFrame); //(2)中央付近以外 回転方向探索
}
//(3)離れている黒線を探索（進行方向探索を遠方へ→だめなら回転方向探索を遠方へ）
if ( (forwardPos == NOT_FOUND) && (rotationPos==NOT_FOUND) ) {
//(3)-1 : 進行方向探索を遠方へ
for (int i=PITCH_SEARCH_START-1; i>PITCH_SEARCH_END; i--) {
forwardPos = searchForwardArea(i, mOutputFrame);
if ( forwardPos >=0 ) {
detectHight = i;
break;
}
}
if ( forwardPos == NOT_FOUND ) {
//(3)-2 : 回転方向探索を遠方へ
for (int i=PITCH_SEARCH_START-1; i>PITCH_SEARCH_END; i-- ) {
rotationPos = searchRotationalDirection(i, mOutputFrame);
if ( rotationPos >=0 ) {
detectHight = i;
break;
}
}
}
}
//加減速のための黒線探索をしておく
int accelerationPos = NOT_FOUND;
accelerationPos = searchForwardArea(PITCH_SEARCH_ACCELERATION, mOutputFrame);
//画像認識結果から制御指示（シリアル通信スレッドへ制御コマンド送信要求）
controlBala(forwardPos, rotationPos, detectHight, accelerationPos);
//2値画像へデバッグ情報描画
drawBWResult( forwardPos, rotationPos, detectHight );
//カラー画像へデバッグ情報描画
drawColorResult( rgba, forwardPos, rotationPos, detectHight);
//return mOutputFrame; //2値画像を描画する場合
return rgba; //カラー画像を描画する場合
}
private void closeCamera() {
if (isEnded) {
return;
}
if (mOpenCvCameraView != null) {
mOpenCvCameraView.disableView();
}
close();
isEnded = true;
}
//=======================================================================================
// デバッグ描画関連
//=======================================================================================
void drawColorResult(Mat rgba, int forwardPos, int rotationPos, int detectHight ){
//カラー補助線
Imgproc.line(rgba, new Point(FRONT_CENTER, PITCH_SEARCH_START), new Point(FRONT_CENTER, PITCH_SEARCH_END), new Scalar(255, 0, 0, 255), 2 );
Imgproc.line(rgba, new Point(FRONT_LIMIT_LEFT, PITCH_SEARCH_START), new Point(FRONT_LIMIT_LEFT, PITCH_SEARCH_END), new Scalar(0, 255, 0, 255), 2 );
Imgproc.line(rgba, new Point(FRONT_LIMIT_RIGHT, PITCH_SEARCH_START), new Point(FRONT_LIMIT_RIGHT, PITCH_SEARCH_END), new Scalar(0, 255, 0, 255), 2 );
Imgproc.line(rgba, new Point(FRONT_LIMIT_LEFT,PITCH_SEARCH_START), new Point(FRONT_LIMIT_RIGHT,PITCH_SEARCH_START), new Scalar(0 ,255, 0, 255), 2 );
Imgproc.line(rgba, new Point(FRONT_LIMIT_LEFT,PITCH_SEARCH_END), new Point(FRONT_LIMIT_RIGHT,PITCH_SEARCH_END), new Scalar(0, 255, 0, 255), 2 );
Imgproc.line(rgba, new Point(0, PITCH_SEARCH_START), new Point(FRONT_LIMIT_LEFT,PITCH_SEARCH_START), new Scalar(255 ,255, 0, 255), 2 );
Imgproc.line(rgba, new Point(0, PITCH_SEARCH_END), new Point(FRONT_LIMIT_LEFT,PITCH_SEARCH_END), new Scalar(255, 255, 0, 255), 2 );
Imgproc.line(rgba, new Point(FRONT_LIMIT_RIGHT, PITCH_SEARCH_START), new Point(639, PITCH_SEARCH_START), new Scalar(255 ,255, 0, 255), 2 );
Imgproc.line(rgba, new Point(FRONT_LIMIT_RIGHT, PITCH_SEARCH_END), new Point(639, PITCH_SEARCH_END), new Scalar(255, 255, 0, 255), 2 );
//draw Target Mark
Scalar targetMarkColor = new Scalar(0,0,0,255);
int targetX = NOT_FOUND;
if ( forwardPos != NOT_FOUND ) {
targetX = forwardPos;
targetMarkColor = new Scalar(0, 255, 0, 255);
}
if ( rotationPos != NOT_FOUND ) {
targetX = rotationPos;
targetMarkColor = new Scalar(255, 255, 0, 255);
}
if ( targetX != NOT_FOUND ) {
Log.d(TAG, "Ctrl targetX=" + String.valueOf(targetX) + ", detectHight=" + String.valueOf(detectHight) );
Imgproc.circle(rgba, new Point(targetX, detectHight), 10, targetMarkColor, 2);
Imgproc.circle(rgba, new Point(targetX, detectHight), 15, targetMarkColor, 2);
Imgproc.line(rgba, new Point(targetX, detectHight+5), new Point(targetX,detectHight+15), targetMarkColor, 2 );
Imgproc.line(rgba, new Point(targetX, detectHight-5), new Point(targetX,detectHight-15), targetMarkColor, 2 );
Imgproc.line(rgba, new Point(targetX+5, detectHight), new Point(targetX+15,detectHight), targetMarkColor, 2 );
Imgproc.line(rgba, new Point(targetX-5, detectHight), new Point(targetX-15,detectHight), targetMarkColor, 2 );
}
//draw motor L R value
Scalar rectColor = new Scalar(0,0,0,255);
Imgproc.rectangle( rgba, new Point(120, 60), new Point(530, 120), rectColor, -1);
Scalar motorLColor = setPwmColor(pwmMotor0);
Imgproc.putText( rgba, "L:"+ String.valueOf(pwmMotor0) +"/255", new Point(130, 100), 0, 1, motorLColor,2, 1, false );
Scalar motorRColor = setPwmColor(pwmMotor1);
Imgproc.putText( rgba, "R:"+ String.valueOf(pwmMotor1) + "/255", new Point(340, 100), 0, 1, motorRColor,2, 1, false );
}
Scalar setPwmColor(int pwmMotorVal) {
Scalar color = new Scalar(0,0,0,255);
int val = (int) Math.abs(pwmMotorVal);
if ( 0<= val && val < 60) {
color = new Scalar(0,0,255,255);
} else if ( 60<= val && val < 120) {
color = new Scalar(0,255,0,255);
} else if ( 120<= val && val < 180) {
color = new Scalar(255,255,0,255);
} else if ( 180<= val) {
color = new Scalar(255,0,0,255);
}
return color;
}
void drawBWResult( int forwardPos, int rotationPos, int detectHight ){
//補助線
Imgproc.line(mOutputFrame, new Point(0,PITCH_SEARCH_START), new Point(639,PITCH_SEARCH_START), new Scalar(0.0,0), 1 );
Imgproc.line(mOutputFrame, new Point(0,PITCH_SEARCH_END), new Point(639,PITCH_SEARCH_END), new Scalar(0.0,0), 1 );
Imgproc.line(mOutputFrame, new Point(FRONT_CENTER,PITCH_SEARCH_START), new Point(FRONT_CENTER,PITCH_SEARCH_END), new Scalar(0.0,0), 1 );
Imgproc.line(mOutputFrame, new Point(FRONT_LIMIT_LEFT,PITCH_SEARCH_START), new Point(FRONT_LIMIT_LEFT,PITCH_SEARCH_END), new Scalar(0.0,0), 1 );
Imgproc.line(mOutputFrame, new Point(FRONT_LIMIT_RIGHT,PITCH_SEARCH_START), new Point(FRONT_LIMIT_RIGHT,PITCH_SEARCH_END), new Scalar(0.0,0), 1 );
//draw Target Mark
//Scalar targetMarkColor = new Scalar(0.0); //黒
Scalar targetMarkColor = new Scalar(255.0); //白
int targetX = NOT_FOUND;
if ( forwardPos != NOT_FOUND ) {
targetX = forwardPos;
}
if ( rotationPos != NOT_FOUND ) {
targetX = rotationPos;
}
if ( targetX != NOT_FOUND ) {
Imgproc.circle(mOutputFrame, new Point(targetX, detectHight), 10, targetMarkColor, 2);
Imgproc.circle(mOutputFrame, new Point(targetX, detectHight), 15, targetMarkColor, 2);
Imgproc.line(mOutputFrame, new Point(targetX, detectHight+5), new Point(targetX,detectHight+15), targetMarkColor, 2 );
Imgproc.line(mOutputFrame, new Point(targetX, detectHight-5), new Point(targetX,detectHight-15), targetMarkColor, 2 );
Imgproc.line(mOutputFrame, new Point(targetX+5, detectHight), new Point(targetX+15,detectHight), targetMarkColor, 2 );
Imgproc.line(mOutputFrame, new Point(targetX-5, detectHight), new Point(targetX-15,detectHight), targetMarkColor, 2 );
}
}
//=======================================================================================
// 画像認識まわりの部品
//=======================================================================================
private int searchForwardArea( int searchHight, Mat inputFrame ) {
int result=NOT_FOUND;
int edgeStart=NOT_FOUND;
int edgeEnd=NOT_FOUND;
double leftEdge = mOutputFrame.get(searchHight, FRONT_LIMIT_LEFT)[0];
double rightEdge = mOutputFrame.get(searchHight, FRONT_LIMIT_RIGHT)[0];
if ( (leftEdge==BIN_WHITE) && (rightEdge==BIN_WHITE) ) {
// 左端=白、右端=白 → 進行方向エリアを普通に探す
Point centerEdge = searcRightDownUpEdge(FRONT_LIMIT_LEFT, FRONT_LIMIT_RIGHT, searchHight, mOutputFrame);
edgeStart = (int) centerEdge.x;
edgeEnd = (int) centerEdge.y;
} else if ( (leftEdge==BIN_BLACK) && (rightEdge==BIN_WHITE) ) {
// 左端=黒、右端=白 → 中央左端から 左右のUpエッジを探す 左は左端頭打ち。右は必ず解あり。
edgeStart = sarchLeftUpEdge(FRONT_LIMIT_LEFT, searchHight, mOutputFrame);
if (edgeStart==NOT_FOUND) {
edgeStart=0;
}
edgeEnd = sarchRightUpEdge(FRONT_LIMIT_LEFT, searchHight, mOutputFrame);
} else if ( (leftEdge==BIN_WHITE) && (rightEdge==BIN_BLACK ) ) {
// 左端=白、右端=黒 → 中央右端から 左右のUpエッジを探す 左は必ず解あり。右は右端頭打ち。
edgeStart = sarchLeftUpEdge(FRONT_LIMIT_RIGHT, searchHight, mOutputFrame);
edgeEnd = sarchRightUpEdge(FRONT_LIMIT_RIGHT, searchHight, mOutputFrame);
if (edgeEnd==NOT_FOUND) {
edgeEnd = 0;
}
} else if ( (leftEdge==BIN_BLACK) && (rightEdge==BIN_BLACK ) ) {
// 左端=黒、右端=黒 → 中央左端と中央右端それぞれからエッジを探し 中心が中央に近いほうを採用
// 中央左端からエッジ探索（両側とも解ありかは不明だが、それぞれ画像端で頭打ちさせる）
int edgeStartLeft = sarchLeftUpEdge(FRONT_LIMIT_LEFT, searchHight, mOutputFrame);
if (edgeStartLeft==NOT_FOUND) { edgeStartLeft=0; }
int edgeEndLeft=sarchRightUpEdge(FRONT_LIMIT_LEFT, searchHight, mOutputFrame);
if (edgeEndLeft==NOT_FOUND) { edgeEndLeft=(mOutputFrame.width()-1); }
//中央右端からエッジ探索
int edgeStartRight = sarchLeftUpEdge(FRONT_LIMIT_RIGHT, searchHight, mOutputFrame);
if (edgeStartRight==NOT_FOUND) { edgeStartRight=0; }
int edgeEndRight = sarchRightUpEdge(FRONT_LIMIT_RIGHT, searchHight, mOutputFrame);
if (edgeEndRight==NOT_FOUND) { edgeEndRight=(mOutputFrame.width()-1); }
int axisLeft = (edgeStartLeft+edgeEndLeft)/2;
int axisRight = (edgeStartRight+edgeEndRight)/2;
if ( Math.abs(FRONT_CENTER - axisLeft) > Math.abs(FRONT_CENTER - axisRight) ) {
edgeStart = edgeStartRight;
edgeEnd = edgeEndRight;
} else {
edgeStart = edgeStartLeft;
edgeEnd = edgeEndLeft;
}
}
if ( (edgeStart>=0) && (edgeEnd>=0) ) {
result = (edgeStart + edgeEnd)/2 ;
}
return result;
}
private int searchRotationalDirection( int searchHight, Mat inputFrame ){
int result=NOT_FOUND;
int edgeStart=NOT_FOUND;
int edgeEnd=NOT_FOUND;
//(2)-1 回転方向 右方探索
Point rightSearchEdge = searcRightDownUpEdge(FRONT_LIMIT_RIGHT, (mOutputFrame.width()-1), searchHight, mOutputFrame);
int edgeStartRight = (int) rightSearchEdge.x;
int edgeEndRight = (int) rightSearchEdge.y;
int axisRight = NOT_FOUND;
if ( (edgeStartRight >= 0) && (edgeEndRight >= 0) ) {
axisRight = (edgeStartRight + edgeEndRight)/2;
} else if ( (edgeStartRight >= 0) && (edgeEndRight == NOT_FOUND) ) {
//左面 左端から中央方向へ 探索を伸ばすのですが、仮で頭打ち
edgeEndRight=mOutputFrame.width()-1;
axisRight = (edgeStartRight + edgeEndRight)/2;
} else {
//みつからなかった
}
//(2)-2 回転方向 左方探索
Point leftSearchEdge = searcLeftDownUpEdge(FRONT_LIMIT_LEFT, 0, searchHight, mOutputFrame);
int edgeStartLeft = (int) leftSearchEdge.x;
int edgeEndLeft = (int) leftSearchEdge.y;
int axisLeft = NOT_FOUND;
if ( (edgeStartLeft>=0) && (edgeEndLeft>=0) ) {
axisLeft = (edgeStartLeft + edgeEndLeft)/2;
} else if ( (edgeStartLeft>=0) && (edgeEndLeft==NOT_FOUND) ) {
//右面 右端から中央方向へ 探索を伸ばすのですが、仮で頭打ち
edgeEndLeft=0;
axisLeft = (edgeStartLeft + edgeEndLeft)/2;
} else {
//みつからなかった
}
// (2)-3 回転方向 総合判断
if ( (axisRight>=0) && (axisLeft>=0) ) {
// 中央に近いほうを採用
if ( Math.abs(FRONT_CENTER - axisLeft) > Math.abs(FRONT_CENTER - axisRight) ) {
edgeStart = edgeStartRight;
edgeEnd = edgeEndRight;
} else {
edgeStart = edgeStartLeft;
edgeEnd = edgeEndLeft;
}
} else if (axisRight>=0) {
//右を採用
edgeStart = edgeStartRight;
edgeEnd = edgeEndRight;
} else if (axisLeft>=0) {
//左を採用
edgeStart = edgeStartLeft;
edgeEnd = edgeEndLeft;
} else {
//みつからなかった
}
if ( (edgeStart>=0) && (edgeEnd>=0) ) {
result = (edgeStart + edgeEnd)/2 ;
}
return result;
}
private Point searcLeftDownUpEdge(int startPos, int endPos, int searchHight, Mat inputFrame) {
double valCur = BIN_WHITE;
double valPre = BIN_WHITE;
int edgeStartPos = NOT_FOUND;
int edgeEndPos = NOT_FOUND;
for(int i=startPos; i>endPos; i--){
valCur = inputFrame.get(searchHight, i)[0] ;
if ( (valCur == BIN_BLACK) && ( valCur != valPre ) ) {
edgeStartPos = i;
} else if ( (valCur == BIN_WHITE) && ( valCur != valPre ) && (edgeStartPos!=NOT_FOUND) ) {
edgeEndPos = i;
int width = edgeStartPos - edgeEndPos;
if (width<LINE_WIDTH_LIMIT) {
edgeStartPos = NOT_FOUND;
edgeEndPos = NOT_FOUND;
} else {
break;
}
}
valPre = valCur;
}
Point resultPt = new Point(edgeStartPos, edgeEndPos);
return resultPt;
}
private Point searcRightDownUpEdge(int startPos, int endPos, int searchHight, Mat inputFrame) {
double valCur = BIN_WHITE;
double valPre = BIN_WHITE;
int edgeStartPos = NOT_FOUND;
int edgeEndPos = NOT_FOUND;
for(int i=startPos; i<=endPos; i++){
valCur = inputFrame.get(searchHight, i)[0] ;
if ( (valCur == BIN_BLACK) && ( valCur != valPre ) ) {
edgeStartPos = i;
} else if ( (valCur == BIN_WHITE) && ( valCur != valPre ) && (edgeStartPos!=NOT_FOUND) ) {
edgeEndPos = i;
int width = edgeEndPos - edgeStartPos;
if (width<LINE_WIDTH_LIMIT) {
edgeStartPos = NOT_FOUND;
edgeEndPos = NOT_FOUND;
} else {
break;
}
}
valPre = valCur;
}
Point resultPt = new Point(edgeStartPos, edgeEndPos);
return resultPt;
}
private int sarchLeftUpEdge(int startPos, int searchHight, Mat inputFrame) {
double valCur = BIN_WHITE;
double valPre = BIN_WHITE;
int leftPos=NOT_FOUND;
for(int i=startPos; i>0; i--){
valCur = inputFrame.get(searchHight, i)[0] ;
if ( (valCur == BIN_WHITE) && ( valCur != valPre ) ) {
leftPos = i;
break;
}
valPre = valCur;
}
return leftPos;
}
private int sarchRightUpEdge(int startPos, int searchHight, Mat inputFrame) {
double valCur = BIN_WHITE;
double valPre = BIN_WHITE;
int rightPos=NOT_FOUND;
for(int i=startPos; i<(inputFrame.width()); i++){
valCur = inputFrame.get(searchHight, i)[0] ;
if ( (valCur == BIN_WHITE) && ( valCur != valPre ) ) {
rightPos = i;
break;
}
valPre = valCur;
}
return rightPos;
}
//=======================================================================================
// 画像認識結果からBALAの制御（シリアル通信スレッドへ制御コマンド送信要求）
//=======================================================================================
//--- 加減速制御 ---
boolean accelerationEnable = false;
final static double ACCELERATION_THRESHOLD = 30; //[pixel/sec]
final static double ACCELERATION_ANGLE_LIMIT = 20; //[pixel]
final static int PITCH_SEARCH_ACCELERATION = PITCH_SEARCH_START - 41 ;
final static double FORWARD_OFFSET_NORMAL = 80; //前進オフセット: PID制御量=0 のときの前進量 motor0には1.0倍で適用する
final static double FORWARD_OFFSET_FAST = 210; // if (lrDiff>=1) { (255/lrDiff) } else { (255*lrDiff) } を超えないこと かつ 制御マージンも持つこと
final static double SPEED_ACCEL_STEP = 12;
final static double SPEED_DECEL_STEP = 32.5;
//--- 直進<->回転 制御変わり目の 動作安定待ち時間（あとで弄りたくなる項目） ---
// ※映像のレイテンシ以上を設定 or 動作安定時間 の長いほうを設定-
int controlSwitchWaitMs;
//--- 回転（調整項目） ---
int rotationPwm;
int rotation180degTime;
//--- 直進係数（調整項目） ---
double forwardOffset = FORWARD_OFFSET_NORMAL; //これはそれほど調節が必要な項目ではないのでここで設定している
double lrDiff;
//--- PID制御パラメータ（調整項目） ---
double Kp;
double Ki;
double Kd;
//--- PID制御変数 ---
double angle = 0; //[pixel]
double angleIntegral=0; //[pixel*sec]
double angleVelocity=0; //[pixel/sec]
double lastAngle = 0;
double samplingSec = 1.0/(double)ThetaView.FPS; //サンプリング周期[sec] = フレームレート依存
//--- 制御状態 ---
final static int LAST_CTRL_FORWARD = 0 ;
final static int LAST_CTRL_ROTATION = 1 ;
final static int LAST_CTRL_ROTATION_WAIT = 2 ;
int lastControl = NOT_FOUND;
boolean linetraceStopReq = false;
void resetControlParam() {
linetraceStopReq=false;
angleIntegral=0;
angleVelocity=0; //放置しても問題ない
lastAngle = 0;
forwardOffset=FORWARD_OFFSET_NORMAL;
}
void controlBala( int forwardPos, int rotationPos, int detectHight, int accelerationPos ) {
if (linetraceEnable) {
if (!sendReq) { //制御可能
//webUIからの停止要求対応
boolean webUiStop = false;
if ( (linetraceStopReq) && (forwardOffset == FORWARD_OFFSET_NORMAL) ) {
linetraceStopReq=false;
controlWait(controlSwitchWaitMs);
linetraceEnable = false;
notificationLedHide(LedTarget.LED3);
notificationLedHide(LedTarget.LED7); //REC
forwardPos=NOT_FOUND;
rotationPos=NOT_FOUND;
webUiStop = true;
}
//前進
if ( forwardPos != NOT_FOUND ) {
notificationLedBlink(LedTarget.LED3, LedColor.GREEN, 1000);
if ( lastControl == LAST_CTRL_ROTATION ) {
//回転動作後の安定待ち
controlWait(controlSwitchWaitMs);
lastControl = LAST_CTRL_ROTATION_WAIT;
} else {
controlForward( forwardPos, detectHight, accelerationPos );
lastControl = LAST_CTRL_FORWARD;
}
}
//回転
if (rotationPos != NOT_FOUND){
notificationLedBlink(LedTarget.LED3, LedColor.YELLOW, 1000);
if ( lastControl != LAST_CTRL_ROTATION_WAIT ) {
//制御変数リセット
resetControlParam();
//直進動作後の安定待ち
controlWait(controlSwitchWaitMs);
lastControl = LAST_CTRL_ROTATION_WAIT;
} else {
controlRotation(rotationPos);
lastControl = LAST_CTRL_ROTATION;
}
}
//黒線が見つからない
if ( (forwardPos==NOT_FOUND) && (rotationPos==NOT_FOUND) ) {
if (!webUiStop) {
notificationLedBlink(LedTarget.LED3, LedColor.MAGENTA, 1000);
}
lastControl = NOT_FOUND;
}
}
}
}
void controlForward( int forwardPos, int detectHight, int accelerationPos ) {
double ctrlVal0 = 0;
double ctrlVal1 = 0;
//操舵のPID制御
angle = FRONT_CENTER - forwardPos; //偏差
angleIntegral += (angle * samplingSec); //偏差の積分
angleVelocity = (angle - lastAngle)/samplingSec; //偏差の微分
lastAngle = angle;
double pidResult = Kp*angle + Ki*angleIntegral + Kd*angleVelocity;
//加減速制御
int accelerationAngle;
if (accelerationPos==NOT_FOUND) {
accelerationAngle = (int)ACCELERATION_ANGLE_LIMIT + 1;
} else {
accelerationAngle = FRONT_CENTER-accelerationPos;
}
if ( (accelerationEnable) &&
(!linetraceStopReq) &&
( lastControl == LAST_CTRL_FORWARD ) &&
( detectHight == PITCH_SEARCH_START ) &&
( Math.abs(angleVelocity) <= ACCELERATION_THRESHOLD ) &&
( Math.abs(accelerationAngle) <= ACCELERATION_ANGLE_LIMIT ) ) {
//加速
notificationLedShow(LedTarget.LED7); //REC
forwardOffset+=SPEED_ACCEL_STEP;
if (forwardOffset>=FORWARD_OFFSET_FAST) {
forwardOffset = FORWARD_OFFSET_FAST;
}
} else {
//減速
notificationLedHide(LedTarget.LED7); //REC
forwardOffset-=SPEED_DECEL_STEP;
if (forwardOffset<=FORWARD_OFFSET_NORMAL) {
forwardOffset = FORWARD_OFFSET_NORMAL;
}
}
//制御量決定
if ( pidResult < 0 ) {
//右へいきたいので motor0加算, motor1減算
ctrlVal0 = forwardOffset + Math.abs(pidResult/2);
ctrlVal1 = forwardOffset - Math.abs(pidResult/2);
} else {
//左へいきたいので motor0減算, motor1加算
ctrlVal0 = forwardOffset - Math.abs(pidResult/2);
ctrlVal1 = forwardOffset + Math.abs(pidResult/2);
}
pwmMotor0 = (int)(ctrlVal0);
pwmMotor1 = (int)(ctrlVal1 * lrDiff);
driveTimeMs = 5; //Step制御をしない場合意味はないが、accept受信までのwait時間(余裕量)にはなる
//driveTimeMs = 100; //Step Mode用
//driveTimeMs = 150; //Step Modeで1回の動作を長く実験をしたとき用
Log.d(TAG, "Ctrl F: m0=" + String.valueOf(pwmMotor0) + ", m1=" + String.valueOf(pwmMotor1) + ", PID=" + String.valueOf(pidResult) );
sendReq = true;
}
void controlRotation( int rotationPos ) {
int val = FRONT_CENTER - rotationPos;
if ( val < 0 ) {
//右回転
pwmMotor0 = rotationPwm;
pwmMotor1 = -1*rotationPwm;
} else {
//左回転/
pwmMotor0 = -1 * rotationPwm;
pwmMotor1 = rotationPwm;
}
double ctrlVal = 10 + rotation180degTime * ( (double)Math.abs(val)/(double)FRONT_CENTER );
driveTimeMs = (int)ctrlVal ;
Log.d(TAG, "Ctrl R: m0=" + String.valueOf(pwmMotor0) + ", m1=" + String.valueOf(pwmMotor1) + ", time=" + String.valueOf(driveTimeMs) + ", ctrlVal=" + String.valueOf(ctrlVal) );
sendReq = true;
}
void controlWait(int waitMs) {
pwmMotor0 = 0;
pwmMotor1 = 0;
driveTimeMs = waitMs;
Log.d(TAG, "Ctrl Stop: time=" + String.valueOf(driveTimeMs) );
sendReq = true;
}
//=======================================================================================
// シリアル通信まわり
//=======================================================================================
boolean linetraceEnable = false;
boolean sendReq = false;
int pwmMotor0 = 0;
int pwmMotor1 = 0;
int driveTimeMs = 0;
//シリアル受信スレッド
public void start_read_thread(){
new Thread(new Runnable(){
@Override
public void run() {
//android.os.Process.setThreadPriority(android.os.Process.THREAD_PRIORITY_FOREGROUND);
try {
Log.d(TAG, "Thread Start");
while(mFinished==false){
try {
//シリアル送信（制御コマンド） -> 受信(応答チェック)
if (sendReq == true){
if ( (pwmMotor0>=0) && (pwmMotor1>=0) ) { //前進
String setCmd = "mov_w " + String.valueOf(pwmMotor0) + " " + String.valueOf(pwmMotor1) + " " + String.valueOf(driveTimeMs) + "\n";
port.write(setCmd.getBytes(), setCmd.length());
Log.d(TAG, "Ctrl T:" + setCmd + ", ms=" + String.valueOf(driveTimeMs));
readWait(1000);
} else { //回転
String setCmd = "mov_s " + String.valueOf(pwmMotor0) + " " + String.valueOf(pwmMotor1) + " " + String.valueOf(driveTimeMs) + "\n";
port.write(setCmd.getBytes(), setCmd.length());
Log.d(TAG, "Ctrl T:" + setCmd);
readWait(1000);
}
sendReq = false;
}
} catch (IOException e) {
// Deal with error.
e.printStackTrace();
Log.d(TAG, "Ctrl T:IOException [" + e + "]");
notificationLedBlink(LedTarget.LED3, LedColor.RED, 1000);
break;
}
//ポーリングが高頻度になりすぎないよう5msスリープする
Thread.sleep(5);
}
Log.d(TAG, "Thread End");
} catch (InterruptedException e) {
// Deal with error.
e.printStackTrace();
Log.d(TAG, "T:InterruptedException");
}
}
}).start();
}
void readWait(int TimeOutMs){
int timeoutCount = TimeOutMs;
String rcvStr = "";
while (timeoutCount>0) {
byte buff[] = new byte[256];
int num=0;
try {
num= port.read(buff, buff.length);
} catch (IOException e) {
e.printStackTrace();
Log.d(TAG, "T:read() IOException");
num = -1;
}
if (num!=0) {
rcvStr = new String(buff, 0, num);
rcvStr = rcvStr.trim();
Log.d(TAG, "Ctrl len=" + rcvStr.length() + ", RcvDat=[" + rcvStr + "]" );
if ( rcvStr.equals("accept") ) {
break;
}
}
timeoutCount--;
}
if ( timeoutCount == 0 ) {
Log.d(TAG, "Ctrl:Timeout!!!" );
}
}
//=======================================================================================
// 調整値 保存と復帰
//=======================================================================================
private static final String SAVE_KEY_SWITCH_WAIT = "controlSwitchWaitMs";
private static final String SAVE_KEY_R_PWM = "rotationPwm";
private static final String SAVE_KEY_R_180_TIME = "rotation180degTime";
private static final String SAVE_KEY_LR_DIFF = "lrDiff";
private static final String SAVE_KEY_KP = "Kp";
private static final String SAVE_KEY_KI = "Ki";
private static final String SAVE_KEY_KD = "Kd";
SharedPreferences sharedPreferences;
void restoreControlParam() {
//保存されていたデータの読み込み
sharedPreferences = PreferenceManager.getDefaultSharedPreferences(getApplicationContext());
//--- 直進<->回転 制御変わり目の 動作安定待ち時間（映像のレイテンシ以上を設定 or 動作安定時間 の長いほうを設定） ---
controlSwitchWaitMs = sharedPreferences.getInt(SAVE_KEY_SWITCH_WAIT, 220);
//--- 回転（調整項目） ---
rotationPwm = sharedPreferences.getInt(SAVE_KEY_R_PWM, 128);
rotation180degTime = sharedPreferences.getInt(SAVE_KEY_R_180_TIME, 745);//バランスウェイト 単三電池 1本時
//--- 直進係数（調整項目） ---
//※トレッドの中心が車体中心からずれると変わる。組み立て注意。
lrDiff = sharedPreferences.getFloat(SAVE_KEY_LR_DIFF, (float) 1.1625);
//--- PID制御パラメータ（調整項目） ---
Kp = sharedPreferences.getFloat(SAVE_KEY_KP, (float)2.25);
Ki = sharedPreferences.getFloat(SAVE_KEY_KI, (float)0.25);
Kd = sharedPreferences.getFloat(SAVE_KEY_KD, (float)0.195);
}
void saveControlParam() {
SharedPreferences.Editor editor = sharedPreferences.edit();
editor.putInt(SAVE_KEY_SWITCH_WAIT, controlSwitchWaitMs);
editor.putInt(SAVE_KEY_R_PWM, rotationPwm);
editor.putInt(SAVE_KEY_R_180_TIME, rotation180degTime);
editor.putFloat(SAVE_KEY_LR_DIFF, (float) lrDiff);
editor.putFloat(SAVE_KEY_KP, (float) Kp);
editor.putFloat(SAVE_KEY_KI, (float) Ki);
editor.putFloat(SAVE_KEY_KD, (float) Kd);
editor.commit();
}
//=======================================================================================
//<<< web server processings >>>
//=======================================================================================
private Context context;
private WebServer webServer;
protected void onDestroy() {
super.onDestroy();
if (this.webServer != null) {
this.webServer.stop();
}
}
private class WebServer extends NanoHTTPD {
private static final int PORT = 8888;
private Context context;
public WebServer(Context context) {
super(PORT);
this.context = context;
}
@Override
public Response serve(IHTTPSession session) {
Method method = session.getMethod();
String uri = session.getUri();
switch (method) {
case GET:
return this.serveHtml(uri);
case POST:
Map<String, List<String>> parameters = this.parseBodyParameters(session);
execButtonAction(parameters);
return this.serveHtml(uri);
default:
return newFixedLengthResponse(Status.METHOD_NOT_ALLOWED, "text/plain",
"Method [" + method + "] is not allowed.");
}
}
private Map<String, List<String>> parseBodyParameters(IHTTPSession session) {
Map<String, String> tmpRequestFile = new HashMap<>();
try {
session.parseBody(tmpRequestFile);
} catch (IOException e) {
e.printStackTrace();
} catch (ResponseException e) {
e.printStackTrace();
}
return session.getParameters();
}
private Response serveHtml(String uri) {
String html="";
switch (uri) {
case "/":
html = editHtml();
return newFixedLengthResponse(Status.OK, "text/html", html);
default:
html = "URI [" + uri + "] is not found.";
return newFixedLengthResponse(Status.NOT_FOUND, "text/plain", html);
}
}
public static final String buttonName1 = "Reflect" ;
public static final String buttonName2 = "Save" ;
public static final String buttonName3 = "Start" ;
public static final String buttonName4 = "Stop" ;
public static final String buttonName5 = "Acceleration On/Off" ;
public static final String buttonName6 = "turn 180" ;
private void execButtonAction( Map<String, List<String>> inParameters ) {
if (inParameters.containsKey("button")) {
List<String> button = inParameters.get("button");
Log.d(TAG, "button=" + button.toString() );
if ( button.get(0).equals(buttonName1) ) { //Reflect
reflectParam( inParameters );
} else if ( button.get(0).equals(buttonName2) ) { //Save
reflectParam( inParameters );
saveControlParam();
} else if ( button.get(0).equals(buttonName3) ) { //Start
if ( (!linetraceEnable) && (!linetraceStopReq) ) {
resetControlParam( );
linetraceEnable = true;
}
} else if ( button.get(0).equals(buttonName4) ) { //Stop
if (!linetraceStopReq) {
linetraceStopReq=true;
}
} else if ( button.get(0).equals(buttonName5) ) { //Acceleration
if (accelerationEnable) {
accelerationEnable = false;
notificationLedHide(LedTarget.LED6); //Live
} else {
accelerationEnable = true;
notificationLedShow(LedTarget.LED6); //Live
}
} else if ( button.get(0).equals(buttonName6) ) { //turn 180
if (!linetraceEnable) {
controlRotation(0);
}
}
}
}
private void reflectParam( Map<String, List<String>> inParameters ) {
List<String> inSwitchWait = inParameters.get(SAVE_KEY_SWITCH_WAIT);
List<String> inRotationPwm = inParameters.get(SAVE_KEY_R_PWM);
List<String> inRotation180degTime = inParameters.get(SAVE_KEY_R_180_TIME);
List<String> inLrDiff = inParameters.get(SAVE_KEY_LR_DIFF);
List<String> inKp = inParameters.get(SAVE_KEY_KP);
List<String> inKi = inParameters.get(SAVE_KEY_KI);
List<String> inKd = inParameters.get(SAVE_KEY_KD);
controlSwitchWaitMs = Integer.parseInt( inSwitchWait.get(0) );
rotationPwm = Integer.parseInt( inRotationPwm.get(0) );
rotation180degTime = Integer.parseInt( inRotation180degTime.get(0) );
lrDiff = Double.parseDouble( inLrDiff.get(0) );
Kp = Double.parseDouble( inKp.get(0) );
Ki = Double.parseDouble( inKi.get(0) );
Kd = Double.parseDouble( inKd.get(0) );
}
private String editHtml() {
String html = "";
html += "<html>";
html += "<head>";
//html += " <meta name='viewport' content='width=device-width,initial-scale=1'>";
html += " <meta name='viewport' content='width=480,initial-scale=0.7'>";
html += " <title>Parameter Settings</title>";
html += " <script type='text/javascript'>";
html += " </script>";
html += "</head>";
html += "<body>";
html += "";
html += "<form action='/' method='post' name='SettingForm'>";
html += " <hr>";
html += " <h2>[Parameter Settings]</h2>";
html += " <hr>";
html += " <table>";
html += " <tr>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td><input type='submit' name='button' value='" + buttonName1 + "'></td>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td><input type='submit' name='button' value='" + buttonName2 + "'></td>";
html += " <td> </td>";
html += " <td> </td>";
html += " </tr>";
html += " </table>";
html += " <hr>";
html += editParameterList();
html += " <hr>";
html += " <table>";
html += " <tr>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td><input type='submit' name='button' value='" + buttonName3 + "'></td>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td><input type='submit' name='button' value='" + buttonName4 + "'></td>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td><input type='submit' name='button' value='" + buttonName5 + "'></td>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td> </td>";
html += " <td><input type='submit' name='button' value='" + buttonName6 + "'></td>";
html += " </tr>";
html += " </table>";
html += " <hr>";
html += "</form>";
html += "";
html += "</body>";
html += "</html>";
return html;
}
private String editParameterList() {
String html = "";
html += " <table>";
html += " <tr>";
html += " <td style='background:#CCCCCC'>Parameter Name</td>";
html += " <td style='background:#CCCCCC'>Value</td>";
html += " </tr>";
html += " <tr>";
html += " <td>" + SAVE_KEY_SWITCH_WAIT + "</td>";
html += " <td><input type='number' name='" + SAVE_KEY_SWITCH_WAIT + "' value='" + String.valueOf(controlSwitchWaitMs) + "' autocomplete='off'></td>";
html += " </tr>";
html += " <tr>";
html += " <td></td>";
html += " <td></td>";
html += " </tr>";
html += " <tr>";
html += " <td>" + SAVE_KEY_R_PWM + "</td>";
html += " <td><input type='number' name='" + SAVE_KEY_R_PWM + "' value='" + String.valueOf(rotationPwm) + "' autocomplete='off'></td>";
html += " </tr>";
html += " <tr>";
html += " <td>" + SAVE_KEY_R_180_TIME + "</td>";
html += " <td><input type='number' name='" + SAVE_KEY_R_180_TIME + "' value='" + String.valueOf(rotation180degTime) + "' autocomplete='off'></td>";
html += " </tr>";
html += " <tr>";
html += " <td></td>";
html += " <td></td>";
html += " </tr>";
html += " <tr>";
html += " <td>" + SAVE_KEY_LR_DIFF + "</td>";
html += " <td><input type='number' name='" + SAVE_KEY_LR_DIFF + "' value='" + String.valueOf(lrDiff) + "' autocomplete='off' step='any' min='0.0'></td>";
html += " </tr>";
html += " <tr>";
html += " <td></td>";
html += " <td></td>";
html += " </tr>";
html += " <tr>";
html += " <td>" + SAVE_KEY_KP + "</td>";
html += " <td><input type='number' name='" + SAVE_KEY_KP + "' value='" + String.valueOf(Kp) + "' autocomplete='off' step='any'min='0.0'></td>";
html += " </tr>";
html += " <tr>";
html += " <td>" + SAVE_KEY_KI + "</td>";
html += " <td><input type='number' name='" + SAVE_KEY_KI + "' value='" + String.valueOf(Ki) + "' autocomplete='off' step='any' min='0.0'></td>";
html += " </tr>";
html += " <tr>";
html += " <td>" + SAVE_KEY_KD + "</td>";
html += " <td><input type='number' name='" + SAVE_KEY_KD + "' value='" + String.valueOf(Kd) + "' autocomplete='off' step='any' min='0.0'></td>";
html += " </tr>";
html += " </table>";
return html;
}
}
}
Arduino Pro Micro側
前回記事「Arduino Pro Micro」の章をみると大枠はわかると思いますので説明を割愛し、ソースコード全文を畳んで掲載しておきます。
ファイル名は「LineTracer_Base2.ino」としています。
「LineTracer_Base2.ino」ソースコード全文
LineTracer_Base2.ino
/**
* Copyright 2018 Ricoh Company, Ltd.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include <Wire.h>
#define M5GO_WHEEL_ADDR 0x56
#define MOTOR_CTRL_ADDR 0x00
#define ENCODER_ADDR 0x04
#define MOTOR_RPM 150
#define MAX_PWM 255
#define DEAD_ZONE 20
int motor0 = 0;
int motor1 = 0;
int timeout = 0;
String serialRead();
int splitParam2( String inStr, int *param1, int *param2 );
int splitParam3( String inStr, int *param1, int *param2, int *param3 );
void setMotor(int16_t pwm0, int16_t pwm1);
void stop();
void move_t(int16_t pwm0, int16_t pwm1, uint16_t duration, bool stopEna );
void setup() {
Serial.begin(115200); // SERIAL
Wire.begin();
Wire.setClock(400000UL); // Set I2C frequency to 400kHz
delay(500);
setMotor( 0, 0 );
}
void loop() {
int speed=0;
int delay_ms=0;
String RcvCmd ="";
RcvCmd = serialRead();
RcvCmd.trim();
if ( !(RcvCmd.equals("")) ) {
//Serial.print("rcv=["+RcvCmd + "]\n");
if ( RcvCmd.equals("go") ) {
move_t( 80, 80, 1000, true );
} else if ( RcvCmd.startsWith("set ") ) {
RcvCmd.replace("set " , "");
splitParam2( RcvCmd, &motor0, &motor1);
setMotor( motor0, motor1 );
timeout = 50; // およそ1秒
} else if ( RcvCmd.startsWith("mov_s ") ) {
RcvCmd.replace("mov_s " , "");
splitParam3( RcvCmd, &motor0, &motor1, &delay_ms);
move_t( motor0, motor1, delay_ms, true );
} else if ( RcvCmd.startsWith("mov_w ") ) {
RcvCmd.replace("mov_w " , "");
splitParam3( RcvCmd, &motor0, &motor1, &delay_ms);
move_t( motor0, motor1, delay_ms, false );
timeout = 50; // およそ1秒
} else {
stop();
}
//受信して動作したことを応答
Serial.print("accept\n");
}
// 安全装置
if ( timeout != 0 ) {
timeout--;
if ( timeout == 0 ) {
stop();
}
}
delay(20);
}
#define SERIAL_BUFF_BYTE 512
String serialRead(){
char sSerialBuf[SERIAL_BUFF_BYTE];
String result = "";
if ( Serial.available() > 0 ) {
int iPos=0;
while (Serial.available()) {
char c = Serial.read();
if ( c == '\n' ) {
break;
} else {
sSerialBuf[iPos] = c;
iPos++;
if (iPos==(SERIAL_BUFF_BYTE-1) ) {
break;
}
}
}
sSerialBuf[iPos] = 0x00;
result = String(sSerialBuf);
}
return result ;
}
int splitParam2( String inStr, int *param1, int *param2 ) {
int ret = 0;
inStr.trim();
int len = inStr.length();
int pos = inStr.indexOf(' ', 0);
if ( (pos > 0) && (len>=3) ){
String Param1 = inStr.substring(0, pos);
String Param2 = inStr.substring(pos+1, len);
//Serial.print("Param1=" + Param1 + ", Param2=" + Param2 +"\n");
*param1 = Param1.toInt();
*param2 = Param2.toInt();
} else {
ret = -1;
}
return ret;
}
int splitParam3( String inStr, int *param1, int *param2, int *param3 ) {
int ret = 0;
inStr.trim();
int len = inStr.length();
int pos = inStr.indexOf(' ', 0);
int pos2 = inStr.indexOf(' ', pos+1);
if ( (pos > 0) && (len>=5) ){
String Param1 = inStr.substring(0, pos);
String Param2 = inStr.substring(pos+1, pos2);
String Param3 = inStr.substring(pos2+1, len);
Serial.print("Param1=" + Param1 + ", Param2=" + Param2 + ", Param3=" + Param3 + "\n");
*param1 = Param1.toInt();
*param2 = Param2.toInt();
*param3 = Param3.toInt();
} else {
ret = -1;
}
return ret;
}
void setMotor(int16_t pwm0, int16_t pwm1) {
// Value range
int16_t m0 = constrain(pwm0, -255, 255);
int16_t m1 = constrain(pwm1, -255, 255);
// Dead zone
if (((m0 > 0) && (m0 < DEAD_ZONE)) || ((m0 < 0) && (m0 > -DEAD_ZONE))) m0 = 0;
if (((m1 > 0) && (m1 < DEAD_ZONE)) || ((m1 < 0) && (m1 > -DEAD_ZONE))) m1 = 0;
// Same value
static int16_t pre_m0, pre_m1;
if ((m0 == pre_m0) && (m1 == pre_m1))
return;
pre_m0 = m0;
pre_m1 = m1;
Wire.beginTransmission(M5GO_WHEEL_ADDR);
Wire.write(MOTOR_CTRL_ADDR); // Motor ctrl reg addr
Wire.write(((uint8_t*)&m0)[0]);
Wire.write(((uint8_t*)&m0)[1]);
Wire.write(((uint8_t*)&m1)[0]);
Wire.write(((uint8_t*)&m1)[1]);
Wire.endTransmission();
}
void stop(){
motor0 = 0;
motor1 = 0;
setMotor( motor0, motor1 );
}
void move_t(int16_t pwm0, int16_t pwm1, uint16_t duration, bool stopEna){
setMotor( pwm0, pwm1 );
if (duration != 0) {
delay(duration);
if (stopEna) {
stop();
}
}
}
まとめ
まず、THETA Vが360°カメラ付きAndroidボードとしても、とても優秀であるということをご理解頂けたかと思います。
かなり、ながーい記事としました。これは画像認識+モーター制御が同時成立していることをキチンと解説したかったためです。
高専や工学系の大学生が学ぶ制御工学の基礎が盛りだくさん詰まってます。学習用教材としてもなかなか使えるなーと思っていただけたら嬉しいです。
この機材でライントレーサーをするにあたり、自身で掲げた「目標」は概ねクリアできたかと思います。
作業時間短縮のために手抜きした点も細々とありますが、生じた問題は各章の「簡略化した処理とその弊害」にまとめてありますし。いちおー解決策も頭の中にはございます。
このハードでは、次のステップへ進もうと思います。
次のステップ（いつになるかは不明です！）では、6軸センサーの値を使って「倒立振子」をしたいです。
演算能力の余力はまだあるようですが、どれくらいの頻度でセンサー値を扱えるか見当がついていません･･･
倒立振子だけならば画像処理はしなくてもという手はあります。しかし、倒立振子の次には補助輪なし走行とかもしたいですし、補助輪があるライントレーサー状態であっても自分の移動軌跡を描いたり、ウィリー姿勢を自分で治したり、etc...
と、発展させたいです。
そろそろPro Microとの通信をバイナリにしたり、THETA側とマイコン側の制御分担を見直したほうが良いかもしれません。
ライントレース動作と同時に、デバッグ用Vysor画面を常時無線LAN経由でPCへ出力していたわけですが、成立していましたね。この画像をwebUIに出したり外部ネットワークへ配信できるようにできないかなーとかも思っています（が、技術調査している余力がない･･･そのうち）。
THETAの画像の下のほうを使って、上のほうは使えていませんでした。
モルフォロジーでだいぶ処理時間を使っていますので15fpsのままでは成立しませんが、もっとフレームレートを落としたり走行速度を落としたりするならば、自身下方のライン認識と同時に、上の方の映像から他の認識処理を行っても面白いかもしれません。
というわけで「THETAプラグイン×M5BALA」シリーズは Part3にとどまらず4か5くらいまではいけそーかな？
どーなることやらですがお楽しみに。
そして、まったく違うTHETAプラグインネタもやりますのでご安心を！
RICOH THETAプラグインパートナープログラムについて
THETAプラグインをご存じない方はこちらをご覧ください。
パートナープログラムへの登録方法はこちらにもまとめてあります。
興味を持たれた方はTwitterのフォローとTHETAプラグイン開発コミュニティ(Slack)への参加もよろしくおねがいします｡