FDIII-HC Starter Kit Guide​/Chapter7.3

プログラミング

処理の流れを図にする

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

初期化

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

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

キーボード入力関数の作成

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

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

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

// キーボード入力関数
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;
}

変数

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

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

トルクリミット

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

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

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

初期ポジションへ移動

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

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

ポジションリミット

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

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

メインループ

プッシュボタンが押されるか、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);

プログラム完成

以上で完成です。まず、名前を付けて保存します。名前は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に戻る