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

char txb[10],rxb[10];                     /* 通信用バッファ */
char data[7] = {255,3,4,2,0,127,20};      /* サーボ駆動用データ */

/* 指定したｍ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)      
{
    data[4] = S_num;
    data[5] = S_pos;
    int i;
    for (i=0; i<7; i++){
      rs_putc(data[i]);
    }
}

/* メイン */
void main(void)
{
  rs_init (br9600, txb, sizeof(txb), rxb, sizeof(rxb));      /* 通信設定 */
  sei();                        /* 割り込みを許可（通信は割り込みを使用） */
  wait_ms(1000);                /* ＲＳＣが起動し終えるまで待つ */
  while(1)                      /* 無限ループ */
  {
    S_Move(0,127);              /* サーボを１２７の位置へ移動 */
    wait_ms(1000);              /* 少し待つ */
    S_Move(0,200);              /* サーボを２００の位置へ移動 */
    wait_ms(1000);              /* 少し待つ */
  }
}