914 lines
35 KiB
C#
914 lines
35 KiB
C#
// 计算8个驱动电机的目标速度和舵角PID
|
|
using CartActivator;
|
|
using FundamentalLib;
|
|
using MDCSToolBox.Commons;
|
|
using System;
|
|
using System.Diagnostics;
|
|
using System.Threading;
|
|
using static MDCSToolBox.Medulla.Chassis.BasicCartDefinition;
|
|
|
|
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 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 =
|
|
DateTime.MinValue;
|
|
public override void Operation(int iteration)
|
|
{
|
|
if (!cart.GhostMode && cart.State == -1) return;
|
|
// SA稳定打开后，使能实体遥控器。
|
|
TriggerOnce(
|
|
cart.TransmitterConnected && cart.Transmitter_SA,
|
|
300,
|
|
() =>
|
|
{
|
|
cart.TransmitterControlEnable = true;
|
|
cart.TransmitterLastTime = DateTime.Now;
|
|
});
|
|
// 遥控器断连或SA关闭时，立即撤销遥控使能。
|
|
if (!cart.TransmitterConnected || !cart.Transmitter_SA)
|
|
cart.TransmitterControlEnable = false;
|
|
var transmitterSelected =
|
|
cart.TransmitterConnected &&
|
|
cart.TransmitterControlEnable &&
|
|
cart.Transmitter_SA &&
|
|
cart.Transmitter_SC == cart.CarNum;
|
|
var transmitterControlling =
|
|
transmitterSelected &&
|
|
CartDefinition.testPriority(5, "TransmitterMode");
|
|
if (transmitterControlling)
|
|
{
|
|
TransmitterChassisControl();
|
|
cart.TransmitterLastTime = DateTime.Now;
|
|
cart.CarStatu = "实体遥控器控制";
|
|
}
|
|
else
|
|
{
|
|
// 只在遥控器刚刚退出时发送一次停车，
|
|
// 不能每周期停车，否则会覆盖C层轨迹控制。
|
|
if (_wasTransmitterControlling)
|
|
{
|
|
cart.ManualControl(
|
|
cart.TransmitterControlMode,
|
|
0, 0, 0,
|
|
cart.TransmitterSpeed,
|
|
DateTime.Now - cart.TransmitterLastTime);
|
|
|
|
StopClampArms();
|
|
cart.TransmitterLastTime = DateTime.Now;
|
|
}
|
|
|
|
cart.CarStatu = "正常运行";
|
|
}
|
|
_wasTransmitterControlling = transmitterControlling;
|
|
// 当前是否由C层控制。
|
|
cart.ClumsyControl = CartDefinition.currentPriority == 0;
|
|
// 计算四个舵轮PID和8个驱动电机最终速度。
|
|
UpdateDiffSteerWheelSpeeds();
|
|
Interlocked.Exchange(
|
|
ref cart.DiffSteerControlTimestamp,
|
|
Stopwatch.GetTimestamp());
|
|
Interlocked.Increment(
|
|
ref cart.DiffSteerControlSequence);
|
|
// 平滑更新硬件速度限制。
|
|
UpdateSendSpeedLimit();
|
|
// 更新红黄绿灯状态。
|
|
UpdateLightMode();
|
|
}
|
|
// M层物理遥控器：只有SB档位连续稳定指定时间后才确认模式切换。
|
|
private bool TryGetStableTransmitterControlMode(
|
|
out DiverCartDefinition.ManualControlMode stableMode)
|
|
{
|
|
stableMode = cart.TransmitterControlMode;
|
|
|
|
DiverCartDefinition.ManualControlMode? requestedMode;
|
|
|
|
switch (cart.Transmitter_SB)
|
|
{
|
|
case TransmitterState.Mode0:
|
|
requestedMode =
|
|
DiverCartDefinition.ManualControlMode.Normal;
|
|
break;
|
|
|
|
case TransmitterState.Mode1:
|
|
requestedMode =
|
|
DiverCartDefinition.ManualControlMode.Crab;
|
|
break;
|
|
|
|
case TransmitterState.Mode2:
|
|
requestedMode =
|
|
DiverCartDefinition.ManualControlMode.Spin;
|
|
break;
|
|
|
|
default:
|
|
_pendingTransmitterControlMode = null;
|
|
_pendingTransmitterControlModeSince =
|
|
DateTime.MinValue;
|
|
return false;
|
|
}
|
|
|
|
if (_pendingTransmitterControlMode != requestedMode)
|
|
{
|
|
_pendingTransmitterControlMode = requestedMode;
|
|
_pendingTransmitterControlModeSince = DateTime.Now;
|
|
return false;
|
|
}
|
|
|
|
var debounceMilliseconds = Math.Max(
|
|
0,
|
|
cart.TransmitterModeDebounceMilliseconds);
|
|
|
|
if ((DateTime.Now -
|
|
_pendingTransmitterControlModeSince)
|
|
.TotalMilliseconds < debounceMilliseconds)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
stableMode = requestedMode.Value;
|
|
return true;
|
|
}
|
|
|
|
// 物理遥控器设置
|
|
public void TransmitterChassisControl()
|
|
{
|
|
var interval = DateTime.Now - cart.TransmitterLastTime;
|
|
|
|
if (!TryGetStableTransmitterControlMode(
|
|
out var stableControlMode))
|
|
{
|
|
// SB处于中间档或尚未稳定时立即停车，
|
|
// 保持当前模式，不下发新的模式目标角。
|
|
cart.ManualControl(
|
|
cart.TransmitterControlMode,
|
|
0, 0, 0,
|
|
cart.TransmitterSpeed,
|
|
interval);
|
|
StopClampArms();
|
|
return;
|
|
}
|
|
|
|
cart.TransmitterControlMode = stableControlMode;
|
|
|
|
// SA关闭后立即停车。
|
|
if (!cart.Transmitter_SA)
|
|
{
|
|
cart.ManualControl(
|
|
cart.TransmitterControlMode,
|
|
0, 0, 0,
|
|
cart.TransmitterSpeed,
|
|
interval);
|
|
StopClampArms();
|
|
return;
|
|
}
|
|
// 限制实体遥控器的最大速度。
|
|
cart.TransmitterSpeed = Math.Max(
|
|
cart.TransmitterSpeedLowerLimit,
|
|
Math.Min(
|
|
cart.TransmitterSpeed,
|
|
cart.TransmitterSpeedUpperLimit));
|
|
// SD的Mode0作为底盘驾驶档。
|
|
if (cart.Transmitter_SD == TransmitterState.Mode0)
|
|
{
|
|
// 底盘驾驶档不允许保留上一周期的夹臂速度。
|
|
StopClampArms();
|
|
|
|
cart.ManualControl(
|
|
cart.TransmitterControlMode,
|
|
cart.TransmitterLeftJoystickValX,
|
|
cart.TransmitterRightJoystickValY,
|
|
0,
|
|
cart.TransmitterSpeed,
|
|
interval);
|
|
|
|
return;
|
|
}
|
|
if (cart.Transmitter_SD == TransmitterState.Mode1)
|
|
{
|
|
// 切换到夹臂档时，先确保底盘停止。
|
|
cart.ManualControl(
|
|
cart.TransmitterControlMode,
|
|
0, 0, 0,
|
|
cart.TransmitterSpeed,
|
|
interval);
|
|
|
|
var armSpeed =
|
|
cart.TransmitterRightJoystickValX *
|
|
cart.ManualArmSpeedFac;
|
|
|
|
cart.SpeedLeftArm = armSpeed;
|
|
cart.SpeedRightArm = armSpeed;
|
|
return;
|
|
}
|
|
// 非驾驶档必须主动停车，防止上一条运动指令残留。
|
|
cart.ManualControl(
|
|
cart.TransmitterControlMode,
|
|
0, 0, 0,
|
|
cart.TransmitterSpeed,
|
|
interval);
|
|
StopClampArms();
|
|
}
|
|
|
|
// M层单车夹臂安全：清除物理遥控器留下的左右夹臂速度命令。
|
|
private void StopClampArms()
|
|
{
|
|
cart.SpeedLeftArm = 0;
|
|
cart.SpeedRightArm = 0;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 根据四个舵轮的目标角速度前馈和实际角度反馈修正八个驱动电机速度。
|
|
/// </summary>
|
|
private void UpdateDiffSteerWheelSpeeds()
|
|
{
|
|
if (cart.LeftFrontPid == null ||
|
|
cart.LeftRearPid == null ||
|
|
cart.RightFrontPid == null ||
|
|
cart.RightRearPid == null)
|
|
{
|
|
cart.SpeedLFL = 0;
|
|
cart.SpeedLFR = 0;
|
|
cart.SpeedRFL = 0;
|
|
cart.SpeedRFR = 0;
|
|
cart.SpeedLRL = 0;
|
|
cart.SpeedLRR = 0;
|
|
cart.SpeedRRL = 0;
|
|
cart.SpeedRRR = 0;
|
|
ResetDiffSteerRateFeedforward();
|
|
return;
|
|
}
|
|
|
|
// 更新左前舵轮PID参数。
|
|
cart.LeftFrontPid.ChangeParameters(
|
|
cart.DiffSteerKp,
|
|
cart.DiffSteerKi,
|
|
cart.DiffSteerKd,
|
|
cart.DiffSteerMaxI,
|
|
cart.DiffSteerDeadZone,
|
|
cart.DiffSteerThresh,
|
|
cart.DiffSteerSpeedAcc);
|
|
|
|
// 更新左后舵轮PID参数。
|
|
cart.LeftRearPid.ChangeParameters(
|
|
cart.DiffSteerKp,
|
|
cart.DiffSteerKi,
|
|
cart.DiffSteerKd,
|
|
cart.DiffSteerMaxI,
|
|
cart.DiffSteerDeadZone,
|
|
cart.DiffSteerThresh,
|
|
cart.DiffSteerSpeedAcc);
|
|
|
|
// 更新右前舵轮PID参数。
|
|
cart.RightFrontPid.ChangeParameters(
|
|
cart.DiffSteerKp,
|
|
cart.DiffSteerKi,
|
|
cart.DiffSteerKd,
|
|
cart.DiffSteerMaxI,
|
|
cart.DiffSteerDeadZone,
|
|
cart.DiffSteerThresh,
|
|
cart.DiffSteerSpeedAcc);
|
|
|
|
// 更新右后舵轮PID参数。
|
|
cart.RightRearPid.ChangeParameters(
|
|
cart.DiffSteerKp,
|
|
cart.DiffSteerKi,
|
|
cart.DiffSteerKd,
|
|
cart.DiffSteerMaxI,
|
|
cart.DiffSteerDeadZone,
|
|
cart.DiffSteerThresh,
|
|
cart.DiffSteerSpeedAcc);
|
|
|
|
// 根据实际舵角计算四条腿的PID反馈修正量。
|
|
var feedbackLf = cart.LeftFrontPid.GetResponse(
|
|
cart.ThLeftFront, false, false, "LF");
|
|
|
|
var feedbackLr = cart.LeftRearPid.GetResponse(
|
|
cart.ThLeftRear, false, false, "LR");
|
|
|
|
var feedbackRf = cart.RightFrontPid.GetResponse(
|
|
cart.ThRightFront, false, false, "RF");
|
|
|
|
var feedbackRr = cart.RightRearPid.GetResponse(
|
|
cart.ThRightRear, false, false, "RR");
|
|
|
|
CalculateDiffSteerRateFeedforward(
|
|
out var feedforwardLf,
|
|
out var feedforwardLr,
|
|
out var feedforwardRf,
|
|
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;
|
|
var diffRf = feedbackRf + feedforwardRf;
|
|
var diffRr = feedbackRr + feedforwardRr;
|
|
|
|
// 分别保留PID、前馈和合成差速，便于独立标定与诊断。
|
|
cart.DiffSteerOutputLeftFront = feedbackLf;
|
|
cart.DiffSteerOutputLeftRear = feedbackLr;
|
|
cart.DiffSteerOutputRightFront = feedbackRf;
|
|
cart.DiffSteerOutputRightRear = feedbackRr;
|
|
cart.DiffSteerRateFeedforwardLeftFront = feedforwardLf;
|
|
cart.DiffSteerRateFeedforwardLeftRear = feedforwardLr;
|
|
cart.DiffSteerRateFeedforwardRightFront = feedforwardRf;
|
|
cart.DiffSteerRateFeedforwardRightRear = feedforwardRr;
|
|
cart.DiffSteerTotalOutputLeftFront = diffLf;
|
|
cart.DiffSteerTotalOutputLeftRear = diffLr;
|
|
cart.DiffSteerTotalOutputRightFront = diffRf;
|
|
cart.DiffSteerTotalOutputRightRear = diffRr;
|
|
|
|
// 左前腿：左右电机施加方向相反的合成差速修正量。
|
|
cart.SpeedLFL = cart.SpeedLeftFrontLeft - diffLf;
|
|
cart.SpeedLFR = cart.SpeedLeftFrontRight + diffLf;
|
|
|
|
// 左后腿。
|
|
cart.SpeedLRL = cart.SpeedLeftRearLeft - diffLr;
|
|
cart.SpeedLRR = cart.SpeedLeftRearRight + diffLr;
|
|
|
|
// 右前腿。
|
|
cart.SpeedRFL = cart.SpeedRightFrontLeft - diffRf;
|
|
cart.SpeedRFR = cart.SpeedRightFrontRight + diffRf;
|
|
|
|
// 右后腿。
|
|
cart.SpeedRRL = cart.SpeedRightRearLeft - diffRr;
|
|
cart.SpeedRRR = cart.SpeedRightRearRight + diffRr;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 更新四轮独立的到位迟滞状态，并计算模型逆前馈。
|
|
/// </summary>
|
|
private void CalculateDiffSteerRateFeedforward(
|
|
out float leftFront,
|
|
out float leftRear,
|
|
out float rightFront,
|
|
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()
|
|
{
|
|
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;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将四轮独立控制状态复制到M层监控和CSV数据源。
|
|
/// </summary>
|
|
private void UpdateDiffSteerStateDiagnostics()
|
|
{
|
|
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;
|
|
}
|
|
|
|
private static void ClearDiffSteerFeedforwardState(
|
|
DiffSteerWheelControlState state)
|
|
{
|
|
state.FilteredInverseFeedforwardMetersPerSecond = 0.0;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 清除差速转舵前馈历史和监控输出，避免恢复控制时使用过期目标角。
|
|
/// </summary>
|
|
private void ResetDiffSteerRateFeedforward()
|
|
{
|
|
_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;
|
|
cart.DiffSteerRateFeedforwardRightRear = 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;
|
|
cart.DiffSteerTotalOutputLeftFront = 0f;
|
|
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>
|
|
/// 判断单精度参数是否可安全参与底盘控制计算。
|
|
/// </summary>
|
|
private static bool IsFinite(float value)
|
|
{
|
|
return !float.IsNaN(value) &&
|
|
!float.IsInfinity(value);
|
|
}
|
|
|
|
// M层单车限速：按照加速度和减速度平滑更新实际下发速度上限。
|
|
private void UpdateSendSpeedLimit()
|
|
{
|
|
if (cart.Chassis == null)
|
|
{
|
|
cart.SendThresSpeed = 0;
|
|
return;
|
|
}
|
|
|
|
var now = DateTime.Now;
|
|
var elapsedSeconds = (float)(now - _lastMoveTime).TotalSeconds;
|
|
_lastMoveTime = now;
|
|
|
|
// 防止调试暂停或线程卡顿后，一次产生过大的速度跳变。
|
|
elapsedSeconds = Math.Clamp(elapsedSeconds, 0f, 0.2f);
|
|
|
|
// 限速值不允许小于零。
|
|
var targetLimit = Math.Max(0f, cart.ThresSpeed);
|
|
var currentLimit = Math.Max(0f, cart.SendThresSpeed);
|
|
|
|
// 增大速度上限时用加速度，减小时用减速度。
|
|
var speedChangingRate =
|
|
targetLimit > currentLimit
|
|
? cart.Chassis.AccPerSecond
|
|
: cart.Chassis.DeAccPerSecond;
|
|
|
|
speedChangingRate = Math.Max(0f, speedChangingRate);
|
|
|
|
var maxChange = speedChangingRate * elapsedSeconds;
|
|
var speedDifference = targetLimit - currentLimit;
|
|
|
|
if (Math.Abs(speedDifference) <= maxChange)
|
|
{
|
|
currentLimit = targetLimit;
|
|
}
|
|
else
|
|
{
|
|
currentLimit += Math.Sign(speedDifference) * maxChange;
|
|
}
|
|
|
|
// 计算当前8个电机目标速度中的最大绝对值。
|
|
var wheelMaxSpeed = 0f;
|
|
|
|
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLFL));
|
|
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLFR));
|
|
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRFL));
|
|
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRFR));
|
|
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLRL));
|
|
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLRR));
|
|
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRRL));
|
|
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRRR));
|
|
|
|
// 没有必要让平滑限速值高于当前所有车轮需要的速度。
|
|
if (targetLimit < wheelMaxSpeed &&
|
|
currentLimit > wheelMaxSpeed)
|
|
{
|
|
currentLimit = wheelMaxSpeed;
|
|
}
|
|
|
|
cart.SendThresSpeed = currentLimit;
|
|
}
|
|
|
|
// M层单车灯光：根据报警、驱动器、限速和电量状态生成红黄绿灯模式。
|
|
private void UpdateLightMode()
|
|
{
|
|
// 二级报警：红灯常亮。
|
|
if (cart.AlarmLevel == 2)
|
|
{
|
|
cart.LightMode = 2;
|
|
return;
|
|
}
|
|
|
|
// 一级报警：黄灯常亮。
|
|
if (cart.AlarmLevel == 1)
|
|
{
|
|
cart.LightMode = 3;
|
|
return;
|
|
}
|
|
|
|
// 驱动轮未使能：黄灯闪烁。
|
|
if (!cart.WheelAbleState)
|
|
{
|
|
FlipFlop(ref cart.LightMode, 500, 0, 3);
|
|
return;
|
|
}
|
|
|
|
// C层正在进行限速：黄灯常亮。
|
|
if (Math.Abs(cart.ThresSpeed - 1f) > 0.001f)
|
|
{
|
|
cart.LightMode = 3;
|
|
return;
|
|
}
|
|
|
|
// 电量低于预警值：黄灯常亮。
|
|
if (cart.Soc <= cart.LowBatteryAlarmThreshold)
|
|
{
|
|
cart.LightMode = 3;
|
|
return;
|
|
}
|
|
|
|
// 正常运行：绿灯闪烁。
|
|
FlipFlop(ref cart.LightMode, 500, 0, 1);
|
|
}
|
|
|
|
}
|
|
}
|