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

修改与轮电机固连杆，使用机体速度计算LQR增益

Browse Source
This commit is contained in:
kai
2024-04-28 16:14:10 +08:00
parent 41e13aee44
commit b7e4c80cb4
8 changed files with 58 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
application/chassis/speed_estimation.h
View File

@@ -17,8 +17,8 @@
void SpeedEstimation(LinkNPodParam *lp, LinkNPodParam *rp, ChassisParam *cp, INS_t *imu, float delta_t)
{
// 修正轮速和距离
lp->wheel_w = lp->w_ecd + lp->phi2_w - cp->pitch_w; // 减去和定子固连的phi2_w
rp->wheel_w = rp->w_ecd + rp->phi2_w - cp->pitch_w;
lp->wheel_w = lp->w_ecd + lp->phi3_w - cp->pitch_w; // 减去和定子固连的phi2_w
rp->wheel_w = rp->w_ecd + rp->phi3_w - cp->pitch_w;
// 以轮子为基点,计算机体两侧髋关节处的速度
lp->body_v = lp->wheel_w * WHEEL_RADIUS + lp->leg_len * lp->theta_w + lp->legd * msin(lp->theta);