/* Wheel Inverted pendulum control */
/* version 2.0 */
/* revision 2 t
 [*/
#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 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 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,MS3;
volatile int PP0,PP1,PP2,PP3;
volatile int PW01,PW23; /* power up flag */
#define MHPower 0x00
#define MLPower 0x80
volatile int MOnOff;

#define MPCLK1    0x01
#define MPCLK1B   B0
#define MPCCW1    0x02
#define MPCCW1B   B1

#define MPCLK2    0x04
#define MPCLK2B   B2
#define MPCCW2    0x08
#define MPCCW2B   B3

#define MPCLKS    0x05
#define MPDIRMASK 0x0a

#define MPENABLE  0x10
#define MPM12     0x40
#define MPTQ      0x80
#define MPDECAY   0x20

int mch;

void InitializeSMDriver(void)
{
  MS0=MS1=MS2=MS3=0;
  PP0=PP1=PP2=PP3=0;
  PW01=PW23=MPM12|MLPower;
  /* 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 P3
#define SMP1 P2

      if(MOnOff) /* on */
        {
          SMP0.DR.BYTE=(SMP0.DR.BYTE&MPDIRMASK)|PW01|MPENABLE|MPCLKS;
          SMP1.DR.BYTE=(SMP1.DR.BYTE&MPDIRMASK)|PW23|MPENABLE|MPCLKS;
        }
      else /* off */
        {
          SMP0.DR.BYTE=(SMP0.DR.BYTE&MPDIRMASK)|PW01|0|MPCLKS;
          SMP1.DR.BYTE=(SMP1.DR.BYTE&MPDIRMASK)|PW23|0|MPCLKS;
        }

/*      if(mch)
        {*/
          /*b=PP0&0xc000;*/
          b=PP0;
          PP0+=MS0;
          if(MS0&0x8000) SMP0.DR.BIT.MPCCW1B=0;
          else           SMP0.DR.BIT.MPCCW1B=1;
          /*a=PP0&0xc000;*/
          /*if((a^b)==0xc000)*/
          if((PP0^b)&0x8000)
            SMP0.DR.BIT.MPCLK1B=0;

/*          b=PP2;
          PP2+=MS2;
          if(MS2&0x8000) SMP1.DR.BIT.MPCCW1B=0;
          else           SMP1.DR.BIT.MPCCW1B=1;
          if((PP2^b)&0x8000)
            SMP1.DR.BIT.MPCLK1B=0;

          mch=0;
        }
      else
        {*/
          b=PP1;
          PP1+=MS1;
          if(MS1&0x8000) SMP0.DR.BIT.MPCCW2B=0;
          else           SMP0.DR.BIT.MPCCW2B=1;
          if((PP1^b)&0x8000)
            SMP0.DR.BIT.MPCLK2B=0;

/*          b=PP3;
          PP3+=MS3;
          if(MS3&0x8000) SMP1.DR.BIT.MPCCW2B=0;
          else           SMP1.DR.BIT.MPCCW2B=1;
          if((PP3^b)&0x8000)
            SMP1.DR.BIT.MPCLK2B=0;

          mch=1;
        }*/
    }

  /* 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;
  /* PA.DDR=0x20; *//* PA-5(DIR) output */
}



void main(void)
{
  long nexttime;
  volatile int i;
  volatile unsigned long sec=0,sw;
  int MotorSpeedX,YawSpeed;
  int cyclerest,ppsc;
  int offsetc;
  unsigned long OF0,OF1,OF2,OF3,OF4,OF5,OF6,OF7; /* ADオフセット */
  long GyroV1;  /* ジャイロ角速度 */
  long GyroA1;  /* ジャイロ角度   */
  long AcclA1;  /* 加速時計角度   */
  long AgFlt1;  /* 加速度フィルタ */
  long sub;
  long CompA1;
  volatile long thx,thofx,thvx; /* 角度状態量 */
  volatile long x,vx,vxcmd,acx,xofs;  /* 位置状態量 */
  long acxt; /* 加速度にフィルタ */
  volatile int mvx,mvrot;
  int lastcont;
  int mx,my,mrot;
  int stbf,stbft,slbf,slbft,buttone;
  int beep;
  int chsel,ledpat,tval,ledf,udcnt,udv;
  
  for(i=0;i<16;i++)
    {
      RegFileS[i]=0;
      RegFileL[i]=0;
    }
  

  SMP0.DR.BYTE=0x00;
  SMP0.DDR=0xff;
  SMP1.DR.BYTE=0x00;
  SMP1.DDR=0xff;

  InitializeSerialLoop(2);
  InitializeKLRlib();

  InitializeSMDriver();
  InitializeITU();

  ppsc=0;
  RegFileS[0]=1;
  lastcont=0;
  
  
  RegFileS[4]=  2; /* 移動加速, 上限 */
  RegFileS[5]=1000;
  RegFileS[6]=  5; /* 旋回加速, 上限 */
  RegFileS[7]= 500;
                     
  RegFileS[3]=0;
/*  RegFileL[4]=120;
  RegFileL[5]=120;
  RegFileL[6]=100;
  RegFileL[7]=100;*/
  /*SetParams(0);*/
  
  mvx=mrot=0;
  vxcmd=0;
  stbf=slbf=0;
  beep=40;
  buttone=0;

  chsel=0;
  ledf=0;
  udcnt=0;
  
  nexttime=PWMsysclock+164*100l;
  while(1)
    {
      cyclerest=(int)(nexttime-PWMsysclock);
      
      while(PWMsysclock<nexttime)
        ;
      nexttime+=164;
      sec++;
      RegFileL[0]=sec;

#define PBUP (((sw & 0x10)==0)&&(buttone))
#define PBDN (((sw & 0x20)==0)&&(buttone))
#define PBLF (((sw & 0x80)==0)&&(buttone))
#define PBRI (((sw & 0x40)==0)&&(buttone))
#define PBST (((sw & 0x04)==0)&&(buttone))
#define PBSL (((sw & 0x08)==0)&&(buttone))
#define PBYB (((sw & 0x02)==0)&&(buttone))
#define PBAB (((sw & 0x01)==0)&&(buttone))

      /* button enable */
      if((sw&0xff)==0xff) buttone=1;
      
      /* startbutton */
      stbft=0;
      if(stbf==0)
        { if(PBST) { stbf=10; stbft=1; } }
      else
        { if(stbf>0) stbf--; if(PBST) stbf=10; }
      
      if(stbft) beep=10;

      if(stbft)
        {
          if(RegFileL[1]==0) RegFileL[1]=1000000;
          else               RegFileL[1]=0;
        }
        
      if((0<RegFileL[1])&&(RegFileL[1]<2000))  /* time up */
        {
          beep=(sec&0x80)?1:0;
        }
      
      mx=mrot=0;
      if(RegFileL[1])
        {
          if(PBUP) { mx++; }
          if(PBDN) { mx--; }
          if(PBRI) { mrot++; }
          if(PBLF) { mrot--; }
        }
      if(!RegFileL[1])
        {
          if((PBYB)&&(PBAB))
            {
              beep=20;
              RegFileS[0]=1;
            }
        }

      /* parameter */
      if(RegFileL[1]==0)
        {
          /* selectbutton */
          slbft=0;
          if(slbf==0)
            { if((PBLF)||(PBRI)) { slbf=10; slbft=1; } }
          else
            { if(slbf>0) slbf--; if((PBLF)||(PBRI)) slbf=10; }
          
          if(slbft) beep=10;
          
          if(slbft)
            {
              if(PBLF) chsel--;
              if(PBRI) chsel++;
              chsel=chsel&0x3;
              ledf=-100;
            }
          
          udv=0;
          if((PBUP)||(PBDN))
            {
              udcnt++; ledf=200;
              if(udcnt<0xc0)
                { if((udcnt&0x3f)==0x2) { udv=1; beep=1;} }
              else if(udcnt<0x180)
                { if((udcnt&0x0f)==0x2) { udv=1; beep=1;} }
              else
                { if((udcnt&0x07)==0x2) { udv=4; beep=1;} }
            }
          else
            udcnt=0;
          
          if(PBUP)
            RegFileL[chsel+4]+=udv;
          if(PBDN)
            RegFileL[chsel+4]-=udv;
          if((signed long)(RegFileL[chsel+4])> 500) RegFileL[chsel+4]= 500;
          if((signed long)(RegFileL[chsel+4])<-500) RegFileL[chsel+4]=-500;
        }
      
#if 0
      
      if((PBP.DR.BYTE&0x5)==0) /* 赤青左 */
        {
          switch((~PBP.DR.BYTE)&0xf0)
            {
            case 0x10: SetParams(0); break;
            case 0x20: SetParams(1); break;
            case 0x40: SetParams(2); break;
            case 0x80: SetParams(3); break;
            }
        }
      if((PBP.DR.BYTE&0xa)==0) /* 赤青右 */
        {
          switch((~PBP.DR.BYTE)&0xf0)
            {
            case 0x10: SetParams(4); break;
            case 0x20: SetParams(5); break;
            case 0x40: SetParams(6); break;
            case 0x80: SetParams(7); break;
            }
        }

 #endif
      /* センサ情報 処理 ================================================== */
      if(RegFileS[0]==1) /* オフセット除去指令 */
        {
          OF0=OF1=OF2=OF3=OF4=OF5=OF6=OF7=0;
          offsetc=256;
          RegFileS[0]=2;
          GyroV1=GyroA1=0;
          AcclA1=0;  AgFlt1=0;  CompA1=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
        { /* 角度計算 */

          GyroV1= (((long)ADF3<<8)-(long)OF3);  /* gyro */
          AcclA1=-(((long)ADF0<<8)-(long)OF0); 
          GyroA1+=(GyroV1+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*190)>>11)-GyroA1;
          AgFlt1+=(sub-AgFlt1+0x100)>>9;
          CompA1=GyroA1+AgFlt1;
          
        }

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

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

      if(RegFileL[1]>0) /* 制御中 */
        {
          RegFileL[1]--; 
          MOnOff=1;
          thx-=thofx;
          acxt=
            (((((signed long)RegFileL[4])*thx           )<<6) +
             ((((signed long)RegFileL[5])*thvx          )<<2) +
             ((((signed long)RegFileL[6])*(x-xofs)  +0x2)>>2)   +
             ((((signed long)RegFileL[7])*(vx-vxcmd)    )   )  +0x800l)>>12;
          /* filter */
          acx=acx+((acxt-acx)>>(RegFileS[3]));
          /* filter */
          vx+=acx;
          x +=(vx>>4);
          MotorSpeedX=(vx>>4);
          YawSpeed=0;
        }
      else /* 制御停止中 */
        {
          MOnOff=0; /* モータカット */
          x=vx=acx=0;
          MotorSpeedX=0;
          YawSpeed=0;
          mvx=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(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;
      vxcmd=(mvx<<4);

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

/*      P2.DDR=0xff;  P3.DDR=0xff;
      P2.DR.BYTE=(sw>>8)&0xff;
      P3.DR.BYTE=sw&0xff;*/
      
      /* 表示その他 */
#define DISPLVAL(LVAL) HexToInt((LVAL>>(((sw>>8)&0xf)*2))+5000)

      switch((sw>>12)&0xf)
        {
        case 1:  KLRLK_LEDValue=DISPLVAL(GyroV1); break; 
        case 2:  KLRLK_LEDValue=DISPLVAL(GyroA1); break; 
        case 3:  KLRLK_LEDValue=DISPLVAL(AcclA1); break; 
        case 4:  KLRLK_LEDValue=DISPLVAL(CompA1); break; 

        case 5:  KLRLK_LEDValue=HexToInt((ADF0>>3)&0x1fff);  break;  /* ch1-rate */
        case 6:  KLRLK_LEDValue=HexToInt((ADF1>>3)&0x1fff);  break;  /* ch1-2.5V */
        case 7:  KLRLK_LEDValue=HexToInt((ADF2>>3)&0x1fff);  break;  /* ch1-yout */
        case 8:  KLRLK_LEDValue=HexToInt((ADF3>>3)&0x1fff);  break;  /* ch1-xout */

        case 9:  KLRLK_LEDValue=sw&0xff;     break;
#if 0
        case 8:  KLRLK_LEDValue=DISPLVAL(AgFlt1); break; 
        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 */
#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 */
        case 0: KLRLK_LEDValue=HexToInt(cyclerest); break;
        case 1: KLRLK_LEDValue=sec>>(sw&0xf); break;
#endif
        }

      __KLR_LEDHKC=(__KLR_LEDHKC+1)&0x3;
      if(((sw>>12)&0xf)!=0) /* mode sw !=0*/
        {
          ledpat=KLR_LEDpat[(KLRLK_LEDValue>>(__KLR_LEDHKC*4))&0xf];
        }
      else /* normal operation */
        {
          if((((sec&0x300)==0)||(ledf<0))&&(ledf<=0)) /* ch */
            {
              if(__KLR_LEDHKC==3) ledpat=KLR_LEDpat[chsel];
              else ledpat=0;
            }
          else
            {
              ledpat=0;
              tval=RegFileL[4+chsel];
              if(tval<0)
                {
                  tval=-tval;
                  if(__KLR_LEDHKC==3) ledpat=0x20;
                }
              KLRLK_LEDValue=HexToInt(tval);
              if(__KLR_LEDHKC!=3)
                ledpat=KLR_LEDpat[(KLRLK_LEDValue>>(__KLR_LEDHKC*4))&0xf];
            }
        }
      KLRLEDKeyRaw(((beep>0)?0x0:0x10)|
                   (0xff0f&~((0x8>>__KLR_LEDHKC)|
                             ((ledpat)<<8))),&sw);
      if(beep) beep--;
      if(ledf>0) ledf--;
      if(ledf<0) ledf++;
      
      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,10,xofs); 
              SLReply32(0x3f,12,thx); 
            }
        }
    }
}


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