#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;


/**
 * @description: 设置pwm
 * @param {int} motor
 * @return {*}
 */
void Set_Pwm(int motor)
{
	if(motor<0)	AIN2=1,	AIN1=0;
	else AIN2=0,AIN1=1;
	PWMA=abs(motor);
}

/**
 * @description: 
 * @param {int} Encoder  编码器速度
 * @param {int} Target   目标数据
 * @return {*}
 */
int velocity_PID(int Encoder,int Target){
	static float velocity_KP = 7.7, velocity_KI = 1.1; 
	static int Last_bias;
	int Bias, pwm = 0;
	Bias = Encoder - Target;  //计算偏差
	pwm = velocity_KP * Bias  + velocity_KI * (Bias - Last_bias); //增量式PI控制器
	if(pwm > 7200) pwm = 7200;
	if(pwm < -7200) pwm = -7200;
	Last_bias = Bias; //保存上一次偏差 
	return pwm;
}

/**
 * @description: 
 * @param {u16} arr
 * @param {u16} psc
 * @return {*}
 */
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 = 0;
	if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET) 
	{
		userInfo.currentSpeed = Read_Encoder(2);
		if(userInfo.run) pwm = velocity_PID(userInfo.currentSpeed, userInfo.targetSpeed);
		userInfo.pwm_val = pwm;
		Set_Pwm(pwm);
		TIM_ClearITPendingBit(TIM3, TIM_IT_Update); 
	}
}