#include <3694.h>       // H8 Tinyの内部I/O定義

// 送受信用バッファ
char txb[20], rxb[20];

/*----------------------------------------------------------------------------*/
/*                                                                            */
/*                                                                            */
/*                          その他関数                                        */
/*                                                                            */
/*                                                                            */
/*----------------------------------------------------------------------------*/

/*----------------------------------------------------------------------------------------*/
/* 引数ｘ20msウェイト                                                                     */
/* (CPUクロック２０MHｚ、1/128クロックで１カウントモード、156倍で19968クロック＝約１ｍＳ) */
/*----------------------------------------------------------------------------------------*/
void wait (unsigned char speed)
{
  long spd;
  spd = speed * 20;         //引数を２０倍
  while (spd){
    /*１ｍＳウェイト*/
    TV.TCRV0.BYTE = 0x03;   //下の行と合わせて1/128クロックで1カウントモード
    TV.TCRV1.BYTE = 0X01;
    while(TV.TCNTV < 156);
    TV.TCNTV = 0;           //カウンタクリア
    spd--;
  }
}


/*----------------------------------------------------------------------------*/
/*                                                                            */
/*                                                                            */
/*                          モーションデータ関連                              */
/*                                                                            */
/*                                                                            */
/*----------------------------------------------------------------------------*/

/*--------------------------------------------------*/
/* 前に起き上がり                                   */
/*--------------------------------------------------*/
void RoboMotion_okiagari_FW (void)
{
//  RoboMotion_home_P(40);
  unsigned char step2[24] = {0,143,1,137,2,190,3,163,4,119,5,127,6,133,7,133,8,87,9,66,10,129,11,127};
  snd_step(step2,35);
  RoboMotion_home_P(40);
}

/*--------------------------------------------------*/
/* 後ろにら起き上がり                               */
/*--------------------------------------------------*/
void RoboMotion_okiagari_BW (void)
{
//  RoboMotion_home_P(20);
  unsigned char step2[24] = {0,143,1,137,2,100,3,183,4,119,5,127,6,133,7,133,8,67,9,156,10,129,11,127};
  snd_step(step2,20);
  RoboMotion_home_P(20);
  unsigned char step3[24] = {0,143,1,137,2,100,3,183,4,119,5,127,6,133,7,133,8,67,9,156,10,129,11,127};
  snd_step(step3,20);
  RoboMotion_home_P(20);
}

/*--------------------------------------------------*/
/* ホームポジション                                 */
/*--------------------------------------------------*/
void RoboMotion_home_P (unsigned char spd)
{
  unsigned char step1[24] = {0,143,1,137,2,100,3,113,4,119,5,127,6,133,7,133,8,137,9,156,10,129,11,127};
  snd_step(step1,spd);
}

/*--------------------------------------------------*/
/* ステップデータ送信                               */
/*--------------------------------------------------*/
int snd_step ( unsigned char * snd, unsigned char snd_speed )
{
  unsigned char cnt;
  for (cnt = 0; cnt < 24; cnt=cnt+2){
    SCI3_OUT_DATA(255);
    SCI3_OUT_DATA(snd[cnt]);
    SCI3_OUT_DATA(snd[cnt+1]);
    SCI3_OUT_DATA(snd_speed);
  }
  wait(snd_speed);
  return 0;
}


/*----------------------------------------------------------------------------*/
/*                                                                            */
/*                                                                            */
/*                          ２軸加速度センサー                                */
/*                          Ｐ１２をＸ出力へ                                  */
/*                          Ｐ１１をＹ出力へ接続                              */
/*----------------------------------------------------------------------------*/
/*------------------------------------------------------*/
/*            Ｘ軸計測関数（Ｐ１２からパルス入力）      */
/*------------------------------------------------------*/
unsigned int Acc_X_OUT(void)
{
    unsigned int tilt = 0;
    while (IO.PDR1.BIT.B2);     //パルスの立下りまで待つ(P12)
    while (!IO.PDR1.BIT.B2);    //パルスの立上がりまで待つ
    TA.TMA.BYTE = 0x13;         //タイマーＡ、クロックの１／５１２カウントモード
    while (IO.PDR1.BIT.B2);     //パルスの立下りまで待つ（計測終了）
    tilt = TA.TCA;              //タイマーの値を変数へ格納
    TA.TMA.BYTE = 0x1F;         //タイマーＡリセット
    return tilt;                //結果を返す
}

/*------------------------------------------------------*/
/*            Ｙ軸計測関数（Ｐ１１からパルス入力）      */
/*------------------------------------------------------*/
unsigned int Acc_Y_OUT(void)
{
    unsigned int tilt = 0;
    while (IO.PDR1.BIT.B1);     //パルスの立下りまで待つ(P11)
    while (!(IO.PDR1.BIT.B1));  //パルスの立上りまで待つ
    TA.TMA.BYTE = 0x13;         //タイマーＡ、クロックの１／５１２カウントモード
    while (IO.PDR1.BIT.B1);     //パルスの立下りまで待つ（計測終了）
    tilt = TA.TCA;              //タイマーの値を変数へ格納
    TA.TMA.BYTE = 0x1F;         //タイマーＡリセット
    return tilt;                //結果を返す
}

/*----------------------------------------------------------------------------*/
/*                                                                            */
/*                                                                            */
/*                          メインルーチン                                    */
/*                                                                            */
/*                                                                            */
/*----------------------------------------------------------------------------*/

/*-----------------------------------------------*/
/* メイン                                        */
/*-----------------------------------------------*/
void main(void)
{ 
  
  /* 通信ポートを初期化 ボーレートは9600[bps]*/
  SCI3_INIT (br9600, txb, sizeof(txb), rxb, sizeof(rxb));
  EI;                             // 初期化後は割り込みを許可状態に
  
  /* ２軸加速度センサー入力ピンの設定*/
  IO.PCR1 = 0x00;                 //P11とP12を入力ポートへ設定
  
  unsigned int angle;
  
  RoboMotion_home_P(60);
  wait(100);
  while(1){
      angle = Acc_X_OUT();
      if (angle > 220){
          wait(50);
          RoboMotion_okiagari_FW();
      }
      if (angle < 170){
          wait(50);
          RoboMotion_okiagari_BW();
      }
  }

}