#include "provalctrl.h"

propotion_valve pv_one;
propotion_valve pv_two;

float data_wr[3] = {0};	//[0,1,2] -> “Kp, Ti, Td”
float data_rd[3] = {0};
void prov_init(void)						//比例阀结构体参数初始化
{
	ee_readfloats(EEPROM_ReadAddress1,data_rd,3);
	pv_one.status = PROV_RUNNING;
	pv_one.tag = 1;								//标签，比例阀1
	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_previous = 0;			//前一个时刻的偏差
	pv_one.bias_area = 0.5;				//允许的误差范围，±a(%)
	
	pv_one.Kp = data_rd[0];	//比例系数
	pv_one.Ti = (data_rd[1] == 0)?(1000000):(data_rd[1]);	//积分时间
	pv_one.Ing = 0;								//积分输出
	pv_one.Ing_max = 50;					//积分上限
	pv_one.Ing_min = -50;					//积分下限
	pv_one.Ing_sum = 0;						//偏差求和
	pv_one.Td = data_rd[2];	//微分时间
	pv_one.Ts = 0.05;							//采样周期，50ms
	pv_one.Div = 0;								//微分输出
	pv_one.pidout = 0;
	pv_one.pidout_max = 0;
	pv_one.pidout_min = 0;
	
	pv_one.cstep_gasin = 0.001;		//逐步接近的电流步长，mA,充气
	pv_one.cstep_gasout = 0.002;	//逐步接近的电流步长，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);
	
/************************************************/

	ee_readfloats(EEPROM_ReadAddress1+12,data_rd,3);
	pv_two.status = PROV_RUNNING;
	pv_two.tag = 2;								//标签，比例阀2
	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_previous = 0;			//前一个时刻的偏差
	pv_two.bias_area = 0.5;
	
	pv_two.Kp = data_rd[0];
	pv_two.Ti = (data_rd[1] == 0)?(1000000):(data_rd[1]);	//积分时间
	pv_two.Ing = 0;
	pv_two.Ing_max = 50;
	pv_two.Ing_min = -50;
	pv_two.Ing_sum = 0;
	pv_two.Td = data_rd[2];
	pv_two.Ts = 0.05;							//采样周期，50ms
	pv_two.Div = 0;
	pv_two.pidout = 0;
	pv_two.pidout_max = 0;
	pv_two.pidout_min = 0;
	
	pv_two.cstep_gasin = 0.001;		//逐步接近的电流步长，mA,充气
	pv_two.cstep_gasout = 0.002;	//逐步接近的电流步长，mA,排气
	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);
}

prov_adjust adj_pv1;
prov_adjust adj_pv2;

void prov_adj_init(void)
{
	adj_pv1.adj_flag = 0;											//自整定标志，0：空闲，1：整定中
	for( uint8_t i = 0;i < OSCILL_TIMES;i++)	//继电整定，振荡幅值,振荡周期
	{
		adj_pv1.relay_a[i] = 0;
		adj_pv1.relay_tc[i] = 0;
	}
	adj_pv1.relay_d = 0.15;										//继电整定，回环幅值
	adj_pv1.air_source = 0;										//气源，单位Kpa
	adj_pv1.middle_current = 0;								//中间气压电流
	adj_pv1.oscil_times = 0;									//振荡次数
	
/*****************************************************/
	
	adj_pv2.adj_flag = 0;											//自整定标志，0：空闲，1：整定中
	for( uint8_t i = 0;i < OSCILL_TIMES;i++)	//继电整定，振荡幅值,振荡周期
	{
		adj_pv2.relay_a[i] = 0;
		adj_pv2.relay_tc[i] = 0;
	}
	adj_pv2.relay_d = 0.15;										//继电整定，回环幅值
	adj_pv2.air_source = 0;										//气源，单位Kpa
	adj_pv2.middle_current = 0;								//中间气压电流
	adj_pv2.oscil_times = 0;									//振荡次数
}

void prov_set(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)0.8;		//逐步输出调节上限
		pvx->cstep_min = pvx->current_input - (float)0.8;		//逐步输出调节下限
		pvx->cstep_wait = 0;																//等待计数清零
	
		pvx->pidout_max = pvx->current_input + (float)0.5;	//pid输出调节上限
		pvx->pidout_min = pvx->current_input - (float)0.5;	//pid输出调节下限
	
		pvx->pvout(pvx->current_input);																//dac输出
}

float abs_bias(float bias)	//绝对值计算
{
	bias =( bias>=0 )?(bias):(-bias);
	return bias;
}
void prov_calibrate_pid(propotion_valve *pvx)	//pid校准
{
	if( (abs_bias(pvx->bias) > pvx->bias_area ) && (abs_bias(pvx->bias) < BIAS_MAX) )	//误差进入目标±BIAS_MAX%以内后再进行控制
	{
		if( (pvx->Ing_sum >= pvx->Ing_min) && (pvx->Ing_sum <= pvx->Ing_max) )	//积分累加与限幅
		{
			pvx->Ing_sum += pvx->bias;																						//偏差求和
		}else
		{
			pvx->Ing_sum = (pvx->Ing_sum > 0)?(pvx->Ing_max):(pvx->Ing_min);
		}
		pvx->Ing = ( pvx->Ts / pvx->Ti ) * pvx->Ing_sum;												//积分项
		
		pvx->Div = ( pvx->Td / pvx->Ts ) * ( pvx->bias - pvx->bias_previous );	//微分项
		
		pvx->pidout = pvx->Kp*( pvx->bias + pvx->Ing + pvx->Div );							//pid输出
		pvx->current_input += (pvx->pidout/100) * (pvx->input_max - pvx->input_min);
		
		pvx->bias_previous = pvx->bias;																				//更新前一个时刻的偏差
		
		pvx->current_input = (pvx->current_input < pvx->pidout_max)?(pvx->current_input):(pvx->pidout_max);		//pid输出限幅
		pvx->current_input = (pvx->current_input > pvx->pidout_min)?(pvx->current_input):(pvx->pidout_min);
		
		pvx->current_input = (pvx->current_input < pvx->input_max)?(pvx->current_input):(pvx->input_max);			//dac输出限幅
		pvx->current_input = (pvx->current_input > pvx->input_min)?(pvx->current_input):(pvx->input_min);
		
		pvx->pvout(pvx->current_input);		//dac输出
	}else
	{
		pvx->Ing_sum = 0;
		pvx->Ing = 0;
	}

}

//4-20mA电流输出
float ao_dwq = 0;																					//AO输出电流值(定位器)
float ao_blf1 = 0;																				//AO输出电流值(比例阀)
float ao_blf2 = 0;																				//AO输出电流值(比例阀)
float atm_pressure = 0;																		//用于存放大气绝压，单位：0.1Kpa
adj_steps astep = ADJ_WAIT;																//自整定步骤

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_set(ao_blf1,&pv_one);														//控制比例阀1
	}
	
	if(ao_blf2 != (float)(HoldReg[2]) / 1000)								//保持寄存器值发生变化时dac输出
	{
		ao_blf2 = (float)(HoldReg[2]) / 1000;									// uA -> mA
		prov_set(ao_blf2,&pv_two);														//控制比例阀2
	}
	
	if( (CoilState[3]&(0x40)) == 0x40  )
	{
		if(astep == ADJ_WAIT)
		{
			pv_one.status = PROV_ADJUSTING;
		}
	}	
	
	if( (CoilState[3]&(0x20)) == 0x20  )
	{
		if(astep == ADJ_WAIT)
		{
			pv_two.status = PROV_ADJUSTING;
		}
	}	
	
	
	if(it_100ms_flag_pv == 1)																//每隔100ms更新一次数据
	{
		it_100ms_flag_pv = 0;
		
		atm_pressure = ( (InputReg[7] - 4000)/(float)16000.0 ) * 2000; 				//大气绝压更新,4~20mA->0~200Kpa
	
		//比例阀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;
	}
	
	if(it_50ms_flag_pv == 1)																//每隔50ms校准一次
	{
		it_50ms_flag_pv = 0;
		
		if( (CoilState[0]&(0x03)) == 0x03  )									//两个电磁阀都开启的情况下才进行控制,否则保持
	  {
			if(pv_two.status != PROV_ADJUSTING)									//对其中一个比例阀进行自整定时，不对另一个进行控制
			{
				prov_ctrl(&pv_one, &adj_pv1);
			}
			if(pv_one.status != PROV_ADJUSTING)									//对其中一个比例阀进行自整定时，不对另一个进行控制
			{
				prov_ctrl(&pv_two, &adj_pv2);
			}
		}

	}
	
}



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->cstep_wait > CSTEP_WAIT_MAX)	//目标更新X秒后，误差仍不符合条件时再进行微步调节
	{
		if( (pvx->bias > pvx->bias_area) && (pvx->bias < BIAS_MAX) ) 	//正偏差（目标-实际），输出偏小
		{
			pvx->current_input += ((pvx->bias < 1))?(pvx->cstep_gasin):(pvx->cstep_gasin*3);
			
			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 > -BIAS_MAX) )	//负偏差（目标-实际），输出偏大
		{
			pvx->current_input -= ((pvx->bias > -1))?(pvx->cstep_gasin):(pvx->cstep_gasout*3);
			
			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);
		}
	}
	

}


void prov_ctrl(propotion_valve *pvx, prov_adjust *adj_pvx)
{
		switch(pvx->status)
		{
			case PROV_RUNNING:
			{
				if(adj_pvx->adj_flag == 1)		//进入此处说明整定被打断
				{
					//整定结束
					prov_adj_init();
				}
				
//			prov_calibrate_step(&pvx);
				prov_calibrate_pid(pvx);
			}
			break;
			
			case PROV_ADJUSTING:
			{
				if(astep == ADJ_WAIT)
				{
					prov_adj_init();
					adj_pvx->adj_flag = 1;		//整定开始
					astep = ADJ_START;
				}
				
				prov_adj(pvx, adj_pvx);
			}
			break;
			
			case PROV_HOLDING:
			{
				if(adj_pvx->adj_flag == 1)		//进入此处说明整定被打断
				{
					//整定结束
					prov_adj_init();
				}
			}
			break;
			
			default:
			{
				return;
			}
		}
}

int tick_previous = -1;
int tick_current = -1;
uint8_t hys_flag = 0;		//0:充气, 1:排气
void prov_adj(propotion_valve *pvx, prov_adjust *adj_pvx)
{
	switch(astep)
	{
		case ADJ_START:
		{
			if(tick_previous == -1)
			{	
				tick_previous = tick500ms;
				prov_set(4,pvx);//排空气体
			}
			tick_current = tick500ms;
			
			if( (tick_current - tick_previous) > 20)		//等待20*500ms
			{
				//记录此时气源压力
				adj_pvx->air_source = (InputReg[18] - atm_pressure) / (float)10;
				
				tick_previous = -1;
				tick_current = -1;
				astep++;
			}
		}
		break;
		
		case ADJ_MOVE2MIDDLE:
		{
			if(tick_previous == -1)
			{	
				tick_previous = tick500ms;
				
				adj_pvx->middle_current = (adj_pvx->air_source/2/900)*(pvx->input_max - pvx->input_min) + pvx->input_min;					//输出气源50%压力
				adj_pvx->middle_current = (adj_pvx->middle_current < pvx->input_max)?(adj_pvx->middle_current):(pvx->input_max);	//dac输出限幅
				adj_pvx->middle_current = (adj_pvx->middle_current > pvx->input_min)?(adj_pvx->middle_current):(pvx->input_min);
				
				prov_set(adj_pvx->middle_current,pvx);	
			}
			tick_current = tick500ms;
			
			if( (tick_current - tick_previous) > 10)		//等待10*500ms
			{
				tick_previous = -1;
				tick_current = -1;
				astep++;
			}
		}
		break;
		
		case ADJ_OSCILLATE:
		{
			if(adj_pvx->oscil_times < OSCILL_TIMES)	//振荡次数是否达到目标
			{
				if(tick_previous == -1)
				{	
					tick_previous = tick500ms;
				}else
				{
					//记录第 oc_times 个周期的峰值
					adj_pvx->relay_a[adj_pvx->oscil_times] = (adj_pvx->relay_a[adj_pvx->oscil_times] < pvx->current_pressure)?(pvx->current_pressure):(adj_pvx->relay_a[adj_pvx->oscil_times]);
				}
				tick_current = tick500ms;
				if( ( pvx->current_pressure < (adj_pvx->air_source/2*(1 + adj_pvx->relay_d)) ) && (hys_flag == 0) )			//充气振荡
				{
					pvx->current_input = adj_pvx->middle_current*(1 + adj_pvx->relay_d);			//输出气源 50*(1+d)% 压力
					pvx->current_input = (pvx->current_input < pvx->input_max)?(pvx->current_input):(pvx->input_max);	//dac输出限幅
					pvx->current_input = (pvx->current_input > pvx->input_min)?(pvx->current_input):(pvx->input_min);
					
					prov_set(pvx->current_input,pvx);	
				}else if( pvx->current_pressure >= (adj_pvx->air_source/2*(1 + adj_pvx->relay_d)) && (hys_flag == 0))		//切换至排气
				{
					hys_flag = 1;												
				}
				
				if( ( pvx->current_pressure > (adj_pvx->air_source/2*(1 - adj_pvx->relay_d)) ) && (hys_flag == 1) )			//排气振荡
				{
					pvx->current_input = adj_pvx->middle_current*(1 - adj_pvx->relay_d);			//输出气源 50*(1-d)% 压力
					pvx->current_input = (pvx->current_input < pvx->input_max)?(pvx->current_input):(pvx->input_max);	//dac输出限幅
					pvx->current_input = (pvx->current_input > pvx->input_min)?(pvx->current_input):(pvx->input_min);
					
					prov_set(pvx->current_input,pvx);	
				}else if( pvx->current_pressure <= (adj_pvx->air_source/2*(1 - adj_pvx->relay_d)) && (hys_flag == 1) )	//切换至充气
				{
					hys_flag = 0;			
					adj_pvx->relay_tc[adj_pvx->oscil_times] = (tick_current - tick_previous) * 500;	//振荡周期，单位ms
					tick_previous = -1;
					tick_current = -1;
					adj_pvx->oscil_times++;																													//记录振荡次数
				}
			}else									//振荡次数达到目标
			{
				if(tick_previous == -1)
				{	
					tick_previous = tick500ms;
				}
				tick_current = tick500ms;
			
				if( (tick_current - tick_previous) > 6)		//等待6*500ms后排气
				{
					prov_set(4,pvx);
					
					tick_previous = -1;
					tick_current = -1;
					
					astep++;
				}
			}
			
		}
		break;
		
		case ADJ_CALCULATE:
		{
			for(uint8_t i = 0;i < OSCILL_TIMES; i++)
			{
				adj_pvx->relay_A += adj_pvx->relay_a[i];
				adj_pvx->relay_Tc += adj_pvx->relay_tc[i];
			}
			adj_pvx->relay_A /= OSCILL_TIMES * ( adj_pvx->air_source/2 * (1 + adj_pvx->relay_d) );
			adj_pvx->relay_Tc /= OSCILL_TIMES;
			
			/* PI  [0.45,0.8]*/
			/* PID  [0.6,0.5,0.12]*/
			pvx->Kp = (float)0.6 * ( 4 * (float)0.1 ) / ( (float)3.1415 * adj_pvx->relay_A ) *(float)0.5;//偏大
			pvx->Ti = (float)0.5 * adj_pvx->relay_Tc/1000 * 100;		//偏小
			pvx->Td = (float)0.12 * adj_pvx->relay_Tc/1000 * (float)0.5;	//偏大	
			
			
			switch(pvx->tag)
			{
				case 1:
				{
					data_wr[0] = pvx->Kp;
					data_wr[1] = pvx->Ti;
					data_wr[2] = pvx->Td;
					ee_writefloats(EEPROM_WriteAddress1,data_wr,3);
				}
				break;
				
				case 2:
				{
					data_wr[0] = pvx->Kp;
					data_wr[1] = pvx->Ti;
					data_wr[2] = pvx->Td;
					ee_writefloats(EEPROM_WriteAddress1 + 12,data_wr,3);
				}
				break;
				default:
				{
				}
				break;
			}
			
			astep++;
		}
		break;
		
		case ADJ_END:
		{
			if(tick_previous == -1)
			{	
				tick_previous = tick500ms;
			}
			tick_current = tick500ms;
			if( (tick_current - tick_previous) > 10)		//等待10*500ms后恢复控制
			{
				switch(pvx->tag)		//重新触发保持寄存器
				{
					case 1:
					{
						ao_blf1 = 0;
						CoilState[3] &= 0xBF;	//D6 = 0
					}
					break;
					
					case 2:
					{
						ao_blf2 = 0;
						CoilState[3] &= 0xDF;	//D5 = 0
					}
					break;
					default:
					{
					}
					break;
				}
				
				tick_previous = -1;
				tick_current = -1;
				prov_adj_init();
				pvx->status = PROV_RUNNING;	
			
				astep++;
			}
		}
		break;
		
		case ADJ_WAIT:
		{
			tick_previous = -1;
			tick_current = -1;
		}
		break;
		
		default:
		{
			return;
		}
	}
}