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

char txb[10],rxb[10];
unsigned char snd_data[7] = {255,100,1,80};       /* サーボ駆動用データ */
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 read_gravity(void)
{
    int i;
    for (i=0; i<4; i++){
      rs_putc(snd_data[i]);
    }
}

 /* -----メイン----- */
void main(void)
{
  rs_init (br9600, txb, sizeof(txb), rxb, sizeof(rxb)); /* 通信設定 */
  sei();                                     /* 全割り込み許可 */
  rs_rx_purge();                    /* 一応、受信バッファをクリアしてます */
  DDRC |= (1<<PC0) | (1<<PC1);              /* IO設定（PC0とPC1を出力へ） */
  PORTC |= ((1<<PC0) | (1<<PC1));           /* PC0とPC1をHighへ（LED消灯) */
  wait_ms(2000);
  int cnt;
  while(1)
  {
    read_gravity();                         /* 重心読取命令発行 */
    for (cnt = 0; cnt < 6; cnt++){          /* ６バイト受信 */
        rcv_data[cnt] = rs_getc ();
    }
    if (rcv_data[4] < (127-20)){
        PORTC &= ~(1<<PC0);                  /* PC0のLEDを点灯 */
    }else if(rcv_data[4] > (127+20)){
        PORTC &= ~(1<<PC1);                  /* PC1のLEDを点灯 */
    }else{
        PORTC |= ((1<<PC0) | (1<<PC1));    /* PC0とPC1をLowへ（LED消灯) */
    }
    wait_ms(100);
  }
}