/* Ball Inverted pendulum control */
/* version 2.0 */
#include "3048f.h"

/* =========================== */
/* SerialLoop のための初期設定 */
/* =========================== */

/* SL に使用するSCI    default SCI1*/
/* 同時に、serloops.mar を参考に割込ベクタを設定する */
#define SCISL SCI0

/* SCI 通信速度レジスタ */
/*#define SCISLBRR 13   25MHz 57600  6, 115200*/
#define SCISLBRR 13

/* 転送許可 : undef すると転送しない＝ループが切れる */
/* 一般のノードでは普通はENABLE */
#define ENABLE_FORWARD

/* 機能選択 */
#define USE_MEMEXT   /* メモリ拡張追加 */
#define USE_PACKETFUNC 
#define BULKBUFFSIZE 32
#define USE_BULK
/* ライブラリ読み込み */
#include "slnode11.c"



/* =========================== */
/* KumaLab RoboLib             */
/* =========================== */
#define USE_KLR_LED_SWITCH
#define KLR_LEDKEYP P6 /* port 8 */

#include "KLRlib.c"


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

#pragma interrupt(ITU0Interrupt)
#pragma interrupt(ITU1Interrupt)

volatile unsigned long PWMsysclock;

#define PWMPeriod 2500  /* 25000000/10000 */

void ITU0Interrupt(void)  /* dummy */
{
  if(ITU0.TSR.BIT.OVF)  /* OVF */
    {
      ITU0.TSR.BIT.OVF=0;
    } 

  if(ITU0.TSR.BIT.IMFB) /* IMFB */
    {
      ITU0.TSR.BIT.IMFB=0;
    }

  if(ITU0.TSR.BIT.IMFA) /* IMFA */
    {
      ITU0.TSR.BIT.IMFA=0;
    }
}


/* ITU 1 interrupt */
void ITU1Interrupt(void)
{
  if(ITU1.TSR.BIT.OVF)  /* OVF */
    {
      ITU1.TSR.BIT.OVF=0;
    } 

  if(ITU1.TSR.BIT.IMFB) /* IMFB */
    {
      ITU1.TSR.BIT.IMFB=0;
    }

  if(ITU1.TSR.BIT.IMFA) /* IMFA */
    {
      ITU1.TSR.BIT.IMFA=0;
      PWMsysclock++;
    }
}


void InitializeITU(void)
{
  /* ITU1: PWM output 20kHz*/
  ITU1.GRA=PWMPeriod;
  ITU1.GRB=0;
  ITU1.TCR.BIT.CCLR=0x1; /* GRA match */
  ITU1.TCR.BIT.TPSC=0;   /* clock: 25M */
  ITU1.TSR.BYTE=0;
  ITU1.TIER.BIT.IMIEA=1;  /* GRA interrupt enable */
  ITU.TMDR.BIT.PWM1=1;
  ITU.TSTR.BIT.STR1=1;
  /* PA.DDR=0x20; *//* PA-5(DIR) output */
}


void SendStatus(long count,int state,int team)
{
  SLReply16(0x3f,8,count&0xffff);
  SLReply16(0x3f,9,((state&0xff)<<8)|((team&0xff)));
  SLReply32(0x3f,8,((state&0xff)<<8)|((team&0xff))|(count<<16)); 
}

void main(void)
{
  long nexttime;
  volatile int i;
  volatile unsigned long sec=0,sw,count,dispcount,teamid;
  int cyclerest,ppsc;
  int lpc;
  char dp[4];
  int holdc,initf;
  
  for(i=0;i<16;i++)
    {
      RegFileS[i]=0;
      RegFileL[i]=0;
    }
  
  InitializeSerialLoop(2);
  InitializeKLRlib();

  InitializeITU();

  
  
  ppsc=0;
  holdc=0;
  RegFileS[0]=1;
  dp[3]=dp[1]=dp[0]=0;
  dp[2]=1;
  initf=1;
  teamid=0;
  lpc=0;
  
  nexttime=PWMsysclock+10;
  while(1)
    {
      cyclerest=(int)(nexttime-PWMsysclock);
      
      while(PWMsysclock<nexttime)
        ;
      nexttime+=10;
      sec++;
      RegFileL[0]=sec;
      if(!initf) count++;
      
      dp[0]=(P1.DR.BIT.B0==0)?1:0;
      dp[1]=(P1.DR.BIT.B1==0)?1:0;
      
      if((P1.DR.BIT.B1==1)&&(holdc==0))
        {
          if(initf==0)  /* lap */
            {
              if(lpc>0)
                {
                  SendStatus(count,1,teamid);
                  holdc=2000;
                }
            }
          else /* start */
            {
              initf=0;
              SendStatus(count,0,teamid);
              holdc=-500;
            }
        }
      if((P1.DR.BIT.B0==1)&&(holdc==0)) /* checkpoint lap */
        {
          if(initf==0)
            {
              SendStatus(count,2,teamid);
              holdc=500;
              lpc++;
            }
        }

      if(holdc==0)
        {
          dispcount=count;
        }
      else if(holdc<0)
        {
          dispcount=count;
          holdc++;
        }
      else
        {
          holdc--;
        }

      /* 表示その他 */
      if(((sw>>16)&1)==0)
        {
          KLRLK_LEDValue=HexToInt((dispcount/10)%10000);
          /*switch((sw>>12)&0xf)*/
        }
      else
        {
          int ntid=sw&0xff;
          if((teamid!=(ntid))||(initf==0))
            SendStatus(0,9,ntid);
          teamid=ntid;
          KLRLK_LEDValue=teamid;
          initf=1; /* stop & init */
          count=0; holdc=0; lpc=0;
        }
      __KLR_LEDHKC=(__KLR_LEDHKC+1)&0x3;
      KLRLEDKeyRaw(~((0x8>>__KLR_LEDHKC)|
                     ((KLR_LEDpat[(KLRLK_LEDValue>>(__KLR_LEDHKC*4))&0xf])<<8)|
                     (dp[__KLR_LEDHKC]?0x1000:0x0000)),&sw);
      RegFileL[14]=sw;

      ppsc++;
      if(ppsc>=1000)
        {
          SLNop();
          ppsc=0;
        }
    }
}