/*  Kumagai Lab Quad controller */
/*  Position captire */
#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 36
#define USE_BULK
/* ライブラリ読み込み */
#include "slnode11.c"

/* BULKbuffer
// 0-15:  cmd servo
// 16,17: cmd other
// 18:    serial id
// 19-34: actual reply
// 35:    serial id
 */

/* =========================== */
/* KumaLab RoboLib             */
/* =========================== */
#define USE_KLR_WDT_CLOCK
#define USE_KLR_RCSERVO
#define USE_KLR_LED_SWITCH
#define KLR_LEDKEYP P8 /* port 8 */

#include "KLRlib.c"


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

#pragma interrupt(ITU1Interrupt)
#pragma interrupt(ITU2Interrupt)

int SVM_NoITU1,SVM_NoITU2;

unsigned short ServoCommand[16];
unsigned short ServoActual[16];

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

  if(ITU1.TSR.BIT.IMFB) /* IMFB */
    {
      unsigned int tcnt,grb;
      ITU1.TSR.BIT.IMFB=0;
      tcnt=ITU3.TCNT;
      grb=ITU3.GRB;
      if((tcnt>grb)&&
         (tcnt-grb>3000)&&
         (tcnt-grb<6000))
        {
          SVM_NoITU1=(PB.DR.BYTE>>4)&0xf;
        }
      else
        SVM_NoITU1=-1;
    }

  if(ITU1.TSR.BIT.IMFA) /* IMFA */
    {
      ITU1.TSR.BIT.IMFA=0;
      if(SVM_NoITU1>=0)
        {
          ServoActual[SVM_NoITU1]=ITU1.GRA;
          SVM_NoITU1=-1;
        }
    }
  /*RegFileL[1]++;*/
}

void ITU2Interrupt(void)
{
  if(ITU2.TSR.BIT.OVF)  /* OVF */
    {
      ITU2.TSR.BIT.OVF=0;
      SVM_NoITU2=-1;
    } 

  if(ITU2.TSR.BIT.IMFB) /* IMFB */
    {
      unsigned int tcnt,grb;
      ITU2.TSR.BIT.IMFB=0;
      tcnt=ITU4.TCNT;
      grb=ITU4.GRB;
      if((tcnt>grb)&&
         (tcnt-grb>3000)&&
         (tcnt-grb<6000))
        {
          SVM_NoITU2=(PB.DR.BYTE>>4)&0xf;
        }
      else
        SVM_NoITU2=-1;
    }

  if(ITU2.TSR.BIT.IMFA) /* IMFA */
    {
      ITU2.TSR.BIT.IMFA=0;
      if(SVM_NoITU2>=0)
        {
          ServoActual[SVM_NoITU2+8]=ITU2.GRA;
          SVM_NoITU2=-1;
        }
    }
  /*RegFileL[1]++;*/
}


void InitializeSVM(void)
{
  ITU1.TCR.BIT.CCLR=0x2; /* GRB match */
  ITU1.TCR.BIT.TPSC=0;   /* clock: 25M */
  ITU1.TIOR.BIT.IOA=0x4; /* rise edge capture */
  ITU1.TIOR.BIT.IOB=0x5; /* down edge capture */
  ITU1.TSR.BYTE=0;
  ITU1.TIER.BYTE=0x7; /* Interrupt enable */
  ITU.TSTR.BIT.STR1=1;
  ITU2.TCR.BIT.CCLR=0x2; /* GRB match */
  ITU2.TCR.BIT.TPSC=0;   /* clock: 25M */
  ITU2.TIOR.BIT.IOA=0x4; /* rise edge capture */
  ITU2.TIOR.BIT.IOB=0x5; /* down edge capture */
  ITU2.TSR.BYTE=0;
  ITU2.TIER.BYTE=0x7; /* Interrupt enable */
  ITU.TSTR.BIT.STR2=1;
}

#define NUMSERVO 16
#define BBSID 18
#define BBROfs 19

void main(void)
{
  long nexttime;
  int i;
  unsigned long sec=0,sw;
  unsigned int lastcmdsec=0,repsec=0;
  unsigned char svc,lastsvc;
  
  InitializeSerialLoop(2);
  InitializeKLRlib();
  SetKLRServoBase(ServoCommand);
  for(i=0;i<16;i++)
    {
      RegFileS[i]=1250;
      ServoCommand[i]=1250;
      BulkBuffer[i]=1250;
    }
  BulkBuffer[16]=BulkBuffer[17]=0;
  BulkBuffer[BBSID]=1;

  InitializeSVM();
  
  nexttime=SysTime;
  while(1)
    {
      KLRLK_LEDValue=RegFileS[14];
      /* not so good to use internal funcs, from KLRLEDKeyCycle() */
      __KLR_LEDHKC=(__KLR_LEDHKC+1)&0x3;
      KLRLEDKeyRaw(~((0x8>>__KLR_LEDHKC)|
                 ((KLR_LEDpat[(KLRLK_LEDValue>>(__KLR_LEDHKC*4))&0xf])<<8)),&sw);
      RegFileL[14]=sw;
      
      while(SysTime<nexttime)
        ;
      nexttime=SysTime+8;  /* 8192Hz/1000Hz */
      sec++;
      RegFileL[0]=sec;
      svc=(PB.DR.BYTE>>4)&0xf;
      if((BulkBuffer[BBSID]!=lastcmdsec)&&(svc==7))
        {
          for(i=0;i<NUMSERVO;i++)
            ServoCommand[i]=BulkBuffer[i];
          lastcmdsec=BulkBuffer[BBSID];
          SLReply32(0x3f,15,BulkBuffer[BBSID]);
          /*SLReply32(0x3f,14,sw);*/
        }
      if((lastsvc==0)&&(svc==1))
        {
          for(i=0;i<NUMSERVO;i++)
            BulkBuffer[i+BBROfs]=ServoActual[i];
          BulkBuffer[BBROfs+NUMSERVO]=repsec;
          repsec++;
          /* SLNop(); */
        }
      lastsvc=svc;
      /*if((sec%5000)==0)
        SLReply32(0x3f,1,sec);  * SLでsermon に自動送信 */
    }
}