From bf7533e7c2bf10a9905955c38f0a3b4699c77e39 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Thu, 2 Oct 2025 16:59:28 +0800 Subject: [PATCH] =?UTF-8?q?=E9=87=8D=E6=9E=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Core/Inc/FreeRTOSConfig.h | 2 +- User/component/lqr.c | 222 +++----- User/component/lqr.h | 97 +--- User/module/balance_chassis.c | 928 ++++++++++++++++------------------ User/module/balance_chassis.h | 4 +- User/module/config.c | 2 +- User/task/rc.c | 2 +- mod_wheelleg_chassis.cpp | 662 ++++++++++++++++++++++++ 8 files changed, 1190 insertions(+), 729 deletions(-) create mode 100644 mod_wheelleg_chassis.cpp diff --git a/Core/Inc/FreeRTOSConfig.h b/Core/Inc/FreeRTOSConfig.h index 16c89ac..fee994f 100644 --- a/Core/Inc/FreeRTOSConfig.h +++ b/Core/Inc/FreeRTOSConfig.h @@ -187,7 +187,7 @@ standard names. */ /* USER CODE BEGIN Defines */ /* Section where parameter definitions can be added (for instance, to override default ones in FreeRTOS.h) */ -#define configAPPLICATION_ALLOCATED_HEAP 1 +// #define configAPPLICATION_ALLOCATED_HEAP 1 /* USER CODE END Defines */ #endif /* FREERTOS_CONFIG_H */ diff --git a/User/component/lqr.c b/User/component/lqr.c index 827f262..0a6dda4 100644 --- a/User/component/lqr.c +++ b/User/component/lqr.c @@ -6,89 +6,82 @@ #include /* Private macros ----------------------------------------------------------- */ -#define LQR_LIMIT(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x))) - /* Private variables -------------------------------------------------------- */ -/* 从MATLAB仿真get_k.m得到的默认增益矩阵多项式拟合系数 */ -/* 这些系数是通过对不同腿长的LQR增益进行3阶多项式拟合得到的 */ -static LQR_GainMatrix_t default_gain_matrix = { - /* K矩阵第一行 - 轮毂力矩T的增益系数 */ - .k11_coeff = {0.0f, -2845.3f, 1899.4f, -123.8f}, /* theta */ - .k12_coeff = {0.0f, -89.7f, 61.2f, -4.8f}, /* d_theta */ - .k13_coeff = {0.0f, 5479.2f, -3298.6f, 489.8f}, /* x */ - .k14_coeff = {0.0f, 312.4f, -178.9f, 34.2f}, /* d_x */ - .k15_coeff = {0.0f, -31250.0f, 18750.0f, -3125.0f}, /* phi */ - .k16_coeff = {0.0f, -89.7f, 61.2f, -4.8f}, /* d_phi */ - - /* K矩阵第二行 - 髋关节力矩Tp的增益系数 */ - .k21_coeff = {0.0f, 486.1f, -324.1f, 21.6f}, /* theta */ - .k22_coeff = {0.0f, 15.3f, -10.4f, 0.8f}, /* d_theta */ - .k23_coeff = {0.0f, -935.4f, 562.2f, -83.5f}, /* x */ - .k24_coeff = {0.0f, -53.3f, 30.5f, -5.8f}, /* d_x */ - .k25_coeff = {0.0f, 5333.3f, -3200.0f, 533.3f}, /* phi */ - .k26_coeff = {0.0f, 15.3f, -10.4f, 0.8f}, /* d_phi */ -}; /* Private function prototypes ---------------------------------------------- */ -static int8_t LQR_CalculateErrorState(LQR_Controller_t *lqr); -static int8_t LQR_ApplyControlLimits(LQR_Controller_t *lqr); -/* Public functions --------------------------------------------------------- */ - -int8_t LQR_Init(LQR_Controller_t *lqr, float max_wheel_torque, float max_joint_torque) { +static int8_t LQR_CalculateErrorState(LQR_t *lqr) { if (lqr == NULL) { return -1; } + /* 计算状态误差 */ + lqr->error_state.theta = lqr->current_state.theta - lqr->target_state.theta; + lqr->error_state.d_theta = lqr->current_state.d_theta - lqr->target_state.d_theta; + lqr->error_state.x = lqr->current_state.x - lqr->target_state.x; + lqr->error_state.d_x = lqr->current_state.d_x - lqr->target_state.d_x; + lqr->error_state.phi = lqr->current_state.phi - lqr->target_state.phi; + lqr->error_state.d_phi = lqr->current_state.d_phi - lqr->target_state.d_phi; + return 0; +} + +static float LQR_PolynomialCalc(const float *coeff, float leg_length) { + if (coeff == NULL) { + return 0.0f; + } + + /* 计算3阶多项式: coeff[0]*L^3 + coeff[1]*L^2 + coeff[2]*L + coeff[3] */ + float L = leg_length; + float L2 = L * L; + float L3 = L2 * L; + + return coeff[0] * L3 + coeff[1] * L2 + coeff[2] * L + coeff[3]; +} + + +// static int8_t LQR_ApplyControlLimits(LQR_t *lqr) { +// if (lqr == NULL) { +// return -1; +// } + +// /* 限制轮毂力矩 */ +// if (fabsf(lqr->control_output.T) > lqr->param.max_wheel_torque) { +// lqr->control_output.T = LQR_LIMIT(lqr->control_output.T, +// -lqr->param.max_wheel_torque, +// lqr->param.max_wheel_torque); +// lqr->wheel_torque_limited = true; +// } + +// /* 限制髋关节力矩 */ +// if (fabsf(lqr->control_output.Tp) > lqr->param.max_joint_torque) { +// lqr->control_output.Tp = LQR_LIMIT(lqr->control_output.Tp, +// -lqr->param.max_joint_torque, +// lqr->param.max_joint_torque); +// lqr->joint_torque_limited = true; +// } + +// return 0; +// } + + +/* Public functions --------------------------------------------------------- */ + +int8_t LQR_Init(LQR_t *lqr, LQR_GainMatrix_t *gain_matrix) { + if (lqr == NULL) { + return -1; + } /* 清零结构体 */ - memset(lqr, 0, sizeof(LQR_Controller_t)); - - /* 设置控制限制 */ - lqr->param.max_wheel_torque = max_wheel_torque; - lqr->param.max_joint_torque = max_joint_torque; - - /* 设置默认系统物理参数（从MATLAB仿真get_k_length.m获取） */ - lqr->param.R = 0.072f; /* 驱动轮半径 */ - lqr->param.l = 0.03f; /* 机体重心到转轴距离 */ - lqr->param.mw = 0.60f; /* 驱动轮质量 */ - lqr->param.mp = 1.8f; /* 摆杆质量 */ - lqr->param.M = 1.8f; /* 机体质量 */ - lqr->param.g = 9.8f; /* 重力加速度 */ - - /* 计算转动惯量 */ - lqr->param.Iw = lqr->param.mw * lqr->param.R * lqr->param.R; - lqr->param.IM = lqr->param.M * (0.3f * 0.3f + 0.12f * 0.12f) / 12.0f; - - /* 设置默认增益矩阵 */ - lqr->gain_matrix = &default_gain_matrix; - - /* 设置默认目标状态（平衡状态） */ - memset(&lqr->param.target_state, 0, sizeof(LQR_State_t)); - - /* 初始化当前腿长为中等值 */ - lqr->current_leg_length = 0.25f; - - /* 计算初始增益矩阵 */ - LQR_CalculateGainMatrix(lqr, lqr->current_leg_length); - + memset(lqr, 0, sizeof(LQR_t)); + + lqr->gain_matrix = gain_matrix; lqr->initialized = true; return 0; } -int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, LQR_GainMatrix_t *gain_matrix) { - if (lqr == NULL || gain_matrix == NULL) { - return -1; - } - - lqr->gain_matrix = gain_matrix; - - /* 重新计算增益矩阵 */ - return LQR_CalculateGainMatrix(lqr, lqr->current_leg_length); -} -int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state) { +int8_t LQR_UpdateState(LQR_t *lqr, const LQR_State_t *state) { if (lqr == NULL || state == NULL) { return -1; } @@ -100,34 +93,25 @@ int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state) { return LQR_CalculateErrorState(lqr); } -int8_t LQR_SetTargetState(LQR_Controller_t *lqr, const LQR_State_t *target_state) { +int8_t LQR_SetTargetState(LQR_t *lqr, const LQR_State_t *target_state) { if (lqr == NULL || target_state == NULL) { return -1; } - lqr->param.target_state = *target_state; + lqr->target_state = *target_state; /* 重新计算状态误差 */ return LQR_CalculateErrorState(lqr); } -int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length) { +int8_t LQR_CalculateGainMatrix(LQR_t *lqr, float leg_length) { if (lqr == NULL || lqr->gain_matrix == NULL) { return -1; } /* 限制腿长范围 */ - leg_length = LQR_LIMIT(leg_length, 0.1f, 0.4f); lqr->current_leg_length = leg_length; - /* 更新与腿长相关的物理参数 */ - lqr->param.L = leg_length / 2.0f; /* 摆杆重心到驱动轮轴距离 */ - lqr->param.LM = leg_length / 2.0f; /* 摆杆重心到其转轴距离 */ - - /* 计算摆杆转动惯量 */ - float leg_total_length = lqr->param.L + lqr->param.LM; - lqr->param.Ip = lqr->param.mp * (leg_total_length * leg_total_length + 0.05f * 0.05f) / 12.0f; - /* 使用多项式拟合计算当前增益矩阵 */ lqr->K_matrix[0][0] = LQR_PolynomialCalc(lqr->gain_matrix->k11_coeff, leg_length); /* K11: theta */ lqr->K_matrix[0][1] = LQR_PolynomialCalc(lqr->gain_matrix->k12_coeff, leg_length); /* K12: d_theta */ @@ -146,7 +130,7 @@ int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length) { return 0; } -int8_t LQR_Control(LQR_Controller_t *lqr) { +int8_t LQR_Control(LQR_t *lqr) { if (lqr == NULL || !lqr->initialized) { return -1; } @@ -175,92 +159,22 @@ int8_t LQR_Control(LQR_Controller_t *lqr) { lqr->K_matrix[1][5] * lqr->error_state.d_phi ); - /* 应用控制限制 */ - return LQR_ApplyControlLimits(lqr); -} - -int8_t LQR_GetOutput(LQR_Controller_t *lqr, LQR_Input_t *output) { - if (lqr == NULL || output == NULL) { - return -1; - } - - *output = lqr->control_output; return 0; } -int8_t LQR_Reset(LQR_Controller_t *lqr) { +int8_t LQR_Reset(LQR_t *lqr) { if (lqr == NULL) { return -1; } /* 清零状态和输出 */ memset(&lqr->current_state, 0, sizeof(LQR_State_t)); + memset(&lqr->target_state, 0, sizeof(LQR_State_t)); memset(&lqr->error_state, 0, sizeof(LQR_State_t)); - memset(&lqr->control_output, 0, sizeof(LQR_Input_t)); - memset(&lqr->param.target_state, 0, sizeof(LQR_State_t)); - - /* 重置限幅标志 */ - lqr->wheel_torque_limited = false; - lqr->joint_torque_limited = false; + memset(&lqr->control_output, 0, sizeof(LQR_Output_t)); return 0; } -float LQR_PolynomialCalc(const float *coeff, float leg_length) { - if (coeff == NULL) { - return 0.0f; - } - - /* 计算3阶多项式: coeff[0]*L^3 + coeff[1]*L^2 + coeff[2]*L + coeff[3] */ - float L = leg_length; - float L2 = L * L; - float L3 = L2 * L; - - return coeff[0] * L3 + coeff[1] * L2 + coeff[2] * L + coeff[3]; -} -/* Private functions -------------------------------------------------------- */ - -static int8_t LQR_CalculateErrorState(LQR_Controller_t *lqr) { - if (lqr == NULL) { - return -1; - } - - /* 计算状态误差 */ - lqr->error_state.theta = lqr->current_state.theta - lqr->param.target_state.theta; - lqr->error_state.d_theta = lqr->current_state.d_theta - lqr->param.target_state.d_theta; - lqr->error_state.x = lqr->current_state.x - lqr->param.target_state.x; - lqr->error_state.d_x = lqr->current_state.d_x - lqr->param.target_state.d_x; - lqr->error_state.phi = lqr->current_state.phi - lqr->param.target_state.phi; - lqr->error_state.d_phi = lqr->current_state.d_phi - lqr->param.target_state.d_phi; - - return 0; -} - -static int8_t LQR_ApplyControlLimits(LQR_Controller_t *lqr) { - if (lqr == NULL) { - return -1; - } - - /* 重置限幅标志 */ - lqr->wheel_torque_limited = false; - lqr->joint_torque_limited = false; - - /* 限制轮毂力矩 */ - if (fabsf(lqr->control_output.T) > lqr->param.max_wheel_torque) { - lqr->control_output.T = LQR_LIMIT(lqr->control_output.T, - -lqr->param.max_wheel_torque, - lqr->param.max_wheel_torque); - lqr->wheel_torque_limited = true; - } - - /* 限制髋关节力矩 */ - if (fabsf(lqr->control_output.Tp) > lqr->param.max_joint_torque) { - lqr->control_output.Tp = LQR_LIMIT(lqr->control_output.Tp, - -lqr->param.max_joint_torque, - lqr->param.max_joint_torque); - lqr->joint_torque_limited = true; - } - - return 0; -} +/* Private functions -------------------------------------------------------- */ \ No newline at end of file diff --git a/User/component/lqr.h b/User/component/lqr.h index 59506e2..e05f224 100644 --- a/User/component/lqr.h +++ b/User/component/lqr.h @@ -1,6 +1,6 @@ /* * LQR控制器 - * 用于轮腿平衡机器人的线性二次调节器控制 + * 用于轮腿平衡机器人的线性二次调节器控制：单腿建模 */ #pragma once @@ -21,16 +21,6 @@ extern "C" { /* Exported types ----------------------------------------------------------- */ -/** - * @brief LQR状态向量 - * 状态定义： - * theta: 摆杆与竖直方向夹角 (rad) - * d_theta: 摆杆角速度 (rad/s) - * x: 驱动轮位移 (m) - * d_x: 驱动轮速度 (m/s) - * phi: 机体与水平夹角 (rad) - * d_phi: 机体角速度 (rad/s) - */ typedef struct { float theta; /* 摆杆角度 */ float d_theta; /* 摆杆角速度 */ @@ -40,19 +30,11 @@ typedef struct { float d_phi; /* 机体俯仰角速度 */ } LQR_State_t; -/** - * @brief LQR控制输入向量 - */ typedef struct { float T; /* 轮毂力矩 (N·m) */ float Tp; /* 髋关节力矩 (N·m) */ -} LQR_Input_t; +} LQR_Output_t; -/** - * @brief LQR增益矩阵参数 - * K矩阵的每个元素的多项式拟合系数 - * K(leg_length) = a[0]*L^3 + a[1]*L^2 + a[2]*L + a[3] - */ typedef struct { /* K矩阵第一行(轮毂力矩T对应的增益) */ float k11_coeff[LQR_POLY_ORDER]; /* K(1,1): theta */ @@ -71,41 +53,18 @@ typedef struct { float k26_coeff[LQR_POLY_ORDER]; /* K(2,6): d_phi */ } LQR_GainMatrix_t; -/** - * @brief LQR控制器参数 - */ -typedef struct { - /* 系统物理参数 */ - float R; /* 驱动轮半径 (m) */ - float L; /* 摆杆重心到驱动轮轴距离 (m) */ - float LM; /* 摆杆重心到其转轴距离 (m) */ - float l; /* 机体重心到其转轴距离 (m) */ - float mw; /* 驱动轮质量 (kg) */ - float mp; /* 摆杆质量 (kg) */ - float M; /* 机体质量 (kg) */ - float Iw; /* 驱动轮转动惯量 (kg·m²) */ - float Ip; /* 摆杆绕质心转动惯量 (kg·m²) */ - float IM; /* 机体绕质心转动惯量 (kg·m²) */ - float g; /* 重力加速度 (m/s²) */ - - /* 控制限制 */ - float max_wheel_torque; /* 轮毂电机最大力矩 (N·m) */ - float max_joint_torque; /* 关节电机最大力矩 (N·m) */ - - /* 目标状态 */ - LQR_State_t target_state; /* 目标状态 */ -} LQR_Param_t; /** * @brief LQR控制器主结构体 */ typedef struct { - LQR_Param_t param; /* 控制器参数 */ LQR_GainMatrix_t *gain_matrix; /* 增益矩阵参数指针 */ LQR_State_t current_state; /* 当前状态 */ + LQR_State_t target_state; /* 目标状态 */ LQR_State_t error_state; /* 状态误差 */ - LQR_Input_t control_output; /* 控制输出 */ + + LQR_Output_t control_output; /* 控制输出 */ /* 运行时变量 */ float current_leg_length; /* 当前腿长 */ @@ -113,10 +72,7 @@ typedef struct { bool initialized; /* 初始化标志 */ - /* 限幅标志 */ - bool wheel_torque_limited; /* 轮毂力矩是否被限幅 */ - bool joint_torque_limited; /* 关节力矩是否被限幅 */ -} LQR_Controller_t; +} LQR_t; /* Exported functions prototypes -------------------------------------------- */ @@ -124,20 +80,11 @@ typedef struct { * @brief 初始化LQR控制器 * * @param lqr LQR控制器结构体指针 - * @param max_wheel_torque 轮毂电机最大力矩 - * @param max_joint_torque 关节电机最大力矩 - * @return int8_t 0-成功, 其他-失败 - */ -int8_t LQR_Init(LQR_Controller_t *lqr, float max_wheel_torque, float max_joint_torque); - -/** - * @brief 设置LQR增益矩阵参数 - * - * @param lqr LQR控制器结构体指针 * @param gain_matrix 增益矩阵参数指针 * @return int8_t 0-成功, 其他-失败 */ -int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, LQR_GainMatrix_t *gain_matrix); +int8_t LQR_Init(LQR_t *lqr, LQR_GainMatrix_t *gain_matrix); + /** * @brief 更新当前状态 @@ -146,7 +93,7 @@ int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, LQR_GainMatrix_t *gain_matrix); * @param state 当前状态 * @return int8_t 0-成功, 其他-失败 */ -int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state); +int8_t LQR_UpdateState(LQR_t *lqr, const LQR_State_t *current_state); /** * @brief 设置目标状态 @@ -155,7 +102,7 @@ int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state); * @param target_state 目标状态 * @return int8_t 0-成功, 其他-失败 */ -int8_t LQR_SetTargetState(LQR_Controller_t *lqr, const LQR_State_t *target_state); +int8_t LQR_SetTargetState(LQR_t *lqr, const LQR_State_t *target_state); /** * @brief 根据当前腿长计算增益矩阵 @@ -164,7 +111,7 @@ int8_t LQR_SetTargetState(LQR_Controller_t *lqr, const LQR_State_t *target_state * @param leg_length 当前腿长 (m) * @return int8_t 0-成功, 其他-失败 */ -int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length); +int8_t LQR_CalculateGainMatrix(LQR_t *lqr, float leg_length); /** * @brief 执行LQR控制计算 @@ -172,16 +119,7 @@ int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length); * @param lqr LQR控制器结构体指针 * @return int8_t 0-成功, 其他-失败 */ -int8_t LQR_Control(LQR_Controller_t *lqr); - -/** - * @brief 获取控制输出 - * - * @param lqr LQR控制器结构体指针 - * @param output 输出控制指令指针 - * @return int8_t 0-成功, 其他-失败 - */ -int8_t LQR_GetOutput(LQR_Controller_t *lqr, LQR_Input_t *output); +int8_t LQR_Control(LQR_t *lqr); /** * @brief 重置LQR控制器 @@ -189,16 +127,7 @@ int8_t LQR_GetOutput(LQR_Controller_t *lqr, LQR_Input_t *output); * @param lqr LQR控制器结构体指针 * @return int8_t 0-成功, 其他-失败 */ -int8_t LQR_Reset(LQR_Controller_t *lqr); - -/** - * @brief 根据多项式系数计算增益值 - * - * @param coeff 多项式系数数组 - * @param leg_length 腿长 - * @return float 增益值 - */ -float LQR_PolynomialCalc(const float *coeff, float leg_length); +int8_t LQR_Reset(LQR_t *lqr); #ifdef __cplusplus } diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index e38bad6..422caa8 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -1,538 +1,494 @@ #include "module/balance_chassis.h" -#include "bsp/time.h" #include "bsp/can.h" -#include "component/user_math.h" +#include "bsp/time.h" #include "component/kinematics.h" #include "component/limiter.h" +#include "component/user_math.h" +#include "device/motor_lk.h" +#include "device/motor_lz.h" #include +#include #include /** * @brief 使能所有电机 * @param c 底盘结构体指针 - * @return + * @return */ static int8_t Chassis_MotorEnable(Chassis_t *c) { - if (c == NULL) return CHASSIS_ERR_NULL; - - for (int i = 0; i < 4; i++) { - MOTOR_LZ_Enable(&c->param->joint_motors[i]); - } - - BSP_TIME_Delay_us(200); - - for (int i = 0; i < 2; i++) { - MOTOR_LK_MotorOn(&c->param->wheel_motors[i]); - } - - return CHASSIS_OK; + if (c == NULL) + return CHASSIS_ERR_NULL; + for (int i = 0; i < 4; i++) { + MOTOR_LZ_Enable(&c->param->joint_motors[i]); + } + BSP_TIME_Delay_us(200); + for (int i = 0; i < 2; i++) { + MOTOR_LK_MotorOn(&c->param->wheel_motors[i]); + } + return CHASSIS_OK; } - -// /** -// * @brief 关闭所有电机 -// * @param c 底盘结构体指针 -// * @return -// */ -// static int8_t Chassis_MotorDisable(Chassis_t *c){ -// if (c == NULL) return CHASSIS_ERR_NULL; - -// for (int i = 0; i < 2; i++) { -// MOTOR_LK_MotorOff(&c->param->wheel_motors[i]); -// } -// for (int i = 0; i < 4; i++) { -// MOTOR_LZ_Disable(&c->param->joint_motors[i], true); -// } - -// return CHASSIS_OK; -// } +/** + * @brief 关闭所有电机 + * @param c 底盘结构体指针 + * @return + */ +static int8_t Chassis_MotorRelax(Chassis_t *c) { + if (c == NULL) + return CHASSIS_ERR_NULL; + for (int i = 0; i < 2; i++) { + MOTOR_LK_Relax(&c->param->wheel_motors[i]); + } + BSP_TIME_Delay_us(200); + for (int i = 0; i < 4; i++) { + MOTOR_LZ_Relax(&c->param->joint_motors[i]); + } + return CHASSIS_OK; +} 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; /* 模式未改变直接返回 */ + if (c == NULL) + return CHASSIS_ERR_NULL; /* 主结构体不能为空 */ + if (mode == c->mode) + return CHASSIS_OK; /* 模式未改变直接返回 */ + Chassis_MotorEnable(c); + PID_Reset(&c->pid.leg_length[0]); + PID_Reset(&c->pid.leg_length[1]); + PID_Reset(&c->pid.yaw); + PID_Reset(&c->pid.roll); + PID_Reset(&c->pid.tp[0]); + PID_Reset(&c->pid.tp[1]); - Chassis_MotorEnable(c); - - PID_Reset(&c->pid.leg_length[0]); - PID_Reset(&c->pid.leg_length[1]); - PID_Reset(&c->pid.yaw); - PID_Reset(&c->pid.roll); - PID_Reset(&c->pid.tp[0]); - PID_Reset(&c->pid.tp[1]); - - c->yaw_control.target_yaw = c->feedback.imu.euler.yaw; - - //清空位移 - 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->yaw_control.target_yaw = c->feedback.imu.euler.yaw; + // 清空位移 + 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; LQR_Reset(&c->lqr[0]); LQR_Reset(&c->lqr[1]); c->mode = mode; - c->state = 0; // 重置状态，确保每次切换模式时都重新初始化 + c->state = 0; // 重置状态，确保每次切换模式时都重新初始化 return CHASSIS_OK; } /* 更新机体状态估计 */ static void Chassis_UpdateChassisState(Chassis_t *c) { - if (c == NULL) return; - - // 从轮子编码器估计机体速度 (参考C++代码) - float left_wheel_speed_dps = c->feedback.wheel[0].rotor_speed; // dps (度每秒) - float right_wheel_speed_dps = c->feedback.wheel[1].rotor_speed; // dps (度每秒) - - // 将dps转换为rad/s - float left_wheel_speed = left_wheel_speed_dps * M_PI / 180.0f; // rad/s - float right_wheel_speed = right_wheel_speed_dps * M_PI / 180.0f; // rad/s - - float wheel_radius = 0.072f; - - 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.velocity_x = (left_wheel_linear_vel + right_wheel_linear_vel) / 2.0f; - - // 积分得到位置 - c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt; + if (c == NULL) + return; + + // 从轮子编码器估计机体速度 (参考C++代码) + float left_wheel_speed_dps = c->feedback.wheel[0].rotor_speed; // dps (度每秒) + float right_wheel_speed_dps = + c->feedback.wheel[1].rotor_speed; // dps (度每秒) + + // 将dps转换为rad/s + float left_wheel_speed = left_wheel_speed_dps * M_PI / 180.0f; // rad/s + float right_wheel_speed = right_wheel_speed_dps * M_PI / 180.0f; // rad/s + + float wheel_radius = 0.072f; + + 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.velocity_x = + (left_wheel_linear_vel + right_wheel_linear_vel) / 2.0f; + + // 积分得到位置 + c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt; } +int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) { + if (c == NULL || param == NULL || target_freq <= 0.0f) { + return -1; // 参数错误 + } + c->param = param; -int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){ - if (c == NULL || param == NULL || target_freq <= 0.0f) { - return -1; // 参数错误 + /*初始化can*/ + BSP_CAN_Init(); + + /*初始化和注册所有电机*/ + MOTOR_LZ_Init(); + + for (int i = 0; i < 4; i++) { + MOTOR_LZ_Register(&c->param->joint_motors[i]); + } + for (int i = 0; i < 2; i++) { + MOTOR_LK_Register(&c->param->wheel_motors[i]); + } + MOTOR_DM_Register(&c->param->yaw_motor); + + // Chassis_MotorEnable(c); + + /*初始化VMC控制器*/ + VMC_Init(&c->vmc_[0], ¶m->vmc_param[0], target_freq); + VMC_Init(&c->vmc_[1], ¶m->vmc_param[1], target_freq); + + /*初始化pid*/ + PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq, + ¶m->leg_length); + PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq, + ¶m->leg_length); + PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, ¶m->yaw); + PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, ¶m->roll); + PID_Init(&c->pid.tp[0], KPID_MODE_CALC_D, target_freq, ¶m->tp); + PID_Init(&c->pid.tp[1], KPID_MODE_CALC_D, target_freq, ¶m->tp); + + /*初始化LQR控制器*/ + LQR_Init(&c->lqr[0], ¶m->lqr_gains); + LQR_Init(&c->lqr[1], ¶m->lqr_gains); + + /*初始化机体状态*/ + 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; + + /*初始化yaw控制状态*/ + c->yaw_control.target_yaw = 0.0f; + c->yaw_control.current_yaw = 0.0f; + c->yaw_control.yaw_force = 0.0f; + + return CHASSIS_OK; +} + +int8_t Chassis_UpdateFeedback(Chassis_t *c) { + if (c == NULL) { + return -1; // 参数错误 + } + /*更新电机反馈*/ + for (int i = 0; i < 4; i++) { + MOTOR_LZ_Update(&c->param->joint_motors[i]); + } + MOTOR_LK_Update(&c->param->wheel_motors[0]); + MOTOR_LK_Update(&c->param->wheel_motors[1]); + + // 更新关节电机反馈并检查离线状态 + for (int i = 0; i < 4; i++) { + MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_motors[i]); + if (joint_motor != NULL) { + if (joint_motor->motor.feedback.rotor_abs_angle > M_PI) { + joint_motor->motor.feedback.rotor_abs_angle -= M_2PI; + } + joint_motor->motor.feedback.rotor_abs_angle = + -joint_motor->motor.feedback.rotor_abs_angle; // 机械零点调整 + + c->feedback.joint[i] = joint_motor->motor.feedback; } - c->param = param; - - /*初始化can*/ - BSP_CAN_Init(); + } - /*初始化和注册所有电机*/ - MOTOR_LZ_Init(); - - for (int i = 0; i < 4; i++) { - MOTOR_LZ_Register(&c->param->joint_motors[i]); + // 更新轮子电机反馈并检查离线状态 + for (int i = 0; i < 2; i++) { + MOTOR_LK_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_motors[i]); + if (wheel_motor != NULL) { + c->feedback.wheel[i] = wheel_motor->motor.feedback; } - for (int i = 0; i < 2; i++) { - MOTOR_LK_Register(&c->param->wheel_motors[i]); - } - MOTOR_DM_Register(&c->param->yaw_motor); + } + /* 更新云台电机反馈数据（yaw轴） */ + MOTOR_DM_Update(&(c->param->yaw_motor)); + MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(c->param->yaw_motor)); + if (dm_motor != NULL) { + c->feedback.yaw = dm_motor->motor.feedback; + } + + // 更新机体状态估计 + Chassis_UpdateChassisState(c); + + return 0; +} + +int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu) { + if (c == NULL) { + return -1; // 参数错误 + } + c->feedback.imu = imu; + return 0; +} + +int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { + if (c == NULL || c_cmd == NULL) { + return CHASSIS_ERR_NULL; // 参数错误 + } + c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) / + 1000000.0f; /* 计算两次调用的时间间隔，单位秒 */ + c->lask_wakeup = BSP_TIME_Get_us(); + + /*设置底盘模式*/ + if (Chassis_SetMode(c, c_cmd->mode) != CHASSIS_OK) { + return CHASSIS_ERR_MODE; // 设置模式失败 + } + + // 更新VMC正解算用于状态估计 + VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, + c->feedback.joint[1].rotor_abs_angle, + c->feedback.imu.euler.pit, c->feedback.imu.gyro.y); + VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, + c->feedback.joint[2].rotor_abs_angle, + c->feedback.imu.euler.pit, c->feedback.imu.gyro.y); + + /*根据底盘模式执行不同的控制逻辑*/ + switch (c->mode) { + case CHASSIS_MODE_RELAX: + // 放松模式，电机不输出 + Chassis_MotorRelax(c); + + Chassis_LQRControl(c, c_cmd); + + break; + + case CHASSIS_MODE_RECOVER: { + float fn; + fn = -25.0f; + + VMC_InverseSolve(&c->vmc_[0], fn, 0.0f); + VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque, + &c->output.joint[1].torque); + VMC_InverseSolve(&c->vmc_[1], fn, 0.0f); + VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque, + &c->output.joint[2].torque); // Chassis_MotorEnable(c); - - /*初始化VMC控制器*/ - VMC_Init(&c->vmc_[0], ¶m->vmc_param[0], target_freq); - VMC_Init(&c->vmc_[1], ¶m->vmc_param[1], target_freq); + c->output.wheel[0] = 0.0f; + c->output.wheel[1] = 0.0f; + Chassis_Output(c); // 统一输出 + } break; - /*初始化pid*/ - PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq, ¶m->leg_length); - PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq, ¶m->leg_length); - PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, ¶m->yaw); - PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, ¶m->roll); - PID_Init(&c->pid.tp[0], KPID_MODE_CALC_D, target_freq, ¶m->tp); - PID_Init(&c->pid.tp[1], KPID_MODE_CALC_D, target_freq, ¶m->tp); + case CHASSIS_MODE_WHELL_LEG_BALANCE: + // 轮腿平衡模式，使用LQR控制和PID腿长控制 + // 执行LQR控制（包含PID腿长控制） + // Chassis_MotorRelax(c); + Chassis_LQRControl(c, c_cmd); + Chassis_Output(c); // 统一输出 + break; - /*初始化LQR控制器*/ - LQR_Init(&c->lqr[0], param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque); - LQR_SetGainMatrix(&c->lqr[0], ¶m->lqr_gains); - LQR_Init(&c->lqr[1], param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque); - LQR_SetGainMatrix(&c->lqr[1], ¶m->lqr_gains); - - /*初始化机体状态*/ - 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; - - /*初始化yaw控制状态*/ - c->yaw_control.target_yaw = 0.0f; - c->yaw_control.current_yaw = 0.0f; - c->yaw_control.yaw_force = 0.0f; + default: + return CHASSIS_ERR_MODE; + } + return CHASSIS_OK; +} + +int8_t Chassis_Output(Chassis_t *c) { + if (c == NULL) + return CHASSIS_ERR_NULL; + for (int i = 0; i < 4; i++) { + MOTOR_LZ_MotionParam_t param = {0}; + param.torque = c->output.joint[i].torque; + MOTOR_LZ_MotionControl(&c->param->joint_motors[i], ¶m); + } + BSP_TIME_Delay_us(200); // 等待CAN总线空闲，确保前面的命令发送完成 + for (int i = 0; i < 2; i++) { + MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]); + // MOTOR_LK_SetOutput(&c->param->wheel_motors[i], 0.0f); + } return CHASSIS_OK; } -int8_t Chassis_UpdateFeedback(Chassis_t *c){ - if (c == NULL) { - return -1; // 参数错误 - } - /*更新电机反馈*/ - for (int i = 0; i < 4; i++) { - MOTOR_LZ_Update(&c->param->joint_motors[i]); - } - MOTOR_LK_Update(&c->param->wheel_motors[0]); - MOTOR_LK_Update(&c->param->wheel_motors[1]); - - /*将电机反馈数据赋值到标准反馈结构体，并检查是否离线*/ - // 更新关节电机反馈并检查离线状态 - for (int i = 0; i < 4; i++) { - MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_motors[i]); - if (joint_motor != NULL) { - if (joint_motor->motor.feedback.rotor_abs_angle > M_PI ){ - joint_motor->motor.feedback.rotor_abs_angle -= M_2PI; - } - joint_motor->motor.feedback.rotor_abs_angle = - joint_motor->motor.feedback.rotor_abs_angle; // 机械零点调整 - - c->feedback.joint[i] = joint_motor->motor.feedback; - } - } - - // 更新轮子电机反馈并检查离线状态 - for (int i = 0; i < 2; i++) { - MOTOR_LK_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_motors[i]); - if (wheel_motor != NULL) { - c->feedback.wheel[i] = wheel_motor->motor.feedback; - } - } - - /* 更新云台电机反馈数据（yaw轴） */ - MOTOR_DM_Update(&(c->param->yaw_motor)); - MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(c->param->yaw_motor)); - if (dm_motor != NULL) { - c->feedback.yaw = dm_motor->motor.feedback; - } - - // 更新机体状态估计 - Chassis_UpdateChassisState(c); - - return 0; -} - -int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu){ - if (c == NULL) { - return -1; // 参数错误 - } - c->feedback.imu = imu; - // c->feedback.imu.euler.pit = - c->feedback.imu.euler.pit; - return 0; -} - -int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){ - if (c == NULL || c_cmd == NULL) { - return CHASSIS_ERR_NULL; // 参数错误 - } - c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) / 1000000.0f; /* 计算两次调用的时间间隔，单位秒 */ - c->lask_wakeup = BSP_TIME_Get_us(); - - /*设置底盘模式*/ - if (Chassis_SetMode(c, c_cmd->mode) != CHASSIS_OK) { - return CHASSIS_ERR_MODE; // 设置模式失败 - } - - /*根据底盘模式执行不同的控制逻辑*/ - switch (c->mode) { - case CHASSIS_MODE_RELAX: - // 放松模式，电机不输出 - - // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10); - MOTOR_LZ_Relax(&c->param->joint_motors[0]); - // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10); - MOTOR_LZ_Relax(&c->param->joint_motors[1]); - // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10); - MOTOR_LZ_Relax(&c->param->joint_motors[2]); - // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10); - MOTOR_LZ_Relax(&c->param->joint_motors[3]); - BSP_TIME_Delay_us(200); // 等待CAN总线空闲，确保前面的命令发送完成 - // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10); - MOTOR_LK_Relax(&c->param->wheel_motors[0]); - // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10); - MOTOR_LK_Relax(&c->param->wheel_motors[1]); - - // 更新VMC正解算用于状态估计 - VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle, - c->feedback.imu.euler.pit, c->feedback.imu.gyro.y); - VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle, - c->feedback.imu.euler.pit, c->feedback.imu.gyro.y); - - Chassis_LQRControl(c, c_cmd); - - break; - - case CHASSIS_MODE_RECOVER: - { - VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle, - c->feedback.imu.euler.pit, c->feedback.imu.gyro.y); - VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle, - c->feedback.imu.euler.pit, c->feedback.imu.gyro.y); - - float fn; - float tp1, tp2; - fn = -25.0f; - - - // tp1 = 3*PID_Calc(&c->pid.tp[0], 0.0f, c->vmc_[0].leg.theta, c->vmc_[0].leg.d_theta, c->dt); - // tp2 = 3*PID_Calc(&c->pid.tp[1], 0.0f, c->vmc_[1].leg.theta, c->vmc_[1].leg.d_theta, c->dt); - - VMC_InverseSolve(&c->vmc_[0], fn, 0.0f); - VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque, &c->output.joint[1].torque); - VMC_InverseSolve(&c->vmc_[1], fn, 0.0f); - VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque, &c->output.joint[2].torque); - // Chassis_MotorEnable(c); - c->output.wheel[0] = 0.0f; - c->output.wheel[1] = 0.0f; - Chassis_Output(c); // 统一输出 - } - break; - - case CHASSIS_MODE_WHELL_BALANCE: - // 更新VMC正解算用于状态估计 - for (int i = 0; i < 4; i++) { - c->output.joint[i].torque = 0.0f; - } - for (int i = 0; i < 2; i++) { - c->output.wheel[i] = 0.0f; - } - - // 更新VMC正解算用于状态估计 - VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle, - c->feedback.imu.euler.pit, c->feedback.imu.gyro.y); - VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle, - c->feedback.imu.euler.pit, c->feedback.imu.gyro.y); - - // VMC_InverseSolve(&c->vmc_[1], fn, tp); - // VMC_GetJointTorques(&c->vmc_[1], &t1, &t2); - - // c->output.joint[3].torque = t1; - // c->output.joint[2].torque = t2; - - Chassis_LQRControl(c, c_cmd); // 即使在放松模式下也执行LQR以保持状态更新 - // c->output.wheel[0] = 0.2f; - // c->output.wheel[1] = 0.2f; - Chassis_Output(c); // 统一输出 - break; - - case CHASSIS_MODE_WHELL_LEG_BALANCE: - // 轮腿平衡模式，使用LQR控制和PID腿长控制 - - // 更新VMC正解算 - VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle, - c->feedback.imu.euler.pit, c->feedback.imu.gyro.y); - VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle, - c->feedback.imu.euler.pit, c->feedback.imu.gyro.y); - - // 执行LQR控制（包含PID腿长控制） - if (Chassis_LQRControl(c, c_cmd) != 0) { - // LQR控制失败，切换到安全模式 - return CHASSIS_ERR; - } - - Chassis_Output(c); // 统一输出 - break; - - default: - return CHASSIS_ERR_MODE; - } - - return CHASSIS_OK; -} - -void Chassis_Output(Chassis_t *c) { - if (c == NULL) return; - for (int i = 0; i < 4; i++) { - MOTOR_LZ_MotionParam_t param = {0}; - param.torque = c->output.joint[i].torque; - MOTOR_LZ_MotionControl(&c->param->joint_motors[i], ¶m); - } - BSP_TIME_Delay_us(400); // 等待CAN总线空闲，确保前面的命令发送完成 - for (int i = 0; i < 2; i++) { - MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]); - // MOTOR_LK_SetOutput(&c->param->wheel_motors[i], 0.0f); - } - -} - int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { - if (c == NULL || c_cmd == NULL) { - return CHASSIS_ERR_NULL; - } + if (c == NULL || c_cmd == NULL) { + return CHASSIS_ERR_NULL; + } - /* 参考C++版本实现的LQR控制 */ - - /* 地面接触检测（参考C++版本） */ - float leg_fn[2]; - bool onground_flag[2]; - - // 暂时关闭离地监测，强制设置为着地状态 - leg_fn[0] = VMC_GroundContactDetection(&c->vmc_[0]); - leg_fn[1] = VMC_GroundContactDetection(&c->vmc_[1]); - onground_flag[0] = true; // 强制设置左腿着地 - onground_flag[1] = true; // 强制设置右腿着地 - - /* 获取VMC状态（等效摆杆参数） */ - float leg_L0[2], leg_theta[2], leg_d_theta[2]; - VMC_GetVirtualLegState(&c->vmc_[0], &leg_L0[0], &leg_theta[0], NULL, &leg_d_theta[0]); - VMC_GetVirtualLegState(&c->vmc_[1], &leg_L0[1], &leg_theta[1], NULL, &leg_d_theta[1]); - - /* 运动参数（参考C++版本的状态估计） */ - static float xhat = 0.0f, x_dot_hat = 0.0f; - float target_dot_x = 0.0f; - - // 简化的速度估计（后续可以改进为C++版本的复杂滤波） - x_dot_hat = c->chassis_state.velocity_x; - xhat = c->chassis_state.position_x; - - // 目标设定 - target_dot_x = c_cmd->move_vec.vx*2; - // target_dot_x = SpeedLimit_TargetSpeed(300.0f, c->chassis_state.velocity_x, target_dot_x, c->dt); // 速度限制器，假设最大加速度为1 m/s² - c->chassis_state.target_x += target_dot_x * c->dt; - - /* 分别计算左右腿的LQR控制 */ - float Tw[2] = {0.0f, 0.0f}; // 轮毂力矩 - float Tp[2] = {0.0f, 0.0f}; // 髋关节力矩 - - for (int leg = 0; leg < 2; leg++) { - /* 构建当前状态 */ - LQR_State_t current_state = {0}; - current_state.theta = leg_theta[leg]; - current_state.d_theta = leg_d_theta[leg]; - current_state.x = xhat; - current_state.d_x = x_dot_hat; - current_state.phi = -c->feedback.imu.euler.pit; - current_state.d_phi = -c->feedback.imu.gyro.y; - - /* 构建目标状态 */ - LQR_State_t target_state = {0}; - target_state.theta = 0.0f; // 目标摆杆角度 - target_state.d_theta = 0.0f; // 目标摆杆角速度 - // target_state.x = 0; // 目标位置 - // target_state.d_x = 0.0f; // 目标速度 - target_state.phi = -0.2f; // 目标俯仰角 - target_state.d_phi = 0.0f; // 目标俯仰角速度 - // target_state.theta = -0.8845f * leg_L0[leg] + 0.40663f; // 目标摆杆角度 - // target_state.d_theta = 0.0f; // 目标摆杆角速度 - target_state.x = c->chassis_state.target_x; // 目标位置 - target_state.d_x = target_dot_x; // 目标速度 - // target_state.phi = 0.16f; // 目标俯仰角 - // target_state.d_phi = 0.0f; // 目标俯仰角速度 - - /* 根据当前腿长更新增益矩阵 */ - LQR_CalculateGainMatrix(&c->lqr[leg], leg_L0[leg]); - - /* 更新LQR状态 */ - LQR_SetTargetState(&c->lqr[leg], &target_state); - LQR_UpdateState(&c->lqr[leg], ¤t_state); - - if (onground_flag[leg]) { - /* 接地状态：使用LQR控制器计算输出 */ - if (LQR_Control(&c->lqr[leg]) == 0) { - LQR_Input_t lqr_output = {0}; - if (LQR_GetOutput(&c->lqr[leg], &lqr_output) == 0) { - Tw[leg] = lqr_output.T; - Tp[leg] = lqr_output.Tp; - // Tw[leg] = 0.0f; // 暂时屏蔽轮毂力矩输出 - // Tp[leg] = -2.5f; // 暂时屏蔽腿部力矩输出 - } else { - Tw[leg] = 0.0f; - Tp[leg] = 0.0f; - } - } else { - Tw[leg] = 0.0f; - Tp[leg] = 0.0f; - } - } else { - /* 离地状态：简化控制，只控制腿部摆动 */ - Tw[leg] = 0.0f; - // 只控制摆杆角度 - float theta_error = current_state.theta - target_state.theta; - float d_theta_error = current_state.d_theta - target_state.d_theta; - Tp[leg] = -(c->lqr[leg].K_matrix[1][0] * theta_error + c->lqr[leg].K_matrix[1][1] * d_theta_error); - } - } + /* 运动参数（参考C++版本的状态估计） */ + static float xhat = 0.0f, x_dot_hat = 0.0f; + float target_dot_x = 0.0f; - // c->yaw_control.current_yaw = c->feedback.imu.euler.yaw; + // 简化的速度估计（后续可以改进为C++版本的复杂滤波） + x_dot_hat = c->chassis_state.velocity_x; + xhat = c->chassis_state.position_x; - // c->yaw_control.target_yaw -= c_cmd->move_vec.vy * 0.005f; // 目标偏航角，假设遥控器输入范围为[-10, 10]，映射到[-0.02, 0.02] rad/s + // 目标设定 + target_dot_x = c_cmd->move_vec.vx * 2; + c->chassis_state.target_x += target_dot_x * c->dt; - // // 修正目标yaw角度到[-pi, pi] - // if (c->yaw_control.target_yaw > M_PI) { - // c->yaw_control.target_yaw -= M_2PI; - // } else if (c->yaw_control.target_yaw < -M_PI) { - // c->yaw_control.target_yaw += M_2PI; - // } + /*更新当前状态*/ + LQR_State_t current_state = {0}; + current_state.theta = c->vmc_[0].leg.theta; // 左腿摆杆角度 + current_state.d_theta = c->vmc_[0].leg.d_theta; // 左腿摆杆角速度 + current_state.x = xhat; // 机体位置 + current_state.d_x = x_dot_hat; // 机体速度 + current_state.phi = -c->feedback.imu.euler.pit; // 机体俯仰角 + current_state.d_phi = -c->feedback.imu.gyro.y; // 机体俯仰角速度 + LQR_UpdateState(&c->lqr[0], ¤t_state); + LQR_CalculateGainMatrix(&c->lqr[0], c->vmc_[0].leg.L0); - // c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, c->feedback.imu.euler.yaw, c->feedback.imu.gyro.z, c->dt); - c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle; - c->yaw_control.target_yaw = c->param->mech_zero_yaw; - 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); - - /* 轮毂力矩输出（参考C++版本的减速比） */ - c->output.wheel[0] = Tw[0] / 4.5f + c->yaw_control.yaw_force; - c->output.wheel[1] = Tw[1] / 4.5f - c->yaw_control.yaw_force; - /* 腿长控制和VMC逆解算（使用PID控制） */ - float virtual_force[2]; - float target_L0[2]; - float leg_d_length[2]; // 腿长变化率 - - /* 横滚角PID补偿计算 */ - float roll_compensation = PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol, c->feedback.imu.gyro.x, c->dt); - - // 目标腿长设定 - target_L0[0] = 0.15f + c_cmd->height * 0.2f + roll_compensation; // 左腿：基础腿长 + 高度调节 + 横滚角补偿 - target_L0[1] = 0.15f + c_cmd->height * 0.2f - roll_compensation; // 右腿：基础腿长 + 高度调节 - 横滚角补偿 - - // 获取腿长变化率 - VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL); - VMC_GetVirtualLegState(&c->vmc_[1], NULL, NULL, &leg_d_length[1], NULL); - - /* 左右腿摆角相互补偿（参考C++版本的Delta_Tp机制） */ - float Delta_Tp[2]; - // 使用tp_pid进行左右腿摆角同步控制 - // 左腿补偿力矩：使左腿theta向右腿theta靠拢 - Delta_Tp[0] = leg_L0[0] * 10.0f * PID_Calc(&c->pid.tp[0], leg_theta[1], leg_theta[0], leg_d_theta[0], c->dt); - // 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反） - Delta_Tp[1] = leg_L0[1] * 10.0f * PID_Calc(&c->pid.tp[1], leg_theta[0], leg_theta[1], leg_d_theta[1], c->dt); - - for (int leg = 0; leg < 2; leg++) { - // 使用PID进行腿长控制 - float pid_output = PID_Calc(&c->pid.leg_length[leg], target_L0[leg], leg_L0[leg], leg_d_length[leg], c->dt); - - // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 - virtual_force[leg] = (Tp[leg]) * sinf(leg_theta[leg]) + - pid_output + 30.0f; - // + // PID腿长控制输出 - // 45.0f; // 基础支撑力 - - // VMC逆解算（包含摆角补偿） - // virtual_force[leg] = 0.0f; // 暂时屏蔽虚拟力输出，避免VMC逆解算失败 - // Tp[leg] = 0.0f; // 暂时屏蔽腿部力矩输出，避免VMC逆解算失败 - // Delta_Tp[leg] = 0.0f; // 暂时屏蔽腿部力矩输出，避免VMC逆解算失败 - if (VMC_InverseSolve(&c->vmc_[leg], virtual_force[leg], Tp[leg] + Delta_Tp[leg]) == 0) { - // if (VMC_InverseSolve(&c->vmc_[leg], 0.0, Tp[leg] + Delta_Tp[leg]) == 0) { - if (leg == 0) { - VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque, &c->output.joint[1].torque); - } else { - VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque, &c->output.joint[2].torque); - } - } else { - // VMC失败，设为0力矩 - if (leg == 0) { - c->output.joint[0].torque = 0.0f; - c->output.joint[1].torque = 0.0f; - } else { - c->output.joint[2].torque = 0.0f; - c->output.joint[3].torque = 0.0f; - } - } + current_state.theta = c->vmc_[1].leg.theta; // 右腿摆杆角度 + current_state.d_theta = c->vmc_[1].leg.d_theta; // 右腿摆杆角速度 + LQR_UpdateState(&c->lqr[1], ¤t_state); + LQR_CalculateGainMatrix(&c->lqr[1], c->vmc_[1].leg.L0); + + LQR_State_t target_state = {0}; + target_state.theta = 0.0f; // 目标摆杆角度 + target_state.d_theta = 0.0f; // 目标摆杆角速度 + target_state.phi = -0.2f; // 目标俯仰角 + target_state.d_phi = 0.0f; // 目标俯仰角速度 + target_state.x = c->chassis_state.target_x; // 目标位置 + target_state.d_x = target_dot_x; // 目标速度 + LQR_SetTargetState(&c->lqr[0], &target_state); + LQR_SetTargetState(&c->lqr[1], &target_state); + + LQR_Control(&c->lqr[0]); + LQR_Control(&c->lqr[1]); + + // /* 分别计算左右腿的LQR控制 */ + // float Tw[2] = {0.0f, 0.0f}; // 轮毂力矩 + // float Tp[2] = {0.0f, 0.0f}; // 髋关节力矩 + + // for (int leg = 0; leg < 2; leg++) { + // /* 构建当前状态 */ + // LQR_State_t current_state = {0}; + // current_state.theta = leg_theta[leg]; + // current_state.d_theta = leg_d_theta[leg]; + // current_state.x = xhat; + // current_state.d_x = x_dot_hat; + // current_state.phi = -c->feedback.imu.euler.pit; + // current_state.d_phi = -c->feedback.imu.gyro.y; + + // /* 构建目标状态 */ + // LQR_State_t target_state = {0}; + // target_state.theta = 0.0f; // 目标摆杆角度 + // target_state.d_theta = 0.0f; // 目标摆杆角速度 + // // target_state.x = 0; // 目标位置 + // // target_state.d_x = 0.0f; // 目标速度 + // target_state.phi = -0.2f; // 目标俯仰角 + // target_state.d_phi = 0.0f; // 目标俯仰角速度 + // // target_state.theta = -0.8845f * leg_L0[leg] + 0.40663f; // + // 目标摆杆角度 + // // target_state.d_theta = 0.0f; // 目标摆杆角速度 + // target_state.x = c->chassis_state.target_x; // 目标位置 + // target_state.d_x = target_dot_x; // 目标速度 + // // target_state.phi = 0.16f; // 目标俯仰角 + // // target_state.d_phi = 0.0f; // 目标俯仰角速度 + + // /* 根据当前腿长更新增益矩阵 */ + // LQR_CalculateGainMatrix(&c->lqr[leg], leg_L0[leg]); + + // /* 更新LQR状态 */ + // LQR_SetTargetState(&c->lqr[leg], &target_state); + // LQR_UpdateState(&c->lqr[leg], ¤t_state); + + // if (onground_flag[leg]) { + // /* 接地状态：使用LQR控制器计算输出 */ + // if (LQR_Control(&c->lqr[leg]) == 0) { + // LQR_Input_t lqr_output = {0}; + // if (LQR_GetOutput(&c->lqr[leg], &lqr_output) == 0) { + // Tw[leg] = lqr_output.T; + // Tp[leg] = lqr_output.Tp; + // // Tw[leg] = 0.0f; // 暂时屏蔽轮毂力矩输出 + // // Tp[leg] = -2.5f; // 暂时屏蔽腿部力矩输出 + // } else { + // Tw[leg] = 0.0f; + // Tp[leg] = 0.0f; + // } + // } else { + // Tw[leg] = 0.0f; + // Tp[leg] = 0.0f; + // } + // } else { + // /* 离地状态：简化控制，只控制腿部摆动 */ + // Tw[leg] = 0.0f; + // // 只控制摆杆角度 + // float theta_error = current_state.theta - target_state.theta; + // float d_theta_error = current_state.d_theta - target_state.d_theta; + // Tp[leg] = -(c->lqr[leg].K_matrix[1][0] * theta_error + + // c->lqr[leg].K_matrix[1][1] * d_theta_error); + // } + // } + + // c->yaw_control.current_yaw = c->feedback.imu.euler.yaw; + + // c->yaw_control.target_yaw -= c_cmd->move_vec.vy * 0.005f; // + // 目标偏航角，假设遥控器输入范围为[-10, 10]，映射到[-0.02, 0.02] rad/s + + // // 修正目标yaw角度到[-pi, pi] + // if (c->yaw_control.target_yaw > M_PI) { + // c->yaw_control.target_yaw -= M_2PI; + // } else if (c->yaw_control.target_yaw < -M_PI) { + // c->yaw_control.target_yaw += M_2PI; + // } + + // c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, + // c->feedback.imu.euler.yaw, c->feedback.imu.gyro.z, c->dt); + + 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; + 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); + + /* 轮毂力矩输出（参考C++版本的减速比） */ + 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 target_L0[2]; + float leg_d_length[2]; // 腿长变化率 + + /* 横滚角PID补偿计算 */ + float roll_compensation = + PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol, + c->feedback.imu.gyro.x, c->dt); + + // 目标腿长设定 + target_L0[0] = 0.15f + c_cmd->height * 0.2f + + roll_compensation; // 左腿：基础腿长 + 高度调节 + 横滚角补偿 + target_L0[1] = 0.15f + c_cmd->height * 0.2f - + roll_compensation; // 右腿：基础腿长 + 高度调节 - 横滚角补偿 + + // 获取腿长变化率 + VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL); + VMC_GetVirtualLegState(&c->vmc_[1], NULL, NULL, &leg_d_length[1], NULL); + + /* 左右腿摆角相互补偿（参考C++版本的Delta_Tp机制） */ + float Delta_Tp[2]; + // 使用tp_pid进行左右腿摆角同步控制 + // 左腿补偿力矩：使左腿theta向右腿theta靠拢 + Delta_Tp[0] = c->vmc_[0].leg.L0 * 10.0f * + PID_Calc(&c->pid.tp[0], c->vmc_[1].leg.theta, + c->vmc_[0].leg.theta, 0, c->dt); + // 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反） + Delta_Tp[1] = -Delta_Tp[0]; + + float out = 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.F_virtual = out; + out = 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.F_virtual = out; + + c->vmc_[0].leg.T_virtual = c->lqr[0].control_output.Tp + Delta_Tp[0]; + c->vmc_[1].leg.T_virtual = c->lqr[1].control_output.Tp + Delta_Tp[1]; + + VMC_InverseSolve(&c->vmc_[0], c->vmc_[0].leg.F_virtual, + c->vmc_[0].leg.T_virtual); + VMC_InverseSolve(&c->vmc_[1], c->vmc_[1].leg.F_virtual, + c->vmc_[1].leg.T_virtual); + + VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque, + &c->output.joint[1].torque); + VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque, + &c->output.joint[2].torque); + + /* 安全限制 */ + for (int i = 0; i < 2; i++) { + if (fabsf(c->output.wheel[i]) > 0.8f) { + c->output.wheel[i] = (c->output.wheel[i] > 0) ? 0.8f : -0.8f; } - - /* 安全限制 */ - for (int i = 0; i < 2; i++) { - if (fabsf(c->output.wheel[i]) > 0.8f) { - c->output.wheel[i] = (c->output.wheel[i] > 0) ? 0.8f : -0.8f; - } + } + + for (int i = 0; i < 4; i++) { + if (fabsf(c->output.joint[i].torque) > 20.0f) { + c->output.joint[i].torque = + (c->output.joint[i].torque > 0) ? 20.0f : -20.0f; } - - for (int i = 0; i < 4; i++) { - if (fabsf(c->output.joint[i].torque) > 20.0f) { - c->output.joint[i].torque = (c->output.joint[i].torque > 0) ? 20.0f : -20.0f; - } - } - - return CHASSIS_OK; + } + + return CHASSIS_OK; } diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index d102d34..a26ce80 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -117,7 +117,7 @@ typedef struct { Chassis_Output_t output; VMC_t vmc_[2]; /* 两条腿的VMC */ - LQR_Controller_t lqr[2]; /* 两条腿的LQR控制器 */ + LQR_t lqr[2]; /* 两条腿的LQR控制器 */ int8_t state; @@ -219,7 +219,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd); * \param s 包含底盘数据的结构体 * \param out CAN设备底盘输出结构体 */ -void Chassis_Output(Chassis_t *c); +int8_t Chassis_Output(Chassis_t *c); #ifdef __cplusplus } diff --git a/User/module/config.c b/User/module/config.c index 7684587..78d9c3b 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -141,7 +141,7 @@ Config_RobotParam_t robot_config = { .reverse = false, }, - .mech_zero_yaw = 1.21085676f, /* 250.5度，机械零点 */ + .mech_zero_yaw = 1.20564249f, /* 250.5度，机械零点 */ .vmc_param = { { // 左腿 diff --git a/User/task/rc.c b/User/task/rc.c index 53430a6..b22edc9 100644 --- a/User/task/rc.c +++ b/User/task/rc.c @@ -83,7 +83,7 @@ void Task_rc(void *argument) { cmd_to_chassis.mode = CHASSIS_MODE_RECOVER; break; case RC_CAN_SW_DOWN: // 下位 - cmd_to_chassis.mode = CHASSIS_MODE_WHELL_BALANCE; + cmd_to_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE; break; default: cmd_to_chassis.mode = CHASSIS_MODE_RELAX; diff --git a/mod_wheelleg_chassis.cpp b/mod_wheelleg_chassis.cpp new file mode 100644 index 0000000..8f82c6c --- /dev/null +++ b/mod_wheelleg_chassis.cpp @@ -0,0 +1,662 @@ +#include "mod_wheelleg_chassis.hpp" + +#include +#include + +using namespace Module; +Wheelleg::Wheelleg(Param& param) + : param_(param), + roll_pid_(param.roll_pid_param, 333.0f), + yaw_pid_(param.yaw_pid_param, 333.0f), + yaccl_pid_(param.yaccl_pid_param, 333.0f), + lowfilter_(333.0f, 50.0f), + acclfilter_(333.0f, 30.0f), + manfilter_(param.manfilter_param), + + ctrl_lock_(true) { + memset(&(this->cmd_), 0, sizeof(this->cmd_)); + + this->hip_motor_.at(0) = + new Device::MitMotor(param.hip_motor_param.at(0), "hip_left_front"); + this->hip_motor_.at(1) = + new Device::MitMotor(param.hip_motor_param.at(1), "hip_left_back"); + this->hip_motor_.at(2) = + new Device::MitMotor(param.hip_motor_param.at(2), "hip_right_front"); + this->hip_motor_.at(3) = + new Device::MitMotor(param.hip_motor_param.at(3), "hip_left_back"); + + this->wheel_motor_.at(0) = + new Device::RMMotor(param.wheel_motor_param.at(0), "wheel_left"); + this->wheel_motor_.at(1) = + new Device::RMMotor(param.wheel_motor_param.at(1), "wheel_right"); + + this->leglength_pid_.at(0) = + new Component::PID(param.leglength_pid_param.at(0), 333.0f); + this->leglength_pid_.at(1) = + new Component::PID(param.leglength_pid_param.at(1), 333.0f); + + this->theta_pid_.at(0) = + new Component::PID(param.theta_pid_param.at(0), 333.0f); + this->theta_pid_.at(1) = + new Component::PID(param.theta_pid_param.at(1), 333.0f); + + this->tp_pid_.at(0) = new Component::PID(param.Tp_pid_param.at(0), 333.0); + this->tp_pid_.at(1) = new Component::PID(param.Tp_pid_param.at(1), 333.0); + + this->leg_.at(0) = new Component::VMC(param_.leg_param.at(0), 333.0f); + this->leg_.at(1) = new Component::VMC(param_.leg_param.at(1), 333.0f); + + auto event_callback = [](Wheelleg::ChassisEvent event, Wheelleg* chassis) { + chassis->ctrl_lock_.Wait(UINT32_MAX); + switch (event) { + case SET_MODE_RELAX: + chassis->SetMode(RELAX); + break; + case SET_MODE_STAND: + chassis->SetMode(STAND); + break; + case SET_MODE_ROTOR: + chassis->SetMode(ROTOR); + break; + case SET_MODE_RESET: + chassis->SetMode(RESET); + break; + default: + break; + } + chassis->ctrl_lock_.Post(); + }; + + Component::CMD::RegisterEvent( + event_callback, this, this->param_.EVENT_MAP); + + auto chassis_thread = [](Wheelleg* chassis) { + auto cmd_sub = + Message::Subscriber("cmd_chassis"); + auto eulr_sub = + Message::Subscriber("chassis_imu_eulr"); + auto gyro_sub = + Message::Subscriber("chassis_gyro"); + + auto yaw_sub = Message::Subscriber("chassis_yaw"); + + auto accl_sub = + Message::Subscriber("chassis_imu_accl_abs"); + // auto yaw_sub = Message::Subscriber("chassis_yaw"); + uint32_t last_online_time = bsp_time_get_ms(); + + while (1) { + eulr_sub.DumpData(chassis->eulr_); /* imu */ + gyro_sub.DumpData(chassis->gyro_); /* imu */ + accl_sub.DumpData(chassis->accl_); + + yaw_sub.DumpData(chassis->yaw_); + cmd_sub.DumpData(chassis->cmd_); + // yaw_sub.DumpData(chassis->yaw_); + + chassis->ctrl_lock_.Wait(UINT32_MAX); + chassis->MotorSetAble(); + chassis->UpdateFeedback(); + chassis->Calculate(); + chassis->Control(); + chassis->ctrl_lock_.Post(); + + /* 运行结束，等待下一次唤醒 */ + chassis->thread_.SleepUntil(3, last_online_time); + } + }; + this->thread_.Create(chassis_thread, this, "chassis_thread", 1536, + System::Thread::MEDIUM); +} + +void Wheelleg::MotorSetAble() { + if (this->hip_motor_flag_ == 0) { + this->hip_motor_[0]->Relax(); + this->hip_motor_[1]->Relax(); + this->hip_motor_[2]->Relax(); + this->hip_motor_[3]->Relax(); + this->dm_motor_flag_ = 0; + } + + else { + if (this->dm_motor_flag_ == 0) { + for (int i = 0; i < 5; i++) { + this->hip_motor_[0]->Enable(); + } + for (int i = 0; i < 5; i++) { + this->hip_motor_[1]->Enable(); + } + for (int i = 0; i < 5; i++) { + this->hip_motor_[2]->Enable(); + } + for (int i = 0; i < 5; i++) { + this->hip_motor_[3]->Enable(); + } + + this->dm_motor_flag_ = 1; + } + }; +} + +void Wheelleg::UpdateFeedback() { + this->now_ = bsp_time_get(); + this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_); + this->last_wakeup_ = this->now_; + + this->wheel_motor_[0]->Update(); + this->wheel_motor_[1]->Update(); + + this->leg_argu_[0].phi4_ = + this->hip_motor_[0]->GetAngle() - + this->param_.wheel_param.mech_zero[0]; // 前关节角度 + this->leg_argu_[0].phi1_ = + this->hip_motor_[1]->GetAngle() - + this->param_.wheel_param.mech_zero[1]; // 后关节角度 + + this->leg_argu_[1].phi4_ = + (-this->hip_motor_[2]->GetAngle() + + this->param_.wheel_param.mech_zero[3]); // 前关节角度 + if (leg_argu_[1].phi4_ > M_PI) { + this->leg_argu_[1].phi4_ = this->leg_argu_[1].phi4_ - 2 * M_PI; + } + + this->leg_argu_[1].phi1_ = + (-this->hip_motor_[3]->GetAngle() + + this->param_.wheel_param.mech_zero[2]); // 后关节角度 + if (leg_argu_[1].phi1_ > M_PI) { + this->leg_argu_[1].phi1_ = this->leg_argu_[1].phi1_ - 2 * M_PI; + } + + this->pit_ = this->eulr_.pit; + if (this->pit_ > M_PI) { + this->pit_ = this->eulr_.pit - 2 * M_PI; + } + + /* 注意极性 */ + std::tuple result0 = + this->leg_[0]->VMCsolve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_, + this->pit_, -this->gyro_.x, this->dt_); + this->leg_argu_[0].L0 = std::get<0>(result0); + this->leg_argu_[0].d_L0 = std::get<1>(result0); + this->leg_argu_[0].theta = -std::get<2>(result0); + this->leg_argu_[0].d_theta = -std::get<3>(result0); + + if (leg_argu_[0].theta > M_PI) { + leg_argu_[0].theta = leg_argu_[0].theta - 2 * M_PI; + } + if (leg_argu_[0].theta < -M_PI) { + leg_argu_[0].theta = leg_argu_[0].theta + 2 * M_PI; + } + + std::tuple result1 = + this->leg_[1]->VMCsolve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_, + this->pit_, -this->gyro_.x, this->dt_); + this->leg_argu_[1].L0 = std::get<0>(result1); + this->leg_argu_[1].d_L0 = std::get<1>(result1); + this->leg_argu_[1].theta = -std::get<2>(result1); + this->leg_argu_[1].d_theta = -std::get<3>(result1); + + if (leg_argu_[1].theta > M_PI) { + leg_argu_[1].theta = leg_argu_[1].theta - 2 * M_PI; + } + if (leg_argu_[1].theta < -M_PI) { + leg_argu_[1].theta = leg_argu_[1].theta + 2 * M_PI; + } + + /* 轮子转速近似于编码器速度 由此推测机体速度 */ + this->leg_argu_[0].single_x_dot = + -wheel_motor_[0]->GetSpeed() * 2 * M_PI * + (this->param_.wheel_param.wheel_radius) / 60.f / 15.765 + + leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) + + leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta); + + this->leg_argu_[1].single_x_dot = + wheel_motor_[1]->GetSpeed() * 2 * M_PI * + (this->param_.wheel_param.wheel_radius) / 60.f / 15.765 + + leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) + + leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta); + + this->move_argu_.last_x_dot = this->move_argu_.x_dot; + this->move_argu_.x_dot = + + (this->leg_argu_[0].single_x_dot + this->leg_argu_[1].single_x_dot) / 2; + this->move_argu_.x_dot = + (-wheel_motor_[0]->GetSpeed() + wheel_motor_[1]->GetSpeed()) * M_PI * + this->param_.wheel_param.wheel_radius / 60.f / 15.765; + this->move_argu_.x = this->move_argu_.x_dot * dt_ + move_argu_.x; + + this->move_argu_.delta_speed = + lowfilter_.Apply((move_argu_.x_dot - move_argu_.last_x_dot) / dt_); + + this->move_argu_.accl = acclfilter_.Apply(this->accl_.y * 9.8f); + + if (now_ > 1000000) { + this->move_argu_.x_dot_hat = + manfilter_.comp_filter(move_argu_.x_dot, move_argu_.delta_speed, + this->move_argu_.last_x_dot, accl_.y * 9.8f, + dt_) - + ((leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) + + leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta)) + + (leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) + + leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta))) / + 2; + this->move_argu_.xhat = dt_ * move_argu_.x_dot_hat + move_argu_.xhat; + } +} + +void Wheelleg::Calculate() { + switch (this->mode_) { + case Wheelleg::RELAX: + // if (fabs(move_argu_.target_dot_x - cmd_.y * 1.5f) > 0.005) { + // if (move_argu_.target_dot_x > cmd_.y * 1.5f) { + // move_argu_.target_dot_x -= 0.004; + // } + // if (move_argu_.target_dot_x < cmd_.y * 1.5f) { + // move_argu_.target_dot_x += 0.004; + // } + // } else { + // move_argu_.target_dot_x = cmd_.y * 1.5f; + // } + // move_argu_.target_x = move_argu_.target_dot_x * dt_ + + // move_argu_.target_x; + move_argu_.target_x = 0; + move_argu_.target_dot_x = 0; + break; + case Wheelleg::STAND: + + /* 0.002为最大加速度 即500hz*0.002 梯度式递增减 */ + if (fabs(move_argu_.target_dot_x - + cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005) { + if (move_argu_.target_dot_x > + cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) { + move_argu_.target_dot_x -= 0.004; + } + if (move_argu_.target_dot_x < + cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) { + move_argu_.target_dot_x += 0.004; + } + } else { + move_argu_.target_dot_x = + cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed; + } + move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x; + + this->move_argu_.target_yaw = 0.0f; + + /*双零点*/ + if (this->yaw_ > M_PI_2) { + this->move_argu_.target_yaw = 3.14158f; + } + if (this->yaw_ < -M_PI_2) { + this->move_argu_.target_yaw = 3.14158f; + } + break; + + case Wheelleg::ROTOR: + if (fabs(move_argu_.target_dot_x - + cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005) { + if (move_argu_.target_dot_x > + cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) { + move_argu_.target_dot_x -= 0.004; + } + if (move_argu_.target_dot_x < + cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) { + move_argu_.target_dot_x += 0.004; + } + } else { + move_argu_.target_dot_x = + cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed; + } + move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x; + this->move_argu_.target_yaw = this->yaw_ + 0.01; + + break; + // move_argu_.target_dot_x = cmd_.x; + // move_argu_.target_x = + // move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x; + // // move_argu_.target_dot_x = sqrtf(cmd_.x * cmd_.x + cmd_.y * + // cmd_.y); + // // move_argu_.x_dot = + // // move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x; + // // move_argu_.target_yaw = atan2(cmd_.x, cmd_.y); + // break; + + case Wheelleg::RESET: + this->move_argu_.target_dot_x = 0; + move_argu_.target_x = 0; + this->move_argu_.target_yaw = this->eulr_.yaw; + this->move_argu_.xhat = 0; + + // move_argu_.target_yaw - atan2(cmd_.x, cmd_.y); + // if ((fabs(move_argu_.target_yaw) - fabs(eulr_.yaw)) > M_PI / 2) { + // this->move_argu_.target_dot_x = -this->move_argu_.target_dot_x; + // } + break; + + default: + XB_ASSERT(false); + return; + } + + leg_argu_[0].Fn = leg_[0]->GndDetector(leg_argu_[0].Tp, leg_argu_[0].F0); + onground0_flag_ = false; + if (leg_argu_[0].Fn > 30) { + onground0_flag_ = true; + } + leg_argu_[1].Fn = leg_[1]->GndDetector(leg_argu_[1].Tp, leg_argu_[1].F0); + onground1_flag_ = false; + if (leg_argu_[1].Fn > 30) { + onground1_flag_ = true; + } + std::tuple result3; + std::tuple result4; + + switch (this->mode_) { + case Wheelleg::RELAX: + case Wheelleg::ROTOR: + case Wheelleg::STAND: + + for (int i = 0; i < 12; i++) { + leg_argu_[0].LQR_K[i] = this->leg_[0]->LQR_K_calc( + &this->param_.wheel_param.K_Poly_Coefficient_L[i][0], + leg_argu_[0].L0); + } + + for (int i = 0; i < 12; i++) { + leg_argu_[1].LQR_K[i] = this->leg_[1]->LQR_K_calc( + &this->param_.wheel_param.K_Poly_Coefficient_R[i][0], + leg_argu_[1].L0); + } + if (now_ > 1000000) + if (fabs(move_argu_.target_dot_x) > 0.5 || + fabs(move_argu_.target_dot_x - move_argu_.x_dot_hat) > 0.7 || + ((!onground0_flag_) & (!onground1_flag_))) { + leg_argu_[0].LQR_K[2] = 0; + leg_argu_[1].LQR_K[2] = 0; + this->move_argu_.xhat = 0; + move_argu_.target_x = 0; + } + + if (onground0_flag_) { + leg_argu_[0].Tw = + (leg_argu_[0].LQR_K[0] * + (-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) + + leg_argu_[0].LQR_K[1] * (-leg_argu_[0].d_theta) + + leg_argu_[0].LQR_K[2] * + (-move_argu_.xhat + move_argu_.target_x - 0.56) + + leg_argu_[0].LQR_K[3] * + (-move_argu_.x_dot_hat + move_argu_.target_dot_x) + + leg_argu_[0].LQR_K[4] * (-this->pit_ + 0.16) + + leg_argu_[0].LQR_K[5] * (-this->gyro_.x + 0.0f)); + leg_argu_[0].Tp = + (leg_argu_[0].LQR_K[6] * + (-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) + + leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta) + + leg_argu_[0].LQR_K[8] * + (-move_argu_.xhat + move_argu_.target_x - 0.56) + + leg_argu_[0].LQR_K[9] * + (-move_argu_.x_dot_hat + move_argu_.target_dot_x) + + leg_argu_[0].LQR_K[10] * (-this->pit_ + 0.16) + + leg_argu_[0].LQR_K[11] * (-this->gyro_.x + 0.0f)); + } else { + leg_argu_[0].Tw = 0; + + leg_argu_[0].Tp = + (leg_argu_[0].LQR_K[6] * (-leg_argu_[0].theta + 0.0f) + + leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta + 0.0f)); + } + if (onground1_flag_) { + leg_argu_[1].Tw = + (leg_argu_[1].LQR_K[0] * + (-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) + + leg_argu_[1].LQR_K[1] * (-leg_argu_[1].d_theta + 0.0f) + + leg_argu_[1].LQR_K[2] * + (-move_argu_.xhat + move_argu_.target_x - 0.56) + + leg_argu_[1].LQR_K[3] * + (-move_argu_.x_dot_hat + move_argu_.target_dot_x) + + leg_argu_[1].LQR_K[4] * (-this->pit_ + 0.16) + + leg_argu_[1].LQR_K[5] * (-this->gyro_.x + 0.0f)); + leg_argu_[1].Tp = + (leg_argu_[1].LQR_K[6] * + (-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) + + leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f) + + leg_argu_[1].LQR_K[8] * + (-move_argu_.xhat + move_argu_.target_x - 0.56) + + leg_argu_[1].LQR_K[9] * + (-move_argu_.x_dot_hat + move_argu_.target_dot_x) + + leg_argu_[1].LQR_K[10] * (-this->pit_ + 0.16) + + leg_argu_[1].LQR_K[11] * (-this->gyro_.x + 0.0f)); + } else { + leg_argu_[1].Tw = 0; + leg_argu_[1].Tp = + (leg_argu_[1].LQR_K[6] * (-leg_argu_[1].theta + 0.0f) + + leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f)); + } + + this->leg_argu_[0].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 + + this->param_.wheel_param.static_L0[0] + + +roll_pid_.Calculate(0, eulr_.rol, dt_); + this->leg_argu_[1].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 + + this->param_.wheel_param.static_L0[1] - + roll_pid_.Calculate(0, eulr_.rol, dt_); + + leg_argu_[0].F0 = + leg_argu_[0].Tp * sinf(leg_argu_[0].theta) + + this->param_.wheel_param.static_F0[0] + + leglength_pid_.at(0)->Calculate(this->leg_argu_[0].Delat_L0, + this->leg_argu_[0].L0, this->dt_); + leg_argu_[1].F0 = + leg_argu_[1].Tp * sinf(leg_argu_[1].theta) + + this->param_.wheel_param.static_F0[1] + + leglength_pid_.at(1)->Calculate(this->leg_argu_[1].Delat_L0, + this->leg_argu_[1].L0, this->dt_); + + this->leg_argu_[0].Delta_Tp = + leg_argu_[0].L0 * 10.0f * + tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta, + this->leg_argu_[0].theta, this->dt_); + this->leg_argu_[1].Delta_Tp = + -leg_argu_[1].L0 * 10.0f * + tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta, + this->leg_argu_[0].theta, this->dt_); + + result3 = leg_[0]->VMCinserve( + -leg_argu_[0].phi1_, -leg_argu_[0].phi4_, + -leg_argu_[0].Tp - this->leg_argu_[0].Delta_Tp, leg_argu_[0].F0); + this->leg_argu_[0].T1 = std::get<0>(result3); + this->leg_argu_[0].T2 = std::get<1>(result3); + + result4 = leg_[1]->VMCinserve( + -leg_argu_[1].phi1_, -leg_argu_[1].phi4_, + -leg_argu_[1].Tp - this->leg_argu_[1].Delta_Tp, leg_argu_[1].F0); + this->leg_argu_[1].T1 = -std::get<0>(result4); + this->leg_argu_[1].T2 = -std::get<1>(result4); + + if (onground0_flag_ & onground1_flag_) { + move_argu_.yaw_force = + -this->yaw_pid_.Calculate(move_argu_.target_yaw, this->yaw_, dt_); + } else { + move_argu_.yaw_force = 0; + } + /*3508不带减速箱是Tw*3.2f*/ + /*2006带减速是Tw/1.8f*/ + /* 3508带15.7减速箱是Tw/4.9256 */ + + this->wheel_motor_out_[0] = + this->leg_argu_[0].Tw / 4.9256f - move_argu_.yaw_force; + + this->wheel_motor_out_[1] = + this->leg_argu_[1].Tw / 4.9256f + move_argu_.yaw_force; + + this->hip_motor_out_[0] = this->leg_argu_[0].T1; + this->hip_motor_out_[1] = this->leg_argu_[0].T2; + this->hip_motor_out_[2] = this->leg_argu_[1].T1; + this->hip_motor_out_[3] = this->leg_argu_[1].T2; + + this->hip_motor_flag_ = 1; + break; + + case Wheelleg::RESET: + if (fabs(pit_) > M_PI / 2 || fabs(leg_argu_[0].theta) > 1.57 || + fabs(leg_argu_[1].theta) > 1.57) { + leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000; + + if (fabs(pit_) > M_PI / 2) { + if ((leg_argu_[0].theta - leg_argu_[1].theta) > 0.03) { + leg_argu_[1].target_theta = leg_argu_[1].theta + 0.001; + } + if ((leg_argu_[0].theta - leg_argu_[1].theta) < -0.03) { + leg_argu_[1].target_theta = leg_argu_[1].theta - 0.001; + } + leg_argu_[0].F0 = 10 * leglength_pid_.at(0)->Calculate( + 0.65f, this->leg_argu_[0].L0, this->dt_); + leg_argu_[1].F0 = 10 * leglength_pid_.at(1)->Calculate( + 0.65f, this->leg_argu_[1].L0, this->dt_); + } + if (fabs(leg_argu_[0].theta) < 1.57) { + leg_argu_[1].target_theta = leg_argu_[1].theta + cmd_.y / 1000; + leg_argu_[0].target_theta = leg_argu_[0].theta; + } + + if (fabs(leg_argu_[1].theta) < 1.57) { + leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000; + leg_argu_[1].target_theta = leg_argu_[1].theta; + } + + if (leg_argu_[0].target_theta > M_PI) { + leg_argu_[0].target_theta -= 2 * M_PI; + } + if (leg_argu_[0].target_theta < -M_PI) { + leg_argu_[0].target_theta += 2 * M_PI; + } + if (leg_argu_[1].target_theta > M_PI) { + leg_argu_[1].target_theta -= 2 * M_PI; + } + if (leg_argu_[1].target_theta < -M_PI) { + leg_argu_[1].target_theta += 2 * M_PI; + } + leg_argu_[0].Tp = + 500 * this->theta_pid_[0]->Calculate(leg_argu_[0].target_theta, + leg_argu_[0].theta, dt_); + leg_argu_[1].Tp = + 500 * this->theta_pid_[1]->Calculate(leg_argu_[1].target_theta, + leg_argu_[1].theta, dt_); + + } else { + leg_argu_[0].F0 = 3 * leglength_pid_.at(0)->Calculate( + 0.10f, this->leg_argu_[0].L0, this->dt_); + leg_argu_[1].F0 = 3 * leglength_pid_.at(1)->Calculate( + 0.10f, this->leg_argu_[1].L0, this->dt_); + + if ((this->leg_argu_[0].L0 < 0.20) && (this->leg_argu_[1].L0 < 0.20)) { + leg_argu_[0].Tp = 5.5 * this->theta_pid_[0]->Calculate( + 0.1, leg_argu_[0].theta, dt_); + leg_argu_[1].Tp = 5.5 * this->theta_pid_[1]->Calculate( + 0.1, leg_argu_[1].theta, dt_); + clampf(&leg_argu_[0].Tp, -10, 10); + clampf(&leg_argu_[1].Tp, -10, 10); + } else { + leg_argu_[0].Tp = 0; + leg_argu_[1].Tp = 0; + } + } + + result3 = leg_[0]->VMCinserve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_, + -leg_argu_[0].Tp, leg_argu_[0].F0); + this->leg_argu_[0].T1 = std::get<0>(result3); + this->leg_argu_[0].T2 = std::get<1>(result3); + + result4 = leg_[1]->VMCinserve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_, + -leg_argu_[1].Tp, leg_argu_[1].F0); + this->leg_argu_[1].T1 = -std::get<0>(result4); + this->leg_argu_[1].T2 = -std::get<1>(result4); + + this->hip_motor_out_[0] = this->leg_argu_[0].T1; + this->hip_motor_out_[1] = this->leg_argu_[0].T2; + this->hip_motor_out_[2] = this->leg_argu_[1].T1; + this->hip_motor_out_[3] = this->leg_argu_[1].T2; + + this->hip_motor_flag_ = 1; + break; + } +} + +void Wheelleg::Control() { + clampf(&wheel_motor_out_[0], -0.8f, 0.8f); + clampf(&wheel_motor_out_[1], -0.8f, 0.8f); + clampf(&hip_motor_out_[0], -20.0f, 20.0f); + clampf(&hip_motor_out_[1], -20.0f, 20.0f); + clampf(&hip_motor_out_[2], -20.0f, 20.0f); + clampf(&hip_motor_out_[3], -20.0f, 20.0f); + + // if (fabs(wheel_motor_[0]->GetSpeed()) > 5000 || + // fabs(wheel_motor_[1]->GetSpeed()) > 5000) { + // wheel_motor_out_[0] = 0; + // wheel_motor_out_[1] = 0; + + // if (fabs(this->pit_) > 0.5f) { + // this->hip_motor_flag_ = 0; + // } + // } + + switch (this->mode_) { + case Wheelleg::RELAX: + + this->wheel_motor_[0]->Relax(); + this->wheel_motor_[1]->Relax(); + hip_motor_flag_ = 0; + hip_motor_[0]->SetMit(0.0f); + hip_motor_[1]->SetMit(0.0f); + hip_motor_[2]->SetMit(0.0f); + hip_motor_[3]->SetMit(0.0f); + break; + + case Wheelleg::STAND: + case Wheelleg::ROTOR: + // this->wheel_motor_[0]->Relax(); + // this->wheel_motor_[1]->Relax(); + this->wheel_motor_[0]->Control(-wheel_motor_out_[0]); + this->wheel_motor_[1]->Control(wheel_motor_out_[1]); + hip_motor_[0]->SetMit(this->hip_motor_out_[0]); + hip_motor_[1]->SetMit(this->hip_motor_out_[1]); + hip_motor_[2]->SetMit(this->hip_motor_out_[2]); + hip_motor_[3]->SetMit(this->hip_motor_out_[3]); + // hip_motor_[0]->SetMit(0.0f); + // hip_motor_[1]->SetMit(0.0f); + // hip_motor_[2]->SetMit(0.0f); + // hip_motor_[3]->SetMit(0.0f); + break; + + case Wheelleg::RESET: + + this->wheel_motor_[0]->Relax(); + this->wheel_motor_[1]->Relax(); + + hip_motor_[0]->SetMit(this->hip_motor_out_[0]); + hip_motor_[1]->SetMit(this->hip_motor_out_[1]); + + hip_motor_[2]->SetMit(this->hip_motor_out_[2]); + hip_motor_[3]->SetMit(this->hip_motor_out_[3]); + + break; + } +} + +void Wheelleg::SetMode(Wheelleg::Mode mode) { + if (mode == this->mode_) { + return; + } + + this->leg_[0]->Reset(); + this->leg_[1]->Reset(); + move_argu_.x = 0; + move_argu_.x_dot = 0; + move_argu_.last_x_dot = 0; + move_argu_.target_x = move_argu_.xhat; + move_argu_.target_yaw = this->eulr_.yaw; + move_argu_.target_dot_x = 0; + move_argu_.xhat = 0; + move_argu_.x_dot_hat = 0; + this->manfilter_.reset_comp(); + this->mode_ = mode; +}