/*
AT-WALKER サーボを中心位置に設定するプログラム 
2005.9.29(c)AsakusaGiken
*/

#include <avr/io.h>               /* ＩＯ関連 */
#include <avr/interrupt.h>        /* 割り込み関連 */
#include <avr/signal.h>           /* 割り込みベクタ関連 */

/* -----グローバル変数----- */
char phase = 0;                         /* フェーズを表す変数(PORTC操作用にも使うのでバイトサイズ) */
char ServoPos[8] = {94, 94, 94, 94, 94, 94, 94, 94};    /* サーボの位置を示す変数 */
char HomePos[8] = {94, 94, 94, 94, 94, 94, 94, 94};     /* ホームポジション */

/*-----タイマカウンタ０(8bit)比較マッチ割り込み（サーボパルス幅生成）-----*/
SIGNAL(SIG_OUTPUT_COMPARE0)
{
  PORTC &= ~((1<<PC0) | (1<<PC1) | (1<<PC2) | (1<<PC3) | (1<<PC4) | (1<<PC5) | (1<<PC6) | (1<<PC7));  /* 割り込みが発生した瞬間にPC0-PC3端子をLOW(0) */
  TCCR0 &= ~(1<<CS02);                  /* タイマカウンタ０を停止 */
  TCNT0 = 0x00;                         /* タイマカウンタ０を０にクリア */
}

/*-----タイマカウンタ１(16bit)比較マッチ割り込み（サーボパルス周期生成）-----*/
SIGNAL(SIG_OUTPUT_COMPARE1A)
{
  PORTC |= (1<<phase);                  /* 割り込みが発生した瞬間にそのフェーズの端子をHIGH(1) */
  TCNT1 = 0x00;                         /* タイマカウンタ１を０にクリア */
  OCR0 = ServoPos[phase];               /* 比較レジスタに位置をセット */
  TCCR0 |= (1<<CS02);                   /* カウンタ０スタート(256分周) */

  phase++;                              /* フェーズを１増やす */
  if (phase > 7)
  {
    phase = 0;                          /* フェーズが7を超えたらフェーズ０に戻る */
  }
}

/* -----サーボをホームポジションへ----- */
void homeP(void)
{
    ServoPos[0] = HomePos[0];
    ServoPos[1] = HomePos[1];
    ServoPos[2] = HomePos[2];
    ServoPos[3] = HomePos[3];
    ServoPos[4] = HomePos[4];
    ServoPos[5] = HomePos[5];
    ServoPos[6] = HomePos[6];
    ServoPos[7] = HomePos[7];
}


/*-----メイン-----*/
void main(void)
{

  /* PC0端子の設定 */
  DDRC |= (1<<PC0) | (1<<PC1) | (1<<PC2) | (1<<PC3) | (1<<PC4) | (1<<PC5) | (1<<PC6) | (1<<PC7);     /* PC0-PC3端子を出力へ設定 */
  PORTC &= ~((1<<PC0) | (1<<PC1) | (1<<PC2) | (1<<PC3) | (1<<PC4) | (1<<PC5) | (1<<PC6) | (1<<PC7)); /* PC0-PC3端子をLOW(0) */

  /* タイマカウンタ１の設定 */
  TIMSK |= (1<<OCIE1A);                 /* タイマカウンタ１比較割り込みＡ許可 */
  TCCR1A |= (1<<WGM12);                 /* タイマカウンタ１をCTCﾓｰﾄﾞに */
  OCR1A = 625;                          /* 8分周625カウント=2.5mS */
  /* タイマカウンタ０の設定 */
  TIMSK |= (1<<OCIE0);                  /* タイマカウンタ０の比較マッチ割り込みを許可 */
  
  SREG |= (1<<SREG_I);                  /* 全割り込み許可 */
  TCCR1B |= (1<<CS11) | (1<<CS10);      /* カウンタ１スタート(8分周) */

  while(1)              /* ----------------ループ開始---------------- */
  {

      homeP();

  }
}