609 lines
25 KiB
C#
609 lines
25 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Numerics;
|
||
using ClumsyCore;
|
||
using ClumsyCore.Interfaces;
|
||
using ClumsyCore.Pilot;
|
||
using FundamentalLib;
|
||
using CommonUsage.Chassis;
|
||
using CommonUsage.Mathematics;
|
||
using MDCSToolBox.Clumsy.Movements;
|
||
using MDCSToolBox.Clumsy.Pilot;
|
||
using MDCSToolBox.Clumsy.Tracks;
|
||
|
||
namespace MultiWheelC;
|
||
|
||
public class MultiForwardTest : MovementDefinition
|
||
{
|
||
public float Speed = 0.2f;
|
||
public float DurationSeconds = 2f;
|
||
|
||
public override IEnumerable<bool> Get()
|
||
{
|
||
var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
|
||
chassis.SetOriginBias(0, 0, 0);
|
||
var end = DateTime.Now.AddSeconds(DurationSeconds);
|
||
while (DateTime.Now < end)
|
||
{
|
||
chassis.SendMotion(Speed, 0, 0);
|
||
yield return true;
|
||
}
|
||
|
||
chassis.SendMotion(0, 0, 0);
|
||
}
|
||
}
|
||
|
||
// 原地旋转到指定世界坐标系朝向：先把舵轮打到旋转所需角度，对齐后再旋转，按目标角度停止（非固定时长）。
|
||
public class MultiRotateToWorldAngle : MovementDefinition
|
||
{
|
||
/// <summary>目标朝向（世界坐标系，单位 deg）。</summary>
|
||
public float TargetWorldDeg;
|
||
|
||
/// <summary>旋转角速度（deg/s，逆时针为正）。</summary>
|
||
public float RotSpeed = 30f;
|
||
|
||
/// <summary>到位角度精度（deg）。</summary>
|
||
public float ArriveDeg = 1f;
|
||
|
||
/// <summary>起转前舵轮对齐精度（deg）。</summary>
|
||
public float WheelAlignDeg = 2f;
|
||
|
||
public override IEnumerable<bool> Get()
|
||
{
|
||
var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
|
||
chassis.SetOriginBias(0, 0, 0);
|
||
|
||
// 阶段一：仅把舵轮打到原地旋转所需角度（下发 0 速度，只对齐不旋转）。
|
||
while (true)
|
||
{
|
||
chassis.SendRotateMotion(0);
|
||
if (WheelsAligned(chassis, WheelAlignDeg)) break;
|
||
yield return true;
|
||
}
|
||
|
||
// 阶段二：旋转到目标世界朝向，到位即停。
|
||
var target = CommonMath.RoundTh(TargetWorldDeg);
|
||
while (true)
|
||
{
|
||
var cur = CommonMath.RoundTh((float)DetourInterface.getCartLocation().th);
|
||
var diff = CommonMath.ThDiff(target, cur); // 逆时针为正
|
||
if (Math.Abs(diff) <= ArriveDeg) break;
|
||
chassis.SendRotateMotion(Math.Sign(diff) * RotSpeed);
|
||
yield return true;
|
||
}
|
||
|
||
chassis.PredefinedDriveStop();
|
||
}
|
||
|
||
private static bool WheelsAligned(MultiWheelChassis chassis, float tolDeg)
|
||
{
|
||
#pragma warning disable CS0612, CS0618
|
||
var wheels = chassis.GetSteerWheels();
|
||
#pragma warning restore CS0612, CS0618
|
||
foreach (var sw in wheels)
|
||
if (Math.Abs(CommonMath.ThDiff(sw.ReadAngle(), sw.GetSendAngle())) > tolDeg)
|
||
return false;
|
||
return true;
|
||
}
|
||
}
|
||
|
||
[MovementTest(name = "多舵轮-前进2秒")]
|
||
public class MultiForwardMovementTest : MovementTest
|
||
{
|
||
private DriveTask _task;
|
||
|
||
public override void Test()
|
||
{
|
||
_task = new DriveTask(new MultiForwardTest().Get());
|
||
_task.Wait();
|
||
}
|
||
|
||
public override void TestStop() => _task?.Stop();
|
||
}
|
||
|
||
[MovementTest(name = "多舵轮-原地旋转到目标角度")]
|
||
public class MultiRotateMovementTest : MovementTest
|
||
{
|
||
private DriveTask _task;
|
||
|
||
public override void Test()
|
||
{
|
||
_task = new DriveTask(new MultiRotateToWorldAngle
|
||
{
|
||
TargetWorldDeg = PilotDefinition.Conf.InPlaceRotateTargetWorldDeg,
|
||
RotSpeed = PilotDefinition.Conf.InPlaceRotateSpeed,
|
||
ArriveDeg = PilotDefinition.Conf.InPlaceRotateArriveDeg,
|
||
WheelAlignDeg = PilotDefinition.Conf.InPlaceRotateWheelAlignDeg
|
||
}.Get());
|
||
_task.Wait();
|
||
}
|
||
|
||
public override void TestStop()
|
||
{
|
||
_task?.Stop();
|
||
((MultiWheelChassis)BasicPilotBase.Chassis).PredefinedDriveStop();
|
||
}
|
||
}
|
||
|
||
// ===== 车队联动-原地旋转动作 =====
|
||
// 等价于 FleetRemote 的「原地旋转」模式（已实测可用）：FleetRemote 通过 Medulla 手动 IO
|
||
// (MultiVehicleManualEnabled + Mode=2 + Vth) 驱动 PilotDefinition.TickMultiVehicle 绕车队中心旋转。
|
||
// 手动 IO 是 [AsLowerIO]（Medulla→Clumsy，每周期回写），Clumsy 侧动作直接写会被覆盖；
|
||
// 因此本动作改用 Clumsy 内部脚本字段 MultiVehicleScript*（TickMultiVehicle 已将其作为手动等价输入），
|
||
// 不写一行底盘指令——实际的 SendRotateMotion + PI 纠偏 + 向从车广播均由 TickMultiVehicle 完成。
|
||
//
|
||
// 前提：在「主车」(MultiVehicleMasterEndpoint == "/") 的 Clumsy 上运行，且从车已注册(编队就绪)。
|
||
// 停止条件：主车 SLAM 朝向累计转过 |TargetDeltaDeg|（刚体原地旋转，整车朝向变化量 == 车队转角）；
|
||
// 无定位时退化为按 |TargetDeltaDeg| / |Omega| 估算时长；并带安全超时。
|
||
public class FleetRotateInPlace : MovementDefinition
|
||
{
|
||
/// <summary>角速度大小(deg/s)；实际方向由 TargetDeltaDeg 的符号决定。</summary>
|
||
public float Omega = 15f;
|
||
|
||
/// <summary>目标相对转角(deg，带符号，+ 为逆时针)。</summary>
|
||
public float TargetDeltaDeg = 90f;
|
||
|
||
/// <summary>到位角度精度(deg)。</summary>
|
||
public float ArriveDeg = 1.5f;
|
||
|
||
/// <summary>减速区宽度(deg)：剩余角度小于此值时，角速度按剩余比例线性降到 MinOmega，抑制惯性超调。</summary>
|
||
public float SlowDeg = 25f;
|
||
|
||
/// <summary>减速区末段最小角速度(deg/s)：避免越接近目标越慢、长尾停不下/到不了位。</summary>
|
||
public float MinOmega = 3f;
|
||
|
||
/// <summary>缓启动角加速度(deg/s²)：起步时角速度从 0 按此斜率爬升到巡航值，抑制起步抖动/队形骤偏。仅作用于起步加速，<=0 关闭缓启动（阶跃起步）。</summary>
|
||
public float AccelDegPerSec2 = 20f;
|
||
|
||
/// <summary>
|
||
/// 是否用 Detour 主车航向闭环判停（读 getCartLocation().th 累计实际转角，到 |TargetDeltaDeg| 停）。
|
||
/// 与 MultiVehicleSyncUseDetour 解耦：转到指定角度需要角度反馈，故默认 true。
|
||
/// false 时退化为按估算时长开环停止（实际转速≠指令时不精确）。注意 true 时若无有效全局定位，
|
||
/// getCartLocation() 会阻塞（与单车 MultiRotateToWorldAngle 行为一致）。
|
||
/// </summary>
|
||
public bool UseDetourHeading = true;
|
||
|
||
// 注：不设超时上限——旋转持续到到位（或无定位时按估算时长结束），或被 Stop()/TestStop() 主动中止。
|
||
|
||
/// <summary>到位后保持脚本使能、角速度归零的安定时长(s)，让纠偏把队形稳住再撤离。</summary>
|
||
public float SettleSec = 0.5f;
|
||
|
||
private void ClearScript()
|
||
{
|
||
var self = PilotDefinition.Self;
|
||
self.MultiVehicleScriptVx = 0;
|
||
self.MultiVehicleScriptVy = 0;
|
||
self.MultiVehicleScriptVth = 0;
|
||
self.MultiVehicleScriptEnabled = false;
|
||
}
|
||
|
||
public void Stop() => ClearScript();
|
||
|
||
public override IEnumerable<bool> Get()
|
||
{
|
||
var self = PilotDefinition.Self;
|
||
var conf = PilotDefinition.Conf;
|
||
|
||
if (conf.MultiVehicleMasterEndpoint != "/")
|
||
{
|
||
Hedingben.ToastText("车队原地旋转需在主车(主车端点=\"/\")运行", "FleetRotate");
|
||
yield break;
|
||
}
|
||
|
||
var dir = Math.Sign(TargetDeltaDeg);
|
||
if (dir == 0) dir = 1;
|
||
var maxOmega = Math.Abs(Omega);
|
||
var minOmega = Math.Min(Math.Abs(MinOmega), maxOmega); // 最小不超过最大
|
||
var slowDeg = Math.Max(1e-3f, SlowDeg); // 减速区宽度
|
||
var accel = AccelDegPerSec2; // 缓启动角加速度，仅作用于起步，<=0 关闭
|
||
var targetMag = Math.Abs(TargetDeltaDeg);
|
||
|
||
var hasPos = UseDetourHeading;
|
||
var prevTh = hasPos ? (float)DetourInterface.getCartLocation().th : 0f;
|
||
var startTh = prevTh;
|
||
var accumulated = 0f; // 累计带符号转角(deg)
|
||
var start = DateTime.Now;
|
||
var lastTime = start;
|
||
var lastLog = DateTime.MinValue;
|
||
var lastCenterLog = DateTime.MinValue;
|
||
var cmdMag = 0f; // 当前实际下发角速度大小(deg/s)，缓启动从 0 斜坡爬升
|
||
var centerTracking = false;
|
||
float centerStartX = 0, centerStartY = 0, centerStartTh = 0;
|
||
float centerLastX = 0, centerLastY = 0, centerLastTh = 0, centerMaxDrift = 0;
|
||
// 无定位按时长估算时，补上缓启动斜坡少转的等效时间(≈ maxOmega/(2·accel))，使时长更接近目标角。
|
||
var estDuration = maxOmega > 1e-3 ? targetMag / maxOmega : 0;
|
||
if (accel > 1e-3) estDuration += maxOmega / (2 * accel);
|
||
|
||
DLog.Log(
|
||
$"REQUEST target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} accel={accel:0.0} " +
|
||
$"slowDeg={slowDeg:0.0} minOmega={minOmega:0.0} useDetourHeading={hasPos} startTh={startTh:0.00} " +
|
||
$"estDuration={estDuration:0.00}s syncUseDetour={conf.MultiVehicleSyncUseDetour}",
|
||
"FleetRotateDbg");
|
||
|
||
// 使能脚本驱动的原地旋转（mode2）。TickMultiVehicle 后台循环据此执行旋转并广播给从车。
|
||
// 起步从 0 角速度开始，由缓启动斜坡爬升，避免阶跃下发导致队形骤偏/抖动。
|
||
self.MultiVehicleScriptVx = 0;
|
||
self.MultiVehicleScriptVy = 0;
|
||
self.MultiVehicleScriptMode = 2;
|
||
self.MultiVehicleScriptVth = 0;
|
||
self.MultiVehicleScriptEnabled = true;
|
||
self.MultiVehicleRotateWheelsReady = false;
|
||
self.MultiVehicleRotateFleetReady = false;
|
||
|
||
DLog.Log("WAIT_ALIGN fleet rotate wheels", "FleetRotateDbg");
|
||
while (!self.MultiVehicleRotateFleetReady)
|
||
{
|
||
self.MultiVehicleScriptVx = 0;
|
||
self.MultiVehicleScriptVy = 0;
|
||
self.MultiVehicleScriptMode = 2;
|
||
self.MultiVehicleScriptVth = 0;
|
||
self.MultiVehicleScriptEnabled = true;
|
||
Hedingben.ToastText("车队原地旋转舵轮预对齐中", "FleetRotate");
|
||
yield return true;
|
||
}
|
||
|
||
float centerStartCarX = 0, centerStartCarY = 0, centerStartCarTh = 0;
|
||
if (hasPos)
|
||
{
|
||
var startPos = DetourInterface.getCartLocation();
|
||
centerStartCarX = (float)startPos.x;
|
||
centerStartCarY = (float)startPos.y;
|
||
centerStartCarTh = (float)startPos.th;
|
||
prevTh = centerStartCarTh;
|
||
startTh = prevTh;
|
||
if (self.TryGetFleetCenterFromPose(centerStartCarX, centerStartCarY, centerStartCarTh,
|
||
out centerStartX, out centerStartY, out centerStartTh))
|
||
{
|
||
centerLastX = centerStartX;
|
||
centerLastY = centerStartY;
|
||
centerLastTh = centerStartTh;
|
||
centerMaxDrift = 0;
|
||
centerTracking = true;
|
||
}
|
||
}
|
||
accumulated = 0f;
|
||
start = DateTime.Now;
|
||
lastTime = start;
|
||
lastLog = DateTime.MinValue;
|
||
lastCenterLog = DateTime.MinValue;
|
||
cmdMag = 0f;
|
||
DLog.Log(
|
||
$"START target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} startTh={startTh:0.00} " +
|
||
$"fleetAligned={self.MultiVehicleRotateFleetReady}",
|
||
"FleetRotateDbg");
|
||
if (centerTracking)
|
||
{
|
||
DLog.Log(
|
||
$"START center=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
|
||
$"car=({centerStartCarX:0.0},{centerStartCarY:0.0},{centerStartCarTh:0.00}) " +
|
||
$"target={TargetDeltaDeg:0.0} omega={maxOmega:0.0}",
|
||
"FleetRotateCenterDbg");
|
||
}
|
||
|
||
var stopReason = "stop()";
|
||
while (true)
|
||
{
|
||
var now = DateTime.Now;
|
||
var dt = (float)Math.Min(0.2, Math.Max(0, (now - lastTime).TotalSeconds));
|
||
lastTime = now;
|
||
var elapsed = (now - start).TotalSeconds;
|
||
|
||
float desiredMag;
|
||
float curTh = 0f, remaining = 0f, actualRate = 0f;
|
||
if (hasPos)
|
||
{
|
||
var carPos = DetourInterface.getCartLocation();
|
||
curTh = (float)carPos.th;
|
||
var step = (float)CommonMath.ThDiff(curTh, prevTh); // 本帧实际转角(逆时针为正)
|
||
accumulated += step;
|
||
actualRate = dt > 1e-3 ? step / dt : 0f; // 实际角速率(deg/s)，用于对比指令
|
||
prevTh = curTh;
|
||
|
||
remaining = targetMag - Math.Abs(accumulated);
|
||
if (remaining <= ArriveDeg) { stopReason = "arrived"; break; }
|
||
|
||
// 减速区：剩余角度 < SlowDeg 时，目标角速度按剩余比例线性降到 MinOmega，
|
||
// 使切断指令瞬间残余动量足够小，抑制惯性滑行造成的超调。宽度直观、便于现场调试。
|
||
desiredMag = remaining < slowDeg
|
||
? Math.Max(minOmega, maxOmega * (remaining / slowDeg))
|
||
: maxOmega;
|
||
|
||
if (centerTracking &&
|
||
self.TryGetFleetCenterFromPose((float)carPos.x, (float)carPos.y, (float)carPos.th,
|
||
out centerLastX, out centerLastY, out centerLastTh))
|
||
{
|
||
var centerDx = centerLastX - centerStartX;
|
||
var centerDy = centerLastY - centerStartY;
|
||
var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
|
||
centerMaxDrift = Math.Max(centerMaxDrift, centerDrift);
|
||
var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
|
||
if ((now - lastCenterLog).TotalMilliseconds >= 250)
|
||
{
|
||
lastCenterLog = now;
|
||
DLog.Log(
|
||
$"ACTION t={elapsed:0.00}s center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
|
||
$"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
|
||
$"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
|
||
$"cmdW={dir * cmdMag:0.000} actualW={actualRate:0.000} acc={accumulated:0.0} remain={remaining:0.0} " +
|
||
$"wheelReady={self.MultiVehicleRotateWheelsReady} fleetReady={self.MultiVehicleRotateFleetReady}",
|
||
"FleetRotateCenterDbg");
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// 无定位：按时长估算，无法测角，目标维持巡航速度到估算时长（仅缓启动整形）。
|
||
desiredMag = maxOmega;
|
||
if (elapsed >= estDuration) { stopReason = "estDuration"; break; }
|
||
}
|
||
|
||
// 缓启动：只对“加速(目标>当前)”按角加速度限斜率，让起步平滑爬升；
|
||
// “减速(目标<当前)”跟随上面的减速曲线立即下调，保证及时刹车不超调。
|
||
if (accel > 1e-3 && desiredMag > cmdMag)
|
||
cmdMag = Math.Min(desiredMag, cmdMag + accel * dt);
|
||
else
|
||
cmdMag = desiredMag;
|
||
|
||
self.MultiVehicleScriptVth = dir * cmdMag;
|
||
|
||
// 落盘诊断(节流~150ms)：实际航向/累计转角/实际角速率 vs 指令角速率，定位"开环转速不足"。
|
||
if ((now - lastLog).TotalMilliseconds >= 150)
|
||
{
|
||
lastLog = now;
|
||
DLog.Log(
|
||
hasPos
|
||
? $"t={elapsed:0.00}s curTh={curTh:0.00} acc={accumulated:0.0} remain={remaining:0.0} " +
|
||
$"cmdW={dir * cmdMag:0.0} actualW={actualRate:0.0} (实际/指令={(Math.Abs(cmdMag) > 1e-3 ? actualRate / (dir * cmdMag) : 0):0.00})"
|
||
: $"t={elapsed:0.00}s/{estDuration:0.00}s (无航向反馈,开环按时长) cmdW={dir * cmdMag:0.0}",
|
||
"FleetRotateDbg");
|
||
}
|
||
|
||
Hedingben.ToastText(
|
||
hasPos
|
||
? $"车队原地旋转 目标{TargetDeltaDeg:0.0}° 已转{accumulated:0.0}° 余{targetMag - Math.Abs(accumulated):0.0}° ω={cmdMag:0.0}"
|
||
: $"车队原地旋转(无定位,按时长) {elapsed:0.0}/{estDuration:0.0}s ω={cmdMag:0.0}",
|
||
"FleetRotate");
|
||
|
||
yield return true;
|
||
}
|
||
|
||
// 到位：角速度先归零，保持脚本使能让 TickMultiVehicle 的 PI 把队形稳住一小段时间再撤离。
|
||
self.MultiVehicleScriptVth = 0;
|
||
var settleEnd = DateTime.Now.AddSeconds(Math.Max(0, SettleSec));
|
||
while (DateTime.Now < settleEnd)
|
||
yield return true;
|
||
|
||
ClearScript();
|
||
DLog.Log(
|
||
$"DONE reason={stopReason} 累计转角={accumulated:0.0}° 目标={TargetDeltaDeg:0.0}° " +
|
||
$"用时={(DateTime.Now - start).TotalSeconds:0.00}s useDetourHeading={hasPos}",
|
||
"FleetRotateDbg");
|
||
if (centerTracking)
|
||
{
|
||
var centerDx = centerLastX - centerStartX;
|
||
var centerDy = centerLastY - centerStartY;
|
||
var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
|
||
var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
|
||
DLog.Log(
|
||
$"DONE reason={stopReason} center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
|
||
$"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
|
||
$"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
|
||
$"acc={accumulated:0.0} target={TargetDeltaDeg:0.0}",
|
||
"FleetRotateCenterDbg");
|
||
}
|
||
Hedingben.ToastText($"车队原地旋转完成({stopReason}) 累计{accumulated:0.0}°", "FleetRotate");
|
||
}
|
||
}
|
||
|
||
[MovementTest(name = "车队联动-原地旋转")]
|
||
public class FleetRotateInPlaceTest : MovementTest
|
||
{
|
||
private FleetRotateInPlace _proc;
|
||
private DriveTask _task;
|
||
|
||
public override void Test()
|
||
{
|
||
_proc = new FleetRotateInPlace
|
||
{
|
||
Omega = PilotDefinition.Conf.FleetRotateOmega,
|
||
TargetDeltaDeg = PilotDefinition.Conf.FleetRotateTargetDeltaDeg,
|
||
ArriveDeg = PilotDefinition.Conf.FleetRotateArriveDeg,
|
||
SlowDeg = PilotDefinition.Conf.FleetRotateSlowDeg,
|
||
MinOmega = PilotDefinition.Conf.FleetRotateMinOmega,
|
||
AccelDegPerSec2 = PilotDefinition.Conf.FleetRotateAccel,
|
||
SettleSec = PilotDefinition.Conf.FleetRotateSettleSec,
|
||
UseDetourHeading = PilotDefinition.Conf.FleetRotateUseDetourHeading
|
||
};
|
||
_task = new DriveTask(_proc.Get());
|
||
_task.Wait();
|
||
}
|
||
|
||
public override void TestStop()
|
||
{
|
||
_proc?.Stop();
|
||
_task?.Stop();
|
||
}
|
||
}
|
||
|
||
[MovementTest(name = "车队联动-曲线行走")]
|
||
public class FleetCurveWalkTest : MovementTest
|
||
{
|
||
private FleetCurveWalk _proc;
|
||
private DriveTask _task;
|
||
|
||
public override void Test()
|
||
{
|
||
var self = PilotDefinition.Self;
|
||
if (!self.TryGetFleetCenterFromMembers(out var x, out var y, out var th) &&
|
||
!self.TryGetFleetCenterFromSlam(out x, out y, out th))
|
||
{
|
||
DLog.Log("FleetCurveWalkTest abort: failed to read fleet center.", "FleetCurveDbg");
|
||
Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
|
||
return;
|
||
}
|
||
|
||
var pointCount = Math.Max(3, PilotDefinition.Conf.FleetCurveTestControlPointCount);
|
||
var controlPoints = new List<Vector2>();
|
||
for (var i = 0; i < pointCount; i++)
|
||
controlPoints.Add(UI.GetPoint($"FleetCurve point {i + 1}/{pointCount}"));
|
||
var fleetCenter = new Vector2(x, y);
|
||
if (Vector2.Distance(fleetCenter, controlPoints[0]) >
|
||
Vector2.Distance(fleetCenter, controlPoints[controlPoints.Count - 1]))
|
||
controlPoints.Reverse();
|
||
var track = new BezierTrack(controlPoints)
|
||
{
|
||
Speed = PilotDefinition.Conf.FleetCurveSpeed,
|
||
CarDirectionBias = 0f
|
||
};
|
||
|
||
_proc = new FleetCurveWalk
|
||
{
|
||
Track = track,
|
||
CurveSpeed = PilotDefinition.Conf.FleetCurveSpeed,
|
||
CarDirectionBias = 0f,
|
||
SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance,
|
||
FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance,
|
||
FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed,
|
||
SlowingPow = PilotDefinition.Conf.FleetCurveSlowingPow,
|
||
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
|
||
StartSyncTimeoutSec = PilotDefinition.Conf.FleetCrabStartSyncTimeoutSec
|
||
};
|
||
_task = new DriveTask(_proc.Get());
|
||
_task.Wait();
|
||
}
|
||
|
||
public override void TestStop()
|
||
{
|
||
_proc?.Stop();
|
||
_task?.Stop();
|
||
}
|
||
}
|
||
|
||
[MovementTest(name = "车队联动-自动蟹行")]
|
||
public class FleetCrabWalkTest : MovementTest
|
||
{
|
||
private FleetCrabWalk _proc;
|
||
private DriveTask _task;
|
||
|
||
public override void Test()
|
||
{
|
||
_proc = new FleetCrabWalk
|
||
{
|
||
CrabAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
|
||
BodyToPathAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
|
||
CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm,
|
||
CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed,
|
||
FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
|
||
FleetCrabStartAccel = PilotDefinition.Conf.FleetCrabStartAccel,
|
||
FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
|
||
FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
|
||
FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,
|
||
FleetCrabSlowingPow = PilotDefinition.Conf.FleetCrabSlowingPow,
|
||
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
|
||
};
|
||
_task = new DriveTask(_proc.Get());
|
||
_task.Wait();
|
||
}
|
||
|
||
public override void TestStop()
|
||
{
|
||
_proc?.Stop();
|
||
_task?.Stop();
|
||
}
|
||
}
|
||
|
||
// ===== 调用 Playground WebAPI 瞬移小车（前移 / 左移 / 旋转）=====
|
||
// 平移/旋转量在 Fields 面板配置：WebApiTranslateMm（默认100mm）、WebApiRotateDeg（默认5度）。
|
||
|
||
[MovementTest(name = "多舵轮-WebAPI前移")]
|
||
public class WebApiForwardMoveTest : MovementTest
|
||
{
|
||
public override void Test()
|
||
{
|
||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||
var name = PilotDefinition.Conf.PlaygroundRobotName;
|
||
var d = PilotDefinition.Conf.WebApiTranslateMm;
|
||
|
||
var pose = PlaygroundWebApi.GetPose(url, name);
|
||
// 车体系前向 (d, 0) 变换到世界系：车头方向即朝向 yaw
|
||
var dst = CommonMath.Transform2D(new Vector2(pose.X, pose.Y), pose.YawDeg, new Vector2(d, 0));
|
||
PlaygroundWebApi.Move(url, name, dst.X, dst.Y, pose.YawDeg);
|
||
Hedingben.ToastText($"前移 {d:f0}mm -> ({dst.X:f0},{dst.Y:f0})", "WebApiForward");
|
||
}
|
||
|
||
public override void TestStop()
|
||
{
|
||
}
|
||
}
|
||
|
||
[MovementTest(name = "多舵轮-WebAPI左移")]
|
||
public class WebApiLeftMoveTest : MovementTest
|
||
{
|
||
public override void Test()
|
||
{
|
||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||
var name = PilotDefinition.Conf.PlaygroundRobotName;
|
||
var d = PilotDefinition.Conf.WebApiTranslateMm;
|
||
|
||
var pose = PlaygroundWebApi.GetPose(url, name);
|
||
// 车体系左向 (0, d) 变换到世界系（车体 +Y 即左侧）
|
||
var dst = CommonMath.Transform2D(new Vector2(pose.X, pose.Y), pose.YawDeg, new Vector2(0, d));
|
||
PlaygroundWebApi.Move(url, name, dst.X, dst.Y, pose.YawDeg);
|
||
Hedingben.ToastText($"左移 {d:f0}mm -> ({dst.X:f0},{dst.Y:f0})", "WebApiLeft");
|
||
}
|
||
|
||
public override void TestStop()
|
||
{
|
||
}
|
||
}
|
||
|
||
[MovementTest(name = "多舵轮-WebAPI旋转")]
|
||
public class WebApiRotateTest : MovementTest
|
||
{
|
||
public override void Test()
|
||
{
|
||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||
var name = PilotDefinition.Conf.PlaygroundRobotName;
|
||
var deg = PilotDefinition.Conf.WebApiRotateDeg;
|
||
|
||
var pose = PlaygroundWebApi.GetPose(url, name);
|
||
var ny = pose.YawDeg + deg; // 逆时针为正
|
||
PlaygroundWebApi.Move(url, name, pose.X, pose.Y, ny);
|
||
Hedingben.ToastText($"旋转 {deg:f1}° -> {ny:f1}°", "WebApiRotate");
|
||
}
|
||
|
||
public override void TestStop()
|
||
{
|
||
}
|
||
}
|
||
|
||
[MovementTest(name = "多舵轮-WebAPI恢复运动")]
|
||
public class WebApiMotionResumeTest : MovementTest
|
||
{
|
||
public override void Test()
|
||
{
|
||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||
PlaygroundWebApi.ResumeMotion(url);
|
||
Hedingben.ToastText("已恢复车辆运动", "WebApiMotion");
|
||
}
|
||
|
||
public override void TestStop()
|
||
{
|
||
}
|
||
}
|
||
|
||
[MovementTest(name = "多舵轮-WebAPI暂停运动")]
|
||
public class WebApiMotionPauseTest : MovementTest
|
||
{
|
||
public override void Test()
|
||
{
|
||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||
PlaygroundWebApi.PauseMotion(url); // 默认 zero 模式：反馈归零
|
||
Hedingben.ToastText("已暂停车辆运动 (zero)", "WebApiMotion");
|
||
}
|
||
|
||
public override void TestStop()
|
||
{
|
||
}
|
||
}
|