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

add ai

Browse Source
This commit is contained in:
Robofish 2026-01-13 19:27:21 +08:00
parent d521f0b485
commit 70233c2f90
11 changed files with 9145 additions and 8702 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -77,6 +77,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/device/motor_lk.c
User/device/motor_lz.c
User/device/motor_rm.c
User/device/vision_bridge.c
# User/module sources
User/module/balance_chassis.c

5
MDK-ARM/CtrBoard-H7_ALL.uvoptx
View File

@ -185,6 +185,11 @@
<WinNumber>1</WinNumber>
<ItemText>RF</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>chassis</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
<THDelay>0</THDelay>

BIN
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
View File

Binary file not shown.

17389
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
View File

File diff suppressed because it is too large Load Diff

120
User/device/vision_bridge.c Normal file
View File

@ -0,0 +1,120 @@
#include "vision_bridge.h"
#include "device/device.h"
#include "bsp/uart.h"
#include "component/crc16.h"
#include <string.h>
#include "bsp/fdcan.h"
#define AI_CMD_CAN_DLC (8u) /* 1字节mode + 3.5字节yaw + 3.5字节pit */
#define AI_CMD_ANGLE_SCALE (10000000.0f) /* 0.0000001 rad per LSB */
int8_t AI_Init(AI_t *ai, AI_Param_t *param) {
if (ai == NULL) return DEVICE_ERR_NULL;
BSP_FDCAN_Init();
memset(ai, 0, sizeof(AI_t));
ai->param = param;
BSP_FDCAN_RegisterId(ai->param->can, ai->param->vision_id, 3);
return 0;
}
int8_t AI_ParseForHost(AI_t* ai, AI_RawData_t* raw_data){
ai->tohost.head[0]='M';
ai->tohost.head[1]='R';
ai->tohost.mode=raw_data->mode;
ai->tohost.pitch=raw_data->pitch;
ai->tohost.yaw=raw_data->yaw;
ai->tohost.pitch_vel=raw_data->pitch_vel;
ai->tohost.yaw_vel=raw_data->yaw_vel;
ai->tohost.q[0]=raw_data->q[0];
ai->tohost.q[1]=raw_data->q[1];
ai->tohost.q[2]=raw_data->q[2];
ai->tohost.q[3]=raw_data->q[3];
ai->tohost.bullet_count=10;
ai->tohost.bullet_speed=10.5;
ai->tohost.crc16=CRC16_Calc(((const uint8_t*)&(ai->tohost)),sizeof(ai->tohost)-sizeof(uint16_t), CRC16_INIT );
if(CRC16_Verify(((const uint8_t*)&(ai->tohost)), sizeof(ai->tohost))!=true){
return DEVICE_ERR;
}
return DEVICE_OK;
}
int8_t AI_Get(AI_t *ai, AI_cmd_t* outcmd) {
if(ai->fromhost.head[0]!='M'&&ai->fromhost.head[1]!='R'){
return DEVICE_ERR;
}
// CRC16_Calc(&ai->fromhost,sizeof(ai->fromhost),ai->fromhost.crc16);
if(CRC16_Verify((const uint8_t*)&(ai->fromhost), sizeof(ai->fromhost))!=true){
return DEVICE_ERR;
}
outcmd->gimbal.setpoint.pit = ai->fromhost.pitch;
outcmd->gimbal.setpoint.yaw = ai->fromhost.yaw;
outcmd->mode = ai->fromhost.mode;
outcmd->gimbal.accl.pit = ai->fromhost.pitch_acc;
outcmd->gimbal.vel.pit = ai->fromhost.pitch_vel;
outcmd->gimbal.accl.yaw = ai->fromhost.yaw_acc;
outcmd->gimbal.vel.yaw = ai->fromhost.yaw_vel;
return DEVICE_OK;
}
/* 打包并通过 CAN2 发送AI命令给下层板（mode + yaw + pit） */
void AI_SendCmdOnFDCAN(AI_t *ai, const AI_cmd_t *cmd) {
if (cmd == NULL) return;
/* 将float角度转换为int32_t定点数（0.0000001 rad/LSB） */
const int32_t yaw = (int32_t)(cmd->gimbal.setpoint.yaw * AI_CMD_ANGLE_SCALE);
const int32_t pit = (int32_t)(cmd->gimbal.setpoint.pit * AI_CMD_ANGLE_SCALE);
BSP_FDCAN_StdDataFrame_t frame = {0};
frame.id = ai->param->vision_id;
frame.dlc = AI_CMD_CAN_DLC;
/* mode(1字节) + yaw(3.5字节) + pit(3.5字节) */
frame.data[0] = cmd->mode;
frame.data[1] = (uint8_t)((yaw >> 20) & 0xFF);
frame.data[2] = (uint8_t)((yaw >> 12) & 0xFF);
frame.data[3] = (uint8_t)((yaw >> 4) & 0xFF);
frame.data[4] = (uint8_t)(((yaw & 0xF) << 4) | ((pit >> 24) & 0xF));
frame.data[5] = (uint8_t)((pit >> 16) & 0xFF);
frame.data[6] = (uint8_t)((pit >> 8) & 0xFF);
frame.data[7] = (uint8_t)(pit & 0xFF);
(void)BSP_FDCAN_TransmitStdDataFrame(ai->param->can, &frame);
}
/* 从CAN消息解析AI命令 (mode + yaw + pit) */
void AI_ParseCmdFromCan(AI_t *ai, AI_cmd_t *cmd) {
if (cmd == NULL) {
return;
}
BSP_FDCAN_Message_t msg;
if (BSP_FDCAN_GetMessage(ai->param->can, ai->param->vision_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) != 0) {
return;
}
/* 解析mode (1字节) */
cmd->mode = msg.data[0];
/* 解析yaw (3.5字节) */
int32_t yaw_raw = (int32_t)((msg.data[1] << 20) | (msg.data[2] << 12) | (msg.data[3] << 4) | ((msg.data[4] >> 4) & 0xF));
if (yaw_raw & 0x08000000) yaw_raw |= 0xF0000000;
cmd->gimbal.setpoint.yaw = (float)yaw_raw / AI_CMD_ANGLE_SCALE;
/* 解析pit (3.5字节) */
int32_t pit_raw = (int32_t)(((msg.data[4] & 0xF) << 24) | (msg.data[5] << 16) | (msg.data[6] << 8) | msg.data[7]);
if (pit_raw & 0x08000000) pit_raw |= 0xF0000000;
cmd->gimbal.setpoint.pit = (float)pit_raw / AI_CMD_ANGLE_SCALE;
/* 其他字段根据需要可以初始化为0 */
cmd->gimbal.vel.yaw = 0.0f;
cmd->gimbal.vel.pit = 0.0f;
cmd->gimbal.accl.yaw = 0.0f;
cmd->gimbal.accl.pit = 0.0f;
}

99
User/device/vision_bridge.h Normal file
View File

@ -0,0 +1,99 @@
/*
* 自瞄模组
*/
#pragma once
#include "component/user_math.h"
#include "bsp/fdcan.h"
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
struct __attribute__((packed)) AI_ToHost
{
uint8_t head[2];
uint8_t mode; // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
float q[4]; // wxyz顺序 /q4,q0,q1,q2/
float yaw;
float yaw_vel;
float pitch;
float pitch_vel;
float bullet_speed;
uint16_t bullet_count; // 子弹累计发送次数
uint16_t crc16;
};
struct __attribute__((packed)) AI_FromHost
{
uint8_t head[2];
uint8_t mode; // 0: 不控制, 1: 控制云台但不开火，2: 控制云台且开火
float yaw;
float yaw_vel;
float yaw_acc;
float pitch;
float pitch_vel;
float pitch_acc;
uint16_t crc16;
};
typedef struct {
uint8_t mode;
struct{
struct{
float yaw;
float pit;
}setpoint;
struct{
float pit;
float yaw;
}accl;
struct{
float pit;
float yaw;
}vel;
}gimbal;
}AI_cmd_t;
typedef struct {
uint8_t mode; // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
float q[4]; // wxyz顺序 /q4,q0,q1,q2/
float yaw;
float yaw_vel;
float pitch;
float pitch_vel;
float bullet_speed;
uint16_t bullet_count; // 子弹累计发送次数
}AI_RawData_t;
typedef struct {
BSP_FDCAN_t can;
uint16_t vision_id;
}AI_Param_t;
typedef struct __attribute__((packed)) {
struct AI_ToHost tohost;
struct AI_FromHost fromhost;
AI_Param_t *param;
}AI_t;
int8_t AI_Init(AI_t *ai, AI_Param_t *param);
int8_t AI_StartReceiving(AI_t *ai);
int8_t AI_Get(AI_t *ai, AI_cmd_t* ai_cmd);
int8_t AI_ParseForHost(AI_t* ai, AI_RawData_t* raw_data);
int8_t AI_StartSend(AI_t *ai);
void AI_SendCmdOnCan(AI_t *ai, const AI_cmd_t *cmd);
void AI_ParseCmdFromCan(AI_t *ai, AI_cmd_t *cmd);
#ifdef __cplusplus
}
#endif

154
User/module/balance_chassis.c
View File

@ -21,12 +21,14 @@
#define MAX_ACCELERATION 2.0f /* 最大加速度 (m/s²) */
#define WHEEL_GEAR_RATIO 8.0f /* 轮毂电机扭矩 */
#define BASE_LEG_LENGTH 0.16f /* 基础腿长 (m) */
#define LEG_LENGTH_RANGE 0.18f /* 腿长调节范围 (m) */
#define LEG_LENGTH_RANGE 0.16f /* 腿长调节范围 (m) */
#define MIN_LEG_LENGTH 0.10f /* 最小腿长 (m) */
#define MAX_LEG_LENGTH 0.34f /* 最大腿长 (m) */
#define NON_CONTACT_THETA 0.16f /* 离地时的摆角目标 (rad) */
#define LEFT_BASE_FORCE 60.0f /* 左腿基础支撑力 (N) */
#define RIGHT_BASE_FORCE 60.0f /* 右腿基础支撑力 (N) */
// float L_fn = 0.0f, L_tp = 0.0f, R_fn = 0.0f, R_tp = 0.0f,LF =0.0f,RF=0.0f;
/* Private function prototypes ---------------------------------------------- */
static int8_t Chassis_MotorEnable(Chassis_t *c);
@ -35,6 +37,7 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode);
static void Chassis_UpdateChassisState(Chassis_t *c);
static void Chassis_ResetControllers(Chassis_t *c);
static void Chassis_SelectYawZeroPoint(Chassis_t *c);
static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force);
/* Private functions -------------------------------------------------------- */
@ -219,7 +222,7 @@ static float Chassis_CalcSpringForce(float leg_length)
const float L_AC = 0.042f; // 弹簧上端高度
const float L_BD = 0.050f; // 杆BD长度
const float L_BE = 0.254f; // 杆BE长度
const float Fs = 360.0f; // 弹簧恒力 (N)
const float Fs = 360.0f *(360.0f/220.0f); // 弹簧恒力 (N)
// 通过余弦定理计算∠ABE的余弦值
float cos_theta = (L_AB*L_AB + L_BE*L_BE - leg_length*leg_length) / (2.0f * L_AB * L_BE);
@ -239,6 +242,88 @@ static float Chassis_CalcSpringForce(float leg_length)
+ (L_AC - L_BD*sin_theta)*(L_AC - L_BD*sin_theta)) * L_BE * leg_length);
}
/**
* @brief 跳跃控制状态机
* @param c 底盘结构体指针
* @param c_cmd 控制指令
* @param target_L0 输出的目标腿长数组[2]
* @param extra_force 输出的额外力数组[2]
*/
static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force) {
uint32_t current_time = (uint32_t)(BSP_TIME_Get_us() / 1000); /* 当前时间 ms */
uint32_t elapsed_time = current_time - c->jump.state_start_time;
/* 初始化额外力为0 */
extra_force[0] = 0.0f;
extra_force[1] = 0.0f;
/* 检测跳跃触发 */
if (c->jump.trigger && c->jump.state == JUMP_IDLE) {
c->jump.state = JUMP_CROUCH;
c->jump.state_start_time = current_time;
c->jump.trigger = false; /* 清除触发标志 */
}
/* 跳跃状态机 */
switch (c->jump.state) {
case JUMP_IDLE:
/* 待机状态，使用正常腿长控制 */
break;
case JUMP_CROUCH:
/* 蓄力阶段：缩短腿长 */
target_L0[0] = c->param->jump_params.crouch_leg_length;
target_L0[1] = c->param->jump_params.crouch_leg_length;
if (elapsed_time >= c->param->jump_params.crouch_time_ms) {
c->jump.state = JUMP_LAUNCH;
c->jump.state_start_time = current_time;
}
break;
case JUMP_LAUNCH:
/* 起跳阶段：发力向下推地面 */
target_L0[0] = MAX_LEG_LENGTH; /* 腿伸长 */
target_L0[1] = MAX_LEG_LENGTH;
extra_force[0] = c->param->jump_params.launch_force; /* 额外向下的力 */
extra_force[1] = c->param->jump_params.launch_force;
if (elapsed_time >= c->param->jump_params.launch_time_ms) {
c->jump.state = JUMP_RETRACT;
c->jump.state_start_time = current_time;
}
break;
case JUMP_RETRACT:
/* 收腿阶段：腿收缩准备落地 */
target_L0[0] = c->param->jump_params.retract_leg_length;
target_L0[1] = c->param->jump_params.retract_leg_length;
extra_force[0] = c->param->jump_params.retract_force; /* 前馈力帮助收腿 */
extra_force[1] = c->param->jump_params.retract_force;
/* 检测落地或超时 */
if ( elapsed_time >= c->param->jump_params.retract_time_ms) {
c->jump.state = JUMP_LAND;
c->jump.state_start_time = current_time;
}
break;
case JUMP_LAND:
/* 落地阶段：缓冲并恢复正常 */
/* 使用正常腿长，让PID自动恢复 */
if (elapsed_time >= c->param->jump_params.land_time_ms) {
c->jump.state = JUMP_IDLE;
c->jump.state_start_time = current_time;
}
break;
default:
c->jump.state = JUMP_IDLE;
break;
}
}
/* Public functions --------------------------------------------------------- */
/**
@ -307,6 +392,14 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
memset(&c->chassis_state, 0, sizeof(c->chassis_state));
memset(&c->yaw_control, 0, sizeof(c->yaw_control));
/* 初始化跳跃状态 */
c->jump.state = JUMP_IDLE;
c->jump.trigger = false;
c->jump.state_start_time = 0;
c->jump.crouch_leg_length = c->param->jump_params.crouch_leg_length;
c->jump.launch_force = c->param->jump_params.launch_force;
c->jump.retract_leg_length = c->param->jump_params.retract_leg_length;
return CHASSIS_OK;
}
@ -429,8 +522,6 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
} break;
// case CHASSIS_MODE_CALIBRATE: {
// Chassis_LQRControl(c, c_cmd);
// LF= Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
// RF= Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
@ -608,9 +699,15 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* ==================== 腿长控制 ==================== */
float target_L0[2];
float jump_extra_force[2] = {0.0f, 0.0f};
/* 基础腿长目标 */
target_L0[0] = BASE_LEG_LENGTH + c_cmd->height * LEG_LENGTH_RANGE + roll_compensation;
target_L0[1] = BASE_LEG_LENGTH + c_cmd->height * LEG_LENGTH_RANGE - roll_compensation;
/* 跳跃控制：可能会修改目标腿长和额外力 */
Chassis_JumpControl(c, c_cmd, target_L0, jump_extra_force);
/* 腿长限幅 */
target_L0[0] = LIMIT(target_L0[0], MIN_LEG_LENGTH, MAX_LEG_LENGTH);
target_L0[1] = LIMIT(target_L0[1], MIN_LEG_LENGTH, MAX_LEG_LENGTH);
@ -634,11 +731,20 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->vmc_[0].leg.L0, leg_d_length[0], c->dt);
float spring_force_left = Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
if (isnan(spring_force_left)) spring_force_left = 0.0f; /* 处理无效值 */
float virtual_force_left = c->lqr[0].control_output.Tp * sinf(c->vmc_[0].leg.theta) +
pid_output_left + LEFT_BASE_FORCE - spring_force_left;
if (VMC_InverseSolve(&c->vmc_[0], virtual_force_left,
c->lqr[0].control_output.Tp + Delta_Tp[0]) == 0) {
/* 收腿阶段特殊处理：完全忽略LQR输出，只用PID+前馈力收腿 */
float virtual_force_left, virtual_torque_left;
if (c->jump.state == JUMP_RETRACT) {
/* 收腿阶段：纯收腿控制，不受LQR和基础支撑力影响 */
virtual_force_left = pid_output_left - spring_force_left + jump_extra_force[0];
virtual_torque_left = 0.0f; /* 收腿时不控制摆角 */
} else {
virtual_force_left = c->lqr[0].control_output.Tp * sinf(c->vmc_[0].leg.theta) +
pid_output_left + LEFT_BASE_FORCE - spring_force_left + jump_extra_force[0];
virtual_torque_left = c->lqr[0].control_output.Tp + Delta_Tp[0];
}
if (VMC_InverseSolve(&c->vmc_[0], virtual_force_left, virtual_torque_left) == 0) {
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
} else {
c->output.joint[0] = 0.0f;
@ -650,11 +756,20 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->vmc_[1].leg.L0, leg_d_length[1], c->dt);
float spring_force_right = Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
if (isnan(spring_force_right)) spring_force_right = 0.0f; /* 处理无效值 */
float virtual_force_right = c->lqr[1].control_output.Tp * sinf(c->vmc_[1].leg.theta) +
pid_output_right + RIGHT_BASE_FORCE - spring_force_right;
if (VMC_InverseSolve(&c->vmc_[1], virtual_force_right,
c->lqr[1].control_output.Tp + Delta_Tp[1]) == 0) {
/* 收腿阶段特殊处理：完全忽略LQR输出，只用PID+前馈力收腿 */
float virtual_force_right, virtual_torque_right;
if (c->jump.state == JUMP_RETRACT) {
/* 收腿阶段：纯收腿控制，不受LQR和基础支撑力影响 */
virtual_force_right = pid_output_right - spring_force_right + jump_extra_force[1];
virtual_torque_right = 0.0f; /* 收腿时不控制摆角 */
} else {
virtual_force_right = c->lqr[1].control_output.Tp * sinf(c->vmc_[1].leg.theta) +
pid_output_right + RIGHT_BASE_FORCE - spring_force_right + jump_extra_force[1];
virtual_torque_right = c->lqr[1].control_output.Tp + Delta_Tp[1];
}
if (VMC_InverseSolve(&c->vmc_[1], virtual_force_right, virtual_torque_right) == 0) {
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
} else {
c->output.joint[2] = 0.0f;
@ -666,8 +781,21 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->output.wheel[i] = LIMIT(c->output.wheel[i], -1.0f, 1.0f);
}
for (int i = 0; i < 4; i++) {
c->output.joint[i] = LIMIT(c->output.joint[i], -15.0f, 15.0f);
c->output.joint[i] = LIMIT(c->output.joint[i], -60.0f, 60.0f);
}
return CHASSIS_OK;
}
/**
* @brief 触发跳跃
* @param c 底盘结构体指针
*/
void Chassis_TriggerJump(Chassis_t *c) {
if (c == NULL) return;
/* 只在平衡模式且待机状态下可触发跳跃 */
if (c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE && c->jump.state == JUMP_IDLE) {
c->jump.trigger = true;
}
}

38
User/module/balance_chassis.h
View File

@ -43,6 +43,15 @@ typedef enum {
CHASSIS_MODE_WHELL_LEG_BALANCE /* 轮子+腿平衡模式，底盘自我平衡 */
} Chassis_Mode_t;
/* 跳跃状态枚举 */
typedef enum {
JUMP_IDLE, /* 待机状态 */
JUMP_CROUCH, /* 蓄力阶段：腿缩短蓄力 */
JUMP_LAUNCH, /* 起跳阶段：发力向下 */
JUMP_RETRACT, /* 收腿阶段：腿收缩准备落地 */
JUMP_LAND /* 落地阶段：恢复正常控制 */
} Jump_State_t;
typedef struct {
Chassis_Mode_t mode; /* 底盘模式 */
MoveVector_t move_vec; /* 底盘运动向量 */
@ -83,6 +92,17 @@ typedef struct {
KPID_Params_t leg_length; /* 腿长PID控制参数，用于控制虚拟腿长度 */
KPID_Params_t leg_theta; /* 摆角PID控制参数，用于控制虚拟腿摆角 */
struct {
uint32_t crouch_time_ms; /* 蓄力时间 (ms) */
uint32_t launch_time_ms; /* 起跳发力时间 (ms) */
uint32_t retract_time_ms; /* 收腿时间 (ms) */
uint32_t land_time_ms; /* 落地缓冲时间 (ms) */
float crouch_leg_length; /* 蓄力时腿长 (m) */
float launch_force; /* 起跳力 (N) */
float retract_leg_length; /* 收腿时腿长 (m) */
float retract_force; /* 收腿前馈力 (N)，负值表示向上收 */
} jump_params;
float mech_zero_yaw; /* 机械零点 */
float theta;
@ -136,6 +156,16 @@ typedef struct {
bool is_reversed; /* 是否使用反转的零点（180度零点），影响前后方向 */
} yaw_control;
/* 跳跃控制相关 */
struct {
Jump_State_t state; /* 跳跃状态 */
bool trigger; /* 跳跃触发标志 */
uint32_t state_start_time; /* 当前状态开始时间 (ms) */
float crouch_leg_length; /* 蓄力时的腿长 */
float launch_force; /* 起跳力 (N) */
float retract_leg_length; /* 收腿时的腿长 */
} jump;
/* PID计算的目标值 */
struct {
AHRS_Eulr_t chassis;
@ -208,7 +238,13 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd);
*/
int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
/**
* \brief 触发跳跃
*
* \param c 包含底盘数据的结构体
* \note 只在CHASSIS_MODE_WHELL_LEG_BALANCE模式下有效
*/
void Chassis_TriggerJump(Chassis_t *c);
/**
* \brief 底盘输出值

34
User/module/config.c
View File

@ -317,19 +317,29 @@ Config_RobotParam_t robot_config = {
},
.lqr_gains = {
.k11_coeff = { -2.120324305163214e+02f, 2.384281822810979e+02f, -1.162210131498081e+02f, -2.405740963481636e+00f }, // theta
.k12_coeff = { 4.320836680770054e-01f, 1.360781729068518e-01f, -8.343612068216379e+00f, -8.600090910123033e-02f }, // d_theta
.k13_coeff = { -4.493046202256553e+01f, 4.577344792125822e+01f, -1.629835599958664e+01f, -1.007247166173255e+00f }, // x
.k14_coeff = { -4.823335755850488e+01f, 4.987409533322209e+01f, -1.917162943665977e+01f, -1.473642463713354e+00f }, // d_x
.k15_coeff = { -4.920796990864941e+01f, 6.450325939254844e+01f, -3.268284723443183e+01f, 7.841340265493823e+00f }, // phi
.k16_coeff = { -1.264530042590822e+01f, 1.663296191858249e+01f, -8.467446023207946e+00f, 2.096442008644146e+00f }, // d_phi
.k21_coeff = { 1.636475235954215e+02f, -1.128937920212271e+02f, 4.887401528348596e+00f, 1.459120525493287e+01f }, // theta
.k22_coeff = { 9.939826270288583e+00f, -8.353709902293666e+00f, 1.640639416293288e+00f, 1.492185865897709e+00f }, // d_theta
.k23_coeff = { -4.968599085108445e+01f, 6.646556717472484e+01f, -3.397333983104631e+01f, 7.847958130301292e+00f }, // x
.k24_coeff = { -7.188031995054928e+01f, 9.082377283123918e+01f, -4.412648091833633e+01f, 9.959213854333724e+00f }, // d_x
.k25_coeff = { 2.360507220981238e+02f, -2.398095392324340e+02f, 8.536061841837561e+01f, 2.469595316477948e+00f }, // phi
.k26_coeff = { 6.296425580760564e+01f, -6.412220242164098e+01f, 2.293354052056732e+01f, 4.870876539985159e-01f }, // d_phi
.k11_coeff = { -2.101987239070885e+02f, 2.347095543874707e+02f, -1.159459871283743e+02f, -3.241021388045613e+00f }, // theta
.k12_coeff = { 4.560316971810678e-01f, -4.034884993782992e-01f, -8.609037541931352e+00f, -1.808248933097993e-01f }, // d_theta
.k13_coeff = { -4.398353138550677e+01f, 4.449488274721128e+01f, -1.570406941791402e+01f, -1.480318394233200e+00f }, // x
.k14_coeff = { -4.453434270476863e+01f, 4.584608571495492e+01f, -1.777654668835657e+01f, -2.075482135428575e+00f }, // d_x
.k15_coeff = { -6.235526028409054e+01f, 7.713157115257103e+01f, -3.685375309973255e+01f, 8.389295941174636e+00f }, // phi
.k16_coeff = { -1.616998524208392e+01f, 2.002347187804290e+01f, -9.587206376699182e+00f, 2.246428025510658e+00f }, // d_phi
.k21_coeff = { 1.125565146862570e+02f, -7.099628921833802e+01f, -3.539441069640339e+00f, 1.352123626909457e+01f }, // theta
.k22_coeff = { 5.499043468869487e+00f, -4.098738780201080e+00f, 3.543127660296783e-01f, 1.459732446406883e+00f }, // d_theta
.k23_coeff = { -5.544629627706907e+01f, 6.998139961765887e+01f, -3.384603004971483e+01f, 7.457602036770122e+00f }, // x
.k24_coeff = { -7.795712085138642e+01f, 9.384282192103521e+01f, -4.331019925766177e+01f, 9.314619186867809e+00f }, // d_x
.k25_coeff = { 2.096775452842998e+02f, -2.114480157454946e+02f, 7.456308940238735e+01f, 4.018425949787725e+00f }, // phi
.k26_coeff = { 5.608234904658202e+01f, -5.668241813181834e+01f, 2.007882601644766e+01f, 9.024904615903195e-01f }, // d_phi
},
.jump_params = {
.crouch_time_ms = 300,
.launch_time_ms = 128,
.retract_time_ms = 100,
.land_time_ms = 300,
.crouch_leg_length = 0.14f,
.launch_force = 200.0f,
.retract_leg_length = 0.16f, /* 收腿目标更短 */
.retract_force = -60.0f, /* 收腿前馈力加大 */
},
.theta = 0.0f,
.x = 0.0f,
.phi = -0.1f,

3
User/task/rc.c
View File

@ -10,6 +10,7 @@
#include "module/shoot.h"
#include "module/balance_chassis.h"
#include "module/gimbal.h"
#include "component/user_math.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
@ -125,7 +126,7 @@ switch (dr16.data.sw_l) {
cmd_for_chassis.move_vec.vx = dr16.data.ch_l_y;
cmd_for_chassis.move_vec.vy = dr16.data.ch_l_x;
cmd_for_chassis.move_vec.wz = dr16.data.ch_r_x;
cmd_for_chassis.height = dr16.data.ch_res;
cmd_for_chassis.height = max(dr16.data.ch_res, 0.0f);
osMessageQueueReset(
task_runtime.msgq.chassis.cmd); // 重置消息队列，防止阻塞

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

@ -49,7 +49,7 @@ function K = get_k_length(leg_length)
B=double(B);
Q=diag([1500 100 2500 1500 8000 500]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[250 0;0 50]; %T Tp
R=[200 0;0 50]; %T Tp
K=lqr(A,B,Q,R);