128 lines
5.4 KiB
C
128 lines
5.4 KiB
C
|
||
/*
|
||
|
||
当前速度current_speed取值范围为0至800， 目标速度target_speed为0、400、500、600、700、800这几种数值。这个函数的作用是下设速度（算出预定速度result_speed后，下设这个预定速度给speed_set_val）。外部的电机会根据speed_set_val来让current_speed达到speed_set_val的速度，但也有可能会因为自身过流等故障，current_speed会掉下去。有以下要求：
|
||
1. result_speed的初值设为此时的current_speed，然后每秒根据加速度acc_val的值往上加。如果加速度为acc_val，预定速度为result_speed，那么1秒后，预定速度为 result_speed = result_speed+acc_val，再过一秒后result_speed继续加acc_val，若result_speed超过了target_speed，则result_speed改为目标速度，然后result_speed一直保持在target_speed；
|
||
2. 若target_speed突然变小，如果current_speed大于新的target_speed，则需要获取当前速度，result_speed一直保持在当前速度不改变；否则若当前速度小于等于新的目标速度，那不用管，result_speed继续根据加速度a来加；
|
||
3. 若target_speed突然变大，则result_speed继续根据加速度acc_val来增加，直到到达target_speed。
|
||
4. 无论什么情况，如果target_speed为0，那么result_speed就要变成0.
|
||
5. 外部可以留个接口，初始化这个状态机。
|
||
6. 除非停机，否则result_speed不要下降，只能不变或者上升。
|
||
*/
|
||
|
||
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;
|
||
}
|
||
} |