/* USER CODE BEGIN Header */
/****************************************************************************************
	* @author  尚知物理
	* @哔哩哔哩链接：https://shop598739109.taobao.com
	* @file           : Function.c
  * @brief          : Function 功能函数集合
  * @version V1.0
  * @date    06-26-2023
  * @LegalDeclaration ：本文档内容难免存在Bug，仅限于交流学习，禁止用于任何的商业用途
	* @Copyright   著作权尚知物理所有
  * @attention
  ******************************************************************************
  */
/* USER CODE END Header */

#include "stdio.h"
#include "function.h"//功能函数头文件

uint16_t ADC1_RESULT[]={0,0,0,0,0};								//ADC采样外设到内存的DMA数据保存寄存器

float Plast = 0;
int D = 800;
int Dlast = 700;
int detaD = 4;


SADC adc;
void SADC_Init(void)
{
	adc.Ui = 0.0;
	adc.Ii = 0.0;
	adc.Ubf = 0.0;
	adc.Uo = 0.0;
	adc.Io = 0.0;
}



/*******************************************************************************
 * @fn Init_lowPass_alpha
 * @brief 初始化低通滤波器滤波系数
 * @param filter 滤波器
 * @param ts 采用周期 单位s
 * @return fc 截至频率 单位hz
 ******************************************************************************/
low_pass_filter_t  low_pass_filter1={0};  //定义滤波器
low_pass_filter_t  low_pass_filter2={0};  //定义滤波器
low_pass_filter_t  low_pass_filter3={0};  //定义滤波器
low_pass_filter_t  low_pass_filter4={0};  //定义滤波器
low_pass_filter_t  low_pass_filter5={0};  //定义滤波器
low_pass_filter_t  low_pass_filter6={0};  //定义滤波器
low_pass_filter_t  low_pass_filter7={0};  //定义滤波器
void Init_lowPass_alpha(low_pass_filter_t* const filter,const float ts, const float fc)
{
  float b=2*M_PI*fc*ts;
  filter->ts=ts;
  filter->fc=fc;
  filter->lastYn=0;
  filter->alpha=b/(b+1);
}
/*******************************************************************************
 * @fn Low_pass_filter
 * @brief 低通滤波函数
 * @param data 采样数据
 * @return 滤波结果
 ******************************************************************************/
float Low_pass_filter(low_pass_filter_t* const filter, const float data)
{
  float tem=filter->lastYn+(filter->alpha*(data-filter->lastYn));
  filter->lastYn=tem;
  return tem;
}


void ADCSample(void)
{

	
  //从DMA缓冲器中获取数据 Q15,并对其进行线性矫正-采用线性矫正
	adc.Ubf   = (int32_t )ADC1_RESULT[2] * Ubf_K + Ubf_B;
	adc.Uo    = (int32_t )ADC1_RESULT[1] * Uo_K + Uo_B;
	adc.Io    = (int32_t )ADC1_RESULT[0] * Io_K + Io_B;
	adc.Ui   = (int32_t )ADC1_RESULT[3] * Ui_K + Ui_B;
	adc.Ii    = (int32_t )ADC1_RESULT[4] * Ii_K + Ii_B;
	
	if(adc.Ii <0)
	{
		adc.Ii = 0;
	}
	//低通滤波
	adc.Ubf = Low_pass_filter(&low_pass_filter1,adc.Ubf);//获取滤波数据
	adc.Uo = Low_pass_filter(&low_pass_filter2,adc.Uo);//获取滤波数据
	adc.Io = Low_pass_filter(&low_pass_filter3,adc.Io);//获取滤波数据
	adc.Ui = Low_pass_filter(&low_pass_filter4,adc.Ui);//获取滤波数据
	adc.Ii = Low_pass_filter(&low_pass_filter5,adc.Ii * 500.0) / 500.0;//获取滤波数据
	
//	//低通滤波
//	adc.Ubf = Low_pass_filter(&low_pass_filter1,adc.Ubf);//获取滤波数据
//	adc.Uo = Low_pass_filter(&low_pass_filter2,adc.Uo);//获取滤波数据
//	adc.Io = Low_pass_filter(&low_pass_filter3,adc.Io);//获取滤波数据
//	adc.Ui = Low_pass_filter(&low_pass_filter4,adc.Ui);//获取滤波数据
//	adc.Ii = Low_pass_filter(&low_pass_filter5,adc.Ii);//获取滤波数据
  
	
}


/******************************* PID 相关函数 *********************************/
PID pid_Ubf;   //创建一个PID结构体变量
PID pid_Uo;   //创建一个PID结构体变量


//用于初始化pid参数的函数
void PID_Init(PID *pid,float p,float i,float d,float maxI,float maxOut)
{
	pid->kp=p;
	pid->ki=i;
	pid->kd=d;
	pid->maxIntegral=maxI;
	pid->maxOutput=maxOut;
	
}


//进行一次pid计算
//参数为(pid结构体,目标值,反馈值)，计算结果放在pid结构体的output成员中
void PID_Calculate(PID *pid,float reference,float feedback)
{
	
 	//更新数据
    pid->lastError=pid->error;//将旧error存起来
    pid->error=reference-feedback;//计算新error
    //计算微分
    float dout=(pid->error-pid->lastError)*pid->kd;
    //计算比例
    float pout=pid->error*pid->kp;
    //计算积分
    pid->integral+=pid->error*pid->ki;
    //积分限幅
    if(pid->integral > pid->maxIntegral) pid->integral=pid->maxIntegral;
    else if(pid->integral < -pid->maxIntegral) pid->integral=-pid->maxIntegral;
    //计算输出
    pid->output=pout+dout+pid->integral;
    //输出限幅
    if(pid->output > pid->maxOutput) pid->output=pid->maxOutput;
    else if(pid->output < -pid->maxOutput) pid->output=-pid->maxOutput;
}

//期望值初始化
Hope_Velue hope_value;
void Hope_Velue_Init(float Ubf,float Uo)
{
	hope_value.Ubf = Ubf;
	hope_value.Uo  = Uo;
}

uint16_t softstart_num = 0;
//期望值软启动
void Soft_Start(uint16_t softnum,float Ubf,float Uo)
{

	if(OUT_flag == 1)
	{
		softstart_num ++;
		if(softstart_num >= softnum)
		{
			softstart_num = 0;
			
			hope_value.Ubf ++;
			if(hope_value.Ubf >= Ubf)
			{
				hope_value.Ubf = Ubf;
			}
			hope_value.Uo ++;
			if(hope_value.Uo >= Uo)
			{
				hope_value.Uo = Uo;
			}
		}
	}
	
	
}


float Rin = 0.0;
float Po = 0.0;
void PIDset(void)
{
//	TIM1->CCR1 = 1000;
//	TIM1->CCR3 = 500;
//	TIM1->CCR1 = 2000 - ( 2000 / pid_Ubf.output);
	
//	TIM1->CCR3 = OUT_flag * pid_Uo.output;
	
	Rin = adc.Ui/adc.Ii;
	Rin = Low_pass_filter(&low_pass_filter6,Rin*500)/500;
	Po = adc.Uo*adc.Io;
	Po  = Low_pass_filter(&low_pass_filter7,Po*500)/500;
	
	if(adc.Ui >= 16)
	{
		
		if(Po >= Plast)
		{
			if(D >= Dlast)
			{
				D = D + detaD;
			}
			if(D < Dlast)
			{
				D = D - detaD;
			}
		}
		else if(Po < Plast)
		{
			if(D >= Dlast)
			{
				D = D - detaD;
			}
			if(D < Dlast)
			{
				D = D + detaD;
			}
		}
		
	}
	
	if(D>=1030)
	{
		D = 1030;
	}

	Dlast = D;
	Plast = Po;
	
	TIM1->CCR3 = D;
	
	
	
}



/*************************** OLED 显示 ***********************************/
void OLEDShow(void)
{
	OLED_ShowNum(40,2,adc.Ui,4,16);
	OLED_ShowFNum(40,4,D,16);
	OLED_ShowFNum(40,6,Po,16);
	
//	OLED_ShowFNum(40,2,adc.Ii,16);
//	OLED_ShowFNum(40,4,adc.Ubf,16);
//	OLED_ShowFNum(40,6,adc.Uo,16);
	
//	OLED_ShowNum(40,2,ADC1_RESULT[0],4,16);
//	OLED_ShowNum(40,4,ADC1_RESULT[1],4,16);
//	OLED_ShowNum(40,6,ADC1_RESULT[2],4,16);

}



