#include <avr/io.h>
#include <avr/interrupt.h>
#include <rs.h>

char txb[10],rxb[10];
unsigned char mov_data[7] = {255,3,4,2,0,127,20}; /* サーボ駆動用データ */
unsigned char cap_data[5] = {255,3,2,22,0};       /* ポジキャプ用データ */
unsigned char rcv_data[6];                        /* 受信結果格納用 */

 /* ｍSウェイト関数 */
void wait_ms(int msec)
{
  int count;
  TCCR2 |= (1<<CS22);
  for (count = 0; count < msec; count++)
  {
    TCNT2 = 0x00;
    while(TCNT2<250){}
  }
}

 /* サーボ駆動関数 */
void S_Move(unsigned char S_num, unsigned char S_pos)
{
    mov_data[4] = S_num;
    mov_data[5] = S_pos;
    int i;
    for (i=0; i<7; i++){
      rs_putc(mov_data[i]);
    }
}

 /* ポジションキャプチャ関数 */
void P_Cap(unsigned char S_num)
{
    cap_data[4] = S_num;
    int i;
    for (i=0; i<5; i++){
      rs_putc(cap_data[i]);
    }
}

 /* メイン */
void main(void)
{
  rs_init (br9600, txb, sizeof(txb), rxb, sizeof(rxb));
  sei();
  rs_rx_purge(); /* 一応、受信バッファをクリアしてます */
  wait_ms(2000);
  int cnt;
  while(1)
  {
    P_Cap(0);                         /* ＃０ポジションキャプチャ実行 */
    for(cnt = 0; cnt < 6; cnt++){     /* ６バイト受信 */
      rcv_data[cnt] = rs_getc();
    }
    S_Move(1,rcv_data[5]);             /* 受信結果より＃１サーボを駆動 */

  }
}