/*
PANDAM-Sロボット側のプログラム 
2005.11.17(c)AsakusaGiken

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

#4 減＝左手下げ 完全上げ＝＋３０ 完全下げ＝−２０
#5 減＝右手上げ 完全上げ＝−４０ 完全開く＝＋２０
#6 減＝頭下げ 完全上げ＝＋５ 完全下げ＝−２５


*/

#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] = {94, 94-3, 94-6, 94-2, 94-10, 94+10, 94-5, 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] + 20;
    ServoPos[1] = HomePos[1] + 20;
    ServoPos[2] = HomePos[2] + 10;
    ServoPos[3] = HomePos[3] + 20;
}

void step2(void)
{
    /* step2 */
    ServoPos[0] = HomePos[0] + 5;
    ServoPos[1] = HomePos[1] - 20;
    ServoPos[2] = HomePos[2] + 10;
    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] - 10;
    ServoPos[1] = HomePos[1] - 20;
    ServoPos[2] = HomePos[2] - 20;
    ServoPos[3] = HomePos[3] - 20;
}

void step5(void)
{
    /* step5 */
    ServoPos[0] = HomePos[0] - 10;
    ServoPos[1] = HomePos[1] + 20;
    ServoPos[2] = HomePos[2] - 5;
    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)    // 前進
{
    /* 動作時処理 */
    TIMSK |= (1<<OCIE1A);   // サーボ有効
    Status = 1;             // 入力ステータス
    
    step1();
    wait_ms(500);
    step2();
    wait_ms(500);
    step3();
    wait_ms(500);
    step4();
    wait_ms(500);
    step5();
    wait_ms(500);
    step0();
    wait_ms(500);
}

void Go_Bw(void)    // バック歩行
{
    /* 動作時処理 */
    TIMSK |= (1<<OCIE1A);   // サーボ有効
    Status = 1;             // 入力ステータス
    
    step5();
    wait_ms(500);
    step4();
    wait_ms(500);
    step3();
    wait_ms(500);
    step2();
    wait_ms(500);
    step1();
    wait_ms(500);
    step0();
    wait_ms(500);
}

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;
    wait_ms(500);
    /* Rtstep3 */
    ServoPos[0] = HomePos[0] ;
    ServoPos[1] = HomePos[1] ;
    ServoPos[2] = HomePos[2] ;
    ServoPos[3] = HomePos[3] - 30;
    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] ;
    wait_ms(500);
    /* Ltstep3 */
    ServoPos[0] = HomePos[0] ;
    ServoPos[1] = HomePos[1] + 30;
    ServoPos[2] = HomePos[2] ;
    ServoPos[3] = HomePos[3] ;
    wait_ms(500);
    /* Ltstep4 */
    homeP();
    wait_ms(500);
    StepNo = 0;       // 歩行状態をリセット  
}

void BLUE_KEY(void)   // あいさつ
{
    /* 動作時処理 */
    TIMSK |= (1<<OCIE1A);   // サーボ有効
    Status = 1;             // 入力ステータス
    
    homeP();
    wait_ms(500);
    /* step1 */
    ServoPos[0] = HomePos[0] + 25;
    ServoPos[2] = HomePos[2] + 10;
//    wait_ms(300);
    ServoPos[4] = HomePos[4] + 30;
    ServoPos[5] = HomePos[5] + 10;
    wait_ms(1000);
    ServoPos[6] = HomePos[6] + 10;
    wait_ms(300);
    ServoPos[6] = HomePos[6] ;
    wait_ms(1000);
    homeP();
}

void RED_KEY(void)    // うなずく
{
    /* 動作時処理 */
    TIMSK |= (1<<OCIE1A);   // サーボ有効
    Status = 1;             // 入力ステータス
    
    /* step1 */
    ServoPos[6] = HomePos[6] - 20;
    ServoPos[4] = HomePos[4] - 10;
    ServoPos[5] = HomePos[5] + 10;
    wait_ms(500);
    ServoPos[6] = HomePos[6] - 5;
}

void YELLOW_KEY(void)    // びっくりする
{
    /* 動作時処理 */
    TIMSK |= (1<<OCIE1A);   // サーボ有効
    Status = 1;             // 入力ステータス
    
    homeP();
    wait_ms(500);
    /* step1 */
    ServoPos[0] = HomePos[0] - 15;
    ServoPos[2] = HomePos[2] + 15;
    ServoPos[4] = HomePos[4] + 15;
    ServoPos[5] = HomePos[5] - 25;
    wait_ms(300);
    ServoPos[6] = HomePos[6] - 15;
    wait_ms(2000);
    homeP();

}
void GREEN_KEY(void)    // 小川のマネ
{
    /* 動作時処理 */
    TIMSK |= (1<<OCIE1A);   // サーボ有効
    Status = 1;             // 入力ステータス
    
    homeP();
    wait_ms(500);
    /* step1 */
    ServoPos[0] = HomePos[0] + 5;
    ServoPos[2] = HomePos[2] - 5;
    ServoPos[4] = HomePos[4] + 5;
    ServoPos[5] = HomePos[5] - 5;
    ServoPos[6] = HomePos[6] + 5;
    wait_ms(1000);
    /* step2 */
    ServoPos[0] = HomePos[0] + 10;
    ServoPos[2] = HomePos[2] - 10;
    ServoPos[4] = HomePos[4] + 10;
    ServoPos[5] = HomePos[5] - 10;
    ServoPos[6] = HomePos[6] + 10;
    wait_ms(1000);
    /* step3 */
    ServoPos[0] = HomePos[0] + 15;
    ServoPos[2] = HomePos[2] - 15;
    ServoPos[4] = HomePos[4] + 15;
    ServoPos[5] = HomePos[5] - 15;
    ServoPos[6] = HomePos[6] + 15;
    wait_ms(1000);
    /* ハッスル！ */
    ServoPos[0] = HomePos[0] + 5;
    ServoPos[2] = HomePos[2] - 5;
    ServoPos[4] = HomePos[4] - 5;
    ServoPos[5] = HomePos[5] + 5;
    ServoPos[6] = HomePos[6] - 5;
    wait_ms(500);
    /* step3 */
    ServoPos[0] = HomePos[0] + 15;
    ServoPos[2] = HomePos[2] - 15;
    ServoPos[4] = HomePos[4] + 15;
    ServoPos[5] = HomePos[5] - 15;
    ServoPos[6] = HomePos[6] + 15;
    wait_ms(500);
    /* ハッスル！ */
    ServoPos[0] = HomePos[0] + 5;
    ServoPos[2] = HomePos[2] - 5;
    ServoPos[4] = HomePos[4] - 5;
    ServoPos[5] = HomePos[5] + 5;
    ServoPos[6] = HomePos[6] - 5;
    wait_ms(500);
    /* step3 */
    ServoPos[0] = HomePos[0] + 15;
    ServoPos[2] = HomePos[2] - 15;
    ServoPos[4] = HomePos[4] + 15;
    ServoPos[5] = HomePos[5] - 15;
    ServoPos[6] = HomePos[6] + 15;
    wait_ms(1000);
    homeP();
}

void BLUEandRED_KEY(void)    // ツイスト
{
    /* 動作時処理 */
    TIMSK |= (1<<OCIE1A);   // サーボ有効
    Status = 1;             // 入力ステータス
    
    /* ホーム */
    ServoPos[0] = HomePos[0];
    ServoPos[1] = HomePos[1];
    ServoPos[2] = HomePos[2];
    ServoPos[3] = HomePos[3];
    ServoPos[6] = HomePos[6] - 10;
    
    wait_ms(50);
    /* 右にいって */
    ServoPos[0] = HomePos[0] + 25;
    ServoPos[2] = HomePos[2] + 10;
    ServoPos[4] = HomePos[4] + 15;
    ServoPos[5] = HomePos[5] + 15;
    ServoPos[6] = HomePos[6] ;
    wait_ms(1000);
    /* ツイスト */
    ServoPos[4] = HomePos[4] - 15;
    ServoPos[5] = HomePos[5] - 15;
    wait_ms(500);
    ServoPos[4] = HomePos[4] + 15;
    ServoPos[5] = HomePos[5] + 15;
    wait_ms(500);
    ServoPos[4] = HomePos[4] - 15;
    ServoPos[5] = HomePos[5] - 15;
    wait_ms(500);
    /* ホーム */
    ServoPos[0] = HomePos[0];
    ServoPos[1] = HomePos[1];
    ServoPos[2] = HomePos[2];
    ServoPos[3] = HomePos[3];
    ServoPos[6] = HomePos[6] - 10;
    wait_ms(500);
    /* 左にいって */
    ServoPos[0] = HomePos[0] - 10;
    ServoPos[2] = HomePos[2] - 25;
    ServoPos[6] = HomePos[6] ;
    wait_ms(1000);
    /* ツイスト */
    ServoPos[4] = HomePos[4] + 15;
    ServoPos[5] = HomePos[5] + 15;
    wait_ms(500);
    ServoPos[4] = HomePos[4] - 15;
    ServoPos[5] = HomePos[5] - 15;
    wait_ms(500);
    ServoPos[4] = HomePos[4] + 15;
    ServoPos[5] = HomePos[5] + 15;
    wait_ms(500);
    /* ホーム */
    ServoPos[0] = HomePos[0];
    ServoPos[1] = HomePos[1];
    ServoPos[2] = HomePos[2];
    ServoPos[3] = HomePos[3];
    ServoPos[6] = HomePos[6] - 10;
}

/*-----メイン-----*/
void main(void)
{
  /* 通信するための準備 */
  rs_init (br9600, txb, sizeof(txb), rxb, sizeof(rxb));
  fdevopen(rs_putchar, rs_getchar, 0);
  SREG |= (1<<SREG_I);                    /* 全割り込み許可 */
  rs_rx_purge();
  unsigned char key;

  /* 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();
  /* -----------------動作開始----------------- */
//  printf("Start\r\n");
  
  while(1)              /* ----------------ループ開始---------------- */
  {
    rs_rx_purge();    // ここでバッファをクリアしとかないと前のキー入力が残る
    key = rs_getc();
    switch(key){
        case 0x01:
            Go_Fw();
            break;
        case 0x02:
            Go_Bw();
            break;
        case 0x04:
            Go_Rt();
            break;
        case 0x08:
            Go_Lt();
            break;
        case 0x10:
            BLUE_KEY();
            break;
        case 0x20:
            RED_KEY();
            break;
        case 0x40:
            YELLOW_KEY();
            break;
        case 0x80:
            GREEN_KEY();
            break;
        case 0x30:
            BLUEandRED_KEY();
            break;
        
    }
    wait_ms(500);
    TIMSK &= ~(1<<OCIE1A);          // 脱力
  }
}