653 lines
20 KiB
C
653 lines
20 KiB
C
#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(PRO1_ADDR,data_rd,3);//读取存储在EEPROM里的PID参数
|
||
pv_one.status = PROV_RUNNING;
|
||
pv_one.tag = 1; //标签，比例阀1
|
||
for(uint8_t i = 0;i < SLDW_PRES_OUT;i++) //前n个时刻的输出气压
|
||
{
|
||
pv_one.previous_pressure[i] = 0;
|
||
}
|
||
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.target_current = 0; //目标值的理论电流
|
||
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.slow_down_flg = 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; //DAC输出函数
|
||
pv_one.pvout(0); //输出0mA
|
||
|
||
/************************************************/
|
||
|
||
ee_readfloats(PRO2_ADDR,data_rd,3);
|
||
pv_two.status = PROV_RUNNING;
|
||
pv_two.tag = 2; //标签，比例阀2
|
||
for(uint8_t i = 0;i < SLDW_PRES_OUT;i++) //前n个时刻的输出气压
|
||
{
|
||
pv_two.previous_pressure[i] = 0;
|
||
}
|
||
pv_two.current_pressure = 0;
|
||
pv_two.current_percent = 0;
|
||
pv_two.target_pressure = 0;
|
||
pv_two.target_percent = 0;
|
||
pv_two.target_current = 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.slow_down_flg = 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; //振荡次数
|
||
}
|
||
|
||
float out_makeup = 0.05;
|
||
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 > pvx->input_min)?(target_p):(pvx->input_min);
|
||
|
||
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->target_current = target_p; //记录当前理论模拟输出
|
||
pvx->pidout_max = pvx->target_current*(1 + out_makeup); //pid输出调节上限
|
||
pvx->pidout_min = pvx->target_current*(1 - out_makeup); //pid输出调节下限
|
||
|
||
pvx->gas_direction = (pvx->target_current >= pvx->current_input)?(GAS_IN):(GAS_OUT); //判断即将进行充气还是排气
|
||
|
||
pvx->current_input = pvx->target_current; //理论值作为当前输出值
|
||
// pvx->cstep_max = pvx->current_input + (float)0.8; //逐步输出调节上限
|
||
// pvx->cstep_min = pvx->current_input - (float)0.8; //逐步输出调节下限
|
||
// pvx->cstep_wait = 0; //等待计数清零
|
||
switch(pvx->gas_direction) //根据气体方向对输出做补偿
|
||
{
|
||
case GAS_IN:
|
||
{
|
||
pvx->current_input = ( (pvx->current_input*(1 - out_makeup)) <= pvx->input_min )?(pvx->input_min):(pvx->current_input*(1 - out_makeup) ); //输出补偿
|
||
}
|
||
break;
|
||
|
||
case GAS_OUT:
|
||
{
|
||
pvx->current_input = ( (pvx->current_input*(1 - out_makeup)) <= pvx->input_min )?(pvx->input_min):(pvx->current_input*(1 - out_makeup)); //输出补偿
|
||
}
|
||
break;
|
||
|
||
default:
|
||
{
|
||
}
|
||
break;
|
||
}
|
||
|
||
pvx->slow_down_flg = 0; //减速标志清零
|
||
|
||
pvx->pvout(pvx->current_input); //dac输出
|
||
}
|
||
|
||
|
||
|
||
//绝对值计算
|
||
float abs_bias(float bias)
|
||
{
|
||
bias =( bias>=0 )?(bias):(-bias);
|
||
return bias;
|
||
}
|
||
|
||
|
||
//pid控制
|
||
void prov_calibrate_pid(propotion_valve *pvx)
|
||
{
|
||
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; //更新前一个时刻的偏差
|
||
|
||
if(pvx->slow_down_flg == 1) //是否已经减速过
|
||
{
|
||
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);
|
||
}else
|
||
{
|
||
switch(pvx->gas_direction) //根据气体方向对输出范围做补偿
|
||
{
|
||
case GAS_IN:
|
||
{
|
||
pvx->current_input = (pvx->current_input < (pvx->pidout_max*(1 - out_makeup) ) )?(pvx->current_input):(pvx->pidout_max*(1 - out_makeup) ); //pid输出限幅
|
||
pvx->current_input = (pvx->current_input > (pvx->pidout_min*(1 - out_makeup) ) )?(pvx->current_input):(pvx->pidout_min*(1 - out_makeup) );
|
||
}
|
||
break;
|
||
|
||
case GAS_OUT:
|
||
{
|
||
pvx->current_input = (pvx->current_input < (pvx->pidout_max*(1 - out_makeup) ) )?(pvx->current_input):(pvx->pidout_max*(1 - out_makeup) ); //pid输出限幅
|
||
pvx->current_input = (pvx->current_input > (pvx->pidout_min*(1 - out_makeup) ) )?(pvx->current_input):(pvx->pidout_min*(1 - out_makeup) );
|
||
}
|
||
break;
|
||
|
||
default:
|
||
{
|
||
}
|
||
break;
|
||
}
|
||
if( abs_bias(pvx->previous_pressure[0] - pvx->previous_pressure[SLDW_PRES_OUT - 1]) <= 10 )
|
||
{
|
||
pvx->slow_down_flg = 1;
|
||
}
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
}
|
||
|
||
|
||
|
||
//模拟量控制
|
||
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 ) //触发比例阀1自整定
|
||
{
|
||
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
|
||
|
||
for(uint8_t i = 1;i< SLDW_PRES_OUT;i++) //滑动窗口，记录历史纯输出气压
|
||
{
|
||
pv_one.previous_pressure[SLDW_PRES_OUT - i] = pv_one.previous_pressure[SLDW_PRES_OUT - i - 1];
|
||
pv_two.previous_pressure[SLDW_PRES_OUT - i] = pv_two.previous_pressure[SLDW_PRES_OUT - i - 1];
|
||
}
|
||
pv_one.previous_pressure[0] = pv_one.current_pressure;
|
||
pv_two.previous_pressure[0] = pv_two.current_pressure;
|
||
|
||
//比例阀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();
|
||
astep = ADJ_WAIT;
|
||
}
|
||
|
||
// 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();
|
||
astep = ADJ_WAIT;
|
||
}
|
||
}
|
||
break;
|
||
|
||
default:
|
||
{
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
//PID参数自整定
|
||
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( (tick_current - tick_previous) > TICK_LIMIT ) //是否超时，TICK_LIMIT*500ms
|
||
{
|
||
astep = ADJ_END; //强制打断
|
||
}
|
||
|
||
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(PRO1_ADDR,data_wr,3);
|
||
}
|
||
break;
|
||
|
||
case 2:
|
||
{
|
||
data_wr[0] = pvx->Kp;
|
||
data_wr[1] = pvx->Ti;
|
||
data_wr[2] = pvx->Td;
|
||
ee_writefloats(PRO2_ADDR,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, [1011 1111]
|
||
}
|
||
break;
|
||
|
||
case 2:
|
||
{
|
||
ao_blf2 = 0;
|
||
CoilState[3] &= 0xDF; //D5 = 0, [1101 1111]
|
||
}
|
||
break;
|
||
default:
|
||
{
|
||
}
|
||
break;
|
||
}
|
||
|
||
tick_previous = -1;
|
||
tick_current = -1;
|
||
prov_adj_init();
|
||
pvx->status = PROV_RUNNING; //恢复控制
|
||
|
||
astep++;
|
||
}
|
||
}
|
||
break;
|
||
|
||
case ADJ_WAIT:
|
||
{
|
||
|
||
}
|
||
break;
|
||
|
||
default:
|
||
{
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
|