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

离地时进行速度闭环，基本实现稳定飞坡

Browse Source
This commit is contained in:
kai
2024-05-03 23:22:05 +08:00
parent 69e228ace0
commit 7fee0c15c3
4 changed files with 50 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
application/chassis/fly_detection.h
View File

@@ -4,7 +4,7 @@
#include "user_lib.h"
// 驱动轮支持力解算
void NormalForceSolve(LinkNPodParam *p, INS_t *imu, float dt)
void NormalForceSolve(LinkNPodParam *p, INS_t *imu)
{
static float accx, accy, accz;
accx = imu->MotionAccel_b[X];
@@ -15,11 +15,17 @@ void NormalForceSolve(LinkNPodParam *p, INS_t *imu, float dt)
pitch = imu->Pitch;
roll = imu->Roll;
// 驱动轮竖直方向加速度
// 机体竖直方向加速度
p->zw_ddot = -msin(roll) * accx + mcos(roll) * msin(pitch) * accy + mcos(pitch) * mcos(roll) * accz;
// 驱动轮支持力解算
static float P;
P = p->F_leg * mcos(p->theta) + p->T_hip * msin(p->theta) / p->leg_len;
p->normal_force = P + WHEEL_MASS * (p->zw_ddot + 9.81f);
// 离地检测
if(p->normal_force < 20.0f)
p->fly_flag = 1;
else
p->fly_flag = 0;
}