#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); }