xiaocheng/Er_sentry
1
1
Fork 1
Code Issues 1 Pull Requests Actions Packages Projects Releases Wiki Activity

没招了

Browse Source
This commit is contained in:
Xiaocheng 2026-03-18 14:36:40 +08:00
parent 9a2ff3e150
commit 2ad0d97189
2 changed files with 44 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
User/module/chassis.c
View File

@ -66,6 +66,12 @@ fp32 vofa_send[8]; //vofa输出数据
#define CHASSIS_ROTOR_WZ_MAX 0.8f /* 小陀螺旋转位移上界 */
#define CHASSIS_FOLLOW_LOCK_DEADBAND_RAD 0.02f /* 跟随锁定死区，约1.1度 */
#define CHASSIS_FOLLOW_VW_DEADBAND 0.03f /* 跟随角速度输出死区 */
#define CHASSIS_FOLLOW_TRANS_DEADBAND 0.02f /* 跟随平移输入死区 */
#define CHASSIS_FOLLOW_FREE_BAND_RAD 0.08f /* 跟随自由区，约4.6度 */
#define CHASSIS_FOLLOW_LOCK_DEADBAND_RAD 0.02f /* 跟随锁定死区，约1.1度 */
#define CHASSIS_FOLLOW_VW_DEADBAND 0.03f /* 跟随角速度输出死区 */
#define CHASSIS_FOLLOW_DAMP_GAIN 0.08f /* 跟随阻尼系数 */
#define CHASSIS_FOLLOW_VW_MAX 1.0f /* 跟随角速度上限 */
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode ,uint32_t now)
{
@ -104,6 +110,41 @@ static float Chassis_CalcWz(const float min, const float max, uint32_t now) {
return wz_vary > 0.8f ? max : wz_vary;
}
// static float Chassis_CalcFollowVw(Chassis_t *c)
// {
// const float follow_target = c->4.517f;
// const float yaw_fb = c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle;
// const float yaw_speed_fb = c->motorfeedback.gimbal_yaw_encoder.rotor_speed;
// const float follow_err = CircleError(follow_target, yaw_fb, M_2PI);
// const float abs_follow_err = fabsf(follow_err);
// if (abs_follow_err < CHASSIS_FOLLOW_LOCK_DEADBAND_RAD) {
// PID_ResetIntegral(&c->pid.chassis_follow_gimbal_pid);
// return 0.0f;
// }
// /* 保留一段相对运动区，避免底盘与大YAW刚性同动 */
// if (abs_follow_err < CHASSIS_FOLLOW_FREE_BAND_RAD) {
// PID_ResetIntegral(&c->pid.chassis_follow_gimbal_pid);
// return 0.0f;
// }
// float follow_vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid,
// follow_target,
// yaw_fb,
// 0.0f,
// c->dt);
// /* 对大YAW速度做阻尼，降低同向拖动感 */
// follow_vw -= CHASSIS_FOLLOW_DAMP_GAIN * yaw_speed_fb;
// follow_vw = AbsClip(follow_vw, CHASSIS_FOLLOW_VW_MAX);
// if (fabsf(follow_vw) < CHASSIS_FOLLOW_VW_DEADBAND) {
// follow_vw = 0.0f;
// }
// return follow_vw;
// }
/*底盘初始化*/
int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq)
@ -407,6 +448,8 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
// 跟随云台模式
c->move_vec.Vx =-c_cmd->Vy;
c->move_vec.Vy =-c_cmd->Vx;
if (fabsf(c->move_vec.Vx) < CHASSIS_FOLLOW_TRANS_DEADBAND) c->move_vec.Vx = 0.0f;
if (fabsf(c->move_vec.Vy) < CHASSIS_FOLLOW_TRANS_DEADBAND) c->move_vec.Vy = 0.0f;
{
const float follow_target = c->mech_zero_4310;
const float follow_err = CircleError(follow_target,
@ -427,8 +470,6 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
c->move_vec.Vw = follow_vw;
}
}
// c->move_vec.Vw = -PID_Calc(&c->pid.chassis_follow_gimbal_pid, c->motorfeedback.gimbal_yaw_encoder-2.07694483f ,c->motorfeedback.gimbal_yaw_encoder, 0.0f, c->dt);
break;

2
User/module/config.c
View File

@ -101,7 +101,7 @@ Config_Param_t config = {
/*这两个数据是云台6020的零点和机械限位*/
.mech_zero = 1.31f,
.travel = 1.4f,
.mech_zero_4310=2.06f,
.mech_zero_4310=4.51735544f,
},