/*
ツクモデモロボＮｏ２のプログラム 
2005.9.23(c)AsakusaGiken

#0 増＝足ヒラ外側下／減＝足ヒラ外側上
#1 増＝足先右手側／減＝足先左手側
#2 増＝足ヒラ外側上／減＝足ヒラ外側下
#3 増＝足先右手側／減＝足先左手側

#4 減＝左手上げ 完全上げ＝−４５ 完全下げ＝＋１０
#5 減＝左手閉じる 完全閉じ＝−３５ 完全開く＝＋３５
#6 減＝右手下げ 完全上げ＝＋４５ 完全下げ＝−１０
#7 減＝右手開く 完全閉じ＝＋３５ 完全開く＝−３５

コントローラ
足 
ADC0 中心＝６２５ 下＝９２８ 上＝２８
ADC1 中心＝６６４ 右＝１９２ 左＝９６７
右手
ADC4 中心＝６８０ 下＝９７７ 上＝２１４
ADC5 中心＝７００ 右＝１００ 左＝９７２
左手
ADC2 中心＝６７５ 下＝９２０ 上＝２３１
ADC3 中心＝６９６ 右＝２２５ 左＝９５２

*/

#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[8] = {94, 94, 94, 94, 94, 94, 94, 94};    /* サーボの位置を示す変数 */
char HomePos[8] = {88, 90, 90, 96, 94-14, 94, 90+10, 94};     /* ホームポジション */
int Status = 0;       // 入力状態（なにか動作があった時には０に戻る。入力なしの場合＋＋、１０００を超えると脱力）
int StepNo = 0;       // 歩行状態
int RoopCnt = 0;      // 入力無し状態のカウント（自動脱力用）

/*-----ｍＳ間ウェイトする関数-----*/
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 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];
}


/*-----ＡＤＣ読み取り（引数は読み取るピン番号0-7）-----*/
int readADC(unsigned char ADCno)
{
    /* ↓変換チャンネルを設定 */
    ADMUX = 0xC0 + ADCno;
    ADCSRA |= (1<<ADSC);                    /* AD変換開始 */
    while( (ADCSRA & (1<<ADSC)) == 1) {}    /* 変換が終わるまで(ADCSRAのADSCが０になるまで)待つ */
    wait_ms(4);                             /* 少しウェイトしないと他に影響が出たのでウェイト入れた*/
    return ADC;
//    printf("\r%d   ",ADC);
}

/*-----ＡＤＣテスト-----*/
void ADCtest(void)
{
  while (1) 
  {
      printf("No.0=%d\n",readADC(0));
      printf("No.1=%d\n",readADC(1));
      printf("No.2=%d\n",readADC(2));
      printf("No.3=%d\n",readADC(3));
      printf("No.4=%d\n",readADC(4));
      printf("No.5=%d\n",readADC(5));
      wait_ms(500);
  }
}

/*-----移動-----*/
void Go_Fw(void)    // 前進
{
    /* 動作時処理 */
    TIMSK |= (1<<OCIE1A);   // サーボ有効
    Status = 1;             // 入力ステータス
    
    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)    // バック歩行
{
    /* 動作時処理 */
    TIMSK |= (1<<OCIE1A);   // サーボ有効
    Status = 1;             // 入力ステータス
    
    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;
    }
}

void Go_Rt(void)    // 右ターン
{
    /* 動作時処理 */
    TIMSK |= (1<<OCIE1A);   // サーボ有効
    Status = 1;             // 入力ステータス
    
    homeP();
    wait_ms(500);
    /* Rtstep1 */
    ServoPos[0] = HomePos[0] + 20;
    ServoPos[1] = HomePos[1] ;
    ServoPos[2] = HomePos[2] + 10;
    ServoPos[3] = HomePos[3] ;
    wait_ms(500);
    /* Rtstep2 */
    ServoPos[0] = HomePos[0] + 15;
    ServoPos[1] = HomePos[1] ;
    ServoPos[2] = HomePos[2] + 15;
    ServoPos[3] = HomePos[3] - 30;
    ServoPos[6] = HomePos[6] + 20;
    wait_ms(500);
    /* Rtstep3 */
    ServoPos[0] = HomePos[0] ;
    ServoPos[1] = HomePos[1] ;
    ServoPos[2] = HomePos[2] ;
    ServoPos[3] = HomePos[3] - 30;
    ServoPos[6] = HomePos[6] ;
    wait_ms(500);
    /* Rtstep4 */
    homeP();
    wait_ms(500);
    StepNo = 0;       // 歩行状態をリセット
}

void Go_Lt(void)    // 左ターン
{
    /* 動作時処理 */
    TIMSK |= (1<<OCIE1A);   // サーボ有効
    Status = 1;             // 入力ステータス
    
    homeP();
    wait_ms(500);
    /* Ltstep1 */
    ServoPos[0] = HomePos[0] - 10;
    ServoPos[1] = HomePos[1] ;
    ServoPos[2] = HomePos[2] - 20;
    ServoPos[3] = HomePos[3] ;
    wait_ms(500);
    /* Ltstep2 */
    ServoPos[0] = HomePos[0] - 15;
    ServoPos[1] = HomePos[1] + 30;
    ServoPos[2] = HomePos[2] - 15;
    ServoPos[3] = HomePos[3] ;
    ServoPos[4] = HomePos[4] - 20;
    wait_ms(500);
    /* Ltstep3 */
    ServoPos[0] = HomePos[0] ;
    ServoPos[1] = HomePos[1] + 30;
    ServoPos[2] = HomePos[2] ;
    ServoPos[3] = HomePos[3] ;
    ServoPos[4] = HomePos[4] ;
    wait_ms(500);
    /* Ltstep4 */
    homeP();
    wait_ms(500);
    StepNo = 0;       // 歩行状態をリセット  
}

/*-----メイン-----*/
void main(void)
{
  /* PCと通信するための準備（デバック用） */
  rs_init (br57600, txb, sizeof(txb), rxb, sizeof(rxb));
  fdevopen(rs_putchar, rs_getchar, 0);

  /* 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分周) */
  
  /* ADC設定 */
  ADCSRA |= (1<<ADEN) | (1<<ADPS2);     /* A/Dの動作許可, 変換クロック16分周に指定 */
  
  /* -----------------ロボット動作開始----------------- */
  printf("Start!!\r\n");
  homeP();              // ホームポジションへ（電源投入時）
  wait_ms(3000);
  
  int stick;            // スティックの状態一時保管変数
  int i;
  
  while(1)              /* ----------------ループ開始---------------- */
  {

      for (i=0; i<3; i++){    // 手判定３回につき歩行判定１回
      
        /* 右手判定 */
        stick = readADC(4);
        if (stick < 580) ServoPos[6] = HomePos[6] - (stick - 680)/10;       // 右手上げの場合
        else if (stick > 760) ServoPos[6] = HomePos[6] - (stick - 680)/10;  // 右手下げの場合
        else ServoPos[6] = HomePos[6];                                      // 中立位置の場合
        stick = readADC(5);
        if (stick < 580) ServoPos[7] = HomePos[7] + (stick - 696)/20;       // 右手開くの場合
        else if (stick > 760) ServoPos[7] = HomePos[7] + (stick - 696)/10;  // 右手閉るの場合
        else ServoPos[7] = HomePos[7];                                      // 中立位置の場合
      
        /* 左手判定 */
        stick = readADC(2);
        if (stick < 580) ServoPos[4] = HomePos[4] + (stick - 680)/10;       // 左手上げの場合
        else if (stick > 740) ServoPos[4] = HomePos[4] + (stick - 680)/10;  // 左手下げの場合
        else ServoPos[4] = HomePos[4];                                      // 中立位置の場合
        stick = readADC(3);
        if (stick < 580) ServoPos[5] = HomePos[5] + (stick - 696)/20;       // 左手開くの場合
        else if (stick > 760) ServoPos[5] = HomePos[5] + (stick - 696)/10;  // 左手閉るの場合
        else ServoPos[5] = HomePos[5];                                      // 中立位置の場合
        
        wait_ms(200);     // 手入力判定は２００ｍS周期
      
      }

      /* 歩行レバー前後を判定 */
      stick = readADC(0);
      if (stick < 400) Go_Fw();
      if (stick > 850) Go_Bw();

      /* 歩行レバー左右を判定 */
      stick = readADC(1);
      if (stick < 400) Go_Rt();
      if (stick > 850) Go_Lt();
      
      /* 入力状態チェック（移動入力無しで６０回ループしたら脱力） */
      if (Status == 0){
          RoopCnt++;
          if (RoopCnt > 60){
              TIMSK &= ~(1<<OCIE1A);          // 脱力
              RoopCnt = 0;
          }
      }
      else RoopCnt = 0;
      Status = 0;

  }
}