/*   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 KLR_LEDKEYP P5  /* 使用ポート */
#define USE_KLR_LED_SWITCH

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

unsigned char CSRedPower,CSGreenPower;
#define KLRCS_REDPOWER CSRedPower
#define KLRCS_GREENPOWER CSGreenPower

#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;
  int ColorCode[4];
  
  InitializeSerialLoop(2);
  InitializeKLRlib();

  nexttime=SysTime;
  while(1)
    {
      while(SysTime<nexttime)
        ;
      nexttime=SysTime+82;  /* 8192Hz/100Hz */
      sec++;
      RegFileL[0]=sec;

      KLRLEDKeyCycle();

      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;
        }
      /* ColorSenseEX */
      KLRLK_LEDValue=0;
      for(i=0;i<4;i++) /* 黄玉でR=40, G=40 にあわせること前提 */
        {
          int cr=(KLRCS_RedAD[0]>>2),cg=(KLRCS_GreenAD[0]>>2);
          if(cr>36) ColorCode[i]=1; /* 黄色 */
          else if(cr>30) ColorCode[i]=2;
          else if(((cr<15)&&(cg-cr>7))||(cg-cr>10)) ColorCode[i]=3;
          else ColorCode[i]=0;
          KLRLK_LEDValue=(KLRLK_LEDValue<<4)|ColorCode[i];
        }
      
      CSRedPower=BCDToInt((KLRLK_SWValue>>8)&0xff);
      CSGreenPower=BCDToInt((KLRLK_SWValue)&0xff);
      if(KLRLK_Push)
        {
          KLRLK_LEDValue=
            ((HexToInt(KLRCS_RedAD[0]>>2)&0xff)<<8)|
              (HexToInt(KLRCS_GreenAD[0]>>2)&0xff);
        }
      /* /ColorSenseEX */

      if((sec%500)==0)
        SLReply32(0x3f,1,HexToInt(sec));  /* SLでsermon に自動送信 */
    }
}