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

zimiaomeil

Browse Source
This commit is contained in:
Robofish 2026-03-20 07:45:19 +08:00
parent ba61c10613
commit e43fc4de1d
3 changed files with 18 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
User/module/balance_chassis.c
View File

@ -118,20 +118,9 @@ static void Chassis_ResetControllers(Chassis_t *c) {
* @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.mech_zero_yaw;
float zero_point_2 = zero_point_1 + M_PI;
float error_to_zero1 = CircleError(zero_point_1, current_yaw, M_2PI);
float error_to_zero2 = CircleError(zero_point_2, current_yaw, M_2PI);
if (fabsf(error_to_zero1) <= fabsf(error_to_zero2)) {
c->yaw_control.target_yaw = zero_point_1;
/* 双零点已关闭，始终使用主零点 */
c->yaw_control.target_yaw = c->param->mech.mech_zero_yaw;
c->yaw_control.is_reversed = false;
} else {
c->yaw_control.target_yaw = zero_point_2;
c->yaw_control.is_reversed = true;
}
}
/**
@ -1252,20 +1241,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
} else {
c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
/* 双零点已关闭，始终使用主零点 */
float manual_offset = c_cmd->move_vec.vy * M_PI_2;
float base_target_1 = c->param->mech.mech_zero_yaw + manual_offset;
float base_target_2 = c->param->mech.mech_zero_yaw + M_PI + manual_offset;
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.target_yaw = c->param->mech.mech_zero_yaw + manual_offset;
c->yaw_control.is_reversed = false;
} else {
c->yaw_control.target_yaw = base_target_2;
c->yaw_control.is_reversed = true;
}
c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);

9
User/module/cmd/cmd.c
View File

@ -5,6 +5,7 @@
#include "bsp/time.h"
#include "module/cmd/cmd_types.h"
#include "module/shoot.h"
#include "module/gimbal.h"
#include <stdint.h>
#include <string.h>
@ -654,6 +655,7 @@ int8_t CMD_Arbitrate(CMD_t *ctx) {
if (ctx->input.online[CMD_SRC_NUC]) {
if (ctx->active_source==CMD_SRC_RC) {
if (ctx->input.rc.sw[0] == CMD_SW_DOWN) {
// 用遥控器调自瞄的时候打开下面两行
ctx->output.gimbal.source = CMD_SRC_NUC;
ctx->output.shoot.source = CMD_SRC_NUC;
#if CMD_ENABLE_MODULE_REFUI
@ -663,7 +665,12 @@ int8_t CMD_Arbitrate(CMD_t *ctx) {
}
}
#endif
// if (ctx->input.rc.sw[0] == CMD_SW_DOWN) {
// ctx->output.balance_chassis.source = CMD_SRC_RC;
// ctx->output.gimbal.source = CMD_SRC_RC;
// ctx->output.balance_chassis.cmd.mode = CHASSIS_MODE_LEG_TEST;
// ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RECOVER;
// }
return CMD_OK;
}

3
User/module/config.c
View File

@ -538,13 +538,14 @@ Config_RobotParam_t robot_config = {
.gimbal_sw_up = GIMBAL_MODE_RELAX,
.gimbal_sw_mid = GIMBAL_MODE_ABSOLUTE,
// .gimbal_sw_down = GIMBAL_MODE_RECOVER,
.gimbal_sw_down = GIMBAL_MODE_AI_CONTROL,
.gimbal_sw_down = GIMBAL_MODE_RECOVER,
#endif
#if CMD_ENABLE_MODULE_BALANCE_CHASSIS
.balance_sw_up = CHASSIS_MODE_RELAX,
// .balance_sw_mid = CHASSIS_MODE_RELAX,
// .balance_sw_down = CHASSIS_MODE_RELAX,
.balance_sw_mid =CHASSIS_MODE_WHELL_LEG_BALANCE,
// .balance_sw_down = CHASSIS_MODE_LEG_TEST,
.balance_sw_down = CHASSIS_MODE_WHELL_LEG_BALANCE,
#endif
},