添加差速舵轮旋转中心前馈与遥控器模式防抖,并补充ParkingRobot解决方案
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -1190,7 +1190,8 @@ namespace CommonUsage.Chassis
|
||||
float vx,
|
||||
float vy,
|
||||
float vth,
|
||||
TimeSpan? deltaTime = null)
|
||||
TimeSpan? deltaTime = null,
|
||||
bool enableDifferentialSteerFeedforward = false)
|
||||
{
|
||||
const string operationName = "SendXYThSpeed";
|
||||
|
||||
@@ -1313,13 +1314,42 @@ namespace CommonUsage.Chassis
|
||||
var driveScale = _xyThWheelsAligned
|
||||
? alignmentSpeedScale
|
||||
: 0f;
|
||||
#region 新增虚拟旋转中心
|
||||
// vth传入单位为deg/s,这里换算成rad/s。
|
||||
var omegaRadiansPerSecond =
|
||||
vth * (float)Math.PI / 180f;
|
||||
|
||||
// 只有调用者主动开启并且存在旋转运动时,
|
||||
// 才启用差速舵轮左右轮的几何速度前馈。
|
||||
var useDifferentialSteerFeedforward =
|
||||
enableDifferentialSteerFeedforward &&
|
||||
Math.Abs(omegaRadiansPerSecond) > 1e-4f;
|
||||
|
||||
var rotationCenter = Vector2.Zero;
|
||||
|
||||
if (useDifferentialSteerFeedforward)
|
||||
{
|
||||
// 根据车体速度场:
|
||||
// vx(point) = vx - omega * y
|
||||
// vy(point) = vy + omega * x
|
||||
// 计算车体坐标系中的瞬时旋转中心。
|
||||
//
|
||||
// vx、vy单位为m/s,计算结果原本是m;
|
||||
// 舵轮Position使用mm,因此乘以1000。
|
||||
rotationCenter = new Vector2(
|
||||
-vy / omegaRadiansPerSecond * 1000f,
|
||||
vx / omegaRadiansPerSecond * 1000f);
|
||||
}
|
||||
#endregion
|
||||
|
||||
|
||||
for (var i = 0; i < _steerWheels.Count; i++)
|
||||
AccumulateSpeed(
|
||||
i,
|
||||
driveScale *
|
||||
sendSpeed[i],
|
||||
false,
|
||||
new Vector2(0f, 0f),
|
||||
useDifferentialSteerFeedforward,
|
||||
rotationCenter,
|
||||
deltaTime);
|
||||
|
||||
if (writeDiagnostics)
|
||||
|
||||
Reference in New Issue
Block a user