First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

完善状态估计、车队协调与舵轮辨识日志

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-24 18:39:41 +08:00
parent 0ab409cd2a
commit 95c0b19a26
40 changed files with 5983 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MedullaAdapter/MotorRoutine.cs
+80 -15
View File
@@ -4,6 +4,7 @@ using FundamentalLib;
using MDCSToolBox.Commons;
using System;
using System.Diagnostics;
using System.Threading;
using static MDCSToolBox.Medulla.Chassis.BasicCartDefinition;
namespace MedullaAdapter
@@ -75,6 +76,11 @@ namespace MedullaAdapter
cart.ClumsyControl = CartDefinition.currentPriority == 0;
// 计算四个舵轮PID和8个驱动电机最终速度。
UpdateDiffSteerWheelSpeeds();
Interlocked.Exchange(
ref cart.DiffSteerControlTimestamp,
Stopwatch.GetTimestamp());
Interlocked.Increment(
ref cart.DiffSteerControlSequence);
// 平滑更新硬件速度限制。
UpdateSendSpeedLimit();
// 更新红黄绿灯状态。
@@ -351,6 +357,15 @@ namespace MedullaAdapter
leftRear = 0f;
rightFront = 0f;
rightRear = 0f;
cart.DiffSteerTargetRateLeftFrontDegreesPerSecond = 0f;
cart.DiffSteerTargetRateLeftRearDegreesPerSecond = 0f;
cart.DiffSteerTargetRateRightFrontDegreesPerSecond = 0f;
cart.DiffSteerTargetRateRightRearDegreesPerSecond = 0f;
cart.DiffSteerFeedforwardDeltaTimeMilliseconds = 0f;
cart.DiffSteerFeedforwardLimitedLeftFront = false;
cart.DiffSteerFeedforwardLimitedLeftRear = false;
cart.DiffSteerFeedforwardLimitedRightFront = false;
cart.DiffSteerFeedforwardLimitedRightRear = false;
var currentTimestamp = Stopwatch.GetTimestamp();
@@ -365,22 +380,45 @@ namespace MedullaAdapter
deltaTimeSeconds <=
MaximumFeedforwardIntervalSeconds)
{
cart.DiffSteerFeedforwardDeltaTimeMilliseconds =
(float)(deltaTimeSeconds * 1000.0);
leftFront = CalculateDiffSteerRateFeedforward(
cart.ThLeftFront,
_previousThLeftFront,
deltaTimeSeconds);
deltaTimeSeconds,
out var targetRateLf,
out var limitedLf);
leftRear = CalculateDiffSteerRateFeedforward(
cart.ThLeftRear,
_previousThLeftRear,
deltaTimeSeconds);
deltaTimeSeconds,
out var targetRateLr,
out var limitedLr);
rightFront = CalculateDiffSteerRateFeedforward(
cart.ThRightFront,
_previousThRightFront,
deltaTimeSeconds);
deltaTimeSeconds,
out var targetRateRf,
out var limitedRf);
rightRear = CalculateDiffSteerRateFeedforward(
cart.ThRightRear,
_previousThRightRear,
deltaTimeSeconds);
deltaTimeSeconds,
out var targetRateRr,
out var limitedRr);
cart.DiffSteerTargetRateLeftFrontDegreesPerSecond =
targetRateLf;
cart.DiffSteerTargetRateLeftRearDegreesPerSecond =
targetRateLr;
cart.DiffSteerTargetRateRightFrontDegreesPerSecond =
targetRateRf;
cart.DiffSteerTargetRateRightRearDegreesPerSecond =
targetRateRr;
cart.DiffSteerFeedforwardLimitedLeftFront = limitedLf;
cart.DiffSteerFeedforwardLimitedLeftRear = limitedLr;
cart.DiffSteerFeedforwardLimitedRightFront = limitedRf;
cart.DiffSteerFeedforwardLimitedRightRear = limitedRr;
}
}
@@ -398,29 +436,44 @@ namespace MedullaAdapter
private float CalculateDiffSteerRateFeedforward(
float targetAngleDegrees,
float previousTargetAngleDegrees,
double deltaTimeSeconds)
double deltaTimeSeconds,
out float targetRateDegreesPerSecond,
out bool limited)
{
targetRateDegreesPerSecond = 0f;
limited = false;
var gain = cart.DiffSteerRateFeedforwardGain;
var wheelDistanceMillimeters =
cart.DiffSteerWheelDistanceMillimeters;
var maximumSpeed =
cart.DiffSteerRateFeedforwardMaximumSpeed;
if (!IsFinite(gain) || gain <= 0f ||
!IsFinite(wheelDistanceMillimeters) ||
wheelDistanceMillimeters <= 0f ||
!IsFinite(maximumSpeed) || maximumSpeed <= 0f ||
!IsFinite(targetAngleDegrees) ||
!IsFinite(previousTargetAngleDegrees))
if (!IsFinite(targetAngleDegrees) ||
!IsFinite(previousTargetAngleDegrees) ||
!double.IsFinite(deltaTimeSeconds) ||
deltaTimeSeconds <= 0.0)
{
return 0f;
}
// 机械舵角受限，必须使用直接差值而不是圆周最短角差。
targetRateDegreesPerSecond =
(float)((targetAngleDegrees -
previousTargetAngleDegrees) /
deltaTimeSeconds);
if (!IsFinite(gain) || gain <= 0f ||
!IsFinite(wheelDistanceMillimeters) ||
wheelDistanceMillimeters <= 0f ||
!IsFinite(maximumSpeed) || maximumSpeed <= 0f)
{
return 0f;
}
var targetRateRadiansPerSecond =
(targetAngleDegrees - previousTargetAngleDegrees) *
Math.PI / 180.0 /
deltaTimeSeconds;
targetRateDegreesPerSecond *
Math.PI / 180.0;
var wheelDistanceMeters =
wheelDistanceMillimeters / 1000.0;
var feedforwardSpeed =
@@ -429,10 +482,13 @@ namespace MedullaAdapter
targetRateRadiansPerSecond *
gain;
return (float)Math.Clamp(
var limitedSpeed = (float)Math.Clamp(
feedforwardSpeed,
-maximumSpeed,
maximumSpeed);
limited = Math.Abs(
feedforwardSpeed - limitedSpeed) > 1e-9;
return limitedSpeed;
}
/// <summary>
@@ -446,6 +502,15 @@ namespace MedullaAdapter
cart.DiffSteerRateFeedforwardLeftRear = 0f;
cart.DiffSteerRateFeedforwardRightFront = 0f;
cart.DiffSteerRateFeedforwardRightRear = 0f;
cart.DiffSteerTargetRateLeftFrontDegreesPerSecond = 0f;
cart.DiffSteerTargetRateLeftRearDegreesPerSecond = 0f;
cart.DiffSteerTargetRateRightFrontDegreesPerSecond = 0f;
cart.DiffSteerTargetRateRightRearDegreesPerSecond = 0f;
cart.DiffSteerFeedforwardDeltaTimeMilliseconds = 0f;
cart.DiffSteerFeedforwardLimitedLeftFront = false;
cart.DiffSteerFeedforwardLimitedLeftRear = false;
cart.DiffSteerFeedforwardLimitedRightFront = false;
cart.DiffSteerFeedforwardLimitedRightRear = false;
cart.DiffSteerTotalOutputLeftFront = 0f;
cart.DiffSteerTotalOutputLeftRear = 0f;
cart.DiffSteerTotalOutputRightFront = 0f;