diff --git a/.DS_Store b/.DS_Store index 49bb25a..4dbb5d0 100644 Binary files a/.DS_Store and b/.DS_Store differ diff --git a/User/component/vmc.c b/User/component/vmc.c index 2943b89..42ffc7e 100644 --- a/User/component/vmc.c +++ b/User/component/vmc.c @@ -122,7 +122,11 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl VMC_ANGLE_NORMALIZE(leg->phi0); // 计算角速度和长度变化率 - leg->d_phi0 = VMC_ComputeNumericDerivative(leg->phi0, leg->last_phi0, vmc->dt); + float dphi0 = leg->phi0 - leg->last_phi0; + /* phi0 在 ±π 跳变时做循环差值处理 */ + if (dphi0 > VMC_PI) dphi0 -= VMC_2PI; + if (dphi0 < -VMC_PI) dphi0 += VMC_2PI; + leg->d_phi0 = (vmc->dt > 0) ? (dphi0 / vmc->dt) : 0.0f; leg->d_alpha = -leg->d_phi0; leg->d_theta = body_pitch_rate + leg->d_phi0; leg->d_L0 = VMC_ComputeNumericDerivative(leg->L0, leg->last_L0, vmc->dt); diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 655aa6a..0e5824a 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -34,6 +34,7 @@ static void Chassis_SelectYawZeroPoint(Chassis_t *c); static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force); static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd); static void Chassis_RecoverControl(Chassis_t *c); +static void Chassis_LegTestControl(Chassis_t *c, const Chassis_CMD_t *c_cmd); /* Private functions -------------------------------------------------------- */ @@ -85,6 +86,8 @@ static void Chassis_ResetControllers(Chassis_t *c) { PID_Reset(&c->pid.roll_force); PID_Reset(&c->pid.tp[0]); PID_Reset(&c->pid.tp[1]); + PID_Reset(&c->pid.leg_test_theta[0]); + PID_Reset(&c->pid.leg_test_theta[1]); /* 重置LQR控制器 */ LQR_Reset(&c->lqr[0]); @@ -209,6 +212,11 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { c->climb.state_start_time = 0; } + /* 进入腿部测试模式：重置初始化标志，进入后锁定当前摆角 */ + if (mode == CHASSIS_MODE_LEG_TEST) { + c->leg_test.initialized = false; + } + c->mode = mode; return CHASSIS_OK; } @@ -665,6 +673,88 @@ static void Chassis_RecoverControl(Chassis_t *c) { } +/** + * @brief 腿部测试控制 + * @param c 底盘结构体指针 + * @param c_cmd 控制指令 + * @note 进入时锁定当前摆角，vx/vy 增减左/右腿目标摆角 + * PID控制摆角，恒力支撑腿长，轮子关闭 + */ +static void Chassis_LegTestControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { + /* 首次进入：锁定当前摆角 */ + if (!c->leg_test.initialized) { + c->leg_test.target_theta[0] = c->vmc_[0].leg.theta; + c->leg_test.target_theta[1] = c->vmc_[1].leg.theta; + c->leg_test.initialized = true; + } + + float sens = c->param->leg_test.theta_sensitivity; + float deadzone = 0.05f; /* 摇杆死区 */ + + /* vx 控制左腿：有输入→累加目标角度（正/反转），松手→锁住当前实际角度 */ + if (fabsf(c_cmd->move_vec.vx) > deadzone) { + c->leg_test.target_theta[0] += c_cmd->move_vec.vx * sens * c->dt; + } else { + c->leg_test.target_theta[0] = c->vmc_[0].leg.theta; + } + + /* vy 控制右腿：同理 */ + if (fabsf(c_cmd->move_vec.vy) > deadzone) { + c->leg_test.target_theta[1] += c_cmd->move_vec.vy * sens * c->dt; + } else { + c->leg_test.target_theta[1] = c->vmc_[1].leg.theta; + } + + /* 目标角度归一化到 [-π, π]，防止无限增长 */ + while (c->leg_test.target_theta[0] > (float)M_PI) c->leg_test.target_theta[0] -= (float)M_2PI; + while (c->leg_test.target_theta[0] < -(float)M_PI) c->leg_test.target_theta[0] += (float)M_2PI; + while (c->leg_test.target_theta[1] > (float)M_PI) c->leg_test.target_theta[1] -= (float)M_2PI; + while (c->leg_test.target_theta[1] < -(float)M_PI) c->leg_test.target_theta[1] += (float)M_2PI; + + /* 轮子关闭：只清零输出，由 Chassis_Output 统一发送，避免重复 CAN 帧 */ + + /* 恒定支撑力 + PID摆角控制 → VMC逆解算 → 关节力矩 */ + float support_force = c->param->leg_test.support_force; + + for (int i = 0; i < 2; i++) { + /* 手动计算循环误差，避免 theta 在 ±π 跳变时 PID 炸 */ + float error = c->leg_test.target_theta[i] - c->vmc_[i].leg.theta; + while (error > (float)M_PI) error -= (float)M_2PI; + while (error < -(float)M_PI) error += (float)M_2PI; + + /* 用误差直接算 P 和 D 输出，绕过 PID 内部的 set/fdb 差值 */ + float kp = c->pid.leg_test_theta[i].param->k * c->pid.leg_test_theta[i].param->p; + float kd = c->pid.leg_test_theta[i].param->k * c->pid.leg_test_theta[i].param->d; + float out_limit = c->pid.leg_test_theta[i].param->out_limit; + float Tp = kp * error - kd * c->vmc_[i].leg.d_theta; + Tp = LIMIT(Tp, -out_limit, out_limit); + + if (VMC_InverseSolve(&c->vmc_[i], support_force, Tp) == 0) { + if (i == 0) { + VMC_GetJointTorques(&c->vmc_[i], &c->output.joint[0], &c->output.joint[1]); + } else { + VMC_GetJointTorques(&c->vmc_[i], &c->output.joint[3], &c->output.joint[2]); + } + } else { + c->output.joint[i * 2] = 0.0f; + c->output.joint[i * 2 + 1] = 0.0f; + } + } + + /* 轮子输出清零 */ + c->output.wheel[0] = 0.0f; + c->output.wheel[1] = 0.0f; + + /* 安全限制 */ + for (int i = 0; i < 4; i++) { + c->output.joint[i] = LIMIT(c->output.joint[i], -30.0f, 30.0f); + } + + /* 发送关节指令 */ + Chassis_Output(c); +} + + /** * @brief 初始化底盘 * @param c 底盘结构体指针 @@ -707,12 +797,14 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float 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.yaw, KPID_MODE_SET_D, target_freq, ¶m->yaw); PID_Init(&c->pid.rotor, KPID_MODE_CALC_D, target_freq, ¶m->rotor); PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, ¶m->roll); PID_Init(&c->pid.roll_force, KPID_MODE_CALC_D, target_freq, ¶m->roll_force); 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); + PID_Init(&c->pid.leg_test_theta[0], KPID_MODE_CALC_D, target_freq, ¶m->leg_test_theta); + PID_Init(&c->pid.leg_test_theta[1], KPID_MODE_CALC_D, target_freq, ¶m->leg_test_theta); /* 初始化LQR控制器 */ LQR_Init(&c->lqr[0], ¶m->lqr_gains); @@ -814,6 +906,11 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) { c->recover.state[1] = RECOVER_STRETCH; c->recover.state_start_time = 0; + /* 初始化腿部测试状态 */ + c->leg_test.initialized = false; + c->leg_test.target_theta[0] = 0.0f; + c->leg_test.target_theta[1] = 0.0f; + return CHASSIS_OK; } @@ -980,6 +1077,10 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { Chassis_ClimbControl(c, c_cmd); break; + case CHASSIS_MODE_LEG_TEST: + Chassis_LegTestControl(c, c_cmd); + break; + default: return CHASSIS_ERR_MODE; } @@ -1018,7 +1119,7 @@ void Chassis_Output(Chassis_t *c) { // c->output.wheel[i] = LowPassFilter2p_Apply(&c->filter.wheel_out[i], c->output.wheel[i]); // } //cap-add//supercap-add - PowerLimit_Output(c->power_limit, c->output.wheel, 2 ); + // PowerLimit_Output(c->power_limit, c->output.wheel, 2 ); //PowerLimit_ChassicOutput() MOTOR_LK_SetOutput(&c->param->wheel_param[0], c->output.wheel[0]); MOTOR_LK_SetOutput(&c->param->wheel_param[1], c->output.wheel[1]); @@ -1123,7 +1224,9 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { /* 对齐完成，切换到平衡模式 */ c->rotor_state.exiting = false; c->mode = CHASSIS_MODE_WHELL_LEG_BALANCE; - Chassis_ResetControllers(c); + /* 只重置 yaw/rotor PID，保留 chassis_state 避免 LQR 突变 */ + PID_Reset(&c->pid.yaw); + PID_Reset(&c->pid.rotor); } } } else { @@ -1141,7 +1244,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { /* 退出过渡减速完成后，用yaw PID对齐零点 */ if (c->rotor_state.exiting && c->rotor_state.current_spin_speed == 0.0f) { 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->feedback.yaw.rotor_abs_angle, + c->feedback.imu.gyro.z, c->dt); } else { float current_yaw_omega = -c->feedback.imu.gyro.z; /* 取反匹配yaw_force→轮子旋转方向 */ c->yaw_control.yaw_force = PID_Calc(&c->pid.rotor, c->rotor_state.current_spin_speed, @@ -1158,16 +1262,30 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { float error_to_target1 = CircleError(base_target_1, c->yaw_control.current_yaw, M_2PI); float error_to_target2 = CircleError(base_target_2, c->yaw_control.current_yaw, M_2PI); - if (fabsf(error_to_target1) <= fabsf(error_to_target2)) { - c->yaw_control.target_yaw = base_target_1; - c->yaw_control.is_reversed = false; + /* 迟滞切换：只有当另一个零点明显更近（差值>0.3rad≈17°）时才切换， + * 避免在两个零点正中间反复跳变 */ + float hysteresis = 0.3f; + if (c->yaw_control.is_reversed) { + /* 当前跟踪 target_2，只有 target_1 明显更近才切 */ + if (fabsf(error_to_target1) < fabsf(error_to_target2) - hysteresis) { + c->yaw_control.target_yaw = base_target_1; + c->yaw_control.is_reversed = false; + } else { + c->yaw_control.target_yaw = base_target_2; + } } else { - c->yaw_control.target_yaw = base_target_2; - c->yaw_control.is_reversed = true; + /* 当前跟踪 target_1，只有 target_2 明显更近才切 */ + if (fabsf(error_to_target2) < fabsf(error_to_target1) - hysteresis) { + c->yaw_control.target_yaw = base_target_2; + c->yaw_control.is_reversed = true; + } else { + c->yaw_control.target_yaw = base_target_1; + } } 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->feedback.yaw.rotor_abs_angle, + c->feedback.imu.gyro.z, c->dt); } /* ==================== 左腿LQR控制 ==================== */ diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index a0ba192..d32428d 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -43,7 +43,8 @@ typedef enum { // CHASSIS_MODE_CALIBRATE, /* 校准模式 */ CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡，支持跳跃 */ CHASSIS_MODE_BALANCE_ROTOR, /*小陀螺*/ - CHASSIS_MODE_CLIMB_STEP /* 上台阶模式 */ + CHASSIS_MODE_CLIMB_STEP, /* 上台阶模式 */ + CHASSIS_MODE_LEG_TEST /* 腿部测试模式：PID锁摆角+恒力支撑，摇杆控制摆角 */ } Chassis_Mode_t; /* 跳跃状态枚举 */ @@ -174,6 +175,14 @@ typedef struct { uint32_t settle_time_ms; /* 收腿后稳定时间 (ms) */ } climb; + /* 腿部测试参数 */ + struct { + float support_force; /* 恒定支撑力 (N) */ + float theta_sensitivity; /* 摇杆→摆角灵敏度 (rad/s) */ + } leg_test; + + KPID_Params_t leg_test_theta; /* 腿部测试摆角PID（独立于tp） */ + /* 倒地自起参数 */ struct { float stretch_velocity; /* 伸腿角速度 (rad/s) */ @@ -267,6 +276,12 @@ typedef struct { uint32_t state_start_time; /* 当前状态开始时间 (ms) */ } climb; + /* 腿部测试相关 */ + struct { + float target_theta[2]; /* 左右腿目标摆角 (rad)，进入时锁定当前值 */ + bool initialized; /* 是否已锁定初始摆角 */ + } leg_test; + /* 电机离线检测 */ struct { bool any_offline; /* 是否有电机离线 */ @@ -286,6 +301,7 @@ typedef struct { KPID_t tp[2]; KPID_t leg_length[2]; /* 两条腿的腿长PID */ KPID_t leg_theta[2]; /* 两条腿的摆角PID */ + KPID_t leg_test_theta[2]; /* 腿部测试摆角PID */ } pid; /* 滤波器 */ diff --git a/User/module/config.c b/User/module/config.c index 2f6c0be..1308e75 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -491,6 +491,20 @@ Config_RobotParam_t robot_config = { .phi_tolerance = 0.5f, /* 体姿接近站立容差 (rad)，约28° */ .kd = 3.0f, /* MIT速度模式阻尼 */ }, + .leg_test = { + .support_force = 30.0f, /* 恒定支撑力 (N) */ + .theta_sensitivity = 1.0f, /* 摇杆→摆角灵敏度 (rad/s) */ + }, + .leg_test_theta = { + .k = 20.0f, + .p = 5.0f, + .i = 0.0f, + .d = 1.0f, + .i_limit = 0.0f, + .out_limit = 30.0f, /* 摆角力矩限幅 (Nm) */ + .d_cutoff_freq = 30.0f, + .range = M_2PI, /* 启用循环误差处理 */ + }, .lqr_offset = { .theta = 0.0f, .x = 0.0f, @@ -522,15 +536,14 @@ Config_RobotParam_t robot_config = { .rc_mode_map = { #if CMD_ENABLE_MODULE_GIMBAL .gimbal_sw_up = GIMBAL_MODE_RELAX, - // .gimbal_sw_mid = GIMBAL_MODE_ABSOLUTE, - .gimbal_sw_mid = GIMBAL_MODE_RECOVER, + .gimbal_sw_mid = GIMBAL_MODE_ABSOLUTE, + // .gimbal_sw_mid = GIMBAL_MODE_RECOVER, .gimbal_sw_down = GIMBAL_MODE_RELATIVE, #endif #if CMD_ENABLE_MODULE_BALANCE_CHASSIS .balance_sw_up = CHASSIS_MODE_RELAX, - // .balance_sw_mid = CHASSIS_MODE_WHELL_LEG_BALANCE, - .balance_sw_mid = CHASSIS_MODE_RELAX, // 先关闭轮腿平衡，避免不熟悉操作导致摔倒 - .balance_sw_down = CHASSIS_MODE_WHELL_LEG_BALANCE, + .balance_sw_mid = CHASSIS_MODE_WHELL_LEG_BALANCE, + .balance_sw_down = CHASSIS_MODE_BALANCE_ROTOR, #endif }, }, diff --git a/User/module/gimbal.c b/User/module/gimbal.c index f6f945c..970b773 100644 --- a/User/module/gimbal.c +++ b/User/module/gimbal.c @@ -63,6 +63,17 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) { } g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw; + /* 进入 RECOVER 时初始化双零点选择 */ + if (mode == GIMBAL_MODE_RECOVER) { + float yaw_enc = g->feedback.motor.yaw.rotor_abs_angle; + float zero0 = g->param->mech_zero.yaw; + float zero1 = zero0 + M_PI; + if (zero1 >= M_2PI) zero1 -= M_2PI; + float err0 = fabsf(CircleError(zero0, yaw_enc, M_2PI)); + float err1 = fabsf(CircleError(zero1, yaw_enc, M_2PI)); + g->recover_yaw_reversed = (err1 < err0); + } + g->mode = mode; return 0; } @@ -220,14 +231,27 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) { g->setpoint.eulr.yaw = g_cmd->setpoint_yaw; g->setpoint.eulr.pit = g_cmd->setpoint_pit; } else if (g_cmd->mode == GIMBAL_MODE_RECOVER) { - /* 双零点选近：mech_zero 和 mech_zero+π，选离当前编码器角度最近的 */ + /* 双零点选近：mech_zero 和 mech_zero+π，带迟滞避免临界点跳变 */ float yaw_enc = g->feedback.motor.yaw.rotor_abs_angle; float zero0 = g->param->mech_zero.yaw; float zero1 = zero0 + M_PI; if (zero1 >= M_2PI) zero1 -= M_2PI; float err0 = fabsf(CircleError(zero0, yaw_enc, M_2PI)); float err1 = fabsf(CircleError(zero1, yaw_enc, M_2PI)); - g->setpoint.eulr.yaw = (err0 <= err1) ? zero0 : zero1; + + float hysteresis = 0.3f; + if (g->recover_yaw_reversed) { + /* 当前跟踪 zero1，只有 zero0 明显更近才切 */ + if (err0 < err1 - hysteresis) { + g->recover_yaw_reversed = false; + } + } else { + /* 当前跟踪 zero0，只有 zero1 明显更近才切 */ + if (err1 < err0 - hysteresis) { + g->recover_yaw_reversed = true; + } + } + g->setpoint.eulr.yaw = g->recover_yaw_reversed ? zero1 : zero0; g->setpoint.eulr.pit = 0.5f * (g->limit.pit.max + g->limit.pit.min); } diff --git a/User/module/gimbal.h b/User/module/gimbal.h index 79e3ca8..faaeaa4 100644 --- a/User/module/gimbal.h +++ b/User/module/gimbal.h @@ -171,6 +171,8 @@ typedef struct { Gimbal_Output_t out; /* 云台输出 */ Gimbal_Feedback_t feedback; /* 反馈 */ + bool recover_yaw_reversed; /* RECOVER模式下跟踪的是第二个零点(+π) */ + } Gimbal_t; /* Exported functions prototypes -------------------------------------------- */