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-12 03:18:02 +08:00
parent 39adf0a2b6
commit a4a8bb6ccb
11 changed files with 9284 additions and 8637 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -135,7 +135,7 @@
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>CMSIS_AGDI</Key> <Key>CMSIS_AGDI</Key>
<Name>-X"Any" -UAny -O206 -S9 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC8000 -FN1 -FF0STM32H72x-73x_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32H723VGTx$CMSIS\Flash\STM32H72x-73x_1024.FLM)</Name> <Name>-X"Any" -UAny -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC8000 -FN1 -FF0STM32H72x-73x_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32H723VGTx$CMSIS\Flash\STM32H72x-73x_1024.FLM)</Name>
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
@ -208,46 +208,6 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>gimbal</ItemText> <ItemText>gimbal</ItemText>
</Ww> </Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>GIMBAL_IMU_Update</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>rx_msg</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>count</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>rx_msg</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>can2_debug</ItemText>
</Ww>
<Ww>
<count>9</count>
<WinNumber>1</WinNumber>
<ItemText>fdcan2_init_debug</ItemText>
</Ww>
<Ww>
<count>10</count>
<WinNumber>1</WinNumber>
<ItemText>rx_debug</ItemText>
</Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_for_gimbal</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>
@ -296,7 +256,7 @@
<EnableFlashSeq>1</EnableFlashSeq> <EnableFlashSeq>1</EnableFlashSeq>
<EnableLog>0</EnableLog> <EnableLog>0</EnableLog>
<Protocol>2</Protocol> <Protocol>2</Protocol>
<DbgClock>5000000</DbgClock> <DbgClock>10000000</DbgClock>
</DebugDescription> </DebugDescription>
</TargetOption> </TargetOption>
</Target> </Target>

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

Binary file not shown.

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

File diff suppressed because it is too large Load Diff

721
User/module/balance_chassis.c
View File

@ -1,3 +1,8 @@
/**
* @file balance_chassis.c
* @brief 平衡底盘控制模块
*/
#include "module/balance_chassis.h" #include "module/balance_chassis.h"
#include "bsp/can.h" #include "bsp/can.h"
#include "bsp/time.h" #include "bsp/time.h"
@ -9,61 +14,70 @@
#include <stddef.h> #include <stddef.h>
#include <string.h> #include <string.h>
/* Private defines ---------------------------------------------------------- */
#define LIMIT(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))
#define WHEEL_RADIUS 0.068f /* 轮子半径 (m) */
#define MAX_ACCELERATION 2.0f /* 最大加速度 (m/s²) */
#define WHEEL_GEAR_RATIO 4.0f /* 轮毂电机扭矩 */
#define BASE_LEG_LENGTH 0.12f /* 基础腿长 (m) */
#define LEG_LENGTH_RANGE 0.22f /* 腿长调节范围 (m) */
#define MIN_LEG_LENGTH 0.10f /* 最小腿长 (m) */
#define MAX_LEG_LENGTH 0.42f /* 最大腿长 (m) */
#define NON_CONTACT_THETA 0.16f /* 离地时的摆角目标 (rad) */
#define LEFT_BASE_FORCE 60.0f /* 左腿基础支撑力 (N) */
#define RIGHT_BASE_FORCE 65.0f /* 右腿基础支撑力 (N) */
/* Private function prototypes ---------------------------------------------- */
static int8_t Chassis_MotorEnable(Chassis_t *c);
static int8_t Chassis_MotorRelax(Chassis_t *c);
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);
/* Private functions -------------------------------------------------------- */
/** /**
* @brief 使能所有电机 * @brief 使能所有电机
* @param c 底盘结构体指针 * @param c 底盘结构体指针
* @return * @return 操作结果
*/ */
static int8_t Chassis_MotorEnable(Chassis_t *c) { static int8_t Chassis_MotorEnable(Chassis_t *c) {
if (c == NULL) if (c == NULL) return CHASSIS_ERR_NULL;
return CHASSIS_ERR_NULL;
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
MOTOR_LZ_Param_t *joint_param = &c->param->joint_param[i]; MOTOR_LZ_Enable(&c->param->joint_param[i]);
MOTOR_LZ_Enable(joint_param);
} }
for (int i = 0; i < 2; i++) { for (int i = 0; i < 2; i++) {
MOTOR_LK_Param_t *wheel_param = &c->param->wheel_param[i]; MOTOR_LK_MotorOn(&c->param->wheel_param[i]);
MOTOR_LK_MotorOn(wheel_param);
} }
return CHASSIS_OK; return CHASSIS_OK;
} }
/**
* @brief 放松所有电机
* @param c 底盘结构体指针
* @return 操作结果
*/
static int8_t Chassis_MotorRelax(Chassis_t *c) { static int8_t Chassis_MotorRelax(Chassis_t *c) {
if (c == NULL) if (c == NULL) return CHASSIS_ERR_NULL;
return CHASSIS_ERR_NULL;
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
MOTOR_LZ_Param_t *joint_param = &c->param->joint_param[i]; MOTOR_LZ_Relax(&c->param->joint_param[i]);
MOTOR_LZ_Relax(joint_param);
} }
for (int i = 0; i < 2; i++) { for (int i = 0; i < 2; i++) {
MOTOR_LK_Param_t *wheel_param = &c->param->wheel_param[i]; MOTOR_LK_Relax(&c->param->wheel_param[i]);
MOTOR_LK_Relax(wheel_param);
} }
return CHASSIS_OK; return CHASSIS_OK;
} }
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { /**
if (c == NULL) * @brief 重置所有控制器
return CHASSIS_ERR_NULL; /* 主结构体不能为空 */ * @param c 底盘结构体指针
if (mode == c->mode) */
return CHASSIS_OK; /* 模式未改变直接返回 */ static void Chassis_ResetControllers(Chassis_t *c) {
/* 重置PID控制器 */
// 特殊处理：从JUMP切换到WHELL_LEG_BALANCE时不重置
// 但从WHELL_LEG_BALANCE切换到JUMP时，需要重置以触发新的跳跃
if (c->mode == CHASSIS_MODE_JUMP && mode == CHASSIS_MODE_WHELL_LEG_BALANCE) {
c->mode = mode;
return CHASSIS_OK;
}
if (c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE && mode == CHASSIS_MODE_JUMP) {
c->mode = mode;
c->state = 0; // 重置状态，确保每次切换模式时都重新初始化
c->jump_time=0; // 重置跳跃时间，切换到JUMP模式时会触发新跳跃
return CHASSIS_OK;
}
Chassis_MotorEnable(c);
PID_Reset(&c->pid.leg_length[0]); PID_Reset(&c->pid.leg_length[0]);
PID_Reset(&c->pid.leg_length[1]); PID_Reset(&c->pid.leg_length[1]);
PID_Reset(&c->pid.yaw); PID_Reset(&c->pid.yaw);
@ -71,105 +85,116 @@ 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函数） /* 重置LQR控制器 */
float current_yaw = c->feedback.yaw.rotor_abs_angle; LQR_Reset(&c->lqr[0]);
float zero_point_1 = c->param->mech_zero_yaw; LQR_Reset(&c->lqr[1]);
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); for (int i = 0; i < 4; i++) {
float error_to_zero2 = CircleError(zero_point_2, current_yaw, M_2PI); LowPassFilter2p_Reset(&c->filter.joint_out[i], 0.0f);
}
// 选择误差绝对值更小的零点作为目标，并记录是否使用了第二个零点 for (int i = 0; i < 2; i++) {
if (fabsf(error_to_zero1) <= fabsf(error_to_zero2)) { LowPassFilter2p_Reset(&c->filter.wheel_out[i], 0.0f);
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;
c->chassis_state.velocity_x = 0.0f; c->chassis_state.velocity_x = 0.0f;
c->chassis_state.last_velocity_x = 0.0f; c->chassis_state.last_velocity_x = 0.0f;
c->chassis_state.target_x = 0.0f; c->chassis_state.target_x = 0.0f;
c->chassis_state.target_velocity_x = 0.0f; c->chassis_state.target_velocity_x = 0.0f;
c->chassis_state.last_target_velocity_x = 0.0f; c->chassis_state.last_target_velocity_x = 0.0f;
}
LQR_Reset(&c->lqr[0]); /**
LQR_Reset(&c->lqr[1]); * @brief 选择yaw轴零点（双零点自动选择）
* @param c 底盘结构体指针
*/
static void Chassis_SelectYawZeroPoint(Chassis_t *c) {
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;
LowPassFilter2p_Reset(&c->filter.joint_out[0], 0.0f); float error_to_zero1 = CircleError(zero_point_1, current_yaw, M_2PI);
LowPassFilter2p_Reset(&c->filter.joint_out[1], 0.0f); float error_to_zero2 = CircleError(zero_point_2, current_yaw, M_2PI);
LowPassFilter2p_Reset(&c->filter.joint_out[2], 0.0f);
LowPassFilter2p_Reset(&c->filter.joint_out[3], 0.0f); if (fabsf(error_to_zero1) <= fabsf(error_to_zero2)) {
LowPassFilter2p_Reset(&c->filter.wheel_out[0], 0.0f); c->yaw_control.target_yaw = zero_point_1;
LowPassFilter2p_Reset(&c->filter.wheel_out[1], 0.0f); c->yaw_control.is_reversed = false;
} else {
c->yaw_control.target_yaw = zero_point_2;
c->yaw_control.is_reversed = true;
}
}
/**
* @brief 设置底盘模式
* @param c 底盘结构体指针
* @param mode 目标模式
* @return 操作结果
*/
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
if (c == NULL) return CHASSIS_ERR_NULL;
if (mode == c->mode) return CHASSIS_OK;
Chassis_MotorEnable(c);
Chassis_ResetControllers(c);
Chassis_SelectYawZeroPoint(c);
c->mode = mode; c->mode = mode;
c->state = 0; // 重置状态，确保每次切换模式时都重新初始化
c->jump_time=0; // 重置跳跃时间，切换到JUMP模式时会触发新跳跃
c->wz_multi=0.0f;
return CHASSIS_OK; return CHASSIS_OK;
} }
/* 更新机体状态估计 */ /**
* @brief 更新机体状态估计
* @param c 底盘结构体指针
*/
static void Chassis_UpdateChassisState(Chassis_t *c) { static void Chassis_UpdateChassisState(Chassis_t *c) {
if (c == NULL) if (c == NULL) return;
return;
// 从轮子编码器估计机体速度 (参考C++代码) /* 从轮子编码器估计机体速度 */
float left_wheel_speed_dps = c->feedback.wheel[0].rotor_speed; // dps (度每秒) float left_speed_dps = c->feedback.wheel[0].rotor_speed;
float right_wheel_speed_dps = float right_speed_dps = c->feedback.wheel[1].rotor_speed;
c->feedback.wheel[1].rotor_speed; // dps (度每秒)
// 将dps转换为rad/s /* 将dps转换为rad/s，再转为线速度 */
float left_wheel_speed = left_wheel_speed_dps * M_PI / 180.0f; // rad/s float left_linear_vel = left_speed_dps * (M_PI / 180.0f) * WHEEL_RADIUS;
float right_wheel_speed = right_wheel_speed_dps * M_PI / 180.0f; // rad/s float right_linear_vel = right_speed_dps * (M_PI / 180.0f) * WHEEL_RADIUS;
float wheel_radius = 0.068f; /* 更新机体速度和位置 */
float left_wheel_linear_vel = left_wheel_speed * wheel_radius;
float right_wheel_linear_vel = right_wheel_speed * wheel_radius;
// 机体x方向速度 (轮子中心速度)
c->chassis_state.last_velocity_x = c->chassis_state.velocity_x; c->chassis_state.last_velocity_x = c->chassis_state.velocity_x;
c->chassis_state.velocity_x = c->chassis_state.velocity_x = (left_linear_vel + right_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;
} }
}
/* Public functions --------------------------------------------------------- */
/**
* @brief 初始化底盘
* @param c 底盘结构体指针
* @param param 底盘参数指针
* @param target_freq 目标控制频率
* @return 操作结果
*/
int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) { int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
if (c == NULL || param == NULL || target_freq <= 0.0f) { if (c == NULL || param == NULL || target_freq <= 0.0f) {
return -1; // 参数错误 return CHASSIS_ERR;
} }
c->param = param;
c->param = param;
c->mode = CHASSIS_MODE_RELAX; c->mode = CHASSIS_MODE_RELAX;
/*初始化can*/ /* 初始化CAN */
BSP_CAN_Init(); BSP_CAN_Init();
/*初始化和注册所有电机*/ /* 初始化和注册电机 */
MOTOR_LZ_Init(); MOTOR_LZ_Init();
// 注册关节电机
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
if (MOTOR_LZ_Register(&c->param->joint_param[i]) != DEVICE_OK) { if (MOTOR_LZ_Register(&c->param->joint_param[i]) != DEVICE_OK) {
return DEVICE_ERR; return DEVICE_ERR;
} }
} }
// 注册轮子电机
for (int i = 0; i < 2; i++) { for (int i = 0; i < 2; i++) {
if (MOTOR_LK_Register(&c->param->wheel_param[i]) != DEVICE_OK) { if (MOTOR_LK_Register(&c->param->wheel_param[i]) != DEVICE_OK) {
return DEVICE_ERR; return DEVICE_ERR;
@ -182,11 +207,9 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
VMC_Init(&c->vmc_[0], &param->vmc_param[0], target_freq); VMC_Init(&c->vmc_[0], &param->vmc_param[0], target_freq);
VMC_Init(&c->vmc_[1], &param->vmc_param[1], target_freq); VMC_Init(&c->vmc_[1], &param->vmc_param[1], target_freq);
/*初始化pid*/ /* 初始化PID控制器 */
PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq, PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq, &param->leg_length);
&param->leg_length); PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq, &param->leg_length);
PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq,
&param->leg_length);
PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, &param->yaw); PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, &param->yaw);
PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, &param->roll); PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, &param->roll);
PID_Init(&c->pid.tp[0], KPID_MODE_CALC_D, target_freq, &param->tp); PID_Init(&c->pid.tp[0], KPID_MODE_CALC_D, target_freq, &param->tp);
@ -196,52 +219,50 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
LQR_Init(&c->lqr[0], &param->lqr_gains); LQR_Init(&c->lqr[0], &param->lqr_gains);
LQR_Init(&c->lqr[1], &param->lqr_gains); LQR_Init(&c->lqr[1], &param->lqr_gains);
LowPassFilter2p_Init(&c->filter.joint_out[0], target_freq, /* 初始化滤波器 */
param->low_pass_cutoff_freq.out); for (int i = 0; i < 4; i++) {
LowPassFilter2p_Init(&c->filter.joint_out[1], target_freq, LowPassFilter2p_Init(&c->filter.joint_out[i], target_freq, param->low_pass_cutoff_freq.out);
param->low_pass_cutoff_freq.out); }
LowPassFilter2p_Init(&c->filter.joint_out[2], target_freq, for (int i = 0; i < 2; i++) {
param->low_pass_cutoff_freq.out); LowPassFilter2p_Init(&c->filter.wheel_out[i], target_freq, param->low_pass_cutoff_freq.out);
LowPassFilter2p_Init(&c->filter.joint_out[3], target_freq, }
param->low_pass_cutoff_freq.out);
LowPassFilter2p_Init(&c->filter.wheel_out[0], target_freq,
param->low_pass_cutoff_freq.out);
LowPassFilter2p_Init(&c->filter.wheel_out[1], target_freq,
param->low_pass_cutoff_freq.out);
Chassis_MotorEnable(c);
/*初始化机体状态*/
c->chassis_state.position_x = 0.0f;
c->chassis_state.velocity_x = 0.0f;
c->chassis_state.last_velocity_x = 0.0f;
c->chassis_state.target_x = 0.0f;
c->chassis_state.target_velocity_x = 0.0f;
c->chassis_state.last_target_velocity_x = 0.0f;
/*初始化yaw控制状态*/ Chassis_MotorEnable(c);
c->yaw_control.target_yaw = 0.0f;
c->yaw_control.current_yaw = 0.0f; /* 初始化状态变量 */
c->yaw_control.yaw_force = 0.0f; memset(&c->chassis_state, 0, sizeof(c->chassis_state));
c->yaw_control.is_reversed = false; memset(&c->yaw_control, 0, sizeof(c->yaw_control));
return CHASSIS_OK; return CHASSIS_OK;
} }
/**
* @brief 更新底盘反馈数据
* @param c 底盘结构体指针
* @return 操作结果
*/
int8_t Chassis_UpdateFeedback(Chassis_t *c) { int8_t Chassis_UpdateFeedback(Chassis_t *c) {
if (c == NULL) { if (c == NULL) return CHASSIS_ERR_NULL;
return -1; // 参数错误
} /* 更新所有电机数据 */
// 更新所有电机数据
MOTOR_LZ_UpdateAll(); MOTOR_LZ_UpdateAll();
MOTOR_LK_UpdateAll(); MOTOR_LK_UpdateAll();
// 更新反馈数据 /* 更新关节电机反馈 */
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_param[i]); MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_param[i]);
if (joint_motor != NULL) { if (joint_motor != NULL) {
c->feedback.joint[i] = joint_motor->motor.feedback; c->feedback.joint[i] = joint_motor->motor.feedback;
} }
/* 机械零点调整 */
if (c->feedback.joint[i].rotor_abs_angle > M_PI) {
c->feedback.joint[i].rotor_abs_angle -= M_2PI;
}
c->feedback.joint[i].rotor_abs_angle = -c->feedback.joint[i].rotor_abs_angle;
} }
/* 更新轮子电机反馈 */
for (int i = 0; i < 2; i++) { for (int i = 0; i < 2; i++) {
MOTOR_LK_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_param[i]); MOTOR_LK_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_param[i]);
if (wheel_motor != NULL) { if (wheel_motor != NULL) {
@ -249,57 +270,44 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c) {
} }
} }
for (int i = 0; i < 4; i++) { /* 更新机体状态估计 */
// 机械零点调整
if (c->feedback.joint[i].rotor_abs_angle > M_PI) {
c->feedback.joint[i].rotor_abs_angle -= M_2PI;
}
c->feedback.joint[i].rotor_abs_angle =
-c->feedback.joint[i].rotor_abs_angle; // 机械零点调整
}
// for (int i = 0; i < 4; i++) {
// c->feedback.joint[i] = virtual_chassis.data.joint[i];
// if (c->feedback.joint[i].rotor_abs_angle > M_PI) {
// c->feedback.joint[i].rotor_abs_angle -= M_2PI;
// }
// c->feedback.joint[i].rotor_abs_angle =
// -c->feedback.joint[i].rotor_abs_angle; // 机械零点调整
// }
// for (int i = 0; i < 2; i++) {
// c->feedback.wheel[i] = virtual_chassis.data.wheel[i];
// }
// c->feedback.imu.accl = virtual_chassis.data.imu.accl;
// c->feedback.imu.gyro = virtual_chassis.data.imu.gyro;
// c->feedback.imu.euler = virtual_chassis.data.imu.euler;
// 更新机体状态估计
Chassis_UpdateChassisState(c); Chassis_UpdateChassisState(c);
return 0; return CHASSIS_OK;
} }
/**
* @brief 更新IMU数据
* @param c 底盘结构体指针
* @param imu IMU数据
* @return 操作结果
*/
int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu) { int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu) {
if (c == NULL) { if (c == NULL) return CHASSIS_ERR_NULL;
return -1; // 参数错误
}
c->feedback.imu = imu; c->feedback.imu = imu;
return 0; return CHASSIS_OK;
} }
/**
* @brief 底盘控制主函数
* @param c 底盘结构体指针
* @param c_cmd 控制指令
* @return 操作结果
*/
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
if (c == NULL || c_cmd == NULL) { if (c == NULL || c_cmd == NULL) return CHASSIS_ERR_NULL;
return CHASSIS_ERR_NULL; // 参数错误
} /* 计算时间间隔 */
c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) / c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) / 1000000.0f;
1000000.0f; /* 计算两次调用的时间间隔，单位秒 */
c->lask_wakeup = BSP_TIME_Get_us(); c->lask_wakeup = BSP_TIME_Get_us();
/* 设置底盘模式 */ /* 设置底盘模式 */
if (Chassis_SetMode(c, c_cmd->mode) != CHASSIS_OK) { if (Chassis_SetMode(c, c_cmd->mode) != CHASSIS_OK) {
return CHASSIS_ERR_MODE; // 设置模式失败 return CHASSIS_ERR_MODE;
} }
/* VMC正解算 */
VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle,
c->feedback.joint[1].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y); c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
@ -307,21 +315,15 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->feedback.joint[2].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y); c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
/*根据底盘模式执行不同的控制逻辑*/ /* 根据模式执行控制 */
switch (c->mode) { switch (c->mode) {
case CHASSIS_MODE_RELAX: case CHASSIS_MODE_RELAX:
// 放松模式，电机不输出
Chassis_MotorRelax(c); Chassis_MotorRelax(c);
Chassis_LQRControl(c, c_cmd); Chassis_LQRControl(c, c_cmd);
break; break;
case CHASSIS_MODE_RECOVER: { case CHASSIS_MODE_RECOVER: {
float fn, tp; float fn = -20.0f, tp = 0.0f;
fn = -20.0f;
tp = 0.0f;
Chassis_LQRControl(c, c_cmd); Chassis_LQRControl(c, c_cmd);
VMC_InverseSolve(&c->vmc_[0], fn, tp); VMC_InverseSolve(&c->vmc_[0], fn, tp);
@ -330,59 +332,15 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]); VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
c->output.wheel[0] = c_cmd->move_vec.vx * 0.2f; c->output.wheel[0] = c_cmd->move_vec.vx * 0.2f;
c->output.wheel[1] = c_cmd->move_vec.vx * 0.2f; c->output.wheel[1] = c_cmd->move_vec.vx * 0.2f;
Chassis_Output(c); // 统一输出 Chassis_Output(c);
} break; } break;
case CHASSIS_MODE_JUMP:
// 跳跃模式状态机
// 状态转换: 0(准备)->1(0.3s)->2(0.2s)->3(0.3s)->0(完成)
// jump_time == 0: 未开始跳跃
// jump_time == 1: 已完成跳跃（不再触发）
// jump_time > 1: 跳跃进行中
// 检测是否需要开始新的跳跃（state为0且jump_time为0）
if (c->state == 0 && c->jump_time == 0) {
// 开始跳跃，进入state 1
c->state = 1;
c->jump_time = BSP_TIME_Get_us();
}
// 只有在跳跃进行中时才处理状态转换（jump_time > 1）
if (c->jump_time > 1) {
// 计算已经过的时间（微秒）
uint64_t elapsed_us = BSP_TIME_Get_us() - c->jump_time;
// 状态转换逻辑
if (c->state == 1 && elapsed_us >= 300000) {
// state 1 保持0.3s后转到state 2
c->state = 2;
c->jump_time = BSP_TIME_Get_us(); // 重置计时
} else if (c->state == 2 && elapsed_us >= 230000) {
// state 2 保持0.25s后转到state 3，确保腿部充分伸展
c->state = 3;
c->jump_time = BSP_TIME_Get_us(); // 重置计时
} else if (c->state == 3 && elapsed_us >= 500000) {
// state 3 保持0.4s后转到state 0，确保收腿到最高
c->state = 0;
c->jump_time = 1; // 设置为1，表示已完成跳跃，不再触发
}
}
// 执行LQR控制（包含PID腿长控制）
Chassis_LQRControl(c, c_cmd);
Chassis_Output(c); // 统一输出
break;
case CHASSIS_MODE_WHELL_LEG_BALANCE: case CHASSIS_MODE_WHELL_LEG_BALANCE:
case CHASSIS_MODE_ROTOR:
// 执行LQR控制（包含PID腿长控制）
Chassis_LQRControl(c, c_cmd); Chassis_LQRControl(c, c_cmd);
Chassis_Output(c); // 统一输出 Chassis_Output(c);
break; break;
break;
default: default:
return CHASSIS_ERR_MODE; return CHASSIS_ERR_MODE;
} }
@ -390,18 +348,19 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
return CHASSIS_OK; return CHASSIS_OK;
} }
/**
* @brief 底盘输出
* @param c 底盘结构体指针
*/
void Chassis_Output(Chassis_t *c) { void Chassis_Output(Chassis_t *c) {
if (c == NULL) if (c == NULL) return;
return;
c->output.joint[0] =
LowPassFilter2p_Apply(&c->filter.joint_out[0], c->output.joint[0]);
c->output.joint[1] =
LowPassFilter2p_Apply(&c->filter.joint_out[1], c->output.joint[1]);
c->output.joint[2] =
LowPassFilter2p_Apply(&c->filter.joint_out[2], c->output.joint[2]);
c->output.joint[3] =
LowPassFilter2p_Apply(&c->filter.joint_out[3], c->output.joint[3]);
/* 关节输出滤波 */
for (int i = 0; i < 4; i++) {
c->output.joint[i] = LowPassFilter2p_Apply(&c->filter.joint_out[i], c->output.joint[i]);
}
/* 发送关节控制指令 */
MOTOR_LZ_MotionParam_t motion_param = { MOTOR_LZ_MotionParam_t motion_param = {
.torque = 0.0f, .torque = 0.0f,
.target_angle = 0.0f, .target_angle = 0.0f,
@ -409,71 +368,52 @@ void Chassis_Output(Chassis_t *c) {
.kp = 0.0f, .kp = 0.0f,
.kd = 0.0f, .kd = 0.0f,
}; };
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
motion_param.torque = c->output.joint[i]; motion_param.torque = c->output.joint[i];
MOTOR_LZ_MotionControl(&c->param->joint_param[i], &motion_param); MOTOR_LZ_MotionControl(&c->param->joint_param[i], &motion_param);
} }
c->output.wheel[0] =
LowPassFilter2p_Apply(&c->filter.wheel_out[0], c->output.wheel[0]); /* 轮子输出滤波并发送 */
c->output.wheel[1] = for (int i = 0; i < 2; i++) {
LowPassFilter2p_Apply(&c->filter.wheel_out[1], c->output.wheel[1]); c->output.wheel[i] = LowPassFilter2p_Apply(&c->filter.wheel_out[i], c->output.wheel[i]);
}
MOTOR_LK_SetOutput(&c->param->wheel_param[0], c->output.wheel[0]); MOTOR_LK_SetOutput(&c->param->wheel_param[0], c->output.wheel[0]);
MOTOR_LK_SetOutput(&c->param->wheel_param[1], c->output.wheel[1]); MOTOR_LK_SetOutput(&c->param->wheel_param[1], c->output.wheel[1]);
// Virtual_Chassis_SendWheelCommand(&virtual_chassis, c->output.wheel[0],
// c->output.wheel[1]);
// Virtual_Chassis_SendWheelCommand(&virtual_chassis, 0.0f,
// 0.0f); // 暂时不让轮子动
} }
/**
* @brief LQR控制
* @param c 底盘结构体指针
* @param c_cmd 控制指令
* @return 操作结果
*/
int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
if (c == NULL || c_cmd == NULL) { if (c == NULL || c_cmd == NULL) return CHASSIS_ERR_NULL;
return CHASSIS_ERR_NULL;
}
/* 运动参数（参考C++版本的状态估计） */ /* ==================== 速度控制 ==================== */
static float xhat = 0.0f, x_dot_hat = 0.0f; float raw_vx = c_cmd->move_vec.vx * 2.0f;
float target_dot_x = 0.0f;
float max_acceleration = 2.0f; // 最大加速度限制: 3 m/s²
// 简化的速度估计（后续可以改进为C++版本的复杂滤波）
x_dot_hat = c->chassis_state.velocity_x;
xhat = c->chassis_state.position_x;
// 目标速度设定（原始期望速度）
float raw_vx = c_cmd->move_vec.vx * 2;
// 根据零点选择情况决定是否反转前后方向
float desired_velocity = c->yaw_control.is_reversed ? -raw_vx : raw_vx; float desired_velocity = c->yaw_control.is_reversed ? -raw_vx : raw_vx;
// 加速度限制处理 /* 加速度限制 */
float velocity_change = float velocity_change = desired_velocity - c->chassis_state.last_target_velocity_x;
desired_velocity - c->chassis_state.last_target_velocity_x; float max_velocity_change = MAX_ACCELERATION * c->dt;
float max_velocity_change = max_acceleration * c->dt; // 最大允许的速度变化 velocity_change = LIMIT(velocity_change, -max_velocity_change, max_velocity_change);
// 限制速度变化率（加速度限制） float target_dot_x = c->chassis_state.last_target_velocity_x + velocity_change;
if (velocity_change > max_velocity_change) {
velocity_change = max_velocity_change;
} else if (velocity_change < -max_velocity_change) {
velocity_change = -max_velocity_change;
}
// 应用加速度限制后的目标速度
target_dot_x = c->chassis_state.last_target_velocity_x + velocity_change;
c->chassis_state.target_velocity_x = target_dot_x; c->chassis_state.target_velocity_x = target_dot_x;
c->chassis_state.last_target_velocity_x = target_dot_x; c->chassis_state.last_target_velocity_x = target_dot_x;
// 更新目标位置
c->chassis_state.target_x += target_dot_x * c->dt; c->chassis_state.target_x += target_dot_x * c->dt;
/* 分别计算左右腿的LQR控制 */ /* ==================== 状态更新 ==================== */
/* 构建当前状态 */ LQR_State_t current_state = {
LQR_State_t current_state = {0}; .theta = c->vmc_[0].leg.theta,
current_state.theta = c->vmc_[0].leg.theta; .d_theta = c->vmc_[0].leg.d_theta,
current_state.d_theta = c->vmc_[0].leg.d_theta; .x = c->chassis_state.position_x,
current_state.x = xhat; .d_x = c->chassis_state.velocity_x,
current_state.d_x = x_dot_hat; .phi = c->feedback.imu.euler.pit,
current_state.phi = c->feedback.imu.euler.pit; .d_phi = -c->feedback.imu.gyro.x,
current_state.d_phi = -c->feedback.imu.gyro.x; };
LQR_UpdateState(&c->lqr[0], &current_state); LQR_UpdateState(&c->lqr[0], &current_state);
@ -481,251 +421,140 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
current_state.d_theta = c->vmc_[1].leg.d_theta; current_state.d_theta = c->vmc_[1].leg.d_theta;
LQR_UpdateState(&c->lqr[1], &current_state); LQR_UpdateState(&c->lqr[1], &current_state);
/* 根据当前腿长更新增益矩阵 */ /* 根据腿长更新增益矩阵 */
LQR_CalculateGainMatrix(&c->lqr[0], c->vmc_[0].leg.L0); LQR_CalculateGainMatrix(&c->lqr[0], c->vmc_[0].leg.L0);
LQR_CalculateGainMatrix(&c->lqr[1], c->vmc_[1].leg.L0); LQR_CalculateGainMatrix(&c->lqr[1], c->vmc_[1].leg.L0);
/* 构建目标状态 */ /* ==================== 目标状态 ==================== */
LQR_State_t target_state = {0}; LQR_State_t target_state = {
.theta = 0.0f,
.d_theta = 0.0f,
.phi = -0.2f,
.d_phi = 0.0f,
.x = c->chassis_state.target_x,
.d_x = c->chassis_state.target_velocity_x,
};
// 在跳跃收腿阶段强制摆角目标为0，确保落地时腿部竖直 /* ==================== Yaw轴控制 ==================== */
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.phi = -0.2f; // 目标俯仰角
target_state.d_phi = 0.0f; // 目标俯仰角速度
target_state.x = 0.0f; // 目标位置
target_state.d_x = 0.0f; // 目标速度
if (c->mode != CHASSIS_MODE_ROTOR){
c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle; c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
// 双零点自动选择逻辑（考虑手动控制偏移，使用CircleError函数）
float manual_offset = c_cmd->move_vec.vy * M_PI_2; 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_1 = c->param->mech_zero_yaw + manual_offset;
float base_target_2 = c->param->mech_zero_yaw + M_PI + 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_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); float error_to_target2 = CircleError(base_target_2, c->yaw_control.current_yaw, M_2PI);
// 选择误差绝对值更小的目标，并更新反转状态
if (fabsf(error_to_target1) <= fabsf(error_to_target2)) { if (fabsf(error_to_target1) <= fabsf(error_to_target2)) {
c->yaw_control.target_yaw = base_target_1; c->yaw_control.target_yaw = base_target_1;
c->yaw_control.is_reversed = false; // 使用第一个零点，不反转 c->yaw_control.is_reversed = false;
} else { } else {
c->yaw_control.target_yaw = base_target_2; c->yaw_control.target_yaw = base_target_2;
c->yaw_control.is_reversed = true; // 使用第二个零点，需要反转前后方向 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);
c->yaw_control.yaw_force = 0.0f; // 关闭偏航控制
}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);
}
/* ==================== 左腿LQR控制 ==================== */
if (c->vmc_[0].leg.is_ground_contact) { if (c->vmc_[0].leg.is_ground_contact) {
/* 更新LQR状态 */
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 = NON_CONTACT_THETA;
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 + NON_CONTACT_THETA) +
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);
c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制 c->yaw_control.yaw_force = 0.0f;
} }
/* ==================== 右腿LQR控制 ==================== */
if (c->vmc_[1].leg.is_ground_contact) { if (c->vmc_[1].leg.is_ground_contact) {
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 = NON_CONTACT_THETA;
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 + NON_CONTACT_THETA) +
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);
c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制 c->yaw_control.yaw_force = 0.0f;
} }
/* 轮毂力矩输出（参考C++版本的减速比） */ /* ==================== 轮毂输出 ==================== */
// 在跳跃的起跳和收腿阶段强制关闭轮子输出，防止离地时轮子猛转 c->output.wheel[0] = c->lqr[0].control_output.T / WHEEL_GEAR_RATIO + c->yaw_control.yaw_force;
if (c->mode == CHASSIS_MODE_JUMP && (c->state == 2 || c->state == 3)) { c->output.wheel[1] = c->lqr[1].control_output.T / WHEEL_GEAR_RATIO - c->yaw_control.yaw_force;
c->output.wheel[0] = 0.0f;
c->output.wheel[1] = 0.0f;
} 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控制） */
float virtual_force[2];
float target_L0[2];
float leg_d_length[2]; // 腿长变化率
/* 横滚角PID补偿计算 */ /* ==================== 横滚角补偿 ==================== */
// 使用PID控制器计算横滚角补偿长度，目标横滚角为0 float roll_compensation = PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol,
float roll_compensation_length =
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);
// 限制补偿长度范围，防止过度补偿 /* 限制补偿范围 */
// 如果任一腿离地，限制补偿长度 float max_compensation = (c->vmc_[0].leg.is_ground_contact && c->vmc_[1].leg.is_ground_contact)
if (!c->vmc_[0].leg.is_ground_contact || !c->vmc_[1].leg.is_ground_contact) { ? 0.05f : 0.02f;
if (roll_compensation_length > 0.02f) roll_compensation = LIMIT(roll_compensation, -max_compensation, max_compensation);
roll_compensation_length = 0.02f;
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) { float target_L0[2];
case 0: // 正常状态，根据高度指令调节腿长 target_L0[0] = BASE_LEG_LENGTH + c_cmd->height * LEG_LENGTH_RANGE + roll_compensation;
target_L0[0] = 0.12f + c_cmd->height * 0.22f + roll_compensation_length; // 左腿：基础腿长 + 高度调节 + 横滚补偿 target_L0[1] = BASE_LEG_LENGTH + c_cmd->height * LEG_LENGTH_RANGE - roll_compensation;
target_L0[1] = 0.12f + c_cmd->height * 0.22f - roll_compensation_length; // 右腿：基础腿长 + 高度调节 - 横滚补偿
break;
case 1: // 准备阶段，腿长收缩
target_L0[0] = 0.14f + roll_compensation_length; // 左腿：收缩到较短腿长 + 横滚补偿
target_L0[1] = 0.14f - roll_compensation_length; // 右腿：收缩到较短腿长 - 横滚补偿
break;
case 2: // 起跳阶段，腿长快速伸展
target_L0[0] = 0.65f; // 左腿：伸展到最大腿长（跳跃时不进行横滚补偿）
target_L0[1] = 0.65f; // 右腿：伸展到最大腿长（跳跃时不进行横滚补偿）
break;
case 3: // 收腿阶段，腿长收缩到最高
target_L0[0] = 0.06f; // 左腿：收缩到最短腿长（确保收腿到最高）
target_L0[1] = 0.06f; // 右腿：收缩到最短腿长（确保收腿到最高）
break;
default:
target_L0[0] = 0.16f + c_cmd->height * 0.22f + roll_compensation_length;
target_L0[1] = 0.16f + c_cmd->height * 0.22f - roll_compensation_length;
break;
}
// 腿长限幅：根据跳跃状态调整限制范围 /* 腿长限幅 */
if (c->mode == CHASSIS_MODE_JUMP && c->state == 3) { 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);
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;
// 获取腿长变化率 /* 获取腿长变化率 */
float leg_d_length[2];
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);
/* 左右腿摆角相互补偿（参考C++版本的Delta_Tp机制） */ /* ==================== 左右腿摆角同步补偿 ==================== */
float Delta_Tp[2]; float Delta_Tp[2];
// 使用tp_pid进行左右腿摆角同步控制
// 左腿补偿力矩：使左腿theta向右腿theta靠拢
Delta_Tp[0] = c->vmc_[0].leg.L0 * 10.0f * Delta_Tp[0] = c->vmc_[0].leg.L0 * 10.0f *
PID_Calc(&c->pid.tp[0], c->vmc_[1].leg.theta, PID_Calc(&c->pid.tp[0], c->vmc_[1].leg.theta,
c->vmc_[0].leg.theta, c->vmc_[0].leg.d_theta, c->dt); c->vmc_[0].leg.theta, c->vmc_[0].leg.d_theta, c->dt);
// 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反）
Delta_Tp[1] = c->vmc_[1].leg.L0 * 10.0f * Delta_Tp[1] = c->vmc_[1].leg.L0 * 10.0f *
PID_Calc(&c->pid.tp[1], c->vmc_[0].leg.theta, PID_Calc(&c->pid.tp[1], c->vmc_[0].leg.theta,
c->vmc_[1].leg.theta, c->vmc_[1].leg.d_theta, c->dt); c->vmc_[1].leg.theta, c->vmc_[1].leg.d_theta, c->dt);
// 左腿控制 /* ==================== 左腿VMC控制 ==================== */
{ float pid_output_left = PID_Calc(&c->pid.leg_length[0], target_L0[0],
// 使用PID进行腿长控制
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);
float virtual_force_left = c->lqr[0].control_output.Tp * sinf(c->vmc_[0].leg.theta) +
pid_output_left + LEFT_BASE_FORCE;
// 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 if (VMC_InverseSolve(&c->vmc_[0], virtual_force_left,
virtual_force[0] =
(c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) +
pid_output + 60.0f;
// VMC逆解算（包含摆角补偿）
if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0],
c->lqr[0].control_output.Tp + Delta_Tp[0]) == 0) { c->lqr[0].control_output.Tp + Delta_Tp[0]) == 0) {
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
&c->output.joint[1]);
} else { } else {
// VMC失败，设为0力矩
c->output.joint[0] = 0.0f; c->output.joint[0] = 0.0f;
c->output.joint[1] = 0.0f; c->output.joint[1] = 0.0f;
} }
}
// 右腿控制 /* ==================== 右腿VMC控制 ==================== */
{ float pid_output_right = PID_Calc(&c->pid.leg_length[1], target_L0[1],
// 使用PID进行腿长控制
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);
float virtual_force_right = c->lqr[1].control_output.Tp * sinf(c->vmc_[1].leg.theta) +
pid_output_right + RIGHT_BASE_FORCE;
// 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 if (VMC_InverseSolve(&c->vmc_[1], virtual_force_right,
virtual_force[1] =
(c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) +
pid_output + 65.0f;
// VMC逆解算（包含摆角补偿）
if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1],
c->lqr[1].control_output.Tp + Delta_Tp[1]) == 0) { c->lqr[1].control_output.Tp + Delta_Tp[1]) == 0) {
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
&c->output.joint[2]);
} else { } else {
// VMC失败，设为0力矩
c->output.joint[2] = 0.0f; c->output.joint[2] = 0.0f;
c->output.joint[3] = 0.0f; c->output.joint[3] = 0.0f;
} }
}
/* 安全限制 */ /* ==================== 安全限制 ==================== */
for (int i = 0; i < 2; i++) { for (int i = 0; i < 2; i++) {
if (fabsf(c->output.wheel[i]) > 1.0f) { c->output.wheel[i] = LIMIT(c->output.wheel[i], -1.0f, 1.0f);
c->output.wheel[i] = (c->output.wheel[i] > 0) ? 1.0f : -1.0f;
} }
}
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
if (fabsf(c->output.joint[i]) > 15.0f) { c->output.joint[i] = LIMIT(c->output.joint[i], -15.0f, 15.0f);
c->output.joint[i] = (c->output.joint[i] > 0) ? 15.0f : -15.0f;
}
} }
return CHASSIS_OK; return CHASSIS_OK;
} }

20
User/module/balance_chassis.h
View File

@ -39,18 +39,9 @@ extern "C" {
typedef enum { typedef enum {
CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */ CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
CHASSIS_MODE_RECOVER, /* 复位模式 */ CHASSIS_MODE_RECOVER, /* 复位模式 */
CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */ CHASSIS_MODE_WHELL_LEG_BALANCE /* 轮子+腿平衡模式，底盘自我平衡 */
CHASSIS_MODE_JUMP, /* 跳跃模式，底盘跳跃 */
CHASSIS_MODE_ROTOR, /* 小陀螺模式，底盘高速旋转 */
} Chassis_Mode_t; } Chassis_Mode_t;
typedef enum {
CHASSIS_JUMP_STATE_READY, /* 准备跳跃 */
CHASSIS_JUMP_STATE_JUMPING, /* 跳跃中 */
CHASSIS_JUMP_STATE_LANDING, /* 着陆中 */
CHASSIS_JUMP_STATE_END, /* 跳跃结束 */
} Chassis_JumpState_t;
typedef struct { typedef struct {
Chassis_Mode_t mode; /* 底盘模式 */ Chassis_Mode_t mode; /* 底盘模式 */
MoveVector_t move_vec; /* 底盘运动向量 */ MoveVector_t move_vec; /* 底盘运动向量 */
@ -126,13 +117,6 @@ typedef struct {
VMC_t vmc_[2]; /* 两条腿的VMC */ VMC_t vmc_[2]; /* 两条腿的VMC */
LQR_t lqr[2]; /* 两条腿的LQR控制器 */ LQR_t lqr[2]; /* 两条腿的LQR控制器 */
int8_t state;
uint64_t jump_time;
float angle;
float height;
/* 机体状态估计 */ /* 机体状态估计 */
struct { struct {
float position_x; /* 机体x位置 */ float position_x; /* 机体x位置 */
@ -151,8 +135,6 @@ typedef struct {
bool is_reversed; /* 是否使用反转的零点（180度零点），影响前后方向 */ bool is_reversed; /* 是否使用反转的零点（180度零点），影响前后方向 */
} yaw_control; } yaw_control;
float wz_multi; /* 小陀螺模式旋转方向 */
/* PID计算的目标值 */ /* PID计算的目标值 */
struct { struct {
AHRS_Eulr_t chassis; AHRS_Eulr_t chassis;

44
User/module/config.c
View File

@ -24,7 +24,7 @@ Config_RobotParam_t robot_config = {
.gimbal_param = { .gimbal_param = {
.pid = { .pid = {
.yaw_omega = { .yaw_omega = {
.k = 1.0f, .k = 2.0f,
.p = 1.0f, .p = 1.0f,
.i = 0.3f, .i = 0.3f,
.d = 0.0f, .d = 0.0f,
@ -34,8 +34,8 @@ Config_RobotParam_t robot_config = {
.range = -1.0f, .range = -1.0f,
}, },
.yaw_angle = { .yaw_angle = {
.k = 8.0f, .k = 7.0f,
.p = 3.0f, .p = 3.5f,
.i = 0.0f, .i = 0.0f,
.d = 0.0f, .d = 0.0f,
.i_limit = 0.0f, .i_limit = 0.0f,
@ -54,10 +54,10 @@ Config_RobotParam_t robot_config = {
.range = -1.0f, .range = -1.0f,
}, },
.pit_angle = { .pit_angle = {
.k = 5.0f, .k = 2.5f,
.p = 5.0f, .p = 5.0f,
.i = 2.5f, .i = 0.2f,
.d = 0.0f, .d = 0.01f,
.i_limit = 0.0f, .i_limit = 0.0f,
.out_limit = 10.0f, .out_limit = 10.0f,
.d_cutoff_freq = -1.0f, .d_cutoff_freq = -1.0f,
@ -66,11 +66,11 @@ Config_RobotParam_t robot_config = {
}, },
.mech_zero = { .mech_zero = {
.yaw = 0.0f, .yaw = 0.0f,
.pit = 2.12f, .pit = 2.2f,
}, },
.travel = { .travel = {
.yaw = -1.0f, .yaw = -1.0f,
.pit = 0.9f, .pit = 0.85f,
}, },
.low_pass_cutoff_freq = { .low_pass_cutoff_freq = {
.out = -1.0f, .out = -1.0f,
@ -180,7 +180,7 @@ Config_RobotParam_t robot_config = {
.chassis_param = { .chassis_param = {
.yaw={ .yaw={
.k=0.0f, .k=1.0f,
.p=1.0f, .p=1.0f,
.i=0.0f, .i=0.0f,
.d=0.3f, .d=0.3f,
@ -297,7 +297,7 @@ Config_RobotParam_t robot_config = {
}, },
}, },
.mech_zero_yaw = 4.34085676f, /* 250.5度，机械零点 */ .mech_zero_yaw = 1.78040409f, /* 机械零点 */
.vmc_param = { .vmc_param = {
{ // 左腿 { // 左腿
@ -317,18 +317,18 @@ Config_RobotParam_t robot_config = {
}, },
.lqr_gains = { .lqr_gains = {
.k11_coeff = { -2.111003343424443e+02f, 2.534552823281283e+02f, -1.288428090302336e+02f, -4.837794661314790e-01f }, // theta .k11_coeff = { -2.702074813939778e+02f, 2.894104138810802e+02f, -1.211372850409846e+02f, -6.933300731936785e-01f }, // theta
.k12_coeff = { 9.842131763802227e+00f, -7.572331320496771e+00f, -6.747008033464407e+00f, 2.794374462854655e-02f }, // d_theta .k12_coeff = { -3.327095836963479e+00f, 5.164647957579151e+00f, -7.601554284013210e+00f, 2.552123800155830e-01f }, // d_theta
.k13_coeff = { -7.026854551050478e+01f, 7.582881862804882e+01f, -2.920474536554779e+01f, 1.123343465922494e+00f }, // x .k13_coeff = { -4.196624710163955e+01f, 4.127695960453792e+01f, -1.402278033829385e+01f, 7.645039434796388e-03f }, // x
.k14_coeff = { -7.955300118741869e+01f, 8.683186601398823e+01f, -3.503519879783562e+01f, 1.204200953017506e+00f }, // d_x .k14_coeff = { -5.313365798300281e+01f, 5.256471525738568e+01f, -1.848736219345810e+01f, -1.124172698659643e-03f }, // d_x
.k15_coeff = { 2.214748921482655e+01f, -5.719424594843836e+00f, -8.215498046486028e+00f, 3.357225411090161e+00f }, // phi .k15_coeff = { -6.095849580858518e+01f, 7.441367039791133e+01f, -3.453552694561191e+01f, 7.000025215843615e+00f }, // phi
.k16_coeff = { -2.588732118811617e-01f, 3.189187555191166e+00f, -3.235989186139448e+00f, 9.848256812196189e-01f }, // d_phi .k16_coeff = { -9.662388366457952e+00f, 1.288897516182337e+01f, -6.650550674687217e+00f, 1.558651627757777e+00f }, // d_phi
.k21_coeff = { 3.783375900877637e+02f, -3.033633007638497e+02f, 4.903946574528503e+01f, 1.550109363173939e+01f }, // theta .k21_coeff = { 1.194019591863143e+02f, -7.294395555463191e+01f, -5.502401903255857e+00f, 1.056061060815681e+01f }, // theta
.k22_coeff = { 2.878430656512739e+01f, -2.993892895824752e+01f, 1.019545567095573e+01f, 1.119137437173934e+00f }, // d_theta .k22_coeff = { 1.953914704492318e+01f, -1.853477522511211e+01f, 5.522863854227361e+00f, 2.440732134290017e-01f }, // d_theta
.k23_coeff = { 2.518975783541210e+01f, 2.088553619504979e+00f, -1.929185285401291e+01f, 8.558202445583612e+00f }, // x .k23_coeff = { -3.113451478996141e+01f, 3.917196236114254e+01f, -1.888229690044390e+01f, 4.105978636011114e+00f }, // x
.k24_coeff = { 2.519335578848752e+01f, 5.878465085842500e+00f, -2.359290011046876e+01f, 1.050067818606354e+01f }, // d_x .k24_coeff = { -3.864995803122945e+01f, 4.873841855321316e+01f, -2.361615127826612e+01f, 5.240190452941591e+00f }, // d_x
.k25_coeff = { 1.146427691025439e+02f, -1.303101485018965e+02f, 5.234994297700838e+01f, 1.889931627651257e+00f }, // phi .k25_coeff = { 2.402815287983674e+02f, -2.353554477048087e+02f, 7.938206986720357e+01f, 7.179992245652441e-01f }, // phi
.k26_coeff = { 3.214791991578748e+01f, -3.467989029865478e+01f, 1.318125503363052e+01f, -3.471228203459202e-01f }, // d_phi .k26_coeff = { 5.110194134606057e+01f, -5.057473149871059e+01f, 1.733200146622070e+01f, -2.070892022734363e-01f }, // d_phi
}, },
.theta = 0.0f, .theta = 0.0f,
.x = 0.0f, .x = 0.0f,

18
User/module/gimbal.c
View File

@ -33,6 +33,9 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
if (mode == g->mode) if (mode == g->mode)
return GIMBAL_OK; return GIMBAL_OK;
// 检查是否relax->absolute
int relax_to_absolute = (g->mode == GIMBAL_MODE_RELAX && mode == GIMBAL_MODE_ABSOLUTE);
PID_Reset(&g->pid.yaw_angle); PID_Reset(&g->pid.yaw_angle);
PID_Reset(&g->pid.yaw_omega); PID_Reset(&g->pid.yaw_omega);
PID_Reset(&g->pid.pit_angle); PID_Reset(&g->pid.pit_angle);
@ -43,14 +46,13 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
MOTOR_DM_Enable(&(g->param->yaw_motor)); MOTOR_DM_Enable(&(g->param->yaw_motor));
AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */ AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
// if (g->mode == GIMBAL_MODE_RELAX) {
// if (mode == GIMBAL_MODE_ABSOLUTE) { if (relax_to_absolute) {
// g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw; // pitch回到限位中点
// } else if (mode == GIMBAL_MODE_RELATIVE) { g->setpoint.eulr.pit = 0.5f * (g->limit.pit.max + g->limit.pit.min);
// g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw; } else {
// }
// }
g->setpoint.eulr.pit = g->feedback.imu.eulr.rol; g->setpoint.eulr.pit = g->feedback.imu.eulr.rol;
}
g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw; g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
g->mode = mode; g->mode = mode;
@ -253,7 +255,7 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
void Gimbal_Output(Gimbal_t *g){ void Gimbal_Output(Gimbal_t *g){
MOTOR_RM_SetOutput(&g->param->pit_motor, g->out.pit); MOTOR_RM_SetOutput(&g->param->pit_motor, g->out.pit);
MOTOR_MIT_Output_t output = {0}; MOTOR_MIT_Output_t output = {0};
output.torque = g->out.yaw * 5.0f; // 乘以减速比 output.torque = g->out.yaw * 5.0f;
output.kd = 0.3f; output.kd = 0.3f;
MOTOR_RM_Ctrl(&g->param->pit_motor); MOTOR_RM_Ctrl(&g->param->pit_motor);
MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output); MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output);

2
User/module/shoot.c
View File

@ -134,7 +134,7 @@ int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
memset(&s->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu)); memset(&s->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu));
memset(&s->output,0,sizeof(s->output)); memset(&s->output,0,sizeof(s->output));
s->target_variable.target_rpm=6000.0f; /* 归一化目标转速 */ s->target_variable.target_rpm=4000.0f; /* 归一化目标转速 */
return 0; return 0;
} }

4
User/task/ctrl_gimbal.c
View File

@ -52,8 +52,8 @@ void Task_ctrl_gimbal(void *argument) {
Gimbal_UpdateFeedback(&gimbal); Gimbal_UpdateFeedback(&gimbal);
// osMessageQueueReset(task_runtime.msgq.chassis_yaw); osMessageQueueReset(task_runtime.msgq.chassis.yaw); // 重置消息队列，防止阻塞
// osMessageQueuePut(task_runtime.msgq.chassis.yaw, &gimbal.feedback.motor.yaw, 0, 0); osMessageQueuePut(task_runtime.msgq.chassis.yaw, &gimbal.feedback.motor.yaw, 0, 0);
Gimbal_Control(&gimbal, &gimbal_cmd); Gimbal_Control(&gimbal, &gimbal_cmd);

2
User/task/init.c
View File

@ -34,7 +34,7 @@ void Task_Init(void *argument) {
/* 创建任务线程 */ /* 创建任务线程 */
task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc); task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
task_runtime.thread.atti_esit = osThreadNew(Task_atti_esit, NULL, &attr_atti_esit); task_runtime.thread.atti_esit = osThreadNew(Task_atti_esit, NULL, &attr_atti_esit);
// task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis); task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
task_runtime.thread.ctrl_gimbal = osThreadNew(Task_ctrl_gimbal, NULL, &attr_ctrl_gimbal); task_runtime.thread.ctrl_gimbal = osThreadNew(Task_ctrl_gimbal, NULL, &attr_ctrl_gimbal);
task_runtime.thread.monitor = osThreadNew(Task_monitor, NULL, &attr_monitor); task_runtime.thread.monitor = osThreadNew(Task_monitor, NULL, &attr_monitor);
task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink); task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink);

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

@ -17,9 +17,9 @@ function K = get_k_length(leg_length)
R1=0.068; %驱动轮半径 R1=0.068; %驱动轮半径
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.5; %杆质量
M1=12.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; %摆杆转动惯量
@ -48,8 +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);
Q=diag([1500 100 2500 1500 8000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 Q=diag([500 1 600 200 8000 100]);%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 60]; %T Tp
K=lqr(A,B,Q,R); K=lqr(A,B,Q,R);