#include <3694.h>       /* H8 Tinyの内部I/O定義をインクルード */

/* 送受信用バッファ */
char txb[10], rxb[10];

/* 位置指定フォーマット [255][ID][長(4)][命令(2)][サーボ番号][サーボ位置][Speed] */
unsigned char mov_data[7] = {255, 3, 4, 2, 0, 200, 5}; /* とりあえずセット */

/* ポジションキャプチャのフォーマット [255] [ID] [長(2)] [命令(22)] [サーボ番号] */
unsigned char cap_data[5] = {255, 3, 2, 22, 0}; /* とりあえずセット */

/* ポジションキャプ結果データ格納用変数 */
unsigned char rcv_data[6];

/*-----------------------------------------------*/
/* 指定したサーボのポジションキャプチャ命令発行  */
/*-----------------------------------------------*/
void P_Cap(unsigned char servo_number)
{
    cap_data[4] = servo_number;   /* サーボ番号をセット */
    /* 命令２２を発行 */
    unsigned char cnt;
    for (cnt = 0; cnt < 5; cnt++){
      SCI3_OUT_DATA(cap_data[cnt]);
    }
}

/*-----------------------------------------------*/
/* 指定したサーボを動かす命令発行                */
/*-----------------------------------------------*/
void S_Move(unsigned char servo_number, unsigned char servo_position)
{
    mov_data[4] = servo_number;     /* サーボ番号をセット */
    mov_data[5] = servo_position;   /* サーボ位置をセット */
    /* 命令２を発行 */
    unsigned char cnt;
    for (cnt = 0; cnt < 7; cnt++){
      SCI3_OUT_DATA(mov_data[cnt]);
    }
}

/*-----------------------------------------------*/
/* メイン                                        */
/*-----------------------------------------------*/
void main(void)
{
  
  /* 通信ポートを初期化 ボーレートは9600[bps] */
  SCI3_INIT (br9600, txb, sizeof(txb), rxb, sizeof(rxb));

  /* 初期化後はを許可状態に */
  EI;
  
  unsigned long j;
  
  /* 受信割り込み許可 */
  SCI3.SCR3.BIT.RIE = 1;

  /* RSCが立ち上がるまで待つ */
  for (j = 0; j < 600000; j++);  /* ウェイト（てきとう） */
  for (j = 0; j < 600000; j++);  /* ウェイト（てきとう） */
  
  /* 無限ループ */
  while(1)
  {
    P_Cap(0);                               /* 0番サーボのポジションキャプチャ */
    
    for (j = 0; j < 5000; j++);             /* ウェイト（短め、てきとう） */
    int cnt1;                               /* ６バイト受信 */
    for (cnt1 = 0; cnt1 < 6; cnt1++){
        rcv_data[cnt1] = SCI3_IN_DATA ();
    }
    S_Move(1,rcv_data[5]);                  /* 取得した位置を１番サーボへ */
    for (j = 0; j < 5000; j++);             /* ウェイト（短め、てきとうう） */
  }

}