/* 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 763  /* 25000000/32768 */

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


/* AD 入力フィルタ */
volatile unsigned int ADF0,ADF1,ADF2,ADF3;
volatile unsigned int ADF4,ADF5,ADF6,ADF7;
int adchan;

/* Stepping motor driver  and  AD convertor*/
/* ITU 1 interrupt */

volatile int MS0,MS1,MS2;
volatile int PP0,PP1,PP2;
volatile int PW0,PW1,PW2; /* power up flag */
#define MHPower 0x8
#define MLPower 0x0
volatile int MOnOff;

unsigned char LEDSWpat[16];
unsigned char LEDSW;
int LEDSWpc;
#define LEDSWP P4

#define MPNRESET  0x01
#define MPDISABLE 0x02
#define MPCWCCW   0x04
#define MPCLK2    0x08
#define MPCLK2B   B3
#define MPCLK1    0x10
#define MPCLK1B   B4
#define MPM1      0x20
#define MPM2      0x40
#define MPREFIN   0x80


void InitializeSMDriver(void)
{
  MS0=MS1=MS2=0;
  PP0=PP1=PP2=0;
  PW0=PW1=PW2=MPNRESET|MPM1|MPM2;
  /* SMMode=0x19;*/  /* 000 11 001 */
  MOnOff=0; /* 00 11 11 11 */ 
}

void ITU1Interrupt(void)
{
  int b,a;
  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++;

#define SMP0 P1
#define SMP1 P2
#define SMP2 P3
      
      if(MOnOff) /* on */
        {
          SMP0.DR.BYTE=PW0|0|MPCLK1|MPCLK2;
          SMP1.DR.BYTE=PW1|0|MPCLK1|MPCLK2;
          SMP2.DR.BYTE=PW2|0|MPCLK1|MPCLK2;
        }
      else /* off */
        {
          SMP0.DR.BYTE=PW0|MPDISABLE|MPCLK1|MPCLK2;
          SMP1.DR.BYTE=PW1|MPDISABLE|MPCLK1|MPCLK2;
          SMP2.DR.BYTE=PW2|MPDISABLE|MPCLK1|MPCLK2;
        }
      
      b=PP0&0xc000;
      PP0+=MS0;
      a=PP0&0xc000;
#if 0
      if((b==0xc000)&&(a==0x0000)) /* minus -> plus */
        SMP0.DR.BIT.MPCLK1B=0;
      else if((b==0x0000)&&(a==0xc000)) /* plus -> minus */
        SMP0.DR.BIT.MPCLK2B=0;
#else
      if(b==0xc000)
        {
          if(a==0x0000) /* minus -> plus */
            SMP0.DR.BIT.MPCLK1B=0;
        }
      else if(a==0xc000)
        {
          if(b==0x0000) /* plus -> minus */
            SMP0.DR.BIT.MPCLK2B=0;
        }
#endif

      b=PP1&0xc000;
      PP1+=MS1;
      a=PP1&0xc000;
#if 0
      if((b==0xc000)&&(a==0x0000)) /* minus -> plus */
        SMP1.DR.BIT.MPCLK1B=0;
      else if((b==0x0000)&&(a==0xc000)) /* plus -> minus */
        SMP1.DR.BIT.MPCLK2B=0;
#else
      if(b==0xc000)
        {
          if(a==0x0000) /* minus -> plus */
            SMP1.DR.BIT.MPCLK1B=0;
        }
      else if(a==0xc000)
        {
          if(b==0x0000) /* plus -> minus */
            SMP1.DR.BIT.MPCLK2B=0;
        }
#endif
      b=PP2&0xc000;
      PP2+=MS2;
      a=PP2&0xc000;
#if 0
      if((b==0xc000)&&(a==0x0000)) /* minus -> plus */
        SMP2.DR.BIT.MPCLK1B=0;
      else if((b==0x0000)&&(a==0xc000)) /* plus -> minus */
        SMP2.DR.BIT.MPCLK2B=0;
#else
      if(b==0xc000)
        {
          if(a==0x0000) /* minus -> plus */
            SMP2.DR.BIT.MPCLK1B=0;
        }
      else if(a==0xc000)
        {
          if(b==0x0000) /* plus -> minus */
            SMP2.DR.BIT.MPCLK2B=0;
        }
#endif

      /* ADC */
#define ADFLT(ADC,ADF) ADF+=((signed int)(ADC-ADF)+0x20)>>6
      if(adchan)
        {
          ADFLT(AD.DRA,ADF4);
          ADFLT(AD.DRB,ADF5);
          ADFLT(AD.DRC,ADF6);
          ADFLT(AD.DRD,ADF7);
          AD.CSR.BYTE=0x1f;  /* 0001 1 011 , AD0-3 */ 
          AD.CSR.BYTE=0x3b;  /* 0011 1 011 , AD0-3 */ 
          adchan=0;
        }
      else
        {
          ADFLT(AD.DRA,ADF0);
          ADFLT(AD.DRB,ADF1);
          ADFLT(AD.DRC,ADF2);
          ADFLT(AD.DRD,ADF3);
          AD.CSR.BYTE=0x1b;  /* 0011 1 111 , AD4-7 */ 
          AD.CSR.BYTE=0x3f;  /* 0011 1 111 , AD4-7 */ 
          adchan=1;
        }
         }
  
  /*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;

  
  DMAC0A.DTCR.BYTE&=0x77;  /* disable */

  /* for LED */
  ITU0.GRA=PWMPeriod*20; /*25000;  /* 1kHz */
  ITU0.GRB=0;
  ITU0.TCR.BIT.CCLR=0x1; /* GRA match */
  ITU0.TCR.BIT.TPSC=0;   /* clock: 25M */
  ITU0.TSR.BYTE=0;
  ITU0.TIER.BIT.IMIEA=1;  /* GRA interrupt enable */
  ITU.TMDR.BIT.PWM0=1;

#if 1
  DMAC0A.MAR=LEDSWpat;
  DMAC0A.IOAR=((int)&(LEDSWP.DR.BYTE))&0xff; 
  DMAC0A.ETCR=0x1010; /* loop, 16unit */
  DMAC0A.DTCR.BYTE=(DMAC0A.DTCR.BYTE&0x80)|0x90;  /* byte, source=ITU0 Cmatch */
  /*               ~~~~~~~~~~~~~~~~~  force reading ZERO of DTE        */
#endif
  
  ITU0.TIER.BIT.IMIEA=1;  /* GRA interrupt enable */
  ITU.TSTR.BIT.STR0=1;

  /* PA.DDR=0x20; *//* PA-5(DIR) output */
}

void SetLEDDMA(void)
{
/*      LEDSWP.DDR=0xff;
      LEDSWpc=(LEDSWpc+1)&0xf;
      LEDSWP.DR.BYTE=LEDSWpat[LEDSWpc];*/
  
}

void SetLEDPower(int ch,int pow)
{
  unsigned char m=(1<<ch);
  int i;
  for(i=0;i<16;i++)
    {
      if(i<pow)
        LEDSWpat[i]|=m;
      else
        LEDSWpat[i]&=~m;
    }
}

void SetLEDPowerMask(unsigned char m,int pow)
{
  int i;
  for(i=0;i<16;i++)
    {
      if(i<pow)
        LEDSWpat[i]|=m;
      else
        LEDSWpat[i]&=~m;
    }
}

/* スイッチ定数 */
#define PBR1 ((LEDSW&0x01))
#define PBR2 ((LEDSW&0x10))
#define PBB1 ((LEDSW&0x02))
#define PBB2 ((LEDSW&0x20))
#define PBG1 ((LEDSW&0x04))
#define PBG2 ((LEDSW&0x40))
#define PBY1 ((LEDSW&0x08))
#define PBY2 ((LEDSW&0x80))


#include "ballipv2_param.c"


void main(void)
{
  long nexttime;
  volatile int i;
  volatile unsigned long sec=0,sw;
  int MotorSpeedX,MotorSpeedY,YawSpeed;
  int cyclerest,ppsc;
  int offsetc;
  unsigned long OF0,OF1,OF2,OF3,OF4,OF5,OF6,OF7; /* ADオフセット */
  long GyroV1,GyroV2;  /* ジャイロ角速度 */
  long GyroA1,GyroA2;  /* ジャイロ角度   */
  long AcclA1,AcclA2;  /* 加速時計角度   */
  long AgFlt1,AgFlt2;  /* 加速度フィルタ */
  long sub;
  long CompA1,CompA2;
  volatile long thx,thy,thofx,thofy,thvx,thvy; /* 角度状態量 */
  volatile long x,y,vx,vy,vxcmd,vycmd,acx,acy,xofs,yofs;  /* 位置状態量 */
  long acxt,acyt; /* 加速度にフィルタ */
  volatile int mvx,mvy,mvrot;
  int lastcont;
  int mx,my,mrot;
  int cmdmode;
  
  SMP0.DR.BYTE=0x00;
  SMP0.DDR=0xff;
  SMP1.DR.BYTE=0x00;
  SMP1.DDR=0xff;
  SMP2.DR.BYTE=0x00;
  SMP2.DDR=0xff;
  
  for(i=0;i<16;i++)
    {
      RegFileS[i]=0;
      RegFileL[i]=0;
    }
  
  InitializeSerialLoop(2);
  InitializeKLRlib();

  InitializeSMDriver();
  InitializeITU();

  SetLEDPowerMask(0xff,0x1);
  SetLEDPower(0,0xf);
  
  ppsc=0;
  RegFileS[0]=1;
  lastcont=0;
  
#if 0
  RegFileS[4]=  2; /* 移動加速, 上限 */
  RegFileS[5]=300;
  RegFileS[6]=  5; /* 旋回加速, 上限 */
  RegFileS[7]=500;
  
  if((PBRL)||(PBRR))
    {
    }
  else if((PBOR)||(PBOL))
    {
      RegFileS[3]=0;
      RegFileL[4]=1500;
      RegFileL[5]= 550;
      RegFileL[6]=  48;
      RegFileL[7]= 600;
      RegFileS[5]=600;
      RegFileS[7]=800;
    }
  else
    {
      RegFileS[3]=0;
      RegFileL[4]=1500;
      RegFileL[5]= 350;
      RegFileL[6]=  48;
      RegFileL[7]= 480;
    }
#endif
  SetParams(0);
  
  mvx=mvy=mrot=0;
  vxcmd=vycmd=0;
  cmdmode=0;

  LEDSW=0;
  
  nexttime=PWMsysclock+164;
  while(1)
    {
      cyclerest=(int)(nexttime-PWMsysclock);
      
      while(PWMsysclock<nexttime)
        ;

      nexttime+=164;
      sec++;
      RegFileL[0]=sec;

      if((sec&3)==0)
        {
          LEDSWP.DDR=0x00;      /* LEDSW */
          if(offsetc)
            SetLEDPowerMask(0xff,0x0);
          else
            SetLEDPowerMask(0xff,0x1);
          SetLEDPower(0,0xf);
          if(RegFileL[1])  SetLEDPower((sec>>5)&0x7,0x4);
          LEDSW=(~LEDSWP.DR.BYTE);      /* LEDSW */
          LEDSWP.DDR=0xff;
          SetLEDPowerMask(LEDSW,0xa);
        }

      if((sec&3)==1)
        {
          cmdmode=0;
          /* スイッチ操作 */
          if((PBR1)&&(PBR2)&&((PBG1)||(PBG2)))  /* 赤同時押し＋緑で制御停止 */
            {
              RegFileL[1]=0; cmdmode=1;
            }
          if((RegFileL[1]==0)&&(PBR1)&&(PBR2)&&((PBB1)||(PBB2)))  /* 赤同時押し＆青でセンサオフセット除去 */
            {
              RegFileS[0]=1;
            }
          if((!PBR1)&&(!PBR2))  /* 赤は触っていない */
            {
              if((PBB1)||(PBB2))  /* 青で制御開始(1[s]) */
                {
                  if(RegFileL[1]<200) RegFileL[1]=200;
                }
              if((PBB1)&&(PBB2))  /* 青で制御開始(10[s]) */
                {
                  if(RegFileL[1]<0x7fffffff) RegFileL[1]=0x7fffffff;
                }
            }

          /* parameter change */
          if((LEDSW&0x33)==0x03) /* 赤1青1 */
            {
              cmdmode=1;
              switch(LEDSW&0xcc)
                {
                case 0x04: SetParams(0); break;
                case 0x08: SetParams(1); break;
                case 0x40: SetParams(2); break;
                case 0x80: SetParams(3); break;
                }
            }
          if((LEDSW&0x33)==0x30) /* 赤2青2 */
            {
              cmdmode=1;
              switch(LEDSW&0xcc)
                {
                case 0x04: SetParams(4); break;
                case 0x08: SetParams(5); break;
                case 0x40: SetParams(6); break;
                case 0x80: SetParams(7); break;
                }
            }
        }
      if((sec&3)==2)
        {
          mx=my=mrot=0;
          if((RegFileL[1]>500)&&(cmdmode==0))
            {
              if((PBG1)&&(PBG2)) { mrot++; }
              else if((PBY1)&&(PBY2)) { mrot--; }
              else if((PBR1)&&(PBR2)) { /* command */ }
              else
                {
                  if(PBR1) {       my++; }
                  if(PBB1) { mx++; my++; }
                  if(PBG1) { mx++;       }
                  if(PBY1) { mx++; my--; }
                  if(PBR2) {       my--; }
                  if(PBB2) { mx--; my--; }
                  if(PBG2) { mx--;       }
                  if(PBY2) { mx--; my++; }
                }
            }
        }
      /* センサ情報 処理 ================================================== */
      if(RegFileS[0]==1) /* オフセット除去指令 */
        {
          OF0=OF1=OF2=OF3=OF4=OF5=OF6=OF7=0;
          offsetc=256;
          RegFileS[0]=2;
          GyroV1=GyroV2=GyroA1=GyroA2=0;
          AcclA1=AcclA2=0;  AgFlt1=AgFlt2=0;  CompA1=CompA2=0;
        }
      if(offsetc)
        { /* オフセット計測 */
          OF0+=ADF0; OF1+=ADF1; OF2+=ADF2; OF3+=ADF3; 
          OF4+=ADF4; OF5+=ADF5; OF6+=ADF6; OF7+=ADF7; 
          offsetc--;
          if(offsetc==0) RegFileS[0]=0;
        }
      else
        { /* 角度計算 */
#define GARATIO1 178  // 1:170 2:180 3:178
#define GARATIO2 182  // 1:166 2:187 3:182

          GyroV1=  ((long)ADF0<<8)-(long)OF0;  /* gyro */
          GyroV2=  ((long)ADF4<<8)-(long)OF4;
          AcclA1=-(((long)ADF2<<8)-(long)OF2); 
          AcclA2=-(((long)ADF7<<8)-(long)OF7); /* accl-yout, negative */
          GyroA1+=(GyroV1+0x400)>>11;
          GyroA2+=(GyroV2+0x400)>>11;
          /*AgFlt1=(AcclA1*181)>>11;
          AgFlt2=(AcclA2*173)>>11;*/
          /* 合成  comp=LPF*AA+(1-LPF,i.e.HPF)*GA=GA+LPF*(AA-GA) */
          sub=((AcclA1*170)>>11)-GyroA1;
          AgFlt1+=(sub-AgFlt1+0x100)>>9;
          CompA1=GyroA1+AgFlt1;
          sub=((AcclA2*166)>>11)-GyroA2;
          AgFlt2+=(sub-AgFlt2+0x100)>>9;
          CompA2=GyroA2+AgFlt2;
          
        }

      /* 制御処理 ================================================== */
      /*  long thx,thy,thofx,thofy,thvx,thvy;  角度状態量 */
      /*  long x,y,vx,vy,acx,acy;   位置状態量 */
#if 0
      thx = -(CompA1>>4); /* 角度、X+ 方向に倒れたら正 */
      thy =  (CompA2>>4); /* 角度、Y+ 方向に倒れたら正 */
      thvx= -(GyroV1>>8); /* 同角速度 */
      thvy=  (GyroV2>>8); /* 同角速度 */
#endif
      thx = -(CompA2>>4); /* 角度、X+ 方向に倒れたら正 */
      thy = -(CompA1>>4); /* 角度、Y+ 方向に倒れたら正 */
      thvx= -(GyroV2>>8); /* 同角速度 */
      thvy= -(GyroV1>>8); /* 同角速度 */

      /* 制御開始時 */
      if((lastcont==0)&&(RegFileL[1]))
        {
          thofx=thx;  thofy=thy;  /* 初期角度保存 */
          xofs=x; yofs=y;
          lastcont=1;
        }
      lastcont=RegFileL[1];

      if(RegFileL[1]>0) /* 制御中 */
        {
          RegFileL[1]--; 
          MOnOff=1;
          thx-=thofx;  thy-=thofy;
          /*acx=
            (((signed long)RegFileL[4]*thx )>>16)+
            (((signed long)RegFileL[5]*thvx)>>16)+
            (((signed long)RegFileL[6]*x   )>>16)+
            (((signed long)RegFileL[7]*vx  )>>16);
          acy=
            (((signed long)RegFileL[4]*thy )>>16)+
            (((signed long)RegFileL[5]*thvy)>>16)+
            (((signed long)RegFileL[6]*y   )>>16)+
            (((signed long)RegFileL[7]*vy  )>>16);*/
          acxt=
            (((signed long)RegFileL[4])*thx +
             ((signed long)RegFileL[5])*thvx+
             ((((signed long)RegFileL[6])*(x-xofs)+0x8)>>4)   +
             ((((signed long)RegFileL[7])*(vx-vxcmd)+0x8)>>4)  +0x800l)>>12;
          acyt=
            (((signed long)RegFileL[4])*thy +
             ((signed long)RegFileL[5])*thvy+
             ((((signed long)RegFileL[6])*(y-yofs)+0x8)>>4)   +
             ((((signed long)RegFileL[7])*(vy-vycmd)+0x8)>>4)  +0x800l)>>12;
          /* filter */
          acx=acx+((acxt-acx)>>(RegFileS[3]));
          acy=acy+((acyt-acy)>>(RegFileS[3]));
          /* filter */
          vx+=acx;  vy+=acy;
          x +=(vx>>4);   y +=(vy>>4);
          MotorSpeedX=(vx>>4);
          MotorSpeedY=(vy>>4);
          YawSpeed=0;
        }
      else /* 制御停止中 */
        {
          MOnOff=0; /* モータカット */
          x=y=vx=vy=acx=acy=0;
          MotorSpeedX=MotorSpeedY=0;
          YawSpeed=0;
          mvx=mvy=mrot=0;
        }
      
      
      /* 仮 */
      /*MotorSpeedX=-(CompA1>>4);
      MotorSpeedY= (CompA2>>4);
      YawSpeed=0;*/

      /* 手動操作重畳 */
      if(mx) { mvx+=RegFileS[4]*mx; }
      else
        {
          if(mvx>0) { mvx-=RegFileS[4]; if(mvx<0) mvx=0; }
          if(mvx<0) { mvx+=RegFileS[4]; if(mvx>0) mvx=0; }
        }
      if(mvx> (int)(RegFileS[5])) { mvx= (int)(RegFileS[5]); }
      if(mvx<-(int)(RegFileS[5])) { mvx=-(int)(RegFileS[5]); }

      if(my) { mvy+=RegFileS[4]*my; }
      else
        {
          if(mvy>0) { mvy-=RegFileS[4]; if(mvy<0) mvy=0; }
          if(mvy<0) { mvy+=RegFileS[4]; if(mvy>0) mvy=0; }
        }
      if(mvy> (int)(RegFileS[5])) { mvy= (int)(RegFileS[5]); }
      if(mvy<-(int)(RegFileS[5])) { mvy=-(int)(RegFileS[5]); }

      if(mrot) { mvrot+=RegFileS[6]*mrot; }
      else
        {
          if(mvrot>0) { mvrot-=RegFileS[6]; if(mvrot<0) mvrot=0; }
          if(mvrot<0) { mvrot+=RegFileS[6]; if(mvrot>0) mvrot=0; }
        }
      if(mvrot> (int)(RegFileS[7])) { mvrot= (int)(RegFileS[7]); }
      if(mvrot<-(int)(RegFileS[7])) { mvrot=-(int)(RegFileS[7]); }

      xofs+=mvx;  yofs+=mvy;
      vxcmd=(mvx<<4);  vycmd=(mvy<<4);

      if((MotorSpeedX>20000)||(MotorSpeedX<-20000)) RegFileL[1]=0;
      if((MotorSpeedY>20000)||(MotorSpeedY<-20000)) RegFileL[1]=0;
      
      MotorSpeedX+=mvx;  MotorSpeedY+=mvy;
      YawSpeed+=mvrot;
         
      /* モータ速度分配 */
      /* motor 1,2,3 , sqrt(3)/2~=222/256 */
      MS0= -(MotorSpeedX   ) +             0                + YawSpeed;
      MS1=  (MotorSpeedX>>1) - (((long)MotorSpeedY*222)>>8) + YawSpeed;
      MS2=  (MotorSpeedX>>1) + (((long)MotorSpeedY*222)>>8) + YawSpeed;

      /*PA.DDR=0xff;  PB.DDR=0xff;
      PA.DR.BYTE=(sw>>8)&0xff;
      PB.DR.BYTE=sw&0xff;*/
      
      /* 表示その他 */
      if(((sw>>16)&1)==0)
        {
          switch((sw>>12)&0xf)
            {
              
              /*case 7:
          KLRLK_LEDValue=lastabs; break;*/
            case 6:
#define DISPLVAL(LVAL) HexToInt((LVAL>>(((sw>>4)&0xf)*2))+5000)
              switch((sw>>8)&0xf)
                {
                case 0:  KLRLK_LEDValue=DISPLVAL(GyroV1); break; 
                case 1:  KLRLK_LEDValue=DISPLVAL(GyroA1); break; 
                case 2:  KLRLK_LEDValue=DISPLVAL(AcclA1); break; 
                case 3:  KLRLK_LEDValue=DISPLVAL(CompA1); break; 
                case 4:  KLRLK_LEDValue=DISPLVAL(GyroV2); break; 
                case 5:  KLRLK_LEDValue=DISPLVAL(GyroA2); break; 
                case 6:  KLRLK_LEDValue=DISPLVAL(AcclA2); break; 
                case 7:  KLRLK_LEDValue=DISPLVAL(CompA2); break; 

                case 8:  KLRLK_LEDValue=DISPLVAL(AgFlt1); break; 
                case 9:  KLRLK_LEDValue=DISPLVAL(AgFlt2); break;

                case 0xa: KLRLK_LEDValue=DISPLVAL(x); break; 
                case 0xb: KLRLK_LEDValue=DISPLVAL(y); break; 
                }
              break;
            case 7:
              switch((sw>>8)&0xf)
                {
                case 0:  KLRLK_LEDValue=HexToInt((ADF0>>3)&0x1fff);  break;  /* ch1-rate */
                case 1:  KLRLK_LEDValue=HexToInt((ADF1>>3)&0x1fff);  break;  /* ch1-2.5V */
                case 2:  KLRLK_LEDValue=HexToInt((ADF2>>3)&0x1fff);  break;  /* ch1-yout */
                case 3:  KLRLK_LEDValue=HexToInt((ADF3>>3)&0x1fff);  break;  /* ch1-xout */
                case 4:  KLRLK_LEDValue=HexToInt((ADF4>>3)&0x1fff);  break;  /* ch2-rate */
                case 5:  KLRLK_LEDValue=HexToInt((ADF5>>3)&0x1fff);  break;  /* ch2-2.5V */
                case 6:  KLRLK_LEDValue=HexToInt((ADF6>>3)&0x1fff);  break;  /* ch2-xout */
                case 7:  KLRLK_LEDValue=HexToInt((ADF7>>3)&0x1fff);  break;  /* ch2-yout */

                case 8:  KLRLK_LEDValue=((ADF0>>4));  break;  /* ch1-rate */
                case 9:  KLRLK_LEDValue=((ADF2>>4));  break;  /* ch1-yout */
                }
              break;
            case 8:
              switch((sw>>8)&0xf)
                {
#define ADF_OF(ADF,OF) HexToInt((( (signed int)(ADF-(OF>>8)) )>>3)+5000)
                case 0:  KLRLK_LEDValue=ADF_OF(ADF0,OF0);  break;  /* ch1-rate */
                case 1:  KLRLK_LEDValue=ADF_OF(ADF1,OF1);  break;  /* ch1-2.5V */
                case 2:  KLRLK_LEDValue=ADF_OF(ADF2,OF2);  break;  /* ch1-yout */
                case 3:  KLRLK_LEDValue=ADF_OF(ADF3,OF3);  break;  /* ch1-xout */
                case 4:  KLRLK_LEDValue=ADF_OF(ADF4,OF4);  break;  /* ch2-rate */
                case 5:  KLRLK_LEDValue=ADF_OF(ADF5,OF5);  break;  /* ch2-2.5V */
                case 6:  KLRLK_LEDValue=ADF_OF(ADF6,OF6);  break;  /* ch2-xout */
                case 7:  KLRLK_LEDValue=ADF_OF(ADF7,OF7);  break;  /* ch2-yout */

                case 8:  KLRLK_LEDValue=((ADF0>>4));  break;  /* ch1-rate */
                case 9:  KLRLK_LEDValue=((ADF2>>4));  break;  /* ch1-yout */
                }
              break;
            case 9:
              switch((sw>>8)&0xf)
                {
                case 0: KLRLK_LEDValue=HexToInt(cyclerest); break;
                case 1: KLRLK_LEDValue=sec>>(sw&0xf); break;
                }
              break;
            }
        }
      __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[8]==0)
        {
          ppsc++;
          if(ppsc>=200)
            {
              SLNop();
              ppsc=0;
            }
        }
      else
        {
          ppsc++;
          if(ppsc>=RegFileL[8])
            {
              ppsc=0;
              SLReply32(0x3f, 8,x); 
              SLReply32(0x3f, 9,y); 
              SLReply32(0x3f,10,xofs); 
              SLReply32(0x3f,11,yofs); 
              SLReply32(0x3f,12,thx); 
              SLReply32(0x3f,13,thy); 
            }
        }
    }
}


/* 試験パラメータ
   080123 set32 2 4 1500; set32 2 5 500; set32 2 6 10; set32 2 7 300
  */