拆分MultiWheelC并新增轨迹投影、Detour状态估计与Stanley跟踪控制
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
@@ -0,0 +1,91 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using ClumsyCore.Pilot;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public class ClampToTarget : MovementDefinition
|
||||
{
|
||||
public float LeftClampTarget;
|
||||
public float RightClampTarget;
|
||||
public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed;
|
||||
public float ClampKp = PilotDefinition.Conf.ClampControlKp;
|
||||
public float ClampKi = PilotDefinition.Conf.ClampControlKi;
|
||||
public float ClampKd = PilotDefinition.Conf.ClampControlKd;
|
||||
public float ClampMaxI = PilotDefinition.Conf.ClampControlMaxI;
|
||||
public float ClampSpeedAcc = PilotDefinition.Conf.ClampControlSpeedAcc;
|
||||
public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
|
||||
public float TimeoutSeconds = 30f;
|
||||
private PIDController leftpid, rightpid;
|
||||
|
||||
// C层单车业务：驱动左右夹臂运动到夹紧或松开目标。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
try
|
||||
{
|
||||
leftpid = new PIDController(
|
||||
() => PilotDefinition.Self.ActualPosLeftArm,
|
||||
ClampKp, ClampKi, ClampKd, ClampMaxI,
|
||||
ClampDeadZone, MaxClampSpeed)
|
||||
{
|
||||
SpeedAccPerSec = ClampSpeedAcc
|
||||
};
|
||||
|
||||
rightpid = new PIDController(
|
||||
() => PilotDefinition.Self.ActualPosRightArm,
|
||||
ClampKp, ClampKi, ClampKd, ClampMaxI,
|
||||
ClampDeadZone, MaxClampSpeed)
|
||||
{
|
||||
SpeedAccPerSec = ClampSpeedAcc
|
||||
};
|
||||
|
||||
var startTime = DateTime.UtcNow;
|
||||
while (true)
|
||||
{
|
||||
if (TimeoutSeconds > 0f &&
|
||||
(DateTime.UtcNow - startTime).TotalSeconds >
|
||||
TimeoutSeconds)
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"夹臂运动超时({TimeoutSeconds:F1}s)，" +
|
||||
"停止左右夹臂。");
|
||||
yield break;
|
||||
}
|
||||
|
||||
var leftspeed =
|
||||
leftpid.GetResponse(LeftClampTarget);
|
||||
var rightspeed =
|
||||
rightpid.GetResponse(RightClampTarget);
|
||||
Console.WriteLine(
|
||||
$"left arm speed:{leftspeed} " +
|
||||
$"right arm speed:{rightspeed}");
|
||||
|
||||
PilotDefinition.Self.SpeedLeftArm = leftspeed;
|
||||
PilotDefinition.Self.SpeedRightArm = rightspeed;
|
||||
|
||||
var leftArrived = leftpid.IsArrived();
|
||||
var rightArrived = rightpid.IsArrived();
|
||||
if (leftArrived)
|
||||
PilotDefinition.Self.SpeedLeftArm = 0f;
|
||||
if (rightArrived)
|
||||
PilotDefinition.Self.SpeedRightArm = 0f;
|
||||
|
||||
if (leftArrived && rightArrived)
|
||||
break;
|
||||
|
||||
yield return true;
|
||||
}
|
||||
|
||||
Console.WriteLine(
|
||||
$"left clamp to target:{LeftClampTarget} " +
|
||||
$"right clamp to target:{RightClampTarget}");
|
||||
}
|
||||
finally
|
||||
{
|
||||
PilotDefinition.Self.SpeedLeftArm = 0f;
|
||||
PilotDefinition.Self.SpeedRightArm = 0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,817 @@
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
using MyParking.Shared;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
// C层单车测试：在可配置的运动坐标系中统一跟踪直线、圆弧或S型曲线。
|
||||
public sealed class CrabMotionFrameTracker : MovementDefinition
|
||||
{
|
||||
public enum ReferencePathKind
|
||||
{
|
||||
Straight = 0,
|
||||
LeftArc = 1,
|
||||
SCurve = 2
|
||||
}
|
||||
|
||||
public enum ChassisCommandBackend
|
||||
{
|
||||
SendXYThSpeed = 0,
|
||||
SendMotion = 1
|
||||
}
|
||||
|
||||
public ReferencePathKind PathKind;
|
||||
public ChassisCommandBackend CommandBackend =
|
||||
ChassisCommandBackend.SendMotion;
|
||||
public Vector2 StartPosition;
|
||||
public double InitialBodyYawRadians;
|
||||
public float LengthMillimeters = 4000f;
|
||||
public float RadiusMillimeters = 2000f;
|
||||
public float SCurveLateralOffsetMillimeters = 400f;
|
||||
public double ArcSweepRadians = Math.PI / 2.0;
|
||||
public float CruiseSpeed = 0.2f;
|
||||
public float SlowDistanceMillimeters = 600f;
|
||||
public float FinishDistanceMillimeters = 30f;
|
||||
public float MinimumSpeed = 0.04f;
|
||||
public double LateralGainPerSecond = 0.8;
|
||||
public double MaximumLateralCorrection = 0.12;
|
||||
public double HeadingGainPerSecond = 1.5;
|
||||
public double MaximumAngularSpeedRadiansPerSecond =
|
||||
AngleMath.DegreesToRadians(30.0);
|
||||
public double MaximumVirtualSteeringRadians =
|
||||
AngleMath.DegreesToRadians(30.0);
|
||||
public float WheelAlignmentToleranceDegrees = 2f;
|
||||
public float WheelAlignmentStableSeconds = 0.3f;
|
||||
public float WheelAlignmentTimeoutSeconds = 10f;
|
||||
public float TrackingTimeoutSeconds = 60f;
|
||||
public Action<float, float, float> CommandObserver;
|
||||
|
||||
// 运动坐标系相对车体坐标系的朝向：普通模式为0，蟹行为π/2。
|
||||
public double MotionFrameYawInBodyRadians = Math.PI / 2.0;
|
||||
private double _lastSCurveProgress;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
ValidateParameters();
|
||||
|
||||
var chassis =
|
||||
PilotDefinition.Chassis as MultiWheelChassis;
|
||||
if (chassis == null)
|
||||
throw new InvalidOperationException(
|
||||
"当前底盘不是MultiWheelChassis，无法执行运动坐标系轨迹测试。");
|
||||
|
||||
var adapter = new MultiWheelChassisAdapter(
|
||||
chassis,
|
||||
PilotDefinition.Self.CarNum);
|
||||
adapter.ResetToBodyFrame();
|
||||
|
||||
var lastCommandTime = DateTime.Now;
|
||||
|
||||
try
|
||||
{
|
||||
// 模式切换阶段只转舵轮，驱动速度始终保持为零。
|
||||
var alignmentStarted = DateTime.Now;
|
||||
DateTime? stableSince = null;
|
||||
while (true)
|
||||
{
|
||||
if (!adapter.PrepareParallelDirection(
|
||||
MotionFrameYawInBodyRadians))
|
||||
throw new InvalidOperationException(
|
||||
"无法生成运动坐标系对应的舵轮准备姿态。");
|
||||
|
||||
var aligned =
|
||||
adapter.AreParallelWheelsAligned(
|
||||
MotionFrameYawInBodyRadians,
|
||||
AngleMath.DegreesToRadians(
|
||||
WheelAlignmentToleranceDegrees));
|
||||
|
||||
if (aligned)
|
||||
{
|
||||
if (stableSince == null)
|
||||
stableSince = DateTime.Now;
|
||||
|
||||
if ((DateTime.Now - stableSince.Value)
|
||||
.TotalSeconds >=
|
||||
WheelAlignmentStableSeconds)
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
stableSince = null;
|
||||
}
|
||||
|
||||
if ((DateTime.Now - alignmentStarted)
|
||||
.TotalSeconds >
|
||||
WheelAlignmentTimeoutSeconds)
|
||||
throw new TimeoutException(
|
||||
"舵轮在限定时间内未稳定到达运动坐标系初始方向。");
|
||||
|
||||
yield return true;
|
||||
}
|
||||
|
||||
if (CommandBackend ==
|
||||
ChassisCommandBackend.SendMotion)
|
||||
{
|
||||
// 舵轮已按真实机械角度完成预对齐；
|
||||
// 现在由Shared适配层激活SendMotion虚拟运动坐标系。
|
||||
adapter.ActivateMotionFrame(
|
||||
MotionFrameYawInBodyRadians);
|
||||
}
|
||||
|
||||
var trackingStarted = DateTime.Now;
|
||||
while (true)
|
||||
{
|
||||
if ((DateTime.Now - trackingStarted)
|
||||
.TotalSeconds >
|
||||
TrackingTimeoutSeconds)
|
||||
throw new TimeoutException(
|
||||
"蟹行轨迹在限定时间内未完成。");
|
||||
|
||||
var location =
|
||||
DetourInterface.getCartLocation();
|
||||
if (!IsFinite(location.x) ||
|
||||
!IsFinite(location.y) ||
|
||||
!IsFinite(location.th))
|
||||
throw new InvalidOperationException(
|
||||
"蟹行轨迹测试期间Detour位姿无效。");
|
||||
|
||||
var currentPosition = new Vector2(
|
||||
(float)location.x,
|
||||
(float)location.y);
|
||||
var currentBodyYaw =
|
||||
AngleMath.DegreesToRadians(location.th);
|
||||
|
||||
CalculateReference(
|
||||
currentPosition,
|
||||
out var tangentYaw,
|
||||
out var referencePoint,
|
||||
out var remainingMillimeters,
|
||||
out var referenceCurvature);
|
||||
|
||||
if (remainingMillimeters <=
|
||||
FinishDistanceMillimeters)
|
||||
break;
|
||||
|
||||
var speed =
|
||||
CalculateSpeed(remainingMillimeters);
|
||||
var tangent = new Vector2(
|
||||
(float)Math.Cos(tangentYaw),
|
||||
(float)Math.Sin(tangentYaw));
|
||||
var leftNormal = new Vector2(
|
||||
-tangent.Y,
|
||||
tangent.X);
|
||||
var positionError =
|
||||
currentPosition - referencePoint;
|
||||
var lateralErrorMeters =
|
||||
Vector2.Dot(
|
||||
positionError,
|
||||
leftNormal) / 1000.0;
|
||||
var normalCorrection =
|
||||
Limit(
|
||||
-LateralGainPerSecond *
|
||||
lateralErrorMeters,
|
||||
MaximumLateralCorrection);
|
||||
|
||||
// 先在世界坐标中组合切向速度与横向纠偏速度。
|
||||
var worldVx =
|
||||
tangent.X * speed +
|
||||
leftNormal.X * (float)normalCorrection;
|
||||
var worldVy =
|
||||
tangent.Y * speed +
|
||||
leftNormal.Y * (float)normalCorrection;
|
||||
|
||||
// 将世界速度表达为当前蟹行运动坐标系速度。
|
||||
var motionYaw =
|
||||
currentBodyYaw +
|
||||
MotionFrameYawInBodyRadians;
|
||||
var motionCos = Math.Cos(motionYaw);
|
||||
var motionSin = Math.Sin(motionYaw);
|
||||
var vxInMotion =
|
||||
motionCos * worldVx +
|
||||
motionSin * worldVy;
|
||||
var vyInMotion =
|
||||
-motionSin * worldVx +
|
||||
motionCos * worldVy;
|
||||
|
||||
var desiredBodyYaw =
|
||||
tangentYaw -
|
||||
MotionFrameYawInBodyRadians;
|
||||
var headingError =
|
||||
AngleMath.ShortestDifferenceRadians(
|
||||
desiredBodyYaw,
|
||||
currentBodyYaw);
|
||||
var omega =
|
||||
speed * referenceCurvature +
|
||||
HeadingGainPerSecond * headingError;
|
||||
omega = Limit(
|
||||
omega,
|
||||
MaximumAngularSpeedRadiansPerSecond);
|
||||
|
||||
var now = DateTime.Now;
|
||||
var interval = now - lastCommandTime;
|
||||
lastCommandTime = now;
|
||||
|
||||
bool commandAccepted;
|
||||
Twist2D bodyTwist;
|
||||
if (CommandBackend ==
|
||||
ChassisCommandBackend.SendMotion)
|
||||
{
|
||||
// 运动坐标系相对车体系旋转+90°：
|
||||
// 运动系正向速度会转换成车体系+Y速度。
|
||||
bodyTwist =
|
||||
FrameTransform2D
|
||||
.TransformTwistAtSamePoint(
|
||||
new Pose2D(
|
||||
0.0,
|
||||
0.0,
|
||||
MotionFrameYawInBodyRadians),
|
||||
new Twist2D(
|
||||
vxInMotion,
|
||||
vyInMotion,
|
||||
omega));
|
||||
|
||||
// 将运动坐标系原点和前后几何控制点处的速度，
|
||||
// 转换为SendMotion需要的前后轴方向。
|
||||
var controlPointRadiusMeters =
|
||||
Math.Max(
|
||||
chassis.ControlPointRadius /
|
||||
1000.0,
|
||||
0.001);
|
||||
var frontVelocityY =
|
||||
vyInMotion +
|
||||
omega *
|
||||
controlPointRadiusMeters;
|
||||
var rearVelocityY =
|
||||
vyInMotion -
|
||||
omega *
|
||||
controlPointRadiusMeters;
|
||||
var frontSteeringRadians =
|
||||
Math.Atan2(
|
||||
frontVelocityY,
|
||||
vxInMotion);
|
||||
var rearSteeringRadians =
|
||||
Math.Atan2(
|
||||
rearVelocityY,
|
||||
vxInMotion);
|
||||
|
||||
// 蟹行测试绕过M层ManualControl并直接调用SendMotion，
|
||||
// 因此需要在C层同步应用蟹行虚拟几何比例和转向符号。
|
||||
if (IsCrabMotionFrame())
|
||||
{
|
||||
var geometryRatio =
|
||||
adapter.HalfTrackWidthMeters /
|
||||
adapter.HalfWheelBaseMeters;
|
||||
|
||||
frontSteeringRadians =
|
||||
ConvertToCrabSteering(
|
||||
frontSteeringRadians,
|
||||
geometryRatio);
|
||||
rearSteeringRadians =
|
||||
ConvertToCrabSteering(
|
||||
rearSteeringRadians,
|
||||
geometryRatio);
|
||||
}
|
||||
|
||||
var frontThetaDegrees =
|
||||
(float)AngleMath.RadiansToDegrees(
|
||||
frontSteeringRadians);
|
||||
var rearThetaDegrees =
|
||||
(float)AngleMath.RadiansToDegrees(
|
||||
rearSteeringRadians);
|
||||
var motionSpeed =
|
||||
(float)Math.Sqrt(
|
||||
vxInMotion * vxInMotion +
|
||||
vyInMotion * vyInMotion);
|
||||
|
||||
commandAccepted =
|
||||
chassis.SendMotion(
|
||||
motionSpeed,
|
||||
frontThetaDegrees,
|
||||
rearThetaDegrees,
|
||||
interval);
|
||||
}
|
||||
else if (CommandBackend ==
|
||||
ChassisCommandBackend
|
||||
.SendXYThSpeed)
|
||||
{
|
||||
// 安全XYTh后端根据舵角误差统一压低驱动轮速。
|
||||
bodyTwist =
|
||||
FrameTransform2D
|
||||
.TransformTwistAtSamePoint(
|
||||
new Pose2D(
|
||||
0.0,
|
||||
0.0,
|
||||
MotionFrameYawInBodyRadians),
|
||||
new Twist2D(
|
||||
vxInMotion,
|
||||
vyInMotion,
|
||||
omega));
|
||||
var command = new ChassisCommand(
|
||||
PilotDefinition.Self.CarNum,
|
||||
bodyTwist);
|
||||
commandAccepted =
|
||||
adapter.Send(
|
||||
command,
|
||||
interval);
|
||||
}
|
||||
else
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"不支持的底盘命令后端：{CommandBackend}。");
|
||||
}
|
||||
|
||||
if (!commandAccepted)
|
||||
throw new InvalidOperationException(
|
||||
"运动坐标系轨迹底盘解算失败：" +
|
||||
chassis
|
||||
.LastMotionDecomposeFailureReason);
|
||||
|
||||
CommandObserver?.Invoke(
|
||||
(float)bodyTwist.VxMetersPerSecond,
|
||||
(float)bodyTwist.VyMetersPerSecond,
|
||||
(float)bodyTwist
|
||||
.OmegaRadiansPerSecond);
|
||||
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
adapter.StopImmediately();
|
||||
if (CommandBackend ==
|
||||
ChassisCommandBackend.SendMotion)
|
||||
{
|
||||
// 测试退出后恢复真实车体坐标系，避免影响后续测试。
|
||||
adapter.ResetToBodyFrame();
|
||||
}
|
||||
CommandObserver?.Invoke(0f, 0f, 0f);
|
||||
}
|
||||
|
||||
yield return false;
|
||||
}
|
||||
|
||||
// 判断当前运动坐标系是否为车体左侧朝前的蟹行坐标系。
|
||||
private bool IsCrabMotionFrame()
|
||||
{
|
||||
return Math.Abs(
|
||||
AngleMath.ShortestDifferenceRadians(
|
||||
Math.PI / 2.0,
|
||||
MotionFrameYawInBodyRadians)) <
|
||||
1e-6;
|
||||
}
|
||||
|
||||
// 按车体几何比例缩小蟹行转角。
|
||||
// +90°运动坐标系已经完成方向映射，此处不能再次反号。
|
||||
private double ConvertToCrabSteering(
|
||||
double normalSteeringRadians,
|
||||
double geometryRatio)
|
||||
{
|
||||
var crabSteeringRadians =
|
||||
Math.Atan(
|
||||
geometryRatio *
|
||||
Math.Tan(
|
||||
normalSteeringRadians));
|
||||
|
||||
return Limit(
|
||||
crabSteeringRadians,
|
||||
MaximumVirtualSteeringRadians);
|
||||
}
|
||||
|
||||
// 计算当前点在直线或圆弧上的参考点、切线和剩余距离。
|
||||
private void CalculateReference(
|
||||
Vector2 currentPosition,
|
||||
out double tangentYaw,
|
||||
out Vector2 referencePoint,
|
||||
out float remainingMillimeters,
|
||||
out double curvaturePerMeter)
|
||||
{
|
||||
var initialMotionYaw =
|
||||
InitialBodyYawRadians +
|
||||
MotionFrameYawInBodyRadians;
|
||||
|
||||
if (PathKind == ReferencePathKind.Straight)
|
||||
{
|
||||
var tangent = new Vector2(
|
||||
(float)Math.Cos(initialMotionYaw),
|
||||
(float)Math.Sin(initialMotionYaw));
|
||||
var relative = currentPosition - StartPosition;
|
||||
var progress =
|
||||
Vector2.Dot(relative, tangent);
|
||||
var clampedProgress =
|
||||
Math.Max(
|
||||
0f,
|
||||
Math.Min(progress, LengthMillimeters));
|
||||
|
||||
tangentYaw = initialMotionYaw;
|
||||
referencePoint =
|
||||
StartPosition +
|
||||
tangent * clampedProgress;
|
||||
remainingMillimeters =
|
||||
Math.Max(
|
||||
0f,
|
||||
LengthMillimeters - progress);
|
||||
curvaturePerMeter = 0.0;
|
||||
return;
|
||||
}
|
||||
|
||||
if (PathKind == ReferencePathKind.SCurve)
|
||||
{
|
||||
CalculateSCurveReference(
|
||||
currentPosition,
|
||||
initialMotionYaw,
|
||||
out tangentYaw,
|
||||
out referencePoint,
|
||||
out remainingMillimeters,
|
||||
out curvaturePerMeter);
|
||||
return;
|
||||
}
|
||||
|
||||
var center = GetArcCenter();
|
||||
var startRadialYaw =
|
||||
initialMotionYaw - Math.PI / 2.0;
|
||||
var radial = currentPosition - center;
|
||||
var currentRadialYaw =
|
||||
Math.Atan2(radial.Y, radial.X);
|
||||
var progressRadians =
|
||||
AngleMath.NormalizeRadians(
|
||||
currentRadialYaw - startRadialYaw);
|
||||
|
||||
// 测试圆弧只有+90°，起点附近的轻微负噪声按0处理。
|
||||
if (progressRadians < 0.0)
|
||||
progressRadians = 0.0;
|
||||
|
||||
var clampedProgressRadians =
|
||||
Math.Min(
|
||||
progressRadians,
|
||||
ArcSweepRadians);
|
||||
var referenceRadialYaw =
|
||||
startRadialYaw +
|
||||
clampedProgressRadians;
|
||||
referencePoint = center + new Vector2(
|
||||
RadiusMillimeters *
|
||||
(float)Math.Cos(referenceRadialYaw),
|
||||
RadiusMillimeters *
|
||||
(float)Math.Sin(referenceRadialYaw));
|
||||
tangentYaw =
|
||||
referenceRadialYaw + Math.PI / 2.0;
|
||||
remainingMillimeters =
|
||||
(float)Math.Max(
|
||||
0.0,
|
||||
(ArcSweepRadians - progressRadians) *
|
||||
RadiusMillimeters);
|
||||
curvaturePerMeter =
|
||||
1000.0 / RadiusMillimeters;
|
||||
}
|
||||
|
||||
// 通过离散最近点和解析导数计算两段三次贝塞尔S曲线的参考状态。
|
||||
private void CalculateSCurveReference(
|
||||
Vector2 currentPosition,
|
||||
double initialMotionYaw,
|
||||
out double tangentYaw,
|
||||
out Vector2 referencePoint,
|
||||
out float remainingMillimeters,
|
||||
out double curvaturePerMeter)
|
||||
{
|
||||
const int nearestPointSamples = 200;
|
||||
var searchStart =
|
||||
Math.Max(
|
||||
0.0,
|
||||
_lastSCurveProgress - 0.02);
|
||||
var bestProgress = _lastSCurveProgress;
|
||||
var bestDistanceSquared = double.MaxValue;
|
||||
|
||||
for (var i = 0;
|
||||
i <= nearestPointSamples;
|
||||
i++)
|
||||
{
|
||||
var progress =
|
||||
searchStart +
|
||||
(1.0 - searchStart) *
|
||||
i / nearestPointSamples;
|
||||
EvaluateSCurve(
|
||||
progress,
|
||||
out var localPoint,
|
||||
out _,
|
||||
out _);
|
||||
var worldPoint =
|
||||
LocalPathPointToWorld(
|
||||
localPoint,
|
||||
initialMotionYaw);
|
||||
var distanceSquared =
|
||||
Vector2.DistanceSquared(
|
||||
currentPosition,
|
||||
worldPoint);
|
||||
|
||||
if (distanceSquared <
|
||||
bestDistanceSquared)
|
||||
{
|
||||
bestDistanceSquared =
|
||||
distanceSquared;
|
||||
bestProgress = progress;
|
||||
}
|
||||
}
|
||||
|
||||
// 轨迹进度不允许因定位噪声倒退，防止控制目标跳回上一段曲线。
|
||||
_lastSCurveProgress =
|
||||
Math.Max(
|
||||
_lastSCurveProgress,
|
||||
bestProgress);
|
||||
EvaluateSCurve(
|
||||
_lastSCurveProgress,
|
||||
out var bestLocalPoint,
|
||||
out var firstDerivative,
|
||||
out var secondDerivative);
|
||||
referencePoint =
|
||||
LocalPathPointToWorld(
|
||||
bestLocalPoint,
|
||||
initialMotionYaw);
|
||||
tangentYaw =
|
||||
initialMotionYaw +
|
||||
Math.Atan2(
|
||||
firstDerivative.Y,
|
||||
firstDerivative.X);
|
||||
|
||||
var derivativeMagnitude =
|
||||
Math.Sqrt(
|
||||
firstDerivative.X *
|
||||
firstDerivative.X +
|
||||
firstDerivative.Y *
|
||||
firstDerivative.Y);
|
||||
if (derivativeMagnitude < 1e-6)
|
||||
{
|
||||
curvaturePerMeter = 0.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
// 导数单位为mm，乘1000后将曲率从1/mm转换成1/m。
|
||||
curvaturePerMeter =
|
||||
(firstDerivative.X *
|
||||
secondDerivative.Y -
|
||||
firstDerivative.Y *
|
||||
secondDerivative.X) *
|
||||
1000.0 /
|
||||
Math.Pow(
|
||||
derivativeMagnitude,
|
||||
3.0);
|
||||
}
|
||||
|
||||
remainingMillimeters =
|
||||
ApproximateSCurveRemainingLength(
|
||||
_lastSCurveProgress);
|
||||
}
|
||||
|
||||
// 计算与普通4m S型测试完全一致的三段三次贝塞尔完整S曲线。
|
||||
private void EvaluateSCurve(
|
||||
double progress,
|
||||
out Vector2 point,
|
||||
out Vector2 firstDerivative,
|
||||
out Vector2 secondDerivative)
|
||||
{
|
||||
progress =
|
||||
Math.Max(
|
||||
0.0,
|
||||
Math.Min(progress, 1.0));
|
||||
|
||||
Vector2 p0;
|
||||
Vector2 p1;
|
||||
Vector2 p2;
|
||||
Vector2 p3;
|
||||
double t;
|
||||
|
||||
if (progress <= 0.25)
|
||||
{
|
||||
t = progress * 4.0;
|
||||
p0 = new Vector2(0f, 0f);
|
||||
p1 = new Vector2(
|
||||
LengthMillimeters / 12f,
|
||||
0f);
|
||||
p2 = new Vector2(
|
||||
LengthMillimeters / 6f,
|
||||
SCurveLateralOffsetMillimeters);
|
||||
p3 = new Vector2(
|
||||
LengthMillimeters * 0.25f,
|
||||
SCurveLateralOffsetMillimeters);
|
||||
}
|
||||
else if (progress <= 0.75)
|
||||
{
|
||||
t = (progress - 0.25) * 2.0;
|
||||
p0 = new Vector2(
|
||||
LengthMillimeters * 0.25f,
|
||||
SCurveLateralOffsetMillimeters);
|
||||
p1 = new Vector2(
|
||||
LengthMillimeters / 3f,
|
||||
SCurveLateralOffsetMillimeters);
|
||||
p2 = new Vector2(
|
||||
LengthMillimeters * 2f / 3f,
|
||||
-SCurveLateralOffsetMillimeters);
|
||||
p3 = new Vector2(
|
||||
LengthMillimeters * 0.75f,
|
||||
-SCurveLateralOffsetMillimeters);
|
||||
}
|
||||
else
|
||||
{
|
||||
t = (progress - 0.75) * 4.0;
|
||||
p0 = new Vector2(
|
||||
LengthMillimeters * 0.75f,
|
||||
-SCurveLateralOffsetMillimeters);
|
||||
p1 = new Vector2(
|
||||
LengthMillimeters * 5f / 6f,
|
||||
-SCurveLateralOffsetMillimeters);
|
||||
p2 = new Vector2(
|
||||
LengthMillimeters * 11f / 12f,
|
||||
0f);
|
||||
p3 = new Vector2(
|
||||
LengthMillimeters,
|
||||
0f);
|
||||
}
|
||||
|
||||
var oneMinusT = 1.0 - t;
|
||||
point =
|
||||
p0 * (float)(
|
||||
oneMinusT *
|
||||
oneMinusT *
|
||||
oneMinusT) +
|
||||
p1 * (float)(
|
||||
3.0 *
|
||||
oneMinusT *
|
||||
oneMinusT *
|
||||
t) +
|
||||
p2 * (float)(
|
||||
3.0 *
|
||||
oneMinusT *
|
||||
t *
|
||||
t) +
|
||||
p3 * (float)(t * t * t);
|
||||
firstDerivative =
|
||||
(p1 - p0) *
|
||||
(float)(
|
||||
3.0 *
|
||||
oneMinusT *
|
||||
oneMinusT) +
|
||||
(p2 - p1) *
|
||||
(float)(
|
||||
6.0 *
|
||||
oneMinusT *
|
||||
t) +
|
||||
(p3 - p2) *
|
||||
(float)(3.0 * t * t);
|
||||
secondDerivative =
|
||||
(p2 - 2f * p1 + p0) *
|
||||
(float)(6.0 * oneMinusT) +
|
||||
(p3 - 2f * p2 + p1) *
|
||||
(float)(6.0 * t);
|
||||
}
|
||||
|
||||
// 通过分段采样估算从当前S曲线进度到终点的实际弧长。
|
||||
private float ApproximateSCurveRemainingLength(
|
||||
double startProgress)
|
||||
{
|
||||
const int lengthSamples = 100;
|
||||
EvaluateSCurve(
|
||||
startProgress,
|
||||
out var previousPoint,
|
||||
out _,
|
||||
out _);
|
||||
var length = 0f;
|
||||
|
||||
for (var i = 1;
|
||||
i <= lengthSamples;
|
||||
i++)
|
||||
{
|
||||
var progress =
|
||||
startProgress +
|
||||
(1.0 - startProgress) *
|
||||
i / lengthSamples;
|
||||
EvaluateSCurve(
|
||||
progress,
|
||||
out var point,
|
||||
out _,
|
||||
out _);
|
||||
length +=
|
||||
Vector2.Distance(
|
||||
previousPoint,
|
||||
point);
|
||||
previousPoint = point;
|
||||
}
|
||||
|
||||
return length;
|
||||
}
|
||||
|
||||
// 将以初始蟹行方向为X轴的局部路径点转换到Detour世界坐标。
|
||||
private Vector2 LocalPathPointToWorld(
|
||||
Vector2 localPoint,
|
||||
double initialMotionYaw)
|
||||
{
|
||||
var cos =
|
||||
(float)Math.Cos(initialMotionYaw);
|
||||
var sin =
|
||||
(float)Math.Sin(initialMotionYaw);
|
||||
|
||||
return StartPosition + new Vector2(
|
||||
localPoint.X * cos -
|
||||
localPoint.Y * sin,
|
||||
localPoint.X * sin +
|
||||
localPoint.Y * cos);
|
||||
}
|
||||
|
||||
// 获取蟹行左转圆弧圆心；它位于初始运动方向的左侧。
|
||||
public Vector2 GetArcCenter()
|
||||
{
|
||||
var initialMotionYaw =
|
||||
InitialBodyYawRadians +
|
||||
MotionFrameYawInBodyRadians;
|
||||
return StartPosition + new Vector2(
|
||||
-RadiusMillimeters *
|
||||
(float)Math.Sin(initialMotionYaw),
|
||||
RadiusMillimeters *
|
||||
(float)Math.Cos(initialMotionYaw));
|
||||
}
|
||||
|
||||
// 获取圆弧测试的理论终点。
|
||||
public Vector2 GetArcDestination()
|
||||
{
|
||||
var initialMotionYaw =
|
||||
InitialBodyYawRadians +
|
||||
MotionFrameYawInBodyRadians;
|
||||
var startRadialYaw =
|
||||
initialMotionYaw - Math.PI / 2.0;
|
||||
var endRadialYaw =
|
||||
startRadialYaw + ArcSweepRadians;
|
||||
var center = GetArcCenter();
|
||||
|
||||
return center + new Vector2(
|
||||
RadiusMillimeters *
|
||||
(float)Math.Cos(endRadialYaw),
|
||||
RadiusMillimeters *
|
||||
(float)Math.Sin(endRadialYaw));
|
||||
}
|
||||
|
||||
// 根据剩余路径长度生成终点减速速度。
|
||||
private float CalculateSpeed(
|
||||
float remainingMillimeters)
|
||||
{
|
||||
if (remainingMillimeters >=
|
||||
SlowDistanceMillimeters)
|
||||
return CruiseSpeed;
|
||||
|
||||
var ratio =
|
||||
remainingMillimeters /
|
||||
Math.Max(
|
||||
SlowDistanceMillimeters,
|
||||
1f);
|
||||
return Math.Max(
|
||||
MinimumSpeed,
|
||||
CruiseSpeed * ratio);
|
||||
}
|
||||
|
||||
private void ValidateParameters()
|
||||
{
|
||||
if (CruiseSpeed <= 0f ||
|
||||
!IsFinite(CruiseSpeed) ||
|
||||
LengthMillimeters <= 0f ||
|
||||
!IsFinite(LengthMillimeters) ||
|
||||
RadiusMillimeters <= 0f ||
|
||||
!IsFinite(RadiusMillimeters) ||
|
||||
SCurveLateralOffsetMillimeters <= 0f ||
|
||||
!IsFinite(
|
||||
SCurveLateralOffsetMillimeters) ||
|
||||
ArcSweepRadians <= 0.0 ||
|
||||
!IsFinite(ArcSweepRadians) ||
|
||||
SlowDistanceMillimeters <= 0f ||
|
||||
!IsFinite(SlowDistanceMillimeters) ||
|
||||
FinishDistanceMillimeters < 0f ||
|
||||
!IsFinite(FinishDistanceMillimeters) ||
|
||||
TrackingTimeoutSeconds <= 0f ||
|
||||
!IsFinite(TrackingTimeoutSeconds) ||
|
||||
MaximumVirtualSteeringRadians <= 0.0 ||
|
||||
MaximumVirtualSteeringRadians >=
|
||||
Math.PI / 2.0 ||
|
||||
!IsFinite(
|
||||
MaximumVirtualSteeringRadians))
|
||||
throw new ArgumentOutOfRangeException(
|
||||
"蟹行轨迹测试参数无效。");
|
||||
}
|
||||
|
||||
private static double Limit(
|
||||
double value,
|
||||
double absoluteLimit)
|
||||
{
|
||||
return Math.Max(
|
||||
-absoluteLimit,
|
||||
Math.Min(value, absoluteLimit));
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return
|
||||
!double.IsNaN(value) &&
|
||||
!double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,123 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using ClumsyCore.Pilot;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public class Sleep : MovementDefinition
|
||||
{
|
||||
public float Second = 2f;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
if (Second <= 0)
|
||||
{
|
||||
yield return false;
|
||||
yield break;
|
||||
}
|
||||
|
||||
var endTime = DateTime.UtcNow.AddSeconds(Second);
|
||||
while (DateTime.UtcNow < endTime)
|
||||
{
|
||||
Thread.Sleep(50);
|
||||
yield return true;
|
||||
}
|
||||
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
|
||||
public class DriverAble : MovementDefinition
|
||||
{
|
||||
public int WaitTimeoutMs = 2000;
|
||||
public int PollIntervalMs = 50;
|
||||
|
||||
// C层单车硬件：请求全部驱动轮复位并恢复使能。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
PilotDefinition.Self.ResetFromC = true;
|
||||
|
||||
try
|
||||
{
|
||||
var start = DateTime.Now;
|
||||
var timeoutMs = Math.Max(0, WaitTimeoutMs);
|
||||
var pollMs = Math.Max(1, PollIntervalMs);
|
||||
|
||||
// 至少保留一个调度周期，确保M层能收到复位请求。
|
||||
yield return true;
|
||||
|
||||
while (!PilotDefinition.Self.WheelAbleState &&
|
||||
(DateTime.Now - start).TotalMilliseconds < timeoutMs)
|
||||
{
|
||||
Thread.Sleep(pollMs);
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
PilotDefinition.Self.ResetFromC = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
public class DriverDisable : MovementDefinition
|
||||
{
|
||||
public int WaitTimeoutMs = 3000;
|
||||
public int PollIntervalMs = 20;
|
||||
|
||||
// C层单车硬件：请求驱动轮退出使能，并等待M层状态反馈。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var timeoutMs = Math.Max(0, WaitTimeoutMs);
|
||||
var pollMs = Math.Max(1, PollIntervalMs);
|
||||
var startTime = DateTime.UtcNow;
|
||||
var success = false;
|
||||
|
||||
PilotDefinition.Self.DisableFromC = true;
|
||||
|
||||
try
|
||||
{
|
||||
// 至少保持一个C层调度周期，确保M层能收到下使能请求。
|
||||
yield return true;
|
||||
|
||||
success = !PilotDefinition.Self.WheelAbleState;
|
||||
|
||||
while (!success &&
|
||||
(DateTime.UtcNow - startTime).TotalMilliseconds <
|
||||
timeoutMs)
|
||||
{
|
||||
Thread.Sleep(pollMs);
|
||||
|
||||
success =
|
||||
!PilotDefinition.Self.WheelAbleState;
|
||||
|
||||
if (!success)
|
||||
{
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
// 无论正常完成、超时、异常还是任务被停止，都撤销请求。
|
||||
PilotDefinition.Self.DisableFromC = false;
|
||||
}
|
||||
if (success)
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"驱动器下使能完成，" +
|
||||
$"WheelAbleState=" +
|
||||
$"{PilotDefinition.Self.WheelAbleState}");
|
||||
}
|
||||
else
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"驱动器下使能超时，" +
|
||||
$"WheelAbleState=" +
|
||||
$"{PilotDefinition.Self.WheelAbleState}，" +
|
||||
$"等待{timeoutMs}ms");
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,55 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
//在世界坐标系下，从路径起点追踪到终点并停车
|
||||
public class DstTracker : MovementDefinition
|
||||
{
|
||||
public Vector2 Src;
|
||||
public Vector2 Dst;
|
||||
// 本次轨迹的巡航速度上限，单位m/s。
|
||||
public float MaxSpeed = PilotDefinition.Conf.DstTrackerMaxSpeed;
|
||||
public float CarDirectionBias = 0f;
|
||||
public Painter Painter = UI.GetPainter("DstTracker");
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
DriveTask task = null;
|
||||
try
|
||||
{
|
||||
Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
|
||||
Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
|
||||
|
||||
var tracker = new ChassisController
|
||||
{
|
||||
BaseSpeed = MaxSpeed
|
||||
}.Get();
|
||||
// 要求路径末端速度下降到零。
|
||||
tracker.FinishSpeed = 0f;
|
||||
var linePath = new LineTrack(Src, Dst)
|
||||
{
|
||||
CarDirectionBias = CarDirectionBias,
|
||||
Speed = MaxSpeed
|
||||
};
|
||||
tracker.AddTrack(linePath);
|
||||
task = new DriveTask(tracker.Track());
|
||||
task.Wait();
|
||||
yield return false;
|
||||
}
|
||||
finally
|
||||
{
|
||||
task?.Stop();
|
||||
chassis.PredefinedDriveStop();
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,178 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
// C层单车底盘：按照车轮里程行驶指定的相对距离。
|
||||
public class LineTracking : MovementDefinition
|
||||
{
|
||||
// 相对动作启动位置的行驶距离，单位mm。
|
||||
// 正数表示前进，负数表示后退。
|
||||
public float TargetDistance;
|
||||
public float MaxSpeed = PilotDefinition.Conf.LineTrackMaxSpeed;
|
||||
public float Kp = PilotDefinition.Conf.LineTrackKp;
|
||||
public float Ki = PilotDefinition.Conf.LineTrackKi;
|
||||
public float Kd = PilotDefinition.Conf.LineTrackKd;
|
||||
public float DeadZone = PilotDefinition.Conf.LineTrackDeadZone;
|
||||
public int SrcId = -1;
|
||||
public int DstId = -1;
|
||||
public Action<int> LeaveSrcFunction;
|
||||
// 接近目标后是否保留速度，交给下一个动作接管。
|
||||
public bool EnableHandover;
|
||||
// 进入动作衔接的剩余距离，单位mm。
|
||||
public float HandoverDistance = 80f;
|
||||
// HandoverSpeed小于0时，使用MaxSpeed的此比例。
|
||||
public float HandoverSpeedRatio = 0.5f;
|
||||
// 大于等于0时，直接作为衔接速度，单位m/s。
|
||||
public float HandoverSpeed = -1f;
|
||||
public float MinHandoverSpeed = 0.05f;
|
||||
private PIDController _pid;
|
||||
// 读取当前单车直线行驶里程，单位mm。
|
||||
private static float ReadPosition()
|
||||
{
|
||||
return
|
||||
(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2f;
|
||||
}
|
||||
|
||||
// 根据动作启动位置和目标距离执行直线里程闭环。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
if (float.IsNaN(TargetDistance) || float.IsInfinity(TargetDistance))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(TargetDistance),
|
||||
"目标行驶距离必须是有限值。");
|
||||
}
|
||||
|
||||
if (float.IsNaN(MaxSpeed) || float.IsInfinity(MaxSpeed) || MaxSpeed <= 0f)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(MaxSpeed),
|
||||
"最大速度必须是大于零的有限值。");
|
||||
}
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
// 每次启动动作时重新读取起始编码器位置。
|
||||
var startPosition = ReadPosition();
|
||||
// PID仍然控制绝对编码器位置，但绝对目标由动作自动计算。
|
||||
var targetPosition = startPosition + TargetDistance;
|
||||
_pid = new PIDController(ReadPosition, Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
|
||||
{
|
||||
SpeedAccPerSec = Math.Abs(MaxSpeed) / 2f
|
||||
};
|
||||
var handoverRequested = false;
|
||||
var keepHandoverSpeed = false;
|
||||
DLog.Log(
|
||||
$"直线里程动作：" +
|
||||
$"起点={startPosition:F1}mm，" +
|
||||
$"距离={TargetDistance:F1}mm，" +
|
||||
$"目标={targetPosition:F1}mm",
|
||||
"straight_line");
|
||||
try
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
var currentPosition = ReadPosition();
|
||||
var remainingDistance = targetPosition - currentPosition;
|
||||
// 接近目标后，保留一定速度交给后续动作。
|
||||
if (EnableHandover && Math.Abs(remainingDistance) <= Math.Max(1f, HandoverDistance))
|
||||
{
|
||||
var direction = Math.Sign(remainingDistance);
|
||||
if (direction == 0)
|
||||
{
|
||||
direction = Math.Sign(TargetDistance);
|
||||
}
|
||||
var requestedSpeed = HandoverSpeed >= 0f ? Math.Abs(HandoverSpeed) : Math.Abs(MaxSpeed) * HandoverSpeedRatio;
|
||||
var maximumSpeed = Math.Abs(MaxSpeed);
|
||||
var minimumSpeed = Math.Min(Math.Abs(MinHandoverSpeed), maximumSpeed);
|
||||
var limitedSpeed = Math.Max(minimumSpeed, Math.Min(requestedSpeed, maximumSpeed));
|
||||
var handoverSpeed = limitedSpeed * direction;
|
||||
chassis.SendXYThSpeed(handoverSpeed, 0f, 0f);
|
||||
handoverRequested = true;
|
||||
// 保持一个调度周期，让速度命令实际生效。
|
||||
yield return true;
|
||||
break;
|
||||
}
|
||||
var speed = _pid.GetResponse(targetPosition);
|
||||
chassis.SendXYThSpeed(speed, 0f, 0f);
|
||||
if (_pid.IsArrived())
|
||||
{
|
||||
break;
|
||||
}
|
||||
yield return true;
|
||||
}
|
||||
if (SrcId != -1 &&
|
||||
LeaveSrcFunction != null)
|
||||
{
|
||||
LeaveSrcFunction(SrcId);
|
||||
DLog.Log($"释放放车点{SrcId}", "straight_line");
|
||||
}
|
||||
// 只有正常完成动作衔接时才允许保留非零速度。
|
||||
keepHandoverSpeed = handoverRequested;
|
||||
}
|
||||
finally
|
||||
{
|
||||
// 普通完成、人工停止或异常退出时都必须停车。
|
||||
if (!keepHandoverSpeed)
|
||||
{
|
||||
chassis.SendXYThSpeed(0f, 0f, 0f);
|
||||
}
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//直线行走基于detour
|
||||
public class LineTracking_based_detour : MovementDefinition
|
||||
{
|
||||
public float LineDistance = 1000f;
|
||||
public int SrcId = -1;
|
||||
public int DstId = -1;
|
||||
public Action<int> LeaveSrcFunction = null;
|
||||
public Painter painter = UI.GetPainter("Line", false);
|
||||
// C层单车轨迹：执行早期版本的两点直线跟踪动作。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var curpose = DetourInterface.getCartLocation();
|
||||
Console.WriteLine($"curpose.th:{curpose.th}");
|
||||
var src = new Vector2((float)curpose.x, (float)curpose.y);
|
||||
var headingRadians =
|
||||
AngleMath.DegreesToRadians(curpose.th);
|
||||
var dst = new Vector2(
|
||||
(float)(curpose.x +
|
||||
LineDistance * Math.Cos(headingRadians)),
|
||||
(float)(curpose.y +
|
||||
LineDistance * Math.Sin(headingRadians)));
|
||||
// var dst = new Vector2((float)curpose.x + LineDistance * (float)Math.Cos(curpose.th),
|
||||
// (float)curpose.y + LineDistance * (float)Math.Sin(curpose.th));
|
||||
Console.WriteLine($"src:{src.X} {src.Y}");
|
||||
Console.WriteLine($"dst:{dst.X} {dst.Y}");
|
||||
painter.DrawLine(Color.Green, src.X, src.Y, dst.X, dst.Y, width: 3);
|
||||
|
||||
var tracker = new ChassisController().Get();
|
||||
var linePath = new LineTrack(src, dst) { CarDirectionBias = LineDistance > 0 ? 0 : 180 };
|
||||
tracker.AddTrack(linePath);
|
||||
var _dt = new DriveTask(tracker.Track());
|
||||
_dt.Wait();
|
||||
if (SrcId != -1 && LeaveSrcFunction != null)
|
||||
{
|
||||
LeaveSrcFunction(SrcId);
|
||||
DLog.Log($"释放放车点{SrcId}", "straight_line");
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,92 @@
|
||||
|
||||
|
||||
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
// C层测试准备：停车并等待四个舵轮稳定回到车体前向0°。
|
||||
public class PrepareWheelsForward : MovementDefinition
|
||||
{
|
||||
public float ToleranceDegrees = 2f;
|
||||
public float StableSeconds = 0.3f;
|
||||
public float TimeoutSeconds = 10f;
|
||||
public bool Completed { get; private set; }
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var chassis =
|
||||
PilotDefinition.Chassis as MultiWheelChassis;
|
||||
if (chassis == null)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"当前底盘不是MultiWheelChassis，无法执行舵轮回正。");
|
||||
}
|
||||
|
||||
var adapter = new MultiWheelChassisAdapter(
|
||||
chassis,
|
||||
PilotDefinition.Self.CarNum);
|
||||
adapter.ResetToBodyFrame();
|
||||
var toleranceRadians =
|
||||
AngleMath.DegreesToRadians(ToleranceDegrees);
|
||||
var startTime = DateTime.UtcNow;
|
||||
DateTime? alignedSince = null;
|
||||
|
||||
Completed = false;
|
||||
if (!adapter.PrepareParallelDirection(0.0))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"无法将所有舵轮下发到车体前向0°。");
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
var aligned =
|
||||
adapter.AreParallelWheelsAligned(
|
||||
0.0,
|
||||
toleranceRadians);
|
||||
|
||||
if (aligned)
|
||||
{
|
||||
if (!alignedSince.HasValue)
|
||||
alignedSince = DateTime.UtcNow;
|
||||
|
||||
if ((DateTime.UtcNow -
|
||||
alignedSince.Value).TotalSeconds >=
|
||||
StableSeconds)
|
||||
{
|
||||
Completed = true;
|
||||
yield break;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
alignedSince = null;
|
||||
}
|
||||
|
||||
if (TimeoutSeconds > 0f &&
|
||||
(DateTime.UtcNow - startTime).TotalSeconds >
|
||||
TimeoutSeconds)
|
||||
{
|
||||
throw new TimeoutException(
|
||||
$"舵轮回正超过{TimeoutSeconds:F1}s，" +
|
||||
"测试已经取消。");
|
||||
}
|
||||
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
// 只清零驱动速度，保留已经下发的0°舵角。
|
||||
adapter.StopImmediately();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,134 @@
|
||||
|
||||
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public class MultiWheelRotateInPlace : MovementDefinition
|
||||
{
|
||||
/// <summary>
|
||||
/// 旋转目标角度
|
||||
/// </summary>
|
||||
public float AngleTarget;
|
||||
|
||||
public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
|
||||
|
||||
public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
|
||||
public Func<PIDParams> PidparamsRead = () => new PIDParams() { };
|
||||
|
||||
public PIDController thPid;
|
||||
|
||||
// 将本周期PID角速度输出提供给实验记录器，单位deg/s。
|
||||
public Action<float> CommandAngularSpeedObserver;
|
||||
|
||||
// 自转前舵轮实际角度允许误差，单位deg。
|
||||
public float WheelAlignmentToleranceDegrees = 2f;
|
||||
|
||||
// 自转舵轮连续保持到位的时间，单位s。
|
||||
public float WheelAlignmentStableSeconds = 0.3f;
|
||||
|
||||
// 自转舵轮准备超时时间，单位s。
|
||||
public float WheelAlignmentTimeoutSeconds = 10f;
|
||||
|
||||
// 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
if (Chassis == null)
|
||||
throw new InvalidOperationException(
|
||||
"当前底盘不是MultiWheelChassis，无法执行原地自转。");
|
||||
|
||||
var adapter = new MultiWheelChassisAdapter(
|
||||
Chassis,
|
||||
PilotDefinition.Self.CarNum);
|
||||
adapter.ResetToBodyFrame();
|
||||
|
||||
try
|
||||
{
|
||||
var alignmentStarted = DateTime.Now;
|
||||
DateTime? alignedSince = null;
|
||||
while (true)
|
||||
{
|
||||
if (!adapter.PrepareSpin())
|
||||
throw new InvalidOperationException(
|
||||
"无法生成原地自转舵轮目标：" +
|
||||
adapter.LastFailureReason);
|
||||
|
||||
if (adapter.AreSpinWheelsAligned)
|
||||
{
|
||||
if (alignedSince == null)
|
||||
alignedSince = DateTime.Now;
|
||||
|
||||
if ((DateTime.Now - alignedSince.Value)
|
||||
.TotalSeconds >=
|
||||
WheelAlignmentStableSeconds)
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
alignedSince = null;
|
||||
}
|
||||
|
||||
if ((DateTime.Now - alignmentStarted)
|
||||
.TotalSeconds >
|
||||
WheelAlignmentTimeoutSeconds)
|
||||
throw new TimeoutException(
|
||||
"原地自转舵轮在限定时间内未稳定到位。");
|
||||
|
||||
yield return true;
|
||||
}
|
||||
|
||||
var targetAngle =
|
||||
(float)AngleMath.NormalizeDegrees(AngleTarget);
|
||||
var p = PidparamsRead();
|
||||
thPid = new PIDController(ThetaReader, p.Kp);
|
||||
thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
|
||||
p.OutputUpperThreshold, p.SpeedAccPerSec);
|
||||
var lastCommandTime = DateTime.Now;
|
||||
|
||||
while (true)
|
||||
{
|
||||
var s = thPid.GetResponse(targetAngle, true);
|
||||
Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
|
||||
CommandAngularSpeedObserver?.Invoke(s);
|
||||
var now = DateTime.Now;
|
||||
var interval = now - lastCommandTime;
|
||||
lastCommandTime = now;
|
||||
|
||||
// PID输出s为deg/s，Shared命令统一使用rad/s。
|
||||
// adapter.Send最终调用普通安全版SendXYThSpeed。
|
||||
var omegaRadiansPerSecond =
|
||||
(float)AngleMath.DegreesToRadians(s);
|
||||
if (!adapter.Send(
|
||||
new ChassisCommand(
|
||||
PilotDefinition.Self.CarNum,
|
||||
new Twist2D(
|
||||
0.0,
|
||||
0.0,
|
||||
omegaRadiansPerSecond)),
|
||||
interval))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"安全XYTh原地旋转底盘解算失败：" +
|
||||
adapter.LastFailureReason);
|
||||
}
|
||||
if (thPid.IsArrived()) break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
Console.WriteLine($"final rotate to {targetAngle}");
|
||||
}
|
||||
finally
|
||||
{
|
||||
CommandAngularSpeedObserver?.Invoke(0f);
|
||||
adapter.StopImmediately();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user