コマンド方式サーボKRS-6003HVの制御 その2
プログラム
#include "mbed.h" Serial pc(USBTX, USBRX); Serial master(p9, p10); AnalogIn an(p15); void setPosition(int in) { unsigned char h, l; int temp1, temp2; temp1 = in >> 7; temp2 = in & 0x7F; h = (unsigned char)temp1; l = (unsigned char)temp2; master.putc(0x82); // ID = 2 master.putc(h); master.putc(l); } int main(void) { master.baud(115200); master.format(8, Serial::Even, 1); // data length = 8bit, parity = even, stop bit = 1bit int val; while(1) { val = an * 10000 + 3500; setPosition(val); wait(0.01); // wait 10ms } }
実装詳細
1kΩの可変抵抗を使い、mbedに0〜2.5Vが入力されるような回路を組みました。mbed側では0〜3.3Vしか読み取れないためです。
一方でサーボへは位置データを3500〜11500の範囲で送る必要があります。そこで、mbed側では0〜3.3Vが0.0〜1.0という値として読み取れるので、ADCの値×10000+3500という数値をサーボへ送ることにしました。こうすることで、0〜2.5Vの電圧値が3500〜11076という位置データに対応します。
その1の方で書いたように位置データは分割して2byteにして送る必要があるため、setPosition()という関数を作り、ここで2byteのデータに変換しています。上位7bitは7bit右シフトさせて作り、下位7bitは0x7Fとの論理積を取ることで得ています。この辺りの解説は近藤科学のホームページに詳しく載っています。
シリアルサーボ制御方法(3) ソフト編
http://kondo-robot.com/sys/archives/2671
また、前回はmbedのSerialHalfDuplexクラスを利用していましたが、サーボ側から返ってくるデータは別にいらないのでは?と思い、ただサーボに対してシリアル通信でデータを投げるだけに変更しました。回路側でもmbedのRXピン(pin10)を抵抗を介して5Vに接続するよう変更しました。
まとめ
以上の変更を加えた結果、可変抵抗の値に応じてサーボの位置が変化するようになりました。シリアル通信も投げっぱなしで問題ないようです。
KRS-6003HVは可動域が270°あるようですが、mbedから取れる5Vの電圧が5Vよりも若干低いため、270°フルには回りませんでした。また、感度が高すぎるためサーボが常に細かく振動するような状態になっているのでその辺りも改善の余地がありそうです。