TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 11:37:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

添加速度位置闭环分离和电机离线保护

Browse Source
This commit is contained in:
kai
2024-05-19 21:20:48 +08:00
parent 51c92b423a
commit 66e23202a7
4 changed files with 49 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
application/chassis/speed_estimation.h
View File

@@ -59,5 +59,13 @@ void SpeedEstimation(LinkNPodParam *lp, LinkNPodParam *rp, ChassisParam *cp, INS
cp->vel_cov = (1 - k) * vel_cov; // 后验协方差
VAL_LIMIT(cp->vel_cov, 0.01, 100); // 协方差限幅
cp->dist = cp->dist + cp->vel * delta_t;
// 速度和位置分离，有速度输入时不进行位置闭环
if(abs(cp->target_v) < 0.001)
{
cp->target_dist = 0;
cp->dist += cp->vel * delta_t;
}
else
cp->target_dist = cp->dist = 0;
}