Page Top

処理の流れを図にする anchor.png

SIMPLE TERMからのキー入力によって、カメラを動かす方向を変えます。 キーボードの方向キー、又はテンキーの'8'、'2'、'4'、'6'を使って上下左右に動き、[ESC]キーでプログラムを終了します。

flow.png
Page Top

初期化 anchor.png

GCC Developer Liteを起動し、初期化部分を記述します。

  fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP);
  fd_SetUVThreshold (7.4);
  DX_ChangeBaudrate (1000000);
Page Top

キーボード入力関数の作成 anchor.png

通常のキー入力では1回の押下で1バイトが入力されます。方向キーの場合は、1回の入力で3バイトも入力されます。
機種により値が異なる可能性がありますので、ご注意ください。

1バイト目2バイト目3バイト目方向キー
0x1b0x5b0x41上キー
0x42下キー
0x43右キー
0x44左キー

方向キーをテンキーの数字に合わせます。

方向キー上キー下キー右キー左キー
テンキー'8''2''6''4'

テンキーはキーボードの右側に位置している数字キー等のことです。

// キーボード入力関数
int getCrossKey () {
  int result = -1;  // 戻り値
  int cnt = 0;      // ループ用カウンタ
  int rcv = 0;      // 入力文字バッファ
  while (fd_rx_buff () && cnt < 3) {          // 入力バイトが残っており、3バイト目の処理が終了するまで繰り返す
    rcv = fd_getc ();                         // 1バイト入力
    switch (cnt) {
      case 0: if (rcv == 0x1b) cnt++; break;  // 1バイト目が0x1bか確認
      case 1: if (rcv == 0x5b) cnt++; break;  // 2バイト目が0x5bか確認
      default:
        switch (rcv) {                        // 十字キー以外は読み飛ばし
          case 0x41: result = '8'; break;     // 上キー
          case 0x42: result = '2'; break;     // 下キー
          case 0x43: result = '6'; break;     // 右キー
          case 0x44: result = '4'; break;     // 左キー
        }
        cnt++;
        break;
    }
  }
  if (cnt != 3) result = rcv; // 通常のキーボード入力
  return result;
}
Page Top

変数 anchor.png

パン(横振り)・チルト(上下振り)のポジション用に使う変数PanPositionとTiltPositionをuint16_t型で用意します。AX-12Aのホーンが真ん中になるよう、初期値を512とします。
入力バッファ用の変数inkeyと、ESCキー入力時の処理の終了判断を行うためのiendをint型で宣言します。

  int inkey, iend = 0;
  uint16_t PanPosition = 512, TiltPosition = 512;
Page Top

トルクリミット anchor.png

ID=1と2のトルクリミットを154にします。

  fd_DXSetTorqueLimit (1, 154); // ID1のトルクリミットに154を設定
  fd_DXSetTorqueLimit (2, 154); // ID2のトルクリミットに154を設定

トルクが不足している場合は適宜増加します。

Page Top

初期ポジションへ移動 anchor.png

トルクリミット設定後、ID1とID2のゴールポジションを変更します。

  fd_DXSetPosition (1, PanPosition);    // パン位置の設定
  fd_DXSetPosition (2, TiltPosition);   // チルト位置の設定
Page Top

ポジションリミット anchor.png

フレームが干渉してしまうようなポジションを送信すると、AX-12Aに過大な負荷がかかり、破損する場合があります。そのようなポジションを送信しないよう、ポジションに仮のリミッタを設けます。

条件移動方向リミッタ
右へ移動パン位置減算パン位置 > 0
左へ移動パン位置加算パン位置 < 1023
上へ移動チルト位置減算チルト位置 > 200
下へ移動チルト位置加算チルト位置 < 800

取り付け方によっては、移動方向やリミッタの数値が異なります。フレームが干渉しないように十分確認して下さい。

Page Top

メインループ anchor.png

プッシュボタンが押されるか、ESCが押されると終了(ここでは終了フラグを参照)するwhile文を作成します。

  while (!fd_GetPB () && !iend) {

キー入力の有無をfd_rx_buff関数で確認し、キー入力があれば作成したキーボード入力関数を呼出します。

if (fd_rx_buff ()) {
  inkey = getCrossKey ();

方向キーはテンキーに置き換えられていますので、テンキーの数字でパン位置とチルト位置を変更します。ESCキーが入力されていれば、終了フラグをセットします。

switch (inkey) {
  case '6':   // 右へ移動
    if (PanPosition > 0) PanPosition--;
    break;
  case '4':   // 左へ移動
    if (PanPosition < 1023) PanPosition++;
    break;
  case '8':   // 上へ移動
    if (TiltPosition > 200) TiltPosition--;
    break;
  case '2':   // 下へ移動
    if (TiltPosition < 800) TiltPosition++;
    break;
  case 0x1b:  // 終了
    iend = 1;
    break;
}

パン位置とチルト位置を設定した後、現在の座標を表示します。

fd_DXSetPosition (1, PanPosition);
fd_DXSetPosition (2, TiltPosition);
fd_printf ("Pan:%5d Tilt:%5d\r", PanPosition, TiltPosition);
Page Top

プログラム完成 anchor.png

以上で完成です。まず、名前を付けて保存します。名前はCameraPlatform.cとします。保存後、ビルドして実行してみましょう。

#include <fd.h>
// キーボード入力関数
int getCrossKey () {
  int result = -1;  // 戻り値
  int cnt = 0;      // ループ用カウンタ
  int rcv = 0;      // 入力文字バッファ
  while (fd_rx_buff () && cnt < 3) {
    rcv = fd_getc ();
    switch (cnt) {
      case 0: if (rcv == 0x1b) cnt++; break;  // 1バイト目が0x1bか確認
      case 1: if (rcv == 0x5b) cnt++; break;  // 2バイト目が0x5bか確認
      default:
        switch (rcv) {                        // 十字キー以外は読み飛ばし
          case 0x41: result = '8'; break;     // 上キー
          case 0x42: result = '2'; break;     // 下キー
          case 0x43: result = '6'; break;     // 右キー
          case 0x44: result = '4'; break;     // 左キー
        }
        cnt++;
        break;
    }
  }
  if (cnt != 3) result = rcv; // 通常のキーボード入力
  return result;
}

void main (void) {
  int inkey, iend = 0;
  uint16_t PanPosition = 512, TiltPosition = 512;

  fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP);
  fd_SetUVThreshold (7.4);
  DX_ChangeBaudrate (1000000);

  fd_DXSetTorqueLimit (1, 154); // ID1のトルクリミットに154を設定
  fd_DXSetTorqueLimit (2, 154); // ID2のトルクリミットに154を設定
  fd_DXSetPosition (1, PanPosition);    // パン位置の設定
  fd_DXSetPosition (2, TiltPosition);   // チルト位置の設定
  
  while (!fd_GetPB () && !iend) {
    if (fd_rx_buff ()) {
      inkey = getCrossKey ();
      switch (inkey) {
        case '6':   // 右へ移動
          if (PanPosition > 0) PanPosition--;
          break;
        case '4':   // 左へ移動
          if (PanPosition < 1023) PanPosition++;
          break;
        case '8':   // 上へ移動
          if (TiltPosition > 200) TiltPosition--;
          break;
        case '2':   // 下へ移動
          if (TiltPosition < 800) TiltPosition++;
          break;
        case 0x1b:  // 終了
          iend = 1;
          break;
      }
      fd_DXSetPosition (1, PanPosition);
      fd_DXSetPosition (2, TiltPosition);
      fd_printf ("Pan:%5d Tilt:%5d\r", PanPosition, TiltPosition);
    }
    fd_Wait (10);
  }
}

次のチャプターへ

FDIII-HC Starter Kit Guideに戻る


Front page   Diff ReloadPrint View   Page list Search Recent changes   RSS of recent changes (RSS 1.0) RSS of recent changes (RSS 2.0) RSS of recent changes (RSS Atom)
Last-modified: 2013-04-09 (Tue) 23:52:52 (JST) (2728d)