robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

添加跳跃标志

Browse Source
This commit is contained in:
Robofish 2026-01-14 10:50:31 +08:00
parent 5abd099f6a
commit 8330656915
8 changed files with 305 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -62,6 +62,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/component/error_detect.c
User/component/filter.c
User/component/freertos_cli.c
User/component/limiter.c
User/component/lqr.c
User/component/pid.c
User/component/user_math.c

3
User/component/component_config.yaml
View File

@ -19,6 +19,9 @@ filter:
freertos_cli:
dependencies: []
enabled: true
limiter:
dependencies: []
enabled: true
pid:
dependencies:
- component/filter

167
User/component/limiter.c Normal file
View File

@ -0,0 +1,167 @@
/*
限制器
*/
#include "limiter.h"
#include "user_math.h"
#include <math.h>
#include <stddef.h>
#define POWER_BUFF_THRESHOLD 20
#define CHASSIS_POWER_CHECK_FREQ 10
#define CHASSIS_POWER_FACTOR_PASS 0.9f
#define CHASSIS_POWER_FACTOR_NO_PASS 1.5f
#define CHASSIS_MOTOR_CIRCUMFERENCE 0.12f
/**
* @brief 限制底盘功率不超过power_limit
*
* @param power_limit 最大功率
* @param motor_out 电机输出值
* @param speed 电机转速
* @param len 电机数量
* @return int8_t 0对应没有错误
*/
int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out,
float *speed, uint32_t len) {
/* power_limit小于0时不进行限制 */
if (motor_out == NULL || speed == NULL || power_limit < 0) return -1;
float sum_motor_out = 0.0f;
for (uint32_t i = 0; i < len; i++) {
/* 总功率计算 P=F(由转矩电流表示)*V(由转速表示) */
sum_motor_out +=
fabsf(motor_out[i]) * fabsf(speed[i]) * CHASSIS_MOTOR_CIRCUMFERENCE;
}
/* 保持每个电机输出值缩小时比例不变 */
if (sum_motor_out > power_limit) {
for (uint32_t i = 0; i < len; i++) {
motor_out[i] *= power_limit / sum_motor_out;
}
}
return 0;
}
/**
* @brief 电容输入功率计算
*
* @param power_in 底盘当前功率
* @param power_limit 裁判系统功率限制值
* @param power_buffer 缓冲能量
* @return float 裁判系统输出最大值
*/
float PowerLimit_CapInput(float power_in, float power_limit,
float power_buffer) {
float target_power = 0.0f;
/* 计算下一个检测周期的剩余缓冲能量 */
float heat_buff = power_buffer - (float)(power_in - power_limit) /
(float)CHASSIS_POWER_CHECK_FREQ;
if (heat_buff < POWER_BUFF_THRESHOLD) { /* 功率限制 */
target_power = power_limit * CHASSIS_POWER_FACTOR_PASS;
} else {
target_power = power_limit * CHASSIS_POWER_FACTOR_NO_PASS;
}
return target_power;
}
/**
* @brief 使用缓冲能量计算底盘最大功率
*
* @param power_limit 裁判系统功率限制值
* @param power_buffer 缓冲能量
* @return float 底盘输出最大值
*/
float PowerLimit_TargetPower(float power_limit, float power_buffer) {
float target_power = 0.0f;
/* 根据剩余缓冲能量计算输出功率 */
target_power = power_limit * (power_buffer - 10.0f) / 20.0f;
if (target_power < 0.0f) target_power = 0.0f;
return target_power;
}
/**
* @brief 射击频率控制
*
* @param heat 当前热量
* @param heat_limit 热量上限
* @param cooling_rate 冷却速率
* @param heat_increase 冷却增加
* @param shoot_freq 经过热量限制后的射击频率
* @return float 射击频率
*/
float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
float heat_increase, bool is_big) {
float heat_percent = heat / heat_limit;
float stable_freq = cooling_rate / heat_increase;
if (is_big)
return stable_freq;
else
return (heat_percent > 0.7f) ? stable_freq : 3.0f * stable_freq;
}
/**
* @brief 计算电机与IMU坐标系之间的偏差（处理跨越±π的情况）
* @param motor_angle 电机角度
* @param imu_angle IMU角度
* @return 偏差值（已归一化到 -π ~ π）
*/
static float CalcMotorImuOffset(float motor_angle, float imu_angle) {
float offset = motor_angle - imu_angle;
if (offset > M_PI) offset -= M_2PI;
if (offset < -M_PI) offset += M_2PI;
return offset;
}
/**
* @brief 圆周角度限位器 - 考虑电机与IMU坐标系偏差
*/
void CircleAngleLimit(float *setpoint, float motor_angle, float imu_angle,
float limit_max, float limit_min, float range) {
if (setpoint == NULL) return;
/* 计算电机与IMU坐标系偏差 */
float motor_imu_offset = CalcMotorImuOffset(motor_angle, imu_angle);
/* 将IMU setpoint转换为电机角度后进行限位检查 */
float motor_target = *setpoint + motor_imu_offset;
/* 检查是否超过最大限位 */
if (CircleError(motor_target, limit_max, range) > 0) {
*setpoint = limit_max - motor_imu_offset;
}
/* 检查是否低于最小限位 */
if (CircleError(motor_target, limit_min, range) < 0) {
*setpoint = limit_min - motor_imu_offset;
}
}
/**
* @brief 圆周角度增量限位器 - 考虑电机与IMU坐标系偏差
*/
void CircleAngleDeltaLimit(float *delta, float setpoint, float motor_angle,
float imu_angle, float limit_max, float limit_min,
float range) {
if (delta == NULL) return;
/* 计算电机与IMU坐标系偏差 */
float motor_imu_offset = CalcMotorImuOffset(motor_angle, imu_angle);
/* 计算应用增量后在电机坐标系下的目标角度 */
float motor_target_after_delta = setpoint + motor_imu_offset + *delta;
/* 计算到限位边界的距离 */
float delta_max = CircleError(limit_max, motor_target_after_delta, range);
float delta_min = CircleError(limit_min, motor_target_after_delta, range);
/* 限制增量 */
if (*delta > delta_max) *delta = delta_max;
if (*delta < delta_min) *delta = delta_min;
}

93
User/component/limiter.h Normal file
View File

@ -0,0 +1,93 @@
/*
限制器
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stdint.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/**
* @brief 限制底盘功率不超过power_limit
*
* @param power_limit 最大功率
* @param motor_out 电机输出值
* @param speed 电机转速
* @param len 电机数量
* @return int8_t 0对应没有错误
*/
int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out,
float *speed, uint32_t len);
/**
* @brief 电容输入功率计算
*
* @param power_in 底盘当前功率
* @param power_limit 裁判系统功率限制值
* @param power_buffer 缓冲能量
* @return float 裁判系统输出最大值
*/
float PowerLimit_CapInput(float power_in, float power_limit,
float power_buffer);
/**
* @brief 使用缓冲能量计算底盘最大功率
*
* @param power_limit 裁判系统功率限制值
* @param power_buffer 缓冲能量
* @return float 底盘输出最大值
*/
float PowerLimit_TargetPower(float power_limit, float power_buffer);
/**
* @brief 射击频率控制
*
* @param heat 当前热量
* @param heat_limit 热量上限
* @param cooling_rate 冷却速率
* @param heat_increase 冷却增加
* @param shoot_freq 经过热量限制后的射击频率
* @return float 射击频率
*/
float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
float heat_increase, bool is_big);
/**
* @brief 圆周角度限位器 - 考虑电机与IMU坐标系偏差
* @note 用于将 IMU 坐标系下的目标角度限制在电机坐标系的限位范围内
*
* @param setpoint 目标角度（IMU坐标系），会被直接修改
* @param motor_angle 电机当前角度
* @param imu_angle IMU当前角度
* @param limit_max 电机坐标系下的最大限位值
* @param limit_min 电机坐标系下的最小限位值
* @param range 角度循环范围（通常为 M_2PI）
*/
void CircleAngleLimit(float *setpoint, float motor_angle, float imu_angle,
float limit_max, float limit_min, float range);
/**
* @brief 圆周角度增量限位器 - 考虑电机与IMU坐标系偏差
* @note 用于限制增量控制时不超过电机限位范围
*
* @param delta 增量值，会被直接修改
* @param setpoint 当前目标角度（IMU坐标系）
* @param motor_angle 电机当前角度
* @param imu_angle IMU当前角度
* @param limit_max 电机坐标系下的最大限位值
* @param limit_min 电机坐标系下的最小限位值
* @param range 角度循环范围（通常为 M_2PI）
*/
void CircleAngleDeltaLimit(float *delta, float setpoint, float motor_angle,
float imu_angle, float limit_max, float limit_min,
float range);

1
User/module/balance_chassis.h
View File

@ -56,6 +56,7 @@ typedef struct {
Chassis_Mode_t mode; /* 底盘模式 */
MoveVector_t move_vec; /* 底盘运动向量 */
float height; /* 目标高度 */
bool jump_trigger; /* 跳跃触发标志 */
} Chassis_CMD_t;
typedef struct {

78
User/module/gimbal.c
View File

@ -8,6 +8,7 @@
#include "bsp/time.h"
#include <math.h>
#include "component/filter.h"
#include "component/limiter.h"
#include "component/pid.h"
#include "device/gimbal_imu.h"
#include "device/motor_dm.h"
@ -173,45 +174,23 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
Gimbal_SetMode(g, g_cmd->mode);
/* 处理yaw控制命令，软件限位 - 使用电机绝对角度 */
/* 处理yaw控制命令，软件限位 */
float delta_yaw = g_cmd->delta_yaw * g->dt * 1.5f;
if (g->param->travel.yaw > 0) {
/* 计算当前电机角度与IMU角度的偏差 */
float motor_imu_offset = g->feedback.motor.yaw.rotor_abs_angle - g->feedback.imu.eulr.yaw;
/* 处理跨越±π的情况 */
if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;
/* 计算到限位边界的距离 */
const float delta_max = CircleError(g->limit.yaw.max,
(g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
const float delta_min = CircleError(g->limit.yaw.min,
(g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
/* 限制控制命令 */
if (delta_yaw > delta_max) delta_yaw = delta_max;
if (delta_yaw < delta_min) delta_yaw = delta_min;
CircleAngleDeltaLimit(&delta_yaw, g->setpoint.eulr.yaw,
g->feedback.motor.yaw.rotor_abs_angle,
g->feedback.imu.eulr.yaw,
g->limit.yaw.max, g->limit.yaw.min, M_2PI);
}
CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw, M_2PI);
/* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */
/* 处理pitch控制命令，软件限位 */
float delta_pit = g_cmd->delta_pit * g->dt;
if (g->param->travel.pit > 0) {
/* 计算当前电机角度与IMU角度的偏差 */
float motor_imu_offset = g->feedback.motor.pit.rotor_abs_angle - g->feedback.imu.eulr.rol;
/* 处理跨越±π的情况 */
if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;
/* 计算到限位边界的距离 */
const float delta_max = CircleError(g->limit.pit.max,
(g->setpoint.eulr.pit + motor_imu_offset + delta_pit), M_2PI);
const float delta_min = CircleError(g->limit.pit.min,
(g->setpoint.eulr.pit + motor_imu_offset + delta_pit), M_2PI);
/* 限制控制命令 */
if (delta_pit > delta_max) delta_pit = delta_max;
if (delta_pit < delta_min) delta_pit = delta_min;
CircleAngleDeltaLimit(&delta_pit, g->setpoint.eulr.pit,
g->feedback.motor.pit.rotor_abs_angle,
g->feedback.imu.eulr.rol,
g->limit.pit.max, g->limit.pit.min, M_2PI);
}
CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI);
@ -227,35 +206,18 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
g->setpoint.eulr.yaw = g_ai->yaw;
g->setpoint.eulr.pit = -g_ai->pit;
/* 限位处理 - 需要考虑电机和IMU之间的偏差 */
/* 限位处理 */
if (g->param->travel.yaw > 0) {
float yaw_motor_imu_offset = g->feedback.motor.yaw.rotor_abs_angle - g->feedback.imu.eulr.yaw;
if (yaw_motor_imu_offset > M_PI) yaw_motor_imu_offset -= M_2PI;
if (yaw_motor_imu_offset < -M_PI) yaw_motor_imu_offset += M_2PI;
/* 将IMU setpoint转换为电机角度后进行限位检查 */
float yaw_motor_target = g->setpoint.eulr.yaw + yaw_motor_imu_offset;
if (CircleError(yaw_motor_target, g->limit.yaw.max, M_2PI) > 0) {
g->setpoint.eulr.yaw = g->limit.yaw.max - yaw_motor_imu_offset;
}
if (CircleError(yaw_motor_target, g->limit.yaw.min, M_2PI) < 0) {
g->setpoint.eulr.yaw = g->limit.yaw.min - yaw_motor_imu_offset;
}
CircleAngleLimit(&g->setpoint.eulr.yaw,
g->feedback.motor.yaw.rotor_abs_angle,
g->feedback.imu.eulr.yaw,
g->limit.yaw.max, g->limit.yaw.min, M_2PI);
}
if (g->param->travel.pit > 0) {
float pit_motor_imu_offset = g->feedback.motor.pit.rotor_abs_angle - g->feedback.imu.eulr.rol;
if (pit_motor_imu_offset > M_PI) pit_motor_imu_offset -= M_2PI;
if (pit_motor_imu_offset < -M_PI) pit_motor_imu_offset += M_2PI;
/* 将IMU setpoint转换为电机角度后进行限位检查 */
float pit_motor_target = g->setpoint.eulr.pit + pit_motor_imu_offset;
if (CircleError(pit_motor_target, g->limit.pit.max, M_2PI) > 0) {
g->setpoint.eulr.pit = g->limit.pit.max - pit_motor_imu_offset;
}
if (CircleError(pit_motor_target, g->limit.pit.min, M_2PI) < 0) {
g->setpoint.eulr.pit = g->limit.pit.min - pit_motor_imu_offset;
}
CircleAngleLimit(&g->setpoint.eulr.pit,
g->feedback.motor.pit.rotor_abs_angle,
g->feedback.imu.eulr.rol,
g->limit.pit.max, g->limit.pit.min, M_2PI);
}
}
/* fallthrough - AI控制模式也需要执行PID计算 */

6
User/task/ctrl_chassis.c
View File

@ -62,6 +62,12 @@ void Task_ctrl_chassis(void *argument) {
Chassis_UpdateFeedback(&chassis);
/* 检查跳跃触发 */
if (chassis_cmd.jump_trigger) {
Chassis_TriggerJump(&chassis);
chassis_cmd.jump_trigger = false; /* 清除触发标志 */
}
Chassis_Control(&chassis, &chassis_cmd);
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */

14
User/task/rc.c
View File

@ -113,6 +113,20 @@ void Task_rc(void *argument) {
osMessageQueuePut(task_runtime.msgq.gimbal.cmd, &cmd_for_gimbal, 0, 0);
/************************* 底盘命令 **************************************/
/* 跳跃触发检测：ch_res 从 -1.0f 松开回 0 时触发 */
static bool ch_res_was_min = false; /* 记录上次是否在最低位置 */
const float CH_RES_MIN_THRESHOLD = -0.9f; /* 判定为最低位置的阈值 */
const float CH_RES_RELEASE_THRESHOLD = -0.3f; /* 判定为松开的阈值 */
cmd_for_chassis.jump_trigger = false; /* 默认不触发 */
if (dr16.data.ch_res < CH_RES_MIN_THRESHOLD) {
ch_res_was_min = true; /* 记录已到达最低位置 */
} else if (ch_res_was_min && dr16.data.ch_res > CH_RES_RELEASE_THRESHOLD) {
/* 从最低位置松开，触发跳跃 */
cmd_for_chassis.jump_trigger = true;
ch_res_was_min = false; /* 重置状态 */
}
switch (dr16.data.sw_l) {
case DR16_SW_UP:
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;