/*

＜AT-WALKERに超音波センサーPINGを取り付ける＞
(c)2005.AsakusaGiken
ある一定の距離に近づくプログラム
*/

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

char txb[10],rxb[10];

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

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

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

/*-----タイマカウンタ０(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 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 Go_Fw(void)    // 前進
{
    switch(StepNo) {
        case 0:
            step1();
            StepNo++;
            break;
        case 1:
            step2();
            StepNo++;
            break;
        case 2:
            step3();
            StepNo++;
            break;
        case 3:
            step4();
            StepNo++;
            break;
        case 4:
            step5();
            StepNo++;
            break;
        case 5:
            step0();
            StepNo=0;
            break;
    }
}

void Go_Bw(void)    // バック歩行
{
    switch(StepNo) {
        case 0:
            step5();
            StepNo=5;
            break;
        case 1:
            step0();
            StepNo--;
            break;
        case 2:
            step1();
            StepNo--;
            break;
        case 3:
            step2();
            StepNo--;
            break;
        case 4:
            step3();
            StepNo--;
            break;
        case 5:
            step4();
            StepNo--;
            break;
    }
}

int i;
/*-----PB6に接続した超音波センサから距離を読み取る関数-----*/
unsigned char ping(void)
{
  DDRB |= (1<<DDB6);                    /* PB6を出力に設定 */
  PORTB |= (1<<PB6);                    /* PB6をHighに */
  for(i = 0; i<4; i++){}                /* 5uS(ぐらい)待つ */
  PORTB &= ~(1<<PB6);                   /* PB6をLowに */
  DDRB &= ~(1<<DDB6);                   /* PB6を入力に設定 */
  while((PINB & (1<<PINB6)) == 0) {};   /* 波形が立ち上がるまで待つ */
  TCNT2 = 0;                            /* タイマカウンタを０にリセット */
  TCCR2 |= (1<<CS22) | (1<<CS21);       /* ２５６分周でタイマ２をスタート */
  while((PINB & (1<<PINB6)) != 0) {};   /* 波形が立ち下がるまで待つ */
  TCCR2 = 0x00;                         /* カウンタ停止 */
  return TCNT2;                         /* カウンタの値を返す */
}

/*-----メイン-----*/
void main(void)
{
  /* PCとの通信設定 */
  rs_init (br57600, txb, sizeof(txb), rxb, sizeof(rxb));
  fdevopen(rs_putchar, rs_getchar, 0);

  /* ポートCの設定 */
  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分周) */
  
  printf("start\n");
  
  unsigned char Dist;
  while(1)                              /* 電源を切るまでwhileを繰り返す */
  {
      Dist = ping();
      printf("Dist=%d\n",Dist);
      if (Dist > 40){         /* 遠かったら */
          /* 前進 */
          step0();
          Go_Fw();
          wait_ms(200);
      }else if (Dist < 30){   /* 近かったら */
          /* 後進 */
          Go_Bw();
          wait_ms(200);
      }
      else{
          wait_ms(200);
      }                  /* ちょうどよい距離だったら */
  }
}