集中停车控制配置并完善终点逼近和周期诊断
This commit is contained in:
@@ -0,0 +1,21 @@
|
||||
// using ClumsyCore;
|
||||
// using MDCSToolBox.Clumsy.AgvInterfaces;
|
||||
// using MDCSToolBox.Clumsy.MotionControllers;
|
||||
|
||||
// namespace MultiWheelC
|
||||
// {
|
||||
// public class AGV : MultiWheelInterface
|
||||
// {
|
||||
// public override AbstractGeometricController GetController()
|
||||
// => new ChassisController().Get();
|
||||
// public override MultiWheelMagTracker GetMagController()
|
||||
// => new MultiWheelMagTracker();
|
||||
// public override NaiveMagnetController GetNaiveMagnetController()
|
||||
// => new NaiveMagnetController();
|
||||
|
||||
// public void Sleep(float seconds)
|
||||
// {
|
||||
// new DriveTask(new Sleep { Second = seconds }.Get()).Wait();
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
@@ -0,0 +1,45 @@
|
||||
// using ClumsyCore;
|
||||
// using ClumsyCore.Pilot;
|
||||
// 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;
|
||||
|
||||
// // 创建单车几何跟踪控制器（直接控本车底盘，不走多车 Auto 通道）
|
||||
// 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,
|
||||
// };
|
||||
// }
|
||||
// }
|
||||
@@ -0,0 +1,90 @@
|
||||
|
||||
|
||||
using System;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public abstract class ClampMovementTestBase : MovementTest
|
||||
{
|
||||
public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
|
||||
|
||||
private DriveTask _task;
|
||||
protected abstract bool Close { get; }
|
||||
|
||||
// 根据派生测试类型驱动左右夹臂同步夹紧或打开。
|
||||
public override void Test()
|
||||
{
|
||||
var leftTarget = Close
|
||||
? PilotDefinition.Self.LeftArmUpperPos
|
||||
: PilotDefinition.Self.LeftArmLowerPos;
|
||||
var rightTarget = Close
|
||||
? PilotDefinition.Self.RightArmUpperPos
|
||||
: PilotDefinition.Self.RightArmLowerPos;
|
||||
|
||||
if (float.IsNaN(leftTarget) ||
|
||||
float.IsInfinity(leftTarget) ||
|
||||
float.IsNaN(rightTarget) ||
|
||||
float.IsInfinity(rightTarget))
|
||||
{
|
||||
Console.WriteLine(
|
||||
"夹臂目标位置无效，取消夹臂运动测试。");
|
||||
return;
|
||||
}
|
||||
|
||||
// 防止重复点击时上一项夹臂任务仍在运行。
|
||||
TestStop();
|
||||
Console.WriteLine(
|
||||
$"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
|
||||
$"左目标={leftTarget}，右目标={rightTarget}");
|
||||
|
||||
var task = new DriveTask(
|
||||
new ClampToTarget
|
||||
{
|
||||
LeftClampTarget = leftTarget,
|
||||
RightClampTarget = rightTarget,
|
||||
TimeoutSeconds = TimeoutSeconds
|
||||
}.Get());
|
||||
_task = task;
|
||||
|
||||
try
|
||||
{
|
||||
task.Wait();
|
||||
}
|
||||
finally
|
||||
{
|
||||
PilotDefinition.Self.SpeedLeftArm = 0f;
|
||||
PilotDefinition.Self.SpeedRightArm = 0f;
|
||||
if (ReferenceEquals(_task, task))
|
||||
_task = null;
|
||||
}
|
||||
}
|
||||
|
||||
// 停止夹臂任务并立即清零左右夹臂下发速度。
|
||||
public override void TestStop()
|
||||
{
|
||||
_task?.Stop();
|
||||
_task = null;
|
||||
PilotDefinition.Self.SpeedLeftArm = 0f;
|
||||
PilotDefinition.Self.SpeedRightArm = 0f;
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "夹臂关闭测试")]
|
||||
public sealed class TestClampCloseMovement
|
||||
: ClampMovementTestBase
|
||||
{
|
||||
protected override bool Close => false;
|
||||
}
|
||||
|
||||
[MovementTest(name = "夹臂启动测试")]
|
||||
public sealed class TestClampOpenMovement
|
||||
: ClampMovementTestBase
|
||||
{
|
||||
protected override bool Close => true;
|
||||
}
|
||||
}
|
||||
@@ -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,635 @@
|
||||
|
||||
|
||||
// using System;
|
||||
// using System.Collections.Generic;
|
||||
// using System.Numerics;
|
||||
// using System.Threading;
|
||||
// using ClumsyCore;
|
||||
// using ClumsyCore.Interfaces;
|
||||
// using ClumsyCore.Pilot;
|
||||
// using FundamentalLib;
|
||||
// using MDCSToolBox.Clumsy.Movements;
|
||||
// using MDCSToolBox.Clumsy.Pilot;
|
||||
// using MDCSToolBox.Clumsy.Tracks;
|
||||
// using MyParking.Shared;
|
||||
|
||||
// namespace MultiWheelC
|
||||
// {
|
||||
// [MovementTest(name = "SendMotion：连续前进4m")]
|
||||
// public class TestForward4m : MovementTest
|
||||
// {
|
||||
// public float DistanceMillimeters = 4000f; // 测试距离，单位mm。
|
||||
// public float CruiseSpeed = 0.3f; // 巡航速度上限，单位m/s。
|
||||
// public int TrialNumber = 1; // 重复实验编号。
|
||||
// private DriveTask _task;
|
||||
// private TrackingExperimentRecorder _recorder;
|
||||
// // 从当前Detour位置沿车头方向生成4m连续直线并记录测试数据。
|
||||
// public override void Test()
|
||||
// {
|
||||
// if (!MovementTestPreparation.AreWheelsForward())
|
||||
// {
|
||||
// return;
|
||||
// }
|
||||
|
||||
// var location = DetourInterface.getCartLocation();
|
||||
// if (double.IsNaN(location.x) ||
|
||||
// double.IsInfinity(location.x) ||
|
||||
// double.IsNaN(location.y) ||
|
||||
// double.IsInfinity(location.y) ||
|
||||
// double.IsNaN(location.th) ||
|
||||
// double.IsInfinity(location.th))
|
||||
// {
|
||||
// Console.WriteLine(
|
||||
// "Detour当前位姿无效，取消连续前进4m测试。");
|
||||
// return;
|
||||
// }
|
||||
// var source = new Vector2((float)location.x, (float)location.y);
|
||||
// // Detour航向单位是度，三角函数需要弧度。
|
||||
// var headingRadians =
|
||||
// AngleMath.DegreesToRadians(location.th);
|
||||
// var destination = new Vector2(
|
||||
// source.X + DistanceMillimeters * (float)Math.Cos(headingRadians),
|
||||
// source.Y + DistanceMillimeters * (float)Math.Sin(headingRadians));
|
||||
// _recorder =
|
||||
// new TrackingExperimentRecorder(
|
||||
// controllerName: "LegacyGeometricController",
|
||||
// trajectoryName: "LegacyStraight4m",
|
||||
// trialNumber: TrialNumber,
|
||||
// referenceStart: source,
|
||||
// referenceEnd: destination,
|
||||
// referenceSpeed: CruiseSpeed);
|
||||
// _recorder.Start();
|
||||
// try
|
||||
// {
|
||||
// _task = new DriveTask(
|
||||
// new DstTracker
|
||||
// {
|
||||
// Src = source,
|
||||
// Dst = destination,
|
||||
// CarDirectionBias = 0f,
|
||||
// MaxSpeed = CruiseSpeed
|
||||
// }.Get());
|
||||
// _task.Wait();
|
||||
// // 保留少量停车后数据，便于观察速度是否回到零。
|
||||
// Thread.Sleep(300);
|
||||
// }
|
||||
// finally
|
||||
// {
|
||||
// _task?.Stop();
|
||||
// _recorder?.UpdateCommand(0f, 0f);
|
||||
// _recorder?.StopAndSave();
|
||||
// _task = null;
|
||||
// _recorder = null;
|
||||
// }
|
||||
// }
|
||||
|
||||
// public override void TestStop()
|
||||
// {
|
||||
// _task?.Stop();
|
||||
// _recorder?.UpdateCommand(0f, 0f);
|
||||
// _recorder?.StopAndSave();
|
||||
// }
|
||||
// }
|
||||
|
||||
// [MovementTest(name = "SendMotion：左转90°半径2m圆弧")]
|
||||
// public class TestArcMovement : MovementTest
|
||||
// {
|
||||
// public float RadiusMillimeters = 2000f; // 左转圆的半径，单位mm。
|
||||
// public float CruiseSpeed = 0.3f; // 圆周运动速度上限，单位m/s。
|
||||
// public int TrialNumber = 1; // 重复实验编号。
|
||||
|
||||
// private DriveTask _task;
|
||||
// private TrackingExperimentRecorder _recorder;
|
||||
|
||||
// // 从当前位姿开始，沿半径2m的圆弧向左转弯90°。
|
||||
// public override void Test()
|
||||
// {
|
||||
// if (float.IsNaN(RadiusMillimeters) ||
|
||||
// float.IsInfinity(RadiusMillimeters) ||
|
||||
// RadiusMillimeters <= 0f ||
|
||||
// float.IsNaN(CruiseSpeed) ||
|
||||
// float.IsInfinity(CruiseSpeed) ||
|
||||
// CruiseSpeed <= 0f)
|
||||
// {
|
||||
// Console.WriteLine("圆弧运动测试参数无效。");
|
||||
// return;
|
||||
// }
|
||||
|
||||
// if (!MovementTestPreparation.AreWheelsForward())
|
||||
// {
|
||||
// return;
|
||||
// }
|
||||
|
||||
// var location = DetourInterface.getCartLocation();
|
||||
// if (double.IsNaN(location.x) ||
|
||||
// double.IsInfinity(location.x) ||
|
||||
// double.IsNaN(location.y) ||
|
||||
// double.IsInfinity(location.y) ||
|
||||
// double.IsNaN(location.th) ||
|
||||
// double.IsInfinity(location.th))
|
||||
// {
|
||||
// Console.WriteLine(
|
||||
// "Detour当前位姿无效，取消圆弧运动测试。");
|
||||
// return;
|
||||
// }
|
||||
|
||||
// var source =
|
||||
// new Vector2((float)location.x, (float)location.y);
|
||||
// var headingRadians =
|
||||
// AngleMath.DegreesToRadians(location.th);
|
||||
|
||||
// // 根据世界航向求车体左法向，左转圆心位于车辆左侧。
|
||||
// var center = new Vector2(
|
||||
// source.X -
|
||||
// RadiusMillimeters *
|
||||
// (float)Math.Sin(headingRadians),
|
||||
// source.Y +
|
||||
// RadiusMillimeters *
|
||||
// (float)Math.Cos(headingRadians));
|
||||
|
||||
// // 从圆心指向车辆起点的极角，比车辆切线航向小90°。
|
||||
// var startRadialAngleDegrees =
|
||||
// (float)location.th - 90f;
|
||||
|
||||
// var controller = new ChassisController
|
||||
// {
|
||||
// BaseSpeed = CruiseSpeed
|
||||
// }.Get();
|
||||
// controller.FinishSpeed = 0f;
|
||||
|
||||
// var arc = new CircularArcTrack(
|
||||
// center,
|
||||
// RadiusMillimeters,
|
||||
// startRadialAngleDegrees,
|
||||
// startRadialAngleDegrees + 90f,
|
||||
// direction: 1)
|
||||
// {
|
||||
// Speed = CruiseSpeed,
|
||||
// CarDirectionBias = 0f
|
||||
// };
|
||||
|
||||
// // 左转90°后，圆心到终点的径向方向等于起始车头方向。
|
||||
// var destination = center + new Vector2(
|
||||
// RadiusMillimeters *
|
||||
// (float)Math.Cos(headingRadians),
|
||||
// RadiusMillimeters *
|
||||
// (float)Math.Sin(headingRadians));
|
||||
|
||||
// if (!controller.AddTrack(arc, "LeftArc90Degrees"))
|
||||
// {
|
||||
// Console.WriteLine(
|
||||
// "左转90°圆弧轨迹添加失败，取消测试。");
|
||||
// return;
|
||||
// }
|
||||
|
||||
// _recorder = new TrackingExperimentRecorder(
|
||||
// controllerName: "LegacyGeometricController",
|
||||
// trajectoryName:
|
||||
// $"LegacyLeftArc90_R{RadiusMillimeters:0}mm",
|
||||
// trialNumber: TrialNumber,
|
||||
// referenceStart: source,
|
||||
// referenceEnd: destination,
|
||||
// referenceSpeed: CruiseSpeed);
|
||||
// _recorder.Start();
|
||||
|
||||
// try
|
||||
// {
|
||||
// _task = new DriveTask(controller.Track());
|
||||
// _task.Wait();
|
||||
|
||||
// // 保留少量停车后的样本，用于观察速度是否回到零。
|
||||
// Thread.Sleep(300);
|
||||
// }
|
||||
// finally
|
||||
// {
|
||||
// _task?.Stop();
|
||||
// _recorder?.UpdateCommand(0f, 0f);
|
||||
// _recorder?.StopAndSave();
|
||||
// _task = null;
|
||||
// _recorder = null;
|
||||
// }
|
||||
// }
|
||||
|
||||
// // 停止圆弧运动并保存当前已经采集的实验数据。
|
||||
// public override void TestStop()
|
||||
// {
|
||||
// _task?.Stop();
|
||||
// _recorder?.UpdateCommand(0f, 0f);
|
||||
// _recorder?.StopAndSave();
|
||||
// }
|
||||
// }
|
||||
|
||||
// [MovementTest(name = "SendMotion：蟹行直线4m")]
|
||||
// public class TestCrabForward4m : MovementTest
|
||||
// {
|
||||
// public float DistanceMillimeters = 4000f;
|
||||
// public float CruiseSpeed = 0.2f;
|
||||
// public int TrialNumber = 1;
|
||||
|
||||
// private DriveTask _task;
|
||||
// private TrackingExperimentRecorder _recorder;
|
||||
|
||||
// // 将车体左侧作为运动前向，沿直线蟹行4m并记录Detour实验数据。
|
||||
// public override void Test()
|
||||
// {
|
||||
// if (!TryReadStartPose(
|
||||
// out var source,
|
||||
// out var bodyYawRadians))
|
||||
// return;
|
||||
|
||||
// var motionYaw =
|
||||
// bodyYawRadians + Math.PI / 2.0;
|
||||
// var destination = new Vector2(
|
||||
// source.X +
|
||||
// DistanceMillimeters *
|
||||
// (float)Math.Cos(motionYaw),
|
||||
// source.Y +
|
||||
// DistanceMillimeters *
|
||||
// (float)Math.Sin(motionYaw));
|
||||
|
||||
// var tracker = new CrabMotionFrameTracker
|
||||
// {
|
||||
// CommandBackend =
|
||||
// CrabMotionFrameTracker
|
||||
// .ChassisCommandBackend
|
||||
// .SendMotion,
|
||||
// PathKind =
|
||||
// CrabMotionFrameTracker
|
||||
// .ReferencePathKind.Straight,
|
||||
// StartPosition = source,
|
||||
// InitialBodyYawRadians =
|
||||
// bodyYawRadians,
|
||||
// LengthMillimeters =
|
||||
// DistanceMillimeters,
|
||||
// CruiseSpeed = CruiseSpeed
|
||||
// };
|
||||
|
||||
// _recorder = new TrackingExperimentRecorder(
|
||||
// controllerName:
|
||||
// "CrabSendMotionTracker",
|
||||
// trajectoryName:
|
||||
// "CrabStraight4m",
|
||||
// trialNumber: TrialNumber,
|
||||
// referenceStart: source,
|
||||
// referenceEnd: destination,
|
||||
// referenceSpeed: CruiseSpeed,
|
||||
// referenceMotionFrameYawDegrees: 90f);
|
||||
// tracker.CommandObserver =
|
||||
// (vx, vy, omega) =>
|
||||
// _recorder?.UpdateBodyCommand(
|
||||
// vx,
|
||||
// vy,
|
||||
// omega);
|
||||
// _recorder.Start();
|
||||
|
||||
// try
|
||||
// {
|
||||
// _task = new DriveTask(tracker.Get());
|
||||
// _task.Wait();
|
||||
// Thread.Sleep(300);
|
||||
// }
|
||||
// finally
|
||||
// {
|
||||
// _task?.Stop();
|
||||
// _recorder?.UpdateBodyCommand(
|
||||
// 0f,
|
||||
// 0f,
|
||||
// 0f);
|
||||
// _recorder?.StopAndSave();
|
||||
// _task = null;
|
||||
// _recorder = null;
|
||||
// }
|
||||
// }
|
||||
|
||||
// public override void TestStop()
|
||||
// {
|
||||
// _task?.Stop();
|
||||
// _recorder?.UpdateBodyCommand(
|
||||
// 0f,
|
||||
// 0f,
|
||||
// 0f);
|
||||
// _recorder?.StopAndSave();
|
||||
// }
|
||||
|
||||
// // 读取并校验测试开始时的Detour世界位姿。
|
||||
// private static bool TryReadStartPose(
|
||||
// out Vector2 source,
|
||||
// out double bodyYawRadians)
|
||||
// {
|
||||
// var location =
|
||||
// DetourInterface.getCartLocation();
|
||||
// if (double.IsNaN(location.x) ||
|
||||
// double.IsInfinity(location.x) ||
|
||||
// double.IsNaN(location.y) ||
|
||||
// double.IsInfinity(location.y) ||
|
||||
// double.IsNaN(location.th) ||
|
||||
// double.IsInfinity(location.th))
|
||||
// {
|
||||
// Console.WriteLine(
|
||||
// "Detour当前位姿无效，取消蟹行直线测试。");
|
||||
// source = Vector2.Zero;
|
||||
// bodyYawRadians = 0.0;
|
||||
// return false;
|
||||
// }
|
||||
|
||||
// source = new Vector2(
|
||||
// (float)location.x,
|
||||
// (float)location.y);
|
||||
// bodyYawRadians =
|
||||
// AngleMath.DegreesToRadians(location.th);
|
||||
// return true;
|
||||
// }
|
||||
// }
|
||||
|
||||
// [MovementTest(name = "SendMotion：蟹行左转90°半径2m圆弧")]
|
||||
// public class TestCrabLeftArc90 : MovementTest
|
||||
// {
|
||||
// public float RadiusMillimeters = 2000f;
|
||||
// public float CruiseSpeed = 0.2f;
|
||||
// public int TrialNumber = 1;
|
||||
|
||||
// private DriveTask _task;
|
||||
// private TrackingExperimentRecorder _recorder;
|
||||
|
||||
// // 将车体左侧作为运动前向，沿半径2m的左转圆弧运动90°。
|
||||
// public override void Test()
|
||||
// {
|
||||
// var location =
|
||||
// DetourInterface.getCartLocation();
|
||||
// if (double.IsNaN(location.x) ||
|
||||
// double.IsInfinity(location.x) ||
|
||||
// double.IsNaN(location.y) ||
|
||||
// double.IsInfinity(location.y) ||
|
||||
// double.IsNaN(location.th) ||
|
||||
// double.IsInfinity(location.th))
|
||||
// {
|
||||
// Console.WriteLine(
|
||||
// "Detour当前位姿无效，取消蟹行圆弧测试。");
|
||||
// return;
|
||||
// }
|
||||
|
||||
// var source = new Vector2(
|
||||
// (float)location.x,
|
||||
// (float)location.y);
|
||||
// var bodyYawRadians =
|
||||
// AngleMath.DegreesToRadians(location.th);
|
||||
// var tracker = new CrabMotionFrameTracker
|
||||
// {
|
||||
// CommandBackend =
|
||||
// CrabMotionFrameTracker
|
||||
// .ChassisCommandBackend
|
||||
// .SendMotion,
|
||||
// PathKind =
|
||||
// CrabMotionFrameTracker
|
||||
// .ReferencePathKind.LeftArc,
|
||||
// StartPosition = source,
|
||||
// InitialBodyYawRadians =
|
||||
// bodyYawRadians,
|
||||
// RadiusMillimeters =
|
||||
// RadiusMillimeters,
|
||||
// ArcSweepRadians = Math.PI / 2.0,
|
||||
// CruiseSpeed = CruiseSpeed
|
||||
// };
|
||||
// var destination =
|
||||
// tracker.GetArcDestination();
|
||||
|
||||
// _recorder = new TrackingExperimentRecorder(
|
||||
// controllerName:
|
||||
// "CrabSendMotionTracker",
|
||||
// trajectoryName:
|
||||
// $"CrabLeftArc90_R{RadiusMillimeters:0}mm",
|
||||
// trialNumber: TrialNumber,
|
||||
// referenceStart: source,
|
||||
// referenceEnd: destination,
|
||||
// referenceSpeed: CruiseSpeed,
|
||||
// referenceMotionFrameYawDegrees: 90f);
|
||||
// tracker.CommandObserver =
|
||||
// (vx, vy, omega) =>
|
||||
// _recorder?.UpdateBodyCommand(
|
||||
// vx,
|
||||
// vy,
|
||||
// omega);
|
||||
// _recorder.Start();
|
||||
|
||||
// try
|
||||
// {
|
||||
// _task = new DriveTask(tracker.Get());
|
||||
// _task.Wait();
|
||||
// Thread.Sleep(300);
|
||||
// }
|
||||
// finally
|
||||
// {
|
||||
// _task?.Stop();
|
||||
// _recorder?.UpdateBodyCommand(
|
||||
// 0f,
|
||||
// 0f,
|
||||
// 0f);
|
||||
// _recorder?.StopAndSave();
|
||||
// _task = null;
|
||||
// _recorder = null;
|
||||
// }
|
||||
// }
|
||||
|
||||
// public override void TestStop()
|
||||
// {
|
||||
// _task?.Stop();
|
||||
// _recorder?.UpdateBodyCommand(
|
||||
// 0f,
|
||||
// 0f,
|
||||
// 0f);
|
||||
// _recorder?.StopAndSave();
|
||||
// }
|
||||
// }
|
||||
|
||||
// [MovementTest(name = "SendMotion：4m S型曲线")]
|
||||
// public class TestSCurve4m : MovementTest
|
||||
// {
|
||||
// public float LengthMillimeters = 4000f; // S型曲线纵向长度，单位mm。
|
||||
// public float LateralOffsetMillimeters = 400f; // S型曲线左右两侧的最大偏移，单位mm。
|
||||
// public float CruiseSpeed = 0.3f; // 首次实车测试建议使用0.3m/s。
|
||||
// public int TrialNumber = 1; // 重复实验编号。
|
||||
|
||||
// private DriveTask _task;
|
||||
// private TrackingExperimentRecorder _recorder;
|
||||
|
||||
// // 从当前Detour位姿开始，沿车头方向跟踪先左偏、再右偏并最终回中的完整S型曲线。
|
||||
// public override void Test()
|
||||
// {
|
||||
// if (float.IsNaN(LengthMillimeters) ||
|
||||
// float.IsInfinity(LengthMillimeters) ||
|
||||
// LengthMillimeters <= 0f ||
|
||||
// float.IsNaN(LateralOffsetMillimeters) ||
|
||||
// float.IsInfinity(LateralOffsetMillimeters) ||
|
||||
// LateralOffsetMillimeters <= 0f ||
|
||||
// float.IsNaN(CruiseSpeed) ||
|
||||
// float.IsInfinity(CruiseSpeed) ||
|
||||
// CruiseSpeed <= 0f)
|
||||
// {
|
||||
// Console.WriteLine("S型曲线测试参数无效。");
|
||||
// return;
|
||||
// }
|
||||
|
||||
// if (!MovementTestPreparation.AreWheelsForward())
|
||||
// return;
|
||||
|
||||
// var location = DetourInterface.getCartLocation();
|
||||
// if (double.IsNaN(location.x) ||
|
||||
// double.IsInfinity(location.x) ||
|
||||
// double.IsNaN(location.y) ||
|
||||
// double.IsInfinity(location.y) ||
|
||||
// double.IsNaN(location.th) ||
|
||||
// double.IsInfinity(location.th))
|
||||
// {
|
||||
// Console.WriteLine(
|
||||
// "Detour当前位姿无效，取消4m S型曲线测试。");
|
||||
// return;
|
||||
// }
|
||||
|
||||
// var source =
|
||||
// new Vector2((float)location.x, (float)location.y);
|
||||
// var headingRadians =
|
||||
// AngleMath.DegreesToRadians(location.th);
|
||||
// var length = LengthMillimeters;
|
||||
// var offset = LateralOffsetMillimeters;
|
||||
|
||||
// // 三段三次贝塞尔依次经过左侧峰值、中心线和右侧峰值，
|
||||
// // 起点、两个峰值和终点的切线均沿初始前向，连接处没有折角。
|
||||
// var firstControlPoints = new List<Vector2>
|
||||
// {
|
||||
// LocalToWorld(source, headingRadians, 0f, 0f),
|
||||
// LocalToWorld(
|
||||
// source, headingRadians,
|
||||
// length / 12f, 0f),
|
||||
// LocalToWorld(
|
||||
// source, headingRadians,
|
||||
// length / 6f, offset),
|
||||
// LocalToWorld(
|
||||
// source, headingRadians,
|
||||
// length * 0.25f, offset)
|
||||
// };
|
||||
// var secondControlPoints = new List<Vector2>
|
||||
// {
|
||||
// LocalToWorld(
|
||||
// source, headingRadians,
|
||||
// length * 0.25f, offset),
|
||||
// LocalToWorld(
|
||||
// source, headingRadians,
|
||||
// length / 3f, offset),
|
||||
// LocalToWorld(
|
||||
// source, headingRadians,
|
||||
// length * 2f / 3f, -offset),
|
||||
// LocalToWorld(
|
||||
// source, headingRadians,
|
||||
// length * 0.75f, -offset)
|
||||
// };
|
||||
// var thirdControlPoints = new List<Vector2>
|
||||
// {
|
||||
// LocalToWorld(
|
||||
// source, headingRadians,
|
||||
// length * 0.75f, -offset),
|
||||
// LocalToWorld(
|
||||
// source, headingRadians,
|
||||
// length * 5f / 6f, -offset),
|
||||
// LocalToWorld(
|
||||
// source, headingRadians,
|
||||
// length * 11f / 12f, 0f),
|
||||
// LocalToWorld(
|
||||
// source, headingRadians,
|
||||
// length, 0f)
|
||||
// };
|
||||
|
||||
// var firstTrack = new BezierTrack(firstControlPoints)
|
||||
// {
|
||||
// Speed = CruiseSpeed,
|
||||
// CarDirectionBias = 0f
|
||||
// };
|
||||
// var secondTrack = new BezierTrack(secondControlPoints)
|
||||
// {
|
||||
// Speed = CruiseSpeed,
|
||||
// CarDirectionBias = 0f
|
||||
// };
|
||||
// var thirdTrack = new BezierTrack(thirdControlPoints)
|
||||
// {
|
||||
// Speed = CruiseSpeed,
|
||||
// CarDirectionBias = 0f
|
||||
// };
|
||||
|
||||
// var controller = new ChassisController
|
||||
// {
|
||||
// BaseSpeed = CruiseSpeed
|
||||
// }.Get();
|
||||
// controller.FinishSpeed = 0f;
|
||||
|
||||
// if (!controller.AddTrack(
|
||||
// firstTrack,
|
||||
// "SCurve4m-Part1") ||
|
||||
// !controller.AddTrack(
|
||||
// secondTrack,
|
||||
// "SCurve4m-Part2") ||
|
||||
// !controller.AddTrack(
|
||||
// thirdTrack,
|
||||
// "SCurve4m-Part3"))
|
||||
// {
|
||||
// Console.WriteLine(
|
||||
// "4m S型曲线轨迹添加失败，取消测试。");
|
||||
// return;
|
||||
// }
|
||||
|
||||
// var destination =
|
||||
// LocalToWorld(
|
||||
// source,
|
||||
// headingRadians,
|
||||
// length,
|
||||
// 0f);
|
||||
// _recorder = new TrackingExperimentRecorder(
|
||||
// controllerName: "LegacyGeometricController",
|
||||
// trajectoryName:
|
||||
// $"LegacySCurve4m_A{LateralOffsetMillimeters:0}mm",
|
||||
// trialNumber: TrialNumber,
|
||||
// referenceStart: source,
|
||||
// referenceEnd: destination,
|
||||
// referenceSpeed: CruiseSpeed);
|
||||
// _recorder.Start();
|
||||
|
||||
// try
|
||||
// {
|
||||
// _task = new DriveTask(controller.Track());
|
||||
// _task.Wait();
|
||||
|
||||
// // 保留少量停止后的数据，用于观察速度是否回到零。
|
||||
// Thread.Sleep(300);
|
||||
// }
|
||||
// finally
|
||||
// {
|
||||
// _task?.Stop();
|
||||
// _recorder?.UpdateCommand(0f, 0f);
|
||||
// _recorder?.StopAndSave();
|
||||
// _task = null;
|
||||
// _recorder = null;
|
||||
// }
|
||||
// }
|
||||
|
||||
// // 停止S型曲线测试并保存当前已经采集的数据。
|
||||
// public override void TestStop()
|
||||
// {
|
||||
// _task?.Stop();
|
||||
// _recorder?.UpdateCommand(0f, 0f);
|
||||
// _recorder?.StopAndSave();
|
||||
// }
|
||||
|
||||
// // 将车体起点局部坐标转换为Detour世界坐标，X向前、Y向左。
|
||||
// private static Vector2 LocalToWorld(
|
||||
// Vector2 origin,
|
||||
// double headingRadians,
|
||||
// float localX,
|
||||
// float localY)
|
||||
// {
|
||||
// var cos = (float)Math.Cos(headingRadians);
|
||||
// var sin = (float)Math.Sin(headingRadians);
|
||||
|
||||
// return new Vector2(
|
||||
// origin.X + localX * cos - localY * sin,
|
||||
// origin.Y + localX * sin + localY * cos);
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
Reference in New Issue
Block a user