#include "provalctrl.h"
#include "modbus.h"
#include "dac7311.h"
#include "main.h"

propotion_valve pv_one;
propotion_valve pv_two;

//4-20mA电流输出
float ao_dwq = 0;		//AO输出电流值(定位器)
float ao_blf1 = 0;		//AO输出电流值(比例阀)
float ao_blf2 = 0;		//AO输出电流值(比例阀)

void prov_init(void)						//比例阀结构体参数初始化
{
	pv_one.current_pressure = 0;	//当前气压，Kpa
	pv_one.current_percent = 0;		//当前气压百分比（ 0~900Kpa -> 0~100% ）
	pv_one.target_pressure = 0;		//目标气压，Kpa
	pv_one.target_percent = 0;		//目标气压百分比（ 0~900Kpa -> 0~100% ）
	pv_one.current_input = 0;			//当前输入电流
	pv_one.input_min = 4;					//输入电流下限4mA
	pv_one.input_max = 20;				//输入电流上限20mA
	pv_one.bias = 0;							//偏差 = 目标气压百分比 - 当前气压百分比
	pv_one.bias_area = 0.5;				//允许的误差范围，±a(%)
	
//	pv_one.Kp = 0;								//pid控制
//	pv_one.Ki = 0;
//	pv_one.Ing = 0;
//	pv_one.Ing_max = 10;
//	pv_one.Ing_min = -10;
//	pv_one.Kd = 0;
//	pv_one.pidout = 0;
	
	pv_one.cstep = 0.005;					//逐步接近的电流步长，mA
	pv_one.cstep_max = 0;					//逐步接近的电流范围上限
	pv_one.cstep_min = 0;					//逐步接近的电流范围下限
	pv_one.cstep_wait = 0;
	
	pv_one.pvout = ao_blf1_set;
	pv_one.pvout(0);
/************************************************/
	pv_two.current_pressure = 0;
	pv_two.current_percent = 0;
	pv_two.target_pressure = 0;
	pv_two.target_percent = 0;
	pv_two.current_input = 0;
	pv_two.input_min = 4;
	pv_two.input_max = 20;
	pv_two.bias = 0;
	pv_two.bias_area = 0.5;
	
//	pv_two.Kp = 0;
//	pv_two.Ki = 0;
//	pv_two.Ing = 0;
//	pv_two.Ing_max = 10;
//	pv_two.Ing_min = -10;
//	pv_two.Kd = 0;
//	pv_two.pidout = 0;
	
	pv_two.cstep = 0.005;
	pv_two.cstep_max = 0;
	pv_two.cstep_min = 0;
	pv_two.cstep_wait = 0;
	
	pv_two.pvout = ao_blf2_set;
	pv_two.pvout(0);
}

void prov_ctrl(float target_p, propotion_valve *pvx)
{
		target_p = (target_p < pvx->input_max)?(target_p):(pvx->input_max);  	//dac输出限幅
		target_p = (target_p > 0)?(target_p):(0);

		pvx->target_percent = (target_p - pvx->input_min) / (pvx->input_max - pvx->input_min)*100;
		pvx->target_pressure = pvx->target_percent/100*900;										//Kpa, 比例阀 (4~20mA -> 0~0.9Mpa)
	
		pvx->current_input = target_p;										//记录当前理论模拟输出
		pvx->cstep_max = pvx->current_input*(float)1.25;	//输出调节上限
		pvx->cstep_min = pvx->current_input*(float)0.75;	//输出调节下限
		pvx->cstep_wait = 0;															//等待计数清零
	
		pvx->pvout(target_p);															//dac输出
}

//float ao_prov_one = 0;
//float ao_prov_two = 0;
//void prov_calibrate_pid(void)
//{
//	if( (pv_one.bias > pv_one.bias_area)||(pv_one.bias < -pv_one.bias_area) )
//	{
//		pv_one.Ing = pv_one.Ing + pv_one.bias;
//		if(pv_one.Ing > pv_one.Ing_max) pv_one.Ing = pv_one.Ing_max;
//		if(pv_one.Ing < pv_one.Ing_min) pv_one.Ing = pv_one.Ing_min;
//		
//		pv_one.pidout = (pv_one.Kp/10000)* pv_one.bias + (pv_one.Ki/100000)*pv_one.Ing;
//		if(pv_one.pidout > (float)0.3) pv_one.pidout = (float)0.3;          //pid out limit
//		if(pv_one.pidout < (float)(-0.3)) pv_one.pidout = (float)(-0.3);
//		
//		ao_prov_one = ao_prov_one + pv_one.pidout*(pv_one.input_max - pv_one.input_min);
//		
//		if(ao_prov_one > pv_one.input_max) ao_prov_one = pv_one.input_max; 	//dac limit
//		if(ao_prov_one < pv_one.input_min) ao_prov_one = pv_one.input_min;	//dac limit
//		ao_blf1_set(ao_prov_one);
//	}else
//	{
//		pv_one.Ing = 0;
//	}
//	
//	if( (pv_two.bias > pv_two.bias_area)||(pv_two.bias < -pv_two.bias_area) )
//	{
//		ao_prov_two = ao_prov_two + pv_two.pidout;
//	}else
//	{
//		pv_two.Ing = 0;
//	}
//}


float atm_pressure = 0;																		//用于存放大气绝压，单位：0.1Kpa
void analog_ctrl(void)
{
	if(ao_dwq != (float)(HoldReg[0]) / 1000)								//保持寄存器值发生变化时dac输出
	{
		ao_dwq  = (float)(HoldReg[0]) / 1000;									// uA -> mA
		if(ao_dwq  > 25)	ao_dwq  = 25;												//定位器控制
		ao_dwq_set(ao_dwq);
	}
	
	if(ao_blf1 != (float)(HoldReg[1]) / 1000)								//保持寄存器值发生变化时dac输出
	{
		ao_blf1 = (float)(HoldReg[1]) / 1000;									// uA -> mA
		prov_ctrl(ao_blf1,&pv_one);														//控制比例阀1
	}
	
	if(ao_blf2 != (float)(HoldReg[2]) / 1000)								//保持寄存器值发生变化时dac输出
	{
		ao_blf2 = (float)(HoldReg[2]) / 1000;									// uA -> mA
		prov_ctrl(ao_blf2,&pv_two);														//控制比例阀2
	}
	
	if(it_100ms_flag_pv == 1)																//每隔100ms对输出进行一次校准
	{
		it_100ms_flag_pv = 0;
		
		prov_calibrate_step(&pv_one);
		prov_calibrate_step(&pv_two);
	}
	
	if(it_50ms_flag_pv == 1)																//每隔50ms更新一次数据
	{
		it_50ms_flag_pv = 0;
		
		atm_pressure = 1012;																									//大气绝压更新
	
		//比例阀1数据更新：当前气压、当前气压百分比、百分比偏差、当前输入电流（单片机->比例阀）
		pv_one.current_pressure = (InputReg[16] - atm_pressure)/(float)10;		//Kpa，sensor1 A口绝压转表压
		pv_one.current_percent = pv_one.current_pressure/900*100;
		pv_one.bias = pv_one.target_percent - pv_one.current_percent;
		
		//比例阀2数据更新：当前气压、当前气压百分比、百分比偏差、当前输入电流（单片机->比例阀）
		pv_two.current_pressure = (InputReg[17] - atm_pressure)/(float)10;		//Kpa，sensor1 B口绝压转表压
		pv_two.current_percent = pv_two.current_pressure/900*100;
		pv_two.bias = pv_two.target_percent - pv_two.current_percent;
	}
	
}



void prov_calibrate_step(propotion_valve *pvx)
{
	pvx->cstep_wait = (pvx->cstep_wait > 254)?(pvx->cstep_wait):(pvx->cstep_wait + 1);	//每100ms加一次，上限255
	
	if( (pvx->bias > pvx->bias_area) && (pvx->bias < 5) && ( pvx->cstep_wait > 50) ) 	//目标更新5秒后，误差仍不符合条件时再进行微步调节
	{
		pvx->current_input += pvx->cstep;
		
		pvx->current_input = (pvx->current_input <= pvx->cstep_max)?(pvx->current_input):(pvx->cstep_max);			//dac输出限幅
		pvx->current_input = (pvx->current_input <= pvx->input_max)?(pvx->current_input):(pvx->input_max);
		
		pvx->pvout(pvx->current_input);
	}
	
	if( (pvx->bias < -pvx->bias_area) && (pvx->bias > -5) && ( pvx->cstep_wait > 50) )	//目标更新5秒后，误差仍不符合条件时再进行微步调节
	{
		pvx->current_input -= pvx->cstep;
		
		pvx->current_input = (pvx->current_input >= pvx->cstep_min)?(pvx->current_input):(pvx->cstep_min);			//dac输出限幅
		pvx->current_input = (pvx->current_input >= pvx->input_min)?(pvx->current_input):(pvx->input_min);
		
		pvx->pvout(pvx->current_input);
	}
	

}