690 lines
28 KiB
C#
690 lines
28 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.MotionControllers;
|
||
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 cmdMag = 0f; // 当前实际下发角速度大小(deg/s)，缓启动从 0 斜坡爬升
|
||
// 无定位按时长估算时，补上缓启动斜坡少转的等效时间(≈ maxOmega/(2·accel))，使时长更接近目标角。
|
||
var estDuration = maxOmega > 1e-3 ? targetMag / maxOmega : 0;
|
||
if (accel > 1e-3) estDuration += maxOmega / (2 * accel);
|
||
|
||
DLog.Log(
|
||
$"START 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;
|
||
|
||
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)
|
||
{
|
||
curTh = (float)DetourInterface.getCartLocation().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;
|
||
}
|
||
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");
|
||
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();
|
||
}
|
||
}
|
||
|
||
// ===== 车队联动-自动蟹行动作 =====
|
||
// 以当前车队中心为起点，构造与车队朝向夹角 x、长度 y 的直线路径；执行侧复用
|
||
// TickMultiVehicle 的脚本手动等价输入(mode=1)，也就是 FleetRemote 手动蟹行同一条下发链路。
|
||
//
|
||
// 手动蟹行已验证丝滑，自动动作只额外做两件事：
|
||
// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度和横向偏差；
|
||
// 2) 用小幅、带斜率限制的方向修正写 MultiVehicleScriptVx/Vy，避免几何控制器 bias/dTh 阶跃造成抖动。
|
||
//
|
||
// 与 FleetRemote 手动蟹行(mode==1)对照：TickMultiVehicle 仍负责合成 frontTh==rearTh 的蟹行舵角并广播给从车。
|
||
//
|
||
// 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
|
||
public class FleetCrabWalk : MovementDefinition
|
||
{
|
||
/// <summary>与当前车队朝向的夹角(deg，逆时针为正)。</summary>
|
||
public float CrabAngleDeg = 45f;
|
||
|
||
/// <summary>路径长度(mm)。</summary>
|
||
public float CrabLengthMm = 2000f;
|
||
|
||
/// <summary>行驶速度(m/s)。</summary>
|
||
public float CrabSpeed = 0.2f;
|
||
|
||
/// <summary>兼容旧配置；当前脚本手动等价实现不再直接使用几何控制器 gcp 上限。</summary>
|
||
public float GcpThetaThreshold = 95f;
|
||
|
||
/// <summary>横向误差转向增益，沿用 Stanley 形式：atan(gain * lateral / speed)。</summary>
|
||
public float CorrectionGain = 1f;
|
||
|
||
/// <summary>自动纠偏最大改向角(deg)。越小越接近手动蟹行，越大收敛越快。</summary>
|
||
public float CorrectionAngleThreshold = 8f;
|
||
|
||
/// <summary>脚本 Vx/Vy 命令斜率限制(m/s^2)，避免纠偏量变化造成舵角阶跃。</summary>
|
||
public float CommandAccel = 0.4f;
|
||
|
||
private bool _stopping;
|
||
|
||
private void Cleanup()
|
||
{
|
||
var self = PilotDefinition.Self;
|
||
self.MultiVehicleScriptVx = 0;
|
||
self.MultiVehicleScriptVy = 0;
|
||
self.MultiVehicleScriptVth = 0;
|
||
self.MultiVehicleScriptMode = 0;
|
||
self.MultiVehicleScriptEnabled = false;
|
||
self.MultiVehicleAutoVx = 0;
|
||
self.MultiVehicleAutoFrontTh = 0;
|
||
self.MultiVehicleAutoRearTh = 0;
|
||
self.MultiVehicleAutoHasIdeal = false;
|
||
self.MultiVehicleAutoEnabled = false;
|
||
}
|
||
|
||
public void Stop()
|
||
{
|
||
_stopping = true;
|
||
Cleanup();
|
||
}
|
||
|
||
private static float Slew(float current, float target, float maxStep)
|
||
{
|
||
var diff = target - current;
|
||
if (Math.Abs(diff) <= maxStep) return target;
|
||
return current + Math.Sign(diff) * maxStep;
|
||
}
|
||
|
||
public override IEnumerable<bool> Get()
|
||
{
|
||
var self = PilotDefinition.Self;
|
||
var conf = PilotDefinition.Conf;
|
||
_stopping = false;
|
||
|
||
DLog.Log(
|
||
$"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " +
|
||
$"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " +
|
||
$"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
|
||
$"manualLike=true corrGain={CorrectionGain:0.00} corrMax={CorrectionAngleThreshold:0.0} accel={CommandAccel:0.00}",
|
||
"FleetCrabDbg");
|
||
|
||
if (conf.MultiVehicleMasterEndpoint != "/")
|
||
{
|
||
DLog.Log($"ABORT: 非主车 (endpoint={conf.MultiVehicleMasterEndpoint})", "FleetCrabDbg");
|
||
Hedingben.ToastText("车队蟹行需在主车(主车端点=\"/\")运行", "FleetCrab");
|
||
yield break;
|
||
}
|
||
|
||
// 注意：getCartLocation() 在无有效 Detour 定位时会阻塞——若卡在这里且后面看不到 CENTER 日志，即定位未就绪。
|
||
DLog.Log("主车校验通过，开始读取车队中心 (getCartLocation 无定位会阻塞)…", "FleetCrabDbg");
|
||
if (!self.TryGetFleetCenterFromSlam(out var x0, out var y0, out var theta))
|
||
{
|
||
DLog.Log("ABORT: TryGetFleetCenterFromSlam 返回 false (无定位)", "FleetCrabDbg");
|
||
Hedingben.ToastText("车队蟹行需要主车 Detour 定位", "FleetCrab");
|
||
yield break;
|
||
}
|
||
DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
|
||
|
||
var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
|
||
var dst = CommonMath.Transform2D(new Vector2(x0, y0), phi, new Vector2(CrabLengthMm, 0));
|
||
var phiRad = phi / 180.0 * Math.PI;
|
||
var pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
|
||
var pathLeft = new Vector2(-pathDir.Y, pathDir.X);
|
||
|
||
DLog.Log(
|
||
$"START center=({x0:0},{y0:0},{theta:0.0}) crabAngle={CrabAngleDeg:0.0} phi={phi:0.0} " +
|
||
$"len={CrabLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000}",
|
||
"FleetCrabDbg");
|
||
|
||
// 手动蟹行已验证丝滑：这里复用 TickMultiVehicle 的脚本手动等价输入(mode=1)，
|
||
// 只在本动作内根据车队中心相对直线的横向误差缓慢调整 Vx/Vy 方向。
|
||
self.MultiVehicleScriptEnabled = true;
|
||
self.MultiVehicleScriptMode = 1;
|
||
self.MultiVehicleScriptVx = 0;
|
||
self.MultiVehicleScriptVy = 0;
|
||
self.MultiVehicleScriptVth = 0;
|
||
self.PrimeMasterAutoFromSlam();
|
||
DLog.Log("WARMUP 已启用脚本蟹行(mode=1)，等待编队成员就位…", "FleetCrabDbg");
|
||
|
||
var warmEnd = DateTime.Now.AddSeconds(2.0);
|
||
var warmIter = 0;
|
||
var warmReady = false;
|
||
while (!_stopping && DateTime.Now < warmEnd)
|
||
{
|
||
warmIter++;
|
||
self.MultiVehicleScriptEnabled = true;
|
||
self.MultiVehicleScriptMode = 1;
|
||
self.PrimeMasterAutoFromSlam();
|
||
var snap = self.GetFleetCenterSnapshot();
|
||
int cnt;
|
||
lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
|
||
if (warmIter % 5 == 0)
|
||
DLog.Log(
|
||
$"WARMUP#{warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) tick={snap.Tick} " +
|
||
$"scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum}",
|
||
"FleetCrabDbg");
|
||
if (cnt >= conf.MultiVehicleFleetNum)
|
||
{
|
||
warmReady = true;
|
||
DLog.Log(
|
||
$"WARMUP done iter={warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt}",
|
||
"FleetCrabDbg");
|
||
break;
|
||
}
|
||
yield return true;
|
||
}
|
||
if (!warmReady)
|
||
DLog.Log("WARMUP 超时：编队仍未就位，继续进入脚本蟹行（若不动请查看 FleetDiagClumsy ready/cnt）",
|
||
"FleetCrabDbg");
|
||
|
||
Hedingben.ToastText($"车队蟹行 夹角{CrabAngleDeg:0.0}° 长度{CrabLengthMm:0}mm", "FleetCrab");
|
||
|
||
var iter = 0;
|
||
var cmdVx = 0f;
|
||
var cmdVy = 0f;
|
||
var lastTime = DateTime.Now;
|
||
var lastLog = DateTime.MinValue;
|
||
var finishDistance = Math.Max(20f, conf.FinishDistance);
|
||
var stopReason = "done";
|
||
|
||
while (!_stopping)
|
||
{
|
||
iter++;
|
||
var now = DateTime.Now;
|
||
var dt = (float)Math.Min(0.2, Math.Max(0.001, (now - lastTime).TotalSeconds));
|
||
lastTime = now;
|
||
|
||
self.TryGetFleetCenterFromSlam(out var cx, out var cy, out var cth);
|
||
var delta = new Vector2(cx - x0, cy - y0);
|
||
var along = Vector2.Dot(delta, pathDir);
|
||
var lateral = Vector2.Dot(delta, pathLeft);
|
||
var remain = CrabLengthMm - along;
|
||
if (remain <= finishDistance)
|
||
break;
|
||
|
||
var speed = Math.Abs(CrabSpeed);
|
||
if (remain < conf.SlowDistance && conf.SlowDistance > 1)
|
||
{
|
||
var ratio = (float)Math.Pow(Math.Max(0, remain) / conf.SlowDistance, conf.SlowingPow);
|
||
speed = ratio * (speed - Math.Abs(conf.FinishSpeed)) + Math.Abs(conf.FinishSpeed);
|
||
}
|
||
|
||
var correction = (float)(-Math.Atan(CorrectionGain * lateral / 1000f / Math.Max(speed, 0.3f)) / Math.PI * 180.0);
|
||
correction = Math.Sign(correction) * Math.Min(Math.Abs(correction), Math.Abs(CorrectionAngleThreshold));
|
||
var desiredWorldAngle = CommonMath.RoundTh(phi + correction);
|
||
var localAngle = (float)CommonMath.ThDiff(desiredWorldAngle, cth);
|
||
var localRad = localAngle / 180.0 * Math.PI;
|
||
var targetVx = speed * (float)Math.Cos(localRad);
|
||
var targetVy = speed * (float)Math.Sin(localRad);
|
||
var maxStep = Math.Max(0.05f, CommandAccel) * dt;
|
||
cmdVx = Slew(cmdVx, targetVx, maxStep);
|
||
cmdVy = Slew(cmdVy, targetVy, maxStep);
|
||
|
||
self.MultiVehicleScriptEnabled = true;
|
||
self.MultiVehicleScriptMode = 1;
|
||
self.MultiVehicleScriptVx = cmdVx;
|
||
self.MultiVehicleScriptVy = cmdVy;
|
||
self.MultiVehicleScriptVth = 0;
|
||
|
||
if ((DateTime.Now - lastLog).TotalMilliseconds >= 300)
|
||
{
|
||
lastLog = DateTime.Now;
|
||
var snap = self.GetFleetCenterSnapshot();
|
||
int fleetCnt;
|
||
lock (self.FleetLock) fleetCnt = self.MultiVehicleFleet.Count;
|
||
DLog.Log(
|
||
$"ITER#{iter} center=({cx:0},{cy:0},{cth:0.0}) snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
|
||
$"along={along:0} lateral={lateral:0} remain={remain:0} corr={correction:0.0} " +
|
||
$"localAngle={localAngle:0.0} cmd=({cmdVx:0.000},{cmdVy:0.000}) cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
|
||
"FleetCrabDbg");
|
||
}
|
||
yield return true;
|
||
}
|
||
|
||
if (_stopping)
|
||
stopReason = "stop";
|
||
|
||
self.MultiVehicleScriptVx = 0;
|
||
self.MultiVehicleScriptVy = 0;
|
||
self.MultiVehicleScriptVth = 0;
|
||
var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
|
||
while (!_stopping && DateTime.Now < settleEnd)
|
||
{
|
||
self.MultiVehicleScriptEnabled = true;
|
||
self.MultiVehicleScriptMode = 1;
|
||
yield return true;
|
||
}
|
||
|
||
Cleanup();
|
||
Hedingben.ToastText("车队蟹行完成", "FleetCrab");
|
||
DLog.Log($"DONE iter={iter} reason={stopReason}", "FleetCrabDbg");
|
||
}
|
||
}
|
||
|
||
[MovementTest(name = "车队联动-自动蟹行")]
|
||
public class FleetCrabWalkTest : MovementTest
|
||
{
|
||
private FleetCrabWalk _proc;
|
||
private DriveTask _task;
|
||
|
||
public override void Test()
|
||
{
|
||
_proc = new FleetCrabWalk
|
||
{
|
||
CrabAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
|
||
CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm,
|
||
CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed,
|
||
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
|
||
CorrectionGain = PilotDefinition.Conf.FleetCrabCorrectionGain,
|
||
CorrectionAngleThreshold = PilotDefinition.Conf.FleetCrabCorrectionAngleDeg,
|
||
CommandAccel = PilotDefinition.Conf.FleetCrabCommandAccel
|
||
};
|
||
_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()
|
||
{
|
||
}
|
||
}
|