/*   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_RCSERVO

/*#define KLRCS_NUMSENSOR 4
#define KLRCS_SPERIOD 8
#define KLRCS_REDPOWER 0x64
#define KLRCS_GREENPOWER 0x10
#define KLRCS_OFFSET 0x28
#define KLRCS_THRESHOLD 0x18 */
#define USE_KLR_COLOR_SENSOR

#include "KLRlib.c"

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

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

  nexttime=SysTime;
  while(1)
    {
      while(SysTime<nexttime)
        ;
      nexttime=SysTime+82;  /* 8192Hz/100Hz */
      sec++;
      RegFileL[0]=sec;
      cs=KLRColorSense();
      if(cs==KLRCS_UPDATED)
        {
          SLReply32(0x3f,2,
            ((long)(KLRCS_Sense[0]&0xff)<<24)|
            ((long)((KLRCS_RedAD[0]>>2)&0xff)<<12)|
            ((long)((KLRCS_GreenAD[0]>>2)&0xff))
            );  /* SLでsermon に自動送信 */
          KLRS_Command[0]=10000+KLRCS_Sense[0]*15000;
        }
         
      if((sec%500)==0)
        SLReply32(0x3f,1,HexToInt(sec));  /* SLでsermon に自動送信 */
    }
}