/*   KumaLab Robot Library test program (robotmotion)   */
#include "3048f.h"

/* =========================== */
/* SerialLoop のための初期設定 */
/* =========================== */

/* SL に使用するSCI    default SCI1*/
/* 同時に、serloops.mar を参考に割込ベクタを設定する */
#define SCISL SCI1

/* SCI 通信速度レジスタ */
/*#define SCISLBRR 13   25MHz 57600 */
#define SCISLBRR 13

/* 転送許可 : undef すると転送しない＝ループが切れる */
/* 一般のノードでは普通はENABLE */
#define ENABLE_FORWARD

/* 機能選択 */
#define USE_MEMEXT   /* メモリ拡張追加 */
#define USE_PACKETFUNC 

/* ライブラリ読み込み */
#include "slnode.c"

/* 基本的に、このレジスタが上位とつながる */
extern volatile unsigned int RegFileS[REGFILEC];
extern volatile unsigned long RegFileL[REGFILEC];



/* =========================== */
/* KumaLab RoboLib             */
/* =========================== */
#define USE_KLR_WDT_CLOCK
#define USE_KLR_STEP_MOTOR
#define USE_KLR_RCSERVO
#define USE_KLR_DEAD_RECKONING
#define KLRM_PORT P1
#include "drtable_old.h"
#include "KLRlib.c"









/* ワールド座標(x,y)→ロボット座標(lx,ly) */
void LocalCoord(long x,long y,int yaw,long *lx,long *ly)
{
  long c,s;
  c=XDeltaTable[yaw]*2;
  s=YDeltaTable[yaw]*2;
  *lx =  Mul_FP16_FP16(x,c)+Mul_FP16_FP16(y,s);
  *ly = -Mul_FP16_FP16(x,s)+Mul_FP16_FP16(y,c);
}



/* mod を tagに近づける、最大加速aclimit 最大減速 brlimit*/
long ModifyLimitL(int mod,int tag,int aclimit,int brlimit)
{
  /* mod: 現在値  tag: 目標値 */
  if(0<mod) 
    {
      if(mod<tag)  /* 0<mod<tag : 加速 */
      {
         if(tag-mod>aclimit) return mod+aclimit;
         return tag;
      }
      else         /* 0<tag<mod : 減速 */
      {
         if(tag<0) tag=0;  /* ゼロを挟む時は一度目標ゼロ */
         if(mod-tag>brlimit) return mod-brlimit;
         return tag;
      }
    }
  else if(0>mod)
    {
      if(mod<tag)  /* mod<tag<0 : 減速 */
      {
         if((mod<0)&&(tag>0)) tag=0;  /* ゼロを挟む時は一度目標ゼロ */
         if(tag-mod>brlimit) return mod+brlimit;
         return tag;
      }
      else  /* tag<mod<0 : 加速 */
      {
         if(mod-tag>aclimit) return mod-aclimit;
         return tag;
      }
    }
  else
    {
      if(tag<0) return -aclimit;
      if(tag>0) return aclimit;
    }
  return 0;
}

/* =================================================================*/
/* 移動系のサポートルーチン　                                       */
/* 相対移動の基準点 */
volatile long RRefX,RRefY;
volatile int  RRefYaw;
volatile long RRefRelYaw;
volatile long RRefAbsStep;
volatile long RRefRelStep;

/* 相対移動の基準点設定 */
void SetRelReference(void)
{
  RRefX=KLRDR_PosX; RRefY=KLRDR_PosY;
  RRefYaw=KLRDR_Yaw;
  RRefAbsStep=KLRDR_AbsStep;
  RRefRelStep=KLRDR_RelStep;
  RRefRelYaw=KLRDR_RelYaw;
}

/* 移動距離 */
long TravelLength(void)
{
  long a=(RRefRelStep-KLRDR_RelStep)>>1;  /* /2 */
  if(a>0) return a;
  return -a;
}

/* 動作ステップ数 */
long TravelStep(void)
{
  long a=(RRefAbsStep-KLRDR_AbsStep)>>1;  /* /2 */
  if(a>0) return a;
  return -a;
}

/* 旋回角度 */
long RelRotateStep(void)
{
  long a=(RRefRelYaw-KLRDR_RelYaw);
  if(a>0) return a;
  return -a;
}



#define SETREF SetRelReference()
#define TLEN TravelLength()
#define TSTEP TravelStep()
#define RRSTEP RelRotateStep()

/* =================================================================*/
/* ここから本体関数                                                 */
/* =================================================================*/

/* ロボット関連グローバル変数 */
#define state (RegFileS[0])
#define posx KLRDR_PosX
#define posy KLRDR_PosY
#define yaw  KLRDR_Yaw
int accellimit;  /* 加速上限 */
int brakelimit; /* 減速上限 */
long clock, stateclock;  /* clock */
int PVR,PVL;    /* 計算上の速度左右 */

#define STATESTART if(stateclock==0)     /* 最初 */
#define STIME(a) if(stateclock==(a))     /* 時刻(a) */
#define STIMEGT(a) if(stateclock>(a))    /* 時間a経過したら*/
#define STIMELT(a) if(stateclock<(a))    /* 時間a経過するまで*/
#define MMTOST(a) ((((long)a)<<16)/MMPERSTEP)  /* 距離mmを内部単位に変換 */
#define DEGTOST(a) (((long)a)*YAWSTEP/360)  /* 角度[deg]を内部単位に変換 */
#define ISSTOP if((KLRM_SpeedL==0)&&(KLRM_SpeedR==0)&&(vr==0)&&(vl==0))

/* 速度設定 */
void SetVelocityProfile(int dvr,int dvl,long rest)
{
  int vmax;
  int sr=(dvr<0)?-1:1; /* 負号 */
  int sl=(dvl<0)?-1:1;

  if(rest<0) { PVR=PVL=0; return ;}

  dvr=(dvr>0)?dvr:(-dvr); /* 絶対値 */
  dvl=(dvl>0)?dvl:(-dvl);
  vmax=(dvr>dvl)?dvr:dvl; /* 大きい方 */
  
  if(rest<0) rest=0;
  rest=SqrtI(2*rest*(brakelimit-1)*100)+2;
  rest=Limit(rest,0,vmax);
  if(dvr>dvl)
    {
      PVR=rest*sr;
      PVL=rest*dvl/dvr*sl;
    }
  else
    {
      PVR=rest*dvr/dvl*sr;
      PVL=rest*sl;
    }
}



void main(void)
{
  long nexttime;
  unsigned long sec;
  int i;

  int laststate;
#define vr PVR
#define vl PVL
  
  long tagx,tagy,lx,ly;
  int vmean,vdelta;

  int loopcount;

  sec=0;
  accellimit=3; brakelimit=10;
  clock=stateclock=0;
  laststate=-1;
  
  InitializeSerialLoop(2);
  InitializeKLRlib();

  for(i=0;i<16;i++)
    {
      RegFileS[i]=0;
      RegFileL[i]=0;
    }


  nexttime=SysTime;
  while(1)
    {
      /* １００Ｈｚを待つ */
      while(SysTime<nexttime)
        ;
      nexttime=SysTime+82;  /* 8192Hz/100Hz */
      sec++;

      /* 時間周り処理 */
      clock++;   /* 時刻、約100カウント/秒 */
      stateclock++;
      if(state!=laststate)   /* モードが変更された */
        stateclock=0;
      laststate=state;

      switch(state)
        {
          /* ======================================================== */
        case 0:     /* なにもしない：目標ゼロ */
          vr=vl=0;
          break;
          
          /* ======================================================== */
        case 1:     /* 左右速度指令 */
          vr=(int)RegFileS[6];
          vl=(int)RegFileS[7];
          break;

         /* ======================================================== */
        case 2:     /* ：固定点目標 */
          /* 目標位置 */
          tagx=(int)(RegFileS[6])*0x10000l;
          tagy=(int)(RegFileS[7])*0x10000l;
          /* ロボットの座標系での目標位置→lx(前),ly(左) */
          LocalCoord(tagx-posx,tagy-posy,yaw,&lx,&ly);
          /* 平均速度 */
          vmean= Mul_FP16_FP16(lx,RegFileS[4])>>12;
          /* 左右速度差 */
          vdelta=Mul_FP16_FP16(ly,RegFileS[5])>>12;
          /* 最大速度 */
          vr=Limit(vmean+vdelta,-600,600);
          vl=Limit(vmean-vdelta,-600,600);
          /* 不感帯 */
          vr=ZeroMask(vr,4);
          vl=ZeroMask(vl,4);
          /* 確認用 */
          RegFileS[2]=vmean;
          RegFileS[3]=vdelta;
          break;



          /* モーションサンプル */
        case 100: /* 時間で止まる */
          STATESTART { vr=100; vl=100; }
          STIMEGT(100) { state=9999; }
          break;

          /* 距離で止まる ：加減速自動：減速時誤差あり*/
        case 110:
          STATESTART { vr=400; vl=400; SETREF; }
          if(TLEN > MMTOST(500)) { state=9999; }  /* MMTOST(距離mm) */
          break;

        case 111:
          STATESTART { SETREF; }
/*          {
            long x=MMTOST(500)-TLEN;
            if(x<0) x=0;
            x=SqrtI(2*x*(brakelimit-1)*100)+2;
            x=Limit(x,0,400);
            vr=vl=x;
          }*/
          SetVelocityProfile(400,400,MMTOST(500)-TLEN);
          if(TLEN > MMTOST(500)) { state=9999; }  /* MMTOST(距離mm) */
          break;
        case 112:
          STATESTART { SETREF; }
          SetVelocityProfile(-400,-400,MMTOST(500)-TLEN);
          if(TLEN > MMTOST(500)) { state=9999; }  /* MMTOST(距離mm) */
          break;
          /* 角度で止まる ：加減速自動：減速時誤差あり*/
        case 120:
          STATESTART { vr=200; vl=-200; SETREF; }
          if(RRSTEP > DEGTOST(90)) { state=9999; }  /* MMTOST(距離mm) */
          break;

        case 121:
          STATESTART { SETREF; }
          SetVelocityProfile(400,-400,(DEGTOST(90)-RRSTEP)/2);
          if(RRSTEP >= DEGTOST(90)) { state=9999; }  /* MMTOST(距離mm) */
          break;
          /* 角度で止まる ：加減速自動：減速時誤差あり*/

          /* 四角 */
        case 200:
          STATESTART { vr=400; vl=400; SETREF; }
          if(TLEN > MMTOST(500)) { vr=vl=0; }  /* MMTOST(距離mm) */
          ISSTOP { state++; }  /* 次へ */
          break;
        case 201:
          STATESTART { vr=400; vl=-400; SETREF; }
          if(RRSTEP > DEGTOST(90)) { vr=vl=0; }  /* MMTOST(距離mm) */
          ISSTOP { state++; }  /* 次へ */
          break;
        case 202:
          STATESTART { vr=400; vl=400; SETREF; }
          if(TLEN > MMTOST(500)) { vr=vl=0; }  /* MMTOST(距離mm) */
          ISSTOP { state++; }  /* 次へ */
          break;
        case 203:
          STATESTART { vr=400; vl=-400; SETREF; }
          if(RRSTEP > DEGTOST(90)) { vr=vl=0; }  /* MMTOST(距離mm) */
          ISSTOP { state++; }  /* 次へ */
          break;
        case 204:
          STATESTART { vr=400; vl=400; SETREF; }
          if(TLEN > MMTOST(500)) { vr=vl=0; }  /* MMTOST(距離mm) */
          ISSTOP { state++; }  /* 次へ */
          break;
        case 205:
          STATESTART { vr=400; vl=-400; SETREF; }
          if(RRSTEP > DEGTOST(90)) { vr=vl=0; }  /* MMTOST(距離mm) */
          ISSTOP { state++; }  /* 次へ */
          break;
        case 206:
          STATESTART { vr=400; vl=400; SETREF; }
          if(TLEN > MMTOST(500)) { vr=vl=0; }  /* MMTOST(距離mm) */
          ISSTOP { state++; }  /* 次へ */
          break;
        case 207:
          STATESTART { vr=400; vl=-400; SETREF; }
          if(RRSTEP > DEGTOST(90)) { vr=vl=0; }  /* MMTOST(距離mm) */
          ISSTOP { state=9999; } 
          break;

        case 210:
          loopcount=4;  state++;  break;
        case 211:
          STATESTART { vr=400; vl=400; SETREF; }
          if(TLEN > MMTOST(400)) { vr=vl=50; }  /* MMTOST(距離mm) */
          if(TLEN > MMTOST(500)) { vr=vl=0; }  /* MMTOST(距離mm) */
          ISSTOP { state++; }  /* 次へ */
          break;
        case 212:
          STATESTART { vr=200; vl=-200; SETREF; }
          if(RRSTEP > DEGTOST(75)) { vr=30; vl=-30; }  /* MMTOST(距離mm) */
          if(RRSTEP > DEGTOST(90)) { vr=vl=0; }  /* MMTOST(距離mm) */
          ISSTOP {
            loopcount--;
            if(loopcount==0) state=9999; else state=211;
          }  /* 次へ */
          break;


        case 220:
          loopcount=4;  state++;  break;
        case 221:
          STATESTART { SETREF; KLRS_Command[0]=13000; }
          SetVelocityProfile(400,400,MMTOST(500)-TLEN);
          ISSTOP { state++; }  /* 次へ */
          break;
        case 222:
          STATESTART { SETREF; KLRS_Command[0]=50000; }
          SetVelocityProfile(400,-400,(DEGTOST(90)-RRSTEP)/2);
          ISSTOP {
            loopcount--;
            if(loopcount==0) state=9999; else state=221;
          }  /* 次へ */
          break;
          
        case 9999:
          vr=vl=0;
          break;
         /* ======================================================== */
         default:     /* その他の場合：何もしない：目標ゼロ */
            vr=vr=0;
            break;
      }

      /* 加速度リミット＋上限設定 */
      KLRM_SpeedR=ModifyLimitL(KLRM_SpeedR,PVR,accellimit,brakelimit);
      KLRM_SpeedL=ModifyLimitL(KLRM_SpeedL,PVL,accellimit,brakelimit);
      RegFileL[4]=KLRM_SpeedR;
      RegFileL[5]=KLRM_SpeedL;

      
      /* ステータス出力 */
      RegFileL[ 0]=sec;
      RegFileL[ 6]=posx;
      RegFileL[ 7]=posy;
      RegFileL[ 8]=HexToIntFP16(Mul_FP16_FP16(posx,MMPERSTEP));
      RegFileL[ 9]=HexToIntFP16(Mul_FP16_FP16(posy,MMPERSTEP));
      RegFileL[10]=HexToIntFP16(DegTable[yaw]);
      
      RegFileS[ 8]=AD.DRA>>6;
      RegFileS[ 9]=AD.DRB>>6;
      RegFileS[10]=AD.DRC>>6;
      RegFileS[11]=AD.DRD>>6;

      if((sec%100)==0)
        {
          SLReply32(0x3f, 8,RegFileL[ 8]);  /* SLでsermon に自動送信 */
          SLReply32(0x3f, 9,RegFileL[ 9]);  /* SLでsermon に自動送信 */
          SLReply32(0x3f,10,RegFileL[10]);
        }

    }
}