1219 lines
36 KiB
C
1219 lines
36 KiB
C
/**
|
||
* @file mode_pwmp_hd.c
|
||
* @author xxx
|
||
* @date 2023-10-10 08:58:43
|
||
* @brief 此文件用于实现定位器自整定、控制功能
|
||
* @copyright Copyright (c) 2023 by xxx, All Rights Reserved.
|
||
*/
|
||
#include <stdio.h>
|
||
#include <math.h>
|
||
#include "sys.h"
|
||
#include "dacs.h"
|
||
#include "board.h"
|
||
#include "../inc/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;
|
||
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; // 执行计数
|
||
|
||
/*程序状态设置*/
|
||
void pwmp_hd_process_state_set(mode_pwmp_hd_process_state_e state)
|
||
{
|
||
if (mode_pwmp_hd->process_state == state && mode_pwmp_hd->adjust_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;
|
||
actual_travel = get_actual_travel(type);
|
||
actual_travel = actual_travel_deal(actual_travel);
|
||
pid_actual = get_pid_travel(actual_travel);
|
||
|
||
if (mode_pwmp_hd->process_state == PWMP_HD_PROCESS_ADJUST)
|
||
{
|
||
p->ctrl_target = p->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];
|
||
}
|
||
|
||
/*PID参数获取*/
|
||
static float32 get_pwmp_control_kp(void)
|
||
{
|
||
if (mode_pwmp_hd->pwmp_save->storage.kp < 1)
|
||
{
|
||
return mode_pwmp_hd->pwmp_save->storage.kp * 10;
|
||
}
|
||
else
|
||
{
|
||
return mode_pwmp_hd->pwmp_save->storage.kp;
|
||
}
|
||
}
|
||
|
||
static float32 get_pwmp_control_ki(void)
|
||
{
|
||
return mode_pwmp_hd->pwmp_save->storage.ki;
|
||
}
|
||
|
||
static float32 get_pwmp_control_kd(void)
|
||
{
|
||
return mode_pwmp_hd->pwmp_save->storage.kd;
|
||
}
|
||
|
||
/*将自己结构体变量中的参数保存到公共参数中*/
|
||
static void pwmp_public_params_update()
|
||
{
|
||
uDevice.TravelVol0 = mode_pwmp_hd->pwmp_save->storage.trip_0;
|
||
uDevice.TravelVol100 = mode_pwmp_hd->pwmp_save->storage.trip_100;
|
||
uDevice.DAC_Min = mode_pwmp_hd->pwmp_save->storage.pwmp_min;
|
||
uDevice.DAC_Max = mode_pwmp_hd->pwmp_save->storage.pwmp_max;
|
||
uDevice.MinorLoopVol0_Val = mode_pwmp_hd->pwmp_save->storage.trip_min0;
|
||
uDevice.MinorLoopVol100_Val = mode_pwmp_hd->pwmp_save->storage.trip_min100;
|
||
uDevice.APID_KP = mode_pwmp_hd->pwmp_save->storage.kp;
|
||
uDevice.APID_TI = mode_pwmp_hd->pwmp_save->storage.ki;
|
||
uDevice.APID_TD = mode_pwmp_hd->pwmp_save->storage.kd;
|
||
uDevice.ProcessChange_Flag = mode_pwmp_hd->pwmp_save->storage.prov_flag;
|
||
uDevice.eAirAction = mode_pwmp_hd->pwmp_save->storage.valve_type;
|
||
}
|
||
|
||
/*程序反初始化*/
|
||
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;
|
||
}
|
||
}
|
||
|
||
//////////////////////////////*控制BEGIN*//////////////////////////////////////////////////////
|
||
|
||
/**
|
||
* @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_control_kp(), get_pwmp_control_ki(), get_pwmp_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_NONE;
|
||
return &rsp;
|
||
}
|
||
}
|
||
|
||
static execute_rsp_hd_t *execute_dac_plan1()
|
||
{
|
||
static float32 index_min = 0; // 加权系数，为了修正排气快的问题
|
||
static float32 index_max = 0;
|
||
static float32 Control_diff;
|
||
Control_diff = mode_pwmp_hd->pwmp_save->storage.pwmp_max - mode_pwmp_hd->pwmp_save->storage.pwmp_min;
|
||
float32 out = 0.0;
|
||
|
||
// 杭电：输出限幅的参数设置
|
||
_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; // 对PID输出结果进行归一化处理
|
||
out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max + Control_diff * index_max) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min - Control_diff * index_min); // 映射实际控制器的范围
|
||
out += mode_pwmp_hd->pwmp_save->storage.pwmp_min - Control_diff * index_min; // 对输出结果进行偏移处理
|
||
|
||
rsp.code = EXECUTE_DAC;
|
||
rsp.dac = (uint32_t)out;
|
||
|
||
/// 在执行了十次之后若系统认为稳定进入以下程序进行微调
|
||
if (execute_run_count >= 10)
|
||
{
|
||
if (actual_travel < target_travel)
|
||
{
|
||
if (fabs(mode_pwmp_hd->control.real_error) < 1)
|
||
{
|
||
rsp.dac += (fabs(mode_pwmp_hd->control.real_error) * 30);
|
||
}
|
||
else if (fabs(mode_pwmp_hd->control.real_error) < 5)
|
||
{
|
||
rsp.dac += (fabs(mode_pwmp_hd->control.real_error) * 10);
|
||
}
|
||
else
|
||
{
|
||
execute_run_count = 0;
|
||
}
|
||
}
|
||
if (target_travel <= actual_travel)
|
||
{
|
||
// 若进入死区范围，那么停止计数，停止微调
|
||
if (fabs(mode_pwmp_hd->control.real_error) <= 0.5)
|
||
{
|
||
execute_run_count = 0;
|
||
}
|
||
else
|
||
{
|
||
if (fabs(mode_pwmp_hd->control.real_error) < 1)
|
||
{
|
||
rsp.dac += -(fabs(mode_pwmp_hd->control.real_error) * 30);
|
||
}
|
||
else if (fabs(mode_pwmp_hd->control.real_error) < 5)
|
||
{
|
||
rsp.dac += -(fabs(mode_pwmp_hd->control.real_error) * 10);
|
||
}
|
||
else
|
||
{
|
||
execute_run_count = 0;
|
||
}
|
||
execute_run_count = 2;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
execute_run_count++;
|
||
}
|
||
return &rsp;
|
||
}
|
||
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
/// <param name=""></param>
|
||
/// <returns></returns>
|
||
|
||
static execute_rsp_hd_t *execute_dac_plan2(void)
|
||
{
|
||
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 void set_step_signal(uint16_t signal_in)
|
||
{
|
||
pdctrl_out(signal_in);
|
||
}
|
||
|
||
/*判断阀门状态：移动、停止、停止但仍在等待*/
|
||
static valve_position_change_hd_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();
|
||
|
||
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;
|
||
/*为指针开辟存储空间*/
|
||
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_hd_e s = pwmp_adjust_hd_valve_position_change(state, next_state, DIFF_ADC_MAX);
|
||
|
||
switch (s)
|
||
{
|
||
case POSITION_NO_CHANGE_FOREVER:
|
||
// 位置不再改变，记录此时阀门位置AD值
|
||
mode_pwmp_hd_adjust->adc_record_1 = mode_pwmp_hd_adjust->psb_adc;
|
||
mode_pwmp_hd_adjust->adc_record_0 = mode_pwmp_hd_adjust->psb_adc;
|
||
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)
|
||
{
|
||
// 找最小推动值arr
|
||
uint16_t current_adc = 0;
|
||
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
// 以10%增加，提高DAC输出，找到能推动阀门的最小值
|
||
mode_pwmp_hd_adjust->arr_current = 50;
|
||
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_hd_e s = pwmp_adjust_hd_valve_position_change(state, next_state, DIFF_ADC_MAX);
|
||
|
||
current_adc = mode_pwmp_hd_adjust->psb_adc;
|
||
if (ABS(current_adc - mode_pwmp_hd_adjust->adc_record_1) >= DIFF_ADC_MAX)
|
||
{
|
||
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.9 : 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;
|
||
}
|
||
*state = next_state;
|
||
}
|
||
else
|
||
{
|
||
switch (s)
|
||
{
|
||
case POSITION_NO_CHANGE_FOREVER: // 位置不再改变
|
||
{
|
||
|
||
if (ABS(current_adc - mode_pwmp_hd_adjust->adc_record_1) <= DIFF_ADC_MAX)
|
||
{
|
||
// 没有发生变化
|
||
mode_pwmp_hd_adjust->arr_last = mode_pwmp_hd_adjust->arr_current;
|
||
if (mode_pwmp_hd_adjust->arr_last < 1000)
|
||
{
|
||
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 * 20;
|
||
*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_bleeding_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))
|
||
{
|
||
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 * 2;
|
||
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_MAX)
|
||
{
|
||
mode_pwmp_hd_adjust->arr_record_0 = mode_pwmp_hd_adjust->arr_last + DIFF_ADC_MAX;
|
||
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_MAX)
|
||
{
|
||
mode_pwmp_hd_adjust->arr_record_0 = mode_pwmp_hd_adjust->arr_last + DIFF_ADC_MAX;
|
||
flag = TRUE;
|
||
}
|
||
}
|
||
}
|
||
|
||
if (flag == TRUE)
|
||
{
|
||
*state = next_state;
|
||
}
|
||
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) > 4)
|
||
{
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 10;
|
||
}
|
||
else
|
||
{
|
||
mode_pwmp_hd_adjust->wait_count = 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_hd_e s = pwmp_adjust_hd_valve_position_change(state, next_state, DIFF_ADC_MAX * 10);
|
||
|
||
current_adc = mode_pwmp_hd_adjust->psb_adc;
|
||
/*执行器发生动作*/
|
||
if (ABS(current_adc - mode_pwmp_hd_adjust->adc_record_1) >= DIFF_ADC_MAX)
|
||
{
|
||
mode_pwmp_hd_adjust->arr_record_1 = mode_pwmp_hd_adjust->arr_last;
|
||
mode_pwmp_hd_adjust->arr_record_2 = mode_pwmp_hd_adjust->arr_last + 100 < 4000 ? mode_pwmp_hd_adjust->arr_last + 100 : 100;
|
||
*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: // 位置不再改变
|
||
{
|
||
if (ABS(current_adc - mode_pwmp_hd_adjust->adc_record_1) <= DIFF_ADC_MAX)
|
||
{
|
||
// 没有发生变化
|
||
mode_pwmp_hd_adjust->arr_last = mode_pwmp_hd_adjust->arr_current;
|
||
mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_current + DIFF_ADC_MAX;
|
||
pdctrl_out(mode_pwmp_hd_adjust->arr_current);
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 5;
|
||
*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;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/*粗略整定冲顶值*/
|
||
static void pwmp_adjust_hd_rough_position100(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
|
||
{
|
||
// 找最大推动值arr
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
/*上一步赋值的count*/
|
||
if (mode_pwmp_hd_adjust->wait_count-- > 0)
|
||
{
|
||
// 等待放气完毕
|
||
return;
|
||
}
|
||
|
||
sys_millis_reset();
|
||
mode_pwmp_hd_adjust->all_open_time = 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 * 100;
|
||
mode_pwmp_hd_adjust->adjust_state = (mode_pwmp_hd_adjust_state_e)*state;
|
||
FSM_WAIT(*state); // 设置等待状态
|
||
}
|
||
else
|
||
{
|
||
valve_position_change_hd_e s = pwmp_adjust_hd_valve_position_change(state, next_state, DIFF_ADC_MAX);
|
||
|
||
switch (s)
|
||
{
|
||
case POSITION_NO_CHANGE_FOREVER:
|
||
// 位置不再改变，记录此时阀门位置AD值
|
||
mode_pwmp_hd_adjust->adc_record_2 = mode_pwmp_hd_adjust->psb_adc;
|
||
mode_pwmp_hd_adjust->all_open_time = mode_pwmp_hd_adjust->tmp_time - mode_pwmp_hd_adjust->all_open_time;
|
||
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_accurate_position100(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
|
||
{
|
||
// 精确找最大推动值arr
|
||
uint16_t current_adc = 0;
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
mode_pwmp_hd_adjust->all_close_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_hd_e s = pwmp_adjust_hd_valve_position_change(state, next_state, DIFF_ADC_MAX);
|
||
|
||
current_adc = mode_pwmp_hd_adjust->psb_adc;
|
||
uint16_t adc_diff = ABS(current_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;
|
||
mode_pwmp_hd_adjust->arr_record_2 = mode_pwmp_hd_adjust->arr_current;
|
||
}
|
||
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
|
||
{
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 10;
|
||
}
|
||
|
||
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 * 3;
|
||
}
|
||
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
/*计算阀门自整定参数*/
|
||
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 = mode_pwmp_hd_adjust->arr_record_2;
|
||
mode_pwmp_hd->pwmp_save->storage.pwmp_min = mode_pwmp_hd_adjust->arr_record_0;
|
||
mode_pwmp_hd->pwmp_save->storage.arr_diff = ABS(mode_pwmp_hd->pwmp_save->storage.pwmp_max - mode_pwmp_hd->pwmp_save->storage.pwmp_min);
|
||
|
||
if (uDevice.eAirAction == 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.ProcessChange_Flag = 1;
|
||
}
|
||
else // 充气，ADC减小，正装
|
||
{
|
||
uDevice.ProcessChange_Flag = 0;
|
||
}
|
||
|
||
uDevice.TravelVol0 = mode_pwmp_hd->pwmp_save->storage.trip_0;
|
||
uDevice.TravelVol100 = mode_pwmp_hd->pwmp_save->storage.trip_100;
|
||
uDevice.DAC_Min = mode_pwmp_hd->pwmp_save->storage.pwmp_min;
|
||
uDevice.DAC_Max = mode_pwmp_hd->pwmp_save->storage.pwmp_max;
|
||
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)
|
||
{
|
||
uint16_t current_adc = 0;
|
||
if (!FSM_IS_WAIT(*state))
|
||
{
|
||
if (mode_pwmp_hd_adjust->all_close_time_flag == TRUE)
|
||
{
|
||
sys_millis_reset();
|
||
mode_pwmp_hd_adjust->all_close_time = sys_millis(); // 记录全关起始时间
|
||
mode_pwmp_hd_adjust->tmp_time = 0;
|
||
}
|
||
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;
|
||
pdctrl_out(0);
|
||
FSM_WAIT(*state); // 设置等待状态
|
||
}
|
||
else
|
||
{
|
||
valve_position_change_hd_e s = pwmp_adjust_hd_valve_position_change(state, mode_pwmp_hd_adjust->adjust_state, DIFF_ADC_MAX);
|
||
current_adc = mode_pwmp_hd_adjust->psb_adc;
|
||
switch (s)
|
||
{
|
||
case POSITION_NO_CHANGE_FOREVER:
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 30; // 排气状态结束后在跳转后的状态中等待3s
|
||
mode_pwmp_hd_adjust->arr_current = 0;
|
||
if (current_adc != mode_pwmp_hd_adjust->adc_record_1)
|
||
{
|
||
mode_pwmp_hd_adjust->adc_record_1 = current_adc;
|
||
mode_pwmp_hd->pwmp_save->storage.trip_0 = current_adc;
|
||
}
|
||
if (mode_pwmp_hd_adjust->all_close_time_flag == TRUE)
|
||
{
|
||
mode_pwmp_hd_adjust->all_close_time = mode_pwmp_hd_adjust->tmp_time - mode_pwmp_hd_adjust->all_close_time;
|
||
mode_pwmp_hd_adjust->all_close_time_flag = FALSE;
|
||
}
|
||
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:
|
||
if (mode_pwmp_hd_adjust->all_close_time_flag == TRUE)
|
||
{
|
||
if (mode_pwmp_hd_adjust->tmp_time == 0)
|
||
{
|
||
mode_pwmp_hd_adjust->tmp_time = sys_millis();
|
||
}
|
||
|
||
if (ABS(current_adc - mode_pwmp_hd_adjust->adc_record_0) > 50)
|
||
{
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 80;
|
||
}
|
||
}
|
||
break;
|
||
default:
|
||
*state = PWMP_HD_ADJUST_STOP;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
/*停止模式*/
|
||
static void pwmp_adjust_hd_stop(uint8_t *state)
|
||
{
|
||
if (mode_pwmp_hd_adjust != NULL)
|
||
{
|
||
osel_mem_free(mode_pwmp_hd_adjust);
|
||
mode_pwmp_hd_adjust = NULL;
|
||
}
|
||
*state = PWMP_HD_ADJUST_IDEL;
|
||
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);
|
||
/*阶跃信号设置*/
|
||
set_step_signal(FULL_TRIP_SIGNAL);
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 80; // 等待八秒
|
||
FSM_WAIT(*state); // 设置等待状态
|
||
}
|
||
else
|
||
{
|
||
valve_position_change_hd_e s = pwmp_adjust_hd_valve_position_change(state, next_state, DIFF_ADC_MAX);
|
||
/*获取实时行程*/
|
||
pid_autotune_hd->data.cur_actual = get_actual_travel_adc();
|
||
/*计算变化率*/
|
||
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;
|
||
}
|
||
/*K的计算*/
|
||
pid_autotune_hd->k_hd = 1;
|
||
/*记录下变化率最大点值及行程坐标、时间*/
|
||
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;
|
||
*state = next_state;
|
||
break;
|
||
case POSITION_CHANGE:
|
||
mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 80;
|
||
break;
|
||
case POSITION_NO_CHANGE:
|
||
break;
|
||
default:
|
||
*state = PWMP_HD_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 = pid_autotune_hd->p_auto_hd;
|
||
mode_pwmp_hd->pwmp_save->storage.ki = pid_autotune_hd->i_auto_hd;
|
||
mode_pwmp_hd->pwmp_save->storage.kd = 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;
|
||
mode_pwmp_hd->pwmp_save->storage.tuned_flag = 1;
|
||
mode_pwmp_hd->params_save_cb();
|
||
mode_pwmp_hd->auto_tune_state = PWMP_HD_ADJUST_RESULT_SUCCESS;
|
||
}
|
||
|
||
//////////////////////////////////*状态机模式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_BLEEDING_POSITION0);
|
||
break;
|
||
case PWMP_HD_ADJUST_BLEEDING_POSITION0:
|
||
pwmp_adjust_hd_bleeding_position0(state, PWMP_HD_ADJUST_ACCURATE_POSITION0);
|
||
break;
|
||
case PWMP_HD_ADJUST_ACCURATE_POSITION0:
|
||
pwmp_adjust_hd_accurate_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;
|
||
case PWMP_HD_ADJUST_ACCURATE_POSITION100:
|
||
pwmp_adjust_hd_accurate_position100(state, PWMP_HD_ADJUST_CALCULATE);
|
||
break;
|
||
case PWMP_HD_ADJUST_CALCULATE:
|
||
pwmp_adjust_hd_calculate(state, PWMP_HD_ADJUST_PID_TUNING);
|
||
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_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_PID_TUNING);
|
||
break;
|
||
case PWMP_HD_ADJUST_STOP:
|
||
pwmp_adjust_hd_stop(state);
|
||
break;
|
||
case PWMP_HD_ADJUST_FAIL:
|
||
pwmp_adjust_hd_fail(state);
|
||
pwmp_adjust_hd_stop(state);
|
||
break;
|
||
default:
|
||
pwmp_adjust_hd_stop(state);
|
||
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, loop_current);
|
||
if (mode_pwmp_hd->process_state == PWMP_HD_PROCESS_CONTROL)
|
||
{
|
||
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, loop_current);
|
||
}
|
||
else
|
||
{
|
||
sprintf(ble_data, "%f\r\n", mode_pwmp_hd->control.ctrl_feedback);
|
||
}
|
||
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 (loop_current >= LOOP_CURRENT_MIN)
|
||
{
|
||
if (loop_current < 4.5)
|
||
{
|
||
pdctrl_out(0); // HD:此处后续需要将填入的数值换成整定出来的全排值（快速）
|
||
}
|
||
else if (loop_current > 19.5)
|
||
{
|
||
pdctrl_out(4095); // HD:此处后续需要将填入的数值换成整定出来的冲顶值（快速）
|
||
}
|
||
else if (loop_current >= 4.5 && loop_current <= 19.5)
|
||
{
|
||
execute_res = execute_dac(EXECUTE_PLAN); // PID计算结果经过执行器处理
|
||
if (execute_res->code == EXECUTE_DAC) // 执行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_STOP:
|
||
{
|
||
// TODO 完成停止自整定，并释放资源
|
||
break;
|
||
}
|
||
case PWMP_HD_PROCESS_TEST:
|
||
{
|
||
pdctrl_out(mode_pwmp_hd->output);
|
||
mode_pwmp_hd->current_adc = get_actual_travel_adc();
|
||
mode_pwmp_hd->current_electric = ip2current();
|
||
break;
|
||
}
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
//////////////////////////////*入口END*//////////////////////////////////////////////////////
|
||
|
||
/*程序初始化*/
|
||
void mode_pwmp_hd_init(mode_pwmp_hd_params_u *params, void (*params_save_cb)(void))
|
||
{
|
||
DBG_ASSERT(params != NULL __DBG_LINE); // 断言检查
|
||
DBG_ASSERT(params_save_cb != NULL __DBG_LINE);
|
||
VIP_H_EN_ENABLE();
|
||
/*为指针开辟存储空间*/
|
||
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));
|
||
/*滤波初始化*/
|
||
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_TEST);
|
||
/*PID初始化*/
|
||
execute_pid_init(EXECUTE_PLAN);
|
||
/*计数最大值*/
|
||
mode_pwmp_hd->wait_count_max = 100 / ((MODE_DEFAULT_AUTOLOAD + 1) * 0.1); // （=10，每减一消耗10ms）
|
||
/*临时参数设置（后续整定）*/
|
||
mode_pwmp_hd->pwmp_save->storage.trip_0 = 2583;
|
||
mode_pwmp_hd->pwmp_save->storage.trip_100 = 1722;
|
||
mode_pwmp_hd->pwmp_save->storage.pwmp_min = 500;
|
||
mode_pwmp_hd->pwmp_save->storage.pwmp_max = 925;
|
||
mode_pwmp_hd->pwmp_save->storage.valve_type = ATO; // 气开阀
|
||
#if INTEGRAL_SEPARATION == 1
|
||
#if 1
|
||
mode_pwmp_hd->pwmp_save->storage.kp = 5;
|
||
mode_pwmp_hd->pwmp_save->storage.ki = 0.02;
|
||
mode_pwmp_hd->pwmp_save->storage.kd = 0.005;
|
||
#else
|
||
mode_pwmp_hd->pwmp_save->storage.kp = 7.5882;
|
||
mode_pwmp_hd->pwmp_save->storage.ki = 0.1339;
|
||
mode_pwmp_hd->pwmp_save->storage.kd = 0.01;
|
||
#endif
|
||
#else
|
||
mode_pwmp_hd->pwmp_save->storage.kp = 4.1332;
|
||
mode_pwmp_hd->pwmp_save->storage.ki = 0.5619;
|
||
mode_pwmp_hd->pwmp_save->storage.kd = 0.5504;
|
||
#endif
|
||
/*阀门参数校准*/
|
||
// calib_parapos_perent();
|
||
mode_pwmp_hd->params_save_cb();
|
||
|
||
/*更新公共参数*/
|
||
pwmp_public_params_update();
|
||
calib_parapos_perent();
|
||
}
|