优化差速转舵并增加纵向辨识测试
This commit is contained in:
+407
-128
@@ -12,14 +12,28 @@ namespace MedullaAdapter
|
||||
public class MotorRoutine : LadderLogic<DiverCartDefinition>
|
||||
{
|
||||
private const double MaximumFeedforwardIntervalSeconds = 0.2;
|
||||
|
||||
private sealed class DiffSteerWheelControlState
|
||||
{
|
||||
public float PreviousTargetAngleDegrees;
|
||||
public double FilteredInverseFeedforwardMetersPerSecond;
|
||||
public bool IsInStopZone;
|
||||
public int SettlingCycleCount;
|
||||
public bool IsSettled;
|
||||
}
|
||||
|
||||
private bool _wasTransmitterControlling;
|
||||
private DateTime _lastMoveTime = DateTime.Now;
|
||||
private bool _diffSteerFeedforwardInitialized;
|
||||
private long _lastDiffSteerFeedforwardTimestamp;
|
||||
private float _previousThLeftFront;
|
||||
private float _previousThLeftRear;
|
||||
private float _previousThRightFront;
|
||||
private float _previousThRightRear;
|
||||
private readonly DiffSteerWheelControlState _leftFrontSteerState =
|
||||
new DiffSteerWheelControlState();
|
||||
private readonly DiffSteerWheelControlState _leftRearSteerState =
|
||||
new DiffSteerWheelControlState();
|
||||
private readonly DiffSteerWheelControlState _rightFrontSteerState =
|
||||
new DiffSteerWheelControlState();
|
||||
private readonly DiffSteerWheelControlState _rightRearSteerState =
|
||||
new DiffSteerWheelControlState();
|
||||
private DiverCartDefinition.ManualControlMode?
|
||||
_pendingTransmitterControlMode;
|
||||
private DateTime _pendingTransmitterControlModeSince =
|
||||
@@ -306,7 +320,16 @@ namespace MedullaAdapter
|
||||
out var feedforwardLf,
|
||||
out var feedforwardLr,
|
||||
out var feedforwardRf,
|
||||
out var feedforwardRr);
|
||||
out var feedforwardRr,
|
||||
out var suppressLf,
|
||||
out var suppressLr,
|
||||
out var suppressRf,
|
||||
out var suppressRr);
|
||||
|
||||
if (suppressLf) feedbackLf = 0f;
|
||||
if (suppressLr) feedbackLr = 0f;
|
||||
if (suppressRf) feedbackRf = 0f;
|
||||
if (suppressRr) feedbackRr = 0f;
|
||||
|
||||
var diffLf = feedbackLf + feedforwardLf;
|
||||
var diffLr = feedbackLr + feedforwardLr;
|
||||
@@ -345,150 +368,383 @@ namespace MedullaAdapter
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据四个机械目标舵角的实际变化率计算本周期差速轮线速度前馈。
|
||||
/// 更新四轮独立的到位迟滞状态，并计算模型逆前馈。
|
||||
/// </summary>
|
||||
private void CalculateDiffSteerRateFeedforward(
|
||||
out float leftFront,
|
||||
out float leftRear,
|
||||
out float rightFront,
|
||||
out float rightRear)
|
||||
out float rightRear,
|
||||
out bool suppressLeftFront,
|
||||
out bool suppressLeftRear,
|
||||
out bool suppressRightFront,
|
||||
out bool suppressRightRear)
|
||||
{
|
||||
leftFront = leftRear = rightFront = rightRear = 0f;
|
||||
suppressLeftFront = suppressLeftRear = false;
|
||||
suppressRightFront = suppressRightRear = false;
|
||||
ResetDiffSteerCycleDiagnostics();
|
||||
|
||||
var currentTimestamp = Stopwatch.GetTimestamp();
|
||||
var deltaTimeSeconds = 0.0;
|
||||
var hasValidControlPeriod = false;
|
||||
|
||||
if (_diffSteerFeedforwardInitialized)
|
||||
{
|
||||
deltaTimeSeconds =
|
||||
(currentTimestamp -
|
||||
_lastDiffSteerFeedforwardTimestamp) /
|
||||
(double)Stopwatch.Frequency;
|
||||
|
||||
if (double.IsFinite(deltaTimeSeconds) &&
|
||||
deltaTimeSeconds > 0.0)
|
||||
{
|
||||
cart.DiffSteerFeedforwardDeltaTimeMilliseconds =
|
||||
(float)(deltaTimeSeconds * 1000.0);
|
||||
hasValidControlPeriod =
|
||||
deltaTimeSeconds <=
|
||||
MaximumFeedforwardIntervalSeconds;
|
||||
}
|
||||
}
|
||||
|
||||
suppressLeftFront = UpdateDiffSteerSettlingState(
|
||||
_leftFrontSteerState,
|
||||
cart.ThLeftFront,
|
||||
cart.ActualThLeftFront,
|
||||
hasValidControlPeriod);
|
||||
suppressLeftRear = UpdateDiffSteerSettlingState(
|
||||
_leftRearSteerState,
|
||||
cart.ThLeftRear,
|
||||
cart.ActualThLeftRear,
|
||||
hasValidControlPeriod);
|
||||
suppressRightFront = UpdateDiffSteerSettlingState(
|
||||
_rightFrontSteerState,
|
||||
cart.ThRightFront,
|
||||
cart.ActualThRightFront,
|
||||
hasValidControlPeriod);
|
||||
suppressRightRear = UpdateDiffSteerSettlingState(
|
||||
_rightRearSteerState,
|
||||
cart.ThRightRear,
|
||||
cart.ActualThRightRear,
|
||||
hasValidControlPeriod);
|
||||
|
||||
leftFront = CalculateDiffSteerRateFeedforward(
|
||||
_leftFrontSteerState,
|
||||
cart.ThLeftFront,
|
||||
deltaTimeSeconds,
|
||||
hasValidControlPeriod,
|
||||
suppressLeftFront,
|
||||
out var targetRateLf,
|
||||
out var rawLf,
|
||||
out var limitedLf);
|
||||
leftRear = CalculateDiffSteerRateFeedforward(
|
||||
_leftRearSteerState,
|
||||
cart.ThLeftRear,
|
||||
deltaTimeSeconds,
|
||||
hasValidControlPeriod,
|
||||
suppressLeftRear,
|
||||
out var targetRateLr,
|
||||
out var rawLr,
|
||||
out var limitedLr);
|
||||
rightFront = CalculateDiffSteerRateFeedforward(
|
||||
_rightFrontSteerState,
|
||||
cart.ThRightFront,
|
||||
deltaTimeSeconds,
|
||||
hasValidControlPeriod,
|
||||
suppressRightFront,
|
||||
out var targetRateRf,
|
||||
out var rawRf,
|
||||
out var limitedRf);
|
||||
rightRear = CalculateDiffSteerRateFeedforward(
|
||||
_rightRearSteerState,
|
||||
cart.ThRightRear,
|
||||
deltaTimeSeconds,
|
||||
hasValidControlPeriod,
|
||||
suppressRightRear,
|
||||
out var targetRateRr,
|
||||
out var rawRr,
|
||||
out var limitedRr);
|
||||
|
||||
cart.DiffSteerTargetRateLeftFrontDegreesPerSecond = targetRateLf;
|
||||
cart.DiffSteerTargetRateLeftRearDegreesPerSecond = targetRateLr;
|
||||
cart.DiffSteerTargetRateRightFrontDegreesPerSecond = targetRateRf;
|
||||
cart.DiffSteerTargetRateRightRearDegreesPerSecond = targetRateRr;
|
||||
cart.DiffSteerInverseFeedforwardRawLeftFront = rawLf;
|
||||
cart.DiffSteerInverseFeedforwardRawLeftRear = rawLr;
|
||||
cart.DiffSteerInverseFeedforwardRawRightFront = rawRf;
|
||||
cart.DiffSteerInverseFeedforwardRawRightRear = rawRr;
|
||||
cart.DiffSteerFeedforwardLimitedLeftFront = limitedLf;
|
||||
cart.DiffSteerFeedforwardLimitedLeftRear = limitedLr;
|
||||
cart.DiffSteerFeedforwardLimitedRightFront = limitedRf;
|
||||
cart.DiffSteerFeedforwardLimitedRightRear = limitedRr;
|
||||
|
||||
// 无论周期是否有效，都保存本周期目标，避免补算过期阶跃。
|
||||
_leftFrontSteerState.PreviousTargetAngleDegrees =
|
||||
cart.ThLeftFront;
|
||||
_leftRearSteerState.PreviousTargetAngleDegrees =
|
||||
cart.ThLeftRear;
|
||||
_rightFrontSteerState.PreviousTargetAngleDegrees =
|
||||
cart.ThRightFront;
|
||||
_rightRearSteerState.PreviousTargetAngleDegrees =
|
||||
cart.ThRightRear;
|
||||
_lastDiffSteerFeedforwardTimestamp = currentTimestamp;
|
||||
_diffSteerFeedforwardInitialized = true;
|
||||
UpdateDiffSteerStateDiagnostics();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 更新单个舵轮的停止区、连续确认和重新启动状态。
|
||||
/// </summary>
|
||||
private bool UpdateDiffSteerSettlingState(
|
||||
DiffSteerWheelControlState state,
|
||||
float targetAngleDegrees,
|
||||
float actualAngleDegrees,
|
||||
bool hasValidControlPeriod)
|
||||
{
|
||||
var stopErrorDegrees = cart.DiffSteerStopErrorDegrees;
|
||||
var restartErrorDegrees = cart.DiffSteerRestartErrorDegrees;
|
||||
var settlingCycles = cart.DiffSteerSettlingCycles;
|
||||
var hasValidAngles =
|
||||
IsFinite(targetAngleDegrees) &&
|
||||
IsFinite(actualAngleDegrees);
|
||||
var hasValidParameters =
|
||||
IsFinite(stopErrorDegrees) &&
|
||||
stopErrorDegrees >= 0f &&
|
||||
IsFinite(restartErrorDegrees) &&
|
||||
restartErrorDegrees > stopErrorDegrees &&
|
||||
settlingCycles > 0;
|
||||
|
||||
if (!hasValidAngles)
|
||||
{
|
||||
state.IsInStopZone = false;
|
||||
state.SettlingCycleCount = 0;
|
||||
state.IsSettled = false;
|
||||
ClearDiffSteerFeedforwardState(state);
|
||||
return true;
|
||||
}
|
||||
|
||||
var absoluteErrorDegrees = Math.Abs(
|
||||
(double)targetAngleDegrees - actualAngleDegrees);
|
||||
state.IsInStopZone =
|
||||
hasValidParameters &&
|
||||
absoluteErrorDegrees <= stopErrorDegrees;
|
||||
|
||||
if (!cart.EnableDiffSteerSettlingHysteresis ||
|
||||
!hasValidParameters)
|
||||
{
|
||||
state.SettlingCycleCount = 0;
|
||||
state.IsSettled = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (state.IsSettled)
|
||||
{
|
||||
if (absoluteErrorDegrees <= restartErrorDegrees)
|
||||
{
|
||||
ClearDiffSteerFeedforwardState(state);
|
||||
return true;
|
||||
}
|
||||
|
||||
state.IsSettled = false;
|
||||
state.SettlingCycleCount = 0;
|
||||
}
|
||||
|
||||
if (!state.IsInStopZone)
|
||||
{
|
||||
state.SettlingCycleCount = 0;
|
||||
return false;
|
||||
}
|
||||
|
||||
ClearDiffSteerFeedforwardState(state);
|
||||
if (hasValidControlPeriod)
|
||||
{
|
||||
state.SettlingCycleCount = Math.Min(
|
||||
state.SettlingCycleCount + 1,
|
||||
settlingCycles);
|
||||
state.IsSettled =
|
||||
state.SettlingCycleCount >= settlingCycles;
|
||||
}
|
||||
else
|
||||
{
|
||||
state.SettlingCycleCount = 0;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将目标机械舵角变化率转换为带一阶滤波的对象逆前馈。
|
||||
/// </summary>
|
||||
private float CalculateDiffSteerRateFeedforward(
|
||||
DiffSteerWheelControlState state,
|
||||
float targetAngleDegrees,
|
||||
double deltaTimeSeconds,
|
||||
bool hasValidControlPeriod,
|
||||
bool suppressOutput,
|
||||
out float targetRateDegreesPerSecond,
|
||||
out float rawInverseFeedforward,
|
||||
out bool limited)
|
||||
{
|
||||
targetRateDegreesPerSecond = 0f;
|
||||
rawInverseFeedforward = 0f;
|
||||
limited = false;
|
||||
|
||||
if (!hasValidControlPeriod ||
|
||||
!IsFinite(targetAngleDegrees) ||
|
||||
!IsFinite(state.PreviousTargetAngleDegrees) ||
|
||||
!double.IsFinite(deltaTimeSeconds) ||
|
||||
deltaTimeSeconds <= 0.0)
|
||||
{
|
||||
ClearDiffSteerFeedforwardState(state);
|
||||
return 0f;
|
||||
}
|
||||
|
||||
// 机械舵角受限，必须使用直接差值而不是圆周最短角差。
|
||||
var targetRate =
|
||||
((double)targetAngleDegrees -
|
||||
state.PreviousTargetAngleDegrees) /
|
||||
deltaTimeSeconds;
|
||||
if (!double.IsFinite(targetRate))
|
||||
{
|
||||
ClearDiffSteerFeedforwardState(state);
|
||||
return 0f;
|
||||
}
|
||||
|
||||
targetRateDegreesPerSecond = (float)targetRate;
|
||||
if (!IsFinite(targetRateDegreesPerSecond))
|
||||
{
|
||||
targetRateDegreesPerSecond = 0f;
|
||||
ClearDiffSteerFeedforwardState(state);
|
||||
return 0f;
|
||||
}
|
||||
|
||||
var plantGain = cart.DiffSteerPlantGain;
|
||||
var timeConstantSeconds =
|
||||
cart.DiffSteerInverseFeedforwardTimeConstantSeconds;
|
||||
var maximumSpeed =
|
||||
cart.DiffSteerRateFeedforwardMaximumSpeed;
|
||||
|
||||
if (!IsFinite(plantGain) || plantGain <= 0f ||
|
||||
!IsFinite(timeConstantSeconds) ||
|
||||
timeConstantSeconds <= 0f ||
|
||||
!IsFinite(maximumSpeed) || maximumSpeed <= 0f)
|
||||
{
|
||||
ClearDiffSteerFeedforwardState(state);
|
||||
return 0f;
|
||||
}
|
||||
|
||||
var inverseFeedforward = targetRate / plantGain;
|
||||
if (!double.IsFinite(inverseFeedforward))
|
||||
{
|
||||
ClearDiffSteerFeedforwardState(state);
|
||||
return 0f;
|
||||
}
|
||||
|
||||
rawInverseFeedforward = (float)inverseFeedforward;
|
||||
if (!IsFinite(rawInverseFeedforward))
|
||||
{
|
||||
rawInverseFeedforward = 0f;
|
||||
ClearDiffSteerFeedforwardState(state);
|
||||
return 0f;
|
||||
}
|
||||
|
||||
if (!cart.EnableDiffSteerInverseFeedforward ||
|
||||
suppressOutput)
|
||||
{
|
||||
ClearDiffSteerFeedforwardState(state);
|
||||
return 0f;
|
||||
}
|
||||
|
||||
var alpha = 1.0 - Math.Exp(
|
||||
-deltaTimeSeconds / timeConstantSeconds);
|
||||
var filteredFeedforward =
|
||||
state.FilteredInverseFeedforwardMetersPerSecond +
|
||||
alpha *
|
||||
(inverseFeedforward -
|
||||
state.FilteredInverseFeedforwardMetersPerSecond);
|
||||
if (!double.IsFinite(alpha) ||
|
||||
!double.IsFinite(filteredFeedforward))
|
||||
{
|
||||
ClearDiffSteerFeedforwardState(state);
|
||||
return 0f;
|
||||
}
|
||||
|
||||
state.FilteredInverseFeedforwardMetersPerSecond =
|
||||
filteredFeedforward;
|
||||
|
||||
var limitedSpeed = (float)Math.Clamp(
|
||||
filteredFeedforward,
|
||||
-maximumSpeed,
|
||||
maximumSpeed);
|
||||
limited = Math.Abs(
|
||||
filteredFeedforward - limitedSpeed) > 1e-9;
|
||||
return limitedSpeed;
|
||||
}
|
||||
|
||||
private void ResetDiffSteerCycleDiagnostics()
|
||||
{
|
||||
leftFront = 0f;
|
||||
leftRear = 0f;
|
||||
rightFront = 0f;
|
||||
rightRear = 0f;
|
||||
cart.DiffSteerTargetRateLeftFrontDegreesPerSecond = 0f;
|
||||
cart.DiffSteerTargetRateLeftRearDegreesPerSecond = 0f;
|
||||
cart.DiffSteerTargetRateRightFrontDegreesPerSecond = 0f;
|
||||
cart.DiffSteerTargetRateRightRearDegreesPerSecond = 0f;
|
||||
cart.DiffSteerFeedforwardDeltaTimeMilliseconds = 0f;
|
||||
cart.DiffSteerInverseFeedforwardRawLeftFront = 0f;
|
||||
cart.DiffSteerInverseFeedforwardRawLeftRear = 0f;
|
||||
cart.DiffSteerInverseFeedforwardRawRightFront = 0f;
|
||||
cart.DiffSteerInverseFeedforwardRawRightRear = 0f;
|
||||
cart.DiffSteerFeedforwardLimitedLeftFront = false;
|
||||
cart.DiffSteerFeedforwardLimitedLeftRear = false;
|
||||
cart.DiffSteerFeedforwardLimitedRightFront = false;
|
||||
cart.DiffSteerFeedforwardLimitedRightRear = false;
|
||||
|
||||
var currentTimestamp = Stopwatch.GetTimestamp();
|
||||
|
||||
if (_diffSteerFeedforwardInitialized)
|
||||
{
|
||||
var deltaTimeSeconds =
|
||||
(currentTimestamp -
|
||||
_lastDiffSteerFeedforwardTimestamp) /
|
||||
(double)Stopwatch.Frequency;
|
||||
|
||||
if (deltaTimeSeconds > 0.0 &&
|
||||
deltaTimeSeconds <=
|
||||
MaximumFeedforwardIntervalSeconds)
|
||||
{
|
||||
cart.DiffSteerFeedforwardDeltaTimeMilliseconds =
|
||||
(float)(deltaTimeSeconds * 1000.0);
|
||||
leftFront = CalculateDiffSteerRateFeedforward(
|
||||
cart.ThLeftFront,
|
||||
_previousThLeftFront,
|
||||
deltaTimeSeconds,
|
||||
out var targetRateLf,
|
||||
out var limitedLf);
|
||||
leftRear = CalculateDiffSteerRateFeedforward(
|
||||
cart.ThLeftRear,
|
||||
_previousThLeftRear,
|
||||
deltaTimeSeconds,
|
||||
out var targetRateLr,
|
||||
out var limitedLr);
|
||||
rightFront = CalculateDiffSteerRateFeedforward(
|
||||
cart.ThRightFront,
|
||||
_previousThRightFront,
|
||||
deltaTimeSeconds,
|
||||
out var targetRateRf,
|
||||
out var limitedRf);
|
||||
rightRear = CalculateDiffSteerRateFeedforward(
|
||||
cart.ThRightRear,
|
||||
_previousThRightRear,
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
_previousThLeftFront = cart.ThLeftFront;
|
||||
_previousThLeftRear = cart.ThLeftRear;
|
||||
_previousThRightFront = cart.ThRightFront;
|
||||
_previousThRightRear = cart.ThRightRear;
|
||||
_lastDiffSteerFeedforwardTimestamp = currentTimestamp;
|
||||
_diffSteerFeedforwardInitialized = true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将单个机械目标舵角变化率转换为带限幅的左右轮差速线速度前馈。
|
||||
/// 将四轮独立控制状态复制到M层监控和CSV数据源。
|
||||
/// </summary>
|
||||
private float CalculateDiffSteerRateFeedforward(
|
||||
float targetAngleDegrees,
|
||||
float previousTargetAngleDegrees,
|
||||
double deltaTimeSeconds,
|
||||
out float targetRateDegreesPerSecond,
|
||||
out bool limited)
|
||||
private void UpdateDiffSteerStateDiagnostics()
|
||||
{
|
||||
targetRateDegreesPerSecond = 0f;
|
||||
limited = false;
|
||||
cart.DiffSteerInStopZoneLeftFront =
|
||||
_leftFrontSteerState.IsInStopZone;
|
||||
cart.DiffSteerInStopZoneLeftRear =
|
||||
_leftRearSteerState.IsInStopZone;
|
||||
cart.DiffSteerInStopZoneRightFront =
|
||||
_rightFrontSteerState.IsInStopZone;
|
||||
cart.DiffSteerInStopZoneRightRear =
|
||||
_rightRearSteerState.IsInStopZone;
|
||||
cart.DiffSteerSettlingCountLeftFront =
|
||||
_leftFrontSteerState.SettlingCycleCount;
|
||||
cart.DiffSteerSettlingCountLeftRear =
|
||||
_leftRearSteerState.SettlingCycleCount;
|
||||
cart.DiffSteerSettlingCountRightFront =
|
||||
_rightFrontSteerState.SettlingCycleCount;
|
||||
cart.DiffSteerSettlingCountRightRear =
|
||||
_rightRearSteerState.SettlingCycleCount;
|
||||
cart.DiffSteerSettledLeftFront =
|
||||
_leftFrontSteerState.IsSettled;
|
||||
cart.DiffSteerSettledLeftRear =
|
||||
_leftRearSteerState.IsSettled;
|
||||
cart.DiffSteerSettledRightFront =
|
||||
_rightFrontSteerState.IsSettled;
|
||||
cart.DiffSteerSettledRightRear =
|
||||
_rightRearSteerState.IsSettled;
|
||||
cart.DiffSteerInverseFeedforwardFilteredLeftFront =
|
||||
(float)_leftFrontSteerState
|
||||
.FilteredInverseFeedforwardMetersPerSecond;
|
||||
cart.DiffSteerInverseFeedforwardFilteredLeftRear =
|
||||
(float)_leftRearSteerState
|
||||
.FilteredInverseFeedforwardMetersPerSecond;
|
||||
cart.DiffSteerInverseFeedforwardFilteredRightFront =
|
||||
(float)_rightFrontSteerState
|
||||
.FilteredInverseFeedforwardMetersPerSecond;
|
||||
cart.DiffSteerInverseFeedforwardFilteredRightRear =
|
||||
(float)_rightRearSteerState
|
||||
.FilteredInverseFeedforwardMetersPerSecond;
|
||||
}
|
||||
|
||||
var gain = cart.DiffSteerRateFeedforwardGain;
|
||||
var wheelDistanceMillimeters =
|
||||
cart.DiffSteerWheelDistanceMillimeters;
|
||||
var maximumSpeed =
|
||||
cart.DiffSteerRateFeedforwardMaximumSpeed;
|
||||
|
||||
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 =
|
||||
targetRateDegreesPerSecond *
|
||||
Math.PI / 180.0;
|
||||
var wheelDistanceMeters =
|
||||
wheelDistanceMillimeters / 1000.0;
|
||||
var feedforwardSpeed =
|
||||
0.5 *
|
||||
wheelDistanceMeters *
|
||||
targetRateRadiansPerSecond *
|
||||
gain;
|
||||
|
||||
var limitedSpeed = (float)Math.Clamp(
|
||||
feedforwardSpeed,
|
||||
-maximumSpeed,
|
||||
maximumSpeed);
|
||||
limited = Math.Abs(
|
||||
feedforwardSpeed - limitedSpeed) > 1e-9;
|
||||
return limitedSpeed;
|
||||
private static void ClearDiffSteerFeedforwardState(
|
||||
DiffSteerWheelControlState state)
|
||||
{
|
||||
state.FilteredInverseFeedforwardMetersPerSecond = 0.0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -498,6 +754,14 @@ namespace MedullaAdapter
|
||||
{
|
||||
_diffSteerFeedforwardInitialized = false;
|
||||
_lastDiffSteerFeedforwardTimestamp = 0;
|
||||
ResetDiffSteerWheelState(_leftFrontSteerState);
|
||||
ResetDiffSteerWheelState(_leftRearSteerState);
|
||||
ResetDiffSteerWheelState(_rightFrontSteerState);
|
||||
ResetDiffSteerWheelState(_rightRearSteerState);
|
||||
cart.DiffSteerOutputLeftFront = 0f;
|
||||
cart.DiffSteerOutputLeftRear = 0f;
|
||||
cart.DiffSteerOutputRightFront = 0f;
|
||||
cart.DiffSteerOutputRightRear = 0f;
|
||||
cart.DiffSteerRateFeedforwardLeftFront = 0f;
|
||||
cart.DiffSteerRateFeedforwardLeftRear = 0f;
|
||||
cart.DiffSteerRateFeedforwardRightFront = 0f;
|
||||
@@ -507,6 +771,10 @@ namespace MedullaAdapter
|
||||
cart.DiffSteerTargetRateRightFrontDegreesPerSecond = 0f;
|
||||
cart.DiffSteerTargetRateRightRearDegreesPerSecond = 0f;
|
||||
cart.DiffSteerFeedforwardDeltaTimeMilliseconds = 0f;
|
||||
cart.DiffSteerInverseFeedforwardRawLeftFront = 0f;
|
||||
cart.DiffSteerInverseFeedforwardRawLeftRear = 0f;
|
||||
cart.DiffSteerInverseFeedforwardRawRightFront = 0f;
|
||||
cart.DiffSteerInverseFeedforwardRawRightRear = 0f;
|
||||
cart.DiffSteerFeedforwardLimitedLeftFront = false;
|
||||
cart.DiffSteerFeedforwardLimitedLeftRear = false;
|
||||
cart.DiffSteerFeedforwardLimitedRightFront = false;
|
||||
@@ -515,6 +783,17 @@ namespace MedullaAdapter
|
||||
cart.DiffSteerTotalOutputLeftRear = 0f;
|
||||
cart.DiffSteerTotalOutputRightFront = 0f;
|
||||
cart.DiffSteerTotalOutputRightRear = 0f;
|
||||
UpdateDiffSteerStateDiagnostics();
|
||||
}
|
||||
|
||||
private static void ResetDiffSteerWheelState(
|
||||
DiffSteerWheelControlState state)
|
||||
{
|
||||
state.PreviousTargetAngleDegrees = 0f;
|
||||
state.FilteredInverseFeedforwardMetersPerSecond = 0.0;
|
||||
state.IsInStopZone = false;
|
||||
state.SettlingCycleCount = 0;
|
||||
state.IsSettled = false;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
Reference in New Issue
Block a user