/*   KumaLab Robot Library test program (WDT)   */
#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"



/* =========================== */
/* KumaLab RoboLib             */
/* =========================== */
#define USE_KLR_WDT_CLOCK
#define USE_KLR_STEP_MOTOR
#define USE_KLR_DEAD_RECKONING
#define KLRM_PORT PA

#include "drtable_old.h"
#include "KLRlib.c"

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

void main(void)
{
  long nexttime;
  unsigned long sec=0;
  long posx,posy;
  long yaw;
  int i;
  
  InitializeSerialLoop(2);
  InitializeKLRlib();

  for(i=0;i<16;i++)
    RegFileS[i]=50;
  
  nexttime=SysTime;
  while(1)
    {
      while(SysTime<nexttime)
        ;
      nexttime=SysTime+82;  /* 8192Hz/100Hz */
      sec++;
      RegFileL[0]=sec;
      KLRM_SpeedR=RegFileS[0];
      KLRM_SpeedL=RegFileS[1];
      if((sec%100)==0)
        {
          posx=KLRDR_PosX;
          posy=KLRDR_PosY;
          yaw=KLRDR_Yaw;
          posx=Mul_FP16_FP16(posx,MMPERSTEP);
          posy=Mul_FP16_FP16(posy,MMPERSTEP);
          yaw=DegTable[yaw];
          SLReply32(0x3f,1,HexToIntFP16(posx));  /* SLでsermon に自動送信 */
          SLReply32(0x3f,2,HexToIntFP16(posy));  /* SLでsermon に自動送信 */
          SLReply32(0x3f,3,HexToIntFP16(yaw));
        }


    }
}