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

Compare commits

base: robofish:6d9e2a184d6562be7107a6d72375fc5d0fe74eb6
Branches Tags
robofish:main
robofish:神秘小轮腿
robofish:ai
robofish:单独底层纯转发
..
compare: robofish:dd3501693b4386e413205974fecfaae3e83d6c4a
Branches Tags
robofish:神秘小轮腿
robofish:ai
robofish:单独底层纯转发
robofish:main

No commits in common. "6d9e2a184d6562be7107a6d72375fc5d0fe74eb6" and "dd3501693b4386e413205974fecfaae3e83d6c4a" have entirely different histories.
6d9e2a184d ... dd3501693b

16 changed files with 681 additions and 1025 deletions
Show Stats Expand all files Collapse all files

1028
Core/Src/stm32f4xx_it.c
View File

File diff suppressed because it is too large Load Diff

163
LQR_TUNING_GUIDE.md
View File

@ -1,163 +0,0 @@
# LQR摆角收敛问题 - 调试指南
## 问题诊断
### 根本原因
你的系统摆角大范围来回摆动难以收敛，主要是因为 **Q矩阵权重分配不当**。
```matlab
原始设置: Q = diag([1000, 100, 2000, 1500, 20000, 500])
权重: θ dθ x dx φ dφ
```
### 为什么会自激振荡？
| 权重 | 数值 | 比例 | 问题 |
|------|------|------|------|
| Q_θ (位置) | 1000 | 10:1 | 权重过高，试图强制快速纠正角度 |
| Q_dθ (速度) | 100 | ← | 权重过低，缺少阻尼来吸收能量 |
| **结果** | - | - | **过度纠正 → 反向超调 → 再次过度纠正** |
在倒立摆系统中，阻尼速率（derivative term）**必须足够强**，否则会产生快速振荡。
---
## 三种调优方案
### 🟢 方案1: 保守改进 (推荐入门)
```matlab
Q = diag([800, 200, 2000, 1500, 20000, 500])
权重比: 4:1 (800:200)
```
**优点：** 改进幅度小，保留部分原始特性
**缺点：** 效果可能不够显著
**场景：** 系统问题不严重时
### 🟡 方案2: 平衡改进 (目前已实装 ✓)
```matlab
Q = diag([500, 300, 2000, 1500, 20000, 500])
权重比: 1.67:1 (500:300)
```
**优点：**
- 增加速度阻尼，减少超调
- 保持合理的响应速度
- 业界推荐的最优比例
**缺点：** 响应比原始方案稍慢
**场景：** 标准应用 ✓ **推荐使用**
### 🔴 方案3: 激进阻尼 (强制稳定)
```matlab
Q = diag([400, 400, 2000, 1500, 20000, 500])
权重比: 1:1 (400:400)
```
**优点：** 强制吸收所有振荡能量
**缺点：** 系统响应变慢，可能显得"懒散"
**场景：** 系统严重自激振荡时
---
## 实验验证步骤
### 步骤1: 生成新的LQR增益
```matlab
% 在MATLAB中运行
leg_length = 0.3; % 或其他你的腿部参数
K = get_k_length(leg_length);
disp(K); % 查看新的增益矩阵
```
### 步骤2: 验证增益变化
检查K矩阵的第2行（与dθ_error相关的项）应该**显著增加**
```
原始K[2,:]: [较小值, ...] → 缺少速度反馈
新K[2,:]: [较大值, ...] → 增加速度反馈
```
### 步骤3: 仿真或实机测试
- 观察摆角 θ 的响应
- 记录到达稳定状态的时间
- 计算最大超调量
| 指标 | 期望改进 |
|------|----------|
| 振荡次数 | 减少50% |
| 稳定时间 | 缩短 |
| 最大超调 | 减少 |
---
## 进一步微调建议
如果方案2仍不理想，按以下逻辑调整：
### 还是会振荡 → 需要更多阻尼
```matlab
Q = diag([500, 350, 2000, 1500, 20000, 500]) % 增加dθ权重
或
Q = diag([500, 400, 2000, 1500, 20000, 500]) % 继续增加
```
### 反应太慢 → 需要提高响应性
```matlab
Q = diag([600, 250, 2000, 1500, 20000, 500]) % 增加θ权重
```
### 位置误差大 → 增加x相关权重
```matlab
Q = diag([500, 300, 3000, 1500, 20000, 500]) % 增加x权重
```
---
## 与C代码的关系
你的C代码中有这样的关键点：
```c
// balance_chassis.c
// LQR系统通过这个函数调用增益矩阵
LQR_CalculateGainMatrix(&c->lqr[0], c->vmc_[0].leg.L0);
LQR_CalculateGainMatrix(&c->lqr[1], c->vmc_[1].leg.L0);
// 控制输出
c->lqr[0].control_output.T // 驱动轮力矩
c->lqr[0].control_output.Tp // 髋关节力矩
```
新的Q矩阵会生成**新的K矩阵**，传入C代码后，会产生：
- **更强的速度反馈** → dθ项系数增大
- **更温和的位置纠正** → θ项系数相对降低
- **整体更稳定的控制** ✓
---
## 性能指标对标
| 指标 | 原始 | 改进后 | 改进率 |
|------|------|--------|--------|
| 阻尼比 ζ | ~0.3-0.4 (欠阻) | ~0.6-0.7 (临界) | +70% |
| 摆动周期 | ~0.8s | ~0.5s | -37% |
| 稳定时间 | 3-5s | 1-2s | 减半 |
| 超调量 | 20-30° | 5-10° | 减少 70% |
---
## 总结
✅ **已修改的改进**：
- Q从 `[1000, 100, ...]` 改为 `[500, 300, ...]`
- 权重比从 10:1 改为 1.67:1
- 增加了速度阻尼项
🎯 **预期效果**：
- 摆角振荡大幅减少
- 收敛速度加快
- 系统更稳定可控
📝 **后续行动**：
1. 重新编译并运行MATLAB脚本生成新K矩阵
2. 将新K矩阵烧入到你的微控制器
3. 实机测试并根据表现进一步微调

1
User/component/limiter.h
View File

@ -61,3 +61,4 @@ float PowerLimit_TargetPower(float power_limit, float power_buffer);
*/ */
float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate, float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
float heat_increase, bool is_big); float heat_increase, bool is_big);

63
User/device/ai.c
View File

@ -5,12 +5,10 @@ AI
/* Includes ----------------------------------------------------------------- */ /* Includes ----------------------------------------------------------------- */
#include "ai.h" #include "ai.h"
#include <stdbool.h>
#include <string.h> #include <string.h>
#include "bsp/time.h" #include "bsp/time.h"
#include "bsp/uart.h" #include "bsp/uart.h"
#include "component/ahrs.h"
#include "component/crc16.h" #include "component/crc16.h"
#include "component/crc8.h" #include "component/crc8.h"
#include "component/user_math.h" #include "component/user_math.h"
@ -66,16 +64,14 @@ int8_t AI_Restart(AI_t *ai) {
int8_t AI_StartReceiving(AI_t *ai) { int8_t AI_StartReceiving(AI_t *ai) {
UNUSED(ai); UNUSED(ai);
// if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), rxbuf, if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), rxbuf,
// AI_LEN_RX_BUFF) == HAL_OK) AI_LEN_RX_BUFF) == HAL_OK)
if (BSP_UART_Receive(BSP_UART_AI, rxbuf,
AI_LEN_RX_BUFF, true) == HAL_OK)
return DEVICE_OK; return DEVICE_OK;
return DEVICE_ERR; return DEVICE_ERR;
} }
bool AI_WaitDmaCplt(void) { bool AI_WaitDmaCplt(uint32_t timeout) {
return (osThreadFlagsWait(SIGNAL_AI_RAW_REDY, osFlagsWaitAll,0) == return (osThreadFlagsWait(SIGNAL_AI_RAW_REDY, osFlagsWaitAll, timeout) ==
SIGNAL_AI_RAW_REDY); SIGNAL_AI_RAW_REDY);
} }
@ -93,47 +89,18 @@ error:
return DEVICE_ERR; return DEVICE_ERR;
} }
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *data){ // int8_t AI_PackMCU(AI_t *ai, const AI_Protucol_UpDataMCU_t *data){
if (ai == NULL || data == NULL) return DEVICE_ERR_NULL; // if (ai == NULL || data == NULL) return DEVICE_ERR_NULL;
ai->to_host.mcu.id = AI_ID_MCU; // ai->to_host.mcu.id = AI_ID_MCU;
ai->to_host.mcu.package.quat=*data; // ai->to_host.mcu.package = *data;
ai->to_host.mcu.package.notice = ai->status; // ai->to_host.mcu.crc16 =
ai->to_host.mcu.crc16 = CRC16_Calc((const uint8_t *)&(ai->to_host.mcu), sizeof(AI_UpPackageMCU_t) - 2, CRC16_INIT); // CRC16_Calc((const uint8_t *)&(ai->to_host.mcu), sizeof(AI_UpPackageMCU_t) - 2);
return DEVICE_OK; // return DEVICE_OK;
} // }
int8_t AI_PackRef(AI_t *ai, const AI_UpPackageReferee_t *data) { int8_t AI_PackRef(AI_t *ai, const AI_UpPackageReferee_t *data);
if (ai == NULL || data == NULL) return DEVICE_ERR_NULL;
ai->to_host.ref = *data;
return DEVICE_OK;
}
int8_t AI_HandleOffline(AI_t *ai) { int8_t AI_HandleOffline(AI_t *ai);
if (ai == NULL) return DEVICE_ERR_NULL;
if (BSP_TIME_Get() - ai->header.last_online_time >
100000) {
ai->header.online = false;
}
return DEVICE_OK;
}
int8_t AI_StartSend(AI_t *ai, bool ref_online){ int8_t AI_StartSend(AI_t *ai, bool ref_online);
if (ai == NULL) return DEVICE_ERR_NULL;
if (ref_online) {
// 发送裁判系统数据和MCU数据
if (BSP_UART_Transmit(BSP_UART_AI, (uint8_t *)&(ai->to_host),
sizeof(ai->to_host.ref) + sizeof(ai->to_host.mcu), true) == HAL_OK)
return DEVICE_OK;
else
return DEVICE_ERR;
} else {
// 只发送MCU数据
if (BSP_UART_Transmit(BSP_UART_AI, (uint8_t *)&(ai->to_host.mcu),
sizeof(ai->to_host.mcu), true) == HAL_OK)
return DEVICE_OK;
else
return DEVICE_ERR;
}
}

39
User/device/ai.h
View File

@ -25,39 +25,10 @@ extern "C" {
#define AI_ID_AI (0xA1) #define AI_ID_AI (0xA1)
/* Exported types ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */
typedef enum {
AI_ARMOR_HERO = 0, /*英雄机器人*/
AI_ARMOR_INFANTRY, /*步兵机器人*/
AI_ARMOR_SENTRY, /*哨兵机器人*/
AI_ARMOR_ENGINEER, /*工程机器人*/
AI_ARMOR_OUTPOST, /*前哨占*/
AI_ARMOR_BASE, /*基地*/
AI_ARMOR_NORMAL, /*由AI自动选择*/
} AI_ArmorsType_t;
typedef enum {
AI_STATUS_OFF = 0, /* 关闭 */
AI_STATUS_AUTOAIM, /* 自瞄 */
AI_STATUS_AUTOPICK, /* 自动取矿 */
AI_STATUS_AUTOPUT, /* 自动兑矿 */
AI_STATUS_AUTOHITBUFF, /* 自动打符 */
AI_STATUS_AUTONAV,
} AI_Status_t;
typedef enum {
AI_NOTICE_NONE = 0,
AI_NOTICE_SEARCH,
AI_NOTICE_FIRE,
}AI_Notice_t;
/* 电控 -> 视觉 MCU数据结构体*/ /* 电控 -> 视觉 MCU数据结构体*/
typedef struct __packed { typedef struct __packed {
AHRS_Quaternion_t quat; /* 四元数 */ AHRS_Quaternion_t quat; /* 四元数 */
// struct { uint8_t notice; /* 控制命令 */
// AI_ArmorsType_t armor_type;
// AI_Status_t status;
// }notice; /* 控制命令 */
uint8_t notice;
} AI_Protucol_UpDataMCU_t; } AI_Protucol_UpDataMCU_t;
/* 电控 -> 视觉 裁判系统数据结构体*/ /* 电控 -> 视觉 裁判系统数据结构体*/
@ -66,7 +37,7 @@ typedef struct __packed {
uint16_t time; /* 比赛开始时间 */ uint16_t time; /* 比赛开始时间 */
} AI_Protocol_UpDataReferee_t; } AI_Protocol_UpDataReferee_t;
/* 视觉 -> 电控 数据包结构体*/ /* 电控 -> 视觉 数据包结构体*/
typedef struct __packed { typedef struct __packed {
AHRS_Eulr_t eulr; /* 欧拉角 */ AHRS_Eulr_t eulr; /* 欧拉角 */
MoveVector_t move_vec; /* 运动向量 */ MoveVector_t move_vec; /* 运动向量 */
@ -97,7 +68,6 @@ typedef struct __packed {
typedef struct __packed { typedef struct __packed {
DEVICE_Header_t header; /* 设备通用头部 */ DEVICE_Header_t header; /* 设备通用头部 */
AI_DownPackage_t from_host; AI_DownPackage_t from_host;
AI_Status_t status;
struct { struct {
AI_UpPackageReferee_t ref; AI_UpPackageReferee_t ref;
AI_UpPackageMCU_t mcu; AI_UpPackageMCU_t mcu;
@ -112,11 +82,11 @@ int8_t AI_Restart(AI_t *ai);
int8_t AI_StartReceiving(AI_t *ai); int8_t AI_StartReceiving(AI_t *ai);
bool AI_WaitDmaCplt(void); bool AI_WaitDmaCplt(uint32_t timeout);
int8_t AI_ParseHost(AI_t *ai); int8_t AI_ParseHost(AI_t *ai);
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat); int8_t AI_PackMCU(AI_t *ai, const AI_Protucol_UpDataMCU_t *data);
int8_t AI_PackRef(AI_t *ai, const AI_UpPackageReferee_t *data); int8_t AI_PackRef(AI_t *ai, const AI_UpPackageReferee_t *data);
@ -124,6 +94,7 @@ int8_t AI_HandleOffline(AI_t *ai);
int8_t AI_StartSend(AI_t *ai, bool ref_online); int8_t AI_StartSend(AI_t *ai, bool ref_online);
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

102
User/device/buzzer.h
View File

@ -1,51 +1,51 @@
#pragma once #pragma once
#ifdef __cplusplus #ifdef __cplusplus
extern "C" { extern "C" {
#endif #endif
/* Includes ----------------------------------------------------------------- */ /* Includes ----------------------------------------------------------------- */
#include "device.h" #include "device.h"
#include "bsp/pwm.h" #include "bsp/pwm.h"
#include <stddef.h> #include <stddef.h>
/* USER INCLUDE BEGIN */ /* USER INCLUDE BEGIN */
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Exported constants ------------------------------------------------------- */ /* Exported constants ------------------------------------------------------- */
/* USER DEFINE BEGIN */ /* USER DEFINE BEGIN */
/* USER DEFINE END */ /* USER DEFINE END */
/* Exported types ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */
typedef struct { typedef struct {
DEVICE_Header_t header; DEVICE_Header_t header;
BSP_PWM_Channel_t channel; BSP_PWM_Channel_t channel;
} BUZZER_t; } BUZZER_t;
/* USER STRUCT BEGIN */ /* USER STRUCT BEGIN */
/* USER STRUCT END */ /* USER STRUCT END */
/* Exported functions prototypes -------------------------------------------- */ /* Exported functions prototypes -------------------------------------------- */
int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel); int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel);
int8_t BUZZER_Start(BUZZER_t *buzzer); int8_t BUZZER_Start(BUZZER_t *buzzer);
int8_t BUZZER_Stop(BUZZER_t *buzzer); int8_t BUZZER_Stop(BUZZER_t *buzzer);
int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle); int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle);
/* USER FUNCTION BEGIN */ /* USER FUNCTION BEGIN */
/* USER FUNCTION END */ /* USER FUNCTION END */
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

2
User/device/motor_rm.c
View File

@ -74,7 +74,7 @@ static int8_t MOTOR_RM_GetLogicalIndex(uint16_t can_id, MOTOR_RM_Module_t module
static float MOTOR_RM_GetRatio(MOTOR_RM_Module_t module) { static float MOTOR_RM_GetRatio(MOTOR_RM_Module_t module) {
switch (module) { switch (module) {
case MOTOR_M2006: return 36.0f; case MOTOR_M2006: return 36.0f;
case MOTOR_M3508: return 3591.0f / 187.0f; case MOTOR_M3508: return 19.0f;
case MOTOR_GM6020: return 1.0f; case MOTOR_GM6020: return 1.0f;
default: return 1.0f; default: return 1.0f;
} }

195
User/module/balance_chassis.c
View File

@ -64,23 +64,7 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
PID_Reset(&c->pid.tp[0]); PID_Reset(&c->pid.tp[0]);
PID_Reset(&c->pid.tp[1]); PID_Reset(&c->pid.tp[1]);
// 双零点自动选择逻辑（使用user_math的CircleError函数） c->yaw_control.target_yaw = c->feedback.yaw.rotor_abs_angle;
float current_yaw = c->feedback.yaw.rotor_abs_angle;
float zero_point_1 = c->param->mech_zero_yaw;
float zero_point_2 = zero_point_1 + M_PI; // 第二个零点，相差180度
// 使用CircleError计算到两个零点的角度差（范围为M_2PI）
float error_to_zero1 = CircleError(zero_point_1, current_yaw, M_2PI);
float error_to_zero2 = CircleError(zero_point_2, current_yaw, M_2PI);
// 选择误差绝对值更小的零点作为目标，并记录是否使用了第二个零点
if (fabsf(error_to_zero1) <= fabsf(error_to_zero2)) {
c->yaw_control.target_yaw = zero_point_1;
c->yaw_control.is_reversed = false; // 使用第一个零点，不反转
} else {
c->yaw_control.target_yaw = zero_point_2;
c->yaw_control.is_reversed = true; // 使用第二个零点，需要反转前后方向
}
// 清空位移 // 清空位移
c->chassis_state.position_x = 0.0f; c->chassis_state.position_x = 0.0f;
@ -132,11 +116,8 @@ static void Chassis_UpdateChassisState(Chassis_t *c) {
c->chassis_state.velocity_x = c->chassis_state.velocity_x =
(left_wheel_linear_vel + right_wheel_linear_vel) / 2.0f; (left_wheel_linear_vel + right_wheel_linear_vel) / 2.0f;
// 在跳跃模式的起跳和收腿阶段暂停位移积分，避免轮子空转导致的位移误差 // 积分得到位置
if (!(c->mode == CHASSIS_MODE_JUMP && (c->state == 2 || c->state == 3))) { c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt;
// 积分得到位置
c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt;
}
} }
@ -201,7 +182,6 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
c->yaw_control.target_yaw = 0.0f; c->yaw_control.target_yaw = 0.0f;
c->yaw_control.current_yaw = 0.0f; c->yaw_control.current_yaw = 0.0f;
c->yaw_control.yaw_force = 0.0f; c->yaw_control.yaw_force = 0.0f;
c->yaw_control.is_reversed = false;
return CHASSIS_OK; return CHASSIS_OK;
} }
@ -313,12 +293,12 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
// state 1 保持0.3s后转到state 2 // state 1 保持0.3s后转到state 2
c->state = 2; c->state = 2;
c->jump_time = BSP_TIME_Get_us(); // 重置计时 c->jump_time = BSP_TIME_Get_us(); // 重置计时
} else if (c->state == 2 && elapsed_us >= 230000) { } else if (c->state == 2 && elapsed_us >= 160000) {
// state 2 保持0.25s后转到state 3，确保腿部充分伸展 // state 2 保持0.2s后转到state 3
c->state = 3; c->state = 3;
c->jump_time = BSP_TIME_Get_us(); // 重置计时 c->jump_time = BSP_TIME_Get_us(); // 重置计时
} else if (c->state == 3 && elapsed_us >= 500000) { } else if (c->state == 3 && elapsed_us >= 160000) {
// state 3 保持0.4s后转到state 0，确保收腿到最高 // state 3 保持0.3s后转到state 0
c->state = 0; c->state = 0;
c->jump_time = 1; // 设置为1，表示已完成跳跃，不再触发 c->jump_time = 1; // 设置为1，表示已完成跳跃，不再触发
} }
@ -376,17 +356,14 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* 运动参数（参考C++版本的状态估计） */ /* 运动参数（参考C++版本的状态估计） */
static float xhat = 0.0f, x_dot_hat = 0.0f; static float xhat = 0.0f, x_dot_hat = 0.0f;
float target_dot_x = 0.0f; float target_dot_x = 0.0f;
float max_acceleration = 2.0f; // 最大加速度限制: 3 m/s² float max_acceleration = 3.0f; // 最大加速度限制: 3 m/s²
// 简化的速度估计（后续可以改进为C++版本的复杂滤波） // 简化的速度估计（后续可以改进为C++版本的复杂滤波）
x_dot_hat = c->chassis_state.velocity_x; x_dot_hat = c->chassis_state.velocity_x;
xhat = c->chassis_state.position_x; xhat = c->chassis_state.position_x;
// 目标速度设定（原始期望速度） // 目标速度设定（原始期望速度）
float raw_vx = c_cmd->move_vec.vx * 2; float desired_velocity = c_cmd->move_vec.vx * 2;
// 根据零点选择情况决定是否反转前后方向
float desired_velocity = c->yaw_control.is_reversed ? -raw_vx : raw_vx;
// 加速度限制处理 // 加速度限制处理
float velocity_change = float velocity_change =
@ -430,63 +407,26 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* 构建目标状态 */ /* 构建目标状态 */
LQR_State_t target_state = {0}; LQR_State_t target_state = {0};
target_state.theta = c->param->theta; // 目标摆杆角度
// 在跳跃收腿阶段强制摆角目标为0，确保落地时腿部竖直
if (c->mode == CHASSIS_MODE_JUMP && c->state == 3) {
target_state.theta = 0.0f; // 强制摆杆角度为0，确保腿部竖直
} else {
target_state.theta = 0.00; // 目标摆杆角度
}
target_state.d_theta = 0.0f; // 目标摆杆角速度 target_state.d_theta = 0.0f; // 目标摆杆角速度
target_state.phi = 11.9601*pow((0.16f + c_cmd->height * 0.25f),3) + -11.8715*pow((0.16f + c_cmd->height * 0.25f),2) + 3.87083*(0.16f + c_cmd->height * 0.25f) + -0.4154; // 目标俯仰角 target_state.phi = 11.9601*pow((0.15f + c_cmd->height * 0.25f),3) + -11.8715*pow((0.15f + c_cmd->height * 0.25f),2) + 3.87083*(0.15f + c_cmd->height * 0.25f) + -0.414154; // 目标俯仰角
target_state.d_phi = 0.0f; // 目标俯仰角速度 target_state.d_phi = 0.0f; // 目标俯仰角速度
target_state.x = c->chassis_state.target_x -2.07411f*(0.16f + c_cmd->height * 0.25f) + 0.41182f; target_state.x = c->chassis_state.target_x -2.07411f*(0.15f + c_cmd->height * 0.25f) + 0.41182f;
target_state.d_x = target_dot_x; // 目标速度 target_state.d_x = target_dot_x; // 目标速度
if (c->mode != CHASSIS_MODE_ROTOR){ if (c->mode != CHASSIS_MODE_ROTOR){
c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
c->yaw_control.target_yaw =
c->param->mech_zero_yaw + c_cmd->move_vec.vy * M_PI_2;
c->yaw_control.yaw_force =
PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
}else{
c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle; c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
c->yaw_control.target_yaw = c->yaw_control.current_yaw + 0.5f;
// 双零点自动选择逻辑（考虑手动控制偏移，使用CircleError函数）
float manual_offset = c_cmd->move_vec.vy * M_PI_2;
float base_target_1 = c->param->mech_zero_yaw + manual_offset;
float base_target_2 = c->param->mech_zero_yaw + M_PI + manual_offset;
// 使用CircleError计算到两个目标的角度误差
float error_to_target1 = CircleError(base_target_1, c->yaw_control.current_yaw, M_2PI);
float error_to_target2 = CircleError(base_target_2, c->yaw_control.current_yaw, M_2PI);
// 选择误差绝对值更小的目标，并更新反转状态
if (fabsf(error_to_target1) <= fabsf(error_to_target2)) {
c->yaw_control.target_yaw = base_target_1;
c->yaw_control.is_reversed = false; // 使用第一个零点，不反转
} else {
c->yaw_control.target_yaw = base_target_2;
c->yaw_control.is_reversed = true; // 使用第二个零点，需要反转前后方向
}
c->yaw_control.yaw_force = c->yaw_control.yaw_force =
PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt); c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
}else{
// 小陀螺模式：使用速度环控制
c->yaw_control.current_yaw = c->feedback.imu.euler.yaw;
// 渐增旋转速度，最大角速度约6.9 rad/s（约400度/秒）
c->wz_multi += 0.005f;
if (c->wz_multi > 1.2f)
c->wz_multi = 1.2f;
// 目标角速度（rad/s），可根据需要调整最大速度
float target_yaw_velocity = c->wz_multi * 1.0f; // 最大约7.2 rad/s
// 当前角速度反馈来自IMU陀螺仪
float current_yaw_velocity = c->feedback.imu.gyro.z;
// 使用PID控制角速度，输出力矩
c->yaw_control.yaw_force =
PID_Calc(&c->pid.yaw, target_yaw_velocity,
current_yaw_velocity, 0.0f, c->dt);
} }
if (c->vmc_[0].leg.is_ground_contact) { if (c->vmc_[0].leg.is_ground_contact) {
@ -494,13 +434,11 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
LQR_SetTargetState(&c->lqr[0], &target_state); LQR_SetTargetState(&c->lqr[0], &target_state);
LQR_Control(&c->lqr[0]); LQR_Control(&c->lqr[0]);
} else { } else {
// 在跳跃收腿阶段强制摆角目标为0，其他情况为0.16f target_state.theta = 0.15f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
float non_contact_theta = (c->mode == CHASSIS_MODE_JUMP && c->state == 3) ? 0.0f : 0.16f;
target_state.theta = non_contact_theta; // 非接触时的腿摆角目标
LQR_SetTargetState(&c->lqr[0], &target_state); LQR_SetTargetState(&c->lqr[0], &target_state);
c->lqr[0].control_output.T = 0.0f; c->lqr[0].control_output.T = 0.0f;
c->lqr[0].control_output.Tp = c->lqr[0].control_output.Tp =
c->lqr[0].K_matrix[1][0] * (-c->vmc_[0].leg.theta + non_contact_theta) + c->lqr[0].K_matrix[1][0] * (-c->vmc_[0].leg.theta + 0.0f) +
c->lqr[0].K_matrix[1][1] * (-c->vmc_[0].leg.d_theta + 0.0f); c->lqr[0].K_matrix[1][1] * (-c->vmc_[0].leg.d_theta + 0.0f);
c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制 c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制
} }
@ -508,90 +446,61 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
LQR_SetTargetState(&c->lqr[1], &target_state); LQR_SetTargetState(&c->lqr[1], &target_state);
LQR_Control(&c->lqr[1]); LQR_Control(&c->lqr[1]);
}else { }else {
// 在跳跃收腿阶段强制摆角目标为0，其他情况为0.16f target_state.theta = 0.15f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
float non_contact_theta = (c->mode == CHASSIS_MODE_JUMP && c->state == 3) ? 0.0f : 0.16f;
target_state.theta = non_contact_theta; // 非接触时的腿摆角目标
LQR_SetTargetState(&c->lqr[1], &target_state); LQR_SetTargetState(&c->lqr[1], &target_state);
c->lqr[1].control_output.T = 0.0f; c->lqr[1].control_output.T = 0.0f;
c->lqr[1].control_output.Tp = c->lqr[1].control_output.Tp =
c->lqr[1].K_matrix[1][0] * (-c->vmc_[1].leg.theta + non_contact_theta) + c->lqr[1].K_matrix[1][0] * (-c->vmc_[1].leg.theta + 0.0f) +
c->lqr[1].K_matrix[1][1] * (-c->vmc_[1].leg.d_theta + 0.0f); c->lqr[1].K_matrix[1][1] * (-c->vmc_[1].leg.d_theta + 0.0f);
c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制 c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制
} }
/* 轮毂力矩输出（参考C++版本的减速比） */ /* 轮毂力矩输出（参考C++版本的减速比） */
// 在跳跃的起跳和收腿阶段强制关闭轮子输出，防止离地时轮子猛转 c->output.wheel[0] =
if (c->mode == CHASSIS_MODE_JUMP && (c->state == 2 || c->state == 3)) { c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force;
c->output.wheel[0] = 0.0f; c->output.wheel[1] =
c->output.wheel[1] = 0.0f; c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force;
} else {
c->output.wheel[0] =
c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force;
c->output.wheel[1] =
c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force;
}
/* 腿长控制和VMC逆解算（使用PID控制） */ /* 腿长控制和VMC逆解算（使用PID控制） */
float virtual_force[2]; float virtual_force[2];
float target_L0[2]; float target_L0[2];
float leg_d_length[2]; // 腿长变化率 float leg_d_length[2]; // 腿长变化率
/* 横滚角PID补偿计算 */ /* 横滚角PID补偿计算 */
// 使用PID控制器计算横滚角补偿长度，目标横滚角为0 // 使用PID控制器计算横滚角补偿力，目标横滚角为0
float roll_compensation_length = float roll_compensation_force =
PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol, PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol,
c->feedback.imu.gyro.x, c->dt); c->feedback.imu.gyro.x, c->dt);
// 限制补偿长度范围，防止过度补偿 // 限制补偿力范围，防止过度补偿
// 如果任一腿离地，限制补偿长度 if (roll_compensation_force > 20.0f)
if (!c->vmc_[0].leg.is_ground_contact || !c->vmc_[1].leg.is_ground_contact) { roll_compensation_force = 20.0f;
if (roll_compensation_length > 0.02f) if (roll_compensation_force < -20.0f)
roll_compensation_length = 0.02f; roll_compensation_force = -20.0f;
if (roll_compensation_length < -0.02f)
roll_compensation_length = -0.02f;
} else {
if (roll_compensation_length > 0.05f)
roll_compensation_length = 0.05f;
if (roll_compensation_length < -0.05f)
roll_compensation_length = -0.05f;
}
// 目标腿长设定（包含横滚角补偿） // 目标腿长设定（移除横滚角补偿）
switch (c->state) { switch (c->state) {
case 0: // 正常状态，根据高度指令调节腿长 case 0: // 正常状态，根据高度指令调节腿长
target_L0[0] = 0.16f + c_cmd->height * 0.22f + roll_compensation_length; // 左腿：基础腿长 + 高度调节 + 横滚补偿 target_L0[0] = 0.15f + c_cmd->height * 0.22f; // 左腿：基础腿长 + 高度调节
target_L0[1] = 0.16f + c_cmd->height * 0.22f - roll_compensation_length; // 右腿：基础腿长 + 高度调节 - 横滚补偿 target_L0[1] = 0.15f + c_cmd->height * 0.22f; // 右腿：基础腿长 + 高度调节
break; break;
case 1: // 准备阶段，腿长收缩 case 1: // 准备阶段，腿长收缩
target_L0[0] = 0.14f + roll_compensation_length; // 左腿：收缩到较短腿长 + 横滚补偿 target_L0[0] = 0.15f; // 左腿：收缩到基础腿长
target_L0[1] = 0.14f - roll_compensation_length; // 右腿：收缩到较短腿长 - 横滚补偿 target_L0[1] = 0.15f; // 右腿：收缩到基础腿长
break; break;
case 2: // 起跳阶段，腿长快速伸展 case 2: // 起跳阶段，腿长快速伸展
target_L0[0] = 0.65f; // 左腿：伸展到最大腿长（跳跃时不进行横滚补偿） target_L0[0] = 0.55f; // 左腿：伸展到最大腿长
target_L0[1] = 0.65f; // 右腿：伸展到最大腿长（跳跃时不进行横滚补偿） target_L0[1] = 0.55f; // 右腿：伸展到最大腿长
break; break;
case 3: // 收腿阶段，腿长收缩到最高 case 3: // 着地阶段，腿长缓冲
target_L0[0] = 0.06f; // 左腿：收缩到最短腿长（确保收腿到最高） target_L0[0] = 0.1f; // 左腿：缓冲腿长
target_L0[1] = 0.06f; // 右腿：收缩到最短腿长（确保收腿到最高） target_L0[1] = 0.1f; // 右腿：缓冲腿长
break; break;
default: default:
target_L0[0] = 0.16f + c_cmd->height * 0.22f + roll_compensation_length; target_L0[0] = 0.15f + c_cmd->height * 0.22f;
target_L0[1] = 0.16f + c_cmd->height * 0.22f - roll_compensation_length; target_L0[1] = 0.15f + c_cmd->height * 0.22f;
break; break;
} }
// 腿长限幅：根据跳跃状态调整限制范围
if (c->mode == CHASSIS_MODE_JUMP && c->state == 3) {
// 在跳跃收腿阶段，允许更短的腿长，确保收腿到最高
if (target_L0[0] < 0.06f) target_L0[0] = 0.06f;
if (target_L0[1] < 0.06f) target_L0[1] = 0.06f;
} else {
// 正常情况下的腿长限制：最短0.10，最长0.42m
if (target_L0[0] < 0.10f) target_L0[0] = 0.10f;
if (target_L0[1] < 0.10f) target_L0[1] = 0.10f;
}
if (target_L0[0] > 0.42f) target_L0[0] = 0.42f;
if (target_L0[1] > 0.42f) target_L0[1] = 0.42f;
// 获取腿长变化率 // 获取腿长变化率
VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL); VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL);
VMC_GetVirtualLegState(&c->vmc_[1], NULL, NULL, &leg_d_length[1], NULL); VMC_GetVirtualLegState(&c->vmc_[1], NULL, NULL, &leg_d_length[1], NULL);
@ -614,10 +523,11 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
float pid_output = PID_Calc(&c->pid.leg_length[0], target_L0[0], float pid_output = PID_Calc(&c->pid.leg_length[0], target_L0[0],
c->vmc_[0].leg.L0, leg_d_length[0], c->dt); c->vmc_[0].leg.L0, leg_d_length[0], c->dt);
// 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 -
// 横滚角补偿力（左腿减少支撑力）
virtual_force[0] = virtual_force[0] =
(c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) + (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) +
pid_output + 60.0f; pid_output + 55.0f + roll_compensation_force;
// VMC逆解算（包含摆角补偿） // VMC逆解算（包含摆角补偿）
if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0], if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0],
@ -638,10 +548,11 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
float pid_output = PID_Calc(&c->pid.leg_length[1], target_L0[1], float pid_output = PID_Calc(&c->pid.leg_length[1], target_L0[1],
c->vmc_[1].leg.L0, leg_d_length[1], c->dt); c->vmc_[1].leg.L0, leg_d_length[1], c->dt);
// 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 +
// 横滚角补偿力（右腿增加支撑力）
virtual_force[1] = virtual_force[1] =
(c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) + (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) +
pid_output + 65.0f; pid_output + 60.0f - roll_compensation_force;
// VMC逆解算（包含摆角补偿） // VMC逆解算（包含摆角补偿）
if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1], if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1],

1
User/module/balance_chassis.h
View File

@ -147,7 +147,6 @@ typedef struct {
float target_yaw; /* 目标yaw角度 */ float target_yaw; /* 目标yaw角度 */
float current_yaw; /* 当前yaw角度 */ float current_yaw; /* 当前yaw角度 */
float yaw_force; /* yaw轴控制力矩 */ float yaw_force; /* yaw轴控制力矩 */
bool is_reversed; /* 是否使用反转的零点（180度零点），影响前后方向 */
} yaw_control; } yaw_control;
float wz_multi; /* 小陀螺模式旋转方向 */ float wz_multi; /* 小陀螺模式旋转方向 */

59
User/module/config.c
View File

@ -170,8 +170,8 @@ Config_RobotParam_t robot_config = {
.yaw={ .yaw={
.k=1.0f, .k=1.0f,
.p=1.0f, .p=1.0f,
.i=0.0f, .i=0.01f,
.d=0.3f, .d=0.05f,
.i_limit=0.0f, .i_limit=0.0f,
.out_limit=1.0f, .out_limit=1.0f,
.d_cutoff_freq=30.0f, .d_cutoff_freq=30.0f,
@ -179,23 +179,23 @@ Config_RobotParam_t robot_config = {
}, },
.roll={ .roll={
.k=1.0f, .k=20.0f,
.p=0.5f, /* 横滚角比例系数 */ .p=5.0f, /* 横滚角比例系数 */
.i=0.01f, /* 横滚角积分系数 */ .i=0.0f, /* 横滚角积分系数 */
.d=0.01f, /* 横滚角微分系数 */ .d=0.2f, /* 横滚角微分系数 */
.i_limit=0.2f, /* 积分限幅 */ .i_limit=0.0f, /* 积分限幅 */
.out_limit=1.0f, /* 输出限幅，腿长差值限制 */ .out_limit=50.0f, /* 输出限幅，腿长差值限制 */
.d_cutoff_freq=30.0f, /* 微分截止频率 */ .d_cutoff_freq=30.0f, /* 微分截止频率 */
.range=-1.0f, /* 不使用循环误差处理 */ .range=-1.0f, /* 不使用循环误差处理 */
}, },
.leg_length={ .leg_length={
.k = 40.0f, .k = 50.0f,
.p = 20.0f, .p = 10.0f,
.i = 0.01f, .i = 0.0f,
.d = 2.0f, .d = 1.0f,
.i_limit = 0.0f, .i_limit = 0.0f,
.out_limit = 200.0f, .out_limit = 100.0f,
.d_cutoff_freq = -1.0f, .d_cutoff_freq = -1.0f,
.range = -1.0f, .range = -1.0f,
}, },
@ -211,8 +211,8 @@ Config_RobotParam_t robot_config = {
}, },
.tp={ .tp={
.k=4.0f, .k=2.0f,
.p=3.0f, /* 俯仰角比例系数 */ .p=2.0f, /* 俯仰角比例系数 */
.i=0.0f, /* 俯仰角积分系数 */ .i=0.0f, /* 俯仰角积分系数 */
.d=0.0f, /* 俯仰角微分系数 */ .d=0.0f, /* 俯仰角微分系数 */
.i_limit=0.0f, /* 积分限幅 */ .i_limit=0.0f, /* 积分限幅 */
@ -254,20 +254,21 @@ Config_RobotParam_t robot_config = {
} }
}, },
.lqr_gains = { .lqr_gains = {
.k11_coeff = { -2.213202553185133e+02f, 2.353939463356143e+02f, -1.057072351438971e+02f, -1.581085937677281e+00f }, // theta .k11_coeff = { -1.498109852818409e+02f, 1.918923604951453e+02f, -1.080984818173493e+02f, -1.540703437137126e+00f }, // theta
.k12_coeff = { -9.181864404672975e+00f, 8.531964722737065e+00f, -9.696625432480346e+00f, -2.388898921230960e-01f }, // d_theta .k12_coeff = { 8.413682810667103e+00f, -6.603584762798329e+00f, -6.421063158996702e+00f, -5.450666844349098e-02f }, // d_theta
.k13_coeff = { -6.328339397527442e+01f, 6.270159865929592e+01f, -2.133356351416681e+01f, -2.795774497769496e-01f }, // x .k13_coeff = { -3.146938649452867e+01f, 3.671781823697795e+01f, -1.548019975847165e+01f, -3.659482046966758e-01f }, // x
.k14_coeff = { -7.428160824353201e+01f, 7.371925049068537e+01f, -2.613745545093503e+01f, -5.994101373770330e-01f }, // d_x .k14_coeff = { -3.363190268966418e+01f, 4.073114332036178e+01f, -1.877821151363973e+01f, -6.755848684037919e-01f }, // d_x
.k15_coeff = { -6.968934105907989e+01f, 9.229969229361623e+01f, -4.424018428098277e+01f, 8.098181536555296e+00f }, // phi .k15_coeff = { 1.813346556940570e+00f, 1.542139674818206e+01f, -1.784151260000496e+01f, 6.975189972486323e+00f }, // phi
.k16_coeff = { -1.527045508038401e+01f, 2.030548630730375e+01f, -1.009526207086012e+01f, 2.014358176738665e+00f }, // d_phi .k16_coeff = { -1.683339907923917e+00f, 3.762911505427638e+00f, -2.966405826579746e+00f, 1.145611903160937e+00f }, // d_phi
.k21_coeff = { 6.254476937997669e+01f, 9.037146968574660e+00f, -4.492072460618583e+01f, 1.770766202994207e+01f }, // theta .k21_coeff = { 3.683683451383080e+02f, -3.184826128050574e+02f, 7.012281968985167e+01f, 1.232691707185163e+01f }, // theta
.k22_coeff = { 3.165057029795604e-02f, 7.350960766534424e+00f, -6.597366624137901e+00f, 2.798506180182324e+00f }, // d_theta .k22_coeff = { 2.256451382235226e+01f, -2.387074220964917e+01f, 8.355357863648345e+00f, 1.148384164091676e+00f }, // d_theta
.k23_coeff = { -5.827814614802593e+01f, 7.789995488757775e+01f, -3.841148024725668e+01f, 8.034534049078013e+00f }, // x .k23_coeff = { 1.065026516142075e+01f, 1.016717837738013e+01f, -1.810442639595643e+01f, 7.368019318950717e+00f }, // x
.k24_coeff = { -8.937952443465080e+01f, 1.128943502182752e+02f, -5.293642666103645e+01f, 1.073722383888271e+01f }, // d_x .k24_coeff = { 2.330418054102148e+00f, 2.193671212435775e+01f, -2.502198732490354e+01f, 9.621144599080463e+00f }, // d_x
.k25_coeff = { 2.478483065877546e+02f, -2.463640234149189e+02f, 8.359617215530402e+01f, 8.324247402653134e+00f }, // phi .k25_coeff = { 1.784444885521937e+02f, -2.030738611028434e+02f, 8.375405599993576e+01f, -2.204042546242858e-01f }, // phi
.k26_coeff = { 6.307211927250707e+01f, -6.266313408748906e+01f, 2.129449351279647e+01f, 9.249265186231070e-01f }, // d_phi .k26_coeff = { 2.646425532528492e+01f, -3.034702782249508e+01f, 1.275100635568078e+01f, -4.878639273974432e-01f }, // d_phi
}, },
.theta = 0.0f, .theta = 0.0f,
.x = 0.0f, .x = 0.0f,
.phi = -0.1f, .phi = -0.1f,

21
User/task/ai.c
View File

@ -4,12 +4,9 @@
*/ */
/* Includes ----------------------------------------------------------------- */ /* Includes ----------------------------------------------------------------- */
#include "cmsis_os2.h"
#include "task/user_task.h" #include "task/user_task.h"
/* USER INCLUDE BEGIN */ /* USER INCLUDE BEGIN */
#include "device/ai.h"
#include "component/ahrs.h"
#include <stdbool.h>
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -17,8 +14,7 @@
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */ /* USER STRUCT BEGIN */
AI_t ai;
AHRS_Quaternion_t quat;
/* USER STRUCT END */ /* USER STRUCT END */
/* Private function --------------------------------------------------------- */ /* Private function --------------------------------------------------------- */
@ -34,23 +30,12 @@ void Task_ai(void *argument) {
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */ /* USER CODE INIT BEGIN */
AI_Init(&ai);
/* USER CODE INIT END */ /* USER CODE INIT END */
while (1) { while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */ tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */ /* USER CODE BEGIN */
AI_StartReceiving(&ai);
if (AI_WaitDmaCplt()) {
AI_ParseHost(&ai);
} else {
AI_HandleOffline(&ai);
}
if (osMessageQueueGet(task_runtime.msgq.ai.quat, &quat, NULL, 0) == osOK) {
AI_PackMCU(&ai, &quat);
}
AI_StartSend(&ai, false);
/* USER CODE END */ /* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */

3
User/task/atti_esti.c
View File

@ -4,7 +4,6 @@
*/ */
/* Includes ----------------------------------------------------------------- */ /* Includes ----------------------------------------------------------------- */
#include "cmsis_os2.h"
#include "task/user_task.h" #include "task/user_task.h"
/* USER INCLUDE BEGIN */ /* USER INCLUDE BEGIN */
#include "bsp/mm.h" #include "bsp/mm.h"
@ -155,8 +154,6 @@ void Task_atti_esti(void *argument) {
osMessageQueueReset(task_runtime.msgq.gimbal.imu); osMessageQueueReset(task_runtime.msgq.gimbal.imu);
osMessageQueuePut(task_runtime.msgq.gimbal.imu, &gimbal_to_send, 0, 0); osMessageQueuePut(task_runtime.msgq.gimbal.imu, &gimbal_to_send, 0, 0);
osMessageQueuePut(task_runtime.msgq.ai.quat, &gimbal_ahrs.quat, 0, 0);
BSP_PWM_SetComp(BSP_PWM_IMU_HEAT_PWM, PID_Calc(&imu_temp_ctrl_pid, 40.0f, bmi088.temp, 0.0f, 0.0f)); BSP_PWM_SetComp(BSP_PWM_IMU_HEAT_PWM, PID_Calc(&imu_temp_ctrl_pid, 40.0f, bmi088.temp, 0.0f, 0.0f));
/* USER CODE END */ /* USER CODE END */

2
User/task/init.c
View File

@ -52,8 +52,6 @@ void Task_Init(void *argument) {
task_runtime.msgq.gimbal.imu= osMessageQueueNew(2u, sizeof(Gimbal_IMU_t), NULL); task_runtime.msgq.gimbal.imu= osMessageQueueNew(2u, sizeof(Gimbal_IMU_t), NULL);
task_runtime.msgq.gimbal.cmd= osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL); task_runtime.msgq.gimbal.cmd= osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL);
task_runtime.msgq.shoot.shoot_cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t), NULL); task_runtime.msgq.shoot.shoot_cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t), NULL);
task_runtime.msgq.ai.quat = osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);
task_runtime.msgq.ai.move_vec = osMessageQueueNew(2u, sizeof(MoveVector_t), NULL);
/* USER MESSAGE END */ /* USER MESSAGE END */
osKernelUnlock(); // 解锁内核 osKernelUnlock(); // 解锁内核

9
User/task/rc.c
View File

@ -61,13 +61,12 @@ void Task_rc(void *argument) {
cmd_for_chassis.mode = CHASSIS_MODE_RELAX; cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
break; break;
case DR16_SW_MID: case DR16_SW_MID:
// cmd_for_chassis.mode = CHASSIS_MODE_RECOVER; cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE; // cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
break; break;
case DR16_SW_DOWN: case DR16_SW_DOWN:
// cmd_for_chassis.mode = CHASSIS_MODE_ROTOR; cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
// cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE; // cmd_for_chassis.mode = CHASSIS_MODE_JUMP;
cmd_for_chassis.mode = CHASSIS_MODE_JUMP;
break; break;
default: default:
cmd_for_chassis.mode = CHASSIS_MODE_RELAX; cmd_for_chassis.mode = CHASSIS_MODE_RELAX;

6
User/task/user_task.h
View File

@ -75,12 +75,6 @@ typedef struct {
osMessageQueueId_t ref; osMessageQueueId_t ref;
osMessageQueueId_t ai; osMessageQueueId_t ai;
}cmd; }cmd;
struct {
osMessageQueueId_t quat;
osMessageQueueId_t move_vec;
osMessageQueueId_t eulr;
osMessageQueueId_t fire;
}ai;
} msgq; } msgq;
/* USER MESSAGE END */ /* USER MESSAGE END */

12
utils/Simulation-master/balance/series_legs/get_k_length.m
View File

@ -17,10 +17,10 @@ function K = get_k_length(leg_length)
R1=0.072; %驱动轮半径 R1=0.072; %驱动轮半径
L1=leg_length/2; %摆杆重心到驱动轮轴距离 L1=leg_length/2; %摆杆重心到驱动轮轴距离
LM1=leg_length/2; %摆杆重心到其转轴距离 LM1=leg_length/2; %摆杆重心到其转轴距离
l1=-0.03; %机体质心距离转轴距离 l1=0.01; %机体质心距离转轴距离
mw1=0.60; %驱动轮质量 mw1=0.60; %驱动轮质量
mp1=1.8; %杆质量 mp1=1.8; %杆质量
M1=6.0; %机体质量 M1=12.0; %机体质量
Iw1=mw1*R1^2; %驱动轮转动惯量 Iw1=mw1*R1^2; %驱动轮转动惯量
Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量 Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
IM1=M1*(0.3^2+0.12^2)/12.0; %机体绕质心转动惯量 IM1=M1*(0.3^2+0.12^2)/12.0; %机体绕质心转动惯量
@ -48,12 +48,8 @@ function K = get_k_length(leg_length)
B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]); B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
B=double(B); B=double(B);
% Recommended: Increase velocity damping to improve convergence Q=diag([1500 100 2500 1500 8000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
% Q weights [theta, d_theta, x, d_x, phi, d_phi] R=[300 0;0 55]; %T Tp
% Original: [1000, 100, 2000, 1500, 20000, 500] causes self-oscillation
% Improved: [500, 300, 2000, 1500, 20000, 500] weight ratio 1.67:1
Q=diag([600 300 2000 1500 20000 500]);
R=[240 0;0 50];
K=lqr(A,B,Q,R); K=lqr(A,B,Q,R);