色を検知して車体制御

E4電子制御実験 2011年度
色を検知して車体制御
センサーの信号を元に車体を動かす
ハンドルを切る
ピンとモータ（M）を接続する極性に
（黒）を判定する。各センサーの検知
よって実際の回転方向は変る。
内容に応じて車体の動きを切り替える
ことで，ライントレースが実現でき
S2
S1
例1）MDの出力ピンとMの接続極性
の組合せをM1，M2で同じにすればマ
イコンからの出力は，MD1とMD2が
逆回転するように与えれば前進。
車体
M1
M2
例2）MDの出力ピンとMの接続極性
の組合せを，M1，M2で逆にするとマ
イコンからの制御出力は，MD1，
車体の駆動方式：独立した二個のモー
タによる二輪駆動。進行方向の変更
MD2同じ向きで前進となる。例2で，
以下のピン接続なら，
は，二個のモータの回転方向や速度を
変えることで行う。
動き
M1
M2
直進
CCW
CW
右折
CCW
ー
左折
ー
CW
後進
CW
CCW
モータドライバ
マイコン
MD 1 IN 1 (5)
RB3
MD 1 IN 2 (6)
RB2
MD 2 IN 1 (5)
RB1
MD 2 IN 1 (6)
RB0
る。S1，S2の検知内容と車体動作の
対応を下図に示す。これ以外に両方と
も黒を検知したら，少しバックさせる
などの処理も必要となる。
例えば，s1，s2でセンサを指定して各
センサーの電圧をA/D変換し，白黒判
定した結果を白なら1，黒なら0で返
す関数 detect がある場合には，
unsigned char out = 0x00 ;
int a1, a2 ; // A/D変換結果保管用
while (1) {
a1 = detect ( s1 ) ; // A/D変換結果
a2 = detect ( s2 ) ;
if ( a1 && a2 ) // 両方白
out = FW ;
else if ( a1 && ! a2 ) // S2が黒
unsigned char
STOP = 0b00000000, // 停止
out = RT ;
else if ( ! a1 && a2 ) // S1が黒
CW：時計回り，CCW：反時計回り，
FW = 0b00000101, // 前進
−：無回転もしくは低速回転
RT = 0b00000100, // 右折
out = LT ;
else
out = BK ;
PORTB = out ;
if ( out != BK )
__delay_ms(20) ; // BK以外
LT = 0b00000001, // 左折
BK = 0b00001010; // 後進
の信号をPORTBに与えることで，そ
モータドライバの駆動と出力信号
れぞれの動作となる。（実際には，自
PORTBの各ピンとモータドライバ
分たちのマシンに合わせること。）
S2
S1
力のパターンは変る。またMDの出力
車体
M1
センサーからの情報と連動させる
センサ（S1，S2）の出力をA/D変換
車体
M2
}
のようになる。
して，床色（白）とコースライン
S2
対応関係によってマイコン側で出す出
S1
（TP7291P，以下MD）の入力ピンの
else
__delay_ms(10) ; // BKの場合
M1
S2
たコースの色判定情報。
S1
外部要因：反射型フォトセンサを使っ
車体
M2
M1
M2
黒ラインをまたいでいる場合：S1，S2と
黒ラインの上に来た場合 (1)：S1は白検
黒ラインの上に来た場合 (2)：S1は黒検
もに白検知。直進させる。
知，S2は黒検知。右折させる。
知，S2は白検知。左折させる。
サンプリング間隔
サンプリング間隔が長すぎると
の無限ループを作った。このループ一
S2
S1
前述のサンプルコードでは，while (1)
両方とも白だから
回に必要な処理時間を考えてみる。if文
車体
や代入などの処理時間は一行あたり二
直進だー！！
M1
命令サイクル時間（一命令サイクル
M2
残念
は，クロック周波数の1/4）なので μ
sec オーダで処理が終わる。A/D変換は
一回あたり最大12 μ sec なので，これ
が関数化されてデータやり取り処理の
S2
S1
時間がかかっても数十 μ sec 程度であろ
う。二回呼び出しても合計100 μ sec 程
度と思われる。__delay_ms( 20 ) や
車体
直進だー！！
__delay_ms( 10 ) は，そのまま20 m
M1
M2
sec，10 m sec の処理時間となる。
従って while (1) の無限ループの繰り返
し時間は，__delay_ms で指定した時間
でほぼ決まることになる。
while ループの繰り返し時間は，ライ
ン検知の時間間隔（サンプリング周期
Ts ）を決めることになるため，適切な
値を検討しなければならない。サンプ
リング間隔が長過ぎた場合には，ライ
ンの曲がりを検知できずに通り過ぎる
し，間隔が短すぎた場合にはライン検
知処理に対して走行時間が短くなるた
め動作が遅くなる。
減速比をGr，駆動するタイヤの直径を
テープを確実に検知するためには，
φとすると車体の速度 v は，以下の式で
テープ通過に必要な時間 Tpよりサンプ
表される。
リング間隔 Ts が短くなければならな
v=
い。
N
[m/sec]
60Gr
1.5 V 印可時の定格回転数 N = 6,400
rpm，Gr = 203.7，φ = 36 mm の場合
v = 5.9 cm / sec となる。実際には，ギ
アによる機械的な損失や車体重量等の
本実験で使用するテープ幅は 1.9 cm だ
からTp = 1.9 / 5.9 = 0.32 sec = 320 m
sec と見積もれるから，Ts=20 m sec
でも Tp > Ts が成立する。
負荷が追加されるため速度は低下する
モータの回転数と車体の速度
が，MDからモータに加えられる電圧は
実験で用いるFA-130モータの1.5 V 印
1.5 V より高ければ速度は早くなるため
可時の定格回転数をN，ギアボックスの
v は5.9 cm / sec で一定ではない。
マイコン，MD, M どう繋ぐ？
MD1
MD1
M1
M1
PIC
PIC
MD2
M2
検討／チェック項目
センサーの検知方式
MD2
M2
PICと周辺機器との接
続ピン
MDとMの接続極性
接続例１）直進の場合
接続例２）直進の場合
M1とM2の接続極性
MD-M間の結線がM1, M2で同じだ
MD-M間の結線がM1, M2で逆だが，
サンプリング間隔と機
が，PICからの信号はM1, M2で逆方
PICからの信号は同じ向きに回転す
PI制御などを導入する
？
向回転するように出す必要がある。
るように出せば自動的に逆回転。
動性

Download Report