298 lines
9.2 KiB
C
298 lines
9.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)
|
|||
|
|
|
|||
|
|
/* 外部调用 速度设置值 */
|
|||
|
|
static uint16_t speed_set_val = 0u;
|
|||
|
|
|
|||
|
|
typedef enum
|
|||
|
|
{
|
|||
|
|
DEHY_NEW_STATE_ACCEL = 0u,
|
|||
|
|
DEHY_NEW_STATE_HOLD_CURRENT,
|
|||
|
|
DEHY_NEW_STATE_HOLD_TARGET
|
|||
|
|
} dehy_new_state_e;
|
|||
|
|
|
|||
|
|
static uint8_t dehy_new_inited = 0u;
|
|||
|
|
static dehy_new_state_e dehy_new_state = DEHY_NEW_STATE_ACCEL;
|
|||
|
|
static uint16_t dehy_new_set_speed = 0u;
|
|||
|
|
static uint16_t dehy_new_hold_speed = 0u;
|
|||
|
|
static uint16_t dehy_new_last_target = 0u;
|
|||
|
|
static uint32_t dehy_new_last_tick = 0u;
|
|||
|
|
|
|||
|
|
static uint16_t dehy_clamp_speed(uint16_t speed_val)
|
|||
|
|
{
|
|||
|
|
if (speed_val > DEHY_SPEED_MAX)
|
|||
|
|
{
|
|||
|
|
return DEHY_SPEED_MAX;
|
|||
|
|
}
|
|||
|
|
return speed_val;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void dehy_speed_set(uint16_t init_cmd, uint16_t acc_val, uint16_t current_speed, uint16_t target_speed)
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
static uint32_t timetick = 0; // 时间戳
|
|||
|
|
static uint16_t add_speed_state = 0; // 本函数的状态机
|
|||
|
|
static uint16_t result_speed = 0; // 预设速度
|
|||
|
|
static uint16_t target_speed_cache = 0; // 缓存目标速度
|
|||
|
|
// static uint16_t current_speed_cache = 0; // 缓存当前速度
|
|||
|
|
if(init_cmd == 0x5AA5)
|
|||
|
|
{
|
|||
|
|
add_speed_state = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
switch(add_speed_state)
|
|||
|
|
{
|
|||
|
|
// init
|
|||
|
|
case 0:
|
|||
|
|
// 时间戳 初始化
|
|||
|
|
timetick = get_systick_ms();
|
|||
|
|
// 预设速度 初始化
|
|||
|
|
result_speed = current_speed;
|
|||
|
|
// 目标速度缓存 初始化
|
|||
|
|
target_speed_cache = target_speed;
|
|||
|
|
// // 刷新 当前速度缓存
|
|||
|
|
// current_speed_cache = current_speed;
|
|||
|
|
// 进入下一阶段
|
|||
|
|
add_speed_state = 1;
|
|||
|
|
break;
|
|||
|
|
// 加速
|
|||
|
|
case 1:
|
|||
|
|
// 当前 触发超震
|
|||
|
|
if(target_speed == 0)
|
|||
|
|
{
|
|||
|
|
add_speed_state = 3;
|
|||
|
|
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 = 2;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
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 = 2;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
// 刷新 目标速度缓存
|
|||
|
|
target_speed_cache = target_speed;
|
|||
|
|
/* 设置速度 */
|
|||
|
|
speed_set_val = result_speed;
|
|||
|
|
}
|
|||
|
|
break;
|
|||
|
|
// 速度保持
|
|||
|
|
case 2:
|
|||
|
|
// 超震停机
|
|||
|
|
if(target_speed == 0)
|
|||
|
|
{
|
|||
|
|
result_speed = 0;
|
|||
|
|
add_speed_state = 3;
|
|||
|
|
}
|
|||
|
|
// 目标速度变好了 外面超震小了
|
|||
|
|
else if(target_speed > target_speed_cache)
|
|||
|
|
{
|
|||
|
|
target_speed_cache = target_speed;
|
|||
|
|
// // 刷新 当前速度缓存
|
|||
|
|
// current_speed_cache = current_speed;
|
|||
|
|
// 回第一步
|
|||
|
|
add_speed_state = 1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* 设置速度 */
|
|||
|
|
speed_set_val = result_speed;
|
|||
|
|
break;
|
|||
|
|
// 超震停机
|
|||
|
|
case 3:
|
|||
|
|
result_speed = 0;
|
|||
|
|
/* 设置速度 */
|
|||
|
|
speed_set_val = result_speed;
|
|||
|
|
break;
|
|||
|
|
default:
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void dehy_speed_set_new_init(uint16_t curr_speed)
|
|||
|
|
{
|
|||
|
|
uint16_t speed_init = dehy_clamp_speed(curr_speed);
|
|||
|
|
|
|||
|
|
dehy_new_inited = 1u;
|
|||
|
|
dehy_new_state = DEHY_NEW_STATE_ACCEL;
|
|||
|
|
dehy_new_set_speed = speed_init;
|
|||
|
|
dehy_new_hold_speed = speed_init;
|
|||
|
|
dehy_new_last_target = speed_init;
|
|||
|
|
dehy_new_last_tick = get_systick_ms();
|
|||
|
|
|
|||
|
|
speed_set_val = dehy_new_set_speed;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
uint16_t dehy_speed_set_new_get(void)
|
|||
|
|
{
|
|||
|
|
return dehy_new_set_speed;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
uint16_t dehy_speed_set_new(uint16_t curr_speed, uint16_t target_speed, uint16_t acc_val)
|
|||
|
|
{
|
|||
|
|
uint32_t now_ms;
|
|||
|
|
uint32_t elapsed_ms;
|
|||
|
|
uint32_t step_count;
|
|||
|
|
uint32_t next_speed;
|
|||
|
|
uint8_t target_decreased;
|
|||
|
|
uint8_t target_increased;
|
|||
|
|
|
|||
|
|
curr_speed = dehy_clamp_speed(curr_speed);
|
|||
|
|
target_speed = dehy_clamp_speed(target_speed);
|
|||
|
|
|
|||
|
|
if (dehy_new_inited == 0u)
|
|||
|
|
{
|
|||
|
|
dehy_speed_set_new_init(curr_speed);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
now_ms = get_systick_ms();
|
|||
|
|
target_decreased = (target_speed < dehy_new_last_target) ? 1u : 0u;
|
|||
|
|
target_increased = (target_speed > dehy_new_last_target) ? 1u : 0u;
|
|||
|
|
|
|||
|
|
/* Requirement 4: target == 0 must force set_speed to 0 immediately. */
|
|||
|
|
if (target_speed == DEHY_SPEED_MIN)
|
|||
|
|
{
|
|||
|
|
dehy_new_set_speed = 0u;
|
|||
|
|
dehy_new_hold_speed = 0u;
|
|||
|
|
dehy_new_last_target = 0u;
|
|||
|
|
dehy_new_state = DEHY_NEW_STATE_ACCEL;
|
|||
|
|
dehy_new_last_tick = now_ms;
|
|||
|
|
speed_set_val = dehy_new_set_speed;
|
|||
|
|
return dehy_new_set_speed;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* Requirement 2: target decreases and current speed is above it -> hold current speed. */
|
|||
|
|
if ((target_decreased > 0u) && (curr_speed > target_speed))
|
|||
|
|
{
|
|||
|
|
dehy_new_hold_speed = curr_speed;
|
|||
|
|
dehy_new_set_speed = dehy_new_hold_speed;
|
|||
|
|
dehy_new_state = DEHY_NEW_STATE_HOLD_CURRENT;
|
|||
|
|
dehy_new_last_target = target_speed;
|
|||
|
|
dehy_new_last_tick = now_ms;
|
|||
|
|
speed_set_val = dehy_new_set_speed;
|
|||
|
|
return dehy_new_set_speed;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* In hold-current state, leave hold when current speed is no longer above target. */
|
|||
|
|
if (dehy_new_state == DEHY_NEW_STATE_HOLD_CURRENT)
|
|||
|
|
{
|
|||
|
|
if (curr_speed > target_speed)
|
|||
|
|
{
|
|||
|
|
dehy_new_set_speed = dehy_new_hold_speed;
|
|||
|
|
dehy_new_last_target = target_speed;
|
|||
|
|
speed_set_val = dehy_new_set_speed;
|
|||
|
|
return dehy_new_set_speed;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
dehy_new_state = DEHY_NEW_STATE_ACCEL;
|
|||
|
|
dehy_new_last_tick = now_ms;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*
|
|||
|
|
* Requirement 3: target increases -> continue accelerating.
|
|||
|
|
* Also for requirement 2 (curr <= new target), keep running normal ramp logic.
|
|||
|
|
*/
|
|||
|
|
if ((target_increased > 0u) && (dehy_new_state == DEHY_NEW_STATE_HOLD_TARGET))
|
|||
|
|
{
|
|||
|
|
dehy_new_state = DEHY_NEW_STATE_ACCEL;
|
|||
|
|
dehy_new_last_tick = now_ms;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
dehy_new_last_target = target_speed;
|
|||
|
|
|
|||
|
|
switch (dehy_new_state)
|
|||
|
|
{
|
|||
|
|
case DEHY_NEW_STATE_ACCEL:
|
|||
|
|
if (dehy_new_set_speed >= target_speed)
|
|||
|
|
{
|
|||
|
|
dehy_new_set_speed = target_speed;
|
|||
|
|
dehy_new_state = DEHY_NEW_STATE_HOLD_TARGET;
|
|||
|
|
dehy_new_last_tick = now_ms;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
elapsed_ms = (uint32_t)(now_ms - dehy_new_last_tick);
|
|||
|
|
if (elapsed_ms < DEHY_SPEED_UPDATE_MS)
|
|||
|
|
{
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
step_count = elapsed_ms / DEHY_SPEED_UPDATE_MS;
|
|||
|
|
dehy_new_last_tick += (step_count * DEHY_SPEED_UPDATE_MS);
|
|||
|
|
|
|||
|
|
next_speed = (uint32_t)dehy_new_set_speed + ((uint32_t)acc_val * step_count);
|
|||
|
|
if (next_speed > (uint32_t)target_speed)
|
|||
|
|
{
|
|||
|
|
next_speed = (uint32_t)target_speed;
|
|||
|
|
}
|
|||
|
|
if (next_speed > DEHY_SPEED_MAX)
|
|||
|
|
{
|
|||
|
|
next_speed = DEHY_SPEED_MAX;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
dehy_new_set_speed = (uint16_t)next_speed;
|
|||
|
|
if (dehy_new_set_speed >= target_speed)
|
|||
|
|
{
|
|||
|
|
dehy_new_set_speed = target_speed;
|
|||
|
|
dehy_new_state = DEHY_NEW_STATE_HOLD_TARGET;
|
|||
|
|
}
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
case DEHY_NEW_STATE_HOLD_CURRENT:
|
|||
|
|
/* Guard path, usually returned above. */
|
|||
|
|
dehy_new_set_speed = dehy_new_hold_speed;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
case DEHY_NEW_STATE_HOLD_TARGET:
|
|||
|
|
dehy_new_set_speed = target_speed;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
default:
|
|||
|
|
dehy_new_state = DEHY_NEW_STATE_ACCEL;
|
|||
|
|
dehy_new_last_tick = now_ms;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
speed_set_val = dehy_new_set_speed;
|
|||
|
|
return dehy_new_set_speed;
|
|||
|
|
}
|