/*   KumaLab Robot Library test-test program (COLOR)   */
#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 KLR_LEDKEYP P5  /* 使用ポート */
#define USE_KLR_LED_SWITCH

#define USE_KLR_COLOR_SENSOR_V2  /* TAOS230 */

#include "KLRlib.c"

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


void main(void)
{
  long nexttime;
  int i,s,cs;
  unsigned long sec=0;
  
  InitializeSerialLoop(2);
  InitializeKLRlib();
  
  nexttime=SysTime;
  while(1)
    {
      while(SysTime<nexttime)
        ;
      nexttime=SysTime+82;  /* 8192Hz/100Hz */
      sec++;
      RegFileL[0]=sec;
      KLRCS_ColorSense();
      KLRLEDKeyCycle();

      if(KLRLK_Push)
        {
          cs=KLRCS_Sense[0][0];
          if(cs>15) cs=15;
          for(i=0;i<3;i++)
            {
              s=KLRCS_Sense[0][i+1]*10/KLRCS_Sense[0][0];
              if(s>9) s=9;
              if(s<0) s=0;
              cs=(cs<<4)+s;
            }
          KLRLK_LEDValue=cs;
        }
      else
        {
          i=(sec/100)%4;
          KLRLK_LEDValue=(i<<12)|HexToInt(KLRCS_Sense[0][i]);
        }
    }
}