/*   KumaLab Robot Library test program (WDT)   */
/*  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 32
#define USE_BULK*/

#define SL11ML 10

/* ライブラリ読み込み */
#include "slnode11.c"



/* =========================== */
/* KumaLab RoboLib             */
/* =========================== */
#define USE_KLR_WDT_CLOCK
#define USE_KLR_RCSERVO
#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
int PWMCommand;
int PWMlast;

volatile unsigned int GRAsum,GRAst;
volatile unsigned int GRBsum,GRBst;
volatile unsigned int GRcnt,GRcst;
volatile unsigned int GRpwmc;

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;
      GRAst+=ITU0.GRA;
      GRBst+=ITU0.GRB;
      GRcst++;
    }
}

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;
      if(((PWMCommand>0)&&(PWMlast<0)||
          (PWMCommand<0)&&(PWMlast>0))) /* sign change */
        {
          ITU1.GRB=1;  /* pulse off (0:NG)*/
        }
      else
        {
          if(PWMCommand<0)
            {
              PA.DR.BIT.B5=1;
              ITU1.GRB=-PWMCommand;
            }
          else
            {
              PA.DR.BIT.B5=0;
              ITU1.GRB=PWMCommand;
            }
        }
      PWMlast=PWMCommand;
      PWMsysclock++;
      GRAst+=ITU0.GRA;
      GRBst+=ITU0.GRB;
      GRcst++;
      GRpwmc++;  GRpwmc&=0x7;
      if((PWMsysclock&0x7)==6)
        {
          GRAsum=GRAst;  GRBsum=GRBst;  GRcnt=GRcst;
          GRAst=GRBst=GRcst=0;
        }

            /* joint encoder */
#define JENCP P2
      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];
      if((JEi==0)&&((JEp^JEpl)&JEp&0x4))  /* Z upedge */
        { JEi=1; jenc=0; }

    }
  /*RegFileL[1]++;*/
}


void InitializeITU(void)
{
  /* ITU0: 2175 PWM count */
  ITU0.TIOR.BIT.IOA=0x4; /* GRA upedge capture */
  ITU0.TIOR.BIT.IOB=0x5; /* GRB downedge capture */
  ITU0.TCR.BIT.CCLR=0x1; /* GRA match clear */
  ITU0.TCR.BIT.TPSC=0;   /* clock: 25M */
  ITU.TSTR.BIT.STR0=1;
  
  /* 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 */
  
  /* ITU2: encoder count */
  ITU.TMDR.BIT.MDF=1;
  ITU2.TCNT=0;
  ITU.TSTR.BIT.STR2=1;

  /* ITU3 and 4 for servopwm */

#if 0 /* 向き合わず */
#define DMACNT 16
unsigned short GRAbuff[DMACNT];
unsigned short GRBbuff[DMACNT];
  /* DMAsetup */
  /* Disable 1B (servo used) */
  DMAC1B.DTCR.BIT.DTE=0;
  /* 0B:ITU0:GRA:lowbyte */
  DMAC0B.MAR=GRAbuff;
  DMAC0B.IOAR=(((unsigned char)&(ITU0.GRA))&0xff)+1;  /* low word= addr+1 */
  DMAC0B.ETCR=DMACNT*0x0101; /* loop, start */
  DMAC0B.DTCR.BIT.DTS=0;  /* trigger ITU0 Cmatch */
  DMAC0B.DTCR.BIT.DTIE=0; /* fin INT disable */
  DMAC0B.DTCR.BIT.RPE=1;  /* repeat */
  DMAC0B.DTCR.BIT.DTID=0; /* increment */
  DMAC0B.DTCR.BIT.DTSZ=0; /* byte trans */
  /* 1B:ITU0:GRB:lowbyte */
  DMAC1B.MAR=GRBbuff;
  DMAC1B.IOAR=(((unsigned char)&(ITU0.GRB))&0xff)+1;  /* low word= addr+1 */
  DMAC1B.ETCR=DMACNT*0x0101; /* loop, start */
  DMAC1B.DTCR.BIT.DTS=0;  /* trigger ITU0 Cmatch */
  DMAC1B.DTCR.BIT.DTIE=0; /* fin INT disable */
  DMAC1B.DTCR.BIT.RPE=1;  /* repeat */
  DMAC1B.DTCR.BIT.DTID=0; /* increment */
  DMAC1B.DTCR.BIT.DTSZ=0; /* byte trans */

  /* DMA enable */
  DMAC0B.DTCR.BIT.DTE=1;
  DMAC1B.DTCR.BIT.DTE=1;
  ITU0.TIER.BIT.IMIEA=1;  /* GRA interrupt enable */
#endif
}

void main(void)
{
  long nexttime;
  long curerri;
  int i;
  unsigned long sec=0,sw;
  unsigned int lastcmdsec=0,repsec,pingsec;
  unsigned char svc,lastsvc;
  unsigned int enclow,enchigh,encll;
  unsigned long enc32,enc32l;
  int velocity;
  
  for(i=0;i<16;i++)
    {
      RegFileS[i]=0;
      RegFileL[i]=0;
    }
  
  InitializeSerialLoop(2);
  InitializeKLRlib();
  SetKLRServoBase(ServoCommand);
  ServoCommand[0]=35000; ServoCommand[1]=35000;
  RegFileS[6]=ServoCommand[0];
  RegFileS[7]=ServoCommand[1];
  RegFileS[2]=10;


  enclow=encll=enchigh=0;
  enc32=enc32l=0;
  curerri=0;
  JEp=0; jenc=0;jerror=0;JEi=0;
  repsec=pingsec=0;
  
  InitializeITU();
  
  nexttime=(PWMsysclock&(~7))+8;
  while(1)
    {
      /* not so good to use internal funcs, from KLRLEDKeyCycle() */
      
      while(PWMsysclock<nexttime)
        ;
      /*nexttime=(PWMsysclock&(~7))+8;  /* 20kHz/8=2.5kHz */
        nexttime+=8;
      sec++;
      RegFileL[0]=sec;

      /* RC servo for steering */
      ServoCommand[0]=RegFileS[6];
      ServoCommand[1]=RegFileS[7];

      /* motor encoder */
      encll=enclow;
      enclow=ITU2.TCNT;
      if((enclow>0xc000)&&(encll<0x4000)) enchigh--;
      else if((enclow<0x4000)&&(encll>0xc000)) enchigh++;
      enc32=~(((((unsigned long)enchigh)<<16)|enclow)); /* エンコーダが正負逆→(*-1)がわりにnot */

      RegFileL[ 1]=enc32;
      RegFileS[ 8]=GRAsum;
      RegFileS[ 9]=GRBsum;
      RegFileS[10]=GRcnt;
      RegFileS[11]=jenc;

      if(RegFileS[0]>0)
        {
          RegFileS[0]--;
        }
      else
        {
          RegFileS[4]=0;
          RegFileS[3]=0;
          RegFileS[2]=1;
        }
      
      /* 速度制御 */
      if((sec&0x3)==0)
        {
          velocity=((long)enc32)-((long)enc32l);
          enc32l=enc32;
          if(RegFileL[6])
            {
              int verror=(int)(RegFileS[4])-velocity;
#define VENOSENSE 2
              if((verror<VENOSENSE)&&(verror>-VENOSENSE)) verror=0;
              RegFileS[3]=(verror*((long)(RegFileL[6])))>>8;
              if((RegFileS[3]>0)&&(verror<=0)&&(verror>-2)) RegFileS[3]= 1; /*  little over error */
              if((RegFileS[3]<0)&&(verror>=0)&&(verror< 2)) RegFileS[3]=-1; /*  little over error */
            }
        }
      else  /* 速度制御しない暇なときに */
        {
          __KLR_LEDHKC=(__KLR_LEDHKC+1)&0x3;
          KLRLEDKeyRaw(~((0x8>>__KLR_LEDHKC)|
                     ((KLR_LEDpat[(KLRLK_LEDValue>>(__KLR_LEDHKC*4))&0xf])<<8)),&sw);
          RegFileL[14]=sw;
        }
      
      /* 電流制御 */
      if(RegFileL[4])
        {
          int curcur,curerr,curcmd;
          long pwmc;
          if(GRAsum<0x600) /* current sensor check */
            {
              curcmd=(signed int)(RegFileS[3]);
              if(curcmd==0) curcmd=-1;
              curcur=GRBsum-0x282;
              RegFileS[13]=curcur;
              
              curerr=curcmd-curcur;
              curerri+=curerr;
              /*if((curcmd>0)&&(curerri<0)) curerri=0;
              if((curcmd<0)&&(curerri>0)) curerri=0;*/
#define CEILIM 0xfff
              if(curerri> CEILIM) curerri= CEILIM;
              if(curerri<-CEILIM) curerri=-CEILIM;

#define PWMBIAS 200
              if(curcmd>=0) pwmc=-PWMBIAS;
              if(curcmd< 0) pwmc= PWMBIAS;
              
              pwmc=pwmc-((curerr*(signed long)(RegFileL[4]))>>8)-((curerri*(signed long)(RegFileL[5]))>>8);
#define PWMMIN 200
              if((curcmd>=0)&&(pwmc>=-PWMMIN)) pwmc=-PWMMIN;  /* ゼロ以下にしない */
              if((curcmd< 0)&&(pwmc<= PWMMIN)) pwmc= PWMMIN;
#define PWMMAX 1200
              if(pwmc> PWMMAX) pwmc= PWMMAX;
              if(pwmc<-PWMMAX) pwmc=-PWMMAX;

              RegFileS[2]=pwmc;
            }
          else
            {
              RegFileS[2]=PWMMIN;   /* for sensor */
            }
        }
      else
        curerri=0;

      if(RegFileS[0]==0)
          RegFileS[2]=1;

      PWMCommand=RegFileS[2];

      if(!(sw>>16))  /* hold */
        {
          switch(sw&0xf)
            {
            case 0:
              KLRLK_LEDValue=RegFileL[1]>>((sw>>4)&0xf); break;
            case 1:
              KLRLK_LEDValue=PWMsysclock>>((sw>>4)&0xf); break;
/*            case 2:
              KLRLK_LEDValue=ITU0.GRA;  break;
            case 3:
              KLRLK_LEDValue=ITU0.GRB;  break;*/
            case 2:
              KLRLK_LEDValue=((JEi?0x8000:0)|(jerror&0x7)<<12)|(jenc&0xfff);     break;
            case 3:
              KLRLK_LEDValue=RegFileL[0]>>((sw>>4)&0xf); break;  /* sec */            
            case 4:
              KLRLK_LEDValue=velocity;  break;
            case 5:
              KLRLK_LEDValue=curerri>>((sw>>4)&0xf); break;
            case 6:
              KLRLK_LEDValue=GRAsum;  break;
            case 7:
              KLRLK_LEDValue=GRBsum;  break;   /*0x270base 1A/0x40*/
            case 8:
              KLRLK_LEDValue=RegFileS[2];  break;
              
            case 9:
              KLRLK_LEDValue=sec>>((sw>>4)&0xf); break;
            }
        }

      pingsec++;
      if(pingsec==25000)
        {
          SLReply32(0x3f,15,sec);  /* SLでsermon に自動送信 */
          pingsec=0;
        }
          
      if(RegFileS[1])
        {
          repsec++;
          if(repsec>=RegFileS[1])
            {
              SLReply32(0x3f,1,RegFileL[1]); /* enc */
              SLReply16(0x3f,11,RegFileS[11]); /* jenc */
              repsec=0;
            }
        }
    }
}