162 lines
5.2 KiB
C
162 lines
5.2 KiB
C
#include "dehy_test.h"
|
|
#include "mw_soft_timer.h"
|
|
|
|
#define DEHY_SPEED_MIN (0u)
|
|
#define DEHY_SPEED_MAX (800u)
|
|
#define DEHY_SPEED_UPDATE_MS (1000u)
|
|
#define SPEEDUP_INIT_CMD (0x5AA5)
|
|
|
|
|
|
|
|
|
|
/* 脱水速度设置 枚举变量 */
|
|
typedef enum
|
|
{
|
|
DEHY_SPEEDSET_INIT, // 初始化
|
|
DEHY_SPEEDSET_SPEED_UP, // 加速
|
|
DEHY_SPEEDSET_SPEED_KEEP, // 稳速
|
|
DEHY_SPEEDSET_SPEED_STOP, // 超震停机
|
|
|
|
DEHY_SPEEDSET_NUM
|
|
}SPEEDUP_ENUM_T;
|
|
|
|
/* 外部调用 速度设置值 */
|
|
static uint16_t speed_set_val = 0u;
|
|
|
|
/*************************************************************************************
|
|
* @brief 下设速度
|
|
* @param[in/out] val
|
|
*
|
|
* @warning
|
|
* @note
|
|
*************************************************************************************/
|
|
void motor_speed_set(uint16_t val)
|
|
{
|
|
speed_set_val = val;
|
|
}
|
|
|
|
|
|
void dehy_speed_up(LOGIC_STATE_ENUM_T init_cmd, uint16_t acc_val, uint16_t current_speed, uint16_t target_speed)
|
|
{
|
|
|
|
static uint32_t timetick = 0; // 时间戳
|
|
static SPEEDUP_ENUM_T add_speed_state = DEHY_SPEEDSET_INIT; // 本函数的状态机
|
|
static uint16_t result_speed = 0; // 预设速度
|
|
static uint16_t target_speed_cache = 0; // 缓存目标速度
|
|
// static uint16_t current_speed_cache = 0; // 缓存当前速度
|
|
if(init_cmd == LOGIC_INIT)
|
|
{
|
|
add_speed_state = DEHY_SPEEDSET_INIT;
|
|
}
|
|
else
|
|
{
|
|
;
|
|
}
|
|
|
|
switch(add_speed_state)
|
|
{
|
|
// init
|
|
case DEHY_SPEEDSET_INIT:
|
|
// 时间戳 初始化
|
|
timetick = get_systick_ms();
|
|
// 预设速度 初始化
|
|
result_speed = current_speed;
|
|
// 目标速度缓存 初始化
|
|
target_speed_cache = target_speed;
|
|
// // 刷新 当前速度缓存
|
|
// current_speed_cache = current_speed;
|
|
// 进入下一阶段
|
|
add_speed_state = DEHY_SPEEDSET_SPEED_UP;
|
|
break;
|
|
// 加速
|
|
case DEHY_SPEEDSET_SPEED_UP:
|
|
// 当前 触发超震
|
|
if(target_speed == 0)
|
|
{
|
|
add_speed_state = DEHY_SPEEDSET_SPEED_STOP;
|
|
result_speed = 0;
|
|
}
|
|
// 周期到
|
|
else if(get_systick_ms() - timetick > 1000u)
|
|
{
|
|
|
|
timetick = get_systick_ms();
|
|
|
|
// 目标速度 小于 目标缓存速度，说明外面超震大 目标速度变小了 高4-> 高3
|
|
if(target_speed < target_speed_cache)
|
|
{
|
|
// 若当前速度 小于 现在的目标速度 则没事 更新一下目标速度缓存 当前就不加速了
|
|
if(current_speed < target_speed)
|
|
{
|
|
// // 刷新 目标速度缓存
|
|
// target_speed_cache = target_speed;
|
|
// // 刷新 当前速度缓存
|
|
// current_speed_cache = current_speed;
|
|
}
|
|
else
|
|
{
|
|
// 否则保持当前缓存的速度 稳住
|
|
// result_speed = current_speed_cache;
|
|
// 记录当前速度
|
|
result_speed = current_speed;
|
|
// 切到速度保持
|
|
add_speed_state = DEHY_SPEEDSET_SPEED_KEEP;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// // 刷新 目标速度缓存
|
|
// target_speed_cache = target_speed;
|
|
// // 刷新 当前速度缓存
|
|
// current_speed_cache = current_speed;
|
|
// 更新 预设速度
|
|
result_speed += acc_val;
|
|
// 进下一阶段判断
|
|
if(result_speed >= target_speed)
|
|
{
|
|
result_speed = target_speed;
|
|
add_speed_state = DEHY_SPEEDSET_SPEED_KEEP;
|
|
}
|
|
|
|
}
|
|
// 刷新 目标速度缓存
|
|
target_speed_cache = target_speed;
|
|
}
|
|
break;
|
|
// 速度保持
|
|
case DEHY_SPEEDSET_SPEED_KEEP:
|
|
// 超震停机
|
|
if(target_speed == 0)
|
|
{
|
|
result_speed = 0;
|
|
add_speed_state = DEHY_SPEEDSET_SPEED_STOP;
|
|
}
|
|
// 目标速度变好了 外面超震小了
|
|
else if(target_speed > target_speed_cache)
|
|
{
|
|
// 若预定速度 小于 新的目标速度，则回去加速
|
|
if(result_speed < target_speed)
|
|
{
|
|
// 回加速
|
|
add_speed_state = DEHY_SPEEDSET_SPEED_UP;
|
|
}
|
|
|
|
target_speed_cache = target_speed;
|
|
// // 刷新 当前速度缓存
|
|
// current_speed_cache = current_speed;
|
|
}
|
|
break;
|
|
// 超震停机
|
|
case DEHY_SPEEDSET_SPEED_STOP:
|
|
result_speed = 0;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
/* 设置速度 */
|
|
motor_speed_set(result_speed);
|
|
}
|
|
|
|
|