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-04-30 21:23:03 +08:00
parent 410c23e527
commit 85badbc297
4 changed files with 38 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
application/chassis/fly_detection.h Normal file
View File

@@ -0,0 +1,25 @@
#include "balance.h"
#include "arm_math.h"
#include "math.h"
#include "user_lib.h"
// 驱动轮支持力解算
void NormalForceSolve(LinkNPodParam *p, INS_t *imu, float dt)
{
static float accx, accy, accz;
accx = imu->MotionAccel_b[X];
accy = imu->MotionAccel_b[Y];
accz = imu->MotionAccel_b[Z];
static float pitch, roll;
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);
}