#include <3052.h>

int cnt = 0;            /* ループカウント */
int sv[25];             /* サーボ変数 */

int cnt1ms = 17;        /* int(1ms/58usec) */
int cnt20ms = 0x014f;   /* int(20 * 1ms/58usec) */
int N = 24;             /* ニュートラル位置の値 */

int MT = 30;            /* サーボ一段階分のウェイト時間 */

int swapM[256];         /* 実行用モーションデータコピー先 */
int doMotion = 0;       /* 実行中のモーション番号 */
int dmNum = 0;          /* モーションデータ添え字 */

/* モーションデータ定義 ====================================
   サーボ番号,角度1,単体動作1, のように指定していく
   サーボ番号は0〜24
   角度は30段階（-15〜15）、-15で0度、0で90度、15で180度のように6度づつ動く
   単体動作を0にすると次のシーケンスと同時に動く
   ウェイトを入れる場合は 255,ウェイト時間, を入れる
   256で終了 */
int Motion01[69] = {
  255,5000,     /*ウェイト*/
  0,15,1,
  1,15,1,
  2,15,1,
  3,15,1,
  255,1000,     /*ウェイト*/
  0,0,0,
  1,0,0,
  2,0,0,
  3,0,1,
  255,1000,     /*ウェイト*/
  0,15,0,
  1,15,0,
  2,15,0,
  3,15,1,
  255,1000,     /*ウェイト*/
  0,-15,0,
  1,-15,0,
  2,-15,0,
  3,-15,1,
  0,0,0,
  1,0,0,
  2,0,0,
  3,0,1,
  256           /*終了*/
  };

int Motion02[15] = {
  0,15,0,
  1,15,1,
  255,1000,     /*ウェイト*/
  2,15,0,
  3,15,1,
  256           /*終了*/
  };
 
/* 関数群 ================================================== */

/* ウェイト ############################ */
void wait(int msec)
{
  ITU.TSTR.BIT.STR1 = 1;
  int i;
  for(i=0;i<msec;i++)
  {
    while(ITU1.TSR.BIT.IMFA == 0);
    ITU1.TSR.BIT.IMFA = 0;
  }
  ITU.TSTR.BIT.STR1 = 0;
  return;
}

/* サーボ動作中断 ###################### */
void stopSv(void)
{
  doMotion = 0;
  dmNum = 0;
}

/* サーボリセット ###################### */
void resetSv(void)
{
  stopSv();
  int i;
  for(i=0; i<=sizeof(sv); i++) sv[i]  = N; /* 初期位置をニュートラルに設定 */
}

/* サーボ設定・動作開始 ################ */
void gotoMotion(int num)
{
  if(doMotion==0)
  {
    doMotion = num;
    dmNum = 0;
    /* モーション1配列コピー */
    if(doMotion == 1)
    {
      int i;
      for (i = 0 ;i < ( sizeof(Motion01) / sizeof(Motion01[0]) ) ;i++) swapM[i] = Motion01[i];
    }
    /* モーション1配列コピー */
    if(doMotion == 2)
    {
      int i;
      for (i = 0 ;i < ( sizeof(Motion02) / sizeof(Motion02[0]) ) ;i++) swapM[i] = Motion02[i];
    }
  }
}

/* サーボ動作・監視 #################### */
void moveSv(void)
{
  if(doMotion!=0)
  {
    /* モーション停止 ================== */
    if(swapM[dmNum]==256) stopSv();
    /* ウェイト実行 ==================== */
    else if(swapM[dmNum]==255)
    {
      wait(swapM[dmNum+1]);
      dmNum+=2; // 次のシーケンスへ
    }
    /* モーション実行 ================== */
    else if(swapM[dmNum]>=0 && swapM[dmNum]<=24)
    {
      int tmpRad=0; // 動作前の位置
      int tmpWait=0; // サーボ移動のウェイト時間
      int tmpWaitBig=0; // サーボ移動のウェイト時間（複数の場合の長いほう）
      while(1)
      {
        tmpRad = sv[swapM[dmNum]]; // 現在のサーボ位置を記憶
        sv[swapM[dmNum]] = N + swapM[dmNum+1]; // サーボを動かす
        if(sv[swapM[dmNum]]>=tmpRad) tmpWait = (sv[swapM[dmNum]]-tmpRad)*MT; // 移動量からウェイトタイム割り出し
        else if(sv[swapM[dmNum]]<tmpRad) tmpWait = (tmpRad-sv[swapM[dmNum]])*MT;
        if(tmpWait>=tmpWaitBig) tmpWaitBig=tmpWait; // でかいほうのウェイトを代入
        if(swapM[dmNum+2]!=0) break; // 単体動作の場合抜ける
        dmNum+=3; // 次のシーケンスへ
      }
      wait(tmpWaitBig); // サーボ用ウェイト
      dmNum+=3; // 次のシーケンスへ
    }
  }
}

/* ITU0のGRA割り込みルーチン =============================== */
void int_imia0(void)
{
  cnt++;
  if(cnt == cnt20ms)
  {
    /* 20msで全て1に */
    PA.DR.BYTE = 0x7c;
    PB.DR.BYTE = 0xff;
    P1.DR.BYTE = 0xff;
    P6.DR.BYTE = 0x7f;
    cnt = 0;
  }
  else 
  {
    /* 各設定角度にてゼロクリア */
    if(cnt == sv[0] ) PA.DR.BIT.B2 = 0; // CN1-10
    if(cnt == sv[1] ) PA.DR.BIT.B3 = 0; // CN1-11
    if(cnt == sv[2] ) PA.DR.BIT.B4 = 0; // CN1-12
    if(cnt == sv[3] ) PA.DR.BIT.B5 = 0; // CN1-13
    if(cnt == sv[4] ) PA.DR.BIT.B6 = 0; // CN1-14
    if(cnt == sv[5] ) PB.DR.BIT.B0 = 0; // CN1-16
    if(cnt == sv[6] ) PB.DR.BIT.B1 = 0; // CN1-17
    if(cnt == sv[7] ) PB.DR.BIT.B2 = 0; // CN1-18
    if(cnt == sv[8] ) PB.DR.BIT.B3 = 0; // CN1-19
    if(cnt == sv[9] ) PB.DR.BIT.B4 = 0; // CN1-20
    if(cnt == sv[10]) PB.DR.BIT.B5 = 0; // CN1-21
    if(cnt == sv[11]) PB.DR.BIT.B6 = 0; // CN1-22
    if(cnt == sv[12]) PB.DR.BIT.B7 = 0; // CN1-23
    if(cnt == sv[13]) P1.DR.BIT.B0 = 0; // CN3-15
    if(cnt == sv[14]) P1.DR.BIT.B1 = 0; // CN3-16
    if(cnt == sv[15]) P1.DR.BIT.B2 = 0; // CN3-17
    if(cnt == sv[16]) P1.DR.BIT.B3 = 0; // CN3-18
    if(cnt == sv[17]) P1.DR.BIT.B4 = 0; // CN3-19
    if(cnt == sv[18]) P1.DR.BIT.B5 = 0; // CN3-20
    if(cnt == sv[19]) P1.DR.BIT.B6 = 0; // CN3-21
    if(cnt == sv[20]) P1.DR.BIT.B7 = 0; // CN3-22
    if(cnt == sv[21]) P6.DR.BIT.B0 = 0; // CN3-35
    if(cnt == sv[23]) P6.DR.BIT.B1 = 0; // CN3-36
    if(cnt == sv[24]) P6.DR.BIT.B2 = 0; // CN3-37
  }
  ITU0.TSR.BIT.IMFA = 0;
}

/* メイン関数 ============================================== */
void main(void)
{
  EI;                         /* イネーブルインタラプト（全割込み許可） */
  
  ITU0.TCR.BYTE = 0x20;       /* GRAでコンペアマッチ、分周無し */
  ITU0.GRA = 0x0590;          /* 0x05c0 信号が安定する限界の最小カウント回数 */
  ITU0.TIER.BYTE = 0xf9;      /* ITU0はGRAのコンペアマッチで割り込み */
  ITU.TSTR.BIT.STR0 = 0;

  ITU1.TCR.BYTE = 0x23;
  ITU1.GRA = 0x0c35;
  ITU.TSTR.BIT.STR1 = 0;
  
  P4.DDR = 0x00;
  P4.PCR.BYTE = 0xf0;
  //P5.DDR = 0x03;
  PA.DDR = 0x7c;    /* 0111 1100 */
  P2.DDR = 0x00;
  P2.PCR.BYTE = 0xff;
  PB.DDR = 0xff;
  P1.DDR = 0xff;
  P6.DDR = 0x7f;

  resetSv(); // 全てニュートラル

  ITU.TSTR.BIT.STR0=1;  /* 割り込みスタート */

  while(1)
  {
    if(P4.DR.BIT.B4 == 0)
    {
      while(P4.DR.BIT.B4 == 0); // 押している間は実行しない
      gotoMotion(1); // モーション1実行
    }
    if(P4.DR.BIT.B5 == 0)
    {
      while(P4.DR.BIT.B5 == 0); // 押している間は実行しない
      gotoMotion(2); // モーション2実行
    }
    if(P4.DR.BIT.B6 == 0)
    {
      while(P4.DR.BIT.B6 == 0); // 押している間は実行しない
      resetSv(); // リセット
    }
    /* サーボ監視・動作 */
    moveSv();
  }
}