Add Clumsy plugins and dual-vehicle fleet sync demo.

Restructure into DiffWheel/MultiWheel folders, add DiffWheelC and MultiWheelC with MovementTests, FleetRemote multi-vehicle manual sync, deploy templates, and documentation. Update dependency paths to D:\MDCS\Release and mirror motor feedback in MotorRoutine for simulation. Co-authored-by: Cursor <cursoragent@cursor.com>
2026-06-28 15:04:55 +08:00
parent e5aeae9c33
commit 235c6c70ac
40 changed files with 2750 additions and 99 deletions
@@ -0,0 +1,241 @@
+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;
+
+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();
+    }
+}
+
+// ===== 调用 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()
+    {
+    }
+}
+
+[MovementTest(name = "多舵轮-车队直线1m")]
+public class FleetStraightMovementTest : MovementTest
+{
+    public override void Test()
+    {
+        if (PilotDefinition.Conf.MultiVehicleMasterEndpoint != "/")
+        {
+            Hedingben.ToastText("仅主车可发起车队轨迹", "FleetGo");
+            return;
+        }
+
+        var pos = DetourInterface.getCartLocation();
+        var agv = new AGV();
+        agv.FleetGo((float)pos.x, (float)pos.y, 1, (float)pos.x + 1000f, (float)pos.y, 2, 10001, 0.2f);
+    }
+
+    public override void TestStop()
+    {
+    }
+}