379 lines
14 KiB
C
379 lines
14 KiB
C
#include "mw_dehy_test.h"
|
||
#include "mw_soft_timer.h"
|
||
#include "string.h"
|
||
|
||
/*
|
||
|
||
当前速度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不要下降，只能不变或者上升。
|
||
*/
|
||
|
||
#ifndef ENABLE
|
||
#define ENABLE (1u)
|
||
#endif
|
||
#ifndef DISABLE
|
||
#define DISABLE (0u)
|
||
#endif
|
||
|
||
/* 脱水速度设置 枚举变量 */
|
||
typedef enum
|
||
{
|
||
DEHY_SPEEDSET_INIT, // 初始化
|
||
DEHY_SPEEDSET_SPEED_UP, // 加速
|
||
DEHY_SPEEDSET_SPEED_KEEP, // 稳速
|
||
DEHY_SPEEDSET_SPEED_STOP, // 超震停机
|
||
|
||
DEHY_SPEEDSET_NUM
|
||
}SPEEDUP_ENUM_T;
|
||
|
||
typedef enum
|
||
{
|
||
SUPERSHAKE_LEVEL_MID_DEHY, // 超震等级 中脱
|
||
SUPERSHAKE_LEVEL_HIGH1_DEHY, // 超震等级 高脱1
|
||
SUPERSHAKE_LEVEL_HIGH2_DEHY, // 超震等级 高脱2
|
||
SUPERSHAKE_LEVEL_HIGH3_DEHY, // 超震等级 高脱3
|
||
SUPERSHAKE_LEVEL_HIGH4_DEHY, // 超震等级 高脱4
|
||
|
||
SUPERSHAKE_LEVEL_NUM
|
||
|
||
}SUPERSHAKE_LEVEL_ENUM_T;
|
||
|
||
|
||
// 超震记录单元
|
||
typedef struct
|
||
{
|
||
/* 基础参数 */
|
||
uint16_t imu_set; // 超震值设定
|
||
uint16_t over_judge_time; // 超标判断时间
|
||
uint16_t recover_judge_time; // 恢复判断时间
|
||
uint16_t speed_limit; // 速度限制
|
||
/* 状态计数 */
|
||
uint16_t over_time_cnt; // 恢复时间计数
|
||
uint16_t recover_time_cnt; // 恢复时间计数
|
||
}SUPERSHAKE_RECORD_STR_T;
|
||
|
||
// 超震限速枚举变量
|
||
typedef enum
|
||
{
|
||
SUPERSHAKE_LIMIT_INIT, // 初始化
|
||
// SUPERSHAKE_LIMIT_IDLE, // 空闲 不检测超震
|
||
SUPERSHAKE_LIMIT_DETECT, // 检测
|
||
|
||
|
||
}SUPERSHAKE_LIMIT_STATE_ENUM_T;
|
||
|
||
// static uint16_t supershake_limit_speed_val = 0; // 超震限制速度值
|
||
static uint16_t speed_set_val = 0u; // 外部调用 最终的速度设置值
|
||
static uint16_t rt_imu_data = 0; // 实时imu数据
|
||
|
||
static SUPERSHAKE_RECORD_STR_T supershake_record[SUPERSHAKE_LEVEL_NUM] = {0}; // 超震变速记录
|
||
|
||
|
||
/* 中脱转速 */
|
||
static uint16_t get_set_mid_dehy_speed_value(void){
|
||
return 400;
|
||
}
|
||
/* 高脱1转速 */
|
||
static uint16_t get_set_high_1_dehy_speed_value(void){
|
||
return 500;
|
||
}
|
||
/* 高脱2转速 */
|
||
static uint16_t get_set_high_2_dehy_speed_value(void){
|
||
return 600;
|
||
}
|
||
/* 高脱3转速 */
|
||
static uint16_t get_set_high_3_dehy_speed_value(void){
|
||
return 700;
|
||
}
|
||
/* 高脱4转速 */
|
||
static uint16_t get_set_high_4_dehy_speed_value(void){
|
||
return 800;
|
||
}
|
||
|
||
|
||
/* 中脱超震值 */
|
||
static uint16_t get_set_mid_dehy_supershake_value(void){
|
||
return 500;
|
||
}
|
||
/* 高脱1超震值 */
|
||
static uint16_t get_set_high_1_dehy_supershake_value(void){
|
||
return 400;
|
||
}
|
||
/* 高脱2超震值 */
|
||
static uint16_t get_set_high_2_dehy_supershake_value(void){
|
||
return 300;
|
||
}
|
||
/* 高脱3超震值 */
|
||
static uint16_t get_set_high_3_dehy_supershake_value(void){
|
||
return 200;
|
||
}
|
||
/* 高脱4超震值 */
|
||
static uint16_t get_set_high_4_dehy_supershake_value(void){
|
||
return 100;
|
||
}
|
||
|
||
/* 获取 实时imu数据 */
|
||
static uint16_t get_imu_data(void)
|
||
{
|
||
return rt_imu_data;
|
||
}
|
||
/*************************************************************************************
|
||
* @brief 初始化 超震记录
|
||
*
|
||
* @warning
|
||
* @note
|
||
*************************************************************************************/
|
||
static void supershake_record_init(void)
|
||
{
|
||
memset((uint8_t *)supershake_record, 0x00, sizeof(supershake_record));
|
||
/* 中脱初始化 */
|
||
supershake_record[SUPERSHAKE_LEVEL_MID_DEHY].imu_set = get_set_mid_dehy_supershake_value();
|
||
supershake_record[SUPERSHAKE_LEVEL_MID_DEHY].speed_limit = get_set_mid_dehy_speed_value();
|
||
supershake_record[SUPERSHAKE_LEVEL_MID_DEHY].over_judge_time = 5;
|
||
supershake_record[SUPERSHAKE_LEVEL_MID_DEHY].recover_judge_time = 10;
|
||
/* 高脱1初始化 */
|
||
supershake_record[SUPERSHAKE_LEVEL_HIGH1_DEHY].imu_set = get_set_high_1_dehy_supershake_value();
|
||
supershake_record[SUPERSHAKE_LEVEL_HIGH1_DEHY].speed_limit = get_set_high_1_dehy_speed_value();
|
||
supershake_record[SUPERSHAKE_LEVEL_HIGH1_DEHY].over_judge_time = 5;
|
||
supershake_record[SUPERSHAKE_LEVEL_HIGH1_DEHY].recover_judge_time = 10;
|
||
/* 高脱2初始化 */
|
||
supershake_record[SUPERSHAKE_LEVEL_HIGH2_DEHY].imu_set = get_set_high_2_dehy_supershake_value();
|
||
supershake_record[SUPERSHAKE_LEVEL_HIGH2_DEHY].speed_limit = get_set_high_2_dehy_speed_value();
|
||
supershake_record[SUPERSHAKE_LEVEL_HIGH2_DEHY].over_judge_time = 5;
|
||
supershake_record[SUPERSHAKE_LEVEL_HIGH2_DEHY].recover_judge_time = 10;
|
||
/* 高脱3初始化 */
|
||
supershake_record[SUPERSHAKE_LEVEL_HIGH3_DEHY].imu_set = get_set_high_3_dehy_supershake_value();
|
||
supershake_record[SUPERSHAKE_LEVEL_HIGH3_DEHY].speed_limit = get_set_high_3_dehy_speed_value();
|
||
supershake_record[SUPERSHAKE_LEVEL_HIGH3_DEHY].over_judge_time = 5;
|
||
supershake_record[SUPERSHAKE_LEVEL_HIGH3_DEHY].recover_judge_time = 10;
|
||
/* 高脱4初始化 */
|
||
supershake_record[SUPERSHAKE_LEVEL_HIGH4_DEHY].imu_set = get_set_high_4_dehy_supershake_value();
|
||
supershake_record[SUPERSHAKE_LEVEL_HIGH4_DEHY].speed_limit = get_set_high_4_dehy_speed_value();
|
||
supershake_record[SUPERSHAKE_LEVEL_HIGH4_DEHY].over_judge_time = 5;
|
||
supershake_record[SUPERSHAKE_LEVEL_HIGH4_DEHY].recover_judge_time = 10;
|
||
|
||
}
|
||
|
||
|
||
/*************************************************************************************
|
||
* @brief 超震速度限制逻辑
|
||
* @return uint16_t 超震限制的速度
|
||
*
|
||
* @warning
|
||
* @note
|
||
*************************************************************************************/
|
||
uint16_t supershake_speed_limit(LOGIC_STATE_ENUM_T cmd)
|
||
{
|
||
static SUPERSHAKE_LIMIT_STATE_ENUM_T supershake_limit_speed_state = SUPERSHAKE_LIMIT_INIT; // 超震限速逻辑状态机
|
||
static uint32_t timetick = 0; // 时间戳
|
||
uint16_t realtime_imu_val = 0; // 超震实时值
|
||
SUPERSHAKE_LEVEL_ENUM_T index_i = (SUPERSHAKE_LEVEL_ENUM_T)0;
|
||
uint16_t supershake_recover_speed = 0; // 恢复速度
|
||
static uint16_t ret_limit_speed = 0; // 对外输出的超震限制速度值
|
||
|
||
/* 外部开关 */
|
||
if(cmd == LOGIC_INIT)
|
||
{
|
||
supershake_limit_speed_state = SUPERSHAKE_LIMIT_INIT;
|
||
}
|
||
|
||
|
||
switch(supershake_limit_speed_state)
|
||
{
|
||
case SUPERSHAKE_LIMIT_INIT:
|
||
supershake_record_init();
|
||
supershake_limit_speed_state = SUPERSHAKE_LIMIT_DETECT;
|
||
ret_limit_speed = get_set_high_4_dehy_speed_value();
|
||
break;
|
||
case SUPERSHAKE_LIMIT_DETECT:
|
||
realtime_imu_val = get_imu_data();
|
||
/* 检测 可指定周期 */
|
||
for(index_i = (SUPERSHAKE_LEVEL_ENUM_T)0; index_i < SUPERSHAKE_LEVEL_NUM; ++index_i)
|
||
{
|
||
if(realtime_imu_val <= supershake_record[index_i].imu_set)
|
||
{
|
||
supershake_record[index_i].over_time_cnt = 0;
|
||
}
|
||
else
|
||
{
|
||
supershake_record[index_i].recover_time_cnt = 0;
|
||
}
|
||
}
|
||
// 每秒处理事情
|
||
if(get_systick_ms() - timetick >= 1000u)
|
||
{
|
||
timetick = get_systick_ms();
|
||
|
||
for(index_i = (SUPERSHAKE_LEVEL_ENUM_T)0; index_i < SUPERSHAKE_LEVEL_NUM; ++index_i)
|
||
{
|
||
// 更新 超震cnt 恢复cnt
|
||
if(supershake_record[index_i].over_time_cnt < 60)
|
||
{
|
||
(supershake_record[index_i].over_time_cnt)++;
|
||
}
|
||
if(supershake_record[index_i].recover_time_cnt < 60)
|
||
{
|
||
(supershake_record[index_i].recover_time_cnt)++;
|
||
}
|
||
/* 平稳 恢复速度 */
|
||
if(supershake_record[index_i].recover_time_cnt >= supershake_record[index_i].recover_judge_time)
|
||
{
|
||
if(ret_limit_speed < supershake_record[index_i].speed_limit)
|
||
{
|
||
ret_limit_speed = supershake_record[index_i].speed_limit;
|
||
}
|
||
}
|
||
/* 超震 限速 */
|
||
if(supershake_record[index_i].over_time_cnt >= supershake_record[index_i].over_judge_time)
|
||
{
|
||
if(index_i == (SUPERSHAKE_LEVEL_ENUM_T)0)
|
||
{
|
||
ret_limit_speed = 0;
|
||
}
|
||
else
|
||
{
|
||
if(ret_limit_speed > supershake_record[index_i-1].speed_limit)
|
||
{
|
||
ret_limit_speed = supershake_record[index_i-1].speed_limit;
|
||
}
|
||
// ret_limit_speed = supershake_record[index_i-1].speed_limit;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
break;
|
||
}
|
||
|
||
return ret_limit_speed;
|
||
}
|
||
|
||
|
||
|
||
/*************************************************************************************
|
||
* @brief 下设速度
|
||
* @param[in/out] val
|
||
*
|
||
* @warning
|
||
* @note
|
||
*************************************************************************************/
|
||
void motor_speed_set(uint16_t val)
|
||
{
|
||
speed_set_val = val;
|
||
}
|
||
|
||
/*************************************************************************************
|
||
* @brief 提速逻辑
|
||
* @param[in/out] init_cmd
|
||
* @param[in/out] acc_val
|
||
* @param[in/out] current_speed
|
||
* @param[in/out] target_speed
|
||
*
|
||
* @warning
|
||
* @note
|
||
*************************************************************************************/
|
||
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;
|
||
// 进入下一阶段
|
||
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();
|
||
|
||
// 若当前速度 小于 现在的目标速度 则没事 更新一下目标速度缓存 当前就不加速了
|
||
// 当前速度 不小于 新的目标速度 则稳住当前速度 进入稳态
|
||
if((target_speed < target_speed_cache) && (current_speed >= target_speed))
|
||
{
|
||
// 记录当前速度
|
||
result_speed = current_speed;
|
||
// 切到速度保持
|
||
add_speed_state = DEHY_SPEEDSET_SPEED_KEEP;
|
||
}
|
||
else
|
||
{
|
||
// 更新 预设速度
|
||
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;
|
||
}
|
||
break;
|
||
// 超震停机
|
||
case DEHY_SPEEDSET_SPEED_STOP:
|
||
result_speed = 0;
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
/* 设置速度 */
|
||
motor_speed_set(result_speed);
|
||
}
|