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

Compare commits

base: robofish:dd6c48c75cc7e280074e7a540dea0841420a3b46
Branches Tags
robofish:main
robofish:MC_02
robofish:ai
...
compare: robofish:5139f4ad6ad18bd3cb136f05392dda86dbe3e6dc
Branches Tags
robofish:MC_02
robofish:main
robofish:ai

4 Commits
dd6c48c75c ... 5139f4ad6a

Author SHA1 Message Date
Robofish
5139f4ad6a Merge branch 'MC_02' of ssh://gitea.qutrobot.top:222/robofish/rm_balance into MC_02 
# Conflicts:
#	User/module/cmd/cmd.c
#	User/module/config.c
 2026-03-21 02:30:31 +08:00
xxxxm
d3096aa537 合并 2026-03-21 02:28:45 +08:00
Robofish
9fa8a6cb81 为了倒地自起 2026-03-21 02:07:34 +08:00
Robofish
e43fc4de1d zimiaomeil 2026-03-20 07:45:19 +08:00
3 changed files with 9 additions and 29 deletions
Show Stats 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);

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

@ -6,6 +6,7 @@
#include "module/cmd/cmd_types.h"
#include "module/gimbal.h"
#include "module/shoot.h"
#include "module/gimbal.h"
#include <stdint.h>
#include <string.h>

2
User/module/config.c
View File

@ -545,7 +545,7 @@ Config_RobotParam_t robot_config = {
// .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
},
},