/*   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 KLRSM_REVR
#define KLRSM_REVL*/
#define USE_KLR_STEP_MOTOR
#define USE_KLR_RCSERVO
#define USE_KLR_DEAD_RECKONING
#define KLRM_PORT P1

#define USE_KLR_COLOR_SENSOR

#include "drtable_old.h"
/*#include "drtable_wakaba.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;
}

/* =================================================================*/
/* 移動系のサポートルーチン　                                       */

/* ロボット関連グローバル変数 */
#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;    /* 計算上の速度左右 */
/* 相対移動の基準点 */
volatile long RRefX,RRefY;
volatile int  RRefYaw;
volatile long RRefRelYaw;
volatile long RRefAbsStep;
volatile long RRefRelStep;

/* 動作スタック */
#define STACKDEPTH 4

struct __Stack {
  unsigned int _state;
  long RRefX,RRefY;
  int  RRefYaw;
  long RRefRelYaw;
  long RRefAbsStep;
  long RRefRelStep;
} StackBuffer[8]; /*STACKDEPTH];*/
unsigned int StackPoint;

int PushState(void)
{
  if(StackPoint>=STACKDEPTH) return -1; /* 普通に作ればあり得ない */
  StackBuffer[StackPoint]._state=state;
  StackBuffer[StackPoint].RRefX=RRefX;
  StackBuffer[StackPoint].RRefY=RRefY;
  StackBuffer[StackPoint].RRefYaw=RRefYaw;
  StackBuffer[StackPoint].RRefRelYaw=RRefRelYaw;
  StackBuffer[StackPoint].RRefAbsStep=RRefAbsStep;
  StackBuffer[StackPoint].RRefRelStep=RRefRelStep;
  StackPoint++;
}

int PopState(void)
{
  if(StackPoint<1) { state=0; return -1; }/* 普通に作ればあり得ない */
  StackPoint--;
  state=StackBuffer[StackPoint]._state;
  RRefX=StackBuffer[StackPoint].RRefX;
  RRefY=StackBuffer[StackPoint].RRefY;
  RRefYaw=StackBuffer[StackPoint].RRefYaw;
  RRefRelYaw=StackBuffer[StackPoint].RRefRelYaw;
  RRefAbsStep=StackBuffer[StackPoint].RRefAbsStep;
  RRefRelStep=StackBuffer[StackPoint].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 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))

#define NEXT state++;
#define GOSTRAIGHT(v,dist) { STATESTART { SETREF; } \
                             SetVelocityProfile(v,v,MMTOST((dist))-TLEN); \
                             ISSTOP { NEXT; } break; }
#define GOTURN(v,angle) { STATESTART { SETREF; } \
                          SetVelocityProfile(v,-(v),(DEGTOST(angle)-RRSTEP)/2); \
                          ISSTOP { NEXT; } break; }
          
#define GOSUB(ns) { PushState(); state=ns; break; }
#define RETSUB    { PopState(); NEXT; break; }


/* 速度設定 */
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;
    }
}

/* ライントレース関数 */
#define LSENSEMASK 0x00ff
#define LSMODEMASK 0xff00
#define LSNORMAL   0x0000
#define LSSTOPLINE 0x0100
#define LSNOSENSE  0x4000
#define LSINVALID  0x8000

/* ライントレース、左右速度を設定 */
/* mean=gain=0 でただのラインセンサ */
#define LINESENSE (LineTrace(0,0))
int LineTrace(int vmean,int gain) 
{
  int i,c,v;
  unsigned char lines=P3.DR.BYTE;
  /* 反応したセンサの数数え */
  c=0;
  for(i=0;i<8;i++)
    if(lines&(1<<i)) c++;
  if(c>4) return LSSTOPLINE | lines;
  if(c>3) return LSINVALID  | lines;
  if(c==0) return LSNOSENSE | lines;
  /* 平均位置検出 */
  v=0;
  for(i=0;i<8;i++)
    if(lines&(1<<i)) v+=i*2-7;
  v/=c;   /* v には平均位置 */
  if((vmean==0)&&(gain==0))
    return v;

  PVR=vmean-v*gain;
  PVL=vmean+v*gain;

  return LSNORMAL|lines;
}

int lastate;
int laclock;
int foundp[8];
int laneed;
unsigned char lamask;
/* ライン合わせ */
int LineAlign(int init)
{
  int i,j,c,v;
  unsigned char lines=P3.DR.BYTE;
  
  if(init) {
    lastate=0; laclock=0;
    for(i=0;i<8;i++) foundp[i]=-1;
    return 0;
  }

  laclock++;
  switch(lastate)
    {
    case 0: /* 微速前進 １秒*/
      PVR=PVL=100;
      LineTrace(30,5);
      if(laclock>70) lastate=1;
      break;
    case 1: 
      LineTrace(10,5);
      if(laclock>100)
        {
          /* ライントレース中は使えないデータがあるはず: 最初から黒の両隣 */
          lamask=~(lines|(lines<<1)|(lines>>1));
          c=0;
          for(i=0;i<8;i++)
            if(lamask&(1<<i)) c++;  /* c に 1の数 */
          laneed=c-1;
          if(laneed<2) laneed=2; /* 何らかのエラー対策が必要 */
          laclock=0; lastate=10;
        }
      break;
      
    case 10:/* 微速後退しつつセンシング */
      PVR=PVL=-50;
      lines=lines&lamask;
      for(i=0;i<8;i++)
        if((foundp[i]<0)&&(lines&(1<<i)))
          {
            foundp[i]=laclock;
            SLReply16(0x3f,i,laclock);
          }
            c=0;
      for(i=0;i<8;i++)
        if(foundp[i]>=0) c++;
      if(c>=laneed) { PVL=PVR=0; lastate++;}
      if(laclock>400) { return 1; } /* 探知失敗 */
      break;
    case 11: /* 姿勢計算 */
      PVR=PVL=0;
      c=v=0;
      for(j=0;j<8;j++)
        if(foundp[j]>=0) break;
      for(i=j;i<8;i++)
        {
          if(foundp[i]<0) continue;
          v+=foundp[i]-foundp[j];
          c+=i-j;
        }
      v=v*16/c;
      SLReply16(0x3f,10,v);
      return v;
    }
  return 0;
}




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


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;
  int lax,lasign;

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

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


  nexttime=SysTime;
  while(1)
    {
      /* １００Ｈｚを待つ */
      KLRColorSense();
      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;


          /* 110 から212 までは参考にする必要なし：過去の遺産 */
          
          /* 距離で止まる ：加減速自動：減速時誤差あり*/
        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;

          /* 110 から212 までは参考にする必要なし：過去の遺産 */

          /* 加減速駆動関数の実験 */
          /* SetVelocityProfile(右最高速,左最高速(step/s), 動作条件(>0)) */
          /* 左右の速度比をなるべく保ちつつ台形加減速する */
          /* ついでにラジコンサーボ KLRS_Command[0]〜[7]に設定。
             S3003 で 10000から58000の範囲を指定：その外は異音 */
        case 220:
          loopcount=4;  state++;  break;
        case 221:
          STATESTART { SETREF; KLRS_Command[0]=13000; }
          SetVelocityProfile(400,400,MMTOST(500)-TLEN); /* 500mm 前進, 速度400 */
          ISSTOP { state++; }  /* 次へ */
          break;
        case 222:
          STATESTART { SETREF; KLRS_Command[0]=50000; }
          SetVelocityProfile(400,-400,(DEGTOST(90)-RRSTEP)/2);
          /*                  絶対値で90度旋回、右+400左-400 */
          ISSTOP {
            loopcount--;
            if(loopcount==0) state=9999; else state=221;
          }  /* 次へ */
          break;

          /* 姿勢検出＋姿勢合わせ旋回 */
        case 300:
          if(LineTrace(300,50)&LSSTOPLINE)
            state++;
          break;
        case 301:
          STATESTART { LineAlign(1); }
          lax=LineAlign(0);
          if(lax>0) { lax= lax; lasign= 1; state++; break; }
          if(lax<0) { lax=-lax; lasign=-1; state++; break; }
          break;
        case 302:
          STATESTART { SETREF; }
          SetVelocityProfile(-200*lasign,200*lasign,(DEGTOST(lax)/7-RRSTEP)/2);
          ISSTOP { state=9999; } 
          break;

          /* ライントレース→姿勢合わせ→姿勢センサの位置で旋回 */
        case 400:
          if(LineTrace(200,60)&LSSTOPLINE)
            NEXT;
          break;
        case 401:
          STATESTART { LineAlign(1); }
          lax=LineAlign(0);
          if(lax>0) { lax= lax; lasign= 1; NEXT; break; }
          if(lax<0) { lax=-lax; lasign=-1; NEXT; break; }
          break;

        case 402:
          STATESTART { SETREF; }
          SetVelocityProfile(-200*lasign,200*lasign,(DEGTOST(lax)/7-RRSTEP)/2);
          ISSTOP { NEXT; } 
          break;
        case 403: /* 超心地位置へ */
          STATESTART { SETREF; }
          SetVelocityProfile(200,200,MMTOST(80)-TLEN);
          ISSTOP {
            lineofs=LINESENSE;
            if((lineofs&LSMODEMASK)!=LSNORMAL) lineofs=0; /* エラー：オフセット補正なし */
            lineofs=lineofs&LSENSEMASK;
            NEXT;
          }  /* 停止したら次へ */
          break;
        case 404: /* 旋回 */
          GOTURN(200,90); break;
        case 405: /* もどり */
          GOSTRAIGHT(-200,80+lineofs*5); break;
          ISSTOP { state=9999; }
          break;

          /* 短縮命令の実験 */
          /* 短縮命令は動作終了後停止してから次のstateに移行 */
          /* GOSTRAIGHT(速度,距離) 直進：前進(+)後進(-)*/
          /* GOTURN(速度,角度) 超心地旋回：反時計(+)時計(-)*/
        case 500: GOSTRAIGHT( 400,500);  break; /* 500mm 前進 */
        case 501: GOSTRAIGHT(-400,500);  break; /* 500mm 後退 */
        case 502: GOTURN(400,90);        break; /* 90度反時計旋回 */
        case 503: GOTURN(-400,90);       break; /* 90度時計旋回 */

          /* サブルーチンコールの実験 */
          /* GOSUB(サブルーチン開始state) */
          /* RETSUB サブルーチン終了 */
          /* サブルーチンの深さは現在４レベル */
          /* RETSUBでもどると GOSUBの次のstateに移行 */
        case 600: GOSUB(610); break; /* 面取り正方形 */
        case 601: GOSUB(620); break;
        case 602: GOSUB(610); break;
        case 603: GOSUB(620); break;
        case 604: GOSUB(610); break;
        case 605: GOSUB(620); break;
        case 606: GOSUB(610); break;
        case 607: GOSUB(620); break;

            /* 300mm前進して45度旋回 */
        case 610: GOSTRAIGHT( 400,300);  break; /* 300mm 前進 */
        case 611: GOTURN(400,45);        break; /* 45度反時計旋回 */
        case 612: RETSUB;

            /* 100mm前進して45度旋回 */
        case 620: GOSTRAIGHT( 400,100);  break; /* 100mm 前進 */
        case 621: GOTURN(400,45);        break; /* 45度反時計旋回 */
        case 622: RETSUB;

        case 700: 
          STATESTART { }
          if(LineTrace(300,60)&LSSTOPLINE)
            NEXT;
          break;
        case 701:
          STATESTART { lax=KLRCS_Sense[0]; KLRS_Command[0]=10000+lax*15000;
            vr=vl=300; lax--; if(lax<=0) state=9999; }
          STIMEGT(20) {NEXT; }
          break;
        case 702:
          if(LineTrace(300,60)&LSSTOPLINE)
            NEXT;
          break;
        case 703:
          STATESTART { vr=vl=300; lax--; if(lax<=0) state=9999; }
          STIMEGT(20) {NEXT; }
          break;
        case 704:
          if(LineTrace(300,60)&LSSTOPLINE)
            NEXT;
          break;

        case 705:
          state=9999;
          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%500)==0)
        {
          SLReply32(0x3f, 8,RegFileL[ 8]);  /* SLでsermon に自動送信 */
          SLReply32(0x3f, 9,RegFileL[ 9]);  /* SLでsermon に自動送信 */
          SLReply32(0x3f,10,RegFileL[10]);
        }
    }
}