/* Spin counter */
#include "3048f.h"

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

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

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

/* 転送許可 : 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 P5 /* 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 short ServoCommand[16];
volatile unsigned long PWMsysclock;

#define PWMPeriod 1250

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;
    }
}

volatile unsigned char JEp0,JEp1,JEp2,JEp,JEpl,JEm,JEi;
int jenc,jerror;
int JEupdown[16]=
{
  0,0,0,0,  /* noedge */
  1,-1,-1,1, /* AdB0 AuB0 AdB1 AuB1 */
  -1,1,1,-1, /* BdA0 AdA1 BuA0 BuA1 */
  9,9,9,9,  /* Error condition */
};


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++;

      /* joint encoder */
#define JENCP P4
      JEp2=JEp1;
      JEp1=JEp0;
      JEp0=JENCP.DR.BYTE;
      JEpl=JEp;
      JEp=JEp|(JEp2&JEp1&JEp0); /* 全１なら１にする */
      JEp=JEp&(JEp2|JEp1|JEp0); /* 全０なら０にする */
      JEm=(((JEp^JEpl)&0x3)<<2)|(JEp&3);
      /*JEm=((JEp0&0x3)<<6)|((JEp1&0x3)<<4)|((JEp2&0x3)<<2)|(JEp&3);*/
      /*P1.DDR=0xff; P1.DR.BYTE=JEm|(JEp0<<4);*/
      if(JEupdown[JEm]==9) jerror++;
      else jenc+=JEupdown[JEm];
    }
  /*RegFileL[1]++;*/
}


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 main(void)
{
  long nexttime;
  volatile int i;
  volatile unsigned long sec=0,sw;
  volatile int absenc,rev,lastabs,msec,rpm;
  volatile int ssec,la;
  volatile int target;
  
  JENCP.DDR=0x4;
  JENCP.DR.BIT.B2=0; /* bell */
  
  for(i=0;i<16;i++)
    {
      RegFileS[i]=0;
      RegFileL[i]=0;
    }
  
  InitializeSerialLoop(2);
  InitializeKLRlib();

  JEp=0; jenc=0; jerror=0;JEi=0;
  msec=0;
  InitializeITU();
  
  nexttime=PWMsysclock+20;
  while(1)
    {
      while(PWMsysclock<nexttime)
        ;
      nexttime+=20;
      sec++;
      RegFileL[0]=sec;
      RegFileS[11]=jenc;

      if(sw>>16) jenc=0; /* reset */

      absenc=jenc;
      if(absenc<0) absenc=-absenc;
      rev=absenc>>2;
      msec++;
      if(msec>999) {
        msec=0;
        rpm=(absenc-lastabs)*15;
        lastabs=absenc;
      }
      if(la!=rev)
        {
          if(rev%10==0)
            ssec=40;
          else
            ssec=10;
          if(target-rev<100) ssec*=2;
          if(target-rev<50) ssec*=2;
          if(target-rev<20) ssec*=2;
          if(target-rev<10) ssec*=2;
          la=rev;
        }
      if(ssec>0)
        {
          JENCP.DR.BIT.B2=1; /* bell */
          ssec--;
        }
      else
          JENCP.DR.BIT.B2=0; /* bell */
      
      
      switch((sw>>12)&0xf)
        {
        case 0:
          KLRLK_LEDValue=HexToInt(rev); break;
        case 1:
          KLRLK_LEDValue=rev; break;
        case 2:
          KLRLK_LEDValue=HexToInt(rpm); break;
        case 3:
          KLRLK_LEDValue=rpm; break;
        case 4:
          KLRLK_LEDValue=HexToInt(jenc); break;
        case 5:
          KLRLK_LEDValue=jenc; break;

        /*case 7:
          KLRLK_LEDValue=lastabs; break;*/
        case 8:
          KLRLK_LEDValue=PWMsysclock>>((sw>>4)&0xf); break;
        case 9:
          KLRLK_LEDValue=sec>>((sw>>4)&0xf); break;
        }
      __KLR_LEDHKC=(__KLR_LEDHKC+1)&0x3;
      KLRLEDKeyRaw(~((0x8>>__KLR_LEDHKC)|
                     ((KLR_LEDpat[(KLRLK_LEDValue>>(__KLR_LEDHKC*4))&0xf])<<8)),&sw);
      RegFileL[14]=sw;
      target=BCDToInt(sw&0xfff);
    }
}