#include <hidef.h> /* common defines and macros */
#include "derivative.h" /* derivative-specific definitions */
#include "OLED12864.h"

#define LL PWMDTY23
#define LH PWMDTY01
#define RL PWMDTY45
#define RH PWMDTY67


#define N 12

uint valuex_buf[N]; //float position collection array
uint valuey_buf[N];
char i=0,i_y=0;


uint ADx_value,ADy_value;

int NET=0;

float A=0,B=0;
float AD_X,AD_Y;


float E_AD_X,E1_AD_X,E_AD_Y,E1_AD_Y;

float P_x =0.8;
float D_x =15.5;
float P_y =-0.8;
float D_y =-15;

float pwm_x,pwm_y;
//float Pwm_x = -300,Pwm_y = -300;


float SET_AD_X = 1400;
float SET_AD_Y = 1370;



//************************************************ **
//****** Set the main frequency *******
//************************************************ **
void Busclock(void) //05,01 48MHz 06,01 56MHz 07,01 64MHz 08,01 72MHz
{
    CLKSEL = 0x00; //Use external crystal oscillator before initializing PLL
    PLLCTL_PLLON = 1;//Turn on PLL
    SYNR = 0xC0 | 0x08;
    REFDV = 0xC0 | 0x01;
    POSTDIV = 0x00;
    _asm(nop);
    _asm(nop);
    while(!(CRGFLG_LOCK==1));//Wait for PLL to stabilize
    CLKSEL_PLLSEL = 1; //fbus is set according to the PLL frequency
}




//******************Timer initialization****************************** ***
void initPIT()//Timed interrupt initialization function 1MS timed interrupt setting
{
   PITCFLMT_PITE=0; //Timer interrupt channel 0 is off
   PITCE_PCE0=1;//Timer channel 0 enabled
   PITMTLD0=64-1; //8-bit timer initial value setting. Divided by 240, under 24MHzBusClock, it is 0.1MHz 1us
   PITLD0= 1000; //16-bit timer initial value setting. PITTIME*0.01MS 1000*0.001=1ms
   PITINTE_PINTE0=1;//Timer interrupt channel 0 interrupt enable
   PITCFLMT_PITE=1;//Timer channel 0 enabled
}

//************************************************ *************
//******************12-digit AD initialization************************** ****
//************************************************** ************
void int_ad() //AD initialization, 12-bit precision
{
     ATD0DIEN = 0;
     ATD0CTL1 = 0x50;
     ATD0CTL2 = 0x40;
     ATD0CTL3 = 0xb0;
     ATD0CTL4 = 0x29;
     ATD0CTL5 = 0x30;
}

//****************Read the AD value**********************
void get_ad() //Read the AD value, the value is placed in the array for median filtering
{

     while(ATD0STAT0&0X80!=1);
      ADx_value=ATD0DR2;
      ADy_value=ATD0DR3;

}



//---------------------------PWM initialization------------------- --------------------
   void PWM_speed_Init(void)
{
       PWME_PWME1=0; //Disabled
       PWM_PWME3=0;
       PWMPRCLK=0x22; //64/4=16MHz //72/4=18MHZ
       PWMCLK_PCLK1=0; //Set A as its clock source
       PWMCLK_PCLK3=0; //B is its clock source
       PWMPOL_PPOL1=1; //Rising edge flip
       PWMCAE_CAE1=0; //Left aligned output
       PWMPOL_PPOL3=1; //Rising edge flip
       PWMCAE_CAE3=0; //Left aligned output
       PWMCTL_CON01=1; //0, 1 level connection mode
       PWMCTL_CON23=1; //2, 3-level connection mode
       PWMDTY01=0; //Initialize the waveform with a duty cycle of 0
       PWMDTY23=0;
       PWMPER01=1600; //The output is a 10KHz wave
       PWMPER23=1600;
       PWMCNT01=0x00; //Clear channel 2 counter to 0
       PWMCNT23=0x00; //3 channel counter cleared
       PWME_PWME1=1; //2 channels are enabled, 2 channels are output channels
       PWME_PWME3=1; // 3 enabled
}

   void PWM_speed_Init_1(void)
{
       PWME_PWME5=0; //Disabled
       PWME_PWME7=0;
       PWMPRCLK=0x22; //64/4=16MHz /72/4=18MHZ
       PWMCLK_PCLK5=0; //Set A as its clock source
       PWMCLK_PCLK7=0; //B is its clock source
       PWMPOL_PPOL5=1; //Rising edge flip
       PWMCAE_CAE5=0; //Left aligned output
       PWMPOL_PPOL7=1; //Rising edge flip
       PWMCAE_CAE7=0; //Left aligned output
       PWMCTL_CON45=1; //0, 1 level connection mode
       PWMCTL_CON67=1; //2, 3-level connection mode
       PWMDTY45=0; //Initialize the waveform with a duty cycle of 0
       PWMDTY67=0;
       PWMPER45=1600; //The output is a 10KHz wave
       PWMPER67=1600;
       PWMCNT45=0x00; //Clear channel 2 counter to 0
       PWMCNT67=0x00; //3 channel counter cleared
       PWME_PWME5=1; //2 channels are enabled, 2 channels are output channels
       PWME_PWME7=1; // 3 enabled
}


//--------------------PWM output function---------------------- -----
void speed(float Left,float Right)
{
   if(Left>0)
   {
   LL = 0; LH = Left+B;
   }
   if(Left<0)
   {
   LL = -Left+B;LH = 0;
   }
   if(Left==0)
   {
    LL = 0; LH = 0;
   }
   if(Right==0)
   {
    RL = 0;RH = 0;
   }
   if(Right>0)
   {
    RL = 0;RH = Right+A;
   }
   if(Right<0)
   {
    RL = -Right+A;RH = 0;
   }
}


/****************************************************** **
******* Delay function ****
*************************************************** **/
void delay(uchar t)
{
uchar a;
for(a=0;a<t;a++);
}

void delaychang(uchar t)
{
uchar a,b;
for(a=0;a<t;a++)
    for(b=0;b<1000;b++);
}
//delay function
void delay_OLED(char mxs)
{
   char yy,zz,bb;
   for(yy=mxs;yy>0;yy--)
     for(zz=253;zz>0;zz--)
       for(bb=252;bb>0;bb--);
}





//---------------------------------
//---------N-order sliding filter program-----
//--------------------------

float filter_x()
   {
     float sum=0;
     char count;
     valuex_buf[i++]=ADx_value;
     if(i==N) i=0;
      for(count=0;count<N;count++)
         sum=sum+valuex_buf[count];
      return sum/N;
   }


float filter_y()
   {
     float sum=0;
     char count;
     valuey_buf[i_y++]=ADy_value;
     if(i_y==N) i_y=0;
      for(count=0;count<N;count++)
         sum=sum+valuey_buf[count];
      return sum/N;
   }





/**************************************************** ***
***** I/O initialization ****
*************************************************** **/
void Init_IO(void)
{
     DDRB=0xff;//PB6, PB5 output,,,0b01111110 = 0x7E, external counter reset, LED
     DDRA=0x00;//PA port collects the value of the external counter CD4520, the lower eight bits of the external 16-bit counter
     DDRM=0x00;//PM port collects the upper eight bits
     DDRH=0x00;

}


//------------------- Display function with parameters ----------------
   void xianshi_LCD(int a,int b,int c)
     {
       switch(c)
         {
         case 0CD_P8x16Str(a,b,"0");break;
         case 1:LCD_P8x16Str(a,b,"1");break;
         case 2:LCD_P8x16Str(a,b,"2");break;
         case 3:LCD_P8x16Str(a,b,"3");break;
         case 4:LCD_P8x16Str(a,b,"4");break;
         case 5:LCD_P8x16Str(a,b,"5");break;
         case 6:LCD_P8x16Str(a,b,"6");break;
         case 7:LCD_P8x16Str(a,b,"7");break;
         case 8:LCD_P8x16Str(a,b,"8");break;
         case 9:LCD_P8x16Str(a,b,"9");break;
         }
     }



void dispiay()
   {
      int bai=0;
      intshi=0;
      int ge1=0;
      int bai1=0;
      int shi1=0;
      intge=0;


     if(ADy_value>=1000)
     {

       xianshi_LCD(30,0,((int)ADy_value)/1000);
       bai=((int)ADy_value)%1000;
       xianshi_LCD(40,0,bai/100);
       shi=bai%100;
       xianshi_LCD(50,0,shi/10);
       ge=shi%10;
       xianshi_LCD(60,0,ge);
     }

     if(ADy_value>=100 && ADy_value<1000)
     {
       xianshi_LCD(30,0,0);
       xianshi_LCD(40,0,ADy_value/100);
       shi=ADy_value%100;
       xianshi_LCD(50,0,shi/10);
       ge=shi%10;
       xianshi_LCD(60,0,ge);
     }
     if(ADy_value<100)
     {
       xianshi_LCD(30,0,0);
       xianshi_LCD(40,0,0);
       xianshi_LCD(50,0,ADy_value/10);
       ge=ADy_value%10;
       xianshi_LCD(60,0,ge);
     }


     if(ADx_value>=1000)
     {

       xianshi_LCD(30,2,((int)ADx_value)/1000);
       bai1=((int)ADx_value)%1000;
       xianshi_LCD(40,2,bai1/100);
       shi1=bai1%100;
       xianshi_LCD(50,2,shi1/10);
       ge1=shi1%10;
       xianshi_LCD(60,2,ge1);
     }

     if(ADx_value>=100 && ADx_value<1000)
     {
       xianshi_LCD(30,2,0);
       xianshi_LCD(40,2,ADx_value/100);
       shi1=ADx_value%100;
       xianshi_LCD(50,2,shi1/10);
       ge1=shi1%10;
       xianshi_LCD(60,2,ge1);
     }
     if(ADx_value<100)
     {
       xianshi_LCD(30,2,0);
       xianshi_LCD(40,2,0);
       xianshi_LCD(50,2,ADx_value/10);
       ge1=ADx_value%10;
       xianshi_LCD(60,2,ge1);
     }

   }


   //********closed-loop control PID************************************** ***********

void AD_X_pid() //X direction PID adjustment
{
   E_AD_X=AD_X-SET_AD_X;
   pwm_x = P_x * E_AD_X + D_x * (E_AD_X - E1_AD_X);
   if(pwm_x>=1550) pwm_x = 1550;
   if(pwm_x<=-1550) pwm_x =-1550;
   E1_AD_X = E_AD_X;
}

   void AD_Y_pid() //Y direction PID adjustment
{
   E_AD_Y=AD_Y-SET_AD_Y;
   pwm_y = P_y * E_AD_Y + D_y * (E_AD_Y - E1_AD_Y);
if(pwm_y>=1550) pwm_y = 1550;
if(pwm_y<=-1550) pwm_y =-1550;
   E1_AD_Y = E_AD_Y;
}



void main(void)
{
      Busclock();
      Init_IO();
      int_ad();
      initPIT();
      get_ad();
      PWM_speed_Init();
      PWM_speed_Init_1();
      LCD_Init();
             EnableInterrupts;


   for(;;)
    {
    delay_OLED(250);
    dispiay();
    LCD_P8x16Str(10,0,"X:");
    LCD_P8x16Str(10,2,"Y:");
    delay_OLED(250);



   _FEED_COP(); /* feeds the dog */
   } /* loop forever */
   /*please make sure that you never leave main */
}



#pragma CODE_SEG __NEAR_SEG NON_BANKED
void interrupt 66 PIT0(void)
{

NET++;
if(NET==1)
   {
   get_ad();
   AD_Y=filter_y();
   AD_X=filter_x();

   AD_X_pid();
   AD_Y_pid();

   speed(pwm_y, pwm_x);
   //eed(Pwm_x,Pwm_y);
     NET=0;
   }
    PITTF_PTF0=1;//Interrupt clear flag
}
#pragma CODE_SEG __NEAR_SEG DEFAULT