SIMPLE TERMからのキー入力によって、カメラを動かす方向を変えます。 キーボードのテンキーを使って6なら右、4なら左、8なら上、2なら下へ動き、0でブートローダへ移行します。
GCC Developer Liteを起動します。以下基本的な部分を入力します。
#include <fd.h> void main (void) { fd_Init (0, BT_CONSOLE, FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP, 7.4); }
#define KEY_QUIT { if(fd_rx_buff()) fd_SoftReset();}
は、何かキー入力があったらブートローダに戻るという記述ですが、今回はキー入力を使うのでKEY_QUITは使えません。 キー入力が0ならfd_SoftReset関数を実行するようにします。
入力された文字を取得する変数cと、パン(横振り)・チルト(上下振り)のポジション用に使う変数PanPositionとTiltPositionを用意します。
char c; uint16_t PanPosition = 512, TiltPosition = 512;
AX-12+のホーンが丁度真ん中になる512に初期化します。
ID=1と2のAX-12+のトルクリミット(アドレス34,35)を400にします。
fd_Initの後に以下を入力します。
fd_DXWriteWordData (1, 34, 400); fd_DXWriteWordData (2, 34, 400);
AX-12+のホーンを最初のポジションへ移動します。ゴールポジションはアドレス30,31です。
トルクリミットの設定の後に以下を入力します。
fd_DXWriteWordData (1, 30, PanPosition); fd_DXWriteWordData (2, 30, TiltPosition);
文字入力を一文字取得します。
初期ポジションへの移動の後に以下を入力します。
while (1) { if (fd_rx_buff() > 0) c = fd_getc (); }
fd_rx_buffは、入力された文字数を返す関数です。fd_getcは入力された文字を1文字取得する関数です。 つまり、「もし入力された文字数が0より大きければ(文字が入力されたら)、1文字取得して変数cに代入する」となります。
取得した文字によって条件を分岐するためにswitch文を使用します。 キーボードのテンキーを使って上下左右にカメラを動かします。
while文内の文字取得の後に以下を入力します。
switch (c) { case '6' : // 右へ break; case '4' : // 左へ break; case '8' : // 上へ break; case '2' : // 下へ break; case '0' : // ブートローダへ break; }
キーボードで6を入力すると右へ、4で左へ、8で上へ、2で下へ動くように、0でブートローダへ移行するようにプログラミングします。
もし6が入力されたらPanPositionを1マイナスし、ポジションを送信します。
もし4が入力されたらPanPositionを1プラスし、ポジションを送信します。
switch文内のcase '6'とcase '4'に以下を入力します。
case '6' : PanPosition --; fd_DXWriteWordData (1, 30, PanPosition); break; case '4' : PanPosition ++; fd_DXWriteWordData (1, 30, PanPosition); break;
取り付け方によってはプラスマイナスが逆になるかもしれません。
もし8が入力されたらTiltPositionを1マイナスし、ポジションを送信します。
もし2が入力されたらTiltPositionを1プラスし、ポジションを送信します。
switch文内のcase '8'とcase '2'に以下を入力します。
case '8' : TiltPosition --; fd_DXWriteWordData (2, 30, TiltPosition); break; case '2' : TiltPosition ++; fd_DXWriteWordData (2, 30, TiltPosition); break;
取り付け方によってはプラスマイナスが逆になるかもしれません。
ブートローダへ移行する関数はfd_SoftResetです。
switch文中のcase '0'にfd_SoftReset関数を入力します。
case '0' : fd_SoftReset(); break;
フレームが干渉してしまうようなポジションを送信すると、AX-12+に過大な負荷がかかり、破損する場合があります。そのようなポジションを送信しないよう、ポジションにリミッターを設けます。
各caseに対し以下のように追記します。
case '6' : PanPosition --; if (PanPosition < 0) PanPosition = 0; fd_DXWriteWordData (1, 30, PanPosition); break;
PanPositionが0より小さいなら、PanPositionを0にします。
case '4' : PanPosition ++; if (PanPosition > 1023) PanPosition = 1023; fd_DXWriteWordData (1, 30, PanPosition); break;
PanPositionが1023より大きいなら、PanPositionを1023にします。
case '8' : TiltPosition --; if (TiltPosition < 200) TiltPosition = 200; fd_DXWriteWordData (2, 30, TiltPosition); break;
TiltPositionが200より小さいなら、TiltPositionを200にします。 取り付け方によっては、数値が異なります。フレームが干渉しないように十分確認して下さい。
case '2' : TiltPosition ++; if (TiltPosition > 800) TiltPosition = 800; fd_DXWriteWordData (2, 30, TiltPosition); break;
TiltPositionが800より大きいなら、TiltPositionを800にします。 取り付け方によっては、数値が異なります。フレームが干渉しないように十分確認して下さい。
PanPositionとTiltPositionの値を表示します。
while文内に以下を入力します。
fd_printf("Pan:%5d Tilt:%5d\r", PanPosition, TiltPosition);
ここでプログラムを実行してみましょう。
6を入力すると、PanPositionがだんだん減って0になり、その後65535という大きな数字になってしまいます。 これは変数の型がuint16_tだからです。uint16_tはC言語でいうところのunsigned shortと同じで、値の範囲は0~65535です。そのため0から1マイナスすると65535になるのです。
以下のように、if文の条件を「PanPositionが60000より大きければ」に変えてみましょう。
case '6' : PanPosition --; if (PanPosition > 60000) PanPosition = 0; fd_DXWriteWordData (1, 30, PanPosition); break;
以上で完成です。実行してみましょう。
#include <fd.h> void main (void) { char c; uint16_t PanPosition = 512, TiltPosition = 512; fd_Init (0, BT_CONSOLE, FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP, 7.4); fd_DXWriteWordData (1, 34, 400); fd_DXWriteWordData (2, 34, 400); fd_DXWriteWordData (1, 30, PanPosition); fd_DXWriteWordData (2, 30, TiltPosition); while (1) { if (fd_rx_buff () > 0) c = fd_getc (); switch (c) { case '6' : PanPosition --; if (PanPosition > 60000) PanPosition = 0; fd_DXWriteWordData (1, 30, PanPosition); break; case '4' : PanPosition ++; if (PanPosition > 1023) PanPosition = 1023; fd_DXWriteWordData (1, 30, PanPosition); break; case '8' : TiltPosition --; if (TiltPosition < 200) TiltPosition = 200; fd_DXWriteWordData (2, 30, TiltPosition); break; case '2' : TiltPosition ++; if (TiltPosition > 800) TiltPosition = 800; fd_DXWriteWordData (2, 30, TiltPosition); break; case '0' : fd_SoftReset (); break; } fd_printf ("Pan:%5d Tilt:%5d\r", PanPosition, TiltPosition); } }