/*
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};     /* ホームポジション */

/*-----ｍＳ間ウェイトする関数-----*/
void wait_ms(int msec)
{
  int count;                            /* 繰り返し用カウンタ変数（for文で使用) */
  TCCR2 |= (1<<CS21) | (1<<CS20);       /* 64分周でタイマカウンタ２をスタート */

  for (count = 0; count < msec; count++) /* msecの回数だけ繰り返す */
  {
    /* １ｍＳを生成 */
    TCNT2 = 0x00;                       /* カウンタを０クリア */
    while(TCNT2 < 250){}                /* 250カウントするまで繰り返し（待つ） */
  }
}

/*-----タイマカウンタ０(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 step0(void)
{
    /* step0 */
    ServoPos[0] = HomePos[0] + 0;
    ServoPos[1] = HomePos[1] + 20;
    ServoPos[2] = HomePos[2] + 0;
    ServoPos[3] = HomePos[3] + 20;
}

void step1(void)
{
    /* step1 */
    ServoPos[0] = HomePos[0] + 30;
    ServoPos[1] = HomePos[1] + 20;
    ServoPos[2] = HomePos[2] + 15;
    ServoPos[3] = HomePos[3] + 20;
}

void step2(void)
{
    /* step2 */
    ServoPos[0] = HomePos[0] + 15;
    ServoPos[1] = HomePos[1] - 20;
    ServoPos[2] = HomePos[2] + 15;
    ServoPos[3] = HomePos[3] - 20;
}

void step3(void)
{
    /* step3 */
    ServoPos[0] = HomePos[0] + 0;
    ServoPos[1] = HomePos[1] - 20;
    ServoPos[2] = HomePos[2] + 0;
    ServoPos[3] = HomePos[3] - 20;
}

void step4(void)
{
    /* step4 */
    ServoPos[0] = HomePos[0] - 15;
    ServoPos[1] = HomePos[1] - 20;
    ServoPos[2] = HomePos[2] - 30;
    ServoPos[3] = HomePos[3] - 20;
}

void step5(void)
{
    /* step5 */
    ServoPos[0] = HomePos[0] - 15;
    ServoPos[1] = HomePos[1] + 20;
    ServoPos[2] = HomePos[2] - 15;
    ServoPos[3] = HomePos[3] + 20;
}


/*-----移動-----*/
void Go_Fw(void)    // 前進
{
    step0();
    wait_ms(1000);
    step1();
    wait_ms(1000);
    step2();
    wait_ms(1000);
    step3();
    wait_ms(1000);
    step4();
    wait_ms(1000);
    step5();
    wait_ms(1000);
}

/* -----サーボをホームポジションへ----- */
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分周) */

  /* ロボット起動 */
  homeP();
  wait_ms(3000);
  
  while(1)              /* ----------------ループ開始---------------- */
  {

      Go_Fw();

  }
}