FDIII-HC Starter Kit Guide​/Chapter7.3 のバックアップソース(No.4) :: Besttechnology

• 差分^[1] を表示
• 現在との差分^[2] を表示
• FDIII-HC Starter Kit Guide/Chapter7.3^[3] へ行く。

TITLE:雲台のプログラミング
*プログラミング [#l7fd2c23]

**処理の流れを図にする [#td3b0eb7]

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


#ref(flow.png)

**初期化 [#m41943fd]

GCC Developer Liteを起動し、初期化部分を記述します。
#html{{
<pre class="brush:c">
  fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP);
  fd_SetUVThreshold (7.4);
  DX_ChangeBaudrate (1000000);
</pre>
}}

**キーボード入力関数の作成 [#y215f66b]

通常のキー入力では1回の押下で1バイトが入力されます。方向キーの場合は、1回の入力で3バイトも入力されます。~
機種により値が異なる可能性がありますので、ご注意ください。
| 1バイト目 | 2バイト目 | 3バイト目 | 方向キー |
| 0x1b      | 0x5b      | 0x41      | 上キー |
|^          |^          | 0x42      | 下キー |
|^          |^          | 0x43      | 右キー |
|^          |^          | 0x44      | 左キー |
方向キーをテンキーの数字に合わせます。
| 方向キー | 上キー | 下キー | 右キー | 左キー |
| テンキー | '8' | '2' | '6' | '4' |
テンキーはキーボードの右側に位置している数字キー等のことです。~
#html{{
<pre class="brush:c">
// キーボード入力関数
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;
}
</pre>
}}

**変数 [#nfde1082]
パン(横振り)・チルト(上下振り)のポジション用に使う変数PanPositionとTiltPositionをuint16_t型で用意します。AX-12+のホーンが真ん中になるよう、初期値を512とします。~
入力バッファ用の変数inkeyと、ESCキー入力時の処理の終了判断を行うためのiendをint型で宣言します。
#html{{
<pre class="brush:c">
  int inkey, iend = 0;
  uint16_t PanPosition = 512, TiltPosition = 512;
</pre>
}}

**トルクリミット [#b23b37f8]

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

#html{{
<pre class="brush:c">
  fd_DXSetTorqueLimit (1, 154); // ID1のトルクリミットに154を設定
  fd_DXSetTorqueLimit (2, 154); // ID2のトルクリミットに154を設定
</pre>
}}

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

**初期ポジションへ移動 [#n7949229]
トルクリミット設定後、ID1とID2のゴールポジションを変更します。

#html{{
<pre class="brush:c">
  fd_DXSetPosition (1, PanPosition);    // パン位置の設定
  fd_DXSetPosition (2, TiltPosition);   // チルト位置の設定
</pre>
}}

**ポジションリミット [#i21b9509]

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

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

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

**メインループ [#vfd65e43]

プッシュボタンが押されるか、ESCが押されると終了(ここでは終了フラグを参照)するwhile文を作成します。
#html{{
<pre class="brush:c">
  while (!fd_GetPB () && !iend) {
</pre>
}}

キー入力の有無を[[fd_rx_buff>FDIIILIB#c1552bcf]]関数で確認し、キー入力があれば作成したキーボード入力関数を呼出します。
#html{{
<pre class="brush:c">
if (fd_rx_buff ()) {
  inkey = getCrossKey ();
</pre>
}}
方向キーはテンキーに置き換えられていますので、テンキーの数字でパン位置とチルト位置を変更します。ESCキーが入力されていれば、終了フラグをセットします。
#html{{
<pre class="brush:c">
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;
}
</pre>
}}
パン位置とチルト位置を設定した後、現在の座標を表示します。
#html{{
<pre class="brush:c">
fd_DXSetPosition (1, PanPosition);
fd_DXSetPosition (2, TiltPosition);
fd_printf ("Pan:%5d Tilt:%5d\r", PanPosition, TiltPosition);
</pre>
}}

**プログラム完成 [#d361e94a]

以上で完成です。まず、名前を付けて保存します。名前はCameraPlatform.cとします。保存後、ビルドして実行してみましょう。
#html{{
<pre class="brush:c">
#include &lt;fd.h&gt;
// キーボード入力関数
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);
  }
}
</pre>
}}
[[次のチャプターへ>FDIIICHAPTER7.4]]~

[[FDIII-HC Starter Kit Guide>LEARNINGGUIDE]]に戻る

• FDIII-HC Starter Kit Guide/Chapter7.3 のバックアップ一覧^[6]
• FDIII-HC Starter Kit Guide/Chapter7.3 のバックアップソース(No. All)
  • 1: 2011-07-11 (月) 14:05:38^[7] yoshida^[8]
  • 2: 2011-07-11 (月) 18:39:16^[9] yoshida^[8]
  • 3: 2011-07-11 (月) 20:10:52^[10] yoshida^[8]
  • 4: 2011-07-21 (木) 13:02:00 yoshida^[8]
  • 現: 2013-04-09 (火) 23:52:52^[11] takaboo^[12]