/* 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 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;
volatile int PP0,PP1,PP2;
volatile int PW0,PW1,PW2; /* power up flag */
#define MHPower 0x8
#define MLPower 0x0
volatile int MOnOff;

/*3ch*/
/* bit 0,1 : CLK1,2 */
/* bit 2   : Enable */
/* bit 3   : REF */


void InitializeSMDriver(void)
{
  MS0=MS1=MS2=0;
  PP0=PP1=PP2=0;
  PW0=PW1=PW2=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 P1
#define SMP1 P2
#define SMP2 P5
      
      
      if(MOnOff) /* on */
        {
          SMP0.DR.BYTE=PW0|0|0x3;
          SMP1.DR.BYTE=PW1|0|0x3;
          SMP2.DR.BYTE=PW2|0|0x3;
        }
      else /* off */
        {
          SMP0.DR.BYTE=PW0|0x4|0x3;
          SMP1.DR.BYTE=PW1|0x4|0x3;
          SMP2.DR.BYTE=PW2|0x4|0x3;
        }
      
      b=PP0&0xc000;
      PP0+=MS0;
      a=PP0&0xc000;
      if((b==0xc000)&&(a==0x0000)) /* minus -> plus */
        SMP0.DR.BIT.B1=0;
      else if((b==0x0000)&&(a==0xc000)) /* plus -> minus */
        SMP0.DR.BIT.B0=0;

      b=PP1&0xc000;
      PP1+=MS1;
      a=PP1&0xc000;
      if((b==0xc000)&&(a==0x0000)) /* minus -> plus */
        SMP1.DR.BIT.B1=0;
      else if((b==0x0000)&&(a==0xc000)) /* plus -> minus */
        SMP1.DR.BIT.B0=0;

      b=PP2&0xc000;
      PP2+=MS2;
      a=PP2&0xc000;
      if((b==0xc000)&&(a==0x0000)) /* minus -> plus */
        SMP2.DR.BIT.B1=0;
      else if((b==0x0000)&&(a==0xc000)) /* plus -> minus */
        SMP2.DR.BIT.B0=0;

    }

  /* 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 */
}


/* スイッチ定数 */
#define PBP P3
#define PBRL (P3.DR.BIT.B0==0)
#define PBRR (P3.DR.BIT.B1==0)
#define PBBL (P3.DR.BIT.B2==0)
#define PBBR (P3.DR.BIT.B3==0)
#define PBOL (P3.DR.BIT.B4==0)
#define PBOR (P3.DR.BIT.B5==0)
#define PBGL (P3.DR.BIT.B6==0)
#define PBGR (P3.DR.BIT.B7==0)

#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;
  
  SMP0.DR.BYTE=0xff;
  SMP0.DDR=0x0f;
  SMP1.DR.BYTE=0xff;
  SMP1.DDR=0x0f;
  SMP2.DR.BYTE=0xff;
  SMP2.DDR=0x0f;
  
  for(i=0;i<16;i++)
    {
      RegFileS[i]=0;
      RegFileL[i]=0;
    }
  
  InitializeSerialLoop(2);
  InitializeKLRlib();

  InitializeSMDriver();
  InitializeITU();

  ppsc=0;
  RegFileS[0]=1;
  lastcont=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;
    }*/
  SetParams(0);
  
  mvx=mvy=mrot=0;
  vxcmd=vycmd=0;
  
  nexttime=PWMsysclock+164;
  while(1)
    {
      cyclerest=(int)(nexttime-PWMsysclock);
      
      while(PWMsysclock<nexttime)
        ;
      nexttime+=164;
      sec++;
      RegFileL[0]=sec;

      
      /* スイッチ操作 */
      if((PBRL)&&(PBRR))  /* 赤同時押しで制御停止 */
        {
          RegFileL[1]=0;
        }
      if((RegFileL[1]==0)&&(PBRL)&&(PBRR)&&((PBBL)||(PBBR)))  /* 赤同時押し＆青でセンサオフセット除去 */
        {
          RegFileS[0]=1;
        }
      if((!PBRL)&&(!PBRR))  /* 赤は触っていない */
        {
          if((PBBL)||(PBBR))  /* 青で制御開始(1[s]) */
            {
              if(RegFileL[1]<200) RegFileL[1]=200;
            }
          if((PBBL)&&(PBBR))  /* 青で制御開始(10[s]) */
            {
              if(RegFileL[1]<0x7fffffff) RegFileL[1]=0x7fffffff;
            }
        }
      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;
            }
        }

      mx=my=mrot=0;
      if((!PBRL)&&(!PBRR)&&(!PBBL)&&(!PBBR))  /* 赤青触らず */
        {
          if((PBOR)) { my++; mx++; }
          if((PBOL)) { my++; mx--; }
          if((PBGR)) { my--; mx++; }
          if((PBGL)) { my--; mx--; }
          if((PBOR)&&(PBGL)) { mrot++; }
          if((PBOL)&&(PBGR)) { mrot--; }
        }
      
      /* センサ情報 処理 ================================================== */
      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>10000)||(MotorSpeedX<-10000)) RegFileL[1]=0;
      if((MotorSpeedY>10000)||(MotorSpeedY<-10000)) 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; 
                }
              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
  */