101 lines
4.7 KiB
C#
101 lines
4.7 KiB
C#
using System;
|
|
using System.Numerics;
|
|
using ClumsyCore;
|
|
using ClumsyCore.Interfaces;
|
|
using ClumsyCore.Pilot;
|
|
using FundamentalLib;
|
|
using MDCSToolBox.Clumsy.MotionControllers;
|
|
using MDCSToolBox.Clumsy.Movements;
|
|
using MDCSToolBox.Clumsy.Pilot;
|
|
|
|
namespace MultiWheelC;
|
|
|
|
public class ChassisController : MovementDefinition<MultiWheelGeometricController>
|
|
{
|
|
public float BaseSpeed = Configuration.conf.basicSpeed;
|
|
|
|
private DateTime _sendMotionDbgLast = DateTime.MinValue;
|
|
|
|
public override MultiWheelGeometricController Get()
|
|
{
|
|
return new MultiWheelGeometricController
|
|
{
|
|
Chassis = BasicPilotBase.Chassis,
|
|
BaseSpeed = BaseSpeed,
|
|
SlowDistance = PilotDefinition.Conf.SlowDistance,
|
|
SlowingPow = PilotDefinition.Conf.SlowingPow,
|
|
FinishDistance = PilotDefinition.Conf.FinishDistance,
|
|
FinishSpeed = PilotDefinition.Conf.FinishSpeed,
|
|
FirstThAccuracy = PilotDefinition.Conf.FirstThAccuracy,
|
|
FirstRotateSpeedFac = PilotDefinition.Conf.FirstRotateSpeedFac,
|
|
FirstRotateMaxSpeed = PilotDefinition.Conf.FirstRotateMaxSpeed,
|
|
NotContinuousAngle = PilotDefinition.Conf.NotContinuousAngle,
|
|
DebugMode = PilotDefinition.Conf.MotionDebugPrint,
|
|
DebugCurvature = PilotDefinition.Conf.DebugCurvature,
|
|
PowerSteeringLookAhead = PilotDefinition.Conf.PowerSteeringLookAhead,
|
|
SpeedLookAhead = PilotDefinition.Conf.SpeedLookAhead,
|
|
SpeedLookAheadCurveDiff = PilotDefinition.Conf.SpeedLookAheadCurveDiff,
|
|
SpeedLookBackCurveDiff = PilotDefinition.Conf.SpeedLookBackCurveDiff,
|
|
SpeedLimitCurveDiffMin = PilotDefinition.Conf.SpeedLimitCurveDiffMin,
|
|
SpeedLimitCurveMin = PilotDefinition.Conf.SpeedLimitCurveMin,
|
|
MaxRotateSpeed = PilotDefinition.Conf.MaxRotateSpeedCurveLimit,
|
|
MaxRotateAcc = PilotDefinition.Conf.MaxRotateAccCurveLimit,
|
|
GcpThetaThreshold = PilotDefinition.Conf.GcpThetaThreshold,
|
|
DthLinearFac = PilotDefinition.Conf.DthLinearFac,
|
|
DthLinearThreshold = PilotDefinition.Conf.DthLinearThreshold,
|
|
BiasFac = PilotDefinition.Conf.BiasFac,
|
|
BiasThreshold = PilotDefinition.Conf.BiasThreshold,
|
|
|
|
MultiVehicleSendMotion = (speed, frontTh, rearTh, idealPos, idealAngle) =>
|
|
{
|
|
var self = PilotDefinition.Self;
|
|
self.MultiVehicleAutoEnabled = true;
|
|
// A: 用固定锁对象（不再锁会被替换的字段引用）。
|
|
int fleetCnt;
|
|
lock (self.FleetLock)
|
|
fleetCnt = self.MultiVehicleFleet.Count;
|
|
|
|
// 诊断（节流 ~300ms）：确认回调被调用、编队是否就绪、是否因数量不符提前 return（导致不下发速度）。
|
|
if ((DateTime.Now - _sendMotionDbgLast).TotalMilliseconds >= 300)
|
|
{
|
|
_sendMotionDbgLast = DateTime.Now;
|
|
DLog.Log(
|
|
$"SENDMOTION speed={speed:0.000} fTh={frontTh:0.0} rTh={rearTh:0.0} " +
|
|
$"ideal=({idealPos.X:0},{idealPos.Y:0},{idealAngle:0.0}) " +
|
|
$"editCnt={fleetCnt}/{PilotDefinition.Conf.MultiVehicleFleetNum} " +
|
|
$"earlyReturn={fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum}",
|
|
"FleetCrabDbg");
|
|
}
|
|
|
|
if (fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum)
|
|
return;
|
|
|
|
self.MultiVehicleAutoVx = speed;
|
|
self.MultiVehicleAutoFrontTh = frontTh;
|
|
self.MultiVehicleAutoRearTh = rearTh;
|
|
// D: 透传路径控制器算出的理想车队中心位姿（此前被丢弃），供各车按 layout 做前馈。
|
|
self.MultiVehicleAutoIdealX = idealPos.X;
|
|
self.MultiVehicleAutoIdealY = idealPos.Y;
|
|
self.MultiVehicleAutoIdealTh = idealAngle;
|
|
self.MultiVehicleAutoHasIdeal = true;
|
|
// B: 标记命令新鲜度。路径结束/早退/卡顿不再刷新此时刻 → 主车超时后清零速度，避免滑行。
|
|
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
|
},
|
|
|
|
// G: 读取车队中心原子快照，避免跨线程读到撕裂的 x/y/th 组合。
|
|
MultiVehicleGetFleetPos = () =>
|
|
{
|
|
var snap = PilotDefinition.Self.GetFleetCenterSnapshot();
|
|
return new Location
|
|
{
|
|
x = snap.X,
|
|
y = snap.Y,
|
|
th = snap.Th,
|
|
l_step = 1,
|
|
tick = DateTime.Now.Ticks
|
|
};
|
|
}
|
|
};
|
|
}
|
|
}
|