Falling_jasmine/FDR-Core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

1.0

Browse Source
This commit is contained in:
nitiantuhao
2026-03-28 13:48:31 +08:00
parent ca76b115ea
commit 0e9f24d3f6
10 changed files with 329 additions and 248 deletions
Download Patch File Download Diff File Expand all files Collapse all files

211
Core/Src/ladrc.c
View File

@@ -1,30 +1,27 @@
#include "ladrc.h"
#include "f4_can_app.h"
/*
* 这个文件实现两层内容：
* 1) 单个电机的 LADRC 控制器。
* 2) 四轮底盘的调度层（读取编码器 -> 滤波 -> LADRC -> PWM 输出）。
*
* 额外升级点：
* - 提供目标 RPM / 控制输出 getter，供上层故障诊断与状态上报使用。
*/
#define LADRC_CONTROL_DT_S 0.005f
#define LADRC_RPM_FILTER_ALPHA 0.20f
#define LADRC_CONTROL_DT_S 0.01f
#define LADRC_RPM_FILTER_ALPHA 0.30f
/* 这组只是保守起始值，b0 仍建议后续辨识 */
#define LADRC_DEFAULT_WC 12.0f
#define LADRC_DEFAULT_WO 25.0f
#define LADRC_DEFAULT_B0 0.80f
#define LADRC_DEFAULT_OUT_MAX 1000.0f
#define LADRC_DEFAULT_WC 20.0f
#define LADRC_DEFAULT_WO 25.0f
#define LADRC_DEFAULT_B0 0.40f
#define LADRC_DEFAULT_OUT_MAX 1000.0f
/* 把目标爬坡放到控制循环内部，避免依赖外部调用频率 */
#define LADRC_TARGET_SLEW_RPM_PER_S 40.0f
/* ================== 内部辅助函数 ================== */
static float LADRC_Abs(float x);
static float LADRC_Clamp(float x, float min_value, float max_value);
static float Ramp_To_Target(float curr, float target, float step);
static void LADRC_ResetStates(LADRC_TypeDef *ladrc);
static float s_filtered_rpm[4] = {0.0f, 0.0f, 0.0f, 0.0f};
LADRC_TypeDef ladrc_motors[4];
static float LADRC_Abs(float x)
{
return (x >= 0.0f) ? x : -x;
@@ -43,6 +40,19 @@ static float LADRC_Clamp(float x, float min_value, float max_value)
return x;
}
static float Ramp_To_Target(float curr, float target, float step)
{
if (target > curr + step)
{
return curr + step;
}
if (target < curr - step)
{
return curr - step;
}
return target;
}
static void LADRC_ResetStates(LADRC_TypeDef *ladrc)
{
if (ladrc == NULL)
@@ -50,19 +60,21 @@ static void LADRC_ResetStates(LADRC_TypeDef *ladrc)
return;
}
ladrc->v = 0.0f;
ladrc->y = 0.0f;
ladrc->z1 = 0.0f;
ladrc->z2 = 0.0f;
ladrc->u = 0.0f;
ladrc->v_cmd = 0.0f;
ladrc->v = 0.0f;
ladrc->y = 0.0f;
ladrc->z1 = 0.0f;
ladrc->z2 = 0.0f;
ladrc->u = 0.0f;
ladrc->eu_prev = 0.0f;
}
/**
* @brief 初始化单个 LADRC 控制器。
* @note 顺手做基础参数保护，避免 b0/h/out_max 为 0 或负值时出问题。
*/
void LADRC_Init(LADRC_TypeDef *ladrc, float wc, float wo, float b0, float h, float max)
{
float h_beta1_2;
float h2_beta2_4;
float delta;
if (ladrc == NULL)
{
return;
@@ -76,6 +88,10 @@ void LADRC_Init(LADRC_TypeDef *ladrc, float wc, float wo, float b0, float h, flo
{
b0 = LADRC_DEFAULT_B0;
}
if (b0 < 0.0f)
{
b0 = -b0;
}
if (wc < 0.0f)
{
wc = -wc;
@@ -89,33 +105,46 @@ void LADRC_Init(LADRC_TypeDef *ladrc, float wc, float wo, float b0, float h, flo
max = LADRC_DEFAULT_OUT_MAX;
}
ladrc->v = 0.0f; /* 目标值 */
ladrc->y = 0.0f; /* 测量值 */
ladrc->z1 = 0.0f; /* 状态观测量 */
ladrc->z2 = 0.0f; /* 总扰动观测量 */
ladrc->u = 0.0f; /* 上一拍控制输出 */
LADRC_ResetStates(ladrc);
ladrc->wc = wc;
ladrc->wo = wo;
ladrc->b0 = b0;
ladrc->h = h;
ladrc->wc = wc;
ladrc->wo = wo;
ladrc->b0 = b0;
ladrc->h = h;
ladrc->out_max = max;
ladrc->beta1 = 2.0f * wo;
ladrc->beta2 = wo * wo;
h_beta1_2 = h * ladrc->beta1 * 0.5f;
h2_beta2_4 = h * h * ladrc->beta2 * 0.25f;
delta = 1.0f + h_beta1_2 + h2_beta2_4;
/* x(k+1) = Ad*x(k) + Bdu*u(k) + Bdy*y(k) + Bdaw*eu(k-1) */
ladrc->Ad11 = (1.0f - h_beta1_2 - h2_beta2_4) / delta;
ladrc->Ad12 = h / delta;
ladrc->Ad21 = (-h * ladrc->beta2) / delta;
ladrc->Ad22 = (1.0f + h_beta1_2 - h2_beta2_4) / delta;
ladrc->Bdu1 = (h * ladrc->b0) / delta;
ladrc->Bdu2 = (-h * h * ladrc->beta2 * ladrc->b0 * 0.5f) / delta;
ladrc->Bdy1 = (h * (ladrc->beta1 + h * ladrc->beta2 * 0.5f)) / delta;
ladrc->Bdy2 = (h * ladrc->beta2) / delta;
/* e_u = u_sat - u_raw; 取负增益让饱和时对 z2 形成恢复作用 */
ladrc->kaw = -(ladrc->wo * ladrc->b0);
ladrc->Bdaw1 = (0.5f * h * h * ladrc->kaw) / delta;
ladrc->Bdaw2 = (h * (1.0f + h_beta1_2) * ladrc->kaw) / delta;
}
/**
* @brief 单个 LADRC 周期运算。
* @param ladrc 控制器实例
* @param actual_val 当前测量速度（RPM）
* @retval 限幅后的控制输出（通常可直接映射到 PWM）
*/
float LADRC_Calc(LADRC_TypeDef *ladrc, float actual_val)
{
float e;
float z1_new;
float z2_new;
float u0;
float out;
float u_raw;
float u_sat;
if (ladrc == NULL)
{
@@ -124,31 +153,29 @@ float LADRC_Calc(LADRC_TypeDef *ladrc, float actual_val)
ladrc->y = actual_val;
/* 第 1 部分：LESO（线性扩张状态观测器） */
e = ladrc->y - ladrc->z1;
ladrc->z1 += (ladrc->z2 + ladrc->b0 * ladrc->u + ladrc->beta1 * e) * ladrc->h;
ladrc->z2 += (ladrc->beta2 * e) * ladrc->h;
/* LESO + anti-windup 输入统一在同一套 Tustin 离散模型里 */
z1_new = ladrc->Ad11 * ladrc->z1 + ladrc->Ad12 * ladrc->z2
+ ladrc->Bdy1 * ladrc->y + ladrc->Bdu1 * ladrc->u
+ ladrc->Bdaw1 * ladrc->eu_prev;
/* 第 2 部分：LSEF + 扰动补偿 */
u0 = ladrc->wc * (ladrc->v - ladrc->z1);
out = (u0 - ladrc->z2) / ladrc->b0;
z2_new = ladrc->Ad21 * ladrc->z1 + ladrc->Ad22 * ladrc->z2
+ ladrc->Bdy2 * ladrc->y + ladrc->Bdu2 * ladrc->u
+ ladrc->Bdaw2 * ladrc->eu_prev;
/* 第 3 部分：输出限幅，保护电机和驱动 */
out = LADRC_Clamp(out, -ladrc->out_max, ladrc->out_max);
ladrc->z1 = z1_new;
ladrc->z2 = z2_new;
ladrc->u = out;
return out;
u0 = ladrc->wc * (ladrc->v - ladrc->z1);
u_raw = (u0 - ladrc->z2) / ladrc->b0;
u_sat = LADRC_Clamp(u_raw, -ladrc->out_max, ladrc->out_max);
/* 当前拍算出的饱和误差，下一拍再经 Bdaw 进入观测器 */
ladrc->eu_prev = u_sat - u_raw;
ladrc->u = u_sat;
return u_sat;
}
/* =====================================================================
* 针对四轮底盘的 LADRC 调度层
* ===================================================================== */
LADRC_TypeDef ladrc_motors[4];
/**
* @brief 一键初始化底层硬件和四个 LADRC 控制器。
*/
void FourWheel_LADRC_Init(void)
{
int i;
@@ -180,40 +207,20 @@ void FourWheel_LADRC_ResetAll(void)
Motor_Brake_All();
}
/**
* @brief 设置四个轮子的目标 RPM。
*/
#define LADRC_TARGET_SLEW_RPM_PER_S 80.0f // 目标斜率，先试 80 rpm/s
static float Ramp_To_Target(float curr, float target, float step)
{
if (target > curr + step) return curr + step;
if (target < curr - step) return curr - step;
return target;
}
void FourWheel_Set_Target_RPM(float fl_rpm, float rl_rpm, float fr_rpm, float rr_rpm)
{
const float step = LADRC_TARGET_SLEW_RPM_PER_S * LADRC_CONTROL_DT_S;
ladrc_motors[MOTOR_FL].v = Ramp_To_Target(ladrc_motors[MOTOR_FL].v, fl_rpm, step);
ladrc_motors[MOTOR_RL].v = Ramp_To_Target(ladrc_motors[MOTOR_RL].v, rl_rpm, step);
ladrc_motors[MOTOR_FR].v = Ramp_To_Target(ladrc_motors[MOTOR_FR].v, fr_rpm, step);
ladrc_motors[MOTOR_RR].v = Ramp_To_Target(ladrc_motors[MOTOR_RR].v, rr_rpm, step);
ladrc_motors[MOTOR_FL].v_cmd = fl_rpm;
ladrc_motors[MOTOR_RL].v_cmd = rl_rpm;
ladrc_motors[MOTOR_FR].v_cmd = fr_rpm;
ladrc_motors[MOTOR_RR].v_cmd = rr_rpm;
}
/**
* @brief 四轮 LADRC 大循环，建议固定 10ms 调用一次。
* @note 调度顺序：
* 1) 更新编码器测速
* 2) 一阶低通滤波，减小测速毛刺
* 3) 每个轮子独立做 LADRC
* 4) 输出到 H 桥
*/
void FourWheel_LADRC_Control_Loop(void)
{
int i;
const float ref_step = LADRC_TARGET_SLEW_RPM_PER_S * LADRC_CONTROL_DT_S;
/* 5ms 调度；内部速度估计窗口在 motor.c 里做成 10ms */
Motor_Update_RPM(LADRC_CONTROL_DT_S);
if (g_robot_ctrl.state != SYSTEM_OPERATIONAL)
@@ -224,21 +231,23 @@ void FourWheel_LADRC_Control_Loop(void)
for (i = 0; i < 4; ++i)
{
float raw_rpm = Get_Motor_RPM((Motor_ID_t)i);
float raw_rpm;
float pwm_out;
s_filtered_rpm[i] = (1.0f - LADRC_RPM_FILTER_ALPHA) * s_filtered_rpm[i]
+ LADRC_RPM_FILTER_ALPHA * raw_rpm;
ladrc_motors[i].v = Ramp_To_Target(ladrc_motors[i].v,
ladrc_motors[i].v_cmd,
ref_step);
raw_rpm = Get_Motor_RPM((Motor_ID_t)i);
/* 外部低通保留，但减为“轻滤波” */
s_filtered_rpm[i] += LADRC_RPM_FILTER_ALPHA * (raw_rpm - s_filtered_rpm[i]);
pwm_out = LADRC_Calc(&ladrc_motors[i], s_filtered_rpm[i]);
Set_Motor_Output((Motor_ID_t)i, (int16_t)pwm_out);
}
}
/**
* @brief 读取指定轮子的“当前目标 RPM”。
* @note 该接口主要供上层做状态上报与故障诊断使用。
*/
float FourWheel_Get_Target_RPM(Motor_ID_t id)
{
if ((id < MOTOR_FL) || (id > MOTOR_RR))
@@ -246,13 +255,19 @@ float FourWheel_Get_Target_RPM(Motor_ID_t id)
return 0.0f;
}
return ladrc_motors[id].v_cmd;
}
float FourWheel_Get_Ref_RPM(Motor_ID_t id)
{
if ((id < MOTOR_FL) || (id > MOTOR_RR))
{
return 0.0f;
}
return ladrc_motors[id].v;
}
/**
* @brief 读取指定轮子的“当前控制输出”。
* @note 该值尚未经过电机左右镜像翻转，但足够用于判断是否长期顶满。
*/
float FourWheel_Get_Control_Output(Motor_ID_t id)
{
if ((id < MOTOR_FL) || (id > MOTOR_RR))

38
Core/Src/main.c
View File

@@ -135,16 +135,23 @@ int main(void)
MX_TIM6_Init();
MX_CAN1_Init();
MX_UART4_Init();
MX_TIM7_Init();
/* USER CODE BEGIN 2 */
CAN_App_Init();
FourWheel_LADRC_Init();
/* 启动 10ms 硬实时控制节拍。 */
/* 启动 10ms 慢速/诊断节拍 */
if (HAL_TIM_Base_Start_IT(&htim6) != HAL_OK)
{
Error_Handler();
}
/* 启动 5ms 硬实时底盘控制节拍 */
if (HAL_TIM_Base_Start_IT(&htim7) != HAL_OK)
{
Error_Handler();
}
/* USER CODE END 2 */
/* Infinite loop */
@@ -242,22 +249,29 @@ void SystemClock_Config(void)
}
/* USER CODE BEGIN 4 */
/**
* @brief 定时器周期中断回调。
* @note TIM6 是整个底盘控制的“硬实时心跳”。
* 固定顺序：
* 1) 控制命令看门狗
* 2) 差速逆解算
* 3) 四轮 LADRC 闭环
* 4) 基于最新反馈做故障诊断
*/
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if (htim->Instance == TIM6)
/* ==================================================
* [快循环] TIM7: 5ms 专用于底盘高频硬实时控制
* ================================================== */
if (htim->Instance == TIM7)
{
CAN_Safety_Watchdog_Tick();
/* 1. 运动学逆解算：根据上层下发的底盘 vx,vy,w 计算四个轮子的目标 RPM */
Kinematics_Update_LADRC();
/* 2. 四轮 LADRC 闭环：获取编码器 -> 滤波 -> 运算 -> 输出 PWM */
FourWheel_LADRC_Control_Loop();
}
/* ==================================================
* [慢循环] TIM6: 10ms 专用于状态机、看门狗与故障诊断
* ================================================== */
else if (htim->Instance == TIM6)
{
/* 1. 检查上位机/遥控器指令是否超时 */
CAN_Safety_Watchdog_Tick();
/* 2. 基于快循环更新的状态（如超调、堵转、离线等）做诊断 */
Chassis_Diagnostics_10ms_Tick();
}
}

110
Core/Src/motor.c
View File

@@ -5,22 +5,15 @@
#include "tim.h"
/*
* 这个文件负责两件事：
* 1) 把 LADRC 输出的有符号控制量，转换成 H 桥两路 PWM
* 2) 读取四路编码器，得到实时 RPM，并累加成供 CAN 上传的里程计增量
*
* 约定：
* - 上层统一把“车体前进”视为正方向
* - 由于左右电机安装方向镜像，左侧电机的硬件方向需要在底层翻转
*/
/* ================== 全局静态变量 ================== */
static uint16_t s_last_count[4] = {0U, 0U, 0U, 0U};
static volatile float s_current_rpm[4] = {0.0f, 0.0f, 0.0f, 0.0f};
static volatile int32_t s_odom_acc_ticks[4] = {0, 0, 0, 0};
/* ================== 内部辅助函数声明 ================== */
/* 速度估计滚动窗口：降低 5ms 单拍增量测速的量化噪声 */
static int32_t s_speed_acc_ticks[4] = {0, 0, 0, 0};
static uint8_t s_speed_window_count = 0U;
static float s_speed_window_dt = 0.0f;
static inline void Motor_WriteBridge(TIM_HandleTypeDef *htim,
uint32_t channel_in1,
uint32_t channel_in2,
@@ -30,12 +23,6 @@ static void Motor_PrimeEncoderCounters(void);
static uint16_t Motor_ClampAbsToPwm(int32_t value);
static int16_t Motor_SaturateToI16(int32_t value);
/**
* @brief 给 H 桥两路输入分别写入 PWM。
* @note AT8236 方向约定：
* - 正转：IN1 = PWM，IN2 = 0
* - 反转：IN1 = 0， IN2 = PWM
*/
static inline void Motor_WriteBridge(TIM_HandleTypeDef *htim,
uint32_t channel_in1,
uint32_t channel_in2,
@@ -46,9 +33,6 @@ static inline void Motor_WriteBridge(TIM_HandleTypeDef *htim,
__HAL_TIM_SET_COMPARE(htim, channel_in2, pwm_in2);
}
/**
* @brief 把任意有符号数限幅并取绝对值，转换成 PWM 占空比。
*/
static uint16_t Motor_ClampAbsToPwm(int32_t value)
{
if (value > PWM_LIMIT)
@@ -68,9 +52,6 @@ static uint16_t Motor_ClampAbsToPwm(int32_t value)
return (uint16_t)value;
}
/**
* @brief int32 饱和到 int16，防止 CAN 打包时溢出回绕。
*/
static int16_t Motor_SaturateToI16(int32_t value)
{
if (value > 32767)
@@ -84,10 +65,6 @@ static int16_t Motor_SaturateToI16(int32_t value)
return (int16_t)value;
}
/**
* @brief 在编码器启动后，用当前计数值作为初值。
* @note 这样可以避免“last_count 初始为 0”导致首个控制周期出现巨大假速度。
*/
static void Motor_PrimeEncoderCounters(void)
{
s_last_count[MOTOR_FL] = (uint16_t)__HAL_TIM_GET_COUNTER(&htim5);
@@ -96,9 +73,6 @@ static void Motor_PrimeEncoderCounters(void)
s_last_count[MOTOR_RR] = (uint16_t)__HAL_TIM_GET_COUNTER(&htim1);
}
/**
* @brief 启动所有 PWM 和编码器接口。
*/
void Motor_Init(void)
{
uint32_t primask = __get_PRIMASK();
@@ -117,7 +91,6 @@ void Motor_Init(void)
if (HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL) != HAL_OK) { Error_Handler(); }
if (HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_ALL) != HAL_OK) { Error_Handler(); }
/* 启动完成后，把输出和测速状态清零。 */
Motor_Brake_All();
Motor_PrimeEncoderCounters();
@@ -126,31 +99,27 @@ void Motor_Init(void)
s_current_rpm[MOTOR_RL] = 0.0f;
s_current_rpm[MOTOR_FR] = 0.0f;
s_current_rpm[MOTOR_RR] = 0.0f;
s_odom_acc_ticks[MOTOR_FL] = 0;
s_odom_acc_ticks[MOTOR_RL] = 0;
s_odom_acc_ticks[MOTOR_FR] = 0;
s_odom_acc_ticks[MOTOR_RR] = 0;
s_speed_acc_ticks[MOTOR_FL] = 0;
s_speed_acc_ticks[MOTOR_RL] = 0;
s_speed_acc_ticks[MOTOR_FR] = 0;
s_speed_acc_ticks[MOTOR_RR] = 0;
s_speed_window_count = 0U;
s_speed_window_dt = 0.0f;
__set_PRIMASK(primask);
}
/**
* @brief 把有符号控制量转换为电机驱动输出。
* @param id 电机编号
* @param control_out 上层控制量，正值表示“希望车体前进”
* @note
* 1) 这里先按“车体坐标系”理解 control_out。
* 2) 左侧电机由于安装镜像，硬件方向要翻转一次。
* 3) 变量 forward 的语义始终保持一致：
* true -> IN1 = PWM, IN2 = 0
* false -> IN1 = 0, IN2 = PWM
*/
void Set_Motor_Output(Motor_ID_t id, int16_t control_out)
{
int32_t hw_command = (int32_t)control_out;
bool forward;
uint16_t pwm_val;
/* 左侧硬件方向镜像：同样的“车体前进”命令，左轮需要反向转。 */
if ((id == MOTOR_FL) || (id == MOTOR_RL))
{
hw_command = -hw_command;
@@ -210,12 +179,6 @@ void Set_Motor_Output(Motor_ID_t id, int16_t control_out)
}
}
/**
* @brief 停止四个电机输出。
* @note 这里使用“两个方向脚都置 0”的空转停车方式。
* 如果你的驱动板支持真正的短刹车，并且你确认接线与驱动逻辑，
* 可以单独再加一个 Brake 模式。
*/
void Motor_Brake_All(void)
{
Motor_WriteBridge(&htim2, TIM_CHANNEL_3, TIM_CHANNEL_4, 0U, 0U);
@@ -224,13 +187,6 @@ void Motor_Brake_All(void)
Motor_WriteBridge(&htim8, TIM_CHANNEL_3, TIM_CHANNEL_4, 0U, 0U);
}
/**
* @brief 读取编码器脉冲并更新实时转速。
* @param dt_s 控制周期，单位秒，例如 10ms 就传 0.01f
* @note
* - 利用 int16_t 差分天然处理 16 位编码器计数器回绕
* - 左侧编码器输入在这里反相，保证上层统一把“车体前进”看成正 RPM
*/
void Motor_Update_RPM(float dt_s)
{
uint16_t curr_count_fl;
@@ -239,6 +195,7 @@ void Motor_Update_RPM(float dt_s)
uint16_t curr_count_rr;
int16_t delta[4];
float rpm_scale;
int i;
if (dt_s <= 0.0f)
{
@@ -260,27 +217,37 @@ void Motor_Update_RPM(float dt_s)
s_last_count[MOTOR_FR] = curr_count_fr;
s_last_count[MOTOR_RR] = curr_count_rr;
/* 左侧输入镜像修正：让“车体前进”统一表现为正脉冲 / 正 RPM。 */
delta[MOTOR_FL] = (int16_t)(-delta[MOTOR_FL]);
delta[MOTOR_RL] = (int16_t)(-delta[MOTOR_RL]);
// delta[MOTOR_FR] = (int16_t)(-delta[MOTOR_FR]);
// delta[MOTOR_RR] = (int16_t)(-delta[MOTOR_RR]);
/* 用 32 位内部累加，避免偶发高速度时 16 位回绕。 */
s_odom_acc_ticks[MOTOR_FL] += delta[MOTOR_FL];
s_odom_acc_ticks[MOTOR_RL] += delta[MOTOR_RL];
s_odom_acc_ticks[MOTOR_FR] += delta[MOTOR_FR];
s_odom_acc_ticks[MOTOR_RR] += delta[MOTOR_RR];
rpm_scale = 60.0f / ((float)PULSES_PER_REV * dt_s);
s_current_rpm[MOTOR_FL] = (float)delta[MOTOR_FL] * rpm_scale;
s_current_rpm[MOTOR_RL] = (float)delta[MOTOR_RL] * rpm_scale;
s_current_rpm[MOTOR_FR] = (float)delta[MOTOR_FR] * rpm_scale;
s_current_rpm[MOTOR_RR] = (float)delta[MOTOR_RR] * rpm_scale;
/* 速度估计窗口累加 */
s_speed_acc_ticks[MOTOR_FL] += delta[MOTOR_FL];
s_speed_acc_ticks[MOTOR_RL] += delta[MOTOR_RL];
s_speed_acc_ticks[MOTOR_FR] += delta[MOTOR_FR];
s_speed_acc_ticks[MOTOR_RR] += delta[MOTOR_RR];
s_speed_window_dt += dt_s;
s_speed_window_count++;
if (s_speed_window_count >= MOTOR_SPEED_WINDOW_SAMPLES)
{
rpm_scale = 60.0f / ((float)PULSES_PER_REV * s_speed_window_dt);
for (i = 0; i < 4; ++i)
{
s_current_rpm[i] = (float)s_speed_acc_ticks[i] * rpm_scale;
s_speed_acc_ticks[i] = 0;
}
s_speed_window_count = 0U;
s_speed_window_dt = 0.0f;
}
}
/**
* @brief 获取某个电机的实时 RPM。
*/
float Get_Motor_RPM(Motor_ID_t id)
{
if ((id < MOTOR_FL) || (id > MOTOR_RR))
@@ -291,11 +258,6 @@ float Get_Motor_RPM(Motor_ID_t id)
return s_current_rpm[id];
}
/**
* @brief 读取并清空供 CAN 上传的里程计增量。
* @note 这个函数会被主循环调用，而累加动作发生在 10ms 中断里，
* 所以“读取 + 清零”必须放在同一个临界区，避免丢计数。
*/
void Motor_Get_And_Clear_Delta_Ticks(int16_t *d_fl, int16_t *d_rl, int16_t *d_fr, int16_t *d_rr)
{
int32_t fl;

15
Core/Src/stm32f4xx_it.c
View File

@@ -58,6 +58,7 @@
extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
extern CAN_HandleTypeDef hcan1;
extern TIM_HandleTypeDef htim6;
extern TIM_HandleTypeDef htim7;
/* USER CODE BEGIN EV */
/* USER CODE END EV */
@@ -242,6 +243,20 @@ void TIM6_DAC_IRQHandler(void)
/* USER CODE END TIM6_DAC_IRQn 1 */
}
/**
* @brief This function handles TIM7 global interrupt.
*/
void TIM7_IRQHandler(void)
{
/* USER CODE BEGIN TIM7_IRQn 0 */
/* USER CODE END TIM7_IRQn 0 */
HAL_TIM_IRQHandler(&htim7);
/* USER CODE BEGIN TIM7_IRQn 1 */
/* USER CODE END TIM7_IRQn 1 */
}
/**
* @brief This function handles USB On The Go FS global interrupt.
*/

63
Core/Src/tim.c
View File

@@ -30,6 +30,7 @@ TIM_HandleTypeDef htim3;
TIM_HandleTypeDef htim4;
TIM_HandleTypeDef htim5;
TIM_HandleTypeDef htim6;
TIM_HandleTypeDef htim7;
TIM_HandleTypeDef htim8;
TIM_HandleTypeDef htim9;
@@ -290,6 +291,39 @@ void MX_TIM6_Init(void)
/* USER CODE END TIM6_Init 2 */
}
/* TIM7 init function */
void MX_TIM7_Init(void)
{
/* USER CODE BEGIN TIM7_Init 0 */
/* USER CODE END TIM7_Init 0 */
TIM_MasterConfigTypeDef sMasterConfig = {0};
/* USER CODE BEGIN TIM7_Init 1 */
/* USER CODE END TIM7_Init 1 */
htim7.Instance = TIM7;
htim7.Init.Prescaler = 84-1;
htim7.Init.CounterMode = TIM_COUNTERMODE_UP;
htim7.Init.Period = 5000-1;
htim7.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim7) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim7, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM7_Init 2 */
/* USER CODE END TIM7_Init 2 */
}
/* TIM8 init function */
void MX_TIM8_Init(void)
@@ -563,6 +597,21 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
/* USER CODE END TIM6_MspInit 1 */
}
else if(tim_baseHandle->Instance==TIM7)
{
/* USER CODE BEGIN TIM7_MspInit 0 */
/* USER CODE END TIM7_MspInit 0 */
/* TIM7 clock enable */
__HAL_RCC_TIM7_CLK_ENABLE();
/* TIM7 interrupt Init */
HAL_NVIC_SetPriority(TIM7_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(TIM7_IRQn);
/* USER CODE BEGIN TIM7_MspInit 1 */
/* USER CODE END TIM7_MspInit 1 */
}
}
void HAL_TIM_MspPostInit(TIM_HandleTypeDef* timHandle)
{
@@ -770,6 +819,20 @@ void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle)
/* USER CODE END TIM6_MspDeInit 1 */
}
else if(tim_baseHandle->Instance==TIM7)
{
/* USER CODE BEGIN TIM7_MspDeInit 0 */
/* USER CODE END TIM7_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_TIM7_CLK_DISABLE();
/* TIM7 interrupt Deinit */
HAL_NVIC_DisableIRQ(TIM7_IRQn);
/* USER CODE BEGIN TIM7_MspDeInit 1 */
/* USER CODE END TIM7_MspDeInit 1 */
}
}
/* USER CODE BEGIN 1 */