ESP32で簡単な"AIのドアマン"作りました。
しばらく電子工作から離れていたので、久しぶりの投稿となります。
ふとドローン以外のものが作りたくなったため、下のような「AIのドアマン」を作ってみることにしました。
試作品の動画
あれこれした結果、試作品ですが完成しました。
アプリに関して
アプリに関しては、
・顔認識：OpenCVをAndroidで使える！
・音声認識：AndroidのAPIを使えば簡単にできる！
・Bluetooth：ドローンのを使いまわせる！
ということで、とりあえず動くものが２週間程度で出来てしまいました。
ハードに関して
ハード面に関しては、PIC＋RN42-I/RMでやろうと思いましたが、
色々調べてみると「ESP-WROOM-32」なるものを発見。
なんと！プログラマブルなICにBluetoothがついていて、秋月電子で一個500円！
PIC＋RN42-I/RMの組み合わせなら2000円以上します！
これは買わなきゃ損だという事で、さっそく秋月電子でDIP品の「AE-ESP-WROOM-32」と、
マルツパーツで書き込み用の「Arduinoシリアル-USB変換モジュール」を購入しました（Spark FunのFTDI Basic 3.3Vのやつです）。
しかもこのESP32はArduinoらしい、ということで今回がArduino初体験になりました。
Arduinoとして実際に触ってみると、実装や配線は滅茶苦茶簡単でした（笑）
なんとBluetooth専用のライブラリがあったり、PWM制御も専用の関数で一瞬で出来ちゃいます。
何の苦労・知識がなくても、電子工作ができる時代が来るんだなぁとしみじみしました（笑）
コンパイルのやりかたは、「Arduino ESP32」とかでググればいろいろ出てくるので詳しくは書きません。
現状、ブレッドボードでの試作段階での回路図を載せます。基板に起こさないので手書きです。
簡単な説明
・6Vを3.3Vに降圧してESP32に供給してます。
・サーボには6Vを直接供給します。
・IO4ピンからPWMを出力します。
・書き込み用にIO0とENにリセットボタンを付けています。
・書き込み用変換モジュールとの接続は赤色で書いてます。
ESP32への書き込みは以下の要領です。
・変換モジュールのRXI⇔ESP32のTXD0
・変換モジュールのTXO⇔ESP32のRXD0
・変換モジュールのGND⇔電源のGND
・電圧低下が怖いのでサーボは外しておく（念のため。Lチカ実験時は、LEDつけっぱでも行けました）
の配線をした状態で、
①IO0のリセットボタンを押しながら、ENのリセットボタンを押して離す
②コンパイル＆書き込み開始
③コンパイルが終わって書き込み中になったら、IO0のリセットボタンを離す
④書き込みが完了したら、ENのリセットボタンを押して離す。
で書き込みが完了しましたとなればOK.
最後に
今回の成果です。
・わずか500円でスマホからの無線制御環境を手に入れました。
・AndroidアプリからOpenCVで画像処理できるようになりました。
・超ブラックボックスですが、AndroidのAPIから音声認識できるようになりました。
今後の課題は、
・ガワの設計（Fusion360で設計して3Dプリント）
・基板の設計（EAGLEでやります。表面実装品を手はんだします。）
・顔認識から個人識別へ発展させる（OpenCVのライブラリにはないので、LBPHなるものを自力で実装するのでしょう、、、）
・超ブラックボックスの音声認識を理解する
です。
以上、ずいぶんと端折りましたが、必要な項目等あれば気づき次第加筆してきます。
完全自作ドローン〜ここまでの進捗〜
仕様概要
完全自作ドローンの仕様概要を説明します。
・制御基板およびモーターを有する
・スマートフォンからの無線操縦により操作可能
この超大雑把な仕様から、当たって砕けろの精神で開発を進めていきます。
もちろん、制御基板は基板からソフトまで全て自作であり、機体構体も完全自作とします。ただし、モーターのみ自作する技術的メリットが少ないと判断したため既製品を購入します。
メカ設計
設計にあたっては、機体構体・制御基板・モーター(プロペラ含む)・バッテリーの各要素の統合が、なるべく少ない重量かつ取り回ししやすく干渉しない機構で実現されなければなりません。
そこで参考にできるのが、下記のような
既製品のミニドローンです。
写真のように解体してみると、以下のことがわかります。
・構体は上部・下部の2つに分かれており、それぞれが基板を挟むような形を取っている
・モーターおよび制御基板は、下部の構体に固定されている
・基板は1枚のみで、モーター線は制御基板上に直接半田付けされている
・バッテリーは下部の構体と制御基板に挟まれて固定されている
上部・下部には分けた構造は、設計のレベルが高いため、敷居を低くして下部のみの構体で実現します。この点とミニドローンの設計を勘案すると、要求される機構的な仕様は
・モーターおよび制御基板は、構体に固定されている
・基板は1枚のみで、モーター線は制御基板上に直接半田付けされている
・バッテリーは構体と制御基板に挟まれて固定されている
となります。
これらを満たすように作成したドローンはこのようになります。
モーター把持部の形状生成が現状の加工技術では困難だと判断し、構体はフリーの3DCADであるfusion360で設計しました。こんな感じになります。
これをDMMの3Dプリントサービスで成形すると、こんな感じになります。
エレキ設計
必要な部品はざっと以下です
・マイコン
・Bluetoothモジュール
・トランジスタアレイ(モータ駆動用)
・電源レギュレータ
・重力、角速度、地磁気センサ
部品選定ですが、マイコン、Bluetoothモジュールに関しては表面実装部品を採用します。理由は明白で、ペイロードと実装面積の節約のためです。センサ部品に関しては、そもそも表面実装部品が個人では手に入らないこと、手はんだが困難であるという理由から、DIP化モジュールを採用します。
ここで超注意点！一時期トランジスタアレイも表面実装部品にしていましたが、モータ駆動に用いるPWMのデューティ比を大きくしていくと、保護回路があるにも関わらず突然モータが動かなくなるという現象が発生しました。トランジスタアレイを交換するとまた動き出し、デューティ比をあげると再度動かなくなるため、トランジスタアレイに問題があることまではわかっていました。また、DIP品ではデューティ比を大きくてしも問題なく動いていたため、表面実装部品とDIP品をデータシートで比較したところ、許容損失に大きく差があることがわかりました。おそらくこれが原因だと思われます。
このため、トランジスタアレイは許容損失の大きいDIP品を採用します。
今回は機体重量を最小限にするため、プリント基板を用いる必要があります。
EDAにはフリーソフトのEAGLEを使いました。
配線図はこんな感じです。ICSPがしたいので、プログラム用ポートはモータ制御等に使用しないようにします。
ボード図はこんな感じ。モーターが基板の四隅になるようにします。また制御基板と結合するためのドリル穴を忘れないようにします。
これをseeed studioという中国の基板プリントサービスでプリントしてもらうと、こんな感じになります。
さらに部品を実装するとこんな感じ。
ソフト設計
このシステムでは大きく分けて2つのソフトが必要です。
１つは制御基板上のマイコンに書き込むソフト、もう1つはスマートフォンからドローンを操作するためのアプリです。
マイコン用のソフトに関しては、
・センサーとのSPI通信
・センサ値からの姿勢角算出
・姿勢角からの指令値算出
・モーターへのPWM制御
・BluetonthモジュールとのUART通信
が必要になります。
受信割り込み(上昇、下降司令)を受けると、センサーから取得した姿勢角を安定させ、かつ上昇・下降司令に沿うような各モーターへの制御司令を算出し、PWM制御を行います。
スマートフォンのアプリに関しては、MacでしかiPhoneアプリを開発できないことから、Bluetonth機能を有するAndroidスマホの中で一番安そうなものを新規に購入しました。
本作品以外でも使えそうなので良い買い物です。
アプリの実装に関しては、私がアプリ開発およびJava未経験であることから、かなり骨が折れました。UIの作成や簡単なイベント制御はそんなに難しくないと思うのですが、Bluetonth通信は書籍の情報も少ないため、あらゆるサイトや書籍で勉強して、なんとか動くものを作れるようになりました。
最後に
開発を進めるにあたって参考になった書籍を挙げておきます。
・fusion360の使い方
ドローンの作例があるため、
一部これを参考にしました。
Fusion 360操作ガイド スーパーアドバンス編―次世代クラウドベース3DCAD
- 作者: 三谷大暁
- 出版社/メーカー: カットシステム
- 発売日: 2016/11/01
- メディア: 単行本
- この商品を含むブログを見る
・EAGLEの使い方
プリント基板作成に関して、最高の一冊だと思います。
実践的で読みながら作業するのにストレスが皆無。
いくつか関連書籍は読みましたが、
この一冊だけで回路作成→基盤発注まで行けます。
基礎からのプリント基板製作―Autodeskの基板設計ソフト「EAGLE」を使う (I・O BOOKS)
- 作者: 佐倉正幸
- 出版社/メーカー: 工学社
- 発売日: 2017/05/01
- メディア: 単行本
- この商品を含むブログを見る
・Androidアプリの作り方
まず手始めにこの書籍でアプリ開発の大まかな流れを学びます。
イラストでよくわかるAndroidアプリのつくり方?Android Studio対応版 イラストでよくわかるシリーズ
- 作者: 羽山博,めじろまち
- 出版社/メーカー: インプレス
- 発売日: 2015/06/05
- メディア: Kindle版
- この商品を含むブログを見る
この本はBluetoothを活用したアプリについて
解説している唯一の書籍と思います。
Bluetoothデバイス開発に必要不可欠です。
作ればわかる！Androidプログラミング 第4版 SDK5/6 Android Studio対応 10の実践サンプルで学ぶAndroidアプリ開発入門
- 作者: 金宏和實
- 出版社/メーカー: 翔泳社
- 発売日: 2016/05/09
- メディア: Kindle版
- この商品を含むブログを見る
完全自作ドローン
4年ほど前からドローンを完全自作してみようと企んでいます。
仕事が忙しく、3年ほど何も手がつきませんでしたが、ここ最近は一念発起して少しずつ開発を再開しています。
ドローンについて知識ゼロからの開発であるため、多くの手戻り(構体の作り直し、基板の作り直し、モーターの再選定等、、)
がありましたが、結構いいとこまで来た感があるので、今後はドローンを完全自作していく過程を少しずつ公開していきたいと思います。
なお、今のところこの開発により下記の技術を習得できました(もしくは今後する予定)。
・3DCAD・3Dプリンタの活用方法
・プリント基板の作成方法
・表面実装部品の手はんだ
・ICSPの実現
・アンドロイドアプリの作成方法
・Bluetoothを用いた無線通信
・重力センサ、角速度センサ、地磁気センサの活用方法
・ドローンの位置・姿勢制御
(あとカルマンフィルタとか本格的に勉強したい、、)
2軸サーボ+ビジュアルフィードバックシステム
以前（４，５年前？）に作った2軸サーボ+ビジュアルフィードバックシステムについて、、
システム構成
①カメラで画像取得→有線でPCへ
②PCで画像処理・司令値計算→無線でマイコンへ
③司令値に基づきマイコンでサーボをPWM制御
大まかにこの①〜③を繰り返して、物体を追従させています。
メカ設計
ロボットのアーム(?)的な部分はホームセンターで買ってきた、アルミ板(2mm厚だったかな?)で作りました。
曲げはポケットベンダー、切断は金切り鋏、穴あけは卓上ボール盤でやりました。3DCADは使っていませんでしたが、試行錯誤することもなく簡単に組み上がったと記憶してます。
エレキ設計
サーボ制御を行うマイコン側回路と、PCから無線を飛ばすためのPC側回路を作りました。
マイコン側回路は、マイコン（PIC24)に対してサーボモータとZigbeeをつないでいます。
PC側回路はZigbee＋ADM3202を組み合わせて、シリアル通信を行えるようにしています。
ソフト設計
マイコン側プログラムとPC側プログラムがあります。
マイコン側プログラムはUART機能を用いており、受信割り込みが発生して指令値を受け取ると、PWMのデューティー比を指令値に応じて変化させます。main関数はひたすらwhileでループさせています。
/**************************************************
* PIC32MX I/Oテストプログラム、
* OC1をRP4,OC2をRP5に割り当ててタイマ２を用いたPWM制御によって2つのサーボモーターの角度を変化させる。
* 角度の目標値は、U1TXをRP6,U1RXをRP7に割り当てて、XBee,UARTを用いてPCから送信された値を設定する。なお、今回PIC側は受信しか行わないため、RXしか使わない。
* 指令値の判別は受信割り込みを用いてソフト的に行う。
*
* PIC PIC24FJ64GA002
* クロック FRCPLL 32MHz→1クロック=1/(32*10^6)=0.00000003125s=0.03125us=31.25ns→１サイクル=２クロック=62.5ns
***************************************************/
#include "p24FJ64GA002.h"
#include <math.h>
#include <stdlib.h>
// コンフィギュレーション設定
// config1(CW1レジスタ)
_CONFIG1(JTAGEN_OFF // JTAG の機能を兼用しているピンのJTAGピン有効無効（ON,OFF）→ JTAG機能の無効化（これにより、JTAG機能を持っているピンを普通に使えるようになる）
& BKBUG_OFF // バックグラウンド デバッガの有効無効(ICD デバイスでのみ使用)(ON,OFF) → バックグラウンドデバッガは無効化
& GWRP_OFF // フラッシュ書き込みプロテクトの有効無効（ON,OFF） → 各種プロテクトは無効化する
& GCP_OFF // コードプロテクトの有効無効（PIC24のGCPに対応）（ON,OFF） → 各種プロテクトは無効化する
& ICS_PGx1 // ICDピンの選択 (RESERVED,ICS_PGx3,ICS_PGx2,ICS_PGx1) → EMUC/EMUDをPGC1/PGD1と共用（ここが怪しい。もしうまくいかなかったら、RESERVEDにしたほうがいいかも）
& FWDTEN_OFF ) // ウォッチドッグタイマの有効無効（ON,OFF）→ ウォッチドッグタイマは無効化（これにより、勝手に一定周期のリセットがかからなくなる）
// ウォッチドッグタイマを使用しない場合なら、これらの設定はいらないと考えられる。
// & WINDIS_OFF //Watchdog Timer Window Enable:Watchdog Timer is in Non-Window Mode
// & WDTPS_PS1 //Watchdog Timer Postscaler:1:1 (PS1,,,,PS1048576)
// & FWPSA_1 //ウォッチドッグタイマのプリスケーラ設定
// config2(CW2レジスタ)
_CONFIG2(IESO_OFF // クロック内外切替制御(ON,OFF) → 内外クロックの切り替えはしない
& FCKSM_CSDCMD // FSCMと切替制御 (CSECME,CSECMD,CSDCMD) //あるサイトからのコピペ説明：Fail-Safe Clock Monitor(FSCM)は、オシレータの故障の場合さえ、デバイスが作動し続けるのを許容するように設計されます。オシレータが故障した場合、FSCMはオシレータ故障割込を発生させ、システムクロックを内部オシレータに切り替えます。システムはFale-Safe状態から脱するまで内部オシレータによって駆動します。Fale-Safeの状態はリセット、sleep命令の実行、OSCCONレジスタへのライトによって抜けられます。内部オシレータの周波数は、IRCFビットの状態に依存します。OSCCONレジスタのIRCF、SCSビットを通して別のクロックソースを選ぶことができます。自分解釈：外部クロックが入力されなくなった場合に、それを検知して内部クロックで動くようにするオプションであろう。 → 緊急時のクロックの切り替えはしない
& OSCIOFNC_OFF // CLKOピン出力有効制御(ON,OFF) → ポート10（OSCO/CLKO/RA3）はデジタルIOピン（RA3）として使う
& IOL1WAY_OFF // ON：IOLOCKは１度だけ変更可（IOLOCKはデフォルトでONなので、一回ONにしたらIOLOCKは変更できなくなる）、OFF：IOLOCKは何度でも変更可 （ IOLOCK = ON：ピン割り付けはデフォルトで固定、OFF：ピン割り付けを変更可（ただし、これは起動・リセット時にデフォルトでOFFになる）） → ピン割り付け（RPxnRレジスタで行う）を何度でも変更できるようにする。
& I2C1SEL_PRI // I2C1ピンを割り付け可能にするかどうか → I2C1ピンを割り付け可能にする？
& FNOSC_FRCPLL // CPUクロック源選択 (FRC,FRCPLL,PRI,PRIPLL,SOSC,LPRC,FRCDIV16,FRCDIV） → 内蔵高速発振回路（FRC：8MHz）をPLLによって32MHzにする設定
& POSCMOD_NONE) // 主発振器制御 (EC,XT,HS,NONE) → 主発振器制御（外付け発振回路によるクロック）は使わない
#define Fosc 32 // クロック周波数[MHz]
extern unsigned long int waiter_100us = Fosc/4*50-1;
extern unsigned long int waiter_1ms = Fosc/4*500-1;
extern unsigned long int waiter_10ms = Fosc/4*5000-1;
// プロトタイプ宣言
extern void WAIT_100us(void);
extern void WAIT_1ms(void);
extern void WAIT_10ms(void);
extern void WAIT_100ms(void);
// 受信データ（0～255）を一時的に格納しておく変数。0<=data_rcv<=126のときサーボ１を現在の角度からdata_rcv－63=－63[deg]～＋63[deg]の範囲で変化させ、127<=data_rcv<=253のときサーボ２を現在の角度からdata_rcv－190=－63[deg]～＋63[deg]の範囲で変化させる。
unsigned int data_rcv = 0;
// タイマ2設定値
int TMR2_PS = 8; // プリスケーラは8倍
int Fcy = Fosc/2; // タイマクロック[Mz]
// タイマ２・PWMモードによるサーボ制御の設定値
int srv_pwm = 15; // サーボモーターのPWM周期[ms]＝タイマ２の割り込み周期
double srv_t0 = 0.8/90; // サーボモーターの角度を1[deg]変化させるのに必要なパルス幅の変化[ms/deg]
int Deg1 = 0; // サーボ１の角度[deg]
int Deg2 = 0; // サーボ２の角度[deg]
////////////////////////////////////////
/////// サブルーチン ///////////
////////////////////////////////////////
// タイマ２を用いて、サーボモーターを中位位置から変化させたい角度d[deg]から、必要なパルス幅を生成するのに必要なタイマサイクル数（つまりOCxRS）を計算する
unsigned long int SRV_DEG2CYC_TMR2(int d){
return( (PR2+1)/srv_pwm*(1.5+d*srv_t0) );
}
// シグナム関数（引数はint型）
int sgn(int a){
if(a>=0){
return(1);
}else{
return(-1);
}
}
////////////////////////////////////////
///////// メイン関数 ///////////
////////////////////////////////////////
int main(void){
/***** 初期設定 *****/
// クロック分周比の設定（デフォルトでは1/2？）
CLKDIV = 0; // クロックをコンフィギュレーションにおける設定値の１倍に
// アナログポートの設定（アナログ機能を持つピン（ANn）はデフォルトでアナログモードになっているので、I/OやUARTなどのデジタルモードとして使う場合は最初にAD1PCFGを操作する必要がある）
//AD1PCFGbits.PCFGn // 全ANnピンをデジタルモードにする
// デジタルIOピンの入出力モード設定
TRISBbits.TRISB6 = 0; // U1TXに割り当てるRB6(RP6)を出力モードに
TRISBbits.TRISB7 = 1; // U1RXに割り当てるRB7(RP7)を入力モードに
// ピン割り付けRPORx,RPINRx
RPOR2bits.RP4R = 18; // RP4ピンにOC1を割り当て
RPOR2bits.RP5R = 19; // RP5ピンにOC2を割り当て
RPOR3bits.RP6R = 3; // RP6ピンにU1TXを割り当て
RPINR18bits.U1RXR = 7; // RP7ピンにU1RXを割り当て
// タイマ２初期設定(タイマクロックFosc/2=16[Mz],プリスケーラ8倍,割り込み周期15[ms])
// 割り込み間隔[s] = (タイマクロック[s]) * （プリスケーラ分周比） * （PRx + 1） = (1/Fcy[Hz]) * （プリスケーラ分周比） * （PRx + 1） －式①となる（PRx + 1となるのは無理やり納得するしかない？）。Fcyはタイマ動作に用いるクロックである。TMR2がFcyの周期でカウントアップし、PR2に設定した値と一致すると割り込みが生じる。
T2CON = 0b1000000000010000; // T32=0においてTONは1として16ビットタイマをONにする、TSIDLは0としてアイドルモード中もタイマ動作を継続させる、TCS=0においてTGATEは0としてゲート時間積算を無効化する（この操作により、TMR2レジスタがPR2レジスタの値と一致した時にT2IFがセットされて割り込みが生じる）、TCKPSは01としてタイマ動作に用いるプリスケーラ分周を8倍にする（8/Fcyの間隔でTMR2がカウントアップする）、T32は0としてTMRxとTMRyを個別の16ビットタイマとする、TCSは0としてタイマ動作に用いるクロックをFcy=Fosc/2とする
PR2 = Fcy/TMR2_PS*srv_pwm*pow(10.0,3.0) - 1; // T2CONにおいて、タイマ動作に用いるクロックFcy=Fosc/2=16[Mz],プリスケーラ分周比を8倍としている。サーボモーターのPWM周期の要請から、割り込み間隔を15[m]sとしたい。これらの条件から式①を用いて逆算すると、PR = (15*10^(-3))/(1/(16*10^6))/8-1 = 29999
// OC1初期設定（タイマ2によるPWMモード、フォルトピン無効）
OC1CON = 0b0000000000000110; // OCSIDLは0としてアイドルモード中も継続動作とする、OCFLTはPWMフォルトを考慮しないので関係なし（ここでは0に）、OCTSELは0としてタイマ2をクロック源とする、OCMは110としてPWMモード,OC1に出力,フォルトピン無効とする
OC1RS = SRV_DEG2CYC_TMR2(Deg1); // 初期位置は中位
// OC2初期設定（タイマ2によるPWMモード、フォルトピン無効）
OC2CON = 0b0000000000000110; // OCSIDLは0としてアイドルモード中も継続動作とする、OCFLTはPWMフォルトを考慮しないので関係なし（ここでは0に）、OCTSELは0としてタイマ2をクロック源とする、OCMは110としてPWMモード,OC2に出力,フォルトピン無効とする
OC2RS = SRV_DEG2CYC_TMR2(Deg2); // 初期位置は中位
// UART1初期設定
U1MODE = 0b1000100000000000; // UARTENは1としてUART1を有効化、USIDLは0としてアイドルモード中も動作継続、RENは0としてIrDAエンコーダ,デコーダを無効にする、RTSMDは1としてフロー制御を用いない、UENは00としてU1TX,U1RXピンのみを有効にする、WAKEは0としてウェイクアップを無効化、LPBACKは0としてループバックモードを無効化、ABAUDは0としてボーレート計測を無効化、RXINVは0としてU1RXのアイドルを1にする、BRGHは0として低速ボーレート（1ビットの送受信に16クロック）、PDSELは00として送受信データの形式を8ビット・パリティなしとする、STSELは0として1ストップビットとする
U1STA = 0b0000010000000000; // UTXISELは00としていずれかの文字が送信シフトレジスタに転送されたとき送信割り込みが発生、IREN=0においてUTXINVは0としてU1TXのアイドル状態を0とする、UTXBRKは0として同期ブレーク送信を無効化、UTXENは1としてUART1送信を有効化、UTXBFは送信バッファが一杯かどうかのフラグなのでなんでもいい（読み出しのみ）、TRMTは送信シフトレジスタが空かどうかのフラグなのでなんでもいい（読み出しのみ）、URXISELは00として文字が受信される都度に受信割り込みフラグビットU1RXIFをセットする、ADDENは0としてアドレス検出モードを無効化、RIDELは受信アイドル中かどうかのフラグなのでなんでもいい（読み出しのみ）、PERRはパリティが検出されているかどうかのフラグなので何でもいい（読み出しのみ）、PERRはパリティエラーが検出されているかどうかのフラグなので何でもいい（読み出しのみ）、FERRはフレーミングエラーが検出されているかどうかのフラグなので何でもいい（読み出しのみ）、OERRは受信バッファのオーバーフローフラグ（クリアor読み出しのみ）なので最初は0としてクリアしておく（クリアすると受信バッファとRSRをリセットし空の状態にする）、URXDAは受信バッファに受信データが有るかどうかのフラグなので何でもいい（読み出しのみ）
U1BRG = 103; // ボーレート設定：BRGH=0の場合、BRG=Fcy/(16*ボーレート)-1で計算する。Fcy=Fosc/2の命令サイクル周波数である。ここでは、Fcy=32[MHz]/2=16[MHz],ボーレート9600bpsとし、BRG=103とする（後閑本p373より）。
// 割り込みの設定
INTCON1bits.NSTDIS = 0; // 多重割り込みを許可
INTCON2bits.ALTIVT = 0; // 標準ベクターテーブルを使用する
IEC0bits.U1RXIE = 1; // UART1の受信割り込みを有効化
IPC2bits.U1RXIP = 0b111; // UART1の受信割り込み優先度を最高に
IFS0bits.U1RXIF = 0; // UART1の受信割り込みフラグをクリア
/***** メインループ *****/
while(1) {
}
}
////////////////////////////////////////
////// 割り込み処理関数 ////////
////////////////////////////////////////
// UART1受信割り込み処理関数
void __attribute__ ((interrupt, auto_psv)) _U1RXInterrupt(void){
IFS0bits.U1RXIF = 0; // UART1の受信割り込みフラグをクリア
// データ受信(PC側から送られる受信データの形式にあわせて設定する必要がある)
if( U1STAbits.PERR==1 || U1STAbits.FERR==1 || U1STAbits.OERR==1 ){ // 受信エラーが生じたならば、エラーフラグをクリアしてUART1をリセット
// エラーフラグクリア
U1STAbits.PERR = 0;
U1STAbits.FERR = 0;
U1STAbits.OERR = 0;
// UART1停止＆有効化（UART1のリセット）
U1MODEbits.UARTEN = 0;
U1MODEbits.UARTEN = 1;
}else{ // 受信エラーが生じていなければ、受信データを元にサーボの指令角を設定する。
// 受信データをセット
data_rcv = U1RXREG;
// 受信データの形式に合わせてサーボの指令角を設定
if(0<=data_rcv && data_rcv<=126){ // もしサーボ１への指令変動値なら
Deg1 += data_rcv - 63;
// もし次の角度の絶対値が90[deg]を超えたら
if(abs(Deg1)>90){
Deg1 = sgn(Deg1)*90;
}
// サーボ１の角度変化
OC1RS = SRV_DEG2CYC_TMR2(Deg1);
}else if(127<=data_rcv && data_rcv<=253){
Deg2 += data_rcv -190;
// もし次の角度の絶対値が90[deg]を超えたら
if(abs(Deg2)>90){
Deg2 = sgn(Deg2)*90;
}
// サーボ２の角度変化
OC2RS = SRV_DEG2CYC_TMR2(Deg2);
}
}
}
PC側プログラムは画像のRGB値を取得して、R値（赤い領域）の重心と画面中心の距離を求めます。さらに縦・横方向のゲインに応じて、縦・横方向のサーボモータをどれくらい動かすのかをPD制御方式で指令値計算しています。ゲイン値は試行錯誤してよさげな値を決めています。
// Lander_ver_1_1が急にエラーが出るようになったので、プロジェクトを作りなおしたもの
// ２つのマーカーの中心を位置とし、２つのマーカーを結ぶ直線の傾きを姿勢角として検出する２次元のPADS(Position and Attitude Determination System)
// ver_1では、色による検出を行う。具体的には、色の異なる２つのマーカーを配し、それぞれに対応する色に対応する画素を検出し、それぞれで検出された画素の座標をすべて足して画素数で割ることで、対応する色として検出された画素の中心点を求める。
// 実際の使用環境におけるカラーマーカーのRGB値を確認する必要がある
// 姿勢角は、マーカー１からマーカー２へのベクトルが、角度の基準となるベクトルとのなす角（画像上時計回りを正）とする
// キャプチャサイズをWIDTH=640,HEIGHT=480など、カメラに合った大きさにしない当マイク行かない模様
// 毎回マーカーの位置、角度をリセットしているため、途中で対象が見つからなかった場合、とんでもない動きをする可能性がある。そこで、もし対象が見つからなかった場合は、前回の位置・角度を指令値決定に用いる必要があると考えられるがまだ実装してない
// clock関数はプロセスの占有時間しか計測しない（sleep中の計測を行っていない）。その一方で、time関数の精度は秒単位である。このことから、clock関数で時間計測を行った値に、sleep関数で待った時間を足せばいいかも
// Sleep関数のみを測る実験をしたところ、clock関数でもちゃんと測れていた。→むしろSleep関数しか測れなかった→測れていなかった（0msと表示された）部分に関しても、何回もループさせたらちゃんと図れるようになった。測れていなかった部分は、clock関数の分解能である1ms以下の実行時間出会ったと考えられる。
// また、ストップウォッチでclock関数の精度を確かめたところ、4分たっても一致していた（実時間が計測されていると考えて問題ない）。
// もし見つからなかったら、フラグを立てて、前回時間と各数値を同じにする処理をするといいかも
#include <stdio.h>
#include <stdlib.h>
#include <windows.h>
#include <time.h>
#include "erslib.h"
#include <math.h>
#include <cv.h>
#include <cxcore.h>
#include <highgui.h>
#define PI 3.14159265358979323846
#define WIDTH 640 // キャプチャ画像の横幅（デフォルトのキャプチャサイズ。いまのところこのサイズしかできない）/////////////////////////////////////////////////////////////////////////////////////////////////////////
#define HEIGHT 480 // キャプチャ画像の縦幅（デフォルトのキャプチャサイズ。いまのところこのサイズしかできない）/////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 実際の環境において吟味すべき値
#define TH 30 // マーカーの色判定の閾値
#define R 200 // マーカーのR成分
#define G 50 // マーカーのG成分
#define B 50 // マーカーのB成分
#define CIRCLE_RADIUS 10 // 円の半径
#define LINE_THICKNESS 2 // 線の太さ
#define LINE_TYPE 8 // 線の種類
#define BUFSIZE 50000 // シリアル通信のバッファサイズ
#define COM 7 // ポート番号
char windowNameCapture[] = "Capture"; // キャプチャした画像を表示するウィンドウの名前
int key; // キー入力用の変数
CvCapture *capture = NULL; // カメラキャプチャ用の構造体
IplImage *frameImage; // キャプチャ画像用IplImage
int Pmrk[2]; // カラーマーカーの中心座標（第一引数：i座標（横方向）、第二引数：j座標（縦方向））
int Nmrk; // カラーマーカーとして認識された画素数
int Trg[2] = {320,240}; // 目標位置
char buf[BUFSIZE]; // シリアル通信のバッファ
int prtstt = 0; // ポートの状態を入れておく
int cmd_srv[2] = {0,0}; // サーボへの角度指令変動値[deg]→第一引数：サーボ１（垂直軸周りに回転、下側のサーボ）の角度指令変動値（PIC側プログラムに依存する値：0～126）、第二引数：サーボ２（水平軸周りに回転、上側のサーボ）の角度指令変動値（PIC側プログラムに依存する値：127～253）
int Pmrk_fwd[2] = {0,0}; // １ステップ前のカラーマーカー位置（１ステップ前からどのくらいカメラが移動したかを逆算するため）
double kp1 = 0.02; // サーボ１のP制御ゲイン[deg/pixel]
double kp2 = 0.02; // サーボ２のP制御ゲイン[deg/pixel]
double kd1 = 0.04; // サーボ１のD制御ゲイン[deg/pixel]
double kd2 = 0.04; // サーボ２のD制御ゲイン[deg/pixel]
double now=0,past=0; // 時間差を計算するための変数
double dt =0.0; // 現在と一つ前のステップの時間差
int sgn(int a){
if(a>=0){
return(1);
}else{
return(-1);
}
}
int main( int argc, char **argv ){
// 書き込み用のファイルオープン
FILE *fpw;
char *fnamew = "C:/Users/dev/Desktop/SentryGun_ver_1.csv";
fpw = fopen( fnamew, "w" );
if( fpw == NULL ){
printf( "%sファイルが開けません\n", fnamew );
return -1;
}
// ポートオープン
prtstt = ERS_Open(COM, BUFSIZE, BUFSIZE);
switch(prtstt){
case 0: printf("ポートオープンが正常終了しました\n"); break;
case 1: printf("ポート番号が範囲外です\n"); break;
case 2: printf("ポートが既にオープンになっています\n"); break;
case 3: printf("装備されていない、もしくは他のアプリケーションで使われているポートです\n"); break;
}
// 通信パラメータ設定
prtstt = ERS_Config(COM, ERS_9600|ERS_1|ERS_NO|ERS_8|ERS_X_N|ERS_DTR_Y|ERS_RTS_Y|ERS_CTS_Y|ERS_DSR_N); // ボーレート9600bps、ストップビット１ビット、パリティなし、データ長８ビット、フロー制御を使わない、DTSを有効にする、RTSを有効にする、CTSを有効にする、DSRを無効にする************************************************RTSを有効にするとPCへの受信タイミングが同期されるっぽい。 CTSを有効にするとPCからの送信タイミングが同期されるっぽい（CTSを無効にした状態において、PCから３種類のパルス幅を続けて送ってLEDをPWM駆動したところ、PIC側との送受信タイミングが取れずに、LED3つが目的のパルス幅ではなく、他のLEDに指令したいパルス幅で点灯したりした。そこで、CTSを有効にしてみたところ、指令したいパルス幅でそれぞれのLEDが点灯するようになった。なお、このときPIC側では5msの待ち時間をそれぞれのパルス幅受信に設けていた。これがなくても機能するか試すこともした方がいい→なくしたらうまく行かなくなった。待ち時間は必要っぽいが、PCとマイコン双方の処理時間のタイミングがたまたま合ってるだけの可能性がある→待ち時間を変えたらまたうまく行かなくなった。やっぱりこの方法はやめて、何らかの方法で確実に同期させた方がいい）。
switch(prtstt){
case 0: printf("通信パラメータ設定が正常終了しました\n"); break;
case 1: printf("オープンされていないか、範囲外のポート番号です\n"); break;
case 2: printf("何らかの理由により設定に失敗しました\n"); break;
}
// カメラを初期化する
if ( ( capture = cvCreateCameraCapture( 0 ) ) == NULL ) {
// カメラが見つからなかった場合
printf( "カメラが見つかりません\n" );
return -1;
}
// キャプチャサイズの変更
cvSetCaptureProperty (capture, CV_CAP_PROP_FRAME_WIDTH, WIDTH);
cvSetCaptureProperty (capture, CV_CAP_PROP_FRAME_HEIGHT, HEIGHT);
// ウィンドウを生成する
cvNamedWindow( windowNameCapture, CV_WINDOW_AUTOSIZE );
// メインループ
while( 1 ) {
// captureの入力画像1フレームをframeImageに格納する
frameImage = cvQueryFrame( capture );
// 計算に用いる変数のリセット
Pmrk[0] = 0; Pmrk[1] = 0;
Nmrk = 0;
// マーカーの中心座標を計算
for(int i=0; i<WIDTH; i++){
for(int j=0; j<HEIGHT; j++){
CvScalar s = cvGet2D( frameImage, j, i ); // 画素(i,j)の画素値を取得
if( ( ( (B-TH)<=s.val[0] ) && ( s.val[0] <= (B+TH) ) ) && ( ( (G-TH)<=s.val[1] ) && ( s.val[1] <= (G+TH) ) ) && ( ( (R-TH)<=s.val[2] ) && ( s.val[2] <= (R+TH) ) ) ){ // カラーマーカーかどうかの判定
Pmrk[0] += i; Pmrk[1] += j;
Nmrk++;
}
}
}
// 時間演算
past = now; // １ステップ前の時刻を更新
now = clock()/1000.0; // 現在の時刻を更新
dt = now-past; // １ステップ前と現在の時間差を計算
if(Nmrk>0){ // カラーマーカーを検出した場合
Pmrk[0] /= Nmrk; Pmrk[1] /= Nmrk; // 「カラーマーカーとして認識された画素の座標値の総和」を、「カラーマーカーとして認識された画素数」で割ることで、カラーマーカーとして認識された画素群の中心座標を求める
// カラーマーカーの中心位置に円を描画する
cvCircle( frameImage, cvPoint( Pmrk[0], Pmrk[1]), CIRCLE_RADIUS, CV_RGB( 0, 0, 255), LINE_THICKNESS, LINE_TYPE, 0 );
// サーボ１・２の角度指令変動値算出
cmd_srv[0] = kp1*(Trg[0]-Pmrk[0])-kd1*(Pmrk[0]-Pmrk_fwd[0]);
cmd_srv[1] = kp2*(Trg[1]-Pmrk[1])-kd2*(Pmrk[1]-Pmrk_fwd[1]);
// １ステップ前の値を更新
Pmrk_fwd[0] = Pmrk[0];
Pmrk_fwd[1] = Pmrk[1];
// 角度指令変動値は-63[deg]～63[deg]で与えるため、この範囲を超えた場合に指令値を制限する
if(abs(cmd_srv[0])>63){
cmd_srv[0] = sgn(cmd_srv[0])*63;
}else if(abs(cmd_srv[1])>63){
cmd_srv[1] = sgn(cmd_srv[1])*63;
}
// 角度指令変動値を送信できる形式にする。サーボ１への指令値は0～126で指定するために元の値（-63[deg]～63[deg]）に63を足す、サーボ２への指令値は127～253で指定するために元の値（-63[deg]～63[deg]）に190を足す。
cmd_srv[0] += 63;
cmd_srv[1] += 190;
printf("cmd_srv_0 = %d cmd_srv_1 = %d\n", cmd_srv[0], cmd_srv[1]); // 算出した指令値を表示
// 指令値を送信
ERS_Putc(COM, cmd_srv[0]); // 第一信号
Sleep(1); // 10ms待つ
ERS_Putc(COM, cmd_srv[1]); // 第二信号
Sleep(1); // 10ms待つ
// 現在地をファイルに書き込み
fprintf(fpw,"%lf,%d,%d\n", now, Pmrk[0], Pmrk[1]);
}else{ // カラーマーカーを検出できなかった場合
printf("マーカーを検出できませんでした\n");
}
// 画像を表示する
cvShowImage( windowNameCapture, frameImage );
// キー入力判定
key = cvWaitKey( 10 );
if( key == 'q' ) {
// 'q'キーが押されたらループを抜ける
break;
} else if(key == 'c') {
// 'c'キーが押されたら画像を保存
cvSaveImage( "C:\\Users\\dev\\Desktop\\capturedImage.bmp", frameImage );
}
}
// キャプチャを解放する
cvReleaseCapture( &capture );
// ウィンドウを破棄する
cvDestroyWindow( windowNameCapture );
// ポートクローズ
prtstt = ERS_Close(COM);
switch(prtstt){
case 0: printf("ポートクローズが正常終了しました\n"); break;
case 1: printf("オープンされていないか、範囲外のポート番号です\n"); break;
case 2: printf("何らかの理由によりクローズに失敗しました\n"); break;
}
// 書き込み用のファイルクローズ
fclose( fpw );
return 0;
}所感
当時は電子工作を始めてから数か月経っており、勉強してきたことの区切りとして作ってみましたが、思ったよりも上手く作れました。
このシステムを応用して、セントリーガン的な物を作ろうとも思ったのですが、技術的に新しく得るものがないと判断してやめました。
ただ、それまで学んできた技術（画像処理・制御・通信・マイコン技術、、）を組み合わせて一つのモノに結び付けられたのは大きな達成感がありました。
