2軸サーボ+ビジュアルフィードバックシステム

以前(4,5年前?)に作った2軸サーボ+ビジュアルフィードバックシステムについて、、


システム構成
①カメラで画像取得→有線でPCへ
②PCで画像処理・司令値計算→無線でマイコン
③司令値に基づきマイコンでサーボをPWM制御
大まかにこの①〜③を繰り返して、物体を追従させています。
f:id:mou-tsukareta:20180225234850p:plain:w200

メカ設計
ロボットのアーム(?)的な部分はホームセンターで買ってきた、アルミ板(2mm厚だったかな?)で作りました。
曲げはポケットベンダー、切断は金切り鋏、穴あけは卓上ボール盤でやりました。3DCADは使っていませんでしたが、試行錯誤することもなく簡単に組み上がったと記憶してます。


エレキ設計
サーボ制御を行うマイコン側回路と、PCから無線を飛ばすためのPC側回路を作りました。
マイコン側回路は、マイコン(PIC24)に対してサーボモータZigbeeをつないでいます。
f:id:mou-tsukareta:20180225232244p:plain


PC側回路はZigbee+ADM3202を組み合わせて、シリアル通信を行えるようにしています。
f:id:mou-tsukareta:20180225232419p:plain


ソフト設計
マイコン側プログラムとPC側プログラムがあります。
マイコン側プログラムはUART機能を用いており、受信割り込みが発生して指令値を受け取ると、PWMのデューティー比を指令値に応じて変化させます。main関数はひたすらwhileでループさせています。

/**************************************************
*  PIC32MX I/Oテストプログラム、
*   OC1をRP4,OC2をRP5に割り当ててタイマ2を用いた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→1サイクル=2クロック=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は1度だけ変更可(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のときサーボ1を現在の角度からdata_rcv-63=-63[deg]~+63[deg]の範囲で変化させ、127<=data_rcv<=253のときサーボ2を現在の角度からdata_rcv-190=-63[deg]~+63[deg]の範囲で変化させる。
unsigned int data_rcv = 0;

// タイマ2設定値
int TMR2_PS = 8;	// プリスケーラは8倍
int Fcy = Fosc/2;	// タイマクロック[Mz]

// タイマ2・PWMモードによるサーボ制御の設定値
int srv_pwm = 15;		// サーボモーターのPWM周期[ms]=タイマ2の割り込み周期
double srv_t0 = 0.8/90;	// サーボモーターの角度を1[deg]変化させるのに必要なパルス幅の変化[ms/deg]


int Deg1 = 0;		// サーボ1の角度[deg]
int Deg2 = 0;		// サーボ2の角度[deg]


////////////////////////////////////////
///////     サブルーチン     ///////////
////////////////////////////////////////

// タイマ2を用いて、サーボモーターを中位位置から変化させたい角度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;		//	クロックをコンフィギュレーションにおける設定値の1倍に		

	// アナログポートの設定(アナログ機能を持つピン(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を割り当て

	
	// タイマ2初期設定(タイマクロック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){	// もしサーボ1への指令変動値なら
			
			Deg1 += data_rcv - 63;	
			
			// もし次の角度の絶対値が90[deg]を超えたら 	 	
			if(abs(Deg1)>90){
				
				Deg1 = sgn(Deg1)*90;
				
			} 
			
			// サーボ1の角度変化
 	 		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;
				
			} 
			
			// サーボ2の角度変化
 	 		OC2RS = SRV_DEG2CYC_TMR2(Deg2);
		
		}
		
	}		
	
}


PC側プログラムは画像のRGB値を取得して、R値(赤い領域)の重心と画面中心の距離を求めます。さらに縦・横方向のゲインに応じて、縦・横方向のサーボモータをどれくらい動かすのかをPD制御方式で指令値計算しています。ゲイン値は試行錯誤してよさげな値を決めています。

//	Lander_ver_1_1が急にエラーが出るようになったので、プロジェクトを作りなおしたもの
//	2つのマーカーの中心を位置とし、2つのマーカーを結ぶ直線の傾きを姿勢角として検出する2次元のPADS(Position and Attitude Determination System)
//	ver_1では、色による検出を行う。具体的には、色の異なる2つのマーカーを配し、それぞれに対応する色に対応する画素を検出し、それぞれで検出された画素の座標をすべて足して画素数で割ることで、対応する色として検出された画素の中心点を求める。
//	実際の使用環境におけるカラーマーカーのRGB値を確認する必要がある
//	姿勢角は、マーカー1からマーカー2へのベクトルが、角度の基準となるベクトルとのなす角(画像上時計回りを正)とする
//	キャプチャサイズを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]→第一引数:サーボ1(垂直軸周りに回転、下側のサーボ)の角度指令変動値(PIC側プログラムに依存する値:0~126)、第二引数:サーボ2(水平軸周りに回転、上側のサーボ)の角度指令変動値(PIC側プログラムに依存する値:127~253)
int Pmrk_fwd[2] = {0,0};	// 1ステップ前のカラーマーカー位置(1ステップ前からどのくらいカメラが移動したかを逆算するため)
double kp1 = 0.02;	// サーボ1のP制御ゲイン[deg/pixel]
double kp2 = 0.02;	// サーボ2のP制御ゲイン[deg/pixel]
double kd1 = 0.04;	// サーボ1のD制御ゲイン[deg/pixel]
double kd2 = 0.04;	// サーボ2の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、ストップビット1ビット、パリティなし、データ長8ビット、フロー制御を使わない、DTSを有効にする、RTSを有効にする、CTSを有効にする、DSRを無効にする************************************************RTSを有効にするとPCへの受信タイミングが同期されるっぽい。    CTSを有効にするとPCからの送信タイミングが同期されるっぽい(CTSを無効にした状態において、PCから3種類のパルス幅を続けて送って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;		// 1ステップ前の時刻を更新
		now = clock()/1000.0;		// 現在の時刻を更新
		dt = now-past;		// 1ステップ前と現在の時間差を計算

		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 );

			// サーボ1・2の角度指令変動値算出
			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]);

			// 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;

			}

			// 角度指令変動値を送信できる形式にする。サーボ1への指令値は0~126で指定するために元の値(-63[deg]~63[deg])に63を足す、サーボ2への指令値は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;
}

所感
当時は電子工作を始めてから数か月経っており、勉強してきたことの区切りとして作ってみましたが、思ったよりも上手く作れました。
このシステムを応用して、セントリーガン的な物を作ろうとも思ったのですが、技術的に新しく得るものがないと判断してやめました。
ただ、それまで学んできた技術(画像処理・制御・通信・マイコン技術、、)を組み合わせて一つのモノに結び付けられたのは大きな達成感がありました。