2179 lines
82 KiB
C
2179 lines
82 KiB
C
#include <stdio.h>
|
||
#include <math.h>
|
||
#include "sys.h"
|
||
#include "dacs.h"
|
||
#include "board.h"
|
||
#include "./mode_pwmp_hd.h"
|
||
#include "delay.h"
|
||
#include "at_hc24.h"
|
||
#include "entity.h"
|
||
#include "eeprom_m95.h"
|
||
#include "pid.h"
|
||
#include "filter.h"
|
||
#include <stdlib.h>
|
||
#include <string.h>
|
||
#include "pdctrl.h"
|
||
#include "app.h"
|
||
#include "data_type_def.h"
|
||
|
||
#define ACTUAL_FILTER 0
|
||
|
||
mode_pwmp_hd_t *mode_pwmp_hd;
|
||
pwmp_hd_adjust_t *mode_pwmp_hd_adjust;
|
||
pid_autotune_hd_t *pid_autotune_hd;
|
||
pwmp_hd_test_t *mode_pwmp_hd_test;
|
||
execute_rsp_hd_t rsp; // 执行结果
|
||
|
||
static execute_rsp_hd_t *execute_dac_plan1(void); // 执行方案1
|
||
static execute_rsp_hd_t *execute_dac_plan2(void); // 执行方案2
|
||
static execute_rsp_hd_t *execute_dac_plan3(void); // 执行方案3
|
||
static execute_rsp_hd_t *execute_dac_plan4(void); // 执行方案4
|
||
// static uint8_t execute_run_count = 0; // 执行计数
|
||
|
||
static void _process_start(void)
|
||
{
|
||
pwmp_hd_process_state_set(PWMP_HD_PROCESS_CONTROL);
|
||
}
|
||
|
||
static void _process_stop(void)
|
||
{
|
||
pwmp_hd_process_state_set(PWMP_HD_PROCESS_STOP);
|
||
}
|
||
|
||
static void _adjust_start(void)
|
||
{
|
||
pwmp_hd_process_state_set(PWMP_HD_PROCESS_ADJUST);
|
||
}
|
||
|
||
static void _adjust_stop(void)
|
||
{
|
||
pwmp_hd_process_state_set(PWMP_HD_PROCESS_ADJUST_STOP);
|
||
}
|
||
|
||
static uint8_t _adjust_result(void)
|
||
{
|
||
if (mode_pwmp_hd->pwmp_save->storage.tuned_flag == TUNED_ONGOING)
|
||
{
|
||
udevice.tuned_flag = TUNED_ONGOING;
|
||
}
|
||
else if (mode_pwmp_hd->pwmp_save->storage.tuned_flag == TUNED_SUCCESS)
|
||
{
|
||
udevice.tuned_flag = TUNED_SUCCESS;
|
||
}
|
||
else
|
||
{
|
||
udevice.tuned_flag = TUNED_FAILED;
|
||
}
|
||
|
||
return udevice.tuned_flag;
|
||
}
|
||
|
||
static uint8_t _adjust_step_count(void)
|
||
{
|
||
return mode_pwmp_hd->lcd_adjust_state;
|
||
}
|
||
|
||
static uint8_t _adjust_step_current(void)
|
||
{
|
||
return mode_pwmp_hd_adjust->adjust_state;
|
||
}
|
||
|
||
static void _adjust_data(mode_adjust_data_t *data)
|
||
{
|
||
data->pos0 = mode_pwmp_hd->pwmp_save->storage.trip_0; ///< 位置0 ad
|
||
data->pos100 = mode_pwmp_hd->pwmp_save->storage.trip_100; ///< 位置100 ad
|
||
data->pot0 = mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin; ///< 位置0 输出
|
||
data->pot100 = mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin; ///< 位置100 输出
|
||
data->current0 = mode_pwmp_hd->pwmp_save->storage.current0; ///< 位置0 电流(毫安，放大100倍, 0.01ma=1)
|
||
data->current100 = mode_pwmp_hd->pwmp_save->storage.current100; ///< 位置100 电流(毫安,放大100倍，0.01ma=1)
|
||
data->open_time = mode_pwmp_hd->pwmp_save->storage.all_open_time; ///< 全开时间(秒)
|
||
data->close_time = mode_pwmp_hd->pwmp_save->storage.all_close_time; ///< 全关时间(秒)
|
||
data->kp = mode_pwmp_hd->pwmp_save->storage.kp; ///< pid 比例系数
|
||
data->ki = mode_pwmp_hd->pwmp_save->storage.ki; ///< pid 积分系数
|
||
data->kd = mode_pwmp_hd->pwmp_save->storage.kd; ///< pid 微分系数
|
||
}
|
||
|
||
static BOOL _control_idle(void)
|
||
{
|
||
return TRUE;
|
||
}
|
||
|
||
static BOOL _adjust_isrun(void)
|
||
{
|
||
return mode_pwmp_hd->process_state == PWMP_HD_PROCESS_ADJUST;
|
||
}
|
||
|
||
/*程序状态设置*/
|
||
void pwmp_hd_process_state_set(mode_pwmp_hd_process_state_e state)
|
||
{
|
||
if (mode_pwmp_hd->process_state == state && mode_pwmp_hd->process_state == PWMP_HD_PROCESS_ADJUST)
|
||
{
|
||
mode_pwmp_hd->process_state = PWMP_HD_PROCESS_STOP;
|
||
}
|
||
else
|
||
{
|
||
mode_pwmp_hd->process_state = state;
|
||
switch (state)
|
||
{
|
||
case PWMP_HD_PROCESS_ADJUST:
|
||
mode_pwmp_hd->adjust_state = PWMP_HD_ADJUST_IDEL;
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
/*行程更新*/
|
||
static void pwmp_control_update(filter_e type)
|
||
{
|
||
mode_pwmp_hd_control_t *p = &mode_pwmp_hd->control;
|
||
rt_data.actual_travel = get_actual_travel(type);
|
||
rt_data.actual_travel = actual_travel_deal(rt_data.actual_travel);
|
||
pid_actual = get_pid_travel(rt_data.actual_travel);
|
||
|
||
if (mode_pwmp_hd->process_state == PWMP_HD_PROCESS_ADJUST)
|
||
{
|
||
p->ctrl_target = mode_pwmp_hd_adjust->ctrl_target;
|
||
}
|
||
else if (mode_pwmp_hd->process_state == PWMP_HD_PROCESS_TEST)
|
||
{
|
||
p->ctrl_target = mode_pwmp_hd_test->ctrl_target;
|
||
}
|
||
else
|
||
{
|
||
p->ctrl_target = lpf_window_update(mode_pwmp_hd->filter.handle, pid_target);
|
||
}
|
||
#if ACTUAL_FILTER == 1
|
||
if (fabs(p->real_error) < 0.5)
|
||
{
|
||
p->ctrl_feedback = lpf_window_update(mode_pwmp_hd->filter.handle, pid_actual);
|
||
}
|
||
else
|
||
{
|
||
p->ctrl_feedback = pid_actual;
|
||
lpf_window_reset(mode_pwmp_hd->filter.handle);
|
||
}
|
||
#else
|
||
p->ctrl_feedback = pid_actual;
|
||
#endif
|
||
p->real_error = p->ctrl_target - p->ctrl_feedback;
|
||
p->current_adc = adc_raw[ADC_PSB_CHANNEL];
|
||
p->small_trip_current_adc = adc_result_average(ADCS_1, ADC_IPSB_CHANNEL);
|
||
/*测试模式赋值*/
|
||
mode_pwmp_hd_test->psb_adc = p->current_adc;
|
||
mode_pwmp_hd_test->small_trip_current_adc = p->small_trip_current_adc;
|
||
mode_pwmp_hd_test->ctrl_feedback = p->ctrl_feedback;
|
||
mode_pwmp_hd_test->real_error = p->real_error;
|
||
/*整定模式赋值*/
|
||
mode_pwmp_hd_adjust->ctrl_feedback = p->ctrl_feedback;
|
||
mode_pwmp_hd_adjust->real_error = p->real_error;
|
||
}
|
||
|
||
/*PID参数获取*/
|
||
static float32 get_pwmp_hd_control_kp(void)
|
||
{
|
||
if (mode_pwmp_hd->pwmp_save->storage.kp < 2)
|
||
{
|
||
// return mode_pwmp_hd->pwmp_save->storage.kp * 8;
|
||
return mode_pwmp_hd->pwmp_save->storage.kp * 15;
|
||
}
|
||
else
|
||
{
|
||
return mode_pwmp_hd->pwmp_save->storage.kp * 4;
|
||
}
|
||
}
|
||
|
||
static float32 get_pwmp_hd_control_ki(void)
|
||
{
|
||
return mode_pwmp_hd->pwmp_save->storage.ki < 0.001 ? mode_pwmp_hd->pwmp_save->storage.ki * 20 : 0.02;
|
||
}
|
||
|
||
static float32 get_pwmp_hd_control_kd(void)
|
||
{
|
||
// return mode_pwmp_hd->pwmp_save->storage.kd < 0.01 ? mode_pwmp_hd->pwmp_save->storage.kd : 0.001;
|
||
if (udevice.value_travel_type == TRAVEL_TYPE_STRAIGHT)
|
||
{
|
||
return mode_pwmp_hd->pwmp_save->storage.kd = 0;
|
||
}
|
||
else
|
||
{
|
||
|
||
if (mode_pwmp_hd->pwmp_save->storage.kd <= 20)
|
||
{
|
||
return mode_pwmp_hd->pwmp_save->storage.kd;
|
||
}
|
||
else
|
||
{
|
||
return mode_pwmp_hd->pwmp_save->storage.kd = 20;
|
||
}
|
||
}
|
||
}
|
||
|
||
/*将自己结构体变量中的参数保存到公共参数中*/
|
||
static void pwmp_public_params_update()
|
||
{
|
||
udevice.pos0_travel_vol = mode_pwmp_hd->pwmp_save->storage.trip_0;
|
||
udevice.pos100_travel_vol = mode_pwmp_hd->pwmp_save->storage.trip_100;
|
||
udevice.output_min = mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin;
|
||
udevice.output_max = mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin;
|
||
udevice.apid_kp = mode_pwmp_hd->pwmp_save->storage.kp;
|
||
udevice.apid_ki = mode_pwmp_hd->pwmp_save->storage.ki;
|
||
udevice.apid_kd = mode_pwmp_hd->pwmp_save->storage.kd;
|
||
udevice.tuned_flag = mode_pwmp_hd->pwmp_save->storage.prov_flag;
|
||
}
|
||
|
||
//////////////////////////////*控制BEGIN*//////////////////////////////////////////////////////
|
||
void mode_pwmp_hd_stop(mode_pwmp_hd_t *handle)
|
||
{
|
||
// 释放内存
|
||
osel_memset((uint8_t *)&handle->control, 0, sizeof(mode_pwmp_hd_control_t));
|
||
}
|
||
|
||
// TODO:需要对进气和排气过程进行判定
|
||
/*将10-20、20-30、30-40...等区间进行局部线性化，并将输入的目标变量进行映射处理*/
|
||
static float32_t convert_target_to_controlvalue()
|
||
{ // FIXME:将函数补足
|
||
uint16_t i = 0;
|
||
float32 tmp = 0;
|
||
static float32 control_value = 0; // 此变量要传回，故定义为static类型。
|
||
|
||
i = (uint16_t)(fabs(mode_pwmp_hd->control.ctrl_target) / 10);
|
||
|
||
/*此处判断为解决处于10、20、30等整十处的特殊情况*/
|
||
if (((uint16_t)fabs(mode_pwmp_hd->control.ctrl_target) % 10 == 0) && (fabs(mode_pwmp_hd->control.ctrl_target) >= 10))
|
||
{
|
||
if (mode_pwmp_hd->action == INFLATE)
|
||
{
|
||
control_value = mode_pwmp_hd->valvepos.valvepos_in[i - 1];
|
||
}
|
||
else if (mode_pwmp_hd->action == DEFLATE)
|
||
{
|
||
control_value = mode_pwmp_hd->valvepos.valvepos_de[i - 1];
|
||
}
|
||
}
|
||
else
|
||
{ /*处于区间内的情况*/
|
||
if (mode_pwmp_hd->action == INFLATE)
|
||
{
|
||
tmp = ((mode_pwmp_hd->control.ctrl_target - (i * 10)) / (10));
|
||
control_value = tmp * mode_pwmp_hd->valvepos.valvepos_in_k[i - 1] + mode_pwmp_hd->valvepos.valvepos_in_b[i - 1];
|
||
}
|
||
else if (mode_pwmp_hd->action == DEFLATE)
|
||
{
|
||
tmp = ((mode_pwmp_hd->control.ctrl_target - (i * 10)) / (10));
|
||
control_value = tmp * mode_pwmp_hd->valvepos.valvepos_de_k[i - 1] + mode_pwmp_hd->valvepos.valvepos_de_b[i - 1];
|
||
}
|
||
}
|
||
|
||
return control_value;
|
||
}
|
||
|
||
/*判断运动状态（充气/排气）*/
|
||
static void inflate_or_deflate()
|
||
{
|
||
if (udevice.value_action_type == ATO)
|
||
{
|
||
if (mode_pwmp_hd->control.real_error < 0)
|
||
{
|
||
mode_pwmp_hd->action = DEFLATE;
|
||
}
|
||
else if (mode_pwmp_hd->control.real_error > 0)
|
||
{
|
||
mode_pwmp_hd->action = INFLATE;
|
||
}
|
||
}
|
||
else if (udevice.value_action_type == ATC)
|
||
{
|
||
if (mode_pwmp_hd->control.real_error < 0)
|
||
{
|
||
mode_pwmp_hd->action = INFLATE;
|
||
}
|
||
else if (mode_pwmp_hd->control.real_error > 0)
|
||
{
|
||
mode_pwmp_hd->action = DEFLATE;
|
||
}
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 算法方案初始化
|
||
* @param {execute_plan_hd_e} plan
|
||
* @return {*}
|
||
* @note
|
||
*/
|
||
void execute_pid_init(execute_plan_hd_e plan)
|
||
{
|
||
switch (plan)
|
||
{
|
||
// HD：位置式PID算法
|
||
case EXECUTE_PLAN_1:
|
||
_pid.type = PID_TYPE_CUSTOM_HANGDIAN;
|
||
pid_constructor(&_pid);
|
||
_pid.pid_u.hd.set_ctrl_prm_position(&_pid.pid_u.hd, get_pwmp_hd_control_kp(), get_pwmp_hd_control_ki(), get_pwmp_hd_control_kd());
|
||
break;
|
||
case EXECUTE_PLAN_2:
|
||
|
||
break;
|
||
case EXECUTE_PLAN_3:
|
||
|
||
break;
|
||
case EXECUTE_PLAN_4:
|
||
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 算法方案执行
|
||
* @param {execute_plan_hd_e} plan
|
||
* @return {*}
|
||
* @note
|
||
*/
|
||
execute_rsp_hd_t *execute_dac(execute_plan_hd_e plan)
|
||
{
|
||
switch (plan)
|
||
{
|
||
case EXECUTE_PLAN_1:
|
||
return execute_dac_plan1(); // 位置式PID
|
||
case EXECUTE_PLAN_2:
|
||
return execute_dac_plan2();
|
||
case EXECUTE_PLAN_3:
|
||
return execute_dac_plan3();
|
||
case EXECUTE_PLAN_4:
|
||
return execute_dac_plan4();
|
||
default:
|
||
rsp.code = EXECUTE_HD_NONE;
|
||
return &rsp;
|
||
}
|
||
}
|
||
static execute_rsp_hd_t *execute_dac_plan1()
|
||
{
|
||
static float32 Control_diff;
|
||
// uint8_t flag_stable = 0;
|
||
Control_diff = mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin - mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin;
|
||
float32 out = 0.0;
|
||
|
||
/*判断此时的充排气状态*/
|
||
inflate_or_deflate();
|
||
/*获取目标值对应的控制值大小*/
|
||
mode_pwmp_hd->valvepos_stable = convert_target_to_controlvalue();
|
||
|
||
// 杭电：输出限幅的参数设置
|
||
_pid.pid_u.hd.set_out_prm_position(&_pid.pid_u.hd, 100, 0);
|
||
out = _pid.pid_u.hd.pid_position(&_pid.pid_u.hd, mode_pwmp_hd->control.real_error);
|
||
out = out / 100;
|
||
if (udevice.value_travel_type == TRAVEL_TYPE_STRAIGHT)
|
||
{
|
||
#if 1
|
||
/* 一阶段加权系数*/
|
||
static float32 index_min_first = 0.3;
|
||
static float32 index_max_first = 0.4;
|
||
/*二阶段加权系数*/
|
||
static float32 index_min_second = 0.2;
|
||
static float32 index_max_second = 0.3;
|
||
if (mode_pwmp_hd->process_state == PWMP_HD_PROCESS_ADJUST)
|
||
{
|
||
if (fabs(mode_pwmp_hd->control.real_error) >= 5.7)
|
||
{ // 对PID输出结果进行归一化处理
|
||
out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin + Control_diff * index_max_first) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_first); // 映射实际控制器的范围
|
||
out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_first;
|
||
}
|
||
else if (fabs(mode_pwmp_hd->control.real_error) < 5.7 && fabs(mode_pwmp_hd->control.real_error) >= 1)
|
||
{ // 对PID输出结果进行归一化处理
|
||
out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin + Control_diff * index_max_second) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_second); // 映射实际控制器的范围
|
||
out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_second;
|
||
}
|
||
else
|
||
{
|
||
out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin); // 映射实际控制器的范围
|
||
out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (fabs(mode_pwmp_hd->control.real_error) >= 5.7)
|
||
{ // 对PID输出结果进行归一化处理
|
||
out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin + Control_diff * index_max_first) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_first); // 映射实际控制器的范围
|
||
out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_first;
|
||
}
|
||
else if (fabs(mode_pwmp_hd->control.real_error) < 5.7 && fabs(mode_pwmp_hd->control.real_error) >= 1)
|
||
{ // 对PID输出结果进行归一化处理
|
||
out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin + Control_diff * index_max_second) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_second); // 映射实际控制器的范围
|
||
out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_second;
|
||
}
|
||
else if (fabs(mode_pwmp_hd->control.real_error) < 1 && fabs(mode_pwmp_hd->control.real_error) > 0.7)
|
||
{
|
||
out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin); // 映射实际控制器的范围
|
||
out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin;
|
||
}
|
||
else
|
||
{
|
||
out = mode_pwmp_hd->valvepos_stable;
|
||
}
|
||
}
|
||
#else
|
||
float32 index = fabs(1 - exp(-(pow(mode_pwmp_hd->control.real_error, 2) - 1)));
|
||
if (fabs(mode_pwmp_hd->control.real_error) >= 1)
|
||
{
|
||
out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin + Control_diff * index) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index); // 映射实际控制器的范围
|
||
out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index;
|
||
}
|
||
else if (fabs(mode_pwmp_hd->control.real_error) > 0.99 && fabs(mode_pwmp_hd->control.real_error) < 1)
|
||
{
|
||
out = mode_pwmp_hd->pwmp_save->storage.startup_value;
|
||
}
|
||
else if (fabs(mode_pwmp_hd->control.real_error) <= 0.99)
|
||
{
|
||
out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin); // 映射实际控制器的范围
|
||
out += (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin);
|
||
}
|
||
#endif
|
||
}
|
||
else if (udevice.value_travel_type == TRAVEL_TYPE_ANGULAR)
|
||
{
|
||
#if 0
|
||
/* 一阶段加权系数*/
|
||
static float32 index_min_first = 1.2;
|
||
static float32 index_max_first = 1.2;
|
||
/*二阶段加权系数*/
|
||
static float32 index_min_second = 0.8;
|
||
static float32 index_max_second = 0.8;
|
||
/*三阶段加权系数*/
|
||
static float32 index_min_third = 0;
|
||
static float32 index_max_third = 0;
|
||
if (fabs(mode_pwmp_hd->control.real_error) >= 5.7)
|
||
{ // 对PID输出结果进行归一化处理
|
||
out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin + Control_diff * index_max_first) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_first); // 映射实际控制器的范围
|
||
out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_first;
|
||
}
|
||
else if (fabs(mode_pwmp_hd->control.real_error) < 5.7 && fabs(mode_pwmp_hd->control.real_error) >= 1)
|
||
{ // 对PID输出结果进行归一化处理
|
||
out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin + Control_diff * index_max_second) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_second); // 映射实际控制器的范围
|
||
out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_second;
|
||
}
|
||
else
|
||
{
|
||
out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin - Control_diff * index_max_third) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin + Control_diff * index_min_third); // 映射实际控制器的范围
|
||
out += (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin + Control_diff * index_min_third);
|
||
}
|
||
#else
|
||
float32 index = fabs(1 - exp(-(pow(mode_pwmp_hd->control.real_error, 2) - 1)));
|
||
if (fabs(mode_pwmp_hd->control.real_error) >= 1)
|
||
{
|
||
out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin + Control_diff * index) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index); // 映射实际控制器的范围
|
||
out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index;
|
||
}
|
||
else if (fabs(mode_pwmp_hd->control.real_error) > 0.99 && fabs(mode_pwmp_hd->control.real_error) < 1)
|
||
{
|
||
out = mode_pwmp_hd->pwmp_save->storage.startup_value;
|
||
}
|
||
else if (fabs(mode_pwmp_hd->control.real_error) <= 0.99)
|
||
{
|
||
if (mode_pwmp_hd_adjust->adjust_state == PWMP_HD_ADJUST_STEP_BY_10_TO_100 || mode_pwmp_hd_adjust->adjust_state == PWMP_HD_ADJUST_STEP_BY_10_TO_0)
|
||
{
|
||
out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin); // 映射实际控制器的范围
|
||
out += (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin);
|
||
}
|
||
else
|
||
{
|
||
out = mode_pwmp_hd->valvepos_stable;
|
||
}
|
||
}
|
||
#endif
|
||
}
|
||
|
||
rsp.code = EXECUTE_HD_OUT;
|
||
rsp.dac = (uint32_t)out;
|
||
|
||
return &rsp;
|
||
}
|
||
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
/// <param name=""></param>
|
||
/// <returns></returns>
|
||
|
||
static execute_rsp_hd_t *execute_dac_plan2()
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
/// <returns></returns>
|
||
|
||
static execute_rsp_hd_t *execute_dac_plan3()
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
/// <returns></returns>
|
||
static execute_rsp_hd_t *execute_dac_plan4()
|
||
{
|
||
return 0;
|
||
}
|
||
//////////////////////////////*控制END*//////////////////////////////////////////////////////
|
||
|
||
//////////////////////////////*整定相关函数BEGIN*//////////////////////////////////////////////////////
|
||
/*PID自整定方法*/
|
||
static void pid_autotune_way_set(pid_autotune_way_e state)
|
||
{
|
||
pid_autotune_hd->autotune_way = state;
|
||
}
|
||
|
||
/*整定过程中判断阀门状态：移动、停止、停止但仍在等待*/
|
||
static valve_position_change_e pwmp_adjust_hd_valve_position_change(uint8_t *state, uint8_t next_state, uint8_t diff_adc_max)
|
||
{
|
||
uint16_t diff_adc = 0;
|
||
// uint16_t adc = get_actual_travel_adc();
|
||
uint16_t adc = get_actual_travel_adc();
|
||
pid_autotune_hd->data.adjust_tmp_actual = adc;
|
||
|
||
if (mode_pwmp_hd_adjust->psb_adc != adc)
|
||
{
|
||
diff_adc = ABS(mode_pwmp_hd_adjust->psb_adc - adc);
|
||
}
|
||
|
||
if (diff_adc > diff_adc_max)
|
||
{
|
||
// 位置还在改变，继续等待
|
||
mode_pwmp_hd_adjust->psb_adc = adc;
|
||
return POSITION_CHANGE;
|
||
}
|
||
else
|
||
{
|
||
// 位置不变
|
||
if (mode_pwmp_hd_adjust->wait_count == 0)
|
||
{
|
||
// 等待次数也到了，位置不再发生变化，可以切换下一个状态
|
||
*state = next_state;
|
||
return POSITION_NO_CHANGE_FOREVER;
|
||
}
|
||
else
|
||
{ // 当位置不变的时候才开始等待
|
||
mode_pwmp_hd_adjust->wait_count--;
|
||
return POSITION_NO_CHANGE;
|
||
}
|
||
}
|
||
}
|
||
|
||
/*获得切线的横坐标（时间）
|
||
输入：actual_in为纵坐标，表示阀位的行程
|
||
*/
|
||
static float32 get_time_tangent(float actual_in)
|
||
{
|
||
return (actual_in - pid_autotune_hd->data.b) / pid_autotune_hd->data.k;
|
||
}
|
||
|
||
//////////////////////////////*整定相关函数END*/////////////////////////////////////////////////////////
|
||
|
||
//////////////////////////////////*状态机模式BEGIN*/////////////////////////////////////////////////////
|
||
/*整定等待状态*/
|
||
static void pwmp_adjust_hd_idle(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
|
||
{
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
mode_pwmp_hd->auto_tune_state = PWMP_HD_ADJUST_RESULT_TUNING;
|
||
sys_millis_reset();
|
||
osel_memset((uint8_t *)&mode_pwmp_hd->pwmp_save->storage, 0, sizeof(mode_pwmp_hd_storage_data_t));
|
||
mode_pwmp_hd->pwmp_save->storage.tuned_flag = TUNED_ONGOING;
|
||
/*为指针开辟存储空间*/
|
||
if (mode_pwmp_hd_adjust == NULL)
|
||
{
|
||
mode_pwmp_hd_adjust = osel_mem_alloc(sizeof(pwmp_hd_adjust_t));
|
||
DBG_ASSERT(mode_pwmp_hd_adjust != NULL __DBG_LINE);
|
||
}
|
||
osel_memset((uint8_t *)mode_pwmp_hd_adjust, 0, sizeof(pwmp_hd_adjust_t));
|
||
|
||
if (pid_autotune_hd == NULL)
|
||
{
|
||
pid_autotune_hd = (pid_autotune_hd_t *)osel_mem_alloc(sizeof(pid_autotune_hd_t));
|
||
}
|
||
osel_memset((uint8_t *)pid_autotune_hd, 0, sizeof(pid_autotune_hd_t));
|
||
|
||
/*输出初始化*/
|
||
mode_pwmp_hd_adjust->arr_default = mode_pwmp_hd->arr_default;
|
||
mode_pwmp_hd_adjust->arr_current = 0;
|
||
|
||
mode_pwmp_hd_adjust->adjust_state = (mode_pwmp_hd_adjust_state_e)*state;
|
||
FSM_WAIT(mode_pwmp_hd_adjust->adjust_state); // 等放气完毕就可以切入到else里面了
|
||
*state = PWMP_HD_ADJUST_BLEEDING;
|
||
}
|
||
else
|
||
{
|
||
valve_position_change_e s = pwmp_adjust_hd_valve_position_change(state, next_state, DIFF_ADC);
|
||
|
||
switch (s)
|
||
{
|
||
case POSITION_NO_CHANGE_FOREVER:
|
||
// 位置不再改变，记录此时阀门位置AD值
|
||
mode_pwmp_hd_adjust->adc_record_1 = pid_autotune_hd->data.adjust_tmp_actual;
|
||
mode_pwmp_hd_adjust->adc_record_0 = pid_autotune_hd->data.adjust_tmp_actual;
|
||
// 记录0位置电流的大小
|
||
if (udevice.value_action_type == ATO)
|
||
{
|
||
mode_pwmp_hd_adjust->current_0 = ip2current();
|
||
}
|
||
else
|
||
{
|
||
mode_pwmp_hd_adjust->current_100 = ip2current();
|
||
}
|
||
break;
|
||
case POSITION_CHANGE:
|
||
*state = PWMP_HD_ADJUST_IDEL;
|
||
// 将等待时间设置为1s
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 10;
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
/*整定粗调0*/
|
||
static void pwmp_adjust_hd_rough_position0(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
|
||
{
|
||
mode_pwmp_hd_adjust->psb_adc = get_actual_travel_adc();
|
||
uint16_t current_adc = 0;
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
// 以10%增加，提高DAC输出，找到能推动阀门的最小值
|
||
mode_pwmp_hd_adjust->arr_current = 200;
|
||
mode_pwmp_hd_adjust->arr_last = mode_pwmp_hd_adjust->arr_current;
|
||
pdctrl_out(mode_pwmp_hd_adjust->arr_current);
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 20;
|
||
mode_pwmp_hd_adjust->adjust_state = (mode_pwmp_hd_adjust_state_e)*state;
|
||
FSM_WAIT(*state); // 设置等待状态
|
||
}
|
||
else
|
||
{
|
||
valve_position_change_e s = pwmp_adjust_hd_valve_position_change(state, next_state, DIFF_ADC);
|
||
|
||
current_adc = pid_autotune_hd->data.adjust_tmp_actual;
|
||
// 3.28修改：将DIFF_ADC_MAX由10改为6
|
||
if (ABS(current_adc - mode_pwmp_hd_adjust->adc_record_1) >= 10U) // 阀位发生改变
|
||
{
|
||
if (mode_pwmp_hd_adjust->arr_last > 4000)
|
||
{
|
||
// 整定失败：位置反馈错误
|
||
*state = PWMP_HD_ADJUST_FAIL;
|
||
}
|
||
else
|
||
{
|
||
/*得到一个粗略的，小于启动量的值*/
|
||
mode_pwmp_hd_adjust->arr_record_1 = mode_pwmp_hd_adjust->arr_last > 1000 ? mode_pwmp_hd_adjust->arr_last * 0.97 : mode_pwmp_hd_adjust->arr_last;
|
||
/*判断位置反馈磁条是正装还是反装*/
|
||
mode_pwmp_hd_adjust->adc_0_100_flag = mode_pwmp_hd_adjust->adc_record_0 < current_adc ? TRUE : FALSE;
|
||
mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_record_1;
|
||
}
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 50;
|
||
*state = next_state;
|
||
}
|
||
else // 阀位未发生变化：继续增大输出
|
||
{
|
||
switch (s)
|
||
{
|
||
case POSITION_NO_CHANGE_FOREVER: // 位置不再改变
|
||
{
|
||
|
||
if (ABS(current_adc - mode_pwmp_hd_adjust->adc_record_1) <= DIFF_ADC)
|
||
{
|
||
// 没有发生变化
|
||
mode_pwmp_hd_adjust->arr_last = mode_pwmp_hd_adjust->arr_current;
|
||
if (mode_pwmp_hd_adjust->arr_last < 500)
|
||
{
|
||
mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_current * 1.1;
|
||
}
|
||
else
|
||
{
|
||
mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_current * 1.05;
|
||
}
|
||
/*若输出信号为最大值时认定为整定错误*/
|
||
if (mode_pwmp_hd_adjust->arr_current > 4000)
|
||
{
|
||
// 整定失败：位置反馈错误
|
||
*state = PWMP_HD_ADJUST_FAIL;
|
||
return;
|
||
}
|
||
pdctrl_out(mode_pwmp_hd_adjust->arr_current);
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 30;
|
||
*state = PWMP_HD_ADJUST_ROUGH_POSITION0;
|
||
FSM_WAIT(*state); // 设置等待状态
|
||
}
|
||
else
|
||
{
|
||
DBG_ASSERT(FALSE __DBG_LINE);
|
||
}
|
||
break;
|
||
}
|
||
case POSITION_CHANGE:
|
||
DBG_ASSERT(FALSE __DBG_LINE);
|
||
break;
|
||
case POSITION_NO_CHANGE:
|
||
{
|
||
break;
|
||
}
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/*放气值（0信号）的整定*/
|
||
static void pwmp_adjust_hd_initiate_position0(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
|
||
{
|
||
BOOL flag = FALSE;
|
||
mode_pwmp_hd_adjust->psb_adc = get_actual_travel_adc();
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
if (mode_pwmp_hd_adjust->wait_count-- > 0)
|
||
{
|
||
// 等待放气完毕
|
||
return;
|
||
}
|
||
mode_pwmp_hd_adjust->flag_stable_begin = TRUE;
|
||
mode_pwmp_hd_adjust->flag_stable_end = FALSE;
|
||
mode_pwmp_hd_adjust->arr_last = mode_pwmp_hd_adjust->arr_current;
|
||
mode_pwmp_hd_adjust->last_adc = mode_pwmp_hd_adjust->psb_adc;
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 20;
|
||
FSM_WAIT(*state); // 设置等待状态
|
||
}
|
||
else
|
||
{
|
||
// if (mode_pwmp_hd_adjust->adc_0_100_flag)
|
||
// {
|
||
// if (mode_pwmp_hd_adjust->psb_adc <= mode_pwmp_hd_adjust->last_adc)
|
||
// {
|
||
// if (ABS(mode_pwmp_hd_adjust->psb_adc - mode_pwmp_hd_adjust->last_adc) > DIFF_ADC)
|
||
// {
|
||
// mode_pwmp_hd_adjust->arr_record_0 = mode_pwmp_hd_adjust->arr_last + DIFF_ADC;
|
||
// flag = TRUE;
|
||
// }
|
||
// }
|
||
// }
|
||
// else
|
||
// {
|
||
// if (mode_pwmp_hd_adjust->psb_adc >= mode_pwmp_hd_adjust->last_adc)
|
||
// {
|
||
// if (ABS(mode_pwmp_hd_adjust->psb_adc - mode_pwmp_hd_adjust->last_adc) > DIFF_ADC)
|
||
// {
|
||
// mode_pwmp_hd_adjust->arr_record_0 = mode_pwmp_hd_adjust->arr_last + DIFF_ADC;
|
||
// flag = TRUE;
|
||
// }
|
||
// }
|
||
// }
|
||
|
||
/*求出阀位稳定信号*/
|
||
// 阀位变化小于3且不为初始阀位附近时认定进入阀位稳定区域
|
||
if (abs(mode_pwmp_hd_adjust->psb_adc - mode_pwmp_hd_adjust->last_adc) < 2 && abs(mode_pwmp_hd_adjust->psb_adc - mode_pwmp_hd_adjust->adc_record_1) > 50)
|
||
{
|
||
if (mode_pwmp_hd_adjust->flag_stable_begin)
|
||
{
|
||
mode_pwmp_hd_adjust->arr_record_stable_begin = mode_pwmp_hd_adjust->arr_current;
|
||
mode_pwmp_hd_adjust->flag_stable_begin = FALSE;
|
||
mode_pwmp_hd_adjust->flag_stable_end = TRUE;
|
||
}
|
||
}
|
||
|
||
if (mode_pwmp_hd_adjust->adc_0_100_flag) // adc_0_100_flag true:ad值增大 false:ad值减小
|
||
{
|
||
if (mode_pwmp_hd_adjust->psb_adc - mode_pwmp_hd_adjust->last_adc < -DIFF_ADC && mode_pwmp_hd_adjust->flag_stable_end)
|
||
{
|
||
mode_pwmp_hd_adjust->flag_stable_end = FALSE;
|
||
// 阀位回落
|
||
mode_pwmp_hd_adjust->arr_record_stable_end = mode_pwmp_hd_adjust->arr_current;
|
||
mode_pwmp_hd_adjust->arr_record_stable = (mode_pwmp_hd_adjust->arr_record_stable_begin + mode_pwmp_hd_adjust->arr_record_stable_end) / 2;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (mode_pwmp_hd_adjust->psb_adc - mode_pwmp_hd_adjust->last_adc > DIFF_ADC && mode_pwmp_hd_adjust->flag_stable_end)
|
||
{
|
||
mode_pwmp_hd_adjust->flag_stable_end = FALSE;
|
||
// 阀位回落
|
||
mode_pwmp_hd_adjust->arr_record_stable_end = mode_pwmp_hd_adjust->arr_current;
|
||
mode_pwmp_hd_adjust->arr_record_stable = (mode_pwmp_hd_adjust->arr_record_stable_begin + mode_pwmp_hd_adjust->arr_record_stable_end) / 2;
|
||
}
|
||
}
|
||
|
||
// 判断阀位是否回落到底部位置
|
||
if (ABS(mode_pwmp_hd_adjust->psb_adc - mode_pwmp_hd_adjust->adc_record_1) < 6)
|
||
{
|
||
mode_pwmp_hd_adjust->arr_record_0 = mode_pwmp_hd_adjust->arr_current;
|
||
flag = TRUE;
|
||
}
|
||
|
||
if (flag == TRUE)
|
||
{
|
||
mode_pwmp_hd_adjust->arr_record_2 = 4095; // 给定最大值输出
|
||
*state = next_state;
|
||
pdctrl_out(0); // 这里要先放气5秒然后计算全开时间
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 50;
|
||
}
|
||
else
|
||
{
|
||
mode_pwmp_hd_adjust->wait_count--;
|
||
if (flag == FALSE && mode_pwmp_hd_adjust->wait_count == 0)
|
||
{
|
||
uint16_t last = mode_pwmp_hd_adjust->arr_last;
|
||
mode_pwmp_hd_adjust->arr_last = mode_pwmp_hd_adjust->arr_current;
|
||
mode_pwmp_hd_adjust->arr_current = last - 4;
|
||
|
||
pdctrl_out(mode_pwmp_hd_adjust->arr_current);
|
||
if (ABS(mode_pwmp_hd_adjust->psb_adc - mode_pwmp_hd_adjust->last_adc) > 6)
|
||
{
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 10;
|
||
}
|
||
else
|
||
{
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 30;
|
||
}
|
||
|
||
mode_pwmp_hd_adjust->last_adc = mode_pwmp_hd_adjust->psb_adc;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// /*启动值细调*/
|
||
// static void pwmp_adjust_hd_accurate_position0(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
|
||
// {
|
||
// uint16_t current_adc = 0;
|
||
// if (!FSM_IS_WAIT(*state))
|
||
// {
|
||
// mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_record_1;
|
||
// mode_pwmp_hd_adjust->arr_last = mode_pwmp_hd_adjust->arr_current;
|
||
// mode_pwmp_hd_adjust->adjust_state = (mode_pwmp_hd_adjust_state_e)*state;
|
||
// FSM_WAIT(mode_pwmp_hd_adjust->adjust_state);
|
||
// *state = PWMP_HD_ADJUST_BLEEDING;
|
||
// }
|
||
// else
|
||
// {
|
||
// valve_position_change_e s = pwmp_adjust_hd_valve_position_change(state, next_state, DIFF_ADC);
|
||
|
||
// current_adc = pid_autotune_hd->data.adjust_tmp_actual;
|
||
// /*执行器发生动作*/
|
||
// if (ABS(current_adc - mode_pwmp_hd_adjust->adc_record_1) >= DIFF_ADC)
|
||
// {
|
||
// mode_pwmp_hd_adjust->arr_record_1 = mode_pwmp_hd_adjust->arr_last;
|
||
// mode_pwmp_hd_adjust->arr_record_2 = 4095; // 给定最大值输出
|
||
// *state = next_state;
|
||
// pdctrl_out(0); // 这里要先放气5秒然后计算全开时间
|
||
// mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 50;
|
||
// }
|
||
// else
|
||
// {
|
||
// switch (s)
|
||
// {
|
||
// case POSITION_NO_CHANGE_FOREVER: // 位置不再改变
|
||
// {
|
||
// /*若执行器在0位置*/
|
||
// if (ABS(current_adc - mode_pwmp_hd_adjust->adc_record_1) <= DIFF_ADC)
|
||
// {
|
||
// // 阀位没有发生变化，以每6增大信号输出
|
||
// mode_pwmp_hd_adjust->arr_last = mode_pwmp_hd_adjust->arr_current;
|
||
// mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_current + 6;
|
||
// pdctrl_out(mode_pwmp_hd_adjust->arr_current);
|
||
// mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 30;
|
||
// *state = PWMP_HD_ADJUST_ACCURATE_POSITION0;
|
||
// FSM_WAIT(*state); // 设置等待状态
|
||
// }
|
||
// else
|
||
// {
|
||
// DBG_ASSERT(FALSE __DBG_LINE);
|
||
// }
|
||
// break;
|
||
// }
|
||
// case POSITION_CHANGE:
|
||
// DBG_ASSERT(FALSE __DBG_LINE);
|
||
// break;
|
||
// default:
|
||
// break;
|
||
// }
|
||
// }
|
||
// }
|
||
// }
|
||
|
||
/*粗略整定得到满位置adc位置反馈*/
|
||
static void pwmp_adjust_hd_rough_position100(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
|
||
{
|
||
uint16_t current_adc = 0;
|
||
// 找最大推动值arr
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
/*上一步赋值的count*/
|
||
if (mode_pwmp_hd_adjust->wait_count-- > 0)
|
||
{
|
||
// 等待放气完毕
|
||
return;
|
||
}
|
||
sys_millis_reset();
|
||
if (udevice.value_action_type == ATO) // 若调节阀为气开阀
|
||
{
|
||
mode_pwmp_hd_adjust->all_open_time_full = sys_millis(); // 记录满信号全开起始时间
|
||
}
|
||
else
|
||
{
|
||
mode_pwmp_hd_adjust->all_close_time_full = sys_millis(); // 记录满信号全关起始时间
|
||
}
|
||
mode_pwmp_hd_adjust->tmp_time = 0;
|
||
|
||
pdctrl_out(mode_pwmp_hd_adjust->arr_record_2); // 以最大值进行输出
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 50; // 等待时间设置为5s
|
||
mode_pwmp_hd_adjust->adjust_state = (mode_pwmp_hd_adjust_state_e)*state;
|
||
FSM_WAIT(*state); // 设置等待状态
|
||
}
|
||
else
|
||
{
|
||
valve_position_change_e s = pwmp_adjust_hd_valve_position_change(state, next_state, DIFF_ADC);
|
||
current_adc = pid_autotune_hd->data.adjust_tmp_actual;
|
||
|
||
switch (s)
|
||
{
|
||
case POSITION_NO_CHANGE_FOREVER:
|
||
// 位置不再改变，记录此时阀门位置AD值
|
||
mode_pwmp_hd_adjust->adc_record_2 = current_adc;
|
||
if (udevice.value_action_type == ATO) // 若调节阀为气开阀
|
||
{
|
||
mode_pwmp_hd_adjust->tmp_time = sys_millis();
|
||
mode_pwmp_hd_adjust->all_open_time_full = mode_pwmp_hd_adjust->tmp_time - mode_pwmp_hd_adjust->all_open_time_full;
|
||
}
|
||
else // 若调节阀为气关阀
|
||
{
|
||
mode_pwmp_hd_adjust->tmp_time = sys_millis();
|
||
mode_pwmp_hd_adjust->all_close_time_full = mode_pwmp_hd_adjust->tmp_time - mode_pwmp_hd_adjust->all_close_time_full;
|
||
}
|
||
break;
|
||
case POSITION_CHANGE:
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 50;
|
||
mode_pwmp_hd_adjust->tmp_time = 0;
|
||
break;
|
||
case POSITION_NO_CHANGE:
|
||
if (mode_pwmp_hd_adjust->tmp_time == 0)
|
||
{
|
||
mode_pwmp_hd_adjust->tmp_time = sys_millis();
|
||
}
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
/*排气值整定*/
|
||
static void pwmp_adjust_hd_bleeding_position0(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
|
||
{
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
mode_pwmp_hd_adjust->adjust_state = (mode_pwmp_hd_adjust_state_e)*state;
|
||
FSM_WAIT(*state); // 设置等待状态
|
||
}
|
||
else
|
||
{
|
||
valve_position_change_e s = pwmp_adjust_hd_valve_position_change(state, next_state, DIFF_ADC);
|
||
mode_pwmp_hd_adjust->psb_adc = pid_autotune_hd->data.adjust_tmp_actual;
|
||
|
||
switch (s)
|
||
{
|
||
case POSITION_NO_CHANGE_FOREVER: // 位置不再改变
|
||
{
|
||
|
||
break;
|
||
}
|
||
case POSITION_CHANGE:
|
||
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
/*精调冲顶值*/
|
||
static void pwmp_adjust_hd_accurate_position100(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
|
||
{
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
mode_pwmp_hd_adjust->all_bleed_time_flag = true;
|
||
mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_record_1; // 初始值为前一步整定出的最小启动值
|
||
mode_pwmp_hd_adjust->arr_last = mode_pwmp_hd_adjust->arr_current;
|
||
mode_pwmp_hd_adjust->adjust_state = (mode_pwmp_hd_adjust_state_e)*state;
|
||
mode_pwmp_hd_adjust->preheat = TRUE;
|
||
FSM_WAIT(mode_pwmp_hd_adjust->adjust_state); // 等放气完毕就可以切入到else里面了
|
||
*state = PWMP_HD_ADJUST_BLEEDING;
|
||
}
|
||
else
|
||
{
|
||
valve_position_change_e s = pwmp_adjust_hd_valve_position_change(state, next_state, DIFF_ADC);
|
||
mode_pwmp_hd_adjust->psb_adc = pid_autotune_hd->data.adjust_tmp_actual;
|
||
|
||
/*当前反馈AD值与最大位置AD值的差值*/
|
||
uint16_t adc_diff = ABS(mode_pwmp_hd_adjust->psb_adc - mode_pwmp_hd_adjust->adc_record_2);
|
||
|
||
if (mode_pwmp_hd_adjust->arr_current == 0 && mode_pwmp_hd_adjust->preheat == TRUE)
|
||
{
|
||
// 放气完毕会进入到这里
|
||
mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_record_1 + 20;
|
||
pdctrl_out(mode_pwmp_hd_adjust->arr_current);
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 30;
|
||
return;
|
||
}
|
||
|
||
switch (s)
|
||
{
|
||
case POSITION_NO_CHANGE_FOREVER: // 位置不再改变
|
||
{
|
||
if (adc_diff <= 10)
|
||
{
|
||
*state = next_state;
|
||
// 记录在100阀位的adc值
|
||
mode_pwmp_hd_adjust->arr_record_2 = mode_pwmp_hd_adjust->arr_current;
|
||
// 记录在100阀位时的电流大小
|
||
if (udevice.value_action_type == ATO)
|
||
{
|
||
mode_pwmp_hd_adjust->current_100 = ip2current();
|
||
}
|
||
else
|
||
{
|
||
mode_pwmp_hd_adjust->current_0 = ip2current();
|
||
}
|
||
}
|
||
else
|
||
{
|
||
uint8_t offset = 0;
|
||
|
||
/*每次增大的信号值*/
|
||
offset = 3;
|
||
|
||
/*若快到顶时等待10s，否则等待1s*/
|
||
if (adc_diff < 20)
|
||
{
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 100;
|
||
}
|
||
else
|
||
{
|
||
/*40mm————30；16mm————40*/
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 1; // 0.1秒钟加一次
|
||
}
|
||
mode_pwmp_hd_adjust->last_adc = mode_pwmp_hd_adjust->psb_adc;
|
||
mode_pwmp_hd_adjust->arr_last = mode_pwmp_hd_adjust->arr_current;
|
||
mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_current + offset;
|
||
|
||
pdctrl_out(mode_pwmp_hd_adjust->arr_current);
|
||
*state = PWMP_HD_ADJUST_ACCURATE_POSITION100;
|
||
FSM_WAIT(*state); // 设置等待状态
|
||
}
|
||
|
||
break;
|
||
}
|
||
case POSITION_CHANGE:
|
||
if (adc_diff < 20)
|
||
{
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 100;
|
||
}
|
||
else
|
||
{
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 20;
|
||
}
|
||
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
/*计算控制的全关时间：以ATO类型角度命名*/
|
||
static void pwmp_adjust_hd_all_close_time(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
|
||
{
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_record_0 - 100; // 此处的输入信号大小根据实际控制的最小值来设定
|
||
|
||
/*记录起始的全开时间*/
|
||
sys_millis_reset();
|
||
if (udevice.value_action_type == ATO)
|
||
{
|
||
mode_pwmp_hd_adjust->all_close_time = sys_millis();
|
||
}
|
||
else
|
||
{
|
||
mode_pwmp_hd_adjust->all_open_time = sys_millis();
|
||
}
|
||
mode_pwmp_hd_adjust->tmp_time = 0;
|
||
|
||
pdctrl_out(mode_pwmp_hd_adjust->arr_current);
|
||
FSM_WAIT(*state); // 设置等待状态
|
||
}
|
||
else
|
||
{
|
||
mode_pwmp_hd_adjust->psb_adc = get_actual_travel_adc();
|
||
if (abs(mode_pwmp_hd_adjust->psb_adc - mode_pwmp_hd_adjust->adc_record_0) < DIFF_ADC)
|
||
{
|
||
if (udevice.value_action_type == ATO) // 若调节阀为气开阀
|
||
{
|
||
mode_pwmp_hd_adjust->tmp_time = sys_millis();
|
||
mode_pwmp_hd_adjust->all_close_time = mode_pwmp_hd_adjust->tmp_time - mode_pwmp_hd_adjust->all_close_time;
|
||
}
|
||
else // 若调节阀为气关阀
|
||
{
|
||
mode_pwmp_hd_adjust->tmp_time = sys_millis();
|
||
mode_pwmp_hd_adjust->all_open_time = mode_pwmp_hd_adjust->tmp_time - mode_pwmp_hd_adjust->all_close_time;
|
||
}
|
||
*state = next_state;
|
||
}
|
||
}
|
||
}
|
||
|
||
/*计算控制的全开时间*/
|
||
static void pwmp_adjust_hd_all_open_time(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
|
||
{
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_record_2 + 100; // 此处的输入信号大小根据实际控制的最大值来设定
|
||
|
||
/*记录起始的全开时间*/
|
||
sys_millis_reset();
|
||
if (udevice.value_action_type == ATO)
|
||
{
|
||
mode_pwmp_hd_adjust->all_open_time = sys_millis();
|
||
}
|
||
else
|
||
{
|
||
mode_pwmp_hd_adjust->all_close_time = sys_millis();
|
||
}
|
||
mode_pwmp_hd_adjust->tmp_time = 0;
|
||
|
||
pdctrl_out(mode_pwmp_hd_adjust->arr_current);
|
||
FSM_WAIT(*state); // 设置等待状态
|
||
}
|
||
else
|
||
{
|
||
mode_pwmp_hd_adjust->psb_adc = get_actual_travel_adc();
|
||
if (abs(mode_pwmp_hd_adjust->psb_adc - mode_pwmp_hd_adjust->adc_record_2) < DIFF_ADC)
|
||
{
|
||
if (udevice.value_action_type == ATO)
|
||
{
|
||
mode_pwmp_hd_adjust->tmp_time = sys_millis();
|
||
mode_pwmp_hd_adjust->all_open_time = mode_pwmp_hd_adjust->tmp_time - mode_pwmp_hd_adjust->all_open_time;
|
||
}
|
||
else
|
||
{
|
||
mode_pwmp_hd_adjust->tmp_time = sys_millis();
|
||
mode_pwmp_hd_adjust->all_close_time = mode_pwmp_hd_adjust->tmp_time - mode_pwmp_hd_adjust->all_close_time;
|
||
}
|
||
*state = next_state;
|
||
}
|
||
}
|
||
}
|
||
|
||
/*计算阀门自整定参数*/
|
||
static void pwmp_adjust_hd_calculate(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
|
||
{
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
/*行程上下限*/
|
||
mode_pwmp_hd->pwmp_save->storage.trip_0 = mode_pwmp_hd_adjust->adc_record_0;
|
||
mode_pwmp_hd->pwmp_save->storage.trip_100 = mode_pwmp_hd_adjust->adc_record_2;
|
||
mode_pwmp_hd->pwmp_save->storage.ad_diff = ABS(mode_pwmp_hd->pwmp_save->storage.trip_0 - mode_pwmp_hd->pwmp_save->storage.trip_100);
|
||
|
||
/*控制区间*/
|
||
mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin = mode_pwmp_hd_adjust->arr_record_2;
|
||
mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin = mode_pwmp_hd_adjust->arr_record_0;
|
||
mode_pwmp_hd->pwmp_save->storage.arr_diff = ABS(mode_pwmp_hd_adjust->arr_record_2 - mode_pwmp_hd_adjust->arr_record_0);
|
||
mode_pwmp_hd->pwmp_save->storage.pwmp_max = mode_pwmp_hd_adjust->arr_record_2 + (mode_pwmp_hd->pwmp_save->storage.arr_diff);
|
||
mode_pwmp_hd->pwmp_save->storage.pwmp_min = mode_pwmp_hd_adjust->arr_record_0 - (mode_pwmp_hd->pwmp_save->storage.arr_diff);
|
||
|
||
/*启动值*/
|
||
mode_pwmp_hd->pwmp_save->storage.startup_value = mode_pwmp_hd_adjust->arr_record_1;
|
||
|
||
/*0与100阀位位置*/
|
||
mode_pwmp_hd->pwmp_save->storage.current0 = mode_pwmp_hd_adjust->current_0;
|
||
mode_pwmp_hd->pwmp_save->storage.current100 = mode_pwmp_hd_adjust->current_100;
|
||
|
||
/*全开及全关时间*/
|
||
mode_pwmp_hd->pwmp_save->storage.all_open_time = mode_pwmp_hd_adjust->all_open_time;
|
||
mode_pwmp_hd->pwmp_save->storage.all_close_time = mode_pwmp_hd_adjust->all_close_time;
|
||
mode_pwmp_hd->pwmp_save->storage.all_open_time_full = mode_pwmp_hd_adjust->all_open_time_full;
|
||
mode_pwmp_hd->pwmp_save->storage.all_close_time_full = mode_pwmp_hd_adjust->all_close_time_full;
|
||
|
||
/*判断调节阀类型（气开/气关），对最低行程和最高行程进行标定*/
|
||
if (udevice.value_action_type == ATO)
|
||
{
|
||
mode_pwmp_hd->pwmp_save->storage.trip_100 = mode_pwmp_hd->pwmp_save->storage.trip_100;
|
||
mode_pwmp_hd->pwmp_save->storage.trip_0 = mode_pwmp_hd->pwmp_save->storage.trip_0;
|
||
}
|
||
else
|
||
{
|
||
uint16_t tmp = mode_pwmp_hd->pwmp_save->storage.trip_100;
|
||
mode_pwmp_hd->pwmp_save->storage.trip_100 = mode_pwmp_hd->pwmp_save->storage.trip_0;
|
||
mode_pwmp_hd->pwmp_save->storage.trip_0 = tmp;
|
||
}
|
||
|
||
// 保存安装方向
|
||
if (mode_pwmp_hd->pwmp_save->storage.trip_100 > mode_pwmp_hd->pwmp_save->storage.trip_0) // 上大下小，正装
|
||
{
|
||
udevice.dev_install_dir = 1;
|
||
}
|
||
else // 充气，ADC减小，正装
|
||
{
|
||
udevice.dev_install_dir = 0;
|
||
}
|
||
|
||
udevice.pos0_travel_vol = mode_pwmp_hd->pwmp_save->storage.trip_0;
|
||
udevice.pos100_travel_vol = mode_pwmp_hd->pwmp_save->storage.trip_100;
|
||
udevice.output_min = mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin;
|
||
udevice.output_max = mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin;
|
||
calib_parapos_perent();
|
||
|
||
mode_pwmp_hd_adjust->adjust_state = (mode_pwmp_hd_adjust_state_e)*state;
|
||
FSM_WAIT(mode_pwmp_hd_adjust->adjust_state); // 等放气完毕就可以切入到else里面了
|
||
*state = PWMP_HD_ADJUST_BLEEDING;
|
||
}
|
||
else
|
||
{
|
||
*state = next_state;
|
||
}
|
||
}
|
||
/*排气状态*/
|
||
static void pwmp_adjust_hd_bleeding(uint8_t *state)
|
||
{
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_default;
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 80;
|
||
if (mode_pwmp_hd_adjust->all_bleed_time_flag == true)
|
||
{
|
||
sys_millis_reset();
|
||
if (udevice.value_action_type == ATO)
|
||
{
|
||
mode_pwmp_hd_adjust->all_close_time_full = sys_millis();
|
||
}
|
||
else
|
||
{
|
||
mode_pwmp_hd_adjust->all_open_time_full = sys_millis();
|
||
}
|
||
mode_pwmp_hd_adjust->tmp_time = 0;
|
||
}
|
||
pdctrl_out(0);
|
||
FSM_WAIT(*state); // 设置等待状态
|
||
}
|
||
else
|
||
{
|
||
valve_position_change_e s = pwmp_adjust_hd_valve_position_change(state, mode_pwmp_hd_adjust->adjust_state, DIFF_ADC);
|
||
mode_pwmp_hd_adjust->psb_adc = mode_pwmp_hd_adjust->psb_adc;
|
||
switch (s)
|
||
{
|
||
case POSITION_NO_CHANGE_FOREVER:
|
||
if (mode_pwmp_hd_adjust->all_bleed_time_flag == true)
|
||
{
|
||
if (udevice.value_action_type == ATO) // 若调节阀为气开阀
|
||
{
|
||
mode_pwmp_hd_adjust->tmp_time = sys_millis();
|
||
mode_pwmp_hd_adjust->all_close_time_full = mode_pwmp_hd_adjust->tmp_time - mode_pwmp_hd_adjust->all_close_time_full;
|
||
}
|
||
else // 若调节阀为气关阀
|
||
{
|
||
mode_pwmp_hd_adjust->tmp_time = sys_millis();
|
||
mode_pwmp_hd_adjust->all_open_time_full = mode_pwmp_hd_adjust->tmp_time - mode_pwmp_hd_adjust->all_open_time_full;
|
||
}
|
||
mode_pwmp_hd_adjust->all_bleed_time_flag = false;
|
||
}
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 50; // 排气状态结束后在跳转后的状态中等待5s
|
||
mode_pwmp_hd_adjust->arr_current = 0;
|
||
break;
|
||
case POSITION_CHANGE:
|
||
if (mode_pwmp_hd_adjust->adc_record_0 == 0)
|
||
{
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 80;
|
||
}
|
||
else
|
||
{
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 50;
|
||
}
|
||
mode_pwmp_hd_adjust->tmp_time = 0;
|
||
break;
|
||
case POSITION_NO_CHANGE:
|
||
break;
|
||
default:
|
||
pwmp_hd_process_state_set(PWMP_HD_PROCESS_ADJUST_STOP);
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
/*停止模式*/
|
||
static void pwmp_adjust_hd_stop()
|
||
{
|
||
if (mode_pwmp_hd_adjust != NULL)
|
||
{
|
||
osel_mem_free(mode_pwmp_hd_adjust);
|
||
mode_pwmp_hd_adjust = NULL;
|
||
}
|
||
if (mode_pwmp_hd->auto_tune_state == PWMP_HD_ADJUST_RESULT_TUNING)
|
||
{
|
||
mode_pwmp_hd->auto_tune_state = PWMP_HD_ADJUST_RESULT_IDEL;
|
||
}
|
||
// 整定结束，将模式设置为控制模式
|
||
pwmp_hd_process_state_set(PWMP_HD_PROCESS_CONTROL);
|
||
}
|
||
|
||
/*整定错误状态*/
|
||
static void pwmp_adjust_hd_fail(uint8_t *state)
|
||
{
|
||
if (mode_pwmp_hd_adjust->arr_record_1 == 0)
|
||
{
|
||
// 阀位反馈错误
|
||
}
|
||
// 反馈结果（整定错误）
|
||
mode_pwmp_hd->auto_tune_state = PWMP_HD_ADJUST_RESULT_FAIL;
|
||
}
|
||
|
||
/*PID参数整定模式*/
|
||
// 通过整定获得K、T、L（tao）
|
||
static void pwmp_adjust_hd_PID_tuning(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
|
||
{
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
/*记录运行时间*/
|
||
sys_millis_reset();
|
||
pid_autotune_hd->data.pid_autotune_time_origin = sys_millis(); // 记录整定起始时间
|
||
mode_pwmp_hd_adjust->tmp_time = 0;
|
||
/*整定方法选择*/
|
||
pid_autotune_way_set(PID_AUTOTUNE_WAY_ZN);
|
||
/*阶跃信号设置*/
|
||
pid_autotune_hd->data.step_signal = mode_pwmp_hd_adjust->arr_record_2;
|
||
pdctrl_out(pid_autotune_hd->data.step_signal);
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 40; // 等待四秒
|
||
FSM_WAIT(*state); // 设置等待状态
|
||
}
|
||
else
|
||
{
|
||
valve_position_change_e s = pwmp_adjust_hd_valve_position_change(state, next_state, DIFF_ADC);
|
||
/*获取实时行程*/
|
||
pid_autotune_hd->data.cur_actual = pid_autotune_hd->data.adjust_tmp_actual;
|
||
/*计算变化率*/
|
||
pid_autotune_hd->data.actual_error = pid_autotune_hd->data.cur_actual - pid_autotune_hd->data.pre_actual;
|
||
if (pid_autotune_hd->data.actual_error > 1000)
|
||
{
|
||
pid_autotune_hd->data.slope = 0;
|
||
}
|
||
else
|
||
{
|
||
pid_autotune_hd->data.slope = pid_autotune_hd->data.actual_error / TIME_CYCLE;
|
||
}
|
||
/*记录下变化率最大点值及行程坐标、时间*/
|
||
if (pid_autotune_hd->data.slope > pid_autotune_hd->data.slope_Max)
|
||
{
|
||
pid_autotune_hd->data.slope_Max = pid_autotune_hd->data.slope;
|
||
pid_autotune_hd->data.actual_slope_Max = pid_autotune_hd->data.cur_actual;
|
||
mode_pwmp_hd_adjust->tmp_time = sys_millis();
|
||
pid_autotune_hd->data.time_slope_Max = mode_pwmp_hd_adjust->tmp_time - pid_autotune_hd->data.pid_autotune_time_origin;
|
||
}
|
||
/*保存前一次的行程*/
|
||
pid_autotune_hd->data.pre_actual = pid_autotune_hd->data.cur_actual;
|
||
/*只有当阀位大于前端*/
|
||
if (pid_autotune_hd->data.cur_actual > 2)
|
||
{
|
||
switch (s)
|
||
{
|
||
case POSITION_NO_CHANGE_FOREVER:
|
||
/*获取稳定时的行程*/
|
||
pid_autotune_hd->data.actual_stable = pid_autotune_hd->data.cur_actual;
|
||
/*计算切线的k和b*/
|
||
pid_autotune_hd->data.k = pid_autotune_hd->data.slope_Max;
|
||
pid_autotune_hd->data.b = pid_autotune_hd->data.actual_slope_Max - (pid_autotune_hd->data.k * pid_autotune_hd->data.time_slope_Max);
|
||
/*求得滞后时间L*/
|
||
pid_autotune_hd->l_hd = get_time_tangent(0);
|
||
/*求得时间系数T*/
|
||
pid_autotune_hd->t_hd = get_time_tangent(pid_autotune_hd->data.actual_stable) - pid_autotune_hd->l_hd;
|
||
/*K的计算*/
|
||
pid_autotune_hd->k_hd = ((pid_autotune_hd->data.actual_stable) / (mode_pwmp_hd_adjust->adc_record_2 - mode_pwmp_hd_adjust->adc_record_0)) / ((pid_autotune_hd->data.step_signal) / (mode_pwmp_hd_adjust->arr_record_2 - mode_pwmp_hd_adjust->arr_record_0));
|
||
|
||
*state = next_state;
|
||
break;
|
||
case POSITION_CHANGE:
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 40;
|
||
break;
|
||
case POSITION_NO_CHANGE:
|
||
break;
|
||
default:
|
||
pwmp_hd_process_state_set(PWMP_HD_PROCESS_ADJUST_STOP);
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/*参数计算模式*/
|
||
static void pwmp_adjust_hd_pid_calculate(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
|
||
{
|
||
switch (pid_autotune_hd->autotune_way)
|
||
{
|
||
case PID_AUTOTUNE_WAY_ZN:
|
||
if ((pid_autotune_hd->l_hd / pid_autotune_hd->t_hd) <= 0.2)
|
||
{
|
||
pid_autotune_hd->p_auto_hd = (pid_autotune_hd->t_hd / (0.85 * pid_autotune_hd->l_hd * pid_autotune_hd->k_hd));
|
||
pid_autotune_hd->ti_auto_hd = 2 * pid_autotune_hd->l_hd;
|
||
pid_autotune_hd->td_auto_hd = 0.5 * pid_autotune_hd->l_hd;
|
||
}
|
||
else if ((pid_autotune_hd->l_hd / pid_autotune_hd->t_hd) > 0.2)
|
||
{
|
||
pid_autotune_hd->p_auto_hd = ((1 / pid_autotune_hd->k_hd) * (pid_autotune_hd->l_hd / pid_autotune_hd->t_hd) + 0.6) / (2.6 * (pid_autotune_hd->l_hd / pid_autotune_hd->t_hd) - 0.08);
|
||
pid_autotune_hd->ti_auto_hd = 0.18 * pid_autotune_hd->t_hd + 0.19 * pid_autotune_hd->l_hd;
|
||
pid_autotune_hd->td_auto_hd = 0.25 * pid_autotune_hd->ti_auto_hd;
|
||
}
|
||
pid_autotune_hd->i_auto_hd = pid_autotune_hd->p_auto_hd / pid_autotune_hd->ti_auto_hd;
|
||
pid_autotune_hd->d_auto_hd = (pid_autotune_hd->td_auto_hd * pid_autotune_hd->p_auto_hd) / pid_autotune_hd->t_hd;
|
||
break;
|
||
case PID_AUTOTUNE_WAY_CC:
|
||
/*Kp、Ti、Td的求取*/
|
||
pid_autotune_hd->p_auto_hd = ((pid_autotune_hd->t_hd / (pid_autotune_hd->k_hd * pid_autotune_hd->l_hd)) * (4 / 3 + (pid_autotune_hd->l_hd / (pid_autotune_hd->t_hd * 4))));
|
||
pid_autotune_hd->ti_auto_hd = pid_autotune_hd->l_hd * ((32 + 6 * (pid_autotune_hd->l_hd / pid_autotune_hd->t_hd)) / (13 + 8 * (pid_autotune_hd->l_hd / pid_autotune_hd->l_hd)));
|
||
pid_autotune_hd->td_auto_hd = (4 * pid_autotune_hd->l_hd) / (11 + 2 * (pid_autotune_hd->l_hd / pid_autotune_hd->t_hd));
|
||
/*Ki、Kd的求取*/
|
||
pid_autotune_hd->i_auto_hd = pid_autotune_hd->p_auto_hd / pid_autotune_hd->ti_auto_hd;
|
||
pid_autotune_hd->d_auto_hd = (pid_autotune_hd->td_auto_hd * pid_autotune_hd->p_auto_hd) / pid_autotune_hd->t_hd;
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
mode_pwmp_hd->pwmp_save->storage.kp = fabs(pid_autotune_hd->p_auto_hd);
|
||
mode_pwmp_hd->pwmp_save->storage.ki = fabs(pid_autotune_hd->i_auto_hd);
|
||
mode_pwmp_hd->pwmp_save->storage.kd = fabs(pid_autotune_hd->d_auto_hd);
|
||
*state = next_state; // 将状态切换
|
||
}
|
||
|
||
/*参数保存模式*/
|
||
static void pwmp_adjust_hd_save(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
|
||
{
|
||
*state = next_state;
|
||
if (mode_pwmp_hd->pwmp_save->storage.tuned_flag == TUNED_ONGOING)
|
||
{
|
||
mode_pwmp_hd->pwmp_save->storage.tuned_flag = TUNED_SUCCESS;
|
||
mode_pwmp_hd->auto_tune_state = PWMP_HD_ADJUST_RESULT_SUCCESS;
|
||
pwmp_public_params_update();
|
||
}
|
||
else
|
||
{
|
||
mode_pwmp_hd->pwmp_save->storage.tuned_flag = TUNED_FAILED;
|
||
}
|
||
mode_pwmp_hd->params_save_cb();
|
||
}
|
||
|
||
// XXX:将0、10、20...90、100处的阀位反馈值记录下来，做小区间的线性化。
|
||
/*以每10%阀位为一步进，由0阀位增加到100阀位*/
|
||
static void pwmp_hd_adjust_step_by_10_to_100(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
|
||
{
|
||
execute_rsp_hd_t *execute_res;
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
|
||
mode_pwmp_hd_adjust->ctrl_target = 10;
|
||
mode_pwmp_hd_adjust->adjust_state = (mode_pwmp_hd_adjust_state_e)*state;
|
||
FSM_WAIT(mode_pwmp_hd_adjust->adjust_state); // 等放气完毕就可以切入到else里面了
|
||
*state = PWMP_HD_ADJUST_BLEEDING;
|
||
}
|
||
else
|
||
{
|
||
static unsigned int i = 0;
|
||
/*将模式调整为控制模式*/
|
||
pwmp_control_update(FILTER_MEDIAN);
|
||
if (rt_data.loop_current >= 4.0)
|
||
{
|
||
if (mode_pwmp_hd_adjust->ctrl_target == 0)
|
||
{
|
||
pdctrl_out(mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin * 0.8);
|
||
}
|
||
else if (mode_pwmp_hd_adjust->ctrl_target == 100)
|
||
{
|
||
pdctrl_out(mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin * 1.5);
|
||
}
|
||
else
|
||
{
|
||
execute_res = execute_dac(EXECUTE_PLAN); // PID计算结果经过执行器处理
|
||
if (execute_res->code == EXECUTE_HD_OUT) // 执行DAC输出
|
||
{
|
||
pdctrl_out(execute_res->dac);
|
||
}
|
||
}
|
||
}
|
||
/*当误差大于0.5%时重置等待时间*/
|
||
if (fabs(mode_pwmp_hd_adjust->real_error) > 1)
|
||
{
|
||
// 此处延时三秒钟
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 30;
|
||
}
|
||
else
|
||
{
|
||
mode_pwmp_hd_adjust->wait_count--;
|
||
if (mode_pwmp_hd_adjust->wait_count == 0)
|
||
{
|
||
if ((unsigned int)mode_pwmp_hd_adjust->ctrl_target < 100)
|
||
{
|
||
mode_pwmp_hd_adjust->valvepos.valvepos_in[i] = execute_res->dac;
|
||
i++;
|
||
}
|
||
else
|
||
{
|
||
*state = next_state;
|
||
}
|
||
mode_pwmp_hd_adjust->ctrl_target += 10;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/*排气值整定*/
|
||
static void pwmp_hd_adjust_step_by_10_to_0(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
|
||
{
|
||
execute_rsp_hd_t *execute_res;
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
mode_pwmp_hd_adjust->ctrl_target = 90;
|
||
mode_pwmp_hd_adjust->adjust_state = (mode_pwmp_hd_adjust_state_e)*state;
|
||
FSM_WAIT(*state); // 设置等待状态
|
||
}
|
||
else
|
||
{
|
||
static unsigned int i = 0;
|
||
/*将模式调整为控制模式*/
|
||
pwmp_control_update(FILTER_MEDIAN);
|
||
if (rt_data.loop_current >= 4.0)
|
||
{
|
||
if (mode_pwmp_hd_adjust->ctrl_target == 0)
|
||
{
|
||
pdctrl_out(mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin * 0.8);
|
||
}
|
||
else if (mode_pwmp_hd_adjust->ctrl_target == 100)
|
||
{
|
||
pdctrl_out(mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin * 1.5);
|
||
}
|
||
else
|
||
{
|
||
execute_res = execute_dac(EXECUTE_PLAN); // PID计算结果经过执行器处理
|
||
if (execute_res->code == EXECUTE_HD_OUT) // 执行DAC输出
|
||
{
|
||
pdctrl_out(execute_res->dac);
|
||
}
|
||
}
|
||
}
|
||
/*当误差大于0.5%时重置等待时间*/
|
||
if (fabs(mode_pwmp_hd_adjust->real_error) > 1)
|
||
{
|
||
// 此处延时三秒钟
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 30;
|
||
}
|
||
else
|
||
{
|
||
mode_pwmp_hd_adjust->wait_count--;
|
||
if (mode_pwmp_hd_adjust->wait_count == 0)
|
||
{
|
||
if ((unsigned int)mode_pwmp_hd_adjust->ctrl_target > 0)
|
||
{
|
||
mode_pwmp_hd_adjust->valvepos.valvepos_de[i] = execute_res->dac;
|
||
i++;
|
||
}
|
||
else
|
||
{
|
||
*state = next_state;
|
||
}
|
||
mode_pwmp_hd_adjust->ctrl_target -= 10;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/*将整定出的参数进行保存*/
|
||
static void pwmp_hd_adjust_valvepos_calculate(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
|
||
{
|
||
*state = next_state;
|
||
for (unsigned int i = 0; i < 9; i++)
|
||
{
|
||
mode_pwmp_hd->valvepos.valvepos_in[i] = mode_pwmp_hd_adjust->valvepos.valvepos_in[i];
|
||
mode_pwmp_hd->valvepos.valvepos_de[i] = mode_pwmp_hd_adjust->valvepos.valvepos_de[i];
|
||
}
|
||
for (unsigned int i = 0; i < 8; i++)
|
||
{
|
||
mode_pwmp_hd->valvepos.valvepos_in_k[i] = mode_pwmp_hd_adjust->valvepos.valvepos_in[i + 1] - mode_pwmp_hd_adjust->valvepos.valvepos_in[i];
|
||
mode_pwmp_hd->valvepos.valvepos_de_k[i] = mode_pwmp_hd_adjust->valvepos.valvepos_de[i + 1] - mode_pwmp_hd_adjust->valvepos.valvepos_de[i];
|
||
mode_pwmp_hd->valvepos.valvepos_in_b[i] = mode_pwmp_hd_adjust->valvepos.valvepos_in[i];
|
||
mode_pwmp_hd->valvepos.valvepos_de_b[i] = mode_pwmp_hd_adjust->valvepos.valvepos_de[i];
|
||
}
|
||
mode_pwmp_hd->params_save_cb();
|
||
}
|
||
|
||
//////////////////////////////////*状态机模式END*//////////////////////////////////////////////////////
|
||
|
||
//////////////////////////////*整定程序入口BEGIN*//////////////////////////////////////////////////////
|
||
|
||
void pwmp_adjust(uint8_t *state)
|
||
{
|
||
uint8_t ts = *state;
|
||
BIT_CLR(ts, BIT7); // 高位清零
|
||
switch (ts)
|
||
{
|
||
case PWMP_HD_ADJUST_ROUGH_POSITION0:
|
||
pwmp_adjust_hd_rough_position0(state, PWMP_HD_ADJUST_INITIATE);
|
||
break;
|
||
case PWMP_HD_ADJUST_INITIATE:
|
||
pwmp_adjust_hd_initiate_position0(state, PWMP_HD_ADJUST_ROUGH_POSITION100);
|
||
break;
|
||
case PWMP_HD_ADJUST_ROUGH_POSITION100:
|
||
pwmp_adjust_hd_rough_position100(state, PWMP_HD_ADJUST_ACCURATE_POSITION100);
|
||
break;
|
||
/*暂时先跳过PWMP_HD_ADJUST_BLEEDING_POSITION0*/
|
||
case PWMP_HD_ADJUST_BLEEDING_POSITION0:
|
||
pwmp_adjust_hd_bleeding_position0(state, PWMP_HD_ADJUST_ACCURATE_POSITION100);
|
||
break;
|
||
case PWMP_HD_ADJUST_ACCURATE_POSITION100:
|
||
pwmp_adjust_hd_accurate_position100(state, PWMP_HD_ADJUST_ALL_CLOSE_TIME);
|
||
break;
|
||
case PWMP_HD_ADJUST_ALL_CLOSE_TIME:
|
||
pwmp_adjust_hd_all_close_time(state, PWMP_HD_ADJUST_ALL_OPEN_TIME);
|
||
break;
|
||
case PWMP_HD_ADJUST_ALL_OPEN_TIME:
|
||
pwmp_adjust_hd_all_open_time(state, PWMP_HD_ADJUST_CALCULATE);
|
||
break;
|
||
case PWMP_HD_ADJUST_CALCULATE:
|
||
pwmp_adjust_hd_calculate(state, PWMP_HD_ADJUST_PID_TUNING);
|
||
break;
|
||
case PWMP_HD_ADJUST_PID_TUNING:
|
||
pwmp_adjust_hd_PID_tuning(state, PWMP_HD_ADJUST_PID_CALCULATE);
|
||
break;
|
||
case PWMP_HD_ADJUST_PID_CALCULATE:
|
||
pwmp_adjust_hd_pid_calculate(state, PWMP_HD_ADJUST_SAVE);
|
||
break;
|
||
case PWMP_HD_ADJUST_SAVE:
|
||
pwmp_adjust_hd_save(state, PWMP_HD_ADJUST_STEP_BY_10_TO_100);
|
||
break;
|
||
/*GPS2000中特有的整定流程，整定得到充排气过程中每百分之10稳定阀位的控制值，并将控制值每10%区间做线性处理*/
|
||
case PWMP_HD_ADJUST_STEP_BY_10_TO_100:
|
||
pwmp_hd_adjust_step_by_10_to_100(state, PWMP_HD_ADJUST_STEP_BY_10_TO_0);
|
||
break;
|
||
case PWMP_HD_ADJUST_STEP_BY_10_TO_0:
|
||
pwmp_hd_adjust_step_by_10_to_0(state, PWMP_HD_ADJUST_VALVEPOS_CALCULATE);
|
||
break;
|
||
case PWMP_HD_ADJUST_VALVEPOS_CALCULATE:
|
||
pwmp_hd_adjust_valvepos_calculate(state, PWMP_HD_ADJUST_STOP);
|
||
break;
|
||
case PWMP_HD_ADJUST_BLEEDING:
|
||
pwmp_adjust_hd_bleeding(state);
|
||
break;
|
||
case PWMP_HD_ADJUST_IDEL:
|
||
pwmp_adjust_hd_idle(state, PWMP_HD_ADJUST_ROUGH_POSITION0);
|
||
break;
|
||
case PWMP_HD_ADJUST_STOP:
|
||
pwmp_hd_process_state_set(PWMP_HD_PROCESS_ADJUST_STOP);
|
||
break;
|
||
case PWMP_HD_ADJUST_FAIL:
|
||
pwmp_adjust_hd_fail(state);
|
||
pwmp_hd_process_state_set(PWMP_HD_PROCESS_ADJUST_STOP);
|
||
break;
|
||
default:
|
||
pwmp_hd_process_state_set(PWMP_HD_PROCESS_ADJUST_STOP);
|
||
break;
|
||
}
|
||
|
||
switch (ts)
|
||
{
|
||
case PWMP_HD_ADJUST_IDEL:
|
||
mode_pwmp_hd->lcd_adjust_state = LCD_ADJUST_IDEL;
|
||
break;
|
||
case PWMP_HD_ADJUST_ROUGH_POSITION0:
|
||
mode_pwmp_hd->lcd_adjust_state = LCD_ADJUST_POSITION0;
|
||
break;
|
||
case PWMP_HD_ADJUST_ACCURATE_POSITION0:
|
||
mode_pwmp_hd->lcd_adjust_state = LCD_ADJUST_POSITION0;
|
||
break;
|
||
case PWMP_HD_ADJUST_ROUGH_POSITION100:
|
||
mode_pwmp_hd->lcd_adjust_state = LCD_ADJUST_POSITION100;
|
||
break;
|
||
case PWMP_HD_ADJUST_ACCURATE_POSITION100:
|
||
mode_pwmp_hd->lcd_adjust_state = LCD_ADJUST_POSITION100;
|
||
break;
|
||
case PWMP_HD_ADJUST_BLEEDING_TIMER:
|
||
mode_pwmp_hd->lcd_adjust_state = LCD_ADJUST_TIMER;
|
||
break;
|
||
case PWMP_HD_ADJUST_AERATE_TIMER:
|
||
mode_pwmp_hd->lcd_adjust_state = LCD_ADJUST_TIMER;
|
||
break;
|
||
case PWMP_HD_ADJUST_TUNING:
|
||
mode_pwmp_hd->lcd_adjust_state = LCD_ADJUST_TUNING;
|
||
break;
|
||
case PWMP_HD_ADJUST_SAVE:
|
||
mode_pwmp_hd->lcd_adjust_state = LCD_ADJUST_SAVE;
|
||
break;
|
||
case PWMP_HD_ADJUST_STOP:
|
||
mode_pwmp_hd->lcd_adjust_state = LCD_ADJUST_STOP;
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
//////////////////////////////*整定程序入口END*//////////////////////////////////////////////////////
|
||
|
||
//////////////////////////////*测试模式开始*///////////////////////////////////////////////////////////////////
|
||
/*整定过程中判断阀门状态：移动、停止、停止但仍在等待*/
|
||
/*
|
||
static valve_position_change_e pwmp_test_hd_valve_position_change(uint8_t *state, uint8_t next_state, uint8_t diff_adc_max)
|
||
{
|
||
uint16_t diff_adc = 0;
|
||
uint16_t adc = mode_pwmp_hd_test->psb_adc;
|
||
|
||
if (mode_pwmp_hd_test->psb_adc != adc)
|
||
{
|
||
diff_adc = ABS(mode_pwmp_hd_test->psb_adc - adc);
|
||
}
|
||
|
||
if (diff_adc > diff_adc_max)
|
||
{
|
||
// 位置还在改变，继续等待
|
||
mode_pwmp_hd_test->psb_adc = adc;
|
||
return POSITION_CHANGE;
|
||
}
|
||
else
|
||
{
|
||
// 位置不变
|
||
if (mode_pwmp_hd_test->wait_count == 0)
|
||
{
|
||
// 等待次数也到了，位置不再发生变化，可以切换下一个状态
|
||
*state = next_state;
|
||
return POSITION_NO_CHANGE_FOREVER;
|
||
}
|
||
else
|
||
{ // 当位置不变的时候才开始等待
|
||
mode_pwmp_hd_test->wait_count--;
|
||
return POSITION_NO_CHANGE;
|
||
}
|
||
}
|
||
}
|
||
*/
|
||
static void pwmp_test_hd_velocity_test(uint8_t *state, mode_pwmp_hd_test_state_e next_state)
|
||
{
|
||
pwmp_control_update(FILTER_AVERAGE);
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
mode_pwmp_hd_test->arr_current = udevice.output_max; // 以最大的信号值输出
|
||
pdctrl_out(mode_pwmp_hd_test->arr_current);
|
||
FSM_WAIT(*state); // 设置等待状态
|
||
}
|
||
else
|
||
{
|
||
|
||
if (abs(mode_pwmp_hd_test->psb_adc - udevice.pos0_travel_vol) < DIFF_ADC_MAX)
|
||
{
|
||
*state = next_state;
|
||
}
|
||
}
|
||
}
|
||
/*开环控制*/
|
||
static void pwmp_test_hd_open_loop_output()
|
||
{
|
||
pwmp_control_update(FILTER_AVERAGE);
|
||
pdctrl_out(mode_pwmp_hd->output);
|
||
mode_pwmp_hd->duty_percent = ((float32)mode_pwmp_hd->output / (float32)DAC_MAX) * 100;
|
||
mode_pwmp_hd->current_electric = ip2current();
|
||
}
|
||
|
||
/*采样得到小行程最大值*/
|
||
static void pwmp_test_hd_get_small_travel_high(uint8_t *state, mode_pwmp_hd_test_small_travel_state_e next_state)
|
||
{
|
||
pwmp_control_update(FILTER_AVERAGE);
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
mode_pwmp_hd_test->arr_current = 4095; // 以最大的信号值输出
|
||
pdctrl_out(mode_pwmp_hd_test->arr_current);
|
||
FSM_WAIT(*state); // 设置等待状态
|
||
}
|
||
else
|
||
{
|
||
if (mode_pwmp_hd_test->small_trip_current_adc > mode_pwmp_hd_test->small_trip_adc_max)
|
||
{
|
||
mode_pwmp_hd_test->small_trip_adc_max = mode_pwmp_hd_test->small_trip_current_adc;
|
||
}
|
||
if (abs(mode_pwmp_hd_test->psb_adc - udevice.pos0_travel_vol) < DIFF_ADC_MAX)
|
||
{
|
||
*state = next_state;
|
||
}
|
||
}
|
||
}
|
||
|
||
/*采样得到小行程最小值*/
|
||
static void pwmp_test_hd_get_small_travel_low(uint8_t *state, mode_pwmp_hd_test_small_travel_state_e next_state)
|
||
{
|
||
pwmp_control_update(FILTER_AVERAGE);
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
mode_pwmp_hd_test->small_trip_adc_min = 4095; // 因为初始化将其赋值为0，为防止出现bug故将其初始化为4095
|
||
mode_pwmp_hd_adjust->arr_current = 0; // 以最小的信号值输出
|
||
pdctrl_out(mode_pwmp_hd_adjust->arr_current);
|
||
FSM_WAIT(*state); // 设置等待状态
|
||
}
|
||
else
|
||
{
|
||
if (mode_pwmp_hd_test->small_trip_current_adc < mode_pwmp_hd_test->small_trip_adc_min)
|
||
{
|
||
mode_pwmp_hd_test->small_trip_adc_min = mode_pwmp_hd_test->small_trip_current_adc;
|
||
}
|
||
if (abs(mode_pwmp_hd_test->psb_adc - udevice.pos0_travel_vol) < DIFF_ADC_MAX)
|
||
{
|
||
*state = next_state;
|
||
}
|
||
}
|
||
}
|
||
|
||
/*采样得到处于50%阀位时稳定的控制信号*/
|
||
static void pwmp_test_hd_get_small_travel_50_per_arr(uint8_t *state, mode_pwmp_hd_test_small_travel_state_e next_state)
|
||
{
|
||
execute_rsp_hd_t *execute_res;
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
mode_pwmp_hd_test->ctrl_target = 50.0; // 将目标阀位设置为50%
|
||
mode_pwmp_hd_test->wait_count = 2000; // 等待20秒
|
||
FSM_WAIT(*state); // 设置等待状态
|
||
}
|
||
else
|
||
{
|
||
pwmp_control_update(FILTER_MEDIAN);
|
||
if (rt_data.loop_current >= 4.0)
|
||
{
|
||
execute_res = execute_dac(EXECUTE_PLAN); // PID计算结果经过执行器处理
|
||
if (execute_res->code == EXECUTE_HD_OUT) // 执行DAC输出
|
||
{
|
||
pdctrl_out(execute_res->dac);
|
||
}
|
||
}
|
||
/*当误差大于0.5%时重置等待时间*/
|
||
if (fabs(mode_pwmp_hd_test->real_error) > 0.5)
|
||
{
|
||
mode_pwmp_hd_test->wait_count = 2000;
|
||
}
|
||
else
|
||
{
|
||
mode_pwmp_hd_test->wait_count--;
|
||
if (mode_pwmp_hd_test->wait_count == 0)
|
||
{
|
||
mode_pwmp_hd_test->arr_50_percent = execute_res->dac;
|
||
mode_pwmp_hd_test->small_trip_adc_50_percent = mode_pwmp_hd_test->small_trip_current_adc;
|
||
*state = next_state;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/*得到小行程稳定区间上限*/
|
||
static void pwmp_test_hd_get_small_travel_stable_range_up(uint8_t *state, mode_pwmp_hd_test_small_travel_state_e next_state)
|
||
{
|
||
pwmp_control_update(FILTER_AVERAGE);
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
mode_pwmp_hd_test->wait_count = 400; // 等待8秒
|
||
mode_pwmp_hd_test->ctrl_target = 50.0; // 将目标阀位设置为50%
|
||
mode_pwmp_hd_test->arr_current = mode_pwmp_hd_test->arr_50_percent; // 以50%阀位稳定信号值输出
|
||
pdctrl_out(mode_pwmp_hd_test->arr_current);
|
||
FSM_WAIT(*state); // 设置等待状态
|
||
}
|
||
else
|
||
{
|
||
if (fabs(mode_pwmp_hd_test->real_error) <= 0.5)
|
||
{
|
||
if (!(mode_pwmp_hd_test->wait_count--))
|
||
{
|
||
mode_pwmp_hd_test->arr_current++;
|
||
mode_pwmp_hd_test->flag_stable = 1;
|
||
mode_pwmp_hd_test->wait_count = 400;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (mode_pwmp_hd_test->flag_stable)
|
||
{
|
||
mode_pwmp_hd_test->small_trip_adc_stable_up = mode_pwmp_hd_test->small_trip_current_adc;
|
||
mode_pwmp_hd_test->wait_count = 800; // 等待8秒
|
||
*state = next_state;
|
||
}
|
||
}
|
||
pdctrl_out(mode_pwmp_hd_test->arr_current);
|
||
}
|
||
}
|
||
|
||
/*得到小行程稳定区间下限*/
|
||
static void pwmp_test_hd_get_small_travel_stable_range_down(uint8_t *state, mode_pwmp_hd_test_small_travel_state_e next_state)
|
||
{
|
||
pwmp_control_update(FILTER_AVERAGE);
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
/*等待上一次延时的时间*/
|
||
if (mode_pwmp_hd_test->wait_count-- > 0)
|
||
{
|
||
return;
|
||
}
|
||
mode_pwmp_hd_test->wait_count = 400; // 等待4秒
|
||
mode_pwmp_hd_test->ctrl_target = 50.0; // 将目标阀位设置为50%
|
||
mode_pwmp_hd_test->arr_current = mode_pwmp_hd_test->arr_50_percent; // 以50%阀位稳定信号值输出
|
||
pdctrl_out(mode_pwmp_hd_test->arr_current);
|
||
FSM_WAIT(*state); // 设置等待状态
|
||
}
|
||
else
|
||
{
|
||
if (fabs(mode_pwmp_hd_test->real_error) <= 0.5)
|
||
{
|
||
if (!(mode_pwmp_hd_test->wait_count--))
|
||
{
|
||
mode_pwmp_hd_test->arr_current--;
|
||
mode_pwmp_hd_test->flag_stable = 1;
|
||
mode_pwmp_hd_test->wait_count = 400;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (mode_pwmp_hd_test->flag_stable)
|
||
{
|
||
mode_pwmp_hd_test->small_trip_adc_stable_down = mode_pwmp_hd_test->small_trip_current_adc;
|
||
*state = next_state;
|
||
}
|
||
}
|
||
pdctrl_out(mode_pwmp_hd_test->arr_current);
|
||
}
|
||
}
|
||
|
||
//////////////////////////////*测试模式结束*////////////////////////////////////////////////////////////////////////
|
||
|
||
//////////////////////////////*小回路测试模式入口*///////////////////////////////////////////////////////////////////
|
||
void small_travel_test(uint8_t *state)
|
||
{
|
||
uint8_t ts = *state;
|
||
BIT_CLR(ts, BIT7); // 高位清零
|
||
switch (ts)
|
||
{
|
||
case PWMP_HD_TEST_GET_SMALL_TRAVEL_HIGH:
|
||
pwmp_test_hd_get_small_travel_high(state, PWMP_HD_TEST_GET_SMALL_TRAVEL_LOW);
|
||
break;
|
||
case PWMP_HD_TEST_GET_SMALL_TRAVEL_LOW:
|
||
pwmp_test_hd_get_small_travel_low(state, PWMP_HD_TEST_GET_SMALL_TRAVEL_50_PER_ARR);
|
||
break;
|
||
case PWMP_HD_TEST_GET_SMALL_TRAVEL_50_PER_ARR:
|
||
pwmp_test_hd_get_small_travel_50_per_arr(state, PWMP_HD_TEST_GET_SMALL_TRAVEL_STABLE_RANGE_UP);
|
||
break;
|
||
case PWMP_HD_TEST_GET_SMALL_TRAVEL_STABLE_RANGE_UP:
|
||
pwmp_test_hd_get_small_travel_stable_range_up(state, PWMP_HD_TEST_GET_SMALL_TRAVEL_STABLE_RANGE_DOWN);
|
||
break;
|
||
case PWMP_HD_TEST_GET_SMALL_TRAVEL_STABLE_RANGE_DOWN:
|
||
pwmp_test_hd_get_small_travel_stable_range_down(state, PWMP_HD_TEST_OPEN_LOOP_OUTPUT_TEST);
|
||
break;
|
||
case PWMP_HD_TEST_OPEN_LOOP_OUTPUT_TEST:
|
||
mode_pwmp_hd_test->test_state = PWMP_HD_TEST_OPEN_LOOP_OUTPUT;
|
||
break;
|
||
default:
|
||
mode_pwmp_hd_test->test_state = PWMP_HD_TEST_OPEN_LOOP_OUTPUT;
|
||
break;
|
||
}
|
||
}
|
||
/////////////////////////////*小回路测试模式出口*////////////////////////////////////////////////////////////////////
|
||
|
||
//////////////////////////////*测试模式入口BEGIN*///////////////////////////////////////////////////////////////////
|
||
void pwmp_test(uint8_t *state)
|
||
{
|
||
uint8_t ts = *state;
|
||
ts = PWMP_HD_TEST_OPEN_LOOP_OUTPUT;
|
||
BIT_CLR(ts, BIT7); // 高位清零
|
||
switch (ts)
|
||
{
|
||
case VELOCITY_TEST:
|
||
pwmp_test_hd_velocity_test(state, SMALL_TRAVEL_TEST);
|
||
break;
|
||
case SMALL_TRAVEL_TEST:
|
||
small_travel_test((uint8_t *)&mode_pwmp_hd_test->small_travel_test_state);
|
||
case PWMP_HD_TEST_OPEN_LOOP_OUTPUT:
|
||
pwmp_test_hd_open_loop_output();
|
||
break;
|
||
default:
|
||
pwmp_test_hd_open_loop_output();
|
||
break;
|
||
}
|
||
}
|
||
//////////////////////////////*测试模式入口END*/////////////////////////////////////////////////////////////////////
|
||
|
||
//////////////////////////////*入口BEGIN*//////////////////////////////////////////////////////
|
||
void mode_pwmp_hd_process(void)
|
||
{
|
||
mode_pwmp_hd_control_t *p = &mode_pwmp_hd->control;
|
||
execute_rsp_hd_t *execute_res;
|
||
p->enter_count++;
|
||
if (p->enter_count % WAIT_COUNT_MAX == 0) // 100ms输出一次
|
||
{
|
||
if (BIT_IS_SET(hc_24_state, BIT2))
|
||
{
|
||
// 蓝牙输出
|
||
char ble_data[128];
|
||
uint8_t ble_len = 0;
|
||
osel_memset((uint8_t *)ble_data, 0, 128);
|
||
// sprintf(ble_data, "%f,%f,%f,%d,%f\r\n", pid_target, show_actual, p->real_error, mode_pwm->output, rt_data.loop_current );
|
||
if (mode_pwmp_hd->process_state == PWMP_HD_PROCESS_TEST)
|
||
{
|
||
// sprintf(ble_data, "%f,%f,%f,%f\r\n", mode_pwmp_hd->control.ctrl_target, mode_pwmp_hd->control.ctrl_feedback, mode_pwmp_hd->control.real_error, rt_data.loop_current );
|
||
if (mode_pwmp_hd_test->test_state == PWMP_HD_TEST_OPEN_LOOP_OUTPUT)
|
||
{
|
||
sprintf(ble_data, "%f,%d,%d,%f\r\n", mode_pwmp_hd->control.ctrl_feedback, mode_pwmp_hd_test->small_trip_current_adc, mode_pwmp_hd->output, mode_pwmp_hd->current_electric);
|
||
}
|
||
else
|
||
{
|
||
sprintf(ble_data, "%f,%f,%f,%d,%d\r\n", mode_pwmp_hd_test->ctrl_feedback, mode_pwmp_hd_test->ctrl_target, mode_pwmp_hd_test->real_error, mode_pwmp_hd_test->small_trip_current_adc, rsp.dac);
|
||
}
|
||
}
|
||
else if (mode_pwmp_hd->process_state == PWMP_HD_PROCESS_CONTROL || mode_pwmp_hd->process_state == PWMP_HD_PROCESS_STOP)
|
||
{
|
||
sprintf(ble_data, "%f,%f,%f,%d,%d\r\n", mode_pwmp_hd->control.ctrl_feedback, mode_pwmp_hd->control.ctrl_target, mode_pwmp_hd->control.real_error, mode_pwmp_hd->control.small_trip_current_adc, rsp.dac);
|
||
}
|
||
else if (mode_pwmp_hd->process_state == PWMP_HD_PROCESS_ADJUST)
|
||
{
|
||
sprintf(ble_data, "%d,%d\r\n", mode_pwmp_hd_adjust->psb_adc, mode_pwmp_hd_adjust->arr_current);
|
||
}
|
||
|
||
ble_len = osel_mstrlen((unsigned char *)ble_data);
|
||
if (ble_len != 0)
|
||
{
|
||
h24_bluetooth_output_dbg((uint8_t *)ble_data, ble_len);
|
||
}
|
||
|
||
// 蓝牙输出
|
||
}
|
||
p->enter_count = 0;
|
||
}
|
||
|
||
switch (mode_pwmp_hd->process_state)
|
||
{
|
||
case PWMP_HD_PROCESS_CONTROL:
|
||
{
|
||
pwmp_control_update(FILTER_MEDIAN);
|
||
if (rt_data.loop_current >= LOOP_CURRENT_MIN)
|
||
{
|
||
if (rt_data.loop_current < 4.5)
|
||
{
|
||
pdctrl_out(mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin * 0.8);
|
||
}
|
||
else if (rt_data.loop_current > 19.5)
|
||
{
|
||
pdctrl_out(mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin * 1.2);
|
||
}
|
||
else if (rt_data.loop_current >= 4.5 && rt_data.loop_current <= 19.5)
|
||
{
|
||
execute_res = execute_dac(EXECUTE_PLAN); // PID计算结果经过执行器处理
|
||
if (execute_res->code == EXECUTE_HD_OUT) // 执行DAC输出
|
||
{
|
||
pdctrl_out(execute_res->dac);
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
pdctrl_out(0);
|
||
}
|
||
break;
|
||
}
|
||
case PWMP_HD_PROCESS_ADJUST:
|
||
{
|
||
pwmp_adjust((uint8_t *)&mode_pwmp_hd->adjust_state);
|
||
break;
|
||
}
|
||
case PWMP_HD_PROCESS_ADJUST_STOP:
|
||
{
|
||
pwmp_adjust_hd_stop();
|
||
break;
|
||
}
|
||
case PWMP_HD_PROCESS_STOP:
|
||
{
|
||
mode_pwmp_hd_stop((mode_pwmp_hd_t *)&mode_pwmp_hd);
|
||
break;
|
||
}
|
||
case PWMP_HD_PROCESS_TEST:
|
||
{
|
||
pwmp_test((uint8_t *)&mode_pwmp_hd_test->test_state);
|
||
break;
|
||
}
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
//////////////////////////////*入口END*//////////////////////////////////////////////////////
|
||
|
||
/*程序初始化*/
|
||
void mode_pwmp_hd_init(mode_interface_req_t *req, uint16_t positioner_model, mode_pwmp_hd_params_u *params, void (*params_save_cb)(void))
|
||
{
|
||
DBG_ASSERT(req != NULL __DBG_LINE);
|
||
DBG_ASSERT(params != NULL __DBG_LINE); // 断言检查
|
||
DBG_ASSERT(params_save_cb != NULL __DBG_LINE);
|
||
req->mode_process_start = _process_start;
|
||
req->mode_process_stop = _process_stop;
|
||
req->mode_adjust_start = _adjust_start;
|
||
req->mode_adjust_stop = _adjust_stop;
|
||
req->mode_get_adjust_data = _adjust_data;
|
||
req->mode_adjust_result = _adjust_result;
|
||
req->mode_adjust_step_count = _adjust_step_count;
|
||
req->mode_adjust_step_current = _adjust_step_current;
|
||
req->mode_control_idle = _control_idle;
|
||
req->mode_is_adjusting = _adjust_isrun;
|
||
|
||
/*大电流输出使能开关*/
|
||
#if VIP_H_EN_HD == 1
|
||
VIP_H_EN_ENABLE();
|
||
#endif
|
||
/*为指针开辟存储空间*/
|
||
// 控制
|
||
if (mode_pwmp_hd == NULL)
|
||
{
|
||
mode_pwmp_hd = (mode_pwmp_hd_t *)osel_mem_alloc(sizeof(mode_pwmp_hd_t));
|
||
}
|
||
osel_memset((uint8_t *)mode_pwmp_hd, 0, sizeof(mode_pwmp_hd_t));
|
||
// 整定
|
||
if (mode_pwmp_hd_adjust == NULL)
|
||
{
|
||
mode_pwmp_hd_adjust = (pwmp_hd_adjust_t *)osel_mem_alloc(sizeof(pwmp_hd_adjust_t));
|
||
}
|
||
osel_memset((uint8_t *)mode_pwmp_hd_adjust, 0, sizeof(pwmp_hd_adjust_t));
|
||
// 调试
|
||
if (mode_pwmp_hd_test == NULL)
|
||
{
|
||
mode_pwmp_hd_test = (pwmp_hd_test_t *)osel_mem_alloc(sizeof(pwmp_hd_test_t));
|
||
}
|
||
osel_memset((uint8_t *)mode_pwmp_hd_test, 0, sizeof(pwmp_hd_test_t));
|
||
/*滤波初始化*/
|
||
mode_pwmp_hd->filter.handle = lpf_window_init(20);
|
||
/*绑定要保存的参数与函数*/
|
||
mode_pwmp_hd->pwmp_save = params;
|
||
mode_pwmp_hd->params_save_cb = params_save_cb;
|
||
/*设定PWMP方式的工作模式*/
|
||
pwmp_hd_process_state_set(PWMP_HD_PROCESS_CONTROL);
|
||
/*PID初始化*/
|
||
execute_pid_init(EXECUTE_PLAN);
|
||
/*计数最大值*/
|
||
// mode_pwmp_hd->wait_count_max = 100 / ((MODE_DEFAULT_AUTOLOAD + 1) * 0.1); // （=10，每减一消耗10ms，总消耗时间100ms）
|
||
mode_pwmp_hd->wait_count_max = 2; // （每减一耗时50ms，共100ms）
|
||
/*保存参数*/
|
||
mode_pwmp_hd->params_save_cb();
|
||
|
||
/*更新公共参数*/
|
||
pwmp_public_params_update();
|
||
calib_parapos_perent();
|
||
}
|
||
|
||
/*程序反初始化*/
|
||
void mode_pwmp_hd_dinit(void)
|
||
{
|
||
if (mode_pwmp_hd != NULL)
|
||
{
|
||
pwmp_hd_process_state_set(PWMP_HD_PROCESS_STOP);
|
||
osel_mem_free(mode_pwmp_hd);
|
||
mode_pwmp_hd = NULL;
|
||
}
|
||
}
|