ジャイロを買って、サーボも買って、mbedもかって・・・
ｼﾘｱﾙ用の回路つけてなんにもしてない日々が流れていたので、ちょっとあそびました。
◆まずはじめに、近藤の半2重通信をmbedにどうつなぐかです。
回路図はこんな感じ
mbedからの信号を5Vにしつつ半2重通信をします。
なぜそういうことをしようと思ったのか、それはもう思い出せません。
↑は赤がサーボのピン、緑がマイコンのRXD、青がマイコンのTXDそれぞれの電圧です。
マイコンから出た信号がマイコンに入っていますが、まー読まないから関係ないと思います？？
ほんとかなぁ。
◆ほんでもって、ICS3.5をさーぼとやるので、ｼﾘｱﾙをはいてやる必要があります。
コマンド方式サーボKRS-6003HVの制御 その1 こちらのプログラムを参考にさせていただきました。
また、本家のリファレンスも参考にしました。
ざっくり、こんな感じ
Serial ko(p9,p10);//txrx void setServoDeg(float deg)//+-135deg { deg=deg+135; if(deg>270)deg=270; if(deg<0)deg=0; targetPosi = 3500 + (int)((deg*800)/27); } //↑3500~11500で0～270°動きます。
float getServoDeg(void) { return (((float)(feedbackPosi-3500)*27)/800)-135; } /*----------------------------------*/ /* KONDOと角度 */ /*----------------------------------*/ int set_posi(char id, int pos) { unsigned char tx[3]; unsigned char rx[6]; int i; int data=0; tx[0] = 0x80 | id; tx[1] = (unsigned char)( (pos >> 7) & 0x7f); tx[2] = (unsigned char)(pos &0x7f); for(i = 0; i<3; i++){ ko.putc(tx[i]); } //wait(16); for(i = 0; i<6; i++){ rx[i] = ko.getc(); } data = (int)(rx[4] & 0x7f); data = (data << 7) + (int)(rx[5] & 0x7f); return data; } int main(){ ko.baud(115200); ko.format(8, Serial::Even, 1); setServoDeg(もくひょうかくど); feedbackPosi=set_posi(1, targetPosi); ↑それっぽいところだけ抜粋。
変数の宣言とか足りないと思う。
◆動かすだけじゃつまらないので、ジャイロとコラボしました。
カメラのジンバルっていうか、そんな感じの動画。
ジャイロの角速度"積分値"を、角度として入力すると、
回路の動いた角度だけ、サーボは逆転動作します。
つまり動いてないかのよう。
今回は5msごとにジャイロ取得、サーボに指令してます。
積分は何にも考えないで、
Σ(現在のジャイロ値×0.005)
です。
VIDEO ジャイロは角速度を電圧にするんですけれでも、
ドリフトっていって動いてない時の電圧がだんだん増えていく弱点があります。
そこで、ハイパスフィルターとか淹れてみようって準備しました。
けど、動画の通りそこまで必要じゃなかったかっていう。
◆ハイパスフィルター
ゆっくりした変化を無視する方法がハイパスフィルターです。
こちらのプログラムを参考にしました。
簡単なデジタルフィルタの実装 ざっくりこんな感じで真似させていただきました。
// それぞれの変数は下記のとおりとする // float samplerate … サンプリング周波数 // float freq … カットオフ周波数 // float q … フィルタのQ値 float omega,alpha,a0,a1,a2,b0,b1,b2; float freq=.01; float q=0.7071; float samplerate=200; /*----------------------------------*/ /* ハイパスフィルターセット */ /* Data入力 */ /*----------------------------------*/ void setHPF(void) { omega = 2.0 * 3.14159 * freq/samplerate; alpha = sin(omega) / (2.0* q); a0 = 1.0 + alpha; a1 = -2.0 * cos(omega); a2 = 1.0 - alpha; b0 = (1.0 + cos(omega)) / 2.0; b1 = -(1.0 + cos(omega)); b2 = (1.0 + cos(omega)) / 2.0; } float in1=0,in2=0,out1=0,out2=0; /*----------------------------------*/ /* ハイパスフィルタ出力 */ /* Data入力 */ /*----------------------------------*/ float outHPF(float input) { // それぞれの変数は下記のとおりとする // float input[] …入力信号の格納されたバッファ。 // flaot output[] …フィルタ処理した値を書き出す出力信号のバッファ。 // int size …入力信号・出力信号のバッファのサイズ。 // float in1, in2, out1, out2 …フィルタ計算用のバッファ変数。初期値は0。 // float a0, a1, a2, b0, b1, b2 …フィルタの係数。 別途算出する。 // 入力信号にフィルタを適用し、出力信号として書き出す。 float output; output = b0/a0 * input + b1/a0 * in1 + b2/a0 * in2 - a1/a0 * out1 - a2/a0 * out2; in2 = in1; // 2つ前の入力信号を更新 in1 = input; // 1つ前の入力信号を更新 out2 = out1; // 2つ前の出力信号を更新 out1 = output; // 1つ前の出力信号を更新 return out1; } ハイパスフィルターですから動画見たいに、動きが無くなる＝高周波成分は0になろうとします。
普通はローパス分を別のセンサーとかを入れて相補するっぽいですが難しいことは・・・。
◆◆◆想像以上に積分値安定してるんで感動した。
[0回]