RCサーボをPID制御する実験。

制御基盤を取り除き、プログラムで制御してみる。

まずは、パーツの入手


　物がないと始まらないので、まずはパーツを購入しに秋葉原へ。
必要なものはFET類とRCサーボモーター、H8マイコンボードです。
H8とRCサーボは既に入手しているので、FETを買いに行きます。


商品名 単価 個数 販売店
AKI-H8/3052F \3,900 1 秋月電子通商
基板（小） \70 1 秋月電子通商
2SJ377 \90 2 千石電商
2SK2231 \100 2 千石電商
2SK1132 \40 2 光南電気
TC74AC08P \50 1 千石電商
TC74HC14P \50 1 千石電商
抵抗器5.1KΩ \1 2 秋月電子通商、千石電商
抵抗器330Ω \1 2 秋月電子通商、千石電商
S3003 \1,700 1 スーパーラジコン

※光南電気は、東京ラジオデパート内1F、2Fにあります。
これで、全部揃いました。FETに関しては宮田さんのところで紹介されているものと同じです。

商品名	単価	個数	販売店
AKI-H8/3052F	\3,900	1	秋月電子通商
基板（小）	\70	1	秋月電子通商
2SJ377	\90	2	千石電商
2SK2231	\100	2	千石電商
2SK1132	\40	2	光南電気
TC74AC08P	\50	1	千石電商
TC74HC14P	\50	1	千石電商
抵抗器5.1KΩ	\1	2	秋月電子通商、千石電商
抵抗器330Ω	\1	2	秋月電子通商、千石電商
S3003	\1,700	1	スーパーラジコン

組み立て1「モータードライブ回路」（半田付け）



　部品は入手したので、これから実験に必要な物を作っていきます。
なお、H8用のマザーボードは既に作られていることを想定して話を進めていきますので、
ご了承ください。

　まずは、モータードライブ回路を製作していきます。
宮田さんの回路を参考に（というか、そっくりそのまま）作らせて頂きました。
以下は実装したものです。







図2-1 図2-2 写真2-1 写真2-2

GND：GNDに接続します。
VCC：モーター用電源の＋側に接続します。
モーター1：モーター端子に接続します。
モーター2：モーター端子に接続します。
信号1：図3-1のOUT1に接続します。
信号2：図3-1のOUT2に接続します。

　図2-1は表から見た基板面です。（実際：写真2-1）図2-1を見ると、FETが４つ並んでいるのが
分かると思います。（灰色の部分はFETの放熱板です。）取り付けるときは、注意してください。
配線ミスすると、簡単に燃えます。

　余談ですが自分は、小さいFETの2SK1132の取り付けを間違え燃えました。（苦笑）

　図2-2は裏から見た基板面です。（実際：写真2-2）特に場所を取らないのであれば、
このように小型化しなくてもいいと思います。（抵抗器の取り付けが大変）
図と写真を見れば、無茶やってることがわかります。（一般的に、これが普通？？？）


図2-1	図2-2	写真2-1	写真2-2

組み立て2「CMOS」（半田付け）



　マイコンのポートから入出力できる電流では足りないらしいので、CMOSの74ACシリーズを使います。
この74ACシリーズは24mA流せるので大丈夫そうです。
CMOS使うので少々細工を施しておきました。（これが仇となっていなければいいのですが。）
以下が今回使った回路図です。






図3-1 写真3-1


IN1：PWM信号を発生できるPAの2bit目に接続しています。
IN2：近場のPAの3bit目に接続しています。
OUT1：モータードライブ回路（図2-1参照）の信号１に接続します。
OUT2：モータードライブ回路（図2-1参照）の信号２に接続します。

　IN1にPWM信号を送り、正転、逆転の比率でモーターを制御するようにしてあります。
IN2に「０」を入力すると、モーターは動かせますが、
「１」を入れると、正転、逆転させようとしても動きません。（ブレーキではありません。）

AND回路は最終段なので、74AC08（24mA流せるCMOS）を使い、前段のNOT回路には74HC14を
使っています。なぜ74HC04でないかというと、ただ単に74HC14があったからです。


　最後に、RCサーボ内のボリュームとマイコン側のAN0に接続すれば、作業は終わりです。

補足：写真3-1の上方に写っているモータードライブ回路やアダプタ用端子は、最初に作った
ものは燃えてしまったものです。名残として残っています。
（面倒くさいので、CMOSの回路をそのまま使った人←）


図3-1	写真3-1

プログラム編



　さぁ、問題のプログラム編です。中身は意外にも簡単なものでした。
が、いろいろトラブルがあり、しかも最後の最後でFET片側（1～3個？）燃えたため、
非常に汚いままになってしまいました。でも、載せないよりはマシなので、載せちゃいます。



#include <3052f.h>
#include "sci1.h"
#include "print.h"

#define ChangeAscii 0x30

long servo_tag = 512;
long mt_duty;
int mt_d0 = 12500;
int servo_r1;
int servo_r2;
int servo_r3;

int P, I, D, Kp = 130, Ki = 75, Kd = 50;

int b = 0;

main()
{
	
	P5.DDR = 0x00;
	PA.DDR = 0xff;
	P5.PCR.BYTE = 0x0f;
	
	ITU0.TCR.BYTE = 0x40;
	ITU0.GRA = 0;
	ITU0.GRB = 25000;
	ITU.TMDR.BIT.PWM0 = 1;
	ITU.TSTR.BIT.STR0 = 1;
	
	AD.CSR.BYTE = 0x33;      /* SCAN MODE / CH0～3　*/
	while(AD.CSR.BIT.ADF == 0); /* 4CH分の変換終了を待つ */
	PA.DR.BIT.B3 = 0;
	
	while(1);
	
	while(1)
	{
		int a0, a1, a2, a3, s, i1, i2;
		/*
		servo_r1 = AD.DRA>>6;
		
		if(P5.DR.BIT.B0 == 0) servo_tag = 0;
		else if(P5.DR.BIT.B1 == 0) servo_tag = 180;
		else if(P5.DR.BIT.B2 == 0) servo_tag = 45;
		else if(P5.DR.BIT.B3 == 0) servo_tag = 135;
		else servo_tag = 90;
		*/
		
		if(P5.DR.BIT.B0 == 0) servo_tag = 0;
		else servo_tag = 90;
		if(P5.DR.BIT.B1 == 1 && P5.DR.BIT.B1 != a1)
		{
			a1 = 1;
			if(b == 0) Kp--;
			else if(b == 1) Ki--;
			else if(b == 2) Kd--;
		}
		
		if(P5.DR.BIT.B2 == 1 && P5.DR.BIT.B2 != a2)
		{
			a2 = 1;
				 if(b == 0) Kp++;
			else if(b == 1) Ki++;
			else if(b == 2) Kd++;
		}
		if(P5.DR.BIT.B3 == 1 && P5.DR.BIT.B3 != a3)
		{
			b++;
			a3 = 1;
		}
		
		if(b >= 3) b = 0;
		if(P5.DR.BIT.B1 == 0) a1 = 0;
		if(P5.DR.BIT.B2 == 0) a2 = 0;
		if(P5.DR.BIT.B3 == 0) a3 = 0;
		
		printn(Kp);
		if(b == 0) sci1_tx('<');
		printn(Ki);
		if(b == 1) sci1_tx('<');
		printn(Kd);
		if(b == 2) sci1_tx('<');
		
		printn(servo_r1);
		printn(( servo_r1 - 112) * 10 / 44);
		
		servo_tag = 112 + ( servo_tag * 44 ) / 10;

		P = Kp * (servo_r1 - servo_tag) ;
		I =  Ki * ( ( servo_r2 - servo_r3 ) + ( servo_r1 - servo_r2 ) ) ;
		D = Kd * (servo_r2 - servo_r1);

		mt_duty = mt_d0 + P + I + D;
		printn(mt_duty);
		
		if(mt_duty < 0) mt_duty = 0;
		if(mt_duty > 24999) mt_duty = 24999;
		ITU0.GRA = mt_duty;
		printn(mt_duty);
		sci1_tx('\r');
		
		servo_r3 = servo_r2;
		servo_r2 = servo_r1;
	}
}





　なかなか、長いですが、ゲイン調整用のプログラムも混じっているのでそんなに長くありません。

	P5.DDR = 0x00;
	PA.DDR = 0xff;
	P5.PCR.BYTE = 0x0f;
	
	ITU0.TCR.BYTE = 0x40;
	ITU0.GRA = 0;
	ITU0.GRB = 25000;
	ITU.TMDR.BIT.PWM0 = 1;
	ITU.TSTR.BIT.STR0 = 1;
	
	AD.CSR.BYTE = 0x33;      /* SCAN MODE / CH0～3　*/
	while(AD.CSR.BIT.ADF == 0); /* 4CH分の変換終了を待つ */
	PA.DR.BIT.B3 = 0;


　上記の部分は初期化するためのプログラムです。注意点はここ「PA.DR.BIT.B3 = 0;」です。
１にすると、モーターが動いてくれません。

　他の部分については、TEKUROBO工作室に詳しいことが載っています。



1:	servo_r1 = AD.DRA>>6;
2:
3:	～　省略（調整用プログラム）　～
4:
5:	servo_tag = 112 + ( servo_tag * 44 ) / 10;
6:
7:	P = Kp * (servo_r1 - servo_tag) ;
8:	I = Ki * ( ( servo_r2 - servo_r3 ) + ( servo_r1 - servo_r2 ) ) ;
9:	D = Kd * (servo_r2 - servo_r1);
10:
11:	mt_duty = mt_d0 + P + I + D;
12:	printn(mt_duty);
13:	
14:	if(mt_duty < 0) mt_duty = 0;
15:	if(mt_duty > 24999) mt_duty = 24999;
16:	ITU0.GRA = mt_duty;
17:	printn(mt_duty);
18:	sci1_tx('\r');
19:	
20:	servo_r3 = servo_r2;
21:	servo_r2 = servo_r1;


ちょっと行が多いので番号を振りました。
１行目はAD変換した値を変数servo_r1に格納しています。（値は0～1023が入ります。）

５行目は、servo_tagの変換を行っています。将来角度を入力しようと考えているので、
このような計算が必要になりました。（精度はよくないです。）

そして、P・I・D制御の各計算式を入れます。
７行目：比例制御Pの計算式は、比例定数　× (現在の位置-目標値）です。
８行目：積分制御Iの計算式は、比例定数　× ((１つ前の位置-２つ前の位置)+(現在の位置-１つ前の位置))です。
９行目：微分制御Dの計算式は、比例定数　× (現在の位置-１つ前の位置)です。

１１行目：モーターの停止するPWMの値とP＋I＋Dを足します。
25000の半分の値がちょうどモーターの停止（正転と逆転が同じ）になるので、１２５００と設定されています。

１４行目：デューティ比がマイナスになることはないので、０より小さい値は０に設定しなおします。
１５行目：25000以上は存在しないので、24999に設定しなおします。
補足：今回は25000などの値を用いているため、比較的オーバーフローしやすいです。
なので、LONG型の変数を用いています。

１６行目：ITU0.GRA にディーティ比を加えて設定が終わります。

２０行目、２１行目：現在の値、一つ前の値を移動します。


　こんな感じでした。後は各ゲインを設定すれば、きちんと動いてくれます。

参考文献



※Miyata's Robot Factory
※TekuRobo工作室
※電子工作の実験室
※自立型ロボット製作バイブル「オーム社」

あとがき・・・



　ここまでお付き合いくださいまして、ありがとうございました。
ここからは、いろいろとお話をさせて頂こうかと思います・・・（＾＾；

　なんで、今ごろになってやり始めたかというと、第４回でのショックが大きかった事ですかね。
「電源が貧弱だから動かなかった！」と信じて設計したのにまた動かなかった。（大会当日）
今まで以上にテンションが下がりました。（笑）

　で、アナログサーボのPS2174FETのサーボホーンに棒を取り付けて、おもりをつるしてみた時。
約６ｋｇの力を加えただけなのに、40°以上傾いて止まりました。
一応、12kgまでは耐えられるはずなんですけどね。で、約12kgを吊るしてみたところ・・・
90°オーバー・・・てか、支えられてないし・・・（泣）

　というわけで、アナログサーボの制御基板じゃ駄目だ・・・と気づいた訳であります。
二足ロボ作り始めて1年半というところでしょうか・・・

　そこで、もう一度PID制御に挑戦してみようと、心に誓った訳であります。
「しかし、今まで散々挑戦してできなかったPID制御・・・できるのか！？」
そんな訳で、しばらく取り掛かれずに時間が過ぎていった・・・

ある日、ふとPWMの事で思い出した。

　「今まではPWMは自力で作ろうとしていたのから、失敗していたに違いない！」
「ならば、実際にPWM使ってやればできるのでは？？？いける・・・かも？」

9月の半ばを過ぎた時の出来事だった。

さっそく、秋葉原で各パーツを買出し、組み上げ、実行！

しかし、バーニング！！も・・・もえたぁ！！（前にもFET使ったときに燃えたので、驚きはナシ）
二度と経験したくなかったが、煙がモクモクと・・・


↑のFET郡から出火（泣）

で、どうせ作り直すなら・・・というわけで、小型化に挑戦！
ちゃんと動作しました。

プログラム中にどうしても納得できない出来事が・・・
それは何かというと。

電源を変えると動かない！！

　実験の時に使用したのは、PC用の電源で、12Vをマイコン用の電源に、5Vをモーター用電源として、
使っていたのですが、これらの片方を秋月で買ってきた電源用アダプタを使うと、動かなかったり、
別の動作をしたりします。この辺の原因も解明できないと、実用化できない。

　しかし、PWMのおかげですんなりと比例制御、微分制御、積分制御までできたので、
楽でした。PWM最高～。（数が多ければ・・・）

で、S3003の最大トルクを実験してみようとしたら、どこでどう間違ったのか、バーニング。
う～ん・・・良く分からん。（片方死亡）

あと、実験中に気になったのですが、モーターがそこそこ熱くなりましたけど、
みなさんもそうなんですかね？（気になるところ。）

正転と逆転の比率でスピードUP,スピードDOWN、ストップは駄目だったかなぁ・・・

今後の課題は、安定化（燃えないとか）、タイニーでの制御とか、PWM増殖化などですね。
FPGAやCPLDだと間に合わないような気がするので、タイニーでがんばってみようかと。
マイコン付属のADコンバーターでどこまでできるのかっ！？（１°制御できるなら良いかなと）

というわけで、駄文でした！ｍ（＿＿）ｍ

[TOP]

SEO		[PR] 爆速!無料ブログ無料ホームページ開設無料ライブ放送