#include "timer.h"
#include "led.h"
#include "wifi.h"
#include "math.h"
#include "encoder.h"
#include "motor.h"

extern char smog_value_buf[7];  
extern char temp_value_buf[7];
extern char beam_value_buf[7]; 

extern struct UserInfo userInfo;


float Velocity_KP_Mode2 = 1.1,Velocity_KI_Mode2=7.7;


void Set_Pwm(int motor)
{
	if(motor<0)	AIN2=1,	AIN1=0;
	else AIN2=0,AIN1=1;
	PWMA=abs(motor);
}

int Incremental_PI_B_Mode2 (int Encoder,int Target)
{ 	
	 static int Bias,Pwm,Last_bias;
	 Bias=Encoder-Target;                //计算偏差
	 Pwm+=Velocity_KP_Mode2*(Bias-Last_bias)+Velocity_KI_Mode2*Bias;   //增量式PI控制器
	 if(Pwm>7200)Pwm=7200;
	 if(Pwm<-7200)Pwm=-7200;
	 Last_bias=Bias;	                   //保存上一次偏差 
	 return Pwm;                         //增量输出
}


void TIM3_Int_Init(u16 arr,u16 psc)
{
    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	NVIC_InitTypeDef NVIC_InitStructure;

	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); 

	TIM_TimeBaseStructure.TIM_Period = arr; 
	TIM_TimeBaseStructure.TIM_Prescaler =psc; 
	TIM_TimeBaseStructure.TIM_ClockDivision = 0; 
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; 
	TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
 
	TIM_ITConfig(  
		TIM3, 
		TIM_IT_Update ,
		ENABLE  
		);
	NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;  
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;  
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;  
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; 
	NVIC_Init(&NVIC_InitStructure);  

	TIM_Cmd(TIM3, ENABLE);  
							 
}


void TIM3_IRQHandler(void)   
{
	int pwm;
	if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET) 
	{
		userInfo.currentSpeed = Read_Encoder(2);
		pwm = Incremental_PI_B_Mode2(userInfo.currentSpeed, 10);
		Set_Pwm(pwm);
		TIM_ClearITPendingBit(TIM3, TIM_IT_Update); 
	}
}