#include "led.h"
#include "delay.h"
#include "key.h"
#include "sys.h"
#include "usart.h"
#include "timer.h"
#include "adc.h"
#include "lcd.h"
#include "encode.h"

u8 mode;
u16 adcx,count,num;
u8 flag=0;

int err,err_last,err2,err2_last,set_num=1000,set_adcx=2075;
int  pid_bianma,pid_adc;
//float  kp=0,kd=0,ki=0,kp2=3.3,kd2=5,ki2=0,i=0,i2=0;   //kp 编码器 kp2 adc
float  kp=0,kd=0,ki=0,kp2=4.3,kd2=15.0,ki2=0,i=0,i2=0;   //kp 编码器 kp2 adc
int pwma=0,pwmb=0;

float pidbianma(void );
float pidadc(void );
u16 Get_Adc_Average(u8 ch,u8 times);

//int Error,Error_Last,Error1,Error1_Last,Target1 = 3000,Target = 535; //515
//float PID_Out,PID_Out1,PID_Out2;
//float Kp =0,Kd = 0,Kp1 = 0,Kp2 = 50,Kd2 =0;
        int main(void)
{               
        //u16 led0pwmval=0;
//u16 adcx,volt;
        u16 temp;
        float jiaodu;
        u8 dir=1;
//        u8 mode=1;
         u16 count;//编码器计数   PB6 PB7
         
        delay_init();                     //延时函数初始化          
        NVIC_Configuration();          //设置NVIC中断分组2:2位抢占优先级，2位响应优先级
        uart_init(9600);         //串口初始化为9600
        LED_Init();                             //LED端口初始化
        LCD_Init();                                
        Adc_Init();                                  //ADC初始化
        TIM4_Init();//定时器编码器接口初始化

        TIM3_PWM_Init(999,17);         //不分频。PWM频率=4Khz
  TIM5_Int_Init(999,1439);  //定时 10ms 719 5ms 1439

        POINT_COLOR=BLUE;//设置字体为蓝 {MOD}
        LCD_ShowString(60,130,200,16,16,"ADC_CH0_VAL:");             
        LCD_ShowString(60,150,200,16,16,"ADC_CH0_JDU:");          
           while(1)
        {
//                 delay_ms(10);         
//                if(dir)led0pwmval++;
//                else led0pwmval--;

//                 if(led0pwmval>300)dir=0;
//                if(led0pwmval==0)dir=1;                                                                                 
//                //TIM_SetCompare2(TIM3,led0pwmval);
               
                if(num>1500) num =1500;
                if(num<500)  num =500;      
               
                  adcx=Get_Adc_Average(ADC_Channel_1,10);
                        LCD_ShowxNum(156,130,adcx,4,16,0);//显示ADC的值
                  jiaodu=adcx*(360/4096);
                        LCD_ShowxNum(156,150,jiaodu ,4,16,0);//显示电压值
                  
                  count = TIM4->CNT/4;//获取计数值
                  num=count ;
                  //delay_ms(500);//每隔1s打印一次编码器角度,用手去拨动编码器  使其慢速旋?
                  printf ("adcx%d  ",adcx );
//                  printf ("%d  ",pid_bianma );
                  printf ("%d  ",pwma  );
            printf("count%d ",num  );
                       
//                        printf("%d ",pwma );
//                        printf("%d ",pwmb );
                       
                        //delay_ms (500);
                  
//      if(mode ==1)
//      {                               
//                                TIM_SetCompare2(TIM3,500);   //TIM3_CH2 PA7
//                                TIM_SetCompare1(TIM3,0);     //TIM3_CH1 PA6
//                                delay_ms (1000);             //TIM3_CH3 PB0
//                                TIM_SetCompare2(TIM3,0);
//                                TIM_SetCompare1(TIM3,500);
//                                delay_ms (500);                               
//      }
//      else if(mode==2)
//      {
//                                TIM_SetCompare2(TIM3,0);
//                                TIM_SetCompare1(TIM3,300);
//                                delay_ms (1000);
//                                TIM_SetCompare2(TIM3,800);
//                                TIM_SetCompare1(TIM3,0);
//                                delay_ms (1000);
//                        }
//                        else if(mode ==3)
//                        {                       
////                                pwma =500+pid_bianma +pid_adc ;
////                                pwmb= 500-pid_bianma -pid_adc ;
//                                if(flag >=4)
//                                {
//                                flag =0;
//                                pwma =500-pidbianma()+pidadc() ;
//                                pwmb =500+pidbianma()-pidadc() ;
//                                }
//                                else
//                                {
//                                        pwma =500+pidadc ();
//                                        pwmb =500-pidadc ();
//                                        flag ++;
//                                }
//                               
//                                if(pwma >999) pwma =999;
//                                if(pwma<0)    pwma =0;
//                           if(pwmb >999) pwmb =999;
//                                if(pwmb<0)    pwmb =0;       
//                               
//                                TIM_SetCompare2(TIM3,pwma );   //TIM3_CH2 PA7
//                                TIM_SetCompare1(TIM3,pwmb );     //TIM3_CH1 PA6
////                                delay_ms (1000);             //TIM3_CH3 PB0
////                                TIM_SetCompare2(TIM3,0);
////                                TIM_SetCompare1(TIM3,500);
////                                delay_ms (500);       
//                               
//                        }
//                        else if(mode ==4)
//                        {
//                                TIM_SetCompare2(TIM3,0 );   //TIM3_CH2 PA7
//                                TIM_SetCompare1(TIM3,0 );     //TIM3_CH1 PA6
//                        }
                        }
                       
               
        }         
// float PID_Cal2()                   //??? ???
//{
//Error1 = Target1 -num;            //Target1 = 3000,Target = 515;
//PID_Out2 = Error1*Kp2 + (Error1 - Error1_Last)*Kd2;           //num = 3000
//Error1_Last = Error1;
//return PID_Out2;
//}
                float pidbianma()
                {
                        err=set_num-num  ;
                        pid_bianma=err*kp+(err-err_last )*kd;
                        err_last =err ;
                        return pid_bianma ;
                }
                float pidadc()
                {
//                 adcx=Get_Adc_Average(ADC_Channel_1,10);
//                        err2 =set_adcx - adcx ;
                        err2 =set_adcx -Get_Adc_Average(ADC_Channel_1,10) ;
                        pid_adc =err2 *kp2 +(err2 -err2_last )*kd2 ;
                        err2_last =err2 ;
                        return pid_adc ;
                }
void TIM5_IRQHandler(void)   //TIM3中断
{
        if (TIM_GetITStatus(TIM5, TIM_IT_Update) != RESET) //检查指定的TIM中断发生与否:TIM 中断源
                {
                TIM_ClearITPendingBit(TIM5, TIM_IT_Update  );  //清除TIMx的中断待处理位:TIM 中断源
                LED1=!LED1;
                        if(adcx <1690||adcx >2570)//2650 3530       
                        {
                                mode =4;
                        }
                        else  mode =3;
                       
//                        if(mode ==1)
//      {                               
//                                TIM_SetCompare2(TIM3,500);   //TIM3_CH2 PA7
//                                TIM_SetCompare1(TIM3,0);     //TIM3_CH1 PA6
//                                delay_ms (1000);             //TIM3_CH3 PB0
//                                TIM_SetCompare2(TIM3,0);
//                                TIM_SetCompare1(TIM3,500);
//                                delay_ms (500);                               
//      }
//      else if(mode==2)
//      {
//                                TIM_SetCompare2(TIM3,0);
//                                TIM_SetCompare1(TIM3,300);
//                                delay_ms (1000);
//                                TIM_SetCompare2(TIM3,800);
//                                TIM_SetCompare1(TIM3,0);
//                                delay_ms (1000);
//                        }
                         if(mode ==3)
                        {                       
        err2 =set_adcx -adcx  ;
                          pid_adc =err2 *kp2 +(err2 -err2_last )*kd2 +i2*ki2 ;
                          err2_last =err2 ;
                                i2=i2+err2;
                               
                                err=set_num-num  ;
                          pid_bianma=err*kp+(err-err_last )*kd+i*ki;
                          err_last =err ;
                                i=i+err ;
                               
                                if(flag >=4)
                                {
                                flag =0;
//                                pwma =500-pidbianma()+pidadc() ;
//                                pwmb =500+pidbianma()-pidadc() ;
                                        pwma =pid_bianma+pid_adc;
                                  pwmb =pid_bianma-pid_adc;
                                }
                                else
                                {
//                                        pwma =500+pidadc ();
//                                        pwmb =500-pidadc ();
                                        pwma =pid_adc;
                                  pwmb =-pid_adc;
                                        flag ++;
                                }
                                if(pwma<0)
                                {
                                        pwma =-pwma ;
                                        if(pwma >600) pwma =600;
                                        if(pwma<0)    pwma =0;
                                       
                                        TIM_SetCompare2(TIM3,pwma );   //TIM3_CH2 PA7
                                  TIM_SetCompare1(TIM3,0  );     //TIM3_CH1 PA6
                                }
                                else
                                {
                                        if(pwma >600) pwma =600;
                                        if(pwma<0)    pwma =0;
                                        TIM_SetCompare2(TIM3,0 );   //TIM3_CH2 PA7
                                  TIM_SetCompare1(TIM3,pwma   );     //TIM3_CH1 PA6
                                }
                               
//                                if(pwma >999) pwma =999;
//                                if(pwma<0)    pwma =0;
//                           if(pwmb >999) pwmb =999;
//                                if(pwmb<0)    pwmb =0;       
//                               
//                                TIM_SetCompare2(TIM3,pwma );   //TIM3_CH2 PA7
//                                TIM_SetCompare1(TIM3,pwmb  );     //TIM3_CH1 PA6
//                                delay_ms (1000);             //TIM3_CH3 PB0
//                                TIM_SetCompare2(TIM3,0);
//                                TIM_SetCompare1(TIM3,500);
//                                delay_ms (500);       
                               
                        }
                        else if(mode ==4)
                        {
                                TIM_SetCompare2(TIM3,0 );   //TIM3_CH2 PA7
                                TIM_SetCompare1(TIM3,0 );     //TIM3_CH1 PA6
                        }
                               
                       
                       
                }
}