update

.vs/ParkingRobot/v17/DocumentLayout.backup.json

@@ -0,0 +1,63 @@
+{
+  "Version": 1,
+  "WorkspaceRootPath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\",
+  "Documents": [],
+  "DocumentGroupContainers": [
+    {
+      "Orientation": 0,
+      "VerticalTabListWidth": 256,
+      "DocumentGroups": [
+        {
+          "DockedWidth": 200,
+          "SelectedChildIndex": -1,
+          "Children": [
+            {
+              "$type": "Bookmark",
+              "Name": "ST:132:0:{1fc202d4-d401-403c-9834-5b218574bb67}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:133:0:{1fc202d4-d401-403c-9834-5b218574bb67}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:134:0:{1fc202d4-d401-403c-9834-5b218574bb67}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:135:0:{1fc202d4-d401-403c-9834-5b218574bb67}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:136:0:{1fc202d4-d401-403c-9834-5b218574bb67}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:137:0:{1fc202d4-d401-403c-9834-5b218574bb67}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:0:0:{3ae79031-e1bc-11d0-8f78-00a0c9110057}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:129:0:{1fc202d4-d401-403c-9834-5b218574bb67}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:130:0:{1fc202d4-d401-403c-9834-5b218574bb67}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:131:0:{1fc202d4-d401-403c-9834-5b218574bb67}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:0:0:{1c64b9c2-e352-428e-a56d-0ace190b99a6}"
+            }
+          ]
+        }
+      ]
+    }
+  ]
+}

.vs/ParkingRobot/v17/DocumentLayout.json

@@ -0,0 +1,63 @@
+{
+  "Version": 1,
+  "WorkspaceRootPath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\",
+  "Documents": [],
+  "DocumentGroupContainers": [
+    {
+      "Orientation": 0,
+      "VerticalTabListWidth": 256,
+      "DocumentGroups": [
+        {
+          "DockedWidth": 200,
+          "SelectedChildIndex": -1,
+          "Children": [
+            {
+              "$type": "Bookmark",
+              "Name": "ST:132:0:{1fc202d4-d401-403c-9834-5b218574bb67}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:133:0:{1fc202d4-d401-403c-9834-5b218574bb67}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:134:0:{1fc202d4-d401-403c-9834-5b218574bb67}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:135:0:{1fc202d4-d401-403c-9834-5b218574bb67}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:136:0:{1fc202d4-d401-403c-9834-5b218574bb67}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:137:0:{1fc202d4-d401-403c-9834-5b218574bb67}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:0:0:{3ae79031-e1bc-11d0-8f78-00a0c9110057}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:129:0:{1fc202d4-d401-403c-9834-5b218574bb67}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:130:0:{1fc202d4-d401-403c-9834-5b218574bb67}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:131:0:{1fc202d4-d401-403c-9834-5b218574bb67}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:0:0:{1c64b9c2-e352-428e-a56d-0ace190b99a6}"
+            }
+          ]
+        }
+      ]
+    }
+  ]
+}

.vs/ProjectSettings.json

@@ -0,0 +1,3 @@
+{
+  "CurrentProjectSetting": null
+}

.vs/VSWorkspaceState.json

@@ -0,0 +1,6 @@
+{
+  "ExpandedNodes": [
+    ""
+  ],
+  "PreviewInSolutionExplorer": false
+}

ClumsyPilot/AGV.cs

@@ -0,0 +1,534 @@
+using ClumsyCore;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Sensors;
+using CommonUsage.Chassis;
+using CommonUsage.Mathematics;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.AgvInterfaces;
+using MDCSToolBox.Clumsy.Calibration;
+using MDCSToolBox.Clumsy.MotionControllers;
+using MDCSToolBox.Clumsy.Tracks;
+using MDCSToolBox.Commons.Controllers;
+using Newtonsoft.Json;
+using System;
+using System.Collections.Generic;
+using System.Net.Http;
+using System.Numerics;
+using System.Security.Cryptography;
+using System.Threading;
+using System.Threading.Tasks;
+using static ClumsyCore.DTools.Painter;
+
+namespace MultiWheelC
+{
+    public class SetLocationRes
+    {
+        public float x, y, th;
+        public int l_step;
+        public long tick;
+        public string error;
+    }
+    public class AGV : MultiWheelInterface
+    {
+        public override AbstractGeometricController GetController()
+        {
+            return new ChassisController().Get();
+        }
+
+        public override MultiWheelMagTracker GetMagController()
+        {
+            return new MultiWheelMagTracker();
+        }
+
+        public override NaiveMagnetController GetNaiveMagnetController()
+        {
+            return new NaiveMagnetController();
+        }
+
+        public void Sleep(float s)
+        {
+            new DriveTask(new Sleep() { Second = s }.Get()).Wait();
+        }
+
+        public void ControlChargePort(bool open)
+        {
+            DLog.Log($"call ControlChargePort({open})");
+            PilotDefinition.Self.OpenChargeByClumsy = open;
+        }
+
+        public void SwitchLidarArea(int area)
+        {
+            DLog.Log($"call SwitchLidarArea({area})");
+            PilotDefinition.Self.AreaChoose = area;
+        }
+
+        public void SwitchIoArea(int area)
+        {
+            if (area != -1)
+            {
+                PilotDefinition.Self.IOObstacleArea = area;
+            }
+        }
+        public void RotateToTarget(float target)
+        {
+            //if (!needrotate) return;
+            var dl = new DriveTask(new MultiWheelRotateInPlace()
+            {
+                AngleTarget = target,
+                PidparamsRead = () => new PIDParams()
+                {
+                    Kp = PilotDefinition.Conf.TireFollowingThkp,
+                    Ki = PilotDefinition.Conf.TireFollowingThki,
+                    Kd = PilotDefinition.Conf.TireFollowingThkd,
+                    DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone,
+                    SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec,
+                    OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh,
+                    MaxI = PilotDefinition.Conf.TireFollowingThMaxI,
+                }
+            }.Get());
+            dl.Wait();
+        }
+
+        //参数1：tireNum 需要钻过的轮胎对数量
+        //参数2：frontLidarDetect true:前雷达识别  false:后雷达识别
+        public void TireFollowing(int tireNum, bool frontLidarDetect, int srcId, int dstId)
+        {
+            while (!TryLock(dstId))
+            {
+                Thread.Sleep(50);
+            }
+            DLog.Log($"锁点{dstId}完成", "TireFollowing");
+            var lidarName = frontLidarDetect ? "前雷达" : "后雷达";
+            DLog.Log($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎", "TireFollowing");
+            if (tireNum != 1 && tireNum != 2)
+            {
+                DLog.Log($"TireNum必须是1或2 (当前输入:{tireNum})", "TireFollowing");
+                return;
+            }
+
+            if (PilotDefinition.Self.GhostMode)
+            {
+                while (!TryLock(dstId))
+                {
+                    Console.WriteLine("等待锁取货点中...");
+                    Thread.Sleep(200);
+                }
+                Console.WriteLine($"锁点{dstId}完成");
+                Thread.Sleep(1000);
+                Console.WriteLine($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎");
+                Thread.Sleep(1000);
+                Leave(srcId);
+                Console.WriteLine($"开始第一段盲走，此时释放预取货点{srcId}");
+                Thread.Sleep(2000);
+                //Leave(dstId);
+                //Console.WriteLine($"结束第一段盲走，此时释放取货点{dstId}");
+                Thread.Sleep(2000);
+                Console.WriteLine($"结束钻车动作");
+                return;
+            }
+
+            var detectors = new List<TireFollowing.DetectorDefinition>()
+            {
+                new TireFollowing.DetectorDefinition()
+                {
+                    DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, frontLidarDetect),
+                    StartGuessingX = frontLidarDetect ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX,
+                    StartGuessingY = 0,
+                    SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
+                    FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
+                    PathTransformation = new Tuple<float, float, float>(
+                    frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
+                    frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
+                    0),
+                    LeaveSrcFunction = Leave,
+                    SrcId = srcId,
+                    DstId = dstId,
+                },
+                new TireFollowing.DetectorDefinition()
+                {
+                    DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, frontLidarDetect),
+                    StartGuessingX = frontLidarDetect ? PilotDefinition.Conf.TireFollowingStage2GuessX : -PilotDefinition.Conf.TireFollowingStage2GuessX,
+                    StartGuessingY = 0,
+                    SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
+                    FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
+                    PathTransformation = new Tuple<float, float, float>(
+                    frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
+                    frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
+                    0)
+                },
+            };
+            DLog.Log($"钻胎为{tireNum}", "TireFollowing");
+            var following = new TireFollowing()
+            {
+                GetController = () => new ChassisController().Get(),
+                GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
+                GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
+                detectors = detectors,
+                SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
+                MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
+                TireNum = tireNum,
+                CarDirection = frontLidarDetect ? 0f : 180f,
+                WalkBlindTh = frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarWalkBlindTh : PilotDefinition.Conf.TireFollowingBackLidarWalkBlindTh,
+            };
+            var _dt = new DriveTask(following.Get());
+            _dt.Wait();
+            DLog.Log("钻车动作结束", "TireFollowing");
+        }
+
+        //离车一定是后雷达识别一个轮胎
+        public void LeaveCar(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
+        {
+            while (!TryLock(dstId))
+            {
+                Thread.Sleep(50);
+            }
+            DLog.Log($"锁点{dstId}完成", "TireFollowing");
+            DLog.Log($"开始钻车动作，通过后雷达识别结果钻1对轮胎", "TireFollowing");
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            chassis.SetOriginBias(0, 0, 0);
+            var following = new TireFollowing()
+            {
+                GetController = () => new ChassisController().Get(),
+                GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
+                GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
+                detectors = new List<TireFollowing.DetectorDefinition>()
+                {
+                    new TireFollowing.DetectorDefinition()
+                    {
+                        DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
+                        StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
+                        StartGuessingY = 0,
+                        SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingLeaveCarWalkBlindSwitchingDistance,
+                        FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
+                        PathTransformation = new Tuple<float, float, float>(
+                        PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
+                        PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
+                        0),
+                        LeaveSrcFunction = Leave,
+                        SrcId = srcId,
+                        DstId = dstId,
+                    },
+                },
+                CarDirection = 180f,
+                SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
+                MaxSpeed = 0.25f,
+                EnableHandover = true,
+                HandoverDistance = 200f,
+                HandoverSpeed = 0.3f,
+                WalkBlindTh = 0,
+                TireNum = 1
+            };
+
+            IEnumerable<bool> LeaveThenFollow()
+            {
+                foreach (var running in following.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
+                DLog.Log("离车TireFollowing结束，开始DstTracker", "TireFollowing");
+
+                foreach (var running in new DstTracker()
+                {
+                    Src = new Vector2(srcX, srcY),
+                    Dst = new Vector2(dstX, dstY),
+                    CarDirectionBias = 180f,
+                    InitialSendSpeed = 0.3f
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+
+                yield return false;
+            }
+
+            var _dt = new DriveTask(LeaveThenFollow());
+            _dt.Wait();
+            DLog.Log("离车动作1结束", "TireFollowing");
+        }
+
+        public void LineTracking(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
+        {
+            while (!TryLock(dstId))
+            {
+                Thread.Sleep(50);
+            }
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            chassis.SetOriginBias(0, 0, 0);
+            DLog.Log($"锁点{dstId}完成", "TireFollowing");
+            IEnumerable<bool> TrackThenFollow()
+            {
+                foreach (var running in new LineTracking()
+                {
+                    Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+                    LeaveSrcFunction = Leave,
+                    SrcId = srcId,
+                    EnableHandover = true,
+                    HandoverDistance = 200, 
+                    HandoverSpeed = 0.3f,
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
+                DLog.Log("离车LineTracking结束，开始DstTracker", "TireFollowing");
+                while (!TryLock(426))
+                {
+                    Thread.Sleep(20);
+                }
+                Leave(dstId);
+                DLog.Log($"释放锁点{dstId}完成", "TireFollowing");
+                foreach (var running in new DstTracker()
+                {
+                    Src = new Vector2(srcX, srcY),
+                    Dst = new Vector2(dstX, dstY),
+                    InitialSendSpeed = 0.3f
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                yield return false;
+            }
+            var _dt = new DriveTask(TrackThenFollow());
+            _dt.Wait();
+            DLog.Log("离车动作2结束", "TireFollowing");
+        }
+
+        //驱动器上使能
+        public void DriverAble()
+        {
+            var dl = new DriveTask(new DriverAble() { }.Get());
+            dl.Wait();
+            DLog.Log("驱动器上使能完成", "TireFollowing");
+        }
+
+        //驱动器下使能
+        public void DriverDisable()
+        {
+            var dl = new DriveTask(new DriverDisable() { }.Get());
+            dl.Wait();
+            DLog.Log("驱动器下使能完成", "TireFollowing");
+        }
+
+        // 夹抱：close 为 true 时关闭夹抱，否则打开夹抱。
+        public void ClamptoTarget(bool close)
+        {
+            if (PilotDefinition.Self.GhostMode)
+            {
+                Thread.Sleep(2000);
+                Console.WriteLine("夹抱完成");
+                return;
+            }
+            new DriveTask(new ClampToTarget()
+            {
+                LeftClampTarget = close ? PilotDefinition.Self.LeftArmUpperPos : PilotDefinition.Self.LeftArmLowerPos,
+                RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos
+            }.Get()).Wait();
+        }
+        // Fleet crab walk: convert scheduler src/dst into the same relative crab-walk path used by MovementTest.
+        public void FleetCrabWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
+            float speed)
+        {
+            var dx = dstX - srcX;
+            var dy = dstY - srcY;
+            var pathLength = (float)Math.Sqrt(dx * dx + dy * dy);
+
+            if (pathLength <= 1f)
+            {
+                DLog.Log("FleetCrabWalk abort: path length is too short.", "FleetCrabDbg");
+                return;
+            }
+
+            var self = PilotDefinition.Self;
+            if (!self.TryGetFleetCenterFromMembers(out var centerX, out var centerY, out var centerTh) &&
+                !self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
+            {
+                DLog.Log("FleetCrabWalk abort: failed to read fleet center.", "FleetCrabDbg");
+                Hedingben.ToastText("FleetCrab requires master localization", "FleetCrab");
+                return;
+            }
+
+            var pathAngle = (float)CommonMath.RoundTh((float)(Math.Atan2(dy, dx) / Math.PI * 180.0));
+            var crabAngle = (float)CommonMath.ThDiff(pathAngle, centerTh);
+            var targetBodyWorldHeading = (float)CommonMath.RoundTh(PilotDefinition.Conf.FleetCrabBodyWorldHeadingDeg);
+            var bodyToPathAngle = (float)CommonMath.ThDiff(pathAngle, targetBodyWorldHeading);
+            DLog.Log(
+                $"call FleetCrabWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
+                $"len={pathLength:0.0}, speed={speed:0.000}, pathAngle={pathAngle:0.0}, " +
+                $"center=({centerX:0},{centerY:0},{centerTh:0.0}), crabAngle={crabAngle:0.0}, " +
+                $"targetBodyWorld={targetBodyWorldHeading:0.0}, bodyToPath={bodyToPathAngle:0.0})",
+                "FleetCrabDbg");
+
+            if (dstId != -1)
+            {
+                while (!TryLock(dstId))
+                {
+                    Thread.Sleep(50);
+                }
+                DLog.Log($"锁点{dstId}完成", "FleetCrabDbg");
+            }
+
+            var action = new MultiWheelC.FleetCrabWalk
+            {
+                CrabAngleDeg = crabAngle,
+                BodyToPathAngleDeg = bodyToPathAngle,
+                CrabLengthMm = pathLength,
+                CrabSpeed = speed,
+                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
+            };
+
+            try
+            {
+                new DriveTask(action.Get()).Wait();
+            }
+            finally
+            {
+                if (srcId != -1)
+                {
+                    Leave(srcId);
+                    DLog.Log($"释放放车点{srcId}", "FleetCrabDbg");
+                }
+            }
+        }
+
+        public void FleetCurveWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
+            float speed, params float[] trackTypeInfo)
+        {
+            if (trackTypeInfo == null || trackTypeInfo.Length < 2)
+            {
+                DLog.Log("FleetCurveWalk abort: invalid trackTypeInfo, expected Bezier type info.", "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve invalid trackTypeInfo", "FleetCurve");
+                return;
+            }
+
+            var trackType = (int)trackTypeInfo[0];
+            if (trackType != 2)
+            {
+                DLog.Log($"FleetCurveWalk abort: unsupported trackType={trackType}, only Bezier(type=2) is supported.",
+                    "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve only supports Bezier trackType=2", "FleetCurve");
+                return;
+            }
+
+            var controlPointNum = (int)trackTypeInfo[1];
+            var expectedLength = 2 + controlPointNum * 2;
+            if (controlPointNum < 3 || trackTypeInfo.Length < expectedLength)
+            {
+                DLog.Log(
+                    $"FleetCurveWalk abort: invalid Bezier trackTypeInfo. controlPointNum={controlPointNum}, " +
+                    $"length={trackTypeInfo.Length}, expected>={expectedLength}.",
+                    "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve invalid Bezier trackTypeInfo", "FleetCurve");
+                return;
+            }
+
+            BezierTrack track;
+            try
+            {
+                track = ProcessTrackTypeInfo(srcX, srcY, dstX, dstY, trackTypeInfo) as BezierTrack;
+            }
+            catch (Exception ex)
+            {
+                DLog.Log($"FleetCurveWalk abort: failed to process trackTypeInfo. {ex.Message}", "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve failed to process track", "FleetCurve");
+                return;
+            }
+
+            if (track == null)
+            {
+                DLog.Log("FleetCurveWalk abort: ProcessTrackTypeInfo did not return BezierTrack.", "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve requires BezierTrack", "FleetCurve");
+                return;
+            }
+
+            track.Speed = speed;
+            track.CarDirectionBias = 0f;
+
+            DLog.Log(
+                $"call FleetCurveWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
+                $"speed={speed:0.000}, trackType={trackType}, controls={controlPointNum}, track={track.GetType().Name}, " +
+                $"carDirectionBias=0.0)",
+                "FleetCurveDbg");
+
+            if (dstId != -1)
+            {
+                while (!TryLock(dstId))
+                {
+                    Thread.Sleep(50);
+                }
+                DLog.Log($"閿佺偣{dstId}瀹屾垚", "FleetCurveDbg");
+            }
+
+            var action = new MultiWheelC.FleetCurveWalk
+            {
+                Track = track,
+                CurveSpeed = speed,
+                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
+            };
+
+            try
+            {
+                new DriveTask(action.Get()).Wait();
+            }
+            finally
+            {
+                if (srcId != -1)
+                {
+                    Leave(srcId);
+                    DLog.Log($"release srcId={srcId}", "FleetCurveDbg");
+                }
+            }
+        }
+        
+        public void ChangeAvoidanceDistance(float stopDistance, float slowDistance)
+        {
+            DLog.Log($"call ChangeAvoidanceDistance({stopDistance},{slowDistance})");
+            PilotDefinition.Self.SlowDistance = slowDistance;
+            PilotDefinition.Self.StopDistance = stopDistance;
+        }
+        public void ChangeAvoidanceParam(float length = -1, float width = -1)
+        {
+
+            PilotDefinition.Self.CarLength = length;
+            PilotDefinition.Self.CarWidth = width;
+
+        }
+        public void SetLocation(float x, float y, float th)
+        {
+            DLog.Log($"call SetLocation({x},{y},{th})");
+            Console.WriteLine($"call SetLocation({x},{y},{th})");
+            Queue(() =>
+            {
+                while (true)
+                {
+                    var str1 = new HttpClient()
+                        .GetStringAsync(
+                            $"http://127.0.0.1:4321/setLocation?x={x}&y={y}&th={th}")
+                        .Result;
+                    Thread.Sleep(500);
+                    Console.WriteLine($"SetLocation str={str1}");
+                    var setLocationRes = JsonConvert.DeserializeObject<SetLocationRes>(str1);
+                    Console.WriteLine(setLocationRes.l_step);
+                    if (setLocationRes != null && setLocationRes.l_step == 2) break;
+                }
+            });
+        }
+        public float baseSpeed = 0;
+    }
+}

ClumsyPilot/ChassisController.cs

@@ -0,0 +1,100 @@
+using System;
+using System.Numerics;
+using ClumsyCore;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using FundamentalLib;
+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;
+
+    private DateTime _sendMotionDbgLast = DateTime.MinValue;
+
+    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,
+
+            MultiVehicleSendMotion = (speed, frontTh, rearTh, idealPos, idealAngle) =>
+            {
+                var self = PilotDefinition.Self;
+                self.MultiVehicleAutoEnabled = true;
+                // A: 用固定锁对象（不再锁会被替换的字段引用）。
+                int fleetCnt;
+                lock (self.FleetLock)
+                    fleetCnt = self.MultiVehicleFleet.Count;
+
+                // 诊断（节流 ~300ms）：确认回调被调用、编队是否就绪、是否因数量不符提前 return（导致不下发速度）。
+                if ((DateTime.Now - _sendMotionDbgLast).TotalMilliseconds >= 300)
+                {
+                    _sendMotionDbgLast = DateTime.Now;
+                    DLog.Log(
+                        $"SENDMOTION speed={speed:0.000} fTh={frontTh:0.0} rTh={rearTh:0.0} " +
+                        $"ideal=({idealPos.X:0},{idealPos.Y:0},{idealAngle:0.0}) " +
+                        $"editCnt={fleetCnt}/{PilotDefinition.Conf.MultiVehicleFleetNum} " +
+                        $"earlyReturn={fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum}",
+                        "FleetCrabDbg");
+                }
+
+                if (fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum)
+                    return;
+
+                self.MultiVehicleAutoVx = speed;
+                self.MultiVehicleAutoFrontTh = frontTh;
+                self.MultiVehicleAutoRearTh = rearTh;
+                // D: 透传路径控制器算出的理想车队中心位姿（此前被丢弃），供各车按 layout 做前馈。
+                self.MultiVehicleAutoIdealX = idealPos.X;
+                self.MultiVehicleAutoIdealY = idealPos.Y;
+                self.MultiVehicleAutoIdealTh = idealAngle;
+                self.MultiVehicleAutoHasIdeal = true;
+                // B: 标记命令新鲜度。路径结束/早退/卡顿不再刷新此时刻 → 主车超时后清零速度，避免滑行。
+                self.MultiVehicleAutoCmdTime = DateTime.Now;
+            },
+
+            // G: 读取车队中心原子快照，避免跨线程读到撕裂的 x/y/th 组合。
+            MultiVehicleGetFleetPos = () =>
+            {
+                var snap = PilotDefinition.Self.GetFleetCenterSnapshot();
+                return new Location
+                {
+                    x = snap.X,
+                    y = snap.Y,
+                    th = snap.Th,
+                    l_step = 1,
+                    tick = DateTime.Now.Ticks
+                };
+            }
+        };
+    }
+}

ClumsyPilot/ClumsyPilot.csproj

@@ -0,0 +1,41 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+  <PropertyGroup>
+    <TargetFramework>netstandard2.0</TargetFramework>
+	<LangVersion>10</LangVersion>
+	<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
+	<AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
+	<OutputPath>..\..\build\Clumsy\</OutputPath>
+  </PropertyGroup>
+
+  <ItemGroup>
+    <PackageReference Include="Newtonsoft.Json" Version="13.0.4" />
+    <PackageReference Include="System.Numerics.Vectors" Version="4.6.1" />
+  </ItemGroup>
+
+  <ItemGroup>
+    <Reference Include="CommonUsage">
+      <HintPath>ref\CommonUsage.dll</HintPath>
+    </Reference>
+    <Reference Include="LessokajiWeaverUtilities">
+      <HintPath>ref\LessokajiWeaverUtilities.dll</HintPath>
+    </Reference>
+    <Reference Include="MDCSToolBox">
+      <HintPath>ref\MDCSToolBox.dll</HintPath>
+    </Reference>
+    <Reference Include="ClumsyCore">
+      <HintPath>ref\RefClumsyCore.dll</HintPath>
+    </Reference>
+    <Reference Include="ClumsyDance">
+      <HintPath>ref\RefClumsyDance.dll</HintPath>
+    </Reference>
+    <Reference Include="FundamentalLib">
+      <HintPath>ref\RefFundamentalLib.dll</HintPath>
+    </Reference>
+  </ItemGroup>
+	
+  <Target Name="PostBuild" AfterTargets="PostBuildEvent">
+	  <Exec Command="if not exist $(SolutionDir)build\Clumsy mkdir $(SolutionDir)build\Clumsy&#xD;&#xA;if not exist $(SolutionDir)build\Clumsy_AGV2 mkdir $(SolutionDir)build\Clumsy_AGV2&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.exe $(SolutionDir)build\Clumsy\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.deps.json $(SolutionDir)build\Clumsy\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.runtimeconfig.json $(SolutionDir)build\Clumsy\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\RefClumsyCore.dll $(SolutionDir)build\Clumsy\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\RefClumsyDance.dll $(SolutionDir)build\Clumsy\&#xD;&#xA;copy /Y D:\MDCS\Release\MDCSToolBox.dll $(SolutionDir)build\Clumsy\&#xD;&#xA;copy /Y D:\MDCS\Release\CommonUsage.dll $(SolutionDir)build\Clumsy\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.exe $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.deps.json $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.runtimeconfig.json $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\RefClumsyCore.dll $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\RefClumsyDance.dll $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y D:\MDCS\Release\MDCSToolBox.dll $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y D:\MDCS\Release\CommonUsage.dll $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y $(TargetDir)$(TargetName).dll $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y $(TargetDir)$(TargetName).pdb $(SolutionDir)build\Clumsy_AGV2\" />
+  </Target>
+	
+</Project>

ClumsyPilot/FleetCrabWalk.cs

@@ -0,0 +1,526 @@
+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;
+
+// ===== 车队联动-自动蟹行动作 =====
+// 以当前车队中心为起点，构造指定方向和长度的直线路径；
+// 执行侧直接写 MultiVehicleAuto...，由 TickMultiVehicle 自动分支统一下发。
+//
+// 控制思路参考 MDCSToolbox 几何控制器，但实现收在 MultiWheelC 内：
+// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度、横向偏差和车身目标朝向偏差；
+// 2) 根据横向偏差给前后 GCP 同向修正，根据车身目标朝向偏差给前后 GCP 反向修正；
+// 3) 根据终点距离减速，并发布 ideal fleet center 给从车做前馈。
+//
+// 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
+public class FleetCrabWalk : MovementDefinition
+{
+    /// <summary>路径方向相对启动时车队朝向的夹角(deg，逆时针为正)。</summary>
+    public float CrabAngleDeg = 45f;
+
+    /// <summary>路径方向相对车身目标朝向的夹角(deg，逆时针为正)。MovementTest 会设为 CrabAngleDeg，以保持启动时车身朝向。</summary>
+    public float BodyToPathAngleDeg = 45f;
+
+    /// <summary>路径长度(mm)。</summary>
+    public float CrabLengthMm = 2000f;
+
+    /// <summary>行驶速度(m/s)。</summary>
+    public float CrabSpeed = 0.2f;
+
+    /// <summary>速度命令加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
+    public float FleetCrabAccel = 0.2f;
+
+    /// <summary>预对齐后正式下发速度前 5 秒加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
+    public float FleetCrabStartAccel = 0.01f;
+
+    /// <summary>末端开始减速距离(mm)。</summary>
+    public float FleetCrabSlowDistance = 2000f;
+
+    /// <summary>完成距离(mm)，低于该剩余距离结束动作。</summary>
+    public float FleetCrabFinishDistance = 20f;
+
+    /// <summary>末端最低速度(m/s)。</summary>
+    public float FleetCrabFinishSpeed = 0.02f;
+
+    /// <summary>末端减速曲线指数。</summary>
+    public float FleetCrabSlowingPow = 0.8f;
+
+    /// <summary>前后 GCP 舵角修正上限(deg)。</summary>
+    public float GcpThetaThreshold = 95f;
+
+    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 Clamp(float value, float min, float max)
+    {
+        if (value < min) return min;
+        if (value > max) return max;
+        return value;
+    }
+
+    private static float ClampAbs(float value, float limit)
+    {
+        var absLimit = Math.Abs(limit);
+        if (absLimit <= 0) return value;
+        if (value > absLimit) return absLimit;
+        if (value < -absLimit) return -absLimit;
+        return value;
+    }
+
+    private static float Slew(float current, float target, float maxDelta)
+    {
+        if (maxDelta <= 0) return target;
+        if (target > current + maxDelta) return current + maxDelta;
+        if (target < current - maxDelta) return current - maxDelta;
+        return target;
+    }
+
+    private static float AverageAngle(float frontTh, float rearTh)
+    {
+        var diff = (float)CommonMath.ThDiff(frontTh, rearTh);
+        return (float)CommonMath.RoundTh(rearTh + diff / 2f);
+    }
+
+    private static void ResolveCrabDriveEquivalent(float speed, float rawFrontTh, float rawRearTh, float steerLimit,
+        out float driveSpeed, out float frontTh, out float rearTh, out bool reverseEquivalent, out float rawBaseTh)
+    {
+        var limit = Math.Min(179f, Math.Max(1f, Math.Abs(steerLimit)));
+        rawBaseTh = AverageAngle(rawFrontTh, rawRearTh);
+        driveSpeed = speed;
+        frontTh = rawFrontTh;
+        rearTh = rawRearTh;
+        reverseEquivalent = false;
+
+        if (rawBaseTh > limit)
+        {
+            frontTh = (float)CommonMath.RoundTh(frontTh - 180f);
+            rearTh = (float)CommonMath.RoundTh(rearTh - 180f);
+            driveSpeed = -driveSpeed;
+            reverseEquivalent = true;
+        }
+        else if (rawBaseTh < -limit)
+        {
+            frontTh = (float)CommonMath.RoundTh(frontTh + 180f);
+            rearTh = (float)CommonMath.RoundTh(rearTh + 180f);
+            driveSpeed = -driveSpeed;
+            reverseEquivalent = true;
+        }
+
+        frontTh = ClampAbs(frontTh, limit);
+        rearTh = ClampAbs(rearTh, limit);
+    }
+
+    private static float ProbeSpeed(float speed)
+    {
+        return Math.Abs(speed) > 1e-4f ? speed : 1f;
+    }
+
+    private static bool TryGetMotionYawSign(float frontTh, float rearTh, float driveSpeed, float controlRadius,
+        out float yawSign)
+    {
+        yawSign = 0f;
+        if (Math.Abs(CommonMath.ThDiff(frontTh, rearTh)) <= 1e-3f)
+            return false;
+
+        var radius = Math.Max(1f, Math.Abs(controlRadius));
+        Vector2 pFront = new(radius, 0), pRear = new(-radius, 0),
+            normFront = CommonMath.Transform2D(pFront, frontTh + 90f, Vector2.UnitX),
+            normRear = CommonMath.Transform2D(pRear, rearTh + 90f, Vector2.UnitX);
+        var (intersect, center) = CommonMath.TwoLinesIntersection(pFront, normFront, pRear, normRear);
+        if (!intersect)
+            return false;
+
+        // Match MultiWheelChassis.SendMotion: the tangent side is selected by
+        // rotCenter.Y > 1, and reverse-equivalent motion flips the yaw direction.
+        var tangentSign = center.Y > 1f ? 1f : -1f;
+        var speedSign = driveSpeed >= 0f ? 1f : -1f;
+        yawSign = speedSign * tangentSign;
+        return true;
+    }
+
+    private static float GetYawSplitSign(float baseTh, float speed, float steerLimit, float controlRadius)
+    {
+        const float probeDth = 1f;
+        ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + probeDth, baseTh - probeDth, steerLimit,
+            out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
+        return TryGetMotionYawSign(probeFrontTh, probeRearTh, probeSpeed, controlRadius, out var yawSign)
+            ? yawSign
+            : 1f;
+    }
+
+    private static float EstimateLateralVelocity(float bodyTh, float frontTh, float rearTh, float driveSpeed,
+        Vector2 pathLeft)
+    {
+        var motionTh = (float)CommonMath.RoundTh(bodyTh + AverageAngle(frontTh, rearTh));
+        var rad = motionTh / 180f * Math.PI;
+        var dir = new Vector2((float)Math.Cos(rad), (float)Math.Sin(rad));
+        if (driveSpeed < 0f)
+            dir = -dir;
+        return Vector2.Dot(dir, pathLeft);
+    }
+
+    private static float ScoreBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
+        float lateral, float biasProbe)
+    {
+        ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + biasProbe, baseTh + biasProbe, steerLimit,
+            out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
+        var lateralVelocity = EstimateLateralVelocity(bodyTh, probeFrontTh, probeRearTh, probeSpeed, pathLeft);
+        return -Math.Sign(lateral) * lateralVelocity;
+    }
+
+    private static float GetLateralBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
+        float lateral)
+    {
+        if (Math.Abs(lateral) <= 1e-3f)
+            return 1f;
+
+        const float probeBias = 1f;
+        var positiveScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, probeBias);
+        var negativeScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, -probeBias);
+        return positiveScore >= negativeScore ? 1f : -1f;
+    }
+
+    private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
+        out float centerTh, out string source)
+    {
+        if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
+        {
+            source = "fleet";
+            return true;
+        }
+
+        if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
+        {
+            source = "slam";
+            return true;
+        }
+
+        source = "none";
+        return false;
+    }
+
+    public override IEnumerable<bool> Get()
+    {
+        var self = PilotDefinition.Self;
+        var conf = PilotDefinition.Conf;
+        var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
+        if (chassis == null)
+        {
+            DLog.Log("ABORT: FleetCrabWalk requires MultiWheelChassis.", "FleetCrabDbg");
+            yield break;
+        }
+        _stopping = false;
+
+        DLog.Log(
+            $"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " +
+            $"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " +
+            $"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
+            $"autoFields=true pathMode=relative pathAngle={CrabAngleDeg:0.0} " +
+            $"bodyToPath={BodyToPathAngleDeg:0.0} gcpLimit={GcpThetaThreshold:0.0} " +
+            $"biasFac={conf.BiasFac:0.00} fleetCrabDthFac={conf.FleetCrabDthLinearFac: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 (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
+        {
+            DLog.Log("ABORT: TryGetFleetCenterFromSlam 返回 false (无定位)", "FleetCrabDbg");
+            Hedingben.ToastText("车队蟹行需要主车 Detour 定位", "FleetCrab");
+            yield break;
+        }
+        DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
+
+        DLog.Log($"CENTER_SOURCE source={initialCenterSource} center=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
+
+        var pathStart = new Vector2(x0, y0);
+        var pathLengthMm = CrabLengthMm;
+        var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
+        var dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
+        var targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
+        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}) pathMode=relative " +
+            $"src=({pathStart.X:0},{pathStart.Y:0}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
+            $"phi={phi:0.0} targetBody={targetBodyTh:0.0} " +
+            $"len={pathLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000} startAccel={FleetCrabStartAccel:0.000} accel={FleetCrabAccel:0.000} " +
+            $"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " +
+            $"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}",
+            "FleetCrabDbg");
+
+        var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
+        var controlRadius = Math.Max(1f, Math.Abs(conf.TestCarSyncDistance) / 2f);
+        ResolveCrabDriveEquivalent(0f, (float)CommonMath.ThDiff(phi, theta),
+            (float)CommonMath.ThDiff(phi, theta), gcpLimit, out _, out var holdFrontTh, out var holdRearTh,
+            out _, out _);
+        var warmStart = DateTime.Now;
+        var warmSeqBaseline = self.BeginFleetMotionWarmup();
+
+        self.MultiVehicleScriptEnabled = false;
+        self.MultiVehicleScriptMode = 0;
+        self.MultiVehicleScriptVx = 0;
+        self.MultiVehicleScriptVy = 0;
+        self.MultiVehicleScriptVth = 0;
+        self.MultiVehicleAutoEnabled = true;
+        self.MultiVehicleAutoVx = 0;
+        self.MultiVehicleAutoFrontTh = holdFrontTh;
+        self.MultiVehicleAutoRearTh = holdRearTh;
+        self.MultiVehicleAutoIdealX = pathStart.X;
+        self.MultiVehicleAutoIdealY = pathStart.Y;
+        self.MultiVehicleAutoIdealTh = targetBodyTh;
+        self.MultiVehicleAutoHasIdeal = true;
+        self.MultiVehicleAutoCmdTime = DateTime.Now;
+        self.PrimeMasterAutoFromSlam();
+        DLog.Log(
+            $"WARMUP auto fields enabled, waiting for fleet startup sync seqBase={warmSeqBaseline} " +
+            $"hold=({holdFrontTh:0.00},{holdRearTh:0.00})",
+            "FleetCrabDbg");
+
+        var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, conf.FleetCrabStartSyncTimeoutSec));
+        var warmIter = 0;
+        var warmReady = false;
+        var warmDetail = "";
+        while (!_stopping && DateTime.Now < warmEnd)
+        {
+            warmIter++;
+            self.MultiVehicleScriptEnabled = false;
+            self.MultiVehicleScriptMode = 0;
+            self.MultiVehicleAutoEnabled = true;
+            self.MultiVehicleAutoVx = 0;
+            self.MultiVehicleAutoFrontTh = holdFrontTh;
+            self.MultiVehicleAutoRearTh = holdRearTh;
+            self.MultiVehicleAutoIdealX = pathStart.X;
+            self.MultiVehicleAutoIdealY = pathStart.Y;
+            self.MultiVehicleAutoIdealTh = targetBodyTh;
+            self.MultiVehicleAutoHasIdeal = true;
+            self.MultiVehicleAutoCmdTime = DateTime.Now;
+            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} " +
+                    $"autoEn={self.MultiVehicleAutoEnabled} scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum} " +
+                    $"detail={warmDetail}",
+                    "FleetCrabDbg");
+            if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
+                    conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
+            {
+                warmReady = true;
+                DLog.Log(
+                    $"WARMUP done iter={warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt} detail={warmDetail}",
+                    "FleetCrabDbg");
+                break;
+            }
+            yield return true;
+        }
+        if (!warmReady)
+        {
+            DLog.Log($"WARMUP timeout: fleet startup sync failed, abort action. detail={warmDetail}",
+                "FleetCrabDbg");
+            Hedingben.ToastText("车队蟹行启动同步超时，已取消", "FleetCrab");
+            Cleanup();
+            yield break;
+        }
+
+        Hedingben.ToastText($"车队蟹行 路径{phi:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{pathLengthMm:0}mm", "FleetCrab");
+
+        if (warmReady && self.TryGetFleetCenterFromMembers(out var warmX, out var warmY, out var warmTh))
+        {
+            x0 = warmX;
+            y0 = warmY;
+            theta = warmTh;
+            pathStart = new Vector2(x0, y0);
+            phi = CommonMath.RoundTh(theta + CrabAngleDeg);
+            dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
+            targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
+            phiRad = phi / 180.0 * Math.PI;
+            pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
+            pathLeft = new Vector2(-pathDir.Y, pathDir.X);
+            self.MultiVehicleAutoIdealX = pathStart.X;
+            self.MultiVehicleAutoIdealY = pathStart.Y;
+            self.MultiVehicleAutoIdealTh = targetBodyTh;
+            self.MultiVehicleAutoCmdTime = DateTime.Now;
+            DLog.Log(
+                $"WARMUP_REBASE source=fleet center=({x0:0},{y0:0},{theta:0.0}) phi={phi:0.0} targetBody={targetBodyTh:0.0} dst=({dst.X:0},{dst.Y:0})",
+                "FleetCrabDbg");
+        }
+
+        var iter = 0;
+        var lastLog = DateTime.MinValue;
+        var finishDistance = Math.Max(0f, FleetCrabFinishDistance);
+        var slowDistance = Math.Max(finishDistance + 1f, FleetCrabSlowDistance);
+        var baseSpeed = Math.Abs(CrabSpeed);
+        var finishSpeed = Math.Min(baseSpeed, Math.Abs(FleetCrabFinishSpeed));
+        var slowingPow = Math.Max(0.01f, FleetCrabSlowingPow);
+        var accel = Math.Abs(FleetCrabAccel);
+        var startAccel = Math.Abs(FleetCrabStartAccel);
+        var cmdSpeed = 0f;
+        var lastTick = DateTime.Now;
+        var speedRampStart = DateTime.Now;
+        var stopReason = "done";
+
+        while (!_stopping)
+        {
+            iter++;
+
+            if (!TryGetControlFleetCenter(self, out var cx, out var cy, out var cth, out var centerSource))
+            {
+                stopReason = "fleet center invalid";
+                DLog.Log("ABORT: TryGetControlFleetCenter returned false during auto crab.", "FleetCrabDbg");
+                break;
+            }
+            var delta = new Vector2(cx - pathStart.X, cy - pathStart.Y);
+            var along = Vector2.Dot(delta, pathDir);
+            var lateral = Vector2.Dot(delta, pathLeft);
+            var remain = pathLengthMm - along;
+            if (remain <= finishDistance)
+                break;
+
+            var targetSpeed = baseSpeed;
+            var slowRatio = 1f;
+            if (remain < slowDistance)
+            {
+                slowRatio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow);
+                targetSpeed = slowRatio * (baseSpeed - finishSpeed) + finishSpeed;
+            }
+            var now = DateTime.Now;
+            var dt = Math.Max(0.001f, (float)(now - lastTick).TotalSeconds);
+            lastTick = now;
+            var rampElapsed = (now - speedRampStart).TotalSeconds;
+            var activeAccel = rampElapsed < 5.0 ? startAccel : accel;
+            var speed = activeAccel > 0 ? Slew(cmdSpeed, targetSpeed, activeAccel * dt) : targetSpeed;
+            cmdSpeed = speed;
+
+            var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
+            var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
+            var headingErrReverse = (float)CommonMath.ThDiff(cth, targetBodyTh);
+            var targetBodyToPath = (float)CommonMath.ThDiff(phi, targetBodyTh);
+            var rawBiasMagnitude = (float)(Math.Atan(conf.BiasFac * Math.Abs(lateral) / 1000f /
+                Math.Max(speed, 0.3f)) / Math.PI * 180.0);
+            var biasSign = GetLateralBiasSign(baseCrabTh, cth, speed, gcpLimit, pathLeft, lateral);
+            var rawBiasItem = rawBiasMagnitude * biasSign;
+            var biasItem = ClampAbs(rawBiasItem, conf.BiasThreshold);
+            var yawSplitSign = GetYawSplitSign(baseCrabTh + biasItem, speed, gcpLimit, controlRadius);
+            var rawDthItem = conf.FleetCrabDthLinearFac * headingErr * yawSplitSign;
+            var dthItem = ClampAbs(rawDthItem, conf.FleetCrabDthLinearThreshold);
+            var rawFrontTh = baseCrabTh + biasItem + dthItem;
+            var rawRearTh = baseCrabTh + biasItem - dthItem;
+            ResolveCrabDriveEquivalent(speed, rawFrontTh, rawRearTh, gcpLimit, out var driveSpeed,
+                out var frontTh, out var rearTh, out var reverseEquivalent, out var rawBaseTh);
+            holdFrontTh = frontTh;
+            holdRearTh = rearTh;
+            var idealAlong = Clamp(along, 0f, pathLengthMm);
+            var ideal = pathStart + pathDir * idealAlong;
+
+            self.MultiVehicleScriptEnabled = false;
+            self.MultiVehicleScriptMode = 0;
+            self.MultiVehicleScriptVx = 0;
+            self.MultiVehicleScriptVy = 0;
+            self.MultiVehicleScriptVth = 0;
+            self.MultiVehicleAutoEnabled = true;
+            self.MultiVehicleAutoVx = driveSpeed;
+            self.MultiVehicleAutoFrontTh = frontTh;
+            self.MultiVehicleAutoRearTh = rearTh;
+            self.MultiVehicleAutoIdealX = ideal.X;
+            self.MultiVehicleAutoIdealY = ideal.Y;
+            self.MultiVehicleAutoIdealTh = targetBodyTh;
+            self.MultiVehicleAutoHasIdeal = true;
+            self.MultiVehicleAutoCmdTime = DateTime.Now;
+
+            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} centerSrc={centerSource} 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} headingErr={headingErr:0.0} " +
+                    $"baseTh={baseCrabTh:0.0} bias={biasItem:0.0} dth={dthItem:0.0} " +
+                    $"slowRatio={slowRatio:0.000} targetV={targetSpeed:0.000} rampT={rampElapsed:0.0} accel={activeAccel:0.000} auto=(vx:{driveSpeed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " +
+                    $"ideal=({ideal.X:0},{ideal.Y:0},{targetBodyTh:0.0}) scriptEn={self.MultiVehicleScriptEnabled} " +
+                    $"cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
+                    "FleetCrabDbg");
+                DLog.Log(
+                    $"CTRL iter={iter} centerSrc:{centerSource} phi:{phi:0.00} targetBody:{targetBodyTh:0.00} startTheta:{theta:0.00} " +
+                    $"cth:{cth:0.00} crabAngle:{CrabAngleDeg:0.00} bodyToPathCfg:{BodyToPathAngleDeg:0.00} " +
+                    $"targetBodyToPath:{targetBodyToPath:0.00} bodyToPathNow:{baseCrabTh:0.00} " +
+                    $"headingErr(target-current):{headingErr:0.00} reverse(current-target):{headingErrReverse:0.00} yawSign:{yawSplitSign:0} " +
+                    $"fleetCrabDthFac:{conf.FleetCrabDthLinearFac:0.000} rawDth:{rawDthItem:0.00} dth:{dthItem:0.00} dthLimit:{conf.FleetCrabDthLinearThreshold:0.00} " +
+                    $"lateral:{lateral:0.0} biasFac:{conf.BiasFac:0.000} biasSign:{biasSign:0} rawBias:{rawBiasItem:0.00} bias:{biasItem:0.00} biasLimit:{conf.BiasThreshold:0.00} " +
+                    $"baseTh:{baseCrabTh:0.00} rawBase:{rawBaseTh:0.00} rawOut(f:{rawFrontTh:0.00},r:{rawRearTh:0.00}) " +
+                    $"out(f:{frontTh:0.00},r:{rearTh:0.00}) gcpLimit:{gcpLimit:0.00} revEq:{reverseEquivalent} " +
+                    $"speedRaw:{speed:0.000} speed:{driveSpeed:0.000} rampT:{rampElapsed:0.0} accel:{activeAccel:0.000} along:{along:0.0} remain:{remain:0.0} ideal=({ideal.X:0.0},{ideal.Y:0.0},{targetBodyTh:0.00})",
+                    "FleetCrabHeadingDbg");
+            }
+            yield return true;
+        }
+
+        if (_stopping)
+            stopReason = "stop";
+
+        self.MultiVehicleAutoVx = 0;
+        self.MultiVehicleAutoFrontTh = holdFrontTh;
+        self.MultiVehicleAutoRearTh = holdRearTh;
+        self.MultiVehicleAutoCmdTime = DateTime.Now;
+        DLog.Log(
+            $"STOP_HOLD iter={iter} reason={stopReason} hold=(fTh:{holdFrontTh:0.0},rTh:{holdRearTh:0.0}) cmdSpeed={cmdSpeed:0.000}",
+            "FleetCrabDbg");
+        var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
+        while (!_stopping && DateTime.Now < settleEnd)
+        {
+            self.MultiVehicleScriptEnabled = false;
+            self.MultiVehicleScriptMode = 0;
+            self.MultiVehicleAutoEnabled = true;
+            self.MultiVehicleAutoVx = 0;
+            self.MultiVehicleAutoFrontTh = holdFrontTh;
+            self.MultiVehicleAutoRearTh = holdRearTh;
+            self.MultiVehicleAutoCmdTime = DateTime.Now;
+            yield return true;
+        }
+
+        Cleanup();
+        Hedingben.ToastText("车队蟹行完成", "FleetCrab");
+        DLog.Log($"DONE iter={iter} reason={stopReason}", "FleetCrabDbg");
+    }
+}

ClumsyPilot/FleetCurveWalk.cs

@@ -0,0 +1,411 @@
+using System;
+using System.Collections.Generic;
+using System.Globalization;
+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 FleetCurveWalk : MovementDefinition
+{
+    public BezierTrack Track;
+    public List<Vector2> ControlPoints = new();
+    public float CurveSpeed = 0.2f;
+    public float CarDirectionBias = 0f;
+    public int BezierResolution = 100;
+    public float SlowDistance = 2000f;
+    public float FinishDistance = 20f;
+    public float FinishSpeed = 0.02f;
+    public float SlowingPow = 0.8f;
+    public float GcpThetaThreshold = 95f;
+    public float StartSyncTimeoutSec = 8f;
+
+    private bool _stopping;
+    private MultiWheelGeometricController _controller;
+    private MultiWheelChassis _chassis;
+    private bool _savedControlPoints;
+    private float _savedControlRadius;
+    private Vector2 _savedGcp0;
+    private Vector2 _savedGcp1;
+
+    public void Stop()
+    {
+        _stopping = true;
+        if (_controller != null)
+            _controller.BreakAndHold = true;
+        Cleanup();
+    }
+
+    public static bool TryParsePointList(string text, out List<Vector2> points, out string error)
+    {
+        points = new List<Vector2>();
+        error = "";
+        if (string.IsNullOrWhiteSpace(text))
+        {
+            error = "empty control point list";
+            return false;
+        }
+
+        var segments = text.Split(new[] { ';', '|' }, StringSplitOptions.RemoveEmptyEntries);
+        for (var i = 0; i < segments.Length; i++)
+        {
+            var pair = segments[i].Split(new[] { ',', ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
+            if (pair.Length != 2)
+            {
+                error = $"invalid point #{i + 1}: {segments[i]}";
+                return false;
+            }
+
+            if (!TryParseFloat(pair[0], out var x) || !TryParseFloat(pair[1], out var y))
+            {
+                error = $"invalid number in point #{i + 1}: {segments[i]}";
+                return false;
+            }
+            points.Add(new Vector2(x, y));
+        }
+
+        if (points.Count < 3)
+        {
+            error = "Bezier curve requires at least 3 control points";
+            return false;
+        }
+
+        return true;
+    }
+
+    public static List<Vector2> BuildRelativeControlPoints(Vector2 start, float startTh, List<Vector2> relativePoints)
+    {
+        var source = relativePoints ?? new List<Vector2>();
+        var normalized = new List<Vector2>();
+        if (source.Count == 0 || Vector2.Distance(source[0], Vector2.Zero) > 1f)
+            normalized.Add(Vector2.Zero);
+        for (var i = 0; i < source.Count; i++)
+            normalized.Add(source[i]);
+        if (normalized.Count < 2)
+            normalized.Add(new Vector2(1000f, 0f));
+        if (normalized.Count < 3)
+            normalized.Add(new Vector2(2000f, 0f));
+
+        var result = new List<Vector2>();
+        for (var i = 0; i < normalized.Count; i++)
+            result.Add(CommonMath.Transform2D(start, startTh, normalized[i]));
+        return result;
+    }
+
+    public static List<Vector2> BuildAgvControlPoints(float srcX, float srcY, float dstX, float dstY,
+        params float[] controlPointCoords)
+    {
+        var src = new Vector2(srcX, srcY);
+        var dst = new Vector2(dstX, dstY);
+        var result = new List<Vector2>();
+        if (controlPointCoords == null || controlPointCoords.Length == 0)
+        {
+            result.Add(src);
+            result.Add((src + dst) / 2f);
+            result.Add(dst);
+            return result;
+        }
+
+        if (controlPointCoords.Length % 2 != 0)
+            throw new ArgumentException("FleetCurve controlPointCoords must contain x,y pairs.");
+
+        var supplied = new List<Vector2>();
+        for (var i = 0; i < controlPointCoords.Length; i += 2)
+            supplied.Add(new Vector2(controlPointCoords[i], controlPointCoords[i + 1]));
+
+        if (supplied.Count >= 3 &&
+            Vector2.Distance(supplied[0], src) <= 10f &&
+            Vector2.Distance(supplied[supplied.Count - 1], dst) <= 10f)
+            return supplied;
+
+        result.Add(src);
+        for (var i = 0; i < supplied.Count; i++)
+            result.Add(supplied[i]);
+        result.Add(dst);
+        if (result.Count < 3)
+            result.Insert(1, (src + dst) / 2f);
+        return result;
+    }
+
+    private static bool TryParseFloat(string text, out float value)
+    {
+        return float.TryParse(text, NumberStyles.Float, CultureInfo.InvariantCulture, out value) ||
+               float.TryParse(text, out value);
+    }
+
+    private static float ClampAbs(float value, float limit)
+    {
+        var absLimit = Math.Abs(limit);
+        if (absLimit <= 0) return value;
+        if (value > absLimit) return absLimit;
+        if (value < -absLimit) return -absLimit;
+        return value;
+    }
+
+    private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
+        out float centerTh, out string source)
+    {
+        if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
+        {
+            source = "fleet";
+            return true;
+        }
+
+        if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
+        {
+            source = "slam";
+            return true;
+        }
+
+        source = "none";
+        return false;
+    }
+
+    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;
+        RestoreControlPointRadius();
+    }
+
+    private void ApplyFleetControlPointRadius(MultiWheelChassis chassis, float radius)
+    {
+        if (!_savedControlPoints)
+        {
+            _chassis = chassis;
+            _savedControlRadius = chassis.ControlPointRadius;
+            var gcps = chassis.GetGeometricControlPoints();
+            if (gcps.Count >= 2)
+            {
+                _savedGcp0 = gcps[0].Position;
+                _savedGcp1 = gcps[1].Position;
+            }
+            _savedControlPoints = true;
+        }
+
+        chassis.ControlPointRadius = radius;
+        var points = chassis.GetGeometricControlPoints();
+        if (points.Count >= 2)
+        {
+            points[0].Position = new Vector2(radius, 0);
+            points[1].Position = new Vector2(-radius, 0);
+        }
+    }
+
+    private void RestoreControlPointRadius()
+    {
+        if (!_savedControlPoints || _chassis == null)
+            return;
+
+        _chassis.ControlPointRadius = _savedControlRadius;
+        var points = _chassis.GetGeometricControlPoints();
+        if (points.Count >= 2)
+        {
+            points[0].Position = _savedGcp0;
+            points[1].Position = _savedGcp1;
+        }
+        _savedControlPoints = false;
+    }
+
+    private static void WriteWarmupAuto(PilotDefinition self, Vector2 idealPos, float idealTh,
+        float frontTh, float rearTh)
+    {
+        self.MultiVehicleScriptEnabled = false;
+        self.MultiVehicleScriptMode = 0;
+        self.MultiVehicleScriptVx = 0;
+        self.MultiVehicleScriptVy = 0;
+        self.MultiVehicleScriptVth = 0;
+        self.MultiVehicleAutoEnabled = true;
+        self.MultiVehicleAutoVx = 0;
+        self.MultiVehicleAutoFrontTh = frontTh;
+        self.MultiVehicleAutoRearTh = rearTh;
+        self.MultiVehicleAutoIdealX = idealPos.X;
+        self.MultiVehicleAutoIdealY = idealPos.Y;
+        self.MultiVehicleAutoIdealTh = idealTh;
+        self.MultiVehicleAutoHasIdeal = true;
+        self.MultiVehicleAutoCmdTime = DateTime.Now;
+    }
+
+    public override IEnumerable<bool> Get()
+    {
+        var self = PilotDefinition.Self;
+        var conf = PilotDefinition.Conf;
+        var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
+        _stopping = false;
+
+        if (chassis == null)
+        {
+            DLog.Log("ABORT: FleetCurveWalk requires MultiWheelChassis.", "FleetCurveDbg");
+            yield break;
+        }
+
+        if (conf.MultiVehicleMasterEndpoint != "/")
+        {
+            DLog.Log($"ABORT: FleetCurveWalk must run on master endpoint, endpoint={conf.MultiVehicleMasterEndpoint}",
+                "FleetCurveDbg");
+            Hedingben.ToastText("FleetCurve requires master vehicle", "FleetCurve");
+            yield break;
+        }
+
+        if (Track == null && (ControlPoints == null || ControlPoints.Count < 3))
+        {
+            DLog.Log("ABORT: FleetCurveWalk requires a BezierTrack or at least 3 control points.", "FleetCurveDbg");
+            Hedingben.ToastText("FleetCurve requires track or >=3 control points", "FleetCurve");
+            yield break;
+        }
+
+        if (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
+        {
+            DLog.Log("ABORT: FleetCurveWalk failed to read fleet center.", "FleetCurveDbg");
+            Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
+            yield break;
+        }
+
+        var baseSpeed = Math.Abs(CurveSpeed);
+        if (baseSpeed <= 1e-4f)
+        {
+            DLog.Log("ABORT: FleetCurveWalk speed is zero.", "FleetCurveDbg");
+            yield break;
+        }
+
+        var resolution = Math.Max(2, BezierResolution);
+        var speedFinish = Math.Min(baseSpeed, Math.Abs(FinishSpeed));
+        var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
+        var controlRadius = Math.Max(1f, Math.Abs(conf.TestCarSyncDistance) / 2f);
+
+        ApplyFleetControlPointRadius(chassis, controlRadius);
+
+        try
+        {
+            var track = Track;
+            var trackSource = "external";
+            if (track == null)
+            {
+                var points = new List<Vector2>(ControlPoints);
+                track = new BezierTrack(points, resolution);
+                trackSource = "controlPoints";
+            }
+            track.CarDirectionBias = CarDirectionBias;
+            track.Speed = baseSpeed;
+
+            var center = new Vector2(x0, y0);
+            var (idealPos, idealAngle, bias, pd) = track.QueryTangentPoint(center);
+            var carDirection = (float)CommonMath.ThDiff(theta, CarDirectionBias);
+            var holdTh = ClampAbs((float)CommonMath.ThDiff(idealAngle, carDirection), gcpLimit);
+            var targetBodyTh = (float)CommonMath.RoundTh(idealAngle + CarDirectionBias);
+
+            DLog.Log(
+                $"START center=({x0:0},{y0:0},{theta:0.0}) source={initialCenterSource} " +
+                $"track={track.GetType().Name} trackSource={trackSource} controls={ControlPoints?.Count ?? 0} " +
+                $"len={track.Length():0} speed={baseSpeed:0.000} bias={CarDirectionBias:0.0} " +
+                $"query=({idealPos.X:0},{idealPos.Y:0}) tangent={idealAngle:0.0} targetBody={targetBodyTh:0.0} " +
+                $"pathBias={bias:0.0} pd={pd:0.0} hold={holdTh:0.0} radius={controlRadius:0}",
+                "FleetCurveDbg");
+
+            var warmStart = DateTime.Now;
+            var warmSeqBaseline = self.BeginFleetMotionWarmup();
+            WriteWarmupAuto(self, idealPos, targetBodyTh, holdTh, holdTh);
+            self.PrimeMasterAutoFromSlam();
+
+            var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, StartSyncTimeoutSec));
+            var warmIter = 0;
+            var warmReady = false;
+            var warmDetail = "";
+            while (!_stopping && DateTime.Now < warmEnd)
+            {
+                warmIter++;
+                WriteWarmupAuto(self, idealPos, targetBodyTh, holdTh, holdTh);
+                self.PrimeMasterAutoFromSlam();
+
+                if (warmIter % 5 == 0)
+                {
+                    var snap = self.GetFleetCenterSnapshot();
+                    int cnt;
+                    lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
+                    DLog.Log(
+                        $"WARMUP#{warmIter} snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
+                        $"cnt={cnt}/{conf.MultiVehicleFleetNum} detail={warmDetail}",
+                        "FleetCurveDbg");
+                }
+
+                if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
+                        conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
+                {
+                    warmReady = true;
+                    DLog.Log($"WARMUP done iter={warmIter} detail={warmDetail}", "FleetCurveDbg");
+                    break;
+                }
+                yield return true;
+            }
+
+            if (!warmReady)
+            {
+                DLog.Log($"WARMUP timeout: fleet startup sync failed, abort curve action. detail={warmDetail}",
+                    "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve startup sync timeout", "FleetCurve");
+                Cleanup();
+                yield break;
+            }
+
+            _controller = new ChassisController { BaseSpeed = baseSpeed }.Get();
+            _controller.MultiVehicleSync = true;
+            _controller.BaseSpeed = baseSpeed;
+            _controller.SlowDistance = Math.Max(FinishDistance + 1f, SlowDistance);
+            _controller.FinishDistance = Math.Max(0f, FinishDistance);
+            _controller.FinishSpeed = speedFinish;
+            _controller.SlowingPow = Math.Max(0.01f, SlowingPow);
+            _controller.GcpThetaThreshold = gcpLimit;
+            _controller.AddTrack(track, "FleetCurve");
+
+            Hedingben.ToastText($"FleetCurve len {track.Length():0}mm speed {baseSpeed:0.00}", "FleetCurve");
+
+            foreach (var running in _controller.Track())
+            {
+                if (_stopping)
+                    break;
+                if (!running)
+                    break;
+                yield return true;
+            }
+
+            if (!_stopping)
+            {
+                self.MultiVehicleAutoVx = 0;
+                self.MultiVehicleAutoCmdTime = DateTime.Now;
+                var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
+                while (!_stopping && DateTime.Now < settleEnd)
+                {
+                    self.MultiVehicleAutoEnabled = true;
+                    self.MultiVehicleAutoVx = 0;
+                    self.MultiVehicleAutoCmdTime = DateTime.Now;
+                    yield return true;
+                }
+            }
+
+            DLog.Log($"DONE stopping={_stopping}", "FleetCurveDbg");
+        }
+        finally
+        {
+            Cleanup();
+            _controller = null;
+        }
+    }
+}

ClumsyPilot/MovementTests.TireFollowing.cs

@@ -0,0 +1,606 @@
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using ClumsyCore.Utilities;
+using ClumsyDance.ClumsyDance.Detectors;
+using ClumsyDance.ClumsyWalk.Detectors;
+using CommonUsage.Chassis;
+using CommonUsage.Mathematics;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.Calibration;
+using MDCSToolBox.Clumsy.Tracks;
+using MDCSToolBox.Commons.Controllers;
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Linq;
+using System.Numerics;
+using System.Security.Cryptography;
+using System.Threading;
+using LineSegment = ClumsyCore.Utilities.LineSegment;
+
+namespace MultiWheelC
+{
+    [MovementTest(name = "轮胎检测")]
+    public class TireDetect : MovementTest
+    {
+        public override void TestStop()
+        {
+            _running = false;
+        }
+
+        public override void Test()
+        {
+            _painter = UI.GetPainter("TwoLegDetectTest", false);
+            _painter.Clear();
+
+            var frontlidar = UI.GetInput("1是用前雷达识别，2是用后雷达识别");
+            var result = int.Parse(frontlidar.ToString());
+            var lastDetectX = result == 1 ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX;
+            var lastDetectY = 0f;
+            while (_running)
+            {
+                var ld = Detect(lastDetectX, SetFilters(lastDetectX, lastDetectY), result == 1 ? true : false);
+                if(ld == null)
+                {
+                    //Console.WriteLine("ld == null");
+                    continue;
+                }
+                _painter.Clear();
+                var center = (ld.Src + ld.Dst) / 2;
+                var distanceToCarOrigin = Vector2.Distance(Vector2.Zero, center);
+                var distanceLabelPos = center / 2;
+                _painter.DrawLine(Color.Cyan, Vector2.Zero, center, width: 2);
+                _painter.DrawText(Color.Yellow, $"{distanceToCarOrigin:F3}", distanceLabelPos.X, distanceLabelPos.Y);
+                lastDetectX = center.X;
+                lastDetectY = center.Y;
+                Thread.Sleep(100);
+            }
+        }
+
+        public static LineSegment Detect(float guessX, List<DetectFilter> filters, bool frontlidar)
+        {
+            return new Lidar2dDetect2LegTray()
+            {
+                BlobDist = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegBlobDist : PilotDefinition.Conf.TireBackTwoLegBlobDist,
+                BlobPtCount = frontlidar ? PilotDefinition.Conf.TireTwoLegBlobPtCount : PilotDefinition.Conf.TireTwoLegBlobPtCount,
+                BlobSize = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegBlobSize : PilotDefinition.Conf.TireBackTwoLegBlobSize,
+                CenterChange = Tuple.Create(frontlidar ? PilotDefinition.Conf.TireFrontTwoLegCenterChangeX : PilotDefinition.Conf.TireBackTwoLegCenterChangeX, 0f, 0f),
+                LegWidth = PilotDefinition.Conf.TireTwoLegWidth,
+                LegWidthErr = frontlidar ? PilotDefinition.Conf.TireTwoLegWidthErr : PilotDefinition.Conf.TireTwoLegWidthErr,
+                Padding = frontlidar ? PilotDefinition.Conf.TireFrontPadding : PilotDefinition.Conf.TireBackPadding,
+                PillarFindingScope = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegPillarFindingScope : PilotDefinition.Conf.TireBackTwoLegPillarFindingScope,
+                SgnDir = PilotDefinition.Conf.TwoLegSgnDir,
+            }.DetectWithGuess(frontlidar ? "frontlidar" : "leftlidar,rightlidar", new LineSegment(new Vector2(guessX, 0), Vector2.Zero),
+                guessCoordinateSystem: CoordinateSystem.Car2D, outCoordinateSystem: CoordinateSystem.Car2D, filters);
+        }
+
+        private List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
+        {
+            var painter = UI.GetPainter("GeneralFollowing.SetFilters", false);
+            painter.Clear();
+            painter.Clear(3000);
+
+            var box = new Vector2[]
+            {
+                new (guessCenterX - PilotDefinition.Conf.TireFilterLength / 2, guessCenterY - PilotDefinition.Conf.TireFilterWidth / 2),
+                new (guessCenterX + PilotDefinition.Conf.TireFilterLength / 2, guessCenterY - PilotDefinition.Conf.TireFilterWidth / 2),
+                new (guessCenterX + PilotDefinition.Conf.TireFilterLength / 2, guessCenterY + PilotDefinition.Conf.TireFilterWidth / 2),
+                new (guessCenterX - PilotDefinition.Conf.TireFilterLength / 2, guessCenterY + PilotDefinition.Conf.TireFilterWidth / 2),
+            };
+
+            for (var i = 0; i < box.Length; ++i)
+                painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
+
+            // PC filter in car coordinate frame
+            return new List<DetectFilter>()
+            {
+                new(CoordinateSystem.Car2D,
+                    p => LessMath.IsPointInPolygon4(
+                        box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))),
+            };
+        }
+
+        private Painter _painter;
+        private bool _running = true;
+    }
+
+    [MovementTest(name = "钻车测试")]
+    public class FollowTire : MovementTest
+    {
+        public override void TestStop()
+        {
+            _dt?.Stop();
+        }
+
+        public override void Test()
+        {
+            var front = UI.GetInput("1是用前雷达识别，2是用后雷达识别");
+            var result = int.Parse(front.ToString());
+            var lidarname = result == 1 ? "前雷达" : "后雷达";
+            DLog.Log($"开始钻车测试，用{lidarname}识别", "TireFollowing");
+
+            var following = new TireFollowing()
+            {
+                GetController = () => new ChassisController().Get(),
+                GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
+                GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
+                detectors = new List<TireFollowing.DetectorDefinition>()
+                {
+                    new TireFollowing.DetectorDefinition()
+                    {
+                        DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, result == 1 ? true : false),
+                        StartGuessingX = result == 1 ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX,
+                        StartGuessingY = 0,
+                        SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
+                        FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
+                        PathTransformation = new Tuple<float, float, float>(
+                        result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
+                        result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
+                        0)
+                    },
+                    new TireFollowing.DetectorDefinition()
+                    {
+                        DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, result == 1 ? true : false),
+                        StartGuessingX = result == 1 ? PilotDefinition.Conf.TireFollowingStage2GuessX : -PilotDefinition.Conf.TireFollowingStage2GuessX,
+                        StartGuessingY = 0,
+                        SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
+                        FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
+                        PathTransformation = new Tuple<float, float, float>(
+                        result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
+                        result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
+                        0)
+                    },
+                },
+                CarDirection = result == 1 ? 0f : 180f,
+                SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
+                MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
+                TireNum = PilotDefinition.Conf.TireFollowingTireNum,
+                WalkBlindTh = result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarWalkBlindTh : PilotDefinition.Conf.TireFollowingBackLidarWalkBlindTh,
+            };
+            _dt = new DriveTask(following.Get());
+            _dt.Wait();
+            DLog.Log($"结束钻车测试", "TireFollowing");
+        }
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "离车测试")]
+    public class LeaveCar : MovementTest
+    {
+        public override void TestStop()
+        {
+            _dt?.Stop();
+        }
+
+        public override void Test()
+        {
+            DLog.Log($"开始离车测试，用后雷达识别", "TireFollowing");
+            var following = new TireFollowing()
+            {
+                GetController = () => new ChassisController().Get(),
+                GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
+                GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
+                detectors = new List<TireFollowing.DetectorDefinition>()
+                {
+                    new TireFollowing.DetectorDefinition()
+                    {
+                        DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
+                        StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
+                        StartGuessingY = 0,
+                        SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingLeaveCarWalkBlindSwitchingDistance,
+                        FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
+                        PathTransformation = new Tuple<float, float, float>(
+                        PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
+                        PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
+                        0)
+                    },
+                },
+                CarDirection = 180f,
+                SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
+                MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
+                WalkBlindTh = 0,
+                TireNum = 1
+            };
+            _dt = new DriveTask(following.Get());
+            _dt.Wait();
+            DLog.Log($"结束离车测试", "TireFollowing");
+        }
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "抱夹关闭")]
+    public class ClampTest1 : MovementTest
+    {
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            PilotDefinition.Self.SpeedLeftArm = 0;
+            PilotDefinition.Self.SpeedRightArm = 0;
+        }
+
+        public override void Test()
+        {
+            _dt = new DriveTask(new ClampToTarget()
+            {
+                LeftClampTarget = PilotDefinition.Self.LeftArmUpperPos,
+                RightClampTarget = PilotDefinition.Self.RightArmUpperPos
+            }.Get());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "抱夹打开")]
+    public class ClampTest2 : MovementTest
+    {
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            PilotDefinition.Self.SpeedLeftArm = 0;
+            PilotDefinition.Self.SpeedRightArm = 0;
+        }
+
+        public override void Test()
+        {
+            _dt = new DriveTask(new ClampToTarget()
+            {
+                LeftClampTarget = PilotDefinition.Self.LeftArmLowerPos,
+                RightClampTarget = PilotDefinition.Self.RightArmLowerPos
+            }.Get());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "测试前进基于轮里程")]
+    public class LineTrackingTest : MovementTest
+    {
+        public override void TestStop()
+        {
+            _dt?.Stop();
+        }
+        public override void Test()
+        {
+            _dt = new DriveTask(new LineTracking()
+            {
+                Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+            }.Get());
+            _dt.Wait();
+        }
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "测试后退基于轮里程")]
+    public class ReverseLineTrackingTest : MovementTest
+    {
+        public override void TestStop()
+        {
+            _dt?.Stop();
+        }
+        public override void Test()
+        {
+            _dt = new DriveTask(new LineTracking()
+            {
+                Target = -PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+            }.Get());
+            _dt.Wait();
+        }
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "测试终点跟踪动作-前进")]
+    public class DstTrackerForward : MovementTest
+    {
+        public bool UseInteractivePick = true;
+        public float srcX;
+        public float srcY;
+        public float dstX;
+        public float dstY;
+        public float carDirectionBias = 0f;
+        private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
+
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            _painter?.Clear();
+        }
+        public override void Test()
+        {
+            var p1 = UI.GetPoint("point1");
+            var p2 = UI.GetPoint("point2");
+
+            _painter.Clear();
+            _dt = new DriveTask(new DstTracker()
+            {
+                Src = p1,
+                Dst = p2,
+                CarDirectionBias = carDirectionBias,
+            }.Get());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "测试终点跟踪动作-后退")]
+    public class DstTrackerhoutui : MovementTest
+    {
+        public bool UseInteractivePick = true;
+        public float srcX;
+        public float srcY;
+        public float dstX;
+        public float dstY;
+        public float carDirectionBias = 180f;
+        private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
+
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            _painter?.Clear();
+        }
+        public override void Test()
+        {
+            var p1 = UI.GetPoint("point1");
+            var p2 = UI.GetPoint("point2");
+
+            _painter.Clear();
+            _dt = new DriveTask(new DstTracker()
+            {
+                Src = p1,
+                Dst = p2,
+                CarDirectionBias = carDirectionBias,
+            }.Get());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "测试先直行再终点跟踪")]
+    public class LineTrackThenDstTrackerTest : MovementTest
+    {
+        public float carDirectionBias = 0f;
+        private readonly Painter _painter = UI.GetPainter("LineTrackThenDstTrackerTest");
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            _painter?.Clear();
+        }
+        public override void Test()
+        {
+            var src = UI.GetPoint("请在上位机选择起点(src)");
+            var dst = UI.GetPoint("请在上位机选择终点(dst)");
+            _painter.Clear();
+            _painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
+            _painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
+            _painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
+            _painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
+            _painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
+            IEnumerable<bool> TrackThenFollow()
+            {
+                foreach (var running in new LineTracking()
+                {
+                    Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+                    EnableHandover = true,
+                    HandoverDistance = 200f,
+                    HandoverSpeed = 0.3f,
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                foreach (var running in new DstTracker()
+                {
+                    Src = src,
+                    Dst = dst,
+                    CarDirectionBias = carDirectionBias,
+                    InitialSendSpeed = 0.3f
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                yield return false;
+            }
+            _dt = new DriveTask(TrackThenFollow());
+            _dt.Wait();
+        }
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "测试先离车再终点跟踪")]
+    public class LeaveCarThenDstTrackerTest : MovementTest
+    {
+        private readonly Painter _painter = UI.GetPainter("LeaveCarThenDstTrackerTest");
+
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            _painter?.Clear();
+        }
+
+        public override void Test()
+        {
+            var src = UI.GetPoint("请在上位机选择离车后起点(src)");
+            var dst = UI.GetPoint("请在上位机选择终点(dst)");
+
+            _painter.Clear();
+            _painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
+            _painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
+            _painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
+            _painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
+            _painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
+
+            IEnumerable<bool> LeaveThenFollow()
+            {
+                var following = new TireFollowing()
+                {
+                    GetController = () => new ChassisController().Get(),
+                    GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
+                    GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
+                    detectors = new List<TireFollowing.DetectorDefinition>()
+                    {
+                        new TireFollowing.DetectorDefinition()
+                        {
+                            DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
+                            StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
+                            StartGuessingY = 0,
+                            SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
+                            FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
+                            PathTransformation = new Tuple<float, float, float>(
+                                PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
+                                PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
+                                0),
+                        },
+                    },
+                    CarDirection = 180f,
+                    SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
+                    MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
+                    WalkBlindTh = 0,
+                    TireNum = 1
+                };
+
+                foreach (var running in following.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+
+                foreach (var running in new DstTracker()
+                {
+                    Src = src,
+                    Dst = dst,
+                    CarDirectionBias = 180f,
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+
+                yield return false;
+            }
+
+            _dt = new DriveTask(LeaveThenFollow());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
+
+    [MovementTest(name = "驱动器下使能测试")]
+    public class DriverDisableTest : MovementTest
+    {
+        public override void TestStop()
+        {
+            throw new NotImplementedException();
+        }
+
+        public override void Test()
+        {
+            new DriveTask(new DriverDisable(){ }.Get()).Wait();
+        }
+    }
+
+    [MovementTest(name = "驱动器复位测试")]
+    public class DriverAbleTest : MovementTest
+    {
+        public override void TestStop()
+        {
+            throw new NotImplementedException();
+        }
+
+        public override void Test()
+        {
+            new DriveTask(new DriverAble(){ }.Get()).Wait();
+        }
+    }
+
+    [MovementTest(name = "底盘旋转测试")]
+    public class RotateToAngleTest : MovementTest
+    {
+        public override void TestStop()
+        {
+            throw new NotImplementedException();
+        }
+
+        public override void Test()
+        {
+            var target = UI.GetInput("输入旋转角度：");
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            new DriveTask(new MultiWheelRotateInPlace()
+            {
+                AngleTarget = float.Parse(target),
+                PidparamsRead = () => new PIDParams()
+                {
+                    Kp = PilotDefinition.Conf.TireFollowingThkp,
+                    Ki = PilotDefinition.Conf.TireFollowingThki,
+                    Kd = PilotDefinition.Conf.TireFollowingThkd,
+                    DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone,
+                    SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec,
+                    OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh,
+                    MaxI = PilotDefinition.Conf.TireFollowingThMaxI,
+                }
+            }.Get()).Wait();
+        }
+    }
+
+    public class utils
+    {
+        public static List<(float x, float y, float th)> RemoveOutliers(List<(float x, float y, float th)> data, float threshold = 2.0f)
+        {
+            var means = CalculateMean(data);
+            var stdDevs = CalculateStandardDeviation(data, means);
+
+            return data.Where(point =>
+                Math.Abs(point.x - means.x) <= threshold * stdDevs.x &&
+                Math.Abs(point.y - means.y) <= threshold * stdDevs.y &&
+                AngularDistance(point.th, means.th) <= threshold * stdDevs.th
+            ).ToList();
+        }
+
+        public static (float x, float y, float th) CalculateMean(List<(float x, float y, float th)> data)
+        {
+            float meanX = data.Average(point => point.x);
+            float meanY = data.Average(point => point.y);
+            float sinSum = data.Sum(point => (float)Math.Sin(DegreeToRadian(point.th)));
+            float cosSum = data.Sum(point => (float)Math.Cos(DegreeToRadian(point.th)));
+            float meanTh = RadianToDegree((float)Math.Atan2(sinSum, cosSum));
+
+            return (meanX, meanY, meanTh);
+        }
+
+        public static (float x, float y, float th) CalculateStandardDeviation(List<(float x, float y, float th)> data, (float x, float y, float th) means)
+        {
+            float varianceX = data.Average(point => (point.x - means.x) * (point.x - means.x));
+            float varianceY = data.Average(point => (point.y - means.y) * (point.y - means.y));
+
+            // 计算角度的方差
+            float varianceTh = data.Average(point => AngularDistance(point.th, means.th) * AngularDistance(point.th, means.th));
+
+            return ((float)Math.Sqrt(varianceX), (float)Math.Sqrt(varianceY), (float)Math.Sqrt(varianceTh));
+        }
+
+        public static float DegreeToRadian(float degree)
+        {
+            return (float)(degree * Math.PI / 180.0);
+        }
+
+        public static float RadianToDegree(float radian)
+        {
+            return (float)(radian * 180.0 / Math.PI);
+        }
+
+        public static float AngularDistance(float angle1, float angle2)
+        {
+            return CommonMath.ThDiff(angle1, angle2);
+        }
+    }
+
+}
+
+

ClumsyPilot/MovementTests.TwoLegDetect.cs

@@ -0,0 +1,136 @@
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Linq;
+using System.Numerics;
+using System.Threading;
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Pilot;
+using ClumsyCore.Utilities;
+using ClumsyDance.ClumsyDance.Detectors;
+using ClumsyDance.ClumsyWalk.Detectors;
+using FundamentalLib;
+using LineSegment = ClumsyCore.Utilities.LineSegment;
+
+namespace MultiWheelC;
+
+/// <summary>
+/// 2腿检测：用单线激光雷达识别两腿托盘/轮胎，按上一帧结果作为下一帧猜测做闭环检测。
+/// 从 StandardMultiWheelLifter 移植；参数全部走 PilotConfig（Fields 面板），雷达选择改为配置项而非阻塞输入。
+/// </summary>
+[MovementTest(name = "多舵轮-2腿检测")]
+public class TwoLegDetect : MovementTest
+{
+    private Painter _painter;
+    private bool _running = true;
+
+    public override void TestStop() => _running = false;
+
+    public override void Test()
+    {
+        _running = true;
+        _painter = UI.GetPainter("MultiWheelTwoLegDetect", false);
+        _painter.Clear();
+
+        var lidar = PilotDefinition.Conf.TwoLegLidarName;
+        var lastDetectX = PilotDefinition.Conf.TwoLegGuessX;
+        var lastDetectY = 0f;
+
+        while (_running)
+        {
+            var ld = Detect(lidar, lastDetectX, SetFilters(lastDetectX, lastDetectY));
+            if (ld == null)
+            {
+                Thread.Sleep(100);
+                continue;
+            }
+
+            var center = (ld.Src + ld.Dst) / 2;
+            var distanceToCarOrigin = Vector2.Distance(Vector2.Zero, center);
+
+            _painter.Clear();
+            _painter.DrawLine(Color.Cyan, Vector2.Zero, center, width: 2);
+            _painter.DrawText(Color.Yellow, $"{distanceToCarOrigin:F1}", center.X / 2, center.Y / 2);
+
+            Hedingben.ToastText(
+                $"[Test检测] lidar:{lidar} guess x:{lastDetectX:F0} y:{lastDetectY:F0} | " +
+                $"中心 x:{center.X:F0} y:{center.Y:F0} dist:{distanceToCarOrigin:F0}",
+                "MultiWheelTwoLegDetect-test");
+
+            // 用本帧中心作为下一帧猜测，实现闭环跟踪
+            lastDetectX = center.X;
+            lastDetectY = center.Y;
+            Thread.Sleep(100);
+        }
+
+        _painter.Clear();
+    }
+
+    /// <summary>在车体坐标系下，按猜测位置检测两腿，返回连接两腿的线段（车体系）。</summary>
+    public static LineSegment Detect(string lidarName, float guessX, List<DetectFilter> filters)
+    {
+        var conf = PilotDefinition.Conf;
+#pragma warning disable CS0612, CS0618
+        var detector = new Lidar2dDetect2LegTray
+        {
+            BlobDist = conf.TwoLegBlobDist,
+            BlobPtCount = conf.TwoLegBlobPtCount,
+            BlobSize = conf.TwoLegBlobSize,
+            CenterChange = Tuple.Create(conf.TwoLegCenterChangeX, 0f, 0f),
+            LegWidth = conf.TwoLegWidth,
+            LegWidthErr = conf.TwoLegWidthErr,
+            Padding = conf.TwoLegPadding,
+            PillarFindingScope = conf.TwoLegPillarFindingScope,
+            SgnDir = conf.TwoLegSgnDir,
+        };
+#pragma warning restore CS0612, CS0618
+
+        var result = detector.DetectWithGuess(
+            lidarName,
+            new LineSegment(new Vector2(guessX, 0), Vector2.Zero),
+            guessCoordinateSystem: CoordinateSystem.Car2D,
+            outCoordinateSystem: CoordinateSystem.Car2D,
+            filters);
+        return ApplyOutputBias(result);
+    }
+
+    private static LineSegment ApplyOutputBias(LineSegment result)
+    {
+        if (result == null) return null;
+
+        var conf = PilotDefinition.Conf;
+        if (Math.Abs(conf.TwoLegOutputBiasX) < 1e-6f && Math.Abs(conf.TwoLegOutputBiasY) < 1e-6f)
+            return result;
+
+        var bias = new Vector2(conf.TwoLegOutputBiasX, conf.TwoLegOutputBiasY);
+        return new LineSegment(result.Src + bias, result.Dst + bias);
+    }
+
+    /// <summary>在猜测中心周围构造一个矩形 ROI，过滤掉框外点云，降低误识别。</summary>
+    public static List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
+    {
+        var conf = PilotDefinition.Conf;
+        var painter = UI.GetPainter("MultiWheelTwoLegDetect.Filter", false);
+        painter.Clear();
+
+        var box = new[]
+        {
+            new Vector2(guessCenterX - conf.TwoLegFilterLength / 2, guessCenterY - conf.TwoLegFilterWidth / 2),
+            new Vector2(guessCenterX + conf.TwoLegFilterLength / 2, guessCenterY - conf.TwoLegFilterWidth / 2),
+            new Vector2(guessCenterX + conf.TwoLegFilterLength / 2, guessCenterY + conf.TwoLegFilterWidth / 2),
+            new Vector2(guessCenterX - conf.TwoLegFilterLength / 2, guessCenterY + conf.TwoLegFilterWidth / 2),
+        };
+
+        for (var i = 0; i < box.Length; ++i)
+            painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
+
+        // 点云滤波在车体坐标系下进行
+        return new List<DetectFilter>
+        {
+            new(CoordinateSystem.Car2D,
+                p => LessMath.IsPointInPolygon4(
+                    box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))),
+        };
+    }
+}

ClumsyPilot/MovementTests.cs

@@ -0,0 +1,608 @@
+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 按此斜率爬升到巡航值，抑制起步抖动/队形骤偏。仅作用于起步加速，&lt;=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()
+    {
+    }
+}

ClumsyPilot/Movements.cs

@@ -0,0 +1,326 @@
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using ClumsyCore.Sensors;
+using ClumsyCore.Utilities;
+using ClumsyDance.ClumsyWalk.Detectors;
+using ClumsyDance.Sensors;
+using CommonUsage.Chassis;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.Calibration;
+using MDCSToolBox.Clumsy.HighLevelSecurity;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+using MDCSToolBox.Clumsy.Tracks;
+using MDCSToolBox.Commons;
+using MDCSToolBox.Commons.Controllers;
+using Newtonsoft.Json;
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Linq;
+using System.Net.Http;
+using System.Numerics;
+using System.Reflection;
+using System.Text;
+using System.Threading;
+using static ClumsyCore.DTools.Painter;
+
+namespace MultiWheelC
+{
+    public class MultiWheelRotateInPlace : MovementDefinition
+    {
+        /// <summary>
+        /// 旋转目标角度
+        /// </summary>
+        public float AngleTarget;
+
+        public float MaxSpeed;
+
+        public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
+
+        public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+
+        public Func<PIDParams> PidparamsRead = () => new PIDParams() { };
+
+        public PIDController thPid;
+
+        private static float RangeAngle(float theta)
+        {
+            return (float)(theta - Math.Round(theta / 360.0f) * 360);
+        }
+
+        public override IEnumerable<bool> Get()
+        {
+            var targetAngle = RangeAngle(AngleTarget);
+            var p = PidparamsRead();
+            thPid = new PIDController(ThetaReader, p.Kp);
+            thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone, p.OutputUpperThreshold, p.SpeedAccPerSec);
+
+            DateTime lastTime = DateTime.Now;
+            while (true)
+            {
+                var s = thPid.GetResponse(targetAngle, true);
+                Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
+                Chassis.SendXYThSpeed(0, 0, s);
+                lastTime = DateTime.Now;
+                if (thPid.IsArrived()) break;
+                yield return true;
+            }
+            Chassis.SendXYThSpeed(0, 0, 0);
+            Console.WriteLine($"final rotate to {targetAngle}");
+        }
+    }
+
+
+    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;
+        private PIDController leftpid, rightpid;
+
+        public override IEnumerable<bool> Get()
+        {
+            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 };
+
+            while (true)
+            {
+                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;
+                if (leftpid.IsArrived()) PilotDefinition.Self.SpeedLeftArm = 0;
+                if (rightpid.IsArrived()) PilotDefinition.Self.SpeedRightArm = 0;
+
+                if (leftpid.IsArrived() && rightpid.IsArrived()) break;
+                yield return true;
+            }
+
+            PilotDefinition.Self.SpeedLeftArm = 0;
+            PilotDefinition.Self.SpeedRightArm = 0;
+            Console.WriteLine($"left clamp to target:{LeftClampTarget}  right clamp to target:{RightClampTarget}");
+        }
+    }
+
+    public class Sleep : MovementDefinition
+    {
+        public float Second = 2;
+        public override IEnumerable<bool> Get()
+        {
+            var start = DateTime.Now;
+            while ((DateTime.Now-start).TotalSeconds<Second)
+            {
+                yield return true;
+                Thread.Sleep(1000);
+                Console.WriteLine("Sleep");
+            }
+
+            yield return false;
+        }
+    }
+
+    //直线行走基于detour
+    public class LineTracking1 : 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);
+        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 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}", "TireFollowing");
+            }
+            yield return false;
+        }
+    }
+
+    //在世界坐标系下，从路径起点追踪到终点并停车
+    public class DstTracker : MovementDefinition
+    {
+        public Vector2 Src;
+        public Vector2 Dst;
+        public float CarDirectionBias = 0f;
+        public Painter Painter = UI.GetPainter("DstTracker");
+
+        public float InitialSendSpeed = 0;
+
+        public override IEnumerable<bool> Get()
+        {
+            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().Get();
+
+           if (InitialSendSpeed != 0)
+            {
+                tracker.SkipInitialRotate = true;
+                tracker.InitialSendSpeed = InitialSendSpeed;
+            }
+
+            var linePath = new LineTrack(Src, Dst) { CarDirectionBias = CarDirectionBias, Speed = PilotDefinition.Conf.DstTrackerMaxSpeed };
+            tracker.AddTrack(linePath);
+            var task = new DriveTask(tracker.Track());
+            task.Wait();
+
+            // 到点后兜底停车
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            chassis.SendXYThSpeed(0f, 0f, 0f);
+            yield return false;
+        }
+    }
+
+    //直线行走基于轮里程
+    public class LineTracking : MovementDefinition
+    {
+        public float Target;
+        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 = null;
+        private PIDController pid;
+        // 末段衔接：接近目标后不再让 PID 把速度降到 0，保留一个接力速度给后续动作接管
+        public bool EnableHandover = false;
+        public float HandoverDistance = 80f; // mm
+        public float HandoverSpeed = 0.15f;  // m/s
+
+        public override IEnumerable<bool> Get()
+        {
+
+            pid = new PIDController(() =>
+                (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+                Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
+            { SpeedAccPerSec = MaxSpeed / 2f };
+
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            //chassis.SetOriginBias(0, 0, 0);
+            DLog.Log($"直线行驶距离:{Target}", "TireFollowing");
+            while (true)
+            {
+                var current = (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2;
+                var remain = Target - current;
+                if (EnableHandover && Math.Abs(remain) <= Math.Max(1f, HandoverDistance))
+                {
+                    var handoverSign = Math.Sign(remain);
+                    if (handoverSign == 0) handoverSign = 1;
+                    var handoverSpeed = Math.Abs(HandoverSpeed) * handoverSign;
+                    Console.WriteLine($"handover speed: {handoverSpeed:F3}, remain: {remain:F2}");
+                    chassis.SendXYThSpeed(handoverSpeed, 0, 0);
+                    // 保留一拍接力速度，让后续 DstTracker 无缝接管
+                    yield return true;
+                    break;
+                }
+                var speed = pid.GetResponse(Target);
+                Console.WriteLine($"output: {speed}  current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
+                
+                chassis.SendXYThSpeed(speed, 0, 0);
+
+                if (pid.IsArrived()) break;
+                yield return true;
+            }
+            if (SrcId != -1 && LeaveSrcFunction != null)
+            {
+                LeaveSrcFunction(SrcId);
+                DLog.Log($"释放放车点{SrcId}", "TireFollowing");
+            }
+            yield return false;
+        }
+    }
+
+    public class DriverAble : MovementDefinition
+    {
+        public int WaitTimeoutMs = 2000;
+        public int PollIntervalMs = 50;
+
+        public override IEnumerable<bool> Get()
+        {
+            Console.WriteLine("驱动器上使能");
+            PilotDefinition.Self.ResetFromC = true;
+
+            var start = DateTime.Now;
+            var timeoutMs = Math.Max(0, WaitTimeoutMs);
+            var pollMs = Math.Max(1, PollIntervalMs);
+            var success = PilotDefinition.Self.WheelAbleState;
+            while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs)
+            {
+                Thread.Sleep(pollMs);
+                success = PilotDefinition.Self.WheelAbleState;
+                if (!success) yield return true;
+            }
+
+            PilotDefinition.Self.ResetFromC = false;
+            if (success)
+                Console.WriteLine($"驱动器上使能完成，WheelAbleState={PilotDefinition.Self.WheelAbleState}");
+            else
+                Console.WriteLine($"驱动器上使能超时，WheelAbleState={PilotDefinition.Self.WheelAbleState}，等待{timeoutMs}ms");
+            yield return false;
+        }
+    }
+
+    public class DriverDisable : MovementDefinition
+    {
+        public int WaitTimeoutMs = 3000;
+        public int PollIntervalMs = 20;
+
+        public override IEnumerable<bool> Get()
+        {
+            Console.WriteLine("驱动器下使能");
+            PilotDefinition.Self.DisableFromC = true;
+
+            var start = DateTime.Now;
+            var timeoutMs = Math.Max(0, WaitTimeoutMs);
+            var pollMs = Math.Max(1, PollIntervalMs);
+            var success = !PilotDefinition.Self.WheelAbleState;
+            while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs)
+            {
+                Thread.Sleep(pollMs);
+                success = !PilotDefinition.Self.WheelAbleState;
+                if (!success) yield return true;
+            }
+
+            PilotDefinition.Self.DisableFromC = false;
+            if (success)
+                Console.WriteLine($"驱动器下使能完成，WheelAbleState={PilotDefinition.Self.WheelAbleState}");
+            else
+                Console.WriteLine($"驱动器下使能超时，WheelAbleState={PilotDefinition.Self.WheelAbleState}，等待{timeoutMs}ms");
+            yield return false;
+        }
+    }
+}

ClumsyPilot/PilotConfig.cs

@@ -0,0 +1,334 @@
+using ClumsyCore;
+using MDCSToolBox.Clumsy.Pilot.MultiWheel;
+using Newtonsoft.Json;
+
+namespace MultiWheelC;
+
+public class PilotConfig : MultiWheelPilotConfig
+{
+    [FieldMember(desc = "[sync] steering angle acceleration(deg/s^2)")] public float SyncThAccPerSec = 30f;
+    [FieldMember(desc = "[sync] fleet member distance(mm)")] public float TestCarSyncDistance = 2400f;
+    [FieldMember(desc = "[sync] fleet layout bias angle(deg)")] public float TestCarSyncTh = 0f;
+    // Fleet manual remote IO values are normalized joystick ratios. Keep all speed/angle scaling here.
+    [FieldMember(desc = "[sync] fleet manual max linear speed(m/s)")] public float FleetManualMaxSpeed = 0.3f;
+    [FieldMember(desc = "[sync] fleet manual normal-mode full-stick steering angle(deg)")] public float FleetManualMaxSteerAngleDeg = 45f;
+    [FieldMember(desc = "[sync] fleet manual crab-mode full-stick steering angle(deg)")] public float FleetManualMaxCrabAngleDeg = 60f;
+    [FieldMember(desc = "[sync] fleet manual rotate-mode full-stick angular speed(deg/s)")] public float FleetManualMaxRotateOmegaDegPerSec = 45f;
+    [FieldMember(desc = "[sync] 蟹行舵角上限(deg，应与Medulla舵轮角度限制匹配，默认120)")] public float MultiVehicleCrabSteerLimitDeg = 120f;
+    [FieldMember(desc = "[sync] 检测中心偏移(mm)")] public float DeltaDetectCenter = 350f;
+    // 仅控制"车队内姿态纠正"(POS 补偿)是否使用 Detour 的 SLAM 位姿，不影响"整个车队姿态的计算"。
+    // 默认 false：定位不参与车队内姿态纠正（各车按编队几何/互识别保持队形，不做 SLAM 逐车纠偏）。
+    // 为 true：额外用 getCartLocation() 反推每台车相对编队中心的偏差并做 POS 补偿。
+    // 注意：无论该开关如何，自动模式下整队姿态(反推/广播车队中心、SLAM 间距、自动安全门)始终依赖 Detour 全局定位；
+    // 主车自动模式必调用 getCartLocation()，若无有效全局定位该调用会阻塞 → 联动线程阻塞不下发速度（安全停车）。
+    [FieldMember(desc = "[sync] 定位是否参与车队内姿态纠正(不影响整队姿态计算)")] public bool MultiVehicleSyncUseDetour = false;
+    // 手动外部遥控联动默认只走 2 腿检测/几何同步，避免 Detour getCartLocation 阻塞导致遥控和检测可视化变慢。
+    [FieldMember(desc = "[sync] 手动联动是否启用定位姿态纠正(默认关闭)")] public bool MultiVehicleManualUseDetourCorrection = false;
+
+    [FieldMember(desc = "多车联动：总车数")] public int MultiVehicleFleetNum = 2;
+    [FieldMember(desc = "联动线程周期(ms)")] public int MultiVehicleSyncInterval = 50;
+    [FieldMember(desc = "多车联动：主车端点 ip:port，/ 表示本车为主车")] public string MultiVehicleMasterEndpoint = "/";
+    [FieldMember(desc = "多车联动：本车同步 IP")] public string SimpleIp = "127.0.0.1";
+
+    [FieldMember(desc = "多车联动：本车回连端点 ip:port，供主车 notify 回连，空=127.0.0.1:本车port")] public string MultiVehicleSelfEndpoint = "";
+
+    [JsonProperty("MultiVehicleMasterIp")]
+    private string LegacyMasterIpSetter
+    {
+        set
+        {
+            if (string.IsNullOrEmpty(value) || value == "/") return;
+            if (MultiVehicleMasterEndpoint == "/")
+                MultiVehicleMasterEndpoint = value.Contains(":") ? value : $"{value}:8008";
+        }
+    }
+
+    [FieldMember(desc = "多车联动：启用互识别纠正")] public bool MultiVehicleUseDetect = false;
+
+    // B: 自动速度命令新鲜度(ms)。主车超过此时长未从路径控制器收到新速度命令(路径结束/早退/卡顿)，
+    //    即视为失效并清零下发速度，避免车队按末速度滑行。0 表示自动取 max(200, interval*4)。
+    [FieldMember(desc = "多车联动：自动速度命令超时(ms，0=auto)")] public int MultiVehicleAutoCmdTimeoutMs = 0;
+    // C: fleet 成员存活 TTL(ms)。主车剔除超过此时长未 register/刷新的从车；编队就绪要求所有成员新鲜。
+    //    0 表示自动取 max(500, interval*6)。
+    [FieldMember(desc = "多车联动：成员存活TTL(ms，0=auto)")] public int MultiVehicleMemberTtlMs = 0;
+    // D: 自动模式下用主车路径控制器的理想车队中心(idealPos/idealAngle)作为各车 layout 目标，
+    //    弧线路径上做 per-car 前馈而非仅共用 frontTh/rearTh 事后纠偏。
+    [FieldMember(desc = "多车联动：自动模式按理想中心前馈(弧线)")] public bool MultiVehicleAutoUseIdealCenter = true;
+    // H: 自动模式必须有有效车队中心(SLAM 可反推)，全程定位丢失时停车，避免纯 SLAM 下盲跑。
+    [FieldMember(desc = "多车联动：自动模式要求有效车队中心")] public bool MultiVehicleAutoRequireFleetCenter = true;
+    [FieldMember(desc = "多车联动：SLAM X补偿系数")] public float MultiVehiclePosBiasXFac = 0.5f;
+    [FieldMember(desc = "多车联动：SLAM Y补偿系数")] public float MultiVehiclePosBiasYFac = 0.5f;
+    [FieldMember(desc = "多车联动：SLAM Th补偿系数")] public float MultiVehiclePosBiasThFac = 0.5f;
+    [FieldMember(desc = "多车联动：X补偿阈值(mm)")] public float MultiVehiclePosBiasXThreshold = 50f;
+    [FieldMember(desc = "多车联动：Y补偿阈值(mm)")] public float MultiVehiclePosBiasYThreshold = 50f;
+    [FieldMember(desc = "多车联动：Th补偿阈值(deg)")] public float MultiVehiclePosBiasThThreshold = 5f;
+
+    [FieldMember(desc = "多车联动：互识别 X补偿系数")] public float MultiVehicleDetectBiasXFac = 0.5f;
+    [FieldMember(desc = "多车联动：互识别 Y补偿系数")] public float MultiVehicleDetectBiasYFac = 0.5f;
+    [FieldMember(desc = "多车联动：互识别 Th补偿系数")] public float MultiVehicleDetectBiasThFac = 0.5f;
+    [FieldMember(desc = "多车联动：互识别 X补偿阈值(mm)")] public float MultiVehicleDetectBiasXThreshold = 50f;
+    [FieldMember(desc = "多车联动：互识别 Y补偿阈值(mm)")] public float MultiVehicleDetectBiasYThreshold = 50f;
+    [FieldMember(desc = "多车联动：互识别 Th补偿阈值(deg)")] public float MultiVehicleDetectBiasThThreshold = 5f;
+
+    // 原地旋转(mode2)闭环纠偏(PI)：把"本车应移动到的位置(dx,dy,mm)/应转角(dth,deg)"作为误差，
+    // 用 PI 控制器换算成车体系修正速度叠加到绕队心旋转上。纯 P 对抗恒定横向滑移扰动有稳态残差，
+    // 加积分项把稳态误差拉到 0；积分带限幅(抗 windup)，总输出限幅在 Max 内防过冲/振荡。
+    // Fac=比例增益(mm/s per mm、deg/s per deg)；IFac=积分增益(mm/s per mm·s、deg/s per deg·s)；Max=总输出上限。
+    [FieldMember(desc = "原地旋转纠偏：平移比例增益P(mm/s per mm)")] public float MultiVehicleRotateCompXyFac = 1.2f;
+    [FieldMember(desc = "原地旋转纠偏：平移积分增益I(mm/s per mm·s)")] public float MultiVehicleRotateCompXyIFac = 0.8f;
+    [FieldMember(desc = "原地旋转纠偏：平移速度上限(mm/s)")] public float MultiVehicleRotateCompXyMax = 150f;
+    [FieldMember(desc = "原地旋转纠偏：转向比例增益P(deg/s per deg)")] public float MultiVehicleRotateCompThFac = 0.8f;
+    [FieldMember(desc = "原地旋转纠偏：转向积分增益I(deg/s per deg·s)")] public float MultiVehicleRotateCompThIFac = 0.8f;
+    [FieldMember(desc = "原地旋转纠偏：转向速度上限(deg/s)")] public float MultiVehicleRotateCompThMax = 15f;
+    // 仅当车队实际被指令旋转(|fleetOmega|超过此阈值)时才运行纠偏 PI；否则清零并复位积分，
+    // 避免松开摇杆后积分残留持续驱动车辆"自行旋转停不下来"。
+    [FieldMember(desc = "原地旋转纠偏：生效的最小角速度阈值(deg/s)")] public float MultiVehicleRotateActiveOmega = 0.5f;
+    // 安全网：每轮纠偏速度幅值 <= 该比例 * 本轮旋转切向速度，限制合速度相对纯切向的最大偏角。
+    // 旧配置若仍为 <0，运行时按安全默认 0.10 处理；确需放宽时可在主车显式调大并同步给从车。
+    [FieldMember(desc = "原地旋转纠偏：纠偏/旋转切向比例硬上限，<0使用安全默认0.10")] public float MultiVehicleRotateCompTangentFrac = 0.10f;
+
+    [FieldMember(desc = "单车同步 xy 精度(mm)")] public float SingleCarSyncPrecisionXy = 10f;
+    [FieldMember(desc = "单车同步 th 精度(deg)")] public float SingleCarSyncPrecisionTh = 0.2f;
+
+    [FieldMember(desc = "Playground WebAPI 基地址")]
+    public string PlaygroundWebApiUrl = "http://localhost:18090";
+
+    [FieldMember(desc = "MultiVehicle rotate pose WebAPI diagnostics (simulation only)")]
+    public bool MultiVehicleRotatePoseWebApiDiagEnabled = false;
+
+    [FieldMember(desc = "Playground 小车名称（场景 robots[].name）")]
+    public string PlaygroundRobotName = "agv_multi_1";
+
+    [FieldMember(desc = "Playground 邻车名称（仅主车用于原地旋转位姿诊断）")]
+    public string PlaygroundNeighborRobotName = "agv_multi_2";
+
+    [FieldMember(desc = "WebAPI 平移测试：平移距离(mm)")]
+    public float WebApiTranslateMm = 100f;
+
+    [FieldMember(desc = "WebAPI 旋转测试：旋转角度(deg)")]
+    public float WebApiRotateDeg = 5f;
+
+    [FieldMember(desc = "原地旋转：目标朝向(世界坐标系, deg)")]
+    public float InPlaceRotateTargetWorldDeg = 90f;
+
+    [FieldMember(desc = "原地旋转：旋转角速度(deg/s)")]
+    public float InPlaceRotateSpeed = 30f;
+
+    [FieldMember(desc = "原地旋转：到位角度精度(deg)")]
+    public float InPlaceRotateArriveDeg = 1f;
+
+    [FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")]
+    public float InPlaceRotateWheelAlignDeg = 2f;
+
+    [FieldMember(desc = "原地旋转：旋转过程中舵轮偏差重对齐阈值(deg)")]
+    public float InPlaceRotateActiveWheelAlignDeg = 10f;
+
+    // ===== 车队联动-原地旋转动作（FleetRotateInPlace / 对应 FleetRemote 原地旋转模式）=====
+    // 通过 Clumsy 内部脚本字段驱动 TickMultiVehicle 的 mode2 旋转（绕车队中心 + PI 纠偏），需主车运行。
+    [FieldMember(desc = "车队原地旋转：角速度大小(deg/s，方向由目标角符号决定)")]
+    public float FleetRotateOmega = 15f;
+
+    [FieldMember(desc = "车队原地旋转：目标相对转角(deg，+逆时针)")]
+    public float FleetRotateTargetDeltaDeg = 90f;
+
+    [FieldMember(desc = "车队原地旋转：到位角度精度(deg)")]
+    public float FleetRotateArriveDeg = 1.5f;
+
+    [FieldMember(desc = "车队原地旋转：减速区宽度(deg)，抑制收尾惯性超调")]
+    public float FleetRotateSlowDeg = 25f;
+
+    [FieldMember(desc = "车队原地旋转：减速区末段最小角速度(deg/s)")]
+    public float FleetRotateMinOmega = 3f;
+
+    [FieldMember(desc = "车队原地旋转：起步缓启动角加速度(deg/s²，<=0关闭)")]
+    public float FleetRotateAccel = 20f;
+
+    [FieldMember(desc = "车队原地旋转：到位后安定时长(s)")]
+    public float FleetRotateSettleSec = 0.5f;
+
+    // 与 MultiVehicleSyncUseDetour 解耦：转到指定角度需航向反馈，默认 true 读主车 SLAM 航向闭环判停。
+    // false 时退化为按估算时长开环停止（实际转速≠指令时不精确，易出现"没转到目标就停"）。
+    [FieldMember(desc = "车队原地旋转：用Detour主车航向闭环判停(默认true，false=按时长开环)")]
+    public bool FleetRotateUseDetourHeading = true;
+
+    // ===== 车队联动-自动蟹行（FleetCrabWalk）=====
+    // 以当前车队中心为起点，构造一条直线路径；MovementTest 中车身保持启动朝向追踪该路径。
+    // 动作侧参考几何控制器的路径跟踪思路，直接写入 MultiVehicleAuto... 字段，不再复用脚本手动链路。
+    [FieldMember(desc = "车队蟹行：路径方向相对启动时车队朝向夹角(deg，逆时针为正；路径在车右侧x度时填-x)")]
+    public float FleetCrabAngleDeg = 45f;
+
+    [FieldMember(desc = "车队蟹行：AGV入口使用的车队世界系目标朝向(deg)")]
+    public float FleetCrabBodyWorldHeadingDeg = 0f;
+
+    [FieldMember(desc = "车队蟹行：路径长度(mm)")]
+    public float FleetCrabLengthMm = 2000f;
+
+    [FieldMember(desc = "车队蟹行：行驶速度(m/s)")]
+    public float FleetCrabSpeed = 0.2f;
+
+    [FieldMember(desc = "车队蟹行：速度命令加速度限制(m/s^2，<=0表示不限制)")]
+    public float FleetCrabAccel = 0.2f;
+
+    [FieldMember(desc = "车队蟹行：预对齐后正式下发速度前5秒加速度(m/s^2，<=0表示不限制)")]
+    public float FleetCrabStartAccel = 0.01f;
+
+    [FieldMember(desc = "车队蟹行：末端开始减速距离(mm)")]
+    public float FleetCrabSlowDistance = 2000f;
+
+    [FieldMember(desc = "车队蟹行：完成距离(mm)，低于该剩余距离结束动作")]
+    public float FleetCrabFinishDistance = 20f;
+
+    [FieldMember(desc = "车队蟹行：末端最低速度(m/s)")]
+    public float FleetCrabFinishSpeed = 0.02f;
+
+    [FieldMember(desc = "车队蟹行：末端减速曲线指数")]
+    public float FleetCrabSlowingPow = 0.8f;
+
+    [FieldMember(desc = "车队蟹行：GCP舵角修正上限(deg)")]
+    public float FleetCrabGcpThetaThreshold = 95f;
+
+    [FieldMember(desc = "车队蟹行：headingErr角度纠偏比例系数")]
+    public float FleetCrabDthLinearFac = 1f;
+
+    [FieldMember(desc = "车队蟹行：headingErr角度纠偏舵角限幅(deg)")]
+    public float FleetCrabDthLinearThreshold = 10f;
+
+    [FieldMember(desc = "FleetCrab startup sync timeout(s)")]
+    public float FleetCrabStartSyncTimeoutSec = 8f;
+
+    [FieldMember(desc = "FleetCrab startup wheel alignment tolerance(deg)")]
+    public float FleetCrabStartWheelAlignDeg = 2f;
+
+    // ===== Fleet linked Bezier curve walk =====
+    [FieldMember(desc = "FleetCurve MovementTest Bezier control point count")]
+    public int FleetCurveTestControlPointCount = 4;
+
+    [FieldMember(desc = "FleetCurve speed(m/s)")]
+    public float FleetCurveSpeed = 0.2f;
+
+    [FieldMember(desc = "FleetCurve slow distance(mm)")]
+    public float FleetCurveSlowDistance = 2000f;
+
+    [FieldMember(desc = "FleetCurve finish distance(mm)")]
+    public float FleetCurveFinishDistance = 20f;
+
+    [FieldMember(desc = "FleetCurve finish speed(m/s)")]
+    public float FleetCurveFinishSpeed = 0.02f;
+
+    [FieldMember(desc = "FleetCurve slowing curve exponent")]
+    public float FleetCurveSlowingPow = 0.8f;
+
+    // ===== 2腿检测（单线雷达识别两腿托盘 / 轮胎）=====
+    [FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")]
+    public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1";
+
+    [FieldMember(desc = "2腿检测：初始猜测X(mm, 车体坐标系)")]
+    public float TwoLegGuessX = 2000f;
+
+    [FieldMember(desc = "2腿检测：两腿间距(mm)")]
+    public float TwoLegWidth = 800f;
+
+    [FieldMember(desc = "2腿检测：两腿间距允许误差(mm)")]
+    public float TwoLegWidthErr = 100f;
+
+    [FieldMember(desc = "2腿检测：聚类点间距(mm)")]
+    public float TwoLegBlobDist = 100f;
+
+    [FieldMember(desc = "2腿检测：聚类尺寸(mm)")]
+    public float TwoLegBlobSize = 200f;
+
+    [FieldMember(desc = "2腿检测：聚类最小点数")]
+    public int TwoLegBlobPtCount = 5;
+
+    [FieldMember(desc = "2腿检测：聚类 padding")]
+    public int TwoLegPadding = 5;
+
+    [FieldMember(desc = "2腿检测：腿柱搜索范围")]
+    public int TwoLegPillarFindingScope = 20;
+
+    [FieldMember(desc = "2腿检测：方向符号(±1)")]
+    public int TwoLegSgnDir = 1;
+
+    [FieldMember(desc = "2腿检测：中心X偏移(mm)")]
+    public float TwoLegCenterChangeX = 0f;
+
+    [FieldMember(desc = "2腿检测：输出X补偿(mm)")]
+    public float TwoLegOutputBiasX = 0f;
+
+    [FieldMember(desc = "2腿检测：输出Y补偿(mm)")]
+    public float TwoLegOutputBiasY = 0f;
+
+    [FieldMember(desc = "2腿检测：ROI滤波框长(mm)")]
+    public float TwoLegFilterLength = 1800f;
+
+    [FieldMember(desc = "2腿检测：ROI滤波框宽(mm)")]
+    public float TwoLegFilterWidth = 600f;
+
+    #region
+    [FieldMember(desc = "轮胎识别:识别框长")] public float TireFilterLength = 1800f;
+    [FieldMember(desc = "轮胎识别:识别框宽")] public float TireFilterWidth = 600f;
+    [FieldMember(desc = "轮胎识别:轮胎间距")] public float TireTwoLegWidth = 800f;
+    [FieldMember(desc = "轮胎识别:轮胎识别允许误差")] public float TireTwoLegWidthErr = 100f;
+    [FieldMember(desc = "轮胎识别:轮胎聚类最小点云数")] public int TireTwoLegBlobPtCount = 15;
+
+    [FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegBlobDist = 100f;
+    [FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegBlobSize = 200f;
+    [FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontPadding = 5;
+    [FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontTwoLegPillarFindingScope = 20;
+    [FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontTwoLegSgnDir = 1;
+    [FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegCenterChangeX = 0;
+    [FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegBlobDist = 100f;
+    [FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegBlobSize = 200f;
+    [FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackPadding = 5;
+    [FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackTwoLegPillarFindingScope = 20;
+    [FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackTwoLegSgnDir = 1;
+    [FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegCenterChangeX = 0;
+
+    [FieldMember(desc = "抱夹控制pid:Kp")] public float ClampControlKp = 0.1f;
+    [FieldMember(desc = "抱夹控制pid:Ki")] public float ClampControlKi = 0f;
+    [FieldMember(desc = "抱夹控制pid:Kd")] public float ClampControlKd = 0f;
+    [FieldMember(desc = "抱夹控制pid:MaxI")] public float ClampControlMaxI = 0f;
+    [FieldMember(desc = "抱夹控制pid:Acc")] public float ClampControlSpeedAcc = 1f;
+    [FieldMember(desc = "抱夹控制pid:Thresh")] public float ClampControlThresh = 0.2f;
+    [FieldMember(desc = "抱夹控制pid:DeadZone")] public float ClampControlDeadZone = 5f;
+    [FieldMember(desc = "抱夹最大速度")] public float MaxClampSpeed = 1.5f;
+
+    [FieldMember(desc = "直线行走距离")] public float LineTrackDistance = 1000f;
+    [FieldMember(desc = "直线行走最大速度")] public float LineTrackMaxSpeed = 0.3f;
+    [FieldMember(desc = "直线行走Kp")] public float LineTrackKp = 0.2f;
+    [FieldMember(desc = "直线行走Ki")] public float LineTrackKi = 0f;
+    [FieldMember(desc = "直线行走Kd")] public float LineTrackKd = 0f;
+    [FieldMember(desc = "直线行走DeadZone")] public float LineTrackDeadZone = 50f;
+
+    [FieldMember(desc = "轮胎跟踪:切换至盲走距离")] public float TireFollowingWalkBlindSwitchingDistance = 1200f;
+    [FieldMember(desc = "轮胎跟踪:识别第一对轮胎的初始距离")] public float TireFollowingStage1GuessX = 2000f;
+    [FieldMember(desc = "轮胎跟踪:识别第二对轮胎的初始距离")] public float TireFollowingStage2GuessX = 2475f;
+    [FieldMember(desc = "轮胎跟踪:盲走停止距离")] public float TireFollowingWalkBlindFinishDistance = 10f;
+    [FieldMember(desc = "轮胎跟踪:减速距离")] public float TireFollowingSlowDistance = 200f;
+    [FieldMember(desc = "轮胎跟踪:最大速度")] public float TireFollowingMaxSpeed = 0.2f;
+    [FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移X")] public float TireFollowingFrontLidarPathTransformationX = 253f;
+    [FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移Y")] public float TireFollowingFrontLidarPathTransformationY = 13f;
+    [FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移Th")] public float TireFollowingFrontLidarWalkBlindTh = -1f;
+    [FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移X")] public float TireFollowingBackLidarPathTransformationX = 148f;
+    [FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移Y")] public float TireFollowingBackLidarPathTransformationY = 2f;
+    [FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移Th")] public float TireFollowingBackLidarWalkBlindTh = 0f;
+    [FieldMember(desc = "轮胎跟踪:离车时后雷达识别路径偏移X")] public float TireFollowingLeaveCarBackLidarPathTransformationX = 1500f;
+    [FieldMember(desc = "轮胎跟踪:离车时切换至盲走距离")] public float TireFollowingLeaveCarWalkBlindSwitchingDistance = 1200f;
+
+    [FieldMember(desc = "轮胎跟踪:测试钻轮胎数量")] public int TireFollowingTireNum = 1;
+    [FieldMember(desc = "轮胎跟踪:过近距离")] public float TireFollowingCloseDistance = 1400;
+    [FieldMember(desc = "轮胎跟踪:距离过近角度忽略阈值")] public float TireFollowingAngleIgnoreThr = 0.2f;
+    [FieldMember(desc = "轮胎跟踪:Y最大平均数")] public int TireFollowingYAverageFrameCount = 5;
+
+    [FieldMember(desc = "终点跟踪:速度")] public float DstTrackerMaxSpeed = 0.3f;
+    [FieldMember(desc = "轮胎跟踪:释放锁点距离")] public float TireFollowingReleaseDistance = 1600;
+    #endregion
+
+    [FieldMember(desc = "轮胎跟踪:角度调整kp")] public float TireFollowingThkp = 0.05f;
+    [FieldMember(desc = "轮胎跟踪:角度调整ki")] public float TireFollowingThki = 0.01f;
+    [FieldMember(desc = "轮胎跟踪:角度调整kd")] public float TireFollowingThkd = 0f;
+    [FieldMember(desc = "轮胎跟踪:角度调整SpeedAcc")] public float TireFollowingThSpeedAccPerSec = 1f;
+    [FieldMember(desc = "轮胎跟踪:角度调整Thresh")] public float TireFollowingThThresh = 0.1f;
+    [FieldMember(desc = "轮胎跟踪:角度调整DeadZone")] public float TireFollowingThDeadZone = 5f;
+    [FieldMember(desc = "轮胎跟踪:角度调整MaxI")] public float TireFollowingThMaxI = 0.01f;
+}

ClumsyPilot/PlaygroundWebApi.cs

@@ -0,0 +1,81 @@
+using System;
+using System.Net.Http;
+using System.Text;
+using Newtonsoft.Json;
+using Newtonsoft.Json.Linq;
+
+namespace MultiWheelC;
+
+/// <summary>
+/// Playground 仿真器 HTTP Web API 轻量客户端：查询小车位姿、瞬移小车。
+/// 服务端实现见 Playground/Web/PlaygroundWebApi.cs，默认监听 http://localhost:18090。
+/// 坐标单位 mm，朝向 yawDeg 单位为度，世界坐标系与场景 JSON 一致。
+/// </summary>
+public static class PlaygroundWebApi
+{
+    // 禁用系统代理：本机 Playground 走 localhost，若经系统代理(如 127.0.0.1:7890)会连接失败。
+    private static readonly HttpClient Http = new HttpClient(new HttpClientHandler { UseProxy = false })
+    {
+        Timeout = TimeSpan.FromSeconds(3)
+    };
+
+    public struct Pose
+    {
+        public float X;
+        public float Y;
+        public float YawDeg;
+    }
+
+    /// <summary>查询单台小车的世界位姿。GET /api/robots/{name}。</summary>
+    public static Pose GetPose(string baseUrl, string robotName)
+    {
+        var url = $"{baseUrl.TrimEnd('/')}/api/robots/{Uri.EscapeDataString(robotName)}";
+        var json = Http.GetStringAsync(url).GetAwaiter().GetResult();
+        var o = JObject.Parse(json);
+        return new Pose
+        {
+            X = o.Value<float>("x"),
+            Y = o.Value<float>("y"),
+            YawDeg = o.Value<float>("yawDeg")
+        };
+    }
+
+    /// <summary>将小车瞬移到目标世界位姿。POST /api/robots/{name}/move。</summary>
+    public static void Move(string baseUrl, string robotName, float x, float y, float yawDeg)
+    {
+        var url = $"{baseUrl.TrimEnd('/')}/api/robots/{Uri.EscapeDataString(robotName)}/move";
+        var body = JsonConvert.SerializeObject(new { x, y, yaw = yawDeg, stop = true });
+        using var content = new StringContent(body, Encoding.UTF8, "application/json");
+        var resp = Http.PostAsync(url, content).GetAwaiter().GetResult();
+        resp.EnsureSuccessStatusCode();
+    }
+
+    /// <summary>查询车辆运动是否启用（暂停时为 false）。GET /api/motion。</summary>
+    public static bool MotionEnabled(string baseUrl)
+    {
+        var url = $"{baseUrl.TrimEnd('/')}/api/motion";
+        var json = Http.GetStringAsync(url).GetAwaiter().GetResult();
+        return JObject.Parse(json).Value<bool>("motionEnabled");
+    }
+
+    /// <summary>恢复车辆运动。POST /api/motion/resume。</summary>
+    public static void ResumeMotion(string baseUrl)
+    {
+        var url = $"{baseUrl.TrimEnd('/')}/api/motion/resume";
+        var resp = Http.PostAsync(url, null).GetAwaiter().GetResult();
+        resp.EnsureSuccessStatusCode();
+    }
+
+    /// <summary>
+    /// 暂停车辆运动（仅冻结运动，不停止仿真；传感器继续扫描）。POST /api/motion/pause。
+    /// feedback: "zero"(默认,反馈归零) / "none"(不上报) / "hold"(保留暂停瞬间值)。
+    /// </summary>
+    public static void PauseMotion(string baseUrl, string feedback = "zero")
+    {
+        var url = $"{baseUrl.TrimEnd('/')}/api/motion/pause";
+        var body = JsonConvert.SerializeObject(new { feedback });
+        using var content = new StringContent(body, Encoding.UTF8, "application/json");
+        var resp = Http.PostAsync(url, content).GetAwaiter().GetResult();
+        resp.EnsureSuccessStatusCode();
+    }
+}

ClumsyPilot/TireFollowing.cs

@@ -0,0 +1,511 @@
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Linq;
+using System.Numerics;
+using System.Reflection;
+using System.Text;
+using System.Threading;
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using ClumsyCore.Utilities;
+using ClumsyDance.ClumsyWalk.Detectors;
+using CommonUsage.Chassis;
+using FundamentalLib;
+using MDCSToolBox;
+using MDCSToolBox.Clumsy.Calibration;
+using MDCSToolBox.Clumsy.MotionControllers;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+using MDCSToolBox.Clumsy.Tracks;
+using MDCSToolBox.Commons.Controllers;
+using static ClumsyCore.DTools.Painter;
+using LineSegment = ClumsyCore.Utilities.LineSegment;
+
+namespace MultiWheelC
+{
+    public class TireFollowing : MovementDefinition
+    {
+        public Func<AbstractGeometricController> GetController;
+
+        /// <summary>
+        /// 车辆方向
+        /// </summary>
+        public float CarDirection = 0;
+
+        /// <summary>
+        /// 停止距离
+        /// </summary>
+        //public float FinishDistance = 1000;
+
+        /// <summary>
+        /// 减速距离
+        /// </summary>
+        public float SlowDistance = 1000;
+
+        /// <summary>
+        /// 最大速度
+        /// </summary>
+        public float MaxSpeed = 0.3f;
+
+        // 末段衔接：接近盲走终点时给非零速度，供后续动作连续接管
+        public bool EnableHandover = false;
+        public float HandoverDistance = 200f; // mm
+        public float HandoverSpeed = 0.2f;    // m/s
+
+        /// <summary>
+        /// 钻轮胎数量
+        /// </summary>
+        public int TireNum = 1;
+
+        /// <summary>
+        /// 盲走角度偏移
+        /// </summary>
+        public float WalkBlindTh = -1f;
+
+        /// <summary>
+        /// 是否检测到目标
+        /// </summary>
+        public bool NoTarget = false;
+
+        public float GuessRangeX;
+
+        public float GuessRangeY;
+
+        /// <summary>
+        /// 检测器定义
+        /// </summary>
+        public class DetectorDefinition
+        {
+            /// <summary>
+            /// 开始检测距离
+            /// </summary>
+            public float StartGuessingX;
+
+            /// <summary>
+            /// 开始检测距离
+            /// </summary>
+            public float StartGuessingY;
+
+            /// <summary>
+            /// 检测函数
+            /// </summary>
+            public Func<float, float, List<DetectFilter>, LineSegment> DetectFunction = null;
+
+            public Action<int> LeaveSrcFunction = null;
+
+            public int SrcId = -1;
+            public int DstId = -1;
+
+            /// <summary>
+            /// 路径偏移
+            /// </summary>
+            public Tuple<float, float, float> PathTransformation = Tuple.Create(0f, 0f, 0f);
+
+            public float PathTransformationAnchorDistance = 0f;
+
+            /// <summary>
+            /// 切换条件
+            /// </summary>
+            public Func<float, bool> SwitchWalkBlindCondition = null;
+
+            /// <summary>
+            /// 盲走停止距离
+            /// </summary>
+            public Func<float, bool> FinishWalkBlindCondition = null;
+        }
+
+        public Func<bool> FinishCondition;
+
+        /// <summary>
+        /// 多个检测器列表
+        /// </summary>
+        public List<DetectorDefinition> detectors = null;
+
+        private Painter _painter;
+        private List<float> _remainDistanceList = new List<float>();
+        private List<float> _remainAngleList = new List<float>();
+        private List<float> _targetYList = new List<float>();
+
+        private List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
+        {
+            var painter = UI.GetPainter("GeneralFollowing.SetFilters", false);
+            painter.Clear();
+            painter.Clear(3000);
+
+            var box = new Vector2[]
+            {
+                new (guessCenterX - GuessRangeX, guessCenterY - GuessRangeY),
+                new (guessCenterX + GuessRangeX, guessCenterY - GuessRangeY),
+                new (guessCenterX + GuessRangeX, guessCenterY + GuessRangeY),
+                new (guessCenterX - GuessRangeX, guessCenterY + GuessRangeY),
+            };
+
+            for (var i = 0; i < box.Length; ++i)
+                painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
+
+            // PC filter in car coordinate frame
+            return new List<DetectFilter>()
+            {
+                new(CoordinateSystem.Car2D,
+                    p => LessMath.IsPointInPolygon4(
+                        box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))),
+            };
+        }
+
+        public void Stop()
+        {
+            _dt?.Stop();
+        }
+
+        /// <summary>
+        ///计算车体中心的位移和角度增量
+        /// </summary>
+        /// <param name="a">a轮在车体坐标系下位置</param>
+        /// <param name="va">a轮在车体坐标系下位移增量</param>
+        /// <param name="b">b轮在车体坐标系下位置</param>
+        /// <param name="vb">b轮在车体坐标系下位移增量</param>
+        /// <returns></returns>
+        private static (float, float, float) CenterMoveFromPoints(Vector2 a,
+            Vector2 aDelta,
+            Vector2 b,
+            Vector2 bDelta)
+        {
+            float th_x = 0, th_y = 0, th = 0, x = 0, y = 0;
+            var eps = 0.0000001;
+            if (Math.Abs(a.Y - b.Y) > eps)
+            {
+                th_x = (aDelta.X - bDelta.X) / (b.Y - a.Y);
+            }
+            if (Math.Abs(a.X - b.X) > eps)
+            {
+                th_y = (aDelta.Y - bDelta.Y) / (a.X - b.X);
+            }
+            th = th_x == 0 ? th_y : th_x;
+
+            x = (aDelta.X + bDelta.X) / 2f - (a.Y - b.Y) / 2f * th;
+
+            y = (aDelta.Y + bDelta.Y) / 2f + (a.X - b.X) / 2f * th;
+
+            return (x, y, th);
+        }
+
+
+        public override IEnumerable<bool> Get()
+        {
+            _painter = UI.GetPainter("GeneralFollowing", false);
+            var lastDetectX = detectors[0].StartGuessingX;
+            var lastDetectY = detectors[0].StartGuessingY;
+            var detectorIndex = 0;
+
+            var controller = (MultiWheelGeometricController)GetController.Invoke();
+            controller.BaseSpeed = MaxSpeed;
+            controller.FinishDistance = float.MinValue;
+            controller.FirstThAccuracy = 999;
+            _dt = new DriveTask(controller.Track(true, CoordinateSystem.Car2D));
+            void HardStop()
+            {
+                _dt?.Stop();
+                ((MultiWheelChassis)PilotDefinition.Chassis).DriveStop();
+                DLog.Log($"Hard Stop!", "TireFollowing");
+            }
+            float WalkBlindCarPathDstX = -1f, WalkBlindCarPathDstY = -1f, WalkBlindCarPathDstTh = -1f;
+            bool WalkBlindStage1 = false, WalkBlindStage2 = false;
+            var angle2target = -1f;
+            float _lastLFLEncoder = -1, _lastLFREncoder = -1, _lastRFLEncoder = -1, _lastRFREncoder = -1;
+            float _lastLRLEncoder = -1, _lastLRREncoder = -1, _lastRRLEncoder = -1, _lastRRREncoder = -1;
+
+            (float, float, float) GetCurrentPos2Dst(float lastX, float lastY, float lastTh)
+            {
+                // Read current encoders
+                var curLFLEncoder = PilotDefinition.Self.LFLActualPos;
+                var curLFREncoder = PilotDefinition.Self.LFRActualPos;
+                var curRFLEncoder = PilotDefinition.Self.RFLActualPos;
+                var curRFREncoder = PilotDefinition.Self.RFRActualPos;
+                var curLRLEncoder = PilotDefinition.Self.LRLActualPos;
+                var curLRREncoder = PilotDefinition.Self.LRRActualPos;
+                var curRRLEncoder = PilotDefinition.Self.RRLActualPos;
+                var curRRREncoder = PilotDefinition.Self.RRRActualPos;
+
+                // Average delta per wheel pair (LF, LR, RF, RR)
+                var lfDelta = (curLFLEncoder - _lastLFLEncoder + curLFREncoder - _lastLFREncoder) / 2f;
+                var lrDelta = (curLRLEncoder - _lastLRLEncoder + curLRREncoder - _lastLRREncoder) / 2f;
+                var rfDelta = (curRFLEncoder - _lastRFLEncoder + curRFREncoder - _lastRFREncoder) / 2f;
+                var rrDelta = (curRRLEncoder - _lastRRLEncoder + curRRREncoder - _lastRRREncoder) / 2f;
+                var deltaList = new List<float> { lfDelta, lrDelta, rfDelta, rrDelta };
+
+                var xs = new List<float>();
+                var ys = new List<float>();
+                var ths = new List<float>();
+                var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
+                var steerWheels = chassis.GetSteerWheels();
+                for (var i = 0; i < steerWheels.Count; ++i)
+                {
+                    var sw1 = steerWheels[i];
+                    var a = sw1.Position;
+                    var tha = sw1.ReadAngle() / 180f * (float)Math.PI;
+                    var deltaa = deltaList[i];
+                    var va = new Vector2(deltaa * (float)Math.Cos(tha), deltaa * (float)Math.Sin(tha));
+                    for (var j = i + 1; j < steerWheels.Count; ++j)
+                    {
+                        var sw2 = steerWheels[j];
+                        var b = sw2.Position;
+                        var thb = sw2.ReadAngle() / 180f * (float)Math.PI;
+                        var deltab = deltaList[j];
+                        var vb = new Vector2(deltab * (float)Math.Cos(thb), deltab * (float)Math.Sin(thb));
+                        var (tempx, tempy, tempth) = CenterMoveFromPoints(a, va, b, vb);
+                        Hedingben.ToastText($"{tempx:f2} {tempy:f2} {tempth / Math.PI * 180f:f2} ", $"{i}_{j}");
+                        xs.Add(tempx);
+                        ys.Add(tempy);
+                        ths.Add(tempth);
+                    }
+                }
+                var x = xs.Average();
+                var y = ys.Average();
+                var Th = ths.Average() / (float)Math.PI * 180;
+                var moveTup = Tuple.Create(x, y, Th);
+                var moved = MathTools.SolveTransform2D(MathTools.SolveTransform2D(Tuple.Create(lastX, lastY, lastTh), moveTup), Tuple.Create(0f, 0f, 0f));
+
+                _lastLFLEncoder = curLFLEncoder;
+                _lastLFREncoder = curLFREncoder;
+                _lastRFLEncoder = curRFLEncoder;
+                _lastRFREncoder = curRFREncoder;
+                _lastLRLEncoder = curLRLEncoder;
+                _lastLRREncoder = curLRREncoder;
+                _lastRRLEncoder = curRRLEncoder;
+                _lastRRREncoder = curRRREncoder;
+                return (moved.Item1, moved.Item2, moved.Item3);
+            }
+
+            while (true)
+            {
+                if (detectorIndex > detectors.Count - 1)
+                    throw new Exception("detector index out of range!");
+
+                _painter.Clear();
+                if (WalkBlindStage1 || WalkBlindStage2)
+                {
+                    //第二次盲走时或只钻一个轮胎时
+                    if (WalkBlindStage2 || detectors.Count == 1 || TireNum == 1)
+                    {
+                        //controller.FinishDistance = 10f;
+                        controller.SlowDistance = SlowDistance;
+                        controller.SlowingPow = 0.7f;
+                    }
+                    if (EnableHandover)
+                    {
+                        controller.SlowDistance = float.MinValue;
+                        controller.FinishSpeed = 0.2f;
+                        controller.FinishDistance = 50;
+                    }
+                    (WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh) = GetCurrentPos2Dst(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
+                    var walkBlindPathEnd = Tuple.Create(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
+                    var walkBlindPathStart = LessMath.Transform2D(walkBlindPathEnd, Tuple.Create(CarDirection == 0 ? -3000f : 3000f, 0f, 0f));
+                    var walkBlindPathDst = new Vector2(WalkBlindCarPathDstX, WalkBlindCarPathDstY);
+                    var walkBlindPathSrc = new Vector2(walkBlindPathStart.Item1, walkBlindPathStart.Item2);
+                    var walkBlindPath = new LineSegment(walkBlindPathSrc, walkBlindPathDst);
+
+                    DLog.Log($"盲走目标点:{walkBlindPath.Src.X:F2} {walkBlindPath.Src.Y:F2} {walkBlindPath.Dst.X:F2} {walkBlindPath.Dst.Y:F2}", "TireFollowing");
+                    _painter.DrawDot(Color.Purple, walkBlindPathDst, sz: 3);
+                    _painter.DrawLine(Color.GreenYellow, walkBlindPath.Src, walkBlindPath.Dst, endArrow: true, width: 2);
+
+                    var track = new LineTrack(walkBlindPath.Src, walkBlindPath.Dst);
+                    track.CarDirectionBias = CarDirection;
+                    controller.UpdateTracks(new List<AbstractTrack> { track });
+                    var rd = (float)LessMath.PerpendicularPosition(0, 0, walkBlindPath.Dst.X, walkBlindPath.Dst.Y,
+                        walkBlindPath.Src.X, walkBlindPath.Src.Y);
+                    _remainDistanceList.Add(rd);
+                    while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0);
+                    rd = _remainDistanceList.Average();
+                    DLog.Log($"盲走投影点剩余距离:{rd:0.0} ", "TireFollowing");
+                    // 检查是否达到盲走结束条件
+                    if (detectors[detectorIndex].FinishWalkBlindCondition(rd))
+                    {
+                        if (WalkBlindStage1)
+                        {
+                            DLog.Log("达到第一次盲走停止距离，停下或开始钻第二对轮胎", "TireFollowing");
+                            //if (detectors[detectorIndex].DstId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
+                            //{
+                            //    detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].DstId);
+                            //    DLog.Log($"释放取车点{detectors[detectorIndex].DstId}", "TireFollowing");
+                            //}
+                            WalkBlindStage1 = false;
+                            _remainAngleList.Clear();
+                            _remainDistanceList.Clear();
+                            detectorIndex++;
+                            if ((detectors.Count == 1 || TireNum == 1) && !EnableHandover)
+                            {
+                                HardStop();
+                                yield return false;
+                            }
+                        }
+                        else if (WalkBlindStage2)
+                        {
+                            DLog.Log("达到第二对轮胎处，停止移动", "TireFollowing");
+                            if (!EnableHandover)
+                            {
+                                HardStop();
+                            }
+                            yield return false;
+                        }
+                    }
+                    yield return true;
+                    continue;
+                }
+
+                var target = detectors[detectorIndex].DetectFunction(CarDirection, lastDetectX,
+                        SetFilters(lastDetectX, lastDetectY));
+
+                if (target == null)
+                {
+                    DLog.Log("无目标，等待下一帧", "TireFollowing");
+                    controller.FirstRotateMaxSpeed = 0;
+                    yield return true;
+                    continue;
+                }
+                else controller.FirstRotateMaxSpeed = 5;
+
+                var targetAngle = CalculateAngle2YAxis(target.Src, target.Dst);
+                var targetPos = new Vector2((target.Src.X + target.Dst.X) / 2f, (target.Src.Y + target.Dst.Y) / 2f);
+                var dis2target = (float)Math.Sqrt(Math.Pow(targetPos.X, 2) + Math.Pow(targetPos.Y, 2));
+                //距离较近以后角度容易跳变
+                if (dis2target < PilotDefinition.Conf.TireFollowingCloseDistance && Math.Abs(targetAngle) > PilotDefinition.Conf.TireFollowingAngleIgnoreThr)
+                {
+                    yield return true;
+                    continue;
+                }
+                else _remainAngleList.Add(targetAngle);
+                while (_remainAngleList.Count > 10) _remainAngleList.RemoveAt(0);
+                angle2target = _remainAngleList.Average();
+                var distanceLabelPos = targetPos / 2f;
+                _painter.DrawLine(Color.Cyan, Vector2.Zero, targetPos, width: 2);
+                _painter.DrawText(Color.Yellow, $"{dis2target:F3}", distanceLabelPos.X, distanceLabelPos.Y);
+
+                var path = DetectorHelper.GetApproachPath(target, CoordinateSystem.Car2D, pathLen: 3000,
+                                                          bias: detectors[detectorIndex].PathTransformation,
+                                                          biasAnchorDistance: detectors[detectorIndex].PathTransformationAnchorDistance);
+
+                if (path == null)
+                {
+                    DLog.Log("no path!", "TireFollowing");
+                    NoTarget = true;
+                }
+                else
+                {
+                    lastDetectX = ((target.Src + target.Dst) / 2f).X;
+                    lastDetectY = ((target.Src + target.Dst) / 2f).Y;
+
+                    var currentY = path.CarPath.Dst.Y;
+                    if (Math.Abs(targetAngle) < PilotDefinition.Conf.TireFollowingAngleIgnoreThr &&
+                        dis2target < PilotDefinition.Conf.TireFollowingCloseDistance)
+                    {
+                        _targetYList.Add(currentY);
+                        while (_targetYList.Count > PilotDefinition.Conf.TireFollowingYAverageFrameCount) _targetYList.RemoveAt(0);
+                    }
+
+                    var trackDstY = _targetYList.Count > 0 ? _targetYList.Average() : currentY;
+                    Hedingben.ToastText($"target Y:{_targetYList.Count}  {trackDstY}", "target Y");
+
+                    var trackDst = new Vector2(path.CarPath.Dst.X, trackDstY);
+                    _painter.DrawLine(Color.GreenYellow, path.CarPath.Src, trackDst, endArrow: true);
+
+                    var rd = (float)LessMath.PerpendicularPosition(0, 0, trackDst.X, trackDst.Y,
+                        path.CarPath.Src.X, path.CarPath.Src.Y);
+                    _remainDistanceList.Add(rd);
+                    while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0);
+                    rd = _remainDistanceList.Average();
+                    _painter.DrawText(Color.Green, $"{rd:F3}", distanceLabelPos.X, distanceLabelPos.Y - 200);
+                    if(rd < PilotDefinition.Conf.TireFollowingReleaseDistance)
+                    {
+                        if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
+                        {
+                            detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
+                            DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
+                        }
+                    }
+
+                    if (detectorIndex < detectors.Count - 1)
+                    {
+                        controller.SlowDistance = 1;
+                        if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
+                        {
+                            WalkBlindStage1 = true;
+                            //if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
+                            //{
+                            //    detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
+                            //    DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
+                            //}
+                            WalkBlindCarPathDstX = trackDst.X;
+                            WalkBlindCarPathDstY = trackDst.Y;
+                            WalkBlindCarPathDstTh = angle2target + WalkBlindTh;
+                            DLog.Log($"切换至第一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
+                                                                $"{WalkBlindCarPathDstY:F2} " +
+                                                                $"{WalkBlindCarPathDstTh:F2}", "TireFollowing");
+                            _lastLFLEncoder = PilotDefinition.Self.LFLActualPos;
+                            _lastLFREncoder = PilotDefinition.Self.LFRActualPos;
+                            _lastRFLEncoder = PilotDefinition.Self.RFLActualPos;
+                            _lastRFREncoder = PilotDefinition.Self.RFRActualPos;
+                            _lastLRLEncoder = PilotDefinition.Self.LRLActualPos;
+                            _lastLRREncoder = PilotDefinition.Self.LRRActualPos;
+                            _lastRRLEncoder = PilotDefinition.Self.RRLActualPos;
+                            _lastRRREncoder = PilotDefinition.Self.RRRActualPos;
+                            _remainDistanceList.Clear();
+                            _targetYList.Clear();
+                            lastDetectX = detectors[detectorIndex + 1].StartGuessingX;
+                            lastDetectY = detectors[detectorIndex + 1].StartGuessingY;
+                            continue;
+                        }
+                    }
+                    else if (detectorIndex == detectors.Count - 1)
+                    {
+                        if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
+                        {
+                            WalkBlindStage2 = true;
+                            WalkBlindCarPathDstX = trackDst.X;
+                            WalkBlindCarPathDstY = trackDst.Y;
+                            WalkBlindCarPathDstTh = angle2target + WalkBlindTh;
+                            DLog.Log($"切换至最后一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
+                                                                $"{WalkBlindCarPathDstY:F2} " +
+                                                                $"{WalkBlindCarPathDstTh:F2}", "TireFollowing");
+                            if (detectors.Count == 1)
+                            {
+                                if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
+                                {
+                                    detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
+                                    DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
+                                }
+                            }
+
+                            _lastLFLEncoder = PilotDefinition.Self.LFLActualPos;
+                            _lastLFREncoder = PilotDefinition.Self.LFRActualPos;
+                            _lastRFLEncoder = PilotDefinition.Self.RFLActualPos;
+                            _lastRFREncoder = PilotDefinition.Self.RFRActualPos;
+                            _lastLRLEncoder = PilotDefinition.Self.LRLActualPos;
+                            _lastLRREncoder = PilotDefinition.Self.LRRActualPos;
+                            _lastRRLEncoder = PilotDefinition.Self.RRLActualPos;
+                            _lastRRREncoder = PilotDefinition.Self.RRRActualPos;
+                            _remainDistanceList.Clear();
+                            _targetYList.Clear();
+                            continue;
+                        }
+                    }
+                    DLog.Log($"投影点剩余距离:{rd:F2}", "TireFollowing");
+                    var track = new LineTrack(path.CarPath.Src, trackDst);
+                    track.CarDirectionBias = CarDirection;
+                    controller.UpdateTracks(new List<AbstractTrack> { track });
+                    NoTarget = false;
+                }
+                yield return true;
+            }
+        }
+
+        private static float CalculateAngle2YAxis(Vector2 point1, Vector2 point2)
+        {
+            return -(float)(Math.Atan((point1.X - point2.X) / (point1.Y - point2.Y)) * 180 / Math.PI);
+        }
+
+        private DriveTask _dt;
+    }
+}

ClumsyPilot/VehicleSyncBinaryCodec.cs

@@ -0,0 +1,277 @@
+using System;
+using System.Collections.Generic;
+using System.IO;
+using System.Text;
+
+namespace MultiWheelC;
+
+internal static class VehicleSyncBinaryCodec
+{
+    private const byte Version = 2;
+    private const byte RegisterType = 1;
+    private const byte NotificationType = 2;
+    private static readonly byte[] Magic = Encoding.ASCII.GetBytes("MVS1");
+
+    public static byte[] EncodeRegister(int carNum, VehicleSyncInfo info)
+    {
+        using var stream = new MemoryStream();
+        using var writer = new BinaryWriter(stream, Encoding.UTF8);
+        WriteHeader(writer, RegisterType);
+        writer.Write(carNum);
+        WriteInfo(writer, info);
+        writer.Flush();
+        return stream.ToArray();
+    }
+
+    public static (int CarNum, VehicleSyncInfo Info) DecodeRegister(byte[] payload)
+    {
+        using var stream = new MemoryStream(payload ?? throw new ArgumentNullException(nameof(payload)));
+        using var reader = new BinaryReader(stream, Encoding.UTF8);
+        var version = ReadHeader(reader, RegisterType);
+        var carNum = reader.ReadInt32();
+        var info = ReadInfo(reader, version);
+        EnsureFullyRead(stream);
+        return (carNum, info);
+    }
+
+    public static byte[] EncodeNotification(VehicleSyncNotification notification)
+    {
+        using var stream = new MemoryStream();
+        using var writer = new BinaryWriter(stream, Encoding.UTF8);
+        WriteHeader(writer, NotificationType);
+
+        writer.Write(notification.Seq);
+        writer.Write(BuildNotificationFlags(notification));
+        writer.Write(notification.Mode);
+        writer.Write(notification.FleetStopSourceCar);
+        writer.Write(notification.CenterX);
+        writer.Write(notification.CenterY);
+        writer.Write(notification.CenterTh);
+        writer.Write(notification.FleetVx);
+        writer.Write(notification.FleetFrontTh);
+        writer.Write(notification.FleetRearTh);
+        writer.Write(notification.FleetOmega);
+        writer.Write(notification.RequestedFleetOmega);
+        writer.Write(notification.SyncTh);
+        writer.Write(notification.SyncDistance);
+        writer.Write(notification.DeltaDetectCenter);
+        writer.Write(notification.RotateActiveOmega);
+        writer.Write(notification.RotateCompXyFac);
+        writer.Write(notification.RotateCompXyIFac);
+        writer.Write(notification.RotateCompXyMax);
+        writer.Write(notification.RotateCompThFac);
+        writer.Write(notification.RotateCompThIFac);
+        writer.Write(notification.RotateCompThMax);
+        writer.Write(notification.RotateCompTangentFrac);
+        writer.Write(notification.RotateStartWheelAlignDeg);
+        writer.Write(notification.RotateActiveWheelAlignDeg);
+        writer.Write(notification.IdealX);
+        writer.Write(notification.IdealY);
+        writer.Write(notification.IdealTh);
+        WriteString(writer, notification.FleetStopReason);
+
+        var fleet = notification.Fleet ?? new Dictionary<int, VehicleSyncInfo>();
+        if (fleet.Count > ushort.MaxValue)
+            throw new InvalidOperationException($"Fleet count {fleet.Count} exceeds binary protocol limit.");
+        writer.Write((ushort)fleet.Count);
+        foreach (var kv in fleet)
+        {
+            writer.Write(kv.Key);
+            WriteInfo(writer, kv.Value);
+        }
+
+        writer.Flush();
+        return stream.ToArray();
+    }
+
+    public static VehicleSyncNotification DecodeNotification(byte[] payload)
+    {
+        using var stream = new MemoryStream(payload ?? throw new ArgumentNullException(nameof(payload)));
+        using var reader = new BinaryReader(stream, Encoding.UTF8);
+        var version = ReadHeader(reader, NotificationType);
+
+        var notification = new VehicleSyncNotification
+        {
+            Seq = reader.ReadInt64()
+        };
+
+        ApplyNotificationFlags(notification, reader.ReadUInt16());
+        notification.Mode = reader.ReadInt32();
+        notification.FleetStopSourceCar = reader.ReadInt32();
+        notification.CenterX = reader.ReadSingle();
+        notification.CenterY = reader.ReadSingle();
+        notification.CenterTh = reader.ReadSingle();
+        notification.FleetVx = reader.ReadSingle();
+        notification.FleetFrontTh = reader.ReadSingle();
+        notification.FleetRearTh = reader.ReadSingle();
+        notification.FleetOmega = reader.ReadSingle();
+        notification.RequestedFleetOmega = reader.ReadSingle();
+        notification.SyncTh = reader.ReadSingle();
+        notification.SyncDistance = reader.ReadSingle();
+        notification.DeltaDetectCenter = reader.ReadSingle();
+        notification.RotateActiveOmega = reader.ReadSingle();
+        notification.RotateCompXyFac = reader.ReadSingle();
+        notification.RotateCompXyIFac = reader.ReadSingle();
+        notification.RotateCompXyMax = reader.ReadSingle();
+        notification.RotateCompThFac = reader.ReadSingle();
+        notification.RotateCompThIFac = reader.ReadSingle();
+        notification.RotateCompThMax = reader.ReadSingle();
+        notification.RotateCompTangentFrac = reader.ReadSingle();
+        notification.RotateStartWheelAlignDeg = reader.ReadSingle();
+        notification.RotateActiveWheelAlignDeg = reader.ReadSingle();
+        notification.IdealX = reader.ReadSingle();
+        notification.IdealY = reader.ReadSingle();
+        notification.IdealTh = reader.ReadSingle();
+        notification.FleetStopReason = ReadString(reader);
+
+        var fleetCount = reader.ReadUInt16();
+        notification.Fleet = new Dictionary<int, VehicleSyncInfo>(fleetCount);
+        for (var i = 0; i < fleetCount; ++i)
+        {
+            var carNum = reader.ReadInt32();
+            notification.Fleet[carNum] = ReadInfo(reader, version);
+        }
+
+        EnsureFullyRead(stream);
+        return notification;
+    }
+
+    private static void WriteHeader(BinaryWriter writer, byte type)
+    {
+        writer.Write(Magic);
+        writer.Write(Version);
+        writer.Write(type);
+        writer.Write((ushort)0);
+    }
+
+    private static byte ReadHeader(BinaryReader reader, byte expectedType)
+    {
+        for (var i = 0; i < Magic.Length; ++i)
+        {
+            if (reader.ReadByte() != Magic[i])
+                throw new InvalidDataException("Invalid multi-vehicle sync binary magic.");
+        }
+
+        var version = reader.ReadByte();
+        if (version < 1 || version > Version)
+            throw new InvalidDataException($"Unsupported multi-vehicle sync binary version {version}.");
+
+        var type = reader.ReadByte();
+        if (type != expectedType)
+            throw new InvalidDataException($"Unexpected multi-vehicle sync packet type {type}.");
+
+        var reserved = reader.ReadUInt16();
+        if (reserved != 0)
+            throw new InvalidDataException("Invalid multi-vehicle sync binary reserved field.");
+
+        return version;
+    }
+
+    private static void WriteInfo(BinaryWriter writer, VehicleSyncInfo info)
+    {
+        writer.Write(BuildInfoFlags(info));
+        WriteString(writer, info.Ip);
+        writer.Write(info.Port);
+        writer.Write(info.X);
+        writer.Write(info.Y);
+        writer.Write(info.Th);
+        writer.Write(info.LayoutX);
+        writer.Write(info.LayoutY);
+        writer.Write(info.LayoutTh);
+        WriteString(writer, info.MotionInfeasibleReason);
+        WriteString(writer, info.RotateWheelAlignDetail);
+        writer.Write(info.AppliedNotificationSeq);
+    }
+
+    private static VehicleSyncInfo ReadInfo(BinaryReader reader, byte version)
+    {
+        var info = new VehicleSyncInfo();
+        ApplyInfoFlags(info, reader.ReadUInt16());
+        info.Ip = ReadString(reader);
+        info.Port = reader.ReadInt32();
+        info.X = reader.ReadSingle();
+        info.Y = reader.ReadSingle();
+        info.Th = reader.ReadSingle();
+        info.LayoutX = reader.ReadSingle();
+        info.LayoutY = reader.ReadSingle();
+        info.LayoutTh = reader.ReadSingle();
+        info.MotionInfeasibleReason = ReadString(reader);
+        info.RotateWheelAlignDetail = ReadString(reader);
+        info.AppliedNotificationSeq = version >= 2 ? reader.ReadInt64() : -1;
+        return info;
+    }
+
+    private static ushort BuildInfoFlags(VehicleSyncInfo info)
+    {
+        ushort flags = 0;
+        if (info.Master) flags |= 1 << 0;
+        if (info.PosAvailable) flags |= 1 << 1;
+        if (info.Aligned) flags |= 1 << 2;
+        if (info.DetectOk) flags |= 1 << 3;
+        if (info.MotionFeasible) flags |= 1 << 4;
+        if (info.RotateWheelsAligned) flags |= 1 << 5;
+        return flags;
+    }
+
+    private static void ApplyInfoFlags(VehicleSyncInfo info, ushort flags)
+    {
+        info.Master = (flags & (1 << 0)) != 0;
+        info.PosAvailable = (flags & (1 << 1)) != 0;
+        info.Aligned = (flags & (1 << 2)) != 0;
+        info.DetectOk = (flags & (1 << 3)) != 0;
+        info.MotionFeasible = (flags & (1 << 4)) != 0;
+        info.RotateWheelsAligned = (flags & (1 << 5)) != 0;
+    }
+
+    private static ushort BuildNotificationFlags(VehicleSyncNotification notification)
+    {
+        ushort flags = 0;
+        if (notification.PosAvailable) flags |= 1 << 0;
+        if (notification.Aligned) flags |= 1 << 1;
+        if (notification.FleetMotionReleased) flags |= 1 << 2;
+        if (notification.FleetStopActive) flags |= 1 << 3;
+        if (notification.AutoEnabled) flags |= 1 << 4;
+        if (notification.ManualEnabled) flags |= 1 << 5;
+        if (notification.HasIdeal) flags |= 1 << 6;
+        if (notification.RotateParamsValid) flags |= 1 << 7;
+        if (notification.UseDetourCorrection) flags |= 1 << 8;
+        return flags;
+    }
+
+    private static void ApplyNotificationFlags(VehicleSyncNotification notification, ushort flags)
+    {
+        notification.PosAvailable = (flags & (1 << 0)) != 0;
+        notification.Aligned = (flags & (1 << 1)) != 0;
+        notification.FleetMotionReleased = (flags & (1 << 2)) != 0;
+        notification.FleetStopActive = (flags & (1 << 3)) != 0;
+        notification.AutoEnabled = (flags & (1 << 4)) != 0;
+        notification.ManualEnabled = (flags & (1 << 5)) != 0;
+        notification.HasIdeal = (flags & (1 << 6)) != 0;
+        notification.RotateParamsValid = (flags & (1 << 7)) != 0;
+        notification.UseDetourCorrection = (flags & (1 << 8)) != 0;
+    }
+
+    private static void WriteString(BinaryWriter writer, string value)
+    {
+        var bytes = Encoding.UTF8.GetBytes(value ?? "");
+        if (bytes.Length > ushort.MaxValue)
+            throw new InvalidOperationException($"String payload length {bytes.Length} exceeds binary protocol limit.");
+        writer.Write((ushort)bytes.Length);
+        writer.Write(bytes);
+    }
+
+    private static string ReadString(BinaryReader reader)
+    {
+        var length = reader.ReadUInt16();
+        var bytes = reader.ReadBytes(length);
+        if (bytes.Length != length)
+            throw new EndOfStreamException("Truncated multi-vehicle sync string payload.");
+        return Encoding.UTF8.GetString(bytes);
+    }
+
+    private static void EnsureFullyRead(MemoryStream stream)
+    {
+        if (stream.Position != stream.Length)
+            throw new InvalidDataException("Unexpected trailing bytes in multi-vehicle sync packet.");
+    }
+}

ClumsyPilot/VehicleSyncModels.cs

@@ -0,0 +1,75 @@
+using System.Collections.Generic;
+using ClumsyCore;
+using Newtonsoft.Json;
+
+namespace MultiWheelC;
+
+public class VehicleSyncInfo
+{
+    [JsonProperty("Master")] public bool Master { get; set; }
+    [JsonProperty("Ip")] public string Ip { get; set; } = "";
+    [JsonProperty("Port")] public int Port { get; set; } = 8008;
+    [JsonProperty("PosAvailable")] public bool PosAvailable { get; set; }
+    [JsonProperty("X")] public float X { get; set; }
+    [JsonProperty("Y")] public float Y { get; set; }
+    [JsonProperty("Th")] public float Th { get; set; }
+    [JsonProperty("LayoutX")] public float LayoutX { get; set; }
+    [JsonProperty("LayoutY")] public float LayoutY { get; set; }
+    [JsonProperty("LayoutTh")] public float LayoutTh { get; set; }
+    [JsonProperty("Aligned")] public bool Aligned { get; set; }
+    // 本车本轮是否成功识别到邻车（关闭互识别时恒为 true）。任一车为 false 则整队停车。
+    [JsonProperty("DetectOk")] public bool DetectOk { get; set; }
+    [JsonProperty("MotionFeasible")] public bool MotionFeasible { get; set; } = true;
+    [JsonProperty("MotionInfeasibleReason")] public string MotionInfeasibleReason { get; set; } = "";
+    [JsonProperty("RotateWheelsAligned")] public bool RotateWheelsAligned { get; set; } = true;
+    [JsonProperty("RotateWheelAlignDetail")] public string RotateWheelAlignDetail { get; set; } = "";
+    [JsonProperty("AppliedNotificationSeq")] public long AppliedNotificationSeq { get; set; } = -1;
+}
+
+public class VehicleSyncNotification
+{
+    [JsonProperty("PosAvailable")] public bool PosAvailable { get; set; }
+    [JsonProperty("CenterX")] public float CenterX { get; set; }
+    [JsonProperty("CenterY")] public float CenterY { get; set; }
+    [JsonProperty("CenterTh")] public float CenterTh { get; set; }
+    [JsonProperty("Aligned")] public bool Aligned { get; set; }
+    [JsonProperty("Fleet")] public Dictionary<int, VehicleSyncInfo> Fleet { get; set; } = new();
+    [JsonProperty("FleetVx")] public float FleetVx { get; set; }
+    [JsonProperty("FleetFrontTh")] public float FleetFrontTh { get; set; }
+    [JsonProperty("FleetRearTh")] public float FleetRearTh { get; set; }
+    // 联动运动模式：0=常规(前进+转向) 1=蟹行(四轮同向平移) 2=原地旋转(绕车队中心)
+    [JsonProperty("Mode")] public int Mode { get; set; }
+    // 原地旋转角速度(deg/s，逆时针为正)，仅 Mode==2 有效
+    [JsonProperty("FleetOmega")] public float FleetOmega { get; set; }
+    [JsonProperty("RequestedFleetOmega")] public float RequestedFleetOmega { get; set; }
+    [JsonProperty("FleetMotionReleased")] public bool FleetMotionReleased { get; set; } = true;
+    [JsonProperty("FleetStopActive")] public bool FleetStopActive { get; set; }
+    [JsonProperty("FleetStopReason")] public string FleetStopReason { get; set; } = "";
+    [JsonProperty("FleetStopSourceCar")] public int FleetStopSourceCar { get; set; }
+    [JsonProperty("AutoEnabled")] public bool AutoEnabled { get; set; }
+    [JsonProperty("ManualEnabled")] public bool ManualEnabled { get; set; }
+    [JsonProperty("UseDetourCorrection")] public bool UseDetourCorrection { get; set; }
+    [JsonProperty("SyncTh")] public float SyncTh { get; set; }
+    [JsonProperty("SyncDistance")] public float SyncDistance { get; set; }
+    [JsonProperty("DeltaDetectCenter")] public float DeltaDetectCenter { get; set; }
+    // 原地旋转纠偏参数由主车广播，从车运行时使用同一套增益/限幅，避免主从补偿强度不一致。
+    [JsonProperty("RotateParamsValid")] public bool RotateParamsValid { get; set; }
+    [JsonProperty("RotateActiveOmega")] public float RotateActiveOmega { get; set; }
+    [JsonProperty("RotateCompXyFac")] public float RotateCompXyFac { get; set; }
+    [JsonProperty("RotateCompXyIFac")] public float RotateCompXyIFac { get; set; }
+    [JsonProperty("RotateCompXyMax")] public float RotateCompXyMax { get; set; }
+    [JsonProperty("RotateCompThFac")] public float RotateCompThFac { get; set; }
+    [JsonProperty("RotateCompThIFac")] public float RotateCompThIFac { get; set; }
+    [JsonProperty("RotateCompThMax")] public float RotateCompThMax { get; set; }
+    [JsonProperty("RotateCompTangentFrac")] public float RotateCompTangentFrac { get; set; }
+    [JsonProperty("RotateStartWheelAlignDeg")] public float RotateStartWheelAlignDeg { get; set; }
+    [JsonProperty("RotateActiveWheelAlignDeg")] public float RotateActiveWheelAlignDeg { get; set; }
+    // F: 单调递增序列号，从车据此丢弃乱序到达的旧 notify 包。
+    [JsonProperty("Seq")] public long Seq { get; set; }
+    // D: 自动模式下主车路径控制器算出的车队中心理想位姿（世界系），由 idealPos/idealAngle 透传而来。
+    // HasIdeal=true 时各从车按各自 layout 推算 per-car 目标位姿做前馈+补偿，弧线路径不再只靠事后纠偏。
+    [JsonProperty("HasIdeal")] public bool HasIdeal { get; set; }
+    [JsonProperty("IdealX")] public float IdealX { get; set; }
+    [JsonProperty("IdealY")] public float IdealY { get; set; }
+    [JsonProperty("IdealTh")] public float IdealTh { get; set; }
+}

ClumsyPilot/obj/ClumsyPilot.csproj.nuget.dgspec.json

@@ -0,0 +1,84 @@
+{
+  "format": 1,
+  "restore": {
+    "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj": {}
+  },
+  "projects": {
+    "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj": {
+      "version": "1.0.0",
+      "restore": {
+        "projectUniqueName": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
+        "projectName": "ClumsyPilot",
+        "projectPath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
+        "packagesPath": "C:\\Users\\Fairyland\\.nuget\\packages\\",
+        "outputPath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\obj\\",
+        "projectStyle": "PackageReference",
+        "fallbackFolders": [
+          "D:\\VS2022\\Shared\\NuGetPackages"
+        ],
+        "configFilePaths": [
+          "C:\\Users\\Fairyland\\AppData\\Roaming\\NuGet\\NuGet.Config",
+          "C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.FallbackLocation.config",
+          "C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.Offline.config"
+        ],
+        "originalTargetFrameworks": [
+          "netstandard2.0"
+        ],
+        "sources": {
+          "C:\\Program Files (x86)\\Microsoft SDKs\\NuGetPackages\\": {},
+          "https://api.nuget.org/v3/index.json": {}
+        },
+        "frameworks": {
+          "netstandard2.0": {
+            "targetAlias": "netstandard2.0",
+            "projectReferences": {}
+          }
+        },
+        "warningProperties": {
+          "warnAsError": [
+            "NU1605"
+          ]
+        },
+        "restoreAuditProperties": {
+          "enableAudit": "true",
+          "auditLevel": "low",
+          "auditMode": "direct"
+        },
+        "SdkAnalysisLevel": "9.0.300"
+      },
+      "frameworks": {
+        "netstandard2.0": {
+          "targetAlias": "netstandard2.0",
+          "dependencies": {
+            "NETStandard.Library": {
+              "suppressParent": "All",
+              "target": "Package",
+              "version": "[2.0.3, )",
+              "autoReferenced": true
+            },
+            "Newtonsoft.Json": {
+              "target": "Package",
+              "version": "[13.0.4, )"
+            },
+            "System.Numerics.Vectors": {
+              "target": "Package",
+              "version": "[4.6.1, )"
+            }
+          },
+          "imports": [
+            "net461",
+            "net462",
+            "net47",
+            "net471",
+            "net472",
+            "net48",
+            "net481"
+          ],
+          "assetTargetFallback": true,
+          "warn": true,
+          "runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.308\\RuntimeIdentifierGraph.json"
+        }
+      }
+    }
+  }
+}

ClumsyPilot/obj/ClumsyPilot.csproj.nuget.g.props

@@ -0,0 +1,16 @@
+<?xml version="1.0" encoding="utf-8" standalone="no"?>
+<Project ToolsVersion="14.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <PropertyGroup Condition=" '$(ExcludeRestorePackageImports)' != 'true' ">
+    <RestoreSuccess Condition=" '$(RestoreSuccess)' == '' ">True</RestoreSuccess>
+    <RestoreTool Condition=" '$(RestoreTool)' == '' ">NuGet</RestoreTool>
+    <ProjectAssetsFile Condition=" '$(ProjectAssetsFile)' == '' ">$(MSBuildThisFileDirectory)project.assets.json</ProjectAssetsFile>
+    <NuGetPackageRoot Condition=" '$(NuGetPackageRoot)' == '' ">$(UserProfile)\.nuget\packages\</NuGetPackageRoot>
+    <NuGetPackageFolders Condition=" '$(NuGetPackageFolders)' == '' ">C:\Users\Fairyland\.nuget\packages\;D:\VS2022\Shared\NuGetPackages</NuGetPackageFolders>
+    <NuGetProjectStyle Condition=" '$(NuGetProjectStyle)' == '' ">PackageReference</NuGetProjectStyle>
+    <NuGetToolVersion Condition=" '$(NuGetToolVersion)' == '' ">6.14.2</NuGetToolVersion>
+  </PropertyGroup>
+  <ItemGroup Condition=" '$(ExcludeRestorePackageImports)' != 'true' ">
+    <SourceRoot Include="C:\Users\Fairyland\.nuget\packages\" />
+    <SourceRoot Include="D:\VS2022\Shared\NuGetPackages\" />
+  </ItemGroup>
+</Project>

ClumsyPilot/obj/ClumsyPilot.csproj.nuget.g.targets

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="utf-8" standalone="no"?>
+<Project ToolsVersion="14.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <ImportGroup Condition=" '$(ExcludeRestorePackageImports)' != 'true' ">
+    <Import Project="$(NuGetPackageRoot)netstandard.library\2.0.3\build\netstandard2.0\NETStandard.Library.targets" Condition="Exists('$(NuGetPackageRoot)netstandard.library\2.0.3\build\netstandard2.0\NETStandard.Library.targets')" />
+  </ImportGroup>
+</Project>

ClumsyPilot/obj/Debug/.NETStandard,Version=v2.0.AssemblyAttributes.cs

@@ -0,0 +1,4 @@
+// <autogenerated />
+using System;
+using System.Reflection;
+[assembly: global::System.Runtime.Versioning.TargetFrameworkAttribute(".NETStandard,Version=v2.0", FrameworkDisplayName = ".NET Standard 2.0")]

ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfo.cs

@@ -0,0 +1,23 @@
+//------------------------------------------------------------------------------
+// <auto-generated>
+//     此代码由工具生成。
+//     运行时版本:4.0.30319.42000
+//
+//     对此文件的更改可能会导致不正确的行为，并且如果
+//     重新生成代码，这些更改将会丢失。
+// </auto-generated>
+//------------------------------------------------------------------------------
+
+using System;
+using System.Reflection;
+
+[assembly: System.Reflection.AssemblyCompanyAttribute("ClumsyPilot")]
+[assembly: System.Reflection.AssemblyConfigurationAttribute("Debug")]
+[assembly: System.Reflection.AssemblyFileVersionAttribute("1.0.0.0")]
+[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0")]
+[assembly: System.Reflection.AssemblyProductAttribute("ClumsyPilot")]
+[assembly: System.Reflection.AssemblyTitleAttribute("ClumsyPilot")]
+[assembly: System.Reflection.AssemblyVersionAttribute("1.0.0.0")]
+
+// 由 MSBuild WriteCodeFragment 类生成。
+

ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfoInputs.cache

@@ -0,0 +1 @@
+5d77794fa0720c6591db5b06ac60427413c18989a6f7b64420ccb07d122d85bc

ClumsyPilot/obj/Debug/ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig

@@ -0,0 +1,8 @@
+is_global = true
+build_property.RootNamespace = ClumsyPilot
+build_property.ProjectDir = D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\
+build_property.EnableComHosting = 
+build_property.EnableGeneratedComInterfaceComImportInterop = 
+build_property.CsWinRTUseWindowsUIXamlProjections = false
+build_property.EffectiveAnalysisLevelStyle = 
+build_property.EnableCodeStyleSeverity = 

ClumsyPilot/obj/Debug/ClumsyPilot.csproj.CoreCompileInputs.cache

@@ -0,0 +1 @@
+622a214d5f25f6580d0c98fb28e188cba923d74f280b70d75d03c4009c18c55b

ClumsyPilot/obj/Debug/ClumsyPilot.csproj.FileListAbsolute.txt

@@ -0,0 +1,11 @@
+D:\@FariyLandTask\@FRLD-GitProject\build\Clumsy\ClumsyPilot.deps.json
+D:\@FariyLandTask\@FRLD-GitProject\build\Clumsy\ClumsyPilot.dll
+D:\@FariyLandTask\@FRLD-GitProject\build\Clumsy\ClumsyPilot.pdb
+D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.csproj.AssemblyReference.cache
+D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig
+D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.AssemblyInfoInputs.cache
+D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.AssemblyInfo.cs
+D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.csproj.CoreCompileInputs.cache
+D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPi.5EF10E9F.Up2Date
+D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.dll
+D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.pdb

ClumsyPilot/obj/Debug/netstandard2.0/.NETStandard,Version=v2.0.AssemblyAttributes.cs

@@ -0,0 +1,4 @@
+// <autogenerated />
+using System;
+using System.Reflection;
+[assembly: global::System.Runtime.Versioning.TargetFrameworkAttribute(".NETStandard,Version=v2.0", FrameworkDisplayName = ".NET Standard 2.0")]

ClumsyPilot/obj/Debug/netstandard2.0/ClumsyPilot.AssemblyInfo.cs

@@ -0,0 +1,23 @@
+//------------------------------------------------------------------------------
+// <auto-generated>
+//     此代码由工具生成。
+//     运行时版本:4.0.30319.42000
+//
+//     对此文件的更改可能会导致不正确的行为，并且如果
+//     重新生成代码，这些更改将会丢失。
+// </auto-generated>
+//------------------------------------------------------------------------------
+
+using System;
+using System.Reflection;
+
+[assembly: System.Reflection.AssemblyCompanyAttribute("ClumsyPilot")]
+[assembly: System.Reflection.AssemblyConfigurationAttribute("Debug")]
+[assembly: System.Reflection.AssemblyFileVersionAttribute("1.0.0.0")]
+[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0")]
+[assembly: System.Reflection.AssemblyProductAttribute("ClumsyPilot")]
+[assembly: System.Reflection.AssemblyTitleAttribute("ClumsyPilot")]
+[assembly: System.Reflection.AssemblyVersionAttribute("1.0.0.0")]
+
+// 由 MSBuild WriteCodeFragment 类生成。
+

ClumsyPilot/obj/Debug/netstandard2.0/ClumsyPilot.AssemblyInfoInputs.cache

@@ -0,0 +1 @@
+5d77794fa0720c6591db5b06ac60427413c18989a6f7b64420ccb07d122d85bc

ClumsyPilot/obj/Debug/netstandard2.0/ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig

@@ -0,0 +1,8 @@
+is_global = true
+build_property.RootNamespace = ClumsyPilot
+build_property.ProjectDir = D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\
+build_property.EnableComHosting = 
+build_property.EnableGeneratedComInterfaceComImportInterop = 
+build_property.CsWinRTUseWindowsUIXamlProjections = false
+build_property.EffectiveAnalysisLevelStyle = 
+build_property.EnableCodeStyleSeverity = 

ClumsyPilot/obj/Debug/netstandard2.0/ClumsyPilot.csproj.CoreCompileInputs.cache

@@ -0,0 +1 @@
+66346c5ecc38869ab6b6f447e1f54c24aba69f81ee10b5736dee140cd5a474b8

ClumsyPilot/obj/Debug/netstandard2.0/ClumsyPilot.csproj.FileListAbsolute.txt

@@ -0,0 +1,5 @@
+D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\netstandard2.0\ClumsyPilot.csproj.AssemblyReference.cache
+D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\netstandard2.0\ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig
+D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\netstandard2.0\ClumsyPilot.AssemblyInfoInputs.cache
+D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\netstandard2.0\ClumsyPilot.AssemblyInfo.cs
+D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\netstandard2.0\ClumsyPilot.csproj.CoreCompileInputs.cache

ClumsyPilot/obj/project.assets.json

@@ -0,0 +1,341 @@
+{
+  "version": 3,
+  "targets": {
+    ".NETStandard,Version=v2.0": {
+      "Microsoft.NETCore.Platforms/1.1.0": {
+        "type": "package",
+        "compile": {
+          "lib/netstandard1.0/_._": {}
+        },
+        "runtime": {
+          "lib/netstandard1.0/_._": {}
+        }
+      },
+      "NETStandard.Library/2.0.3": {
+        "type": "package",
+        "dependencies": {
+          "Microsoft.NETCore.Platforms": "1.1.0"
+        },
+        "compile": {
+          "lib/netstandard1.0/_._": {}
+        },
+        "runtime": {
+          "lib/netstandard1.0/_._": {}
+        },
+        "build": {
+          "build/netstandard2.0/NETStandard.Library.targets": {}
+        }
+      },
+      "Newtonsoft.Json/13.0.4": {
+        "type": "package",
+        "compile": {
+          "lib/netstandard2.0/Newtonsoft.Json.dll": {
+            "related": ".xml"
+          }
+        },
+        "runtime": {
+          "lib/netstandard2.0/Newtonsoft.Json.dll": {
+            "related": ".xml"
+          }
+        }
+      },
+      "System.Numerics.Vectors/4.6.1": {
+        "type": "package",
+        "compile": {
+          "lib/netstandard2.0/System.Numerics.Vectors.dll": {
+            "related": ".xml"
+          }
+        },
+        "runtime": {
+          "lib/netstandard2.0/System.Numerics.Vectors.dll": {
+            "related": ".xml"
+          }
+        }
+      }
+    }
+  },
+  "libraries": {
+    "Microsoft.NETCore.Platforms/1.1.0": {
+      "sha512": "kz0PEW2lhqygehI/d6XsPCQzD7ff7gUJaVGPVETX611eadGsA3A877GdSlU0LRVMCTH/+P3o2iDTak+S08V2+A==",
+      "type": "package",
+      "path": "microsoft.netcore.platforms/1.1.0",
+      "files": [
+        ".nupkg.metadata",
+        ".signature.p7s",
+        "ThirdPartyNotices.txt",
+        "dotnet_library_license.txt",
+        "lib/netstandard1.0/_._",
+        "microsoft.netcore.platforms.1.1.0.nupkg.sha512",
+        "microsoft.netcore.platforms.nuspec",
+        "runtime.json"
+      ]
+    },
+    "NETStandard.Library/2.0.3": {
+      "sha512": "st47PosZSHrjECdjeIzZQbzivYBJFv6P2nv4cj2ypdI204DO+vZ7l5raGMiX4eXMJ53RfOIg+/s4DHVZ54Nu2A==",
+      "type": "package",
+      "path": "netstandard.library/2.0.3",
+      "files": [
+        ".nupkg.metadata",
+        ".signature.p7s",
+        "LICENSE.TXT",
+        "THIRD-PARTY-NOTICES.TXT",
+        "build/netstandard2.0/NETStandard.Library.targets",
+        "build/netstandard2.0/ref/Microsoft.Win32.Primitives.dll",
+        "build/netstandard2.0/ref/System.AppContext.dll",
+        "build/netstandard2.0/ref/System.Collections.Concurrent.dll",
+        "build/netstandard2.0/ref/System.Collections.NonGeneric.dll",
+        "build/netstandard2.0/ref/System.Collections.Specialized.dll",
+        "build/netstandard2.0/ref/System.Collections.dll",
+        "build/netstandard2.0/ref/System.ComponentModel.Composition.dll",
+        "build/netstandard2.0/ref/System.ComponentModel.EventBasedAsync.dll",
+        "build/netstandard2.0/ref/System.ComponentModel.Primitives.dll",
+        "build/netstandard2.0/ref/System.ComponentModel.TypeConverter.dll",
+        "build/netstandard2.0/ref/System.ComponentModel.dll",
+        "build/netstandard2.0/ref/System.Console.dll",
+        "build/netstandard2.0/ref/System.Core.dll",
+        "build/netstandard2.0/ref/System.Data.Common.dll",
+        "build/netstandard2.0/ref/System.Data.dll",
+        "build/netstandard2.0/ref/System.Diagnostics.Contracts.dll",
+        "build/netstandard2.0/ref/System.Diagnostics.Debug.dll",
+        "build/netstandard2.0/ref/System.Diagnostics.FileVersionInfo.dll",
+        "build/netstandard2.0/ref/System.Diagnostics.Process.dll",
+        "build/netstandard2.0/ref/System.Diagnostics.StackTrace.dll",
+        "build/netstandard2.0/ref/System.Diagnostics.TextWriterTraceListener.dll",
+        "build/netstandard2.0/ref/System.Diagnostics.Tools.dll",
+        "build/netstandard2.0/ref/System.Diagnostics.TraceSource.dll",
+        "build/netstandard2.0/ref/System.Diagnostics.Tracing.dll",
+        "build/netstandard2.0/ref/System.Drawing.Primitives.dll",
+        "build/netstandard2.0/ref/System.Drawing.dll",
+        "build/netstandard2.0/ref/System.Dynamic.Runtime.dll",
+        "build/netstandard2.0/ref/System.Globalization.Calendars.dll",
+        "build/netstandard2.0/ref/System.Globalization.Extensions.dll",
+        "build/netstandard2.0/ref/System.Globalization.dll",
+        "build/netstandard2.0/ref/System.IO.Compression.FileSystem.dll",
+        "build/netstandard2.0/ref/System.IO.Compression.ZipFile.dll",
+        "build/netstandard2.0/ref/System.IO.Compression.dll",
+        "build/netstandard2.0/ref/System.IO.FileSystem.DriveInfo.dll",
+        "build/netstandard2.0/ref/System.IO.FileSystem.Primitives.dll",
+        "build/netstandard2.0/ref/System.IO.FileSystem.Watcher.dll",
+        "build/netstandard2.0/ref/System.IO.FileSystem.dll",
+        "build/netstandard2.0/ref/System.IO.IsolatedStorage.dll",
+        "build/netstandard2.0/ref/System.IO.MemoryMappedFiles.dll",
+        "build/netstandard2.0/ref/System.IO.Pipes.dll",
+        "build/netstandard2.0/ref/System.IO.UnmanagedMemoryStream.dll",
+        "build/netstandard2.0/ref/System.IO.dll",
+        "build/netstandard2.0/ref/System.Linq.Expressions.dll",
+        "build/netstandard2.0/ref/System.Linq.Parallel.dll",
+        "build/netstandard2.0/ref/System.Linq.Queryable.dll",
+        "build/netstandard2.0/ref/System.Linq.dll",
+        "build/netstandard2.0/ref/System.Net.Http.dll",
+        "build/netstandard2.0/ref/System.Net.NameResolution.dll",
+        "build/netstandard2.0/ref/System.Net.NetworkInformation.dll",
+        "build/netstandard2.0/ref/System.Net.Ping.dll",
+        "build/netstandard2.0/ref/System.Net.Primitives.dll",
+        "build/netstandard2.0/ref/System.Net.Requests.dll",
+        "build/netstandard2.0/ref/System.Net.Security.dll",
+        "build/netstandard2.0/ref/System.Net.Sockets.dll",
+        "build/netstandard2.0/ref/System.Net.WebHeaderCollection.dll",
+        "build/netstandard2.0/ref/System.Net.WebSockets.Client.dll",
+        "build/netstandard2.0/ref/System.Net.WebSockets.dll",
+        "build/netstandard2.0/ref/System.Net.dll",
+        "build/netstandard2.0/ref/System.Numerics.dll",
+        "build/netstandard2.0/ref/System.ObjectModel.dll",
+        "build/netstandard2.0/ref/System.Reflection.Extensions.dll",
+        "build/netstandard2.0/ref/System.Reflection.Primitives.dll",
+        "build/netstandard2.0/ref/System.Reflection.dll",
+        "build/netstandard2.0/ref/System.Resources.Reader.dll",
+        "build/netstandard2.0/ref/System.Resources.ResourceManager.dll",
+        "build/netstandard2.0/ref/System.Resources.Writer.dll",
+        "build/netstandard2.0/ref/System.Runtime.CompilerServices.VisualC.dll",
+        "build/netstandard2.0/ref/System.Runtime.Extensions.dll",
+        "build/netstandard2.0/ref/System.Runtime.Handles.dll",
+        "build/netstandard2.0/ref/System.Runtime.InteropServices.RuntimeInformation.dll",
+        "build/netstandard2.0/ref/System.Runtime.InteropServices.dll",
+        "build/netstandard2.0/ref/System.Runtime.Numerics.dll",
+        "build/netstandard2.0/ref/System.Runtime.Serialization.Formatters.dll",
+        "build/netstandard2.0/ref/System.Runtime.Serialization.Json.dll",
+        "build/netstandard2.0/ref/System.Runtime.Serialization.Primitives.dll",
+        "build/netstandard2.0/ref/System.Runtime.Serialization.Xml.dll",
+        "build/netstandard2.0/ref/System.Runtime.Serialization.dll",
+        "build/netstandard2.0/ref/System.Runtime.dll",
+        "build/netstandard2.0/ref/System.Security.Claims.dll",
+        "build/netstandard2.0/ref/System.Security.Cryptography.Algorithms.dll",
+        "build/netstandard2.0/ref/System.Security.Cryptography.Csp.dll",
+        "build/netstandard2.0/ref/System.Security.Cryptography.Encoding.dll",
+        "build/netstandard2.0/ref/System.Security.Cryptography.Primitives.dll",
+        "build/netstandard2.0/ref/System.Security.Cryptography.X509Certificates.dll",
+        "build/netstandard2.0/ref/System.Security.Principal.dll",
+        "build/netstandard2.0/ref/System.Security.SecureString.dll",
+        "build/netstandard2.0/ref/System.ServiceModel.Web.dll",
+        "build/netstandard2.0/ref/System.Text.Encoding.Extensions.dll",
+        "build/netstandard2.0/ref/System.Text.Encoding.dll",
+        "build/netstandard2.0/ref/System.Text.RegularExpressions.dll",
+        "build/netstandard2.0/ref/System.Threading.Overlapped.dll",
+        "build/netstandard2.0/ref/System.Threading.Tasks.Parallel.dll",
+        "build/netstandard2.0/ref/System.Threading.Tasks.dll",
+        "build/netstandard2.0/ref/System.Threading.Thread.dll",
+        "build/netstandard2.0/ref/System.Threading.ThreadPool.dll",
+        "build/netstandard2.0/ref/System.Threading.Timer.dll",
+        "build/netstandard2.0/ref/System.Threading.dll",
+        "build/netstandard2.0/ref/System.Transactions.dll",
+        "build/netstandard2.0/ref/System.ValueTuple.dll",
+        "build/netstandard2.0/ref/System.Web.dll",
+        "build/netstandard2.0/ref/System.Windows.dll",
+        "build/netstandard2.0/ref/System.Xml.Linq.dll",
+        "build/netstandard2.0/ref/System.Xml.ReaderWriter.dll",
+        "build/netstandard2.0/ref/System.Xml.Serialization.dll",
+        "build/netstandard2.0/ref/System.Xml.XDocument.dll",
+        "build/netstandard2.0/ref/System.Xml.XPath.XDocument.dll",
+        "build/netstandard2.0/ref/System.Xml.XPath.dll",
+        "build/netstandard2.0/ref/System.Xml.XmlDocument.dll",
+        "build/netstandard2.0/ref/System.Xml.XmlSerializer.dll",
+        "build/netstandard2.0/ref/System.Xml.dll",
+        "build/netstandard2.0/ref/System.dll",
+        "build/netstandard2.0/ref/mscorlib.dll",
+        "build/netstandard2.0/ref/netstandard.dll",
+        "build/netstandard2.0/ref/netstandard.xml",
+        "lib/netstandard1.0/_._",
+        "netstandard.library.2.0.3.nupkg.sha512",
+        "netstandard.library.nuspec"
+      ]
+    },
+    "Newtonsoft.Json/13.0.4": {
+      "sha512": "pdgNNMai3zv51W5aq268sujXUyx7SNdE2bj1wZcWjAQrKMFZV260lbqYop1d2GM67JI1huLRwxo9ZqnfF/lC6A==",
+      "type": "package",
+      "path": "newtonsoft.json/13.0.4",
+      "files": [
+        ".nupkg.metadata",
+        ".signature.p7s",
+        "LICENSE.md",
+        "README.md",
+        "lib/net20/Newtonsoft.Json.dll",
+        "lib/net20/Newtonsoft.Json.xml",
+        "lib/net35/Newtonsoft.Json.dll",
+        "lib/net35/Newtonsoft.Json.xml",
+        "lib/net40/Newtonsoft.Json.dll",
+        "lib/net40/Newtonsoft.Json.xml",
+        "lib/net45/Newtonsoft.Json.dll",
+        "lib/net45/Newtonsoft.Json.xml",
+        "lib/net6.0/Newtonsoft.Json.dll",
+        "lib/net6.0/Newtonsoft.Json.xml",
+        "lib/netstandard1.0/Newtonsoft.Json.dll",
+        "lib/netstandard1.0/Newtonsoft.Json.xml",
+        "lib/netstandard1.3/Newtonsoft.Json.dll",
+        "lib/netstandard1.3/Newtonsoft.Json.xml",
+        "lib/netstandard2.0/Newtonsoft.Json.dll",
+        "lib/netstandard2.0/Newtonsoft.Json.xml",
+        "newtonsoft.json.13.0.4.nupkg.sha512",
+        "newtonsoft.json.nuspec",
+        "packageIcon.png"
+      ]
+    },
+    "System.Numerics.Vectors/4.6.1": {
+      "sha512": "sQxefTnhagrhoq2ReR0D/6K0zJcr9Hrd6kikeXsA1I8kOCboTavcUC4r7TSfpKFeE163uMuxZcyfO1mGO3EN8Q==",
+      "type": "package",
+      "path": "system.numerics.vectors/4.6.1",
+      "files": [
+        ".nupkg.metadata",
+        ".signature.p7s",
+        "Icon.png",
+        "PACKAGE.md",
+        "buildTransitive/net461/System.Numerics.Vectors.targets",
+        "buildTransitive/net462/_._",
+        "lib/net462/System.Numerics.Vectors.dll",
+        "lib/net462/System.Numerics.Vectors.xml",
+        "lib/netcoreapp2.0/_._",
+        "lib/netstandard2.0/System.Numerics.Vectors.dll",
+        "lib/netstandard2.0/System.Numerics.Vectors.xml",
+        "lib/netstandard2.1/_._",
+        "system.numerics.vectors.4.6.1.nupkg.sha512",
+        "system.numerics.vectors.nuspec"
+      ]
+    }
+  },
+  "projectFileDependencyGroups": {
+    ".NETStandard,Version=v2.0": [
+      "NETStandard.Library >= 2.0.3",
+      "Newtonsoft.Json >= 13.0.4",
+      "System.Numerics.Vectors >= 4.6.1"
+    ]
+  },
+  "packageFolders": {
+    "C:\\Users\\Fairyland\\.nuget\\packages\\": {},
+    "D:\\VS2022\\Shared\\NuGetPackages": {}
+  },
+  "project": {
+    "version": "1.0.0",
+    "restore": {
+      "projectUniqueName": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
+      "projectName": "ClumsyPilot",
+      "projectPath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
+      "packagesPath": "C:\\Users\\Fairyland\\.nuget\\packages\\",
+      "outputPath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\obj\\",
+      "projectStyle": "PackageReference",
+      "fallbackFolders": [
+        "D:\\VS2022\\Shared\\NuGetPackages"
+      ],
+      "configFilePaths": [
+        "C:\\Users\\Fairyland\\AppData\\Roaming\\NuGet\\NuGet.Config",
+        "C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.FallbackLocation.config",
+        "C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.Offline.config"
+      ],
+      "originalTargetFrameworks": [
+        "netstandard2.0"
+      ],
+      "sources": {
+        "C:\\Program Files (x86)\\Microsoft SDKs\\NuGetPackages\\": {},
+        "https://api.nuget.org/v3/index.json": {}
+      },
+      "frameworks": {
+        "netstandard2.0": {
+          "targetAlias": "netstandard2.0",
+          "projectReferences": {}
+        }
+      },
+      "warningProperties": {
+        "warnAsError": [
+          "NU1605"
+        ]
+      },
+      "restoreAuditProperties": {
+        "enableAudit": "true",
+        "auditLevel": "low",
+        "auditMode": "direct"
+      },
+      "SdkAnalysisLevel": "9.0.300"
+    },
+    "frameworks": {
+      "netstandard2.0": {
+        "targetAlias": "netstandard2.0",
+        "dependencies": {
+          "NETStandard.Library": {
+            "suppressParent": "All",
+            "target": "Package",
+            "version": "[2.0.3, )",
+            "autoReferenced": true
+          },
+          "Newtonsoft.Json": {
+            "target": "Package",
+            "version": "[13.0.4, )"
+          },
+          "System.Numerics.Vectors": {
+            "target": "Package",
+            "version": "[4.6.1, )"
+          }
+        },
+        "imports": [
+          "net461",
+          "net462",
+          "net47",
+          "net471",
+          "net472",
+          "net48",
+          "net481"
+        ],
+        "assetTargetFallback": true,
+        "warn": true,
+        "runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.308\\RuntimeIdentifierGraph.json"
+      }
+    }
+  }
+}

ClumsyPilot/obj/project.nuget.cache

@@ -0,0 +1,13 @@
+{
+  "version": 2,
+  "dgSpecHash": "gHGYwM2i+So=",
+  "success": true,
+  "projectFilePath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
+  "expectedPackageFiles": [
+    "C:\\Users\\Fairyland\\.nuget\\packages\\microsoft.netcore.platforms\\1.1.0\\microsoft.netcore.platforms.1.1.0.nupkg.sha512",
+    "C:\\Users\\Fairyland\\.nuget\\packages\\netstandard.library\\2.0.3\\netstandard.library.2.0.3.nupkg.sha512",
+    "C:\\Users\\Fairyland\\.nuget\\packages\\newtonsoft.json\\13.0.4\\newtonsoft.json.13.0.4.nupkg.sha512",
+    "C:\\Users\\Fairyland\\.nuget\\packages\\system.numerics.vectors\\4.6.1\\system.numerics.vectors.4.6.1.nupkg.sha512"
+  ],
+  "logs": []
+}

MedullaAdapter/AlarmRoutine.cs

@@ -0,0 +1,21 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using CartActivator;
+using FundamentalLib;
+using MDCSToolBox.Medulla.Chassis;
+using MDCSToolBox.Medulla.Chassis.MultiWheel;
+
+namespace MedullaAdapter
+{
+    public class AlarmRoutine : MultiWheelAlarmRoutine<DiverCartDefinition>
+    {
+        public override void SetOtherAlarms()
+        {
+            AddAlarm("左夹臂驱动报警", 2, () => cart.LeftArmErrorCode != 0);
+            AddAlarm("右夹臂驱动报警", 2, () => cart.RightArmErrorCode != 0);
+            //AddAlarm("夹臂不同步报警", 2, () => cart.ClampOutOfSync);
+        }
+    }
+}

MedullaAdapter/DiverCartDefinition.cs

@@ -0,0 +1,278 @@
+using CartActivator;
+using MCUSerialBridgeCLR;
+using MDCSToolBox.Medulla.Chassis.MultiWheel;
+using Medulla.Types;
+using System;
+using System.Collections.Generic;
+using System.Runtime.InteropServices;
+using System.Text;
+using System.Threading;
+
+
+namespace MedullaAdapter
+{
+    [UseLadderLogic(logic = typeof(AlarmRoutine), scanInterval = 50)]
+    [UseLadderLogic(logic = typeof(MotorRoutine), scanInterval = 50)]
+    [UseLadderLogic(logic = typeof(MCURoutine), scanInterval = 20)]
+    [UseManualController(manualController = typeof(Remote))]
+    public partial class DiverCartDefinition : MultiWheelCartDefinition
+    {
+        public DiverCommunication Embedded;
+        public MCUSerialBridge Bridge;
+        [AsUpperIO(desc = "转弯半径")] public float Radius;
+        #region MultiVehicleCoordination
+
+        [AsInitParam(desc = "遥控器最大线速度")]
+        public float MaxManualSpeed = 0.3f;
+
+        [AsInitParam(desc = "遥控器最大角速度")]
+        public float MaxManualAngularSpeed = 45f;
+
+
+        [AsLowerIO(desc = "启用（手动）多车联动")] public bool MultiVehicleManualEnabled = false;
+        [AsLowerIO(desc = "（手动）多车联动模式")] public int MultiVehicleManualMode = 0;
+
+        [AsUpperIO(desc = "多车联动：声光同步，-1未启动，0灭，1亮")] public int MultiVehicleLightSync = -1;
+
+        [AsLowerIO(desc = "多车联动模式-暂停")] public bool MultiVehicleHold = false;
+
+        [AsLowerIO(desc = "多车联动：遥控器Vx")] public float MultiVehicleManualVx = 0f;
+        [AsLowerIO(desc = "多车联动：蟹行方向比例")] public float MultiVehicleManualVy = 0f;
+        [AsLowerIO(desc = "多车联动：遥控器Vth")] public float MultiVehicleManualVth = 0f;
+
+        #endregion
+        [IOObjectMonitor(desc = "左前左轮下发速度")] public float SpeedLFL;
+        [IOObjectMonitor(desc = "左前右轮下发速度")] public float SpeedLFR;
+        [IOObjectMonitor(desc = "右前左轮下发速度")] public float SpeedRFL;
+        [IOObjectMonitor(desc = "右前右轮下发速度")] public float SpeedRFR;
+        [IOObjectMonitor(desc = "左后左轮下发速度")] public float SpeedLRL;
+        [IOObjectMonitor(desc = "左后右轮下发速度")] public float SpeedLRR;
+        [IOObjectMonitor(desc = "右后左轮下发速度")] public float SpeedRRL;
+        [IOObjectMonitor(desc = "右后右轮下发速度")] public float SpeedRRR;
+        //[AsUpperIO(desc = "左前左轮下发速度", timeOutReset = true)] public float SpeedLFL;
+        //[AsUpperIO(desc = "左前右轮下发速度", timeOutReset = true)] public float SpeedLFR;
+        //[AsUpperIO(desc = "右前左轮下发速度", timeOutReset = true)] public float SpeedRFL;
+        //[AsUpperIO(desc = "右前右轮下发速度", timeOutReset = true)] public float SpeedRFR;
+        //[AsUpperIO(desc = "左后左轮下发速度", timeOutReset = true)] public float SpeedLRL;
+        //[AsUpperIO(desc = "左后右轮下发速度", timeOutReset = true)] public float SpeedLRR;
+        //[AsUpperIO(desc = "右后左轮下发速度", timeOutReset = true)] public float SpeedRRL;
+        //[AsUpperIO(desc = "右后右轮下发速度", timeOutReset = true)] public float SpeedRRR;
+
+        [AsLowerIO(desc = "左前左轮实际位置")] public float LFLActualPos;
+        [AsLowerIO(desc = "左前右轮实际位置")] public float LFRActualPos;
+        [AsLowerIO(desc = "右前左轮实际位置")] public float RFLActualPos;
+        [AsLowerIO(desc = "右前右轮实际位置")] public float RFRActualPos;
+        [AsLowerIO(desc = "左后左轮实际位置")] public float LRLActualPos;
+        [AsLowerIO(desc = "左后右轮实际位置")] public float LRRActualPos;
+        [AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
+        [AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
+
+        [AsUpperIO(desc = "左夹臂下发速度", timeOutReset = true)] public float SpeedLeftArm;
+        [AsUpperIO(desc = "右夹臂下发速度", timeOutReset = true)] public float SpeedRightArm;
+        [AsUpperIO(desc = "左夹臂实际速度")] public float ActualSpeedLeftArm;
+        [AsUpperIO(desc = "右夹臂实际速度")] public float ActualSpeedRightArm;
+        [AsLowerIO(desc = "左夹臂状态字")] public int LeftArmStateCode;
+        [AsLowerIO(desc = "右夹臂状态字")] public int RightArmStateCode;
+        [AsLowerIO(desc = "左夹臂错误字")] public int LeftArmErrorCode;
+        [AsLowerIO(desc = "右夹臂错误字")] public int RightArmErrorCode;
+        [AsLowerIO(desc = "左夹臂电流")] public float LeftArmElectric;
+        [AsLowerIO(desc = "右夹臂电流")] public float RightArmElectric;
+        [AsLowerIO(desc = "左夹臂实际位置")] public float ActualPosLeftArm;
+        [AsLowerIO(desc = "右夹臂实际位置")] public float ActualPosRightArm;
+        [AsLowerIO(desc = "test")] public float test;
+        [AsLowerIO(desc = "test1")] public byte test1;
+        [AsLowerIO(desc = "test2")] public byte test2;
+        [AsInitParam(desc = "test3")] public int test3=24;
+        [IOObjectMonitor(desc = "灯光模式")] public int LightMode = 0;
+        [AsInitParam(desc = "触发模式")] public int trigger = 0;
+
+        [AsInitParam(desc = "手动控制夹臂速度系数")] public float ManualArmSpeedFac = 1.0f;
+        [AsInitParam(desc = "车号")][AsLowerIO] public int CarNum = 1;
+        [AsInitParam(desc = "初始音量")] public int MusicVolume = 5;
+
+        [AsInitParam(desc = "左夹臂低限位")][AsLowerIO] public int LeftArmLowerPos = -10000;
+        [AsInitParam(desc = "左夹臂高限位")][AsLowerIO] public int LeftArmUpperPos = 5927610;
+        [AsInitParam(desc = "右夹臂低限位")][AsLowerIO] public int RightArmLowerPos = -17295;
+        [AsInitParam(desc = "右夹臂高限位")][AsLowerIO] public int RightArmUpperPos = 5927610;
+
+        [AsInitParam(desc = "MCU端口号")] public string MCUPort = "COM4";
+
+        [AsLowerIO(desc = "陀螺仪角度")] public float GyrosTh;
+        [AsLowerIO(desc = "电池健康状态")] public float SOH;
+
+        [AsLowerIO(desc = "左前左驱动器远程帧701")] public byte LFLRemoteCode = 0;
+        [AsLowerIO(desc = "左前右驱动器远程帧702")] public byte LFRRemoteCode = 0;
+        [AsLowerIO(desc = "右前左驱动器远程帧703")] public byte RFLRemoteCode = 0;
+        [AsLowerIO(desc = "右前右驱动器远程帧704")] public byte RFRRemoteCode = 0;
+        [AsLowerIO(desc = "左后左驱动器远程帧705")] public byte LRLRemoteCode = 0;
+        [AsLowerIO(desc = "左后右驱动器远程帧706")] public byte LRRRemoteCode = 0;
+        [AsLowerIO(desc = "右后左驱动器远程帧707")] public byte RRLRemoteCode = 0;
+        [AsLowerIO(desc = "右后右驱动器远程帧708")] public byte RRRRemoteCode = 0;
+        [AsLowerIO(desc = "左夹臂驱动器远程帧709")] public byte LArmRemoteCode = 0;
+        [AsLowerIO(desc = "右夹臂驱动器远程帧70A")] public byte RArmRemoteCode = 0;
+        [AsUpperIO(desc = "从M上复位")] public bool ResetFromM;
+        [AsUpperIO(desc = "从M将驱动轮下使能")] public bool DisableFromM;
+        [AsUpperIO(desc = "从C上复位")] public bool ResetFromC;
+        [AsUpperIO(desc = "从C将驱动轮下使能")] public bool DisableFromC;
+        [AsUpperIO(desc = "夹臂不同步报警")] public bool ClampOutOfSync;
+
+        internal ManualControlMode TransmitterControlMode = ManualControlMode.Normal;
+
+        [IOObjectMonitor] public float TransmitterSpeed = 0.3f;
+        [AsLowerIO(desc = "驱动轮使能状态")] public bool WheelAbleState = true;
+
+        [AsInitParam(desc = "遥控器速度上限")] public float TransmitterSpeedUpperLimit = 1.0f;
+        [AsInitParam(desc = "遥控器速度下限")] public float TransmitterSpeedLowerLimit = 0.0f;
+        internal DateTime TransmitterLastTime;
+
+        [IOObjectUtility]
+        public void WheelReset()
+        {
+            ResetFromM = true;
+        }
+
+        [IOObjectUtility]
+        public void WheelDisable()
+        {
+            DisableFromM = true;
+        }
+
+        public override void CommunicationInit()
+        {
+            if (GhostMode) return;
+
+            Bridge = new MCUSerialBridge();
+            //Step1:打开指定串口连接
+            var err = Bridge.Open(MCUPort, 1000000u);
+            if (err != MCUSerialBridgeError.OK)
+            {
+                Console.WriteLine("MCU Open FAILED: {0}", err.ToDescription());
+                return;
+            }
+            else
+            {
+                Console.WriteLine("MCU Open OK");
+            }
+
+            //Step2:远程复位MCU
+            err = Bridge.Reset();
+            if (err != MCUSerialBridgeError.OK)
+            {
+                Console.WriteLine("MCU Reset FAILED: {0}", err.ToDescription());
+                return;
+            }
+            else
+            {
+                Thread.Sleep(500);
+                Console.WriteLine("MCU Reset OK");
+            }
+
+            //Step3:GetVersion
+            err = Bridge.GetVersion(out var version, 100);
+            if (err != MCUSerialBridgeError.OK)
+            {
+                Console.WriteLine("MCU GetVersion FAILED: {0}", err.ToDescription());
+                return;
+            }
+            else
+            {
+                Console.WriteLine("MCU GetVersion OK: {0}", version.ToString());
+            }
+
+            //Step4:GetState
+            err = Bridge.GetState(out var state, 100);
+            if (err != MCUSerialBridgeError.OK)
+            {
+                Console.WriteLine("MCU GetState FAILED: {0}", err.ToDescription());
+                return;
+            }
+            else
+            {
+                Console.WriteLine("MCU GetState OK: {0}", state.ToString());
+            }
+
+            //Step5:串口/CAN配置
+            try
+            {
+                var ports = new List<PortConfig>();
+                for (int i = 0; i < 1; i++)
+                    ports.Add(new CANPortConfig(500000, 10));
+                for (int i = 0; i < 3; i++)
+                    ports.Add(new SerialPortConfig(9600, 10));
+                Console.WriteLine("=== Port Configuration ===");
+                for (int i = 0; i < ports.Count; i++)
+                {
+                    if (ports[i] is SerialPortConfig s)
+                        Console.WriteLine("Port {0}: Serial, Baud={1}, ReceiveFrameMs={2}",
+                            i,
+                            s.Baud,
+                            s.ReceiveFrameMs);
+                    else if (ports[i] is CANPortConfig c)
+                        Console.WriteLine("Port {0}: CAN, Baud={1}, RetryTimeMs={2}", i, c.Baud, c.RetryTimeMs);
+                }
+
+                var ret = Bridge.Configure(ports, 200);
+                if (ret != MCUSerialBridgeError.OK)
+                {
+                    Console.WriteLine("MCU Configure FAILED: {0}", ret.ToDescription());
+                    return;
+                }
+                else
+                {
+                    Console.WriteLine("MCU Configure OK");
+                }
+                Console.WriteLine("MCU Configure {0}", ret == MCUSerialBridgeError.OK ? "OK" : $"FAILED: 0x{(uint)ret:X8}");
+            }
+            catch (Exception ex)
+            {
+                Console.WriteLine("Configure Exception: {0}", ex.Message);
+            }
+            //MCUInterface<DiverCartDefinition>.Start(this);
+            State = 0;
+        }
+
+        internal void ManualControl(ManualControlMode mode, float x, float y, float frontDirection,
+                    float speedThreshold, TimeSpan? interval = null)
+        {
+            if (Chassis == null) return;
+
+            var speed = speedThreshold * y;
+            var thPow = (float)Math.Pow(Math.Abs(x), ManualThetaPow) * Math.Sign(x);
+            var frontTh = -thPow * MaxManualTheta;
+            var rearTh = thPow * MaxManualTheta;
+            switch (mode)
+            {
+                case ManualControlMode.Normal:
+                    ManualMode = 0;
+                    Chassis.DirectionAngle = frontDirection;
+                    Chassis.SendMotion(speed, frontTh, rearTh, interval);
+                    break;
+
+                case ManualControlMode.Sway:
+                    ManualMode = 1;
+                    Chassis.DirectionAngle = frontDirection;
+                    Chassis.SendMotion(speed, frontTh, -rearTh, interval);
+                    break;
+
+                case ManualControlMode.Crab:
+                    ManualMode = 2;
+                    Chassis.DirectionAngle = 90;
+                    Chassis.SendMotion(speed, frontTh, rearTh, interval);
+                    break;
+
+                case ManualControlMode.Spin:
+                    ManualMode = 3;
+                    Chassis.SendRotateMotion(speed * MaxAngularSpeed, interval);
+                    break;
+            }
+        }
+       
+        internal enum ManualControlMode
+        {
+            Normal = 0,
+            Spin = 1,
+            Crab = 2,
+            Sway = 3
+        }
+    }
+}

MedullaAdapter/DiverCommunication.cs

@@ -0,0 +1,144 @@
+using System;
+using System.Collections.Generic;
+using System.IO.Ports;
+using System.Text;
+using System.Threading;
+using FundamentalLib;
+
+
+namespace MedullaAdapter
+{
+    public class DiverCommunication
+    {
+        private byte[] _receiveBytes ;
+        private SerialPort _port;
+        private int _state = 0;
+        private List<byte> _receiveList = new List<byte>();
+        private int _length = 0;
+        
+        private MCUInterface<DiverCartDefinition> _interface;
+
+        public DiverCommunication(string name, int baudRate)
+        {
+            _interface = new MCUInterface<DiverCartDefinition>();
+            _port = new SerialPort();
+            _port.PortName = name; // 根据你的实际串口名称修改
+            _port.BaudRate = baudRate;
+            _port.Parity = Parity.None;
+            _port.DataBits = 8;
+            _port.StopBits = StopBits.One;
+            _port.Handshake = Handshake.None;
+            _port.Open();
+            Console.WriteLine($"buffer:{_port.ReadBufferSize}");
+            //_port.DataReceived += OnDataReceived;
+            new Thread(() =>
+            {
+                while (true)
+                {
+                    Thread.Sleep(10);
+                    try
+                    {
+                        int bytesToRead = _port.BytesToRead;
+                        if (bytesToRead > 0)
+                        {
+                            var readBuffer = new byte[bytesToRead];
+                            var start = DateTime.Now;
+                            _port.Read(readBuffer, 0, bytesToRead);
+                            var time1 = DateTime.Now - start;
+                            ProcessBuffer(readBuffer);
+                            var time2 = DateTime.Now - start;
+                            Hedingben.ToastText($"total time:{time2.TotalMilliseconds},read time:{time1},read count:{bytesToRead}", "timeDebug");
+                        }
+                    }
+                    catch (Exception exception)
+                    {
+                        Console.WriteLine(exception);
+                    }
+                }
+            }).Start();
+        }
+
+        private void ProcessBuffer(byte[] buffer)
+        {
+            for (int i = 0; i < buffer.Length; i++)
+            {
+                var newByte = buffer[i];
+                switch (_state)
+                {
+                    case 0:
+                        _receiveList.Clear();
+                        if (newByte == 0xBB)
+                        {
+                            _receiveList.Add(newByte);
+                            _state = 1;
+                        }
+                        break;
+                    case 1:
+                        if (newByte == 0xAA)
+                        {
+                            _receiveList.Add(newByte);
+                            _state = 2;
+                        }
+                        else
+                        {
+                            _state = 0;
+                        }
+                        break;
+                    case 2:
+                        _receiveList.Add(newByte);
+                        if (_receiveList.Count >= 4)
+                        {
+                            _length = BitConverter.ToUInt16(_receiveList.ToArray(), 2);
+                        }
+
+                        if (_receiveList.Count == _length + 7)
+                        {
+                            _state = 0;
+                            if (_receiveList[_receiveList.Count - 1] == 0xEE)
+                            {
+                                _receiveBytes = _receiveList.ToArray();
+                                Hedingben.ToastText($"receive from mcu:{BitConverter.ToString(_receiveBytes)}","DiverReceive");
+                                if (_receiveBytes[5] == 0xA0)
+                                {
+                                    var dataLength1 = BitConverter.ToUInt32(_receiveBytes, 6);
+                                    var data1 = new byte[dataLength1];
+                                    Array.Copy(_receiveBytes, 14, data1, 0, dataLength1);
+                                    MCUInterface<DiverCartDefinition>.NotifyLowerData("default", data1);
+                                    var logBytes = new byte[BitConverter.ToInt32(_receiveBytes, 10)];
+                                    var memorySize = BitConverter.ToInt32(_receiveBytes, 6);
+                                    if (logBytes.Length > 0)
+                                    {
+                                        Array.Copy(_receiveBytes, 14 + memorySize, logBytes, 0, logBytes.Length);
+                                        string log = System.Text.Encoding.ASCII.GetString(logBytes);
+                                        string[] result = log.Split(new[] { "\r\n", "\n" }, StringSplitOptions.None);
+                                        for (int j = 0; j < result.Length; j++)
+                                        {
+                                            Hedingben.ToastText(result[j], $"mcuLog" + j);
+                                        }
+                                    }
+                                }
+                            }
+                            else
+                            {
+                                _state = 0;
+                                Hedingben.ToastText($"mcu date error,should end with 0xEE , actual {_receiveList[_receiveList.Count - 1]}","DiverError");
+                            }
+                        }
+                        break;
+                    default:break;
+                }
+            }
+        }
+
+        public void SendMessage(byte[] data)
+        {
+            Hedingben.ToastText($"send to mcu:{BitConverter.ToString(data)}", "DiverSend");
+            _port.Write(data, 0, data.Length);
+        }
+
+        public byte[] GetMessage()
+        {
+            return _receiveBytes;
+        }
+    }
+}

MedullaAdapter/EmbeddedCommunication.cs

@@ -0,0 +1,181 @@
+using System;
+using System.Collections.Generic;
+using System.IO.Ports;
+using System.Linq;
+using System.Threading;
+using FundamentalLib;
+using Medulla;
+
+namespace MedullaAdapter
+{
+    public class EmbeddedCommunication
+    {
+        private SerialPort _port;
+
+        private byte[] _receiveBytes;
+
+        private byte[] _receiveData;
+
+        private const int BufferThreshold = 300; // 缓冲区大小阈值
+
+        private List<byte> _buffer = new List<byte>(); // 缓存接收到的数据
+
+        public bool CommunicationError = false;
+
+        private DateTime _lastTime = DateTime.Now;
+
+        private MCUInterface<DiverCartDefinition> _interface;
+
+        public EmbeddedCommunication(string name, int baudRate)
+        {
+            _interface = new MCUInterface<DiverCartDefinition>();
+            _port = new SerialPort();
+            _port.PortName = name; // 根据你的实际串口名称修改
+            _port.BaudRate = baudRate;
+            _port.Parity = Parity.None;
+            _port.DataBits = 8;
+            _port.StopBits = StopBits.One;
+            _port.Handshake = Handshake.None;
+            _port.Open();
+            _port.DataReceived += OnDataReceived;
+        }
+
+        public void SendMessage(byte[] data)
+        {
+            Hedingben.ToastText($"send to mcu:{BitConverter.ToString(data)}","DiverSend");
+            _port.Write(data, 0, data.Length);
+        }
+
+        public byte[] GetMessage()
+        {
+            return _receiveBytes;
+        }
+
+        private void OnDataReceived(object sender, SerialDataReceivedEventArgs e)
+        {
+            try
+            {
+                int bytesToRead = _port.BytesToRead;
+                byte[] buffer = new byte[bytesToRead];
+                var startRead = DateTime.Now;
+                _port.Read(buffer, 0, bytesToRead);
+                var time = DateTime.Now - startRead;
+                // 将新接收到的数据添加到缓冲区
+                startRead = DateTime.Now;
+                _buffer.AddRange(buffer);
+                var time2 = DateTime.Now - startRead;
+                Hedingben.ToastText($"buffer length:{_buffer.Count}","bufferDebug");
+                DLog.Log($"buffer length:{_buffer.Count}");
+                //if (_buffer.Count > BufferThreshold)
+                //{
+                //    _buffer.Clear();
+                //    return;
+                //}
+
+                // 尝试解析缓冲区中的报文
+                var parseTime = DateTime.Now;
+                ParseBuffer();
+                var time3 = DateTime.Now - parseTime;
+                Hedingben.ToastText(
+                    $"读取报文时间:{time.TotalMilliseconds},增加到缓存区:{time2.TotalMilliseconds},处理时间:{time3.TotalMilliseconds}",
+                    "timeDebug1");
+            }
+            catch (Exception ex)
+            {
+                Console.WriteLine($"读取串口数据时发生错误: {ex.Message}"+ex.StackTrace);
+            }
+        }
+        private void ParseBuffer()
+        {
+            while (_buffer.Count >= 7) // 报文的最小长度是 7
+            {
+                // 查找报文头部
+                var findTime = DateTime.Now;
+                int startIndex = _buffer.FindIndex(0, b => b == 0xBB);
+                var time1 = DateTime.Now - findTime;
+                if (startIndex == -1)
+                {
+                    // 如果没有找到头部或剩余长度不足最小报文长度，结束解析
+                    _buffer.Clear();
+                    return;
+                }
+
+                // 确保头部后还有至少 6 个字节
+                if (startIndex + 1 >= _buffer.Count || _buffer[startIndex + 1] != 0xAA)
+                {
+                    // 如果第二字节不是 0xAA，丢弃无效字节
+                    _buffer.RemoveAt(startIndex);
+                    continue;
+                }
+
+                // 检查数据段长度
+                if (startIndex + 4 >= _buffer.Count) break; // 数据不足，等待下次接收
+                int dataLength = _buffer[startIndex + 2] | (_buffer[startIndex + 3] << 8);
+
+                // 计算报文总长度
+                int totalLength = dataLength + 7;
+
+                // 检查总长度是否足够
+                if (startIndex + totalLength > _buffer.Count) break; // 数据不足，等待下次接收
+
+                // 检查尾部是否是 0xCC 0xEE
+                if (_buffer[startIndex + totalLength - 2] == 0xCC && _buffer[startIndex + totalLength - 1] == 0xEE)
+                {
+                    var time2 = DateTime.Now - findTime;
+                    var copyTime = DateTime.Now;
+                    // 提取完整报文
+                    _receiveBytes = _buffer.Skip(startIndex).Take(totalLength).ToArray();
+                    var time3 = DateTime.Now - copyTime;
+                    if (_receiveBytes[5] == 0xA0)
+                    {
+                        var dataLength1 = BitConverter.ToUInt32(_receiveBytes, 6);
+                        var data1 = new byte[dataLength1];
+                        Array.Copy(_receiveBytes, 14, data1, 0, dataLength1);
+                        var notifyTime = DateTime.Now;
+                        MCUInterface<DiverCartDefinition>.NotifyLowerData("default", data1);
+                        var time4 = DateTime.Now - notifyTime;
+                        var logBytes = new byte[BitConverter.ToInt32(_receiveBytes, 10)];
+                        var memorySize = BitConverter.ToInt32(_receiveBytes, 6);
+                        if (logBytes.Length > 0)
+                        {
+                            Array.Copy(_receiveBytes, 14 + memorySize, logBytes, 0, logBytes.Length);
+                            string log = System.Text.Encoding.ASCII.GetString(logBytes);
+                            string[] result = log.Split(new[] { "\r\n", "\n" }, StringSplitOptions.None);
+                            for (int i = 0; i < result.Length; i++)
+                            {
+                                Hedingben.ToastText(result[i], $"mcuLog" + i);
+                            }
+                        }
+
+                        Hedingben.ToastText(
+                            $"find head time:{time1.TotalMilliseconds},find total time:{time2.TotalMilliseconds},copy time:{time3.TotalMilliseconds},notify time:{time4.TotalMilliseconds}","timeDebug2");
+                    }
+
+                    Hedingben.ToastText(
+                        $"receive from mcu:{BitConverter.ToString(_receiveBytes)},time:{(DateTime.Now - _lastTime).TotalMilliseconds}",
+                        "DiverReceive");
+                    if ((DateTime.Now - _lastTime).TotalMilliseconds > 200)
+                    {
+                        Hedingben.ToastText($"{(DateTime.Now - _lastTime).TotalMilliseconds}ms no message from diver","DiverTimeout");
+                    }
+                    _lastTime = DateTime.Now;
+                    // 从缓冲区中移除已解析的报文
+                    _buffer.RemoveRange(0, startIndex + totalLength);
+                    return; // 成功解析一条报文后退出本轮解析
+
+                }
+                else
+                {
+                    // 如果尾部无效，丢弃头部并继续解析
+                    DLog.Log("DIVER报文无效");
+                    Console.WriteLine("DIVER报文无效");
+                    var errorBytes = _buffer.Skip(startIndex).Take(totalLength).ToArray();
+                    DLog.Log("错误报文"+string.Join(" ",errorBytes.Select(p=>$"{p:X2}")));
+                    _buffer.RemoveAt(startIndex);
+                }
+                
+            }
+        }
+
+    }
+}

MedullaAdapter/FleetRemote.cs

@@ -0,0 +1,246 @@
+using System;
+using CartActivator;
+using CycleGUI;
+using CycleGUI.API;
+using FundamentalLib;
+using Medulla.Types;
+
+namespace MedullaAdapter
+{
+    public partial class DiverCartDefinition
+    {
+        private bool _fleetRemoteActive;
+        private DateTime _fleetDiagLastStick = DateTime.MinValue;
+
+        private void FleetDiag(string msg)
+        {
+            DLog.Log($"car{CarNum} {msg}", "FleetDiagMedulla");
+        }
+
+        private void ZeroFleetManualFields()
+        {
+            MultiVehicleManualEnabled = false;
+            MultiVehicleManualMode = 0;
+            MultiVehicleManualVx = 0;
+            MultiVehicleManualVy = 0;
+            MultiVehicleManualVth = 0;
+        }
+
+        public float ApplyFleetManualCommand(int mode, float px, float py, float speedRatio)
+        {
+            var ratio = Clamp(speedRatio, 0, 1);
+            px = Clamp(px, -1, 1);
+            py = Clamp(py, -1, 1);
+
+            if (mode < 0 || mode > 2) mode = 0;
+            MultiVehicleManualMode = mode;
+
+            if (mode == 2)
+            {
+                MultiVehicleManualVx = 0;
+                MultiVehicleManualVy = 0;
+                MultiVehicleManualVth = px * MaxManualAngularSpeed * ratio;
+            }
+            else if (mode == 1)
+            {
+                MultiVehicleManualVx = py * MaxManualSpeed * ratio;
+                MultiVehicleManualVy = px * MaxManualSpeed * ratio;
+                MultiVehicleManualVth = 0;
+            }
+            else
+            {
+                MultiVehicleManualVx = py * MaxManualSpeed * ratio;
+                MultiVehicleManualVy = 0;
+                MultiVehicleManualVth = px * MaxManualAngularSpeed * ratio;
+            }
+
+            return ratio;
+        }
+
+        [IOObjectUtility]
+        public void FleetRemote()
+        {
+            if (_fleetRemoteActive)
+            {
+                return;
+            }
+
+            _fleetRemoteActive = true;
+            var fleetOn = false;
+            var crabOn = false;
+            var rotateOn = false;
+            var speedRatio = 1f;
+            UseGesture manip = null;
+
+            int CurrentMode()
+            {
+                if (crabOn) return 1;
+                if (rotateOn) return 2;
+                return 0;
+            }
+
+            void ApplyMode()
+            {
+                MultiVehicleManualMode = fleetOn ? CurrentMode() : 0;
+            }
+
+            void CleanupFleetRemote()
+            {
+                if (manip != null)
+                {
+                    manip.End();
+                    manip = null;
+                }
+
+                fleetOn = false;
+                crabOn = false;
+                rotateOn = false;
+                ZeroFleetManualFields();
+                _fleetRemoteActive = false;
+            }
+
+            manip = new UseGesture();
+
+            manip.AddWidget(new UseGesture.StickWidget
+            {
+                name = "fleet_stick",
+                text = "速度摇杆",
+                position = "37.5%+10px, 18%+10px",
+                size = "37.5%-10px, 32%-10px",
+                bounceBack = true,
+                keyboard = "Up,Down,Left,Right",
+                joystick = "Axis0,Axis1",
+                OnValue = (pos, manipulating) =>
+                {
+                    if (!fleetOn)
+                    {
+                        MultiVehicleManualVx = 0;
+                        MultiVehicleManualVy = 0;
+                        MultiVehicleManualVth = 0;
+                        if ((DateTime.Now - _fleetDiagLastStick).TotalMilliseconds >= 200)
+                        {
+                            _fleetDiagLastStick = DateTime.Now;
+                            FleetDiag($"STICK(ignored,fleetOff) pos=({pos.X:0.00},{pos.Y:0.00}) manip={manipulating}");
+                        }
+                        return;
+                    }
+
+                    var mode = CurrentMode();
+                    var ratio = ApplyFleetManualCommand(mode, pos.X, pos.Y, speedRatio);
+
+                    if ((DateTime.Now - _fleetDiagLastStick).TotalMilliseconds >= 200)
+                    {
+                        _fleetDiagLastStick = DateTime.Now;
+                        FleetDiag($"STICK mode={mode} pos=({pos.X:0.00},{pos.Y:0.00}) manip={manipulating} ratio={ratio:0.00} " +
+                                  $"-> Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.0} en={MultiVehicleManualEnabled}");
+                    }
+                }
+            });
+
+            manip.AddWidget(new UseGesture.ToggleWidget
+            {
+                name = "fleet_crab",
+                text = "横移模式",
+                position = "12.5%+10px, 52%+10px",
+                size = "37.5%-10px, 10%-10px",
+                OnValue = b =>
+                {
+                    crabOn = b;
+                    ApplyMode();
+                    FleetDiag($"TOGGLE crab={b} -> mode={MultiVehicleManualMode}");
+                }
+            });
+
+            manip.AddWidget(new UseGesture.ToggleWidget
+            {
+                name = "fleet_rotate",
+                text = "原地旋转",
+                position = "50%+10px, 52%+10px",
+                size = "37.5%-10px, 10%-10px",
+                OnValue = b =>
+                {
+                    rotateOn = b;
+                    ApplyMode();
+                    FleetDiag($"TOGGLE rotate={b} -> mode={MultiVehicleManualMode}");
+                }
+            });
+
+            manip.AddWidget(new UseGesture.ToggleWidget
+            {
+                name = "fleet_enable",
+                text = "车队联动",
+                position = "12.5%+10px, 64%+10px",
+                size = "37.5%-10px, 10%-10px",
+                OnValue = b =>
+                {
+                    fleetOn = b;
+                    MultiVehicleManualEnabled = b;
+                    ApplyMode();
+                    if (!b) ZeroFleetManualFields();
+                    FleetDiag($"TOGGLE fleetOn={b} -> ManualEnabled={MultiVehicleManualEnabled} mode={MultiVehicleManualMode}");
+                }
+            });
+
+            manip.AddWidget(new UseGesture.ThrottleWidget
+            {
+                name = "fleet_speed_ratio",
+                text = "速度比例",
+                position = "50%+10px, 64%+10px",
+                size = "37.5%-10px, 10%-10px",
+                bounceBack = false,
+                OnValue = (val, _) => speedRatio = Clamp(val, 0, 1)
+            });
+
+            manip.AddWidget(new UseGesture.ButtonWidget
+            {
+                name = "fleet_stop",
+                text = "急停/归零",
+                position = "31.25%+10px, 76%+10px",
+                size = "37.5%-10px, 10%-10px",
+                OnPressed = pressed =>
+                {
+                    if (!pressed) return;
+                    MultiVehicleManualVx = 0;
+                    MultiVehicleManualVy = 0;
+                    MultiVehicleManualVth = 0;
+                    FleetDiag("BUTTON stop/zero");
+                }
+            });
+
+            manip.ChangeState(new SetAppearance { drawGuizmo = false });
+            manip.Start();
+
+            GUI.PromptOrBringToFront(pb =>
+            {
+                if (pb.Closing())
+                {
+                    CleanupFleetRemote();
+                    pb.Panel.Exit();
+                    return;
+                }
+
+                pb.Panel.TopMost(true)
+                    .SetDefaultDocking(Panel.Docking.None)
+                    .ShowTitle("车队联动遥控")
+                    .InitSize(340, 190)
+                    .InitPos(false, -32, 32, 1, 0, 1, 0);
+
+                pb.SeparatorText("状态");
+                var modeName = MultiVehicleManualMode == 2 ? "原地旋转" : MultiVehicleManualMode == 1 ? "横移" : "常规";
+                pb.Label($"车队联动: {MultiVehicleManualEnabled}  模式: {modeName}");
+                pb.Label($"速度比例: {speedRatio:0.00}");
+                pb.Label($"Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} m/s, Vth={MultiVehicleManualVth:0.0}");
+                pb.Label($"优先级: {CartActivator.CartDefinition.currentPriority} ({CartActivator.CartDefinition.currentPriorityDesc})");
+                pb.Label("请勿同时打开手动控制面板");
+                pb.Panel.Repaint();
+            }, instancingObject: this);
+        }
+
+        private static float Clamp(float value, float min, float max)
+        {
+            if (value < min) return min;
+            if (value > max) return max;
+            return value;
+        }
+    }
+}

MedullaAdapter/FodyWeavers.xml

@@ -0,0 +1,3 @@
+<Weavers xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="FodyWeavers.xsd">
+  <DiverCompiler />
+</Weavers>

MedullaAdapter/FodyWeavers.xsd

@@ -0,0 +1,26 @@
+<?xml version="1.0" encoding="utf-8"?>
+<xs:schema xmlns:xs="http://www.w3.org/2001/XMLSchema">
+  <!-- This file was generated by Fody. Manual changes to this file will be lost when your project is rebuilt. -->
+  <xs:element name="Weavers">
+    <xs:complexType>
+      <xs:all>
+        <xs:element name="DiverCompiler" minOccurs="0" maxOccurs="1" type="xs:anyType" />
+      </xs:all>
+      <xs:attribute name="VerifyAssembly" type="xs:boolean">
+        <xs:annotation>
+          <xs:documentation>'true' to run assembly verification (PEVerify) on the target assembly after all weavers have been executed.</xs:documentation>
+        </xs:annotation>
+      </xs:attribute>
+      <xs:attribute name="VerifyIgnoreCodes" type="xs:string">
+        <xs:annotation>
+          <xs:documentation>A comma-separated list of error codes that can be safely ignored in assembly verification.</xs:documentation>
+        </xs:annotation>
+      </xs:attribute>
+      <xs:attribute name="GenerateXsd" type="xs:boolean">
+        <xs:annotation>
+          <xs:documentation>'false' to turn off automatic generation of the XML Schema file.</xs:documentation>
+        </xs:annotation>
+      </xs:attribute>
+    </xs:complexType>
+  </xs:element>
+</xs:schema>

MedullaAdapter/MCUInterface.cs

@@ -0,0 +1,291 @@
+using CartActivator;
+using System;
+using System.Collections.Generic;
+using System.IO;
+using System.Linq;
+using System.Reflection;
+using System.Text;
+using System.Threading;
+using Newtonsoft.Json;
+using FundamentalLib;
+
+
+namespace MedullaAdapter
+{
+    // todo: currently use this ladderlogic to act interaction with Medulla.
+    // todo: directly integrate into CartActivator.
+
+    // MCU->Medulla is intervally exchanged information.
+    public class MCUInterface<T> : LadderLogic<T> where T : DiverCartDefinition
+    {
+        
+        /// ////////////////////////////// IMPLEMENT THESE ////////////////////////////////////////////
+
+        public static void SetMCUProgram(string mcu_device_url, byte[] program)
+        {
+            List<byte[]> SplitArrayIntoChunks(byte[] array, int chunkSize)
+            {
+                List<byte[]> chunks = new List<byte[]>();
+
+                for (int i = 0; i < array.Length; i += chunkSize)
+                {
+                    int currentChunkSize = Math.Min(chunkSize, array.Length - i);
+                    byte[] chunk = new byte[currentChunkSize];
+                    Array.Copy(array, i, chunk, 0, currentChunkSize);
+                    chunks.Add(chunk);
+                }
+                return chunks;
+            }
+            //todo: modify this.
+            var codeList = SplitArrayIntoChunks(program, 1024);
+            int i = 0;
+            foreach (var codePack in codeList)
+            {
+                var downloadCode =
+                    new DownloadCodePack(program.Length, 1024 * i, codePack.Length, codePack).GetPack();
+                Console.WriteLine(string.Join(" ", downloadCode.Select(p => $"{p:X2}")));
+                cart.Embedded.SendMessage(downloadCode);
+                Console.WriteLine($"send bytes length:{downloadCode.Length}");
+                i++;
+                Thread.Sleep(50);
+                //while (true)
+                //{
+                //    var receive = cart.Embedded.GetMessage();
+                //    if (receive == null) continue;
+                //    if (receive[5] == 0x90 && (receive[6] == 0x05 || receive[6]==0x06)) break;
+                //}
+            }
+            var controlPack2 = new ControlPack(0x01).GetPack();
+            cart.Embedded.SendMessage(controlPack2);
+            Console.WriteLine("下发代码");
+            //MCUTestRunner.DebugSetMCUProgram(program, (bs) => NotifyLowerData("default", bs));
+        }
+
+        public static void SendUpperData(string mcu_device_url, byte[] data)
+        {
+            //todo: this is for VM data exchange, contains upperIO/lowerIO modifications.
+            var upperPack = new UpperIOPack(data, data.Length).GetPack();
+            cart.Embedded.SendMessage(upperPack);
+            //MCUTestRunner.DebugSendUpper(data);
+        }
+
+
+        /// ////////////////////////////// INTERFACES ////////////////////////////////////////////
+
+        public static void NotifyPrint(string mcu_device_url, string message)
+        {
+            Console.WriteLine($"{mcu_device_url}:{message}");
+        }
+
+        // whenever a lower io data is uploaded, call this.
+        public static void NotifyLowerData(string mcu_device_url, byte[] lowerIOData)
+        {
+            using var ms = new MemoryStream(lowerIOData);
+            using var br = new BinaryReader(ms);
+            if (mcu_logics.TryGetValue(mcu_device_url, out var tup))
+            {
+                Hedingben.ToastText($"recv iter {br.ReadInt32()} lowerIO data from {mcu_device_url}, operation {tup.name}", $"DIVER-{tup.name}");
+                while (ms.Position < lowerIOData.Length)
+                {
+                    var cid = br.ReadInt16();
+                    if (cid < 0 || cid > tup.fields.Length) throw new Exception("invalid Cartfield id!");
+                    // if it's upperio skip, otherwise write data.
+                    var typeid = br.ReadByte();
+                    if (tup.fields[cid].typeid != typeid)
+                        throw new Exception($"??? typeid not match for {tup.fields[cid].field}({cid}), expected {tup.fields[cid].typeid} got {typeid}");
+                    
+                    object value;
+                    switch (tup.fields[cid].typeid)
+                    {
+                        case 0:
+                            value = br.ReadBoolean();
+                            break;
+                        case 1:
+                            value = br.ReadByte();
+                            break;
+                        case 2:
+                            value = br.ReadSByte();
+                            break;
+                        case 3:
+                            value = br.ReadChar();
+                            break;
+                        case 4:
+                            value = br.ReadInt16();
+                            break;
+                        case 5:
+                            value = br.ReadUInt16();
+                            break;
+                        case 6:
+                            value = br.ReadInt32();
+                            break;
+                        case 7:
+                            value = br.ReadUInt32();
+                            break;
+                        case 8:
+                            value = br.ReadSingle();
+                            break;
+                        default:
+                            throw new Exception($"Unsupported type ID: {tup.fields[cid].typeid}");
+                    }
+                    
+                    if (tup.fields[cid].isUpper) continue;
+                    tup.fields[cid].fi.SetValue(cart, value);
+                }
+
+                // ok to send current data.
+                using var sends = new MemoryStream();
+                using var bw = new BinaryWriter(sends);
+                bw.Write(tup.iterations++);
+                for (var cid = 0; cid < tup.fields.Length; cid++)
+                {
+                    bw.Write((short)cid);
+                    bw.Write((byte)tup.fields[cid].typeid);
+                    var val = tup.fields[cid].fi.GetValue(cart);
+                    switch (tup.fields[cid].typeid)
+                    {
+                        case 0:
+                            bw.Write((bool)val);
+                            break;
+                        case 1:
+                            bw.Write((byte)val);
+                            break;
+                        case 2:
+                            bw.Write((sbyte)val);
+                            break;
+                        case 3:
+                            bw.Write((char)val);
+                            break;
+                        case 4:
+                            bw.Write((short)val);
+                            break;
+                        case 5:
+                            bw.Write((ushort)val);
+                            break;
+                        case 6:
+                            bw.Write((int)val);
+                            break;
+                        case 7:
+                            bw.Write((uint)val);
+                            break;
+                        case 8:
+                            bw.Write((float)val);
+                            break;
+                    }
+                }
+
+                SendUpperData(mcu_device_url, sends.ToArray());
+            }
+            else
+            {
+                Hedingben.ToastText($"warning: {mcu_device_url} received lowerIOData but not registered","DIVERWarning");
+            }
+        }
+       
+        public static void Start(T c)
+        {
+            cart = c;
+
+            var logics = typeof(T).Assembly.GetTypes()
+                .Where(p => p.GetCustomAttribute<LogicRunOnMCUAttribute>() != null).ToArray();
+            foreach (var logic in logics)
+            {
+                var attr = logic.GetCustomAttribute<LogicRunOnMCUAttribute>();
+                Console.WriteLine($"Set logic {logic.Name} to run on MCU-VM @ device {attr.mcu_url}");
+
+                byte[] ReadAllBytes(Stream stream)
+                {
+                    using (var ms = new MemoryStream())
+                    {
+                        stream.CopyTo(ms);
+                        return ms.ToArray();
+                    }
+                }
+                var bytes = ReadAllBytes(Assembly.GetExecutingAssembly().GetManifestResourceStream($"{logic.Name}.bin"));
+                SetMCUProgram(attr.mcu_url, bytes);
+                var json = UTF8Encoding.UTF8.GetString(ReadAllBytes(Assembly.GetExecutingAssembly()
+                    .GetManifestResourceStream($"{logic.Name}.bin.json")));
+                Console.WriteLine(json);
+                var fields = JsonConvert.DeserializeObject<PField[]>(json);
+                if (mcu_logics.ContainsKey(attr.mcu_url))
+                    throw new Exception($"Already have logic for {attr.mcu_url}: LadderLogic {logic.Name}");
+                mcu_logics[attr.mcu_url] = new LogicInfo() { fields = fields, name = logic.Name };
+                foreach (var pField in fields)
+                {
+                    pField.fi = typeof(T).GetField(pField.field);
+                    if (pField.fi == null)
+                        throw new Exception($"field {pField.field} doesn't exist in cart object?");
+                    pField.isUpper = pField.fi.IsDefined(typeof(AsUpperIO));
+                    // todo: check type.
+                }
+            }
+        }
+
+        /// ///////////////////////////////// DONT CARE /////////////////////////////////////
+        private static T cart;
+
+        class PField
+        {
+            public string field;
+            public FieldInfo fi;
+            public bool isUpper;
+            public int typeid, offset;
+        }
+
+        class LogicInfo
+        {
+            public string name;
+            public PField[] fields;
+            public int iterations;
+        }
+        private static Dictionary<string, LogicInfo> mcu_logics = new();
+        public override void Operation(int iteration)
+        {
+            // just send LIO.
+        }
+    }
+
+    //unsafe class MCUTestRunner
+    //{
+
+    //    public delegate void NotifyLowerDelegate(byte* changedStates, int length);
+    //    [DllImport("MCURuntime.dll", CallingConvention = CallingConvention.Cdecl)]
+    //    public static extern void set_lowerio_cb(NotifyLowerDelegate callback);
+
+    //    private static NotifyLowerDelegate DNotifyStateChanged = StateChanged;
+
+    //    [DllImport("MCURuntime.dll")]
+    //    static extern void test(byte* bin, int len);
+    //    [DllImport("MCURuntime.dll")]
+    //    static extern void put_upper(byte* bin, int len);
+
+    //    private static Action<byte[]> lo_notifier;
+    //    private static void StateChanged(byte* changedstates, int length)
+    //    {
+    //        byte[] byteArray = new byte[length];
+    //        Marshal.Copy((IntPtr)changedstates, byteArray, 0, length);
+    //        lo_notifier(byteArray); 
+    //    }
+
+    //    public static void DebugSendUpper(byte[] data)
+    //    { 
+    //        fixed (byte* ptr = data)
+    //        {
+    //            put_upper(ptr, data.Length);
+    //        }
+    //    }
+    //    public static void DebugSetMCUProgram(byte[] program, Action<byte[]> notifier)
+    //    {
+    //        lo_notifier = notifier;
+    //        set_lowerio_cb(DNotifyStateChanged);
+    //        new Thread(() =>
+    //        {
+    //            var allb = new byte[10240]; //10K runtime
+    //            Array.Copy(program, allb, program.Length);
+    //            fixed (byte* ptr = allb)
+    //            {
+    //                test(ptr, 10240);
+    //            }
+    //        }).Start();
+    //    }
+    //}
+}

MedullaAdapter/MCURoutine.cs

@@ -0,0 +1,879 @@
+using CartActivator;
+using FundamentalLib;
+using MCUSerialBridgeCLR;
+using System;
+using System.Collections.Generic;
+
+namespace MedullaAdapter
+{
+    //[LogicRunOnMCU(scanInterval = 20)]
+    public class MCURoutine:LadderLogic<DiverCartDefinition>
+    {
+        private int _lastIteration = 0;
+        private int _count = 0;
+        private int _operationTime = 0;
+        private bool _driversDisabled;
+        private bool _canCallbackRegistered;
+        private bool _serialCallbackRegistered;
+        private byte _resetCode = 0x86;
+        private byte _enableCode1 = 0x06;
+        private byte _enableCode2 = 0x07;
+        private byte _enableCode3 = 0x0F;
+        private bool io_bit0 = false;//继电器
+        private bool io_bit1 = false;//抱闸
+        private bool io_bit2 = false;//红灯
+        private bool io_bit3 = false;//绿灯
+        private bool io_bit4 = false;//黄灯
+        private const byte BatteryPortIndex = 3;
+        private static readonly byte[] BatteryRequest = BuildBatteryRequest();
+
+        private static float ConvertMps2Rpm(float mps)
+        {
+            return (float)(mps / (Math.PI * 85f) * 10.5f * 60f * 1000f);
+        }
+        private static float ConvertRpm2Mps(float rpm)
+        {
+            return (float)(rpm / 10.5f / 60f * Math.PI * 85f / 1000f);
+        }
+        private float ConvertR2MM(float r)
+        {
+            return (float)(r / 10.5f * Math.PI * 85);
+        }
+        private static float DecodeRpmFromPayload(byte[] payload, int offset = 4)
+        {
+            return BitConverter.ToInt32(payload, offset) * 1875f / 512f / 10000f;
+        }
+        public override void Operation(int iteration)
+        {
+            if (_lastIteration != iteration)
+            {
+                _lastIteration = iteration;
+                _count = 0;
+            }
+            else
+            {
+                _count++;
+            }
+            if (_count >= 15)
+            {
+                cart.AlarmLevel = 2;
+                Console.WriteLine("mcu lost connection \n");
+            }
+
+            if (_count >= 30)
+            {
+                return;
+            }
+            if (cart?.Bridge == null)
+            {
+                return;
+            }
+            //注册CAN回调
+            EnsureCanCallbacksRegistered();
+            //注册串口回调
+            EnsureSerialCallbacksRegistered();
+            PollBatterySerial(iteration);
+
+            #region io部分
+            var ioReadBuffer = new byte[4];
+            var readInputErr = cart.Bridge.ReadInput(out ioReadBuffer, 20);
+            if (readInputErr == MCUSerialBridgeError.OK)
+            {
+                cart.test = ioReadBuffer[0];
+                cart.Start = (ioReadBuffer[0] & (1 << 0)) != 0;
+                cart.ResetPressed = (ioReadBuffer[0] & (1 << 1)) != 0;
+                cart.ChassisMode = ((ioReadBuffer[0] & (1 << 2)) == 0) ? 0 : 1;
+                cart.BrakeEnable = (ioReadBuffer[0] & (1 << 3)) != 0;
+                if ((ioReadBuffer[0] & (1 << 4)) == 0)
+                {
+                    cart.EmergencyPressed = 1;
+                }
+            }
+            else
+            {
+                Console.WriteLine("IO Read FAILED");
+            }
+
+            var ioWriteBuffer = new byte[4];
+            io_bit0 = cart.ChargePort ? true : false;
+            io_bit1 = cart.EmergencyPressed == 1 ? false : true;
+            if (cart.MultiVehicleLightSync == 0)
+            {
+                io_bit2 = false;
+                io_bit3 = false;
+                io_bit4 = false;
+            }
+            else if (cart.MultiVehicleLightSync == 1)
+            {
+                io_bit2 = false;
+                io_bit3 = true;
+                io_bit4 = false;
+            }
+            else
+            {
+                if (cart.LightMode == 2)
+                {
+                    io_bit2 = true;//红
+                    io_bit3 = false;//绿
+                    io_bit4 = false;//黄
+                }
+                else if (cart.LightMode == 3)
+                {
+                    io_bit2 = false;
+                    io_bit3 = false;
+                    io_bit4 = true;
+                }
+                else if (cart.LightMode == 1)
+                {
+                    io_bit2 = false;
+                    io_bit3 = true;
+                    io_bit4 = false;
+                }
+                else
+                {
+                    io_bit2 = false;
+                    io_bit3 = false;
+                    io_bit4 = false;
+                }
+            }
+
+            ioWriteBuffer[0] = (byte)((io_bit0 ? 1 << 0 : 0) | //继电器
+                                    (io_bit1 ? 1 << 1 : 0) | //抱闸
+                                    (io_bit2 ? 1 << 2 : 0) | //红灯
+                                    (io_bit3 ? 1 << 3 : 0) | //绿灯
+                                    (io_bit4 ? 1 << 4 : 0)); //黄灯
+            if (cart.Bridge.WriteOutput(ioWriteBuffer, 20) != MCUSerialBridgeError.OK)
+            {
+                Console.WriteLine("IO Write FAILED");
+            }
+            #endregion
+
+            #region 驱动部分
+            cart.ActualSpeedLeftFront = (cart.ActualSpeedLeftFrontLeft + cart.ActualSpeedLeftFrontRight) / 2;
+            cart.ActualSpeedLeftRear = (cart.ActualSpeedLeftRearLeft + cart.ActualSpeedLeftRearRight) / 2;
+            cart.ActualSpeedRightFront = (cart.ActualSpeedRightFrontLeft + cart.ActualSpeedRightFrontRight) / 2;
+            cart.ActualSpeedRightRear = (cart.ActualSpeedRightRearLeft + cart.ActualSpeedRightRearRight) / 2;
+            MCUSerialBridgeError SendCan(byte port, ushort standardId, byte[] payload, bool RTR = false, uint timeout = 2)
+            {
+                var canSend = new CANMessage
+                {
+                    ID = standardId,
+                    RTR = RTR,
+                    DLC = !RTR ? (byte)8 : (byte)0,
+                    Payload = payload ?? new byte[] { }
+                };
+                return cart.Bridge.WriteCAN(port, canSend, timeout);
+            }
+
+            var resetPayload = new byte[8] { 0xFD, _resetCode, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+            var enable1Payload = new byte[8] { 0xFD, _enableCode1, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+            var enable2Payload = new byte[8] { 0xFD, _enableCode2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+            var enable3Payload = new byte[8] { 0xFD, _enableCode3, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+
+            Hedingben.ToastText($"_operationTime:{_operationTime}", "OperationTime");
+            if (_operationTime == 0)
+            {
+                SendCan(0, 0x00, new byte[] { 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 });
+                _operationTime++;
+                return;
+            }
+            else if (_operationTime == 1)
+            {
+                if (cart.LeftFrontLeftErrorCode != 0)
+                {
+                    SendCan(0, 0x201, enable1Payload);
+                    SendCan(0, 0x201, resetPayload);
+                }
+                if (cart.LeftFrontRightErrorCode != 0)
+                {
+                    SendCan(0, 0x202, enable1Payload);
+                    SendCan(0, 0x202, resetPayload);
+                }
+                if (cart.RightFrontLeftErrorCode != 0)
+                {
+                    SendCan(0, 0x203, enable1Payload);
+                    SendCan(0, 0x203, resetPayload);
+                }
+                if (cart.RightFrontRightErrorCode != 0)
+                {
+                    SendCan(0, 0x204, enable1Payload);
+                    SendCan(0, 0x204, resetPayload);
+                }
+                if (cart.LeftRearLeftErrorCode != 0)
+                {
+                    SendCan(0, 0x205, enable1Payload);
+                    SendCan(0, 0x205, resetPayload);
+                }
+                if (cart.LeftRearRightErrorCode != 0)
+                {
+                    SendCan(0, 0x206, enable1Payload);
+                    SendCan(0, 0x206, resetPayload);
+                }
+                if (cart.RightRearLeftErrorCode != 0)
+                {
+                    SendCan(0, 0x207, enable1Payload);
+                    SendCan(0, 0x207, resetPayload);
+                }
+                if (cart.RightRearRightErrorCode != 0)
+                {
+                    SendCan(0, 0x208, enable1Payload);
+                    SendCan(0, 0x208, resetPayload);
+                }
+                if (cart.LeftArmErrorCode != 0)
+                {
+                    SendCan(0, 0x209, enable1Payload);
+                    SendCan(0, 0x209, resetPayload);
+                }
+                if (cart.RightArmErrorCode != 0)
+                {
+                    SendCan(0, 0x20A, enable1Payload);
+                    SendCan(0, 0x20A, resetPayload);
+                }
+                _operationTime++;
+                return;
+            }
+            else if (_operationTime == 2)
+            {
+                //SendNodeGuardRequests(SendCan);
+                //if (AreAllNodesOperational())
+                //{
+                //    _operationTime++;
+                //}
+                _operationTime++;
+                return;
+            }
+            //检查错误是否被消除 若没被消除就得重新返回上一步发复位 若消除了就继续往下
+            else if (_operationTime == 3)
+            {
+                if (cart.LeftFrontLeftErrorCode != 0 || cart.LeftFrontRightErrorCode != 0 ||
+                cart.LeftRearLeftErrorCode != 0 || cart.LeftRearRightErrorCode != 0
+                || cart.RightFrontLeftErrorCode != 0 || cart.RightFrontRightErrorCode != 0 ||
+                cart.RightRearLeftErrorCode != 0 || cart.RightRearRightErrorCode != 0 ||
+                cart.LeftArmErrorCode != 0 || cart.RightArmErrorCode != 0)
+                {
+                    _operationTime = 1;
+                    return;
+                }
+                else
+                {
+                    _operationTime++;
+                    return;
+                }
+            }
+            // 没有错误 没有节点保护 就发06 07 0F使能
+            else if (_operationTime == 4)
+            {
+                SendCan(0, 0x201, enable1Payload);
+                SendCan(0, 0x202, enable1Payload);
+                SendCan(0, 0x203, enable1Payload);
+                SendCan(0, 0x204, enable1Payload);
+                SendCan(0, 0x205, enable1Payload);
+                SendCan(0, 0x206, enable1Payload);
+                SendCan(0, 0x207, enable1Payload);
+                SendCan(0, 0x208, enable1Payload);
+                SendCan(0, 0x209, enable1Payload);
+                SendCan(0, 0x20A, enable1Payload);
+                _operationTime++;
+                return;
+            }
+            else if (_operationTime == 5)
+            {
+                SendCan(0, 0x201, enable2Payload);
+                SendCan(0, 0x202, enable2Payload);
+                SendCan(0, 0x203, enable2Payload);
+                SendCan(0, 0x204, enable2Payload);
+                SendCan(0, 0x205, enable2Payload);
+                SendCan(0, 0x206, enable2Payload);
+                SendCan(0, 0x207, enable2Payload);
+                SendCan(0, 0x208, enable2Payload);
+                SendCan(0, 0x209, enable2Payload);
+                SendCan(0, 0x20A, enable2Payload);
+                _operationTime++;
+                return;
+            }
+            else if (_operationTime == 6)
+            {
+                SendCan(0, 0x201, enable3Payload);
+                SendCan(0, 0x202, enable3Payload);
+                SendCan(0, 0x203, enable3Payload);
+                SendCan(0, 0x204, enable3Payload);
+                SendCan(0, 0x205, enable3Payload);
+                SendCan(0, 0x206, enable3Payload);
+                SendCan(0, 0x207, enable3Payload);
+                SendCan(0, 0x208, enable3Payload);
+                SendCan(0, 0x209, enable3Payload);
+                SendCan(0, 0x20A, enable3Payload);
+                if (cart.LeftFrontLeftErrorCode != 0 || cart.LeftFrontRightErrorCode != 0 ||
+                cart.LeftRearLeftErrorCode != 0 || cart.LeftRearRightErrorCode != 0
+                || cart.RightFrontLeftErrorCode != 0 || cart.RightFrontRightErrorCode != 0 ||
+                cart.RightRearLeftErrorCode != 0 || cart.RightRearRightErrorCode != 0 ||
+                cart.LeftArmErrorCode != 0 || cart.RightArmErrorCode != 0)
+                {
+                    _operationTime = 1;
+                    return;
+                }
+                else
+                {
+                    _operationTime++;
+                    return;
+                }
+            }
+
+            if (cart.ResetPressed || cart.ResetFromM || cart.ResetFromC)
+            {
+                cart.AlarmLevel = -1;
+                cart.EmergencyPressed = 0;
+                _count = 0;
+                _operationTime = 0;
+                _driversDisabled = false;
+                cart.WheelAbleState = true;
+                cart.ResetFromM = false;
+            }
+
+            if (cart.DisableFromM || cart.DisableFromC)
+            {
+                _driversDisabled = true;
+                cart.WheelAbleState = false;
+                cart.DisableFromM = false;
+            }
+
+            var lfl = Math.Sign(cart.SpeedLFL) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedLFL));
+            var lfr = Math.Sign(cart.SpeedLFR) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedLFR));
+            var rfl = Math.Sign(cart.SpeedRFL) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedRFL));
+            var rfr = Math.Sign(cart.SpeedRFR) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedRFR));
+            var lrl = Math.Sign(cart.SpeedLRL) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedLRL));
+            var lrr = Math.Sign(cart.SpeedLRR) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedLRR));
+            var rrl = Math.Sign(cart.SpeedRRL) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedRRL));
+            var rrr = Math.Sign(cart.SpeedRRR) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedRRR));
+
+            var v1 = ConvertMps2Rpm(lfl);
+            var v2 = ConvertMps2Rpm(-lfr);
+            var v3 = ConvertMps2Rpm(rfl);
+            var v4 = ConvertMps2Rpm(-rfr);
+            var v5 = ConvertMps2Rpm(lrl);
+            var v6 = ConvertMps2Rpm(-lrr);
+            var v7 = ConvertMps2Rpm(rrl);
+            var v8 = ConvertMps2Rpm(-rrr);
+            var v9 = ConvertMps2Rpm(cart.SpeedLeftArm);
+            var v10 = ConvertMps2Rpm(cart.SpeedRightArm);
+            if (cart.AlarmLevel == 2 || cart.WaitStart || cart.PreparaStart)
+            {
+                v1 = v2 = v3 = v4 = v5 = v6 = v7 = v8 = v9 = v10 = 0;
+            }
+            if ((cart.ActualPosLeftArm <= cart.LeftArmLowerPos && cart.SpeedLeftArm < 0) || (cart.ActualPosLeftArm >= cart.LeftArmUpperPos && cart.SpeedLeftArm > 0))
+            {
+                v9 = 0;
+            }
+            if ((cart.ActualPosRightArm <= cart.RightArmLowerPos && cart.SpeedRightArm < 0) || (cart.ActualPosRightArm >= cart.RightArmUpperPos && cart.SpeedRightArm > 0))
+            {
+                v10 = 0;
+            }
+
+            var sendLFL = BitConverter.GetBytes((int)Math.Round(v1 * 512f * 10000f / 1875f));
+            var sendLFR = BitConverter.GetBytes((int)Math.Round(v2 * 512f * 10000f / 1875f));
+            var sendRFL = BitConverter.GetBytes((int)Math.Round(v3 * 512f * 10000f / 1875f));
+            var sendRFR = BitConverter.GetBytes((int)Math.Round(v4 * 512f * 10000f / 1875f));
+            var sendLRL = BitConverter.GetBytes((int)Math.Round(v5 * 512f * 10000f / 1875f));
+            var sendLRR = BitConverter.GetBytes((int)Math.Round(v6 * 512f * 10000f / 1875f));
+            var sendRRL = BitConverter.GetBytes((int)Math.Round(v7 * 512f * 10000f / 1875f));
+            var sendRRR = BitConverter.GetBytes((int)Math.Round(v8 * 512f * 10000f / 1875f));
+            var sendLArm = BitConverter.GetBytes((int)Math.Round(v9 * 512f * 10000f / 1875f));
+            var sendRArm = BitConverter.GetBytes((int)Math.Round(v10 * 512f * 10000f / 1875f));
+
+            if (iteration % 2 == 0)
+            {
+                //SendNodeGuardRequests(SendCan);
+            }
+
+            if (_driversDisabled)
+            {
+                SendCan(0, 0x201, enable1Payload);
+                SendCan(0, 0x202, enable1Payload);
+                SendCan(0, 0x203, enable1Payload);
+                SendCan(0, 0x204, enable1Payload);
+                SendCan(0, 0x205, enable1Payload);
+                SendCan(0, 0x206, enable1Payload);
+                SendCan(0, 0x207, enable1Payload);
+                SendCan(0, 0x208, enable1Payload);
+            }
+            else
+            {
+                SendCan(0, 0x201, new byte[] { 0xFD, _enableCode3, 0x00, sendLFL[0], sendLFL[1], sendLFL[2], sendLFL[3], 0x00 });
+                SendCan(0, 0x202, new byte[] { 0xFD, _enableCode3, 0x00, sendLFR[0], sendLFR[1], sendLFR[2], sendLFR[3], 0x00 });
+                SendCan(0, 0x203, new byte[] { 0xFD, _enableCode3, 0x00, sendRFL[0], sendRFL[1], sendRFL[2], sendRFL[3], 0x00 });
+                SendCan(0, 0x204, new byte[] { 0xFD, _enableCode3, 0x00, sendRFR[0], sendRFR[1], sendRFR[2], sendRFR[3], 0x00 });
+                SendCan(0, 0x205, new byte[] { 0xFD, _enableCode3, 0x00, sendLRL[0], sendLRL[1], sendLRL[2], sendLRL[3], 0x00 });
+                SendCan(0, 0x206, new byte[] { 0xFD, _enableCode3, 0x00, sendLRR[0], sendLRR[1], sendLRR[2], sendLRR[3], 0x00 });
+                SendCan(0, 0x207, new byte[] { 0xFD, _enableCode3, 0x00, sendRRL[0], sendRRL[1], sendRRL[2], sendRRL[3], 0x00 });
+                SendCan(0, 0x208, new byte[] { 0xFD, _enableCode3, 0x00, sendRRR[0], sendRRR[1], sendRRR[2], sendRRR[3], 0x00 });
+            }
+            SendCan(0, 0x209, new byte[] { 0xFD, _enableCode3, 0x00, sendLArm[0], sendLArm[1], sendLArm[2], sendLArm[3], 0x00 });
+            SendCan(0, 0x20A, new byte[] { 0xFD, _enableCode3, 0x00, sendRArm[0], sendRArm[1], sendRArm[2], sendRArm[3], 0x00 });
+
+            if (cart.LeftFrontLeftErrorCode != 0 || cart.LeftFrontRightErrorCode != 0 ||
+                cart.LeftRearLeftErrorCode != 0 || cart.LeftRearRightErrorCode != 0
+                || cart.RightFrontLeftErrorCode != 0 || cart.RightFrontRightErrorCode != 0 ||
+                cart.RightRearLeftErrorCode != 0 || cart.RightRearRightErrorCode != 0)
+            {
+                //_errorCount++;
+            }
+            
+            #endregion
+
+        }
+
+        private static bool IsNodeOperational(byte remoteCode)
+        {
+            return remoteCode == 5 || remoteCode == 133;
+        }
+
+        private bool AreAllNodesOperational()
+        {
+            return IsNodeOperational(cart.LFLRemoteCode)
+                && IsNodeOperational(cart.LFRRemoteCode)
+                && IsNodeOperational(cart.RFLRemoteCode)
+                && IsNodeOperational(cart.RFRRemoteCode)
+                && IsNodeOperational(cart.LRLRemoteCode)
+                && IsNodeOperational(cart.LRRRemoteCode)
+                && IsNodeOperational(cart.RRLRemoteCode)
+                && IsNodeOperational(cart.RRRRemoteCode)
+                && IsNodeOperational(cart.LArmRemoteCode)
+                && IsNodeOperational(cart.RArmRemoteCode);
+        }
+
+        private static void SendNodeGuardRequests(Func<byte, ushort, byte[], bool, uint, MCUSerialBridgeError> sendCan)
+        {
+            sendCan(0, 0x709, Array.Empty<byte>(), true, 2);
+            sendCan(0, 0x70A, Array.Empty<byte>(), true, 2);
+            sendCan(0, 0x701, Array.Empty<byte>(), true, 2);
+            sendCan(0, 0x702, Array.Empty<byte>(), true, 2);
+            sendCan(0, 0x703, Array.Empty<byte>(), true, 2);
+            sendCan(0, 0x704, Array.Empty<byte>(), true, 2);
+            sendCan(0, 0x705, Array.Empty<byte>(), true, 2);
+            sendCan(0, 0x706, Array.Empty<byte>(), true, 2);
+            sendCan(0, 0x707, Array.Empty<byte>(), true, 2);
+            sendCan(0, 0x708, Array.Empty<byte>(), true, 2);
+        }
+
+        private void EnsureCanCallbacksRegistered()
+        {
+            if (_canCallbackRegistered)
+            {
+                return;
+            }
+            if (cart?.Bridge == null)
+            {
+                return;
+            }
+            var dispatch = BuildCanDispatchTable();
+
+            var err0 = cart.Bridge.RegisterCANPortCallback(0, msg =>
+            {
+                if (msg == null) return;
+                if (dispatch.TryGetValue(msg.ID, out var handler))
+                {
+                    handler(msg);
+                }
+            });
+            _canCallbackRegistered = true;
+        }
+
+        private void EnsureSerialCallbacksRegistered()
+        {
+            if (_serialCallbackRegistered)
+            {
+                return;
+            }
+            if (cart?.Bridge == null)
+            {
+                return;
+            }
+            var err1 = cart.Bridge.RegisterSerialPortCallback(1, msg =>
+            {
+                if (msg == null) return;
+                Hedingben.ToastText($"Callback Serial 1 Callback Received: {BitConverter.ToString(msg)}", "Serial 1");
+            });
+            Hedingben.ToastText($"Register Serial 1: {err1}", "Register Serial");
+            _serialCallbackRegistered = err1 == MCUSerialBridgeError.OK;
+        }
+
+        private void PollBatterySerial(int iteration)
+        {
+            if (cart?.Bridge == null) return;
+            if (iteration % 20 != 0) return;
+
+            var writeErr = cart.Bridge.WriteSerial(BatteryPortIndex, BatteryRequest, 60);
+            if (writeErr != MCUSerialBridgeError.OK)
+            {
+                Hedingben.ToastText($"Battery write err: {writeErr}", "Serial 2");
+                return;
+            }
+
+            var readErr = cart.Bridge.ReadSerial(BatteryPortIndex, out var msg, 40);
+            if (readErr == MCUSerialBridgeError.NoData) return;
+            if (readErr != MCUSerialBridgeError.OK)
+            {
+                Hedingben.ToastText($"Battery read err: {readErr}", "Serial 2");
+                return;
+            }
+            if (msg == null || msg.Length == 0) return;
+
+            Hedingben.ToastText($"Serial 2 RX: {BitConverter.ToString(msg)}", "Serial 2");
+            TryUpdateBatteryData(msg);
+        }
+
+        private void TryUpdateBatteryData(byte[] msg)
+        {
+            // Modbus RTU response: [id,03,08,data(8),crc(2)]
+            if (msg.Length < 13 || msg[0] != 0x01 || msg[1] != 0x03 || msg[2] < 8) return;
+            ushort calc = ComputeModbusCrc(msg, msg.Length - 2);
+            ushort recv = (ushort)(msg[msg.Length - 2] | (msg[msg.Length - 1] << 8));
+            if (calc != recv) return;
+
+            cart.Voltage = ReadInt16BE(msg, 3) * 0.1f;
+            cart.Soc = ReadUInt16BE(msg, 5);
+            cart.SOH = ReadUInt16BE(msg, 7);
+            cart.ElectricCurrent = -ReadInt16BE(msg, 9) * 0.1f;
+        }
+
+        private static byte[] BuildBatteryRequest()
+        {
+            // 01 03 00 03 00 04 CRC (读取寄存器 03~06)
+            byte[] req = new byte[] { 0x01, 0x03, 0x00, 0x03, 0x00, 0x04, 0x00, 0x00 };
+            ushort crc = ComputeModbusCrc(req, 6);
+            req[6] = (byte)(crc & 0xFF);
+            req[7] = (byte)((crc >> 8) & 0xFF);
+            return req;
+        }
+
+        private static ushort ComputeModbusCrc(byte[] data, int length)
+        {
+            ushort crc = 0xFFFF;
+            for (int i = 0; i < length; i++)
+            {
+                crc ^= data[i];
+                for (int j = 0; j < 8; j++)
+                {
+                    bool lsb = (crc & 0x0001) != 0;
+                    crc >>= 1;
+                    if (lsb) crc ^= 0xA001;
+                }
+            }
+            return crc;
+        }
+
+        private static short ReadInt16BE(byte[] data, int offset)
+        {
+            return (short)((data[offset] << 8) | data[offset + 1]);
+        }
+
+        private static ushort ReadUInt16BE(byte[] data, int offset)
+        {
+            return (ushort)((data[offset] << 8) | data[offset + 1]);
+        }
+
+        private Dictionary<ushort, Action<CANMessage>> BuildCanDispatchTable()
+        {
+            return new Dictionary<ushort, Action<CANMessage>>
+            {
+                // 速度/位置反馈 0x281~0x28A
+                [0x281] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x281 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 8) return;
+                    var rpm = DecodeRpmFromPayload(payload);
+                    cart.ActualSpeedLeftFrontLeft = ConvertRpm2Mps(rpm);
+                    cart.LFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
+                },
+                [0x282] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x282 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 8) return;
+                    var rpm = DecodeRpmFromPayload(payload);
+                    cart.ActualSpeedLeftFrontRight = -ConvertRpm2Mps(rpm);
+                    cart.LFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
+                },
+                [0x283] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x283 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 8) return;
+                    var rpm = DecodeRpmFromPayload(payload);
+                    cart.ActualSpeedRightFrontLeft = ConvertRpm2Mps(rpm);
+                    cart.RFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
+                },
+                [0x284] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x284 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 8) return;
+                    var rpm = DecodeRpmFromPayload(payload);
+                    cart.ActualSpeedRightFrontRight = -ConvertRpm2Mps(rpm);
+                    cart.RFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
+                },
+                [0x285] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x285 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 8) return;
+                    var rpm = DecodeRpmFromPayload(payload);
+                    cart.ActualSpeedLeftRearLeft = ConvertRpm2Mps(rpm);
+                    cart.LRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
+                },
+                [0x286] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x286 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 8) return;
+                    var rpm = DecodeRpmFromPayload(payload);
+                    cart.ActualSpeedLeftRearRight = -ConvertRpm2Mps(rpm);
+                    cart.LRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
+                },
+                [0x287] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x287 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 8) return;
+                    var rpm = DecodeRpmFromPayload(payload);
+                    cart.ActualSpeedRightRearLeft = ConvertRpm2Mps(rpm);
+                    cart.RRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
+                },
+                [0x288] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x288 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 8) return;
+                    var rpm = DecodeRpmFromPayload(payload);
+                    cart.ActualSpeedRightRearRight = -ConvertRpm2Mps(rpm);
+                    cart.RRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
+                },
+                [0x289] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x289 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 8) return;
+                    var rpm = DecodeRpmFromPayload(payload);
+                    cart.ActualSpeedLeftArm = ConvertRpm2Mps(rpm);
+                    cart.ActualPosLeftArm = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
+                },
+                [0x28A] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x28A CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 8) return;
+                    var rpm = DecodeRpmFromPayload(payload);
+                    cart.ActualSpeedRightArm = ConvertRpm2Mps(rpm);
+                    cart.ActualPosRightArm = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
+                },
+                // 状态/错误码 0x181~0x18A
+                [0x181] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x181 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 4) return;
+                    cart.LeftFrontLeftStateCode = BitConverter.ToInt16(payload, 0);
+                    cart.LeftFrontLeftErrorCode = BitConverter.ToInt16(payload, 2);
+                    cart.LeftFrontLeftElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
+                },
+                [0x182] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x182 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 4) return;
+                    cart.LeftFrontRightStateCode = BitConverter.ToInt16(payload, 0);
+                    cart.LeftFrontRightErrorCode = BitConverter.ToInt16(payload, 2);
+                    cart.LeftFrontRightElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
+                },
+                [0x183] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x183 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 4) return;
+                    cart.RightFrontLeftStateCode = BitConverter.ToInt16(payload, 0);
+                    cart.RightFrontLeftErrorCode = BitConverter.ToInt16(payload, 2);
+                    cart.RightFrontLeftElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
+                },
+                [0x184] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x184 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 4) return;
+                    cart.RightFrontRightStateCode = BitConverter.ToInt16(payload, 0);
+                    cart.RightFrontRightErrorCode = BitConverter.ToInt16(payload, 2);
+                    cart.RightFrontRightElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
+                },
+                [0x185] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x185 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 4) return;
+                    cart.LeftRearLeftStateCode = BitConverter.ToInt16(payload, 0);
+                    cart.LeftRearLeftErrorCode = BitConverter.ToInt16(payload, 2);
+                    cart.LeftRearLeftElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
+                },
+                [0x186] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x186 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 4) return;
+                    cart.LeftRearRightStateCode = BitConverter.ToInt16(payload, 0);
+                    cart.LeftRearRightErrorCode = BitConverter.ToInt16(payload, 2);
+                    cart.LeftRearRightElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
+                },
+                [0x187] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x187 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 4) return;
+                    cart.RightRearLeftStateCode = BitConverter.ToInt16(payload, 0);
+                    cart.RightRearLeftErrorCode = BitConverter.ToInt16(payload, 2);
+                    cart.RightRearLeftElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
+                },
+                [0x188] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x188 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 4) return;
+                    cart.RightRearRightStateCode = BitConverter.ToInt16(payload, 0);
+                    cart.RightRearRightErrorCode = BitConverter.ToInt16(payload, 2);
+                    cart.RightRearRightElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
+                },
+                [0x189] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x189 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 4) return;
+                    cart.LeftArmStateCode = BitConverter.ToInt16(payload, 0);
+                    cart.LeftArmErrorCode = BitConverter.ToInt16(payload, 2);
+                    cart.LeftArmElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
+                },
+                [0x18A] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x18A CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 4) return;
+                    cart.RightArmStateCode = BitConverter.ToInt16(payload, 0);
+                    cart.RightArmErrorCode = BitConverter.ToInt16(payload, 2);
+                    cart.RightArmElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
+                },
+
+                // 舵角 0x18B~0x18E 
+                [0x18B] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x18B CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 4) return;
+                    cart.ActualThLeftFront = BitConverter.ToInt32(payload, 0);
+                    cart.ActualThLeftFront = cart.ActualThLeftFront >= 16384
+                    ? (cart.ActualThLeftFront - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftFront
+                    : cart.ActualThLeftFront / 4096f / 5f * 360 - cart.ThBiasLeftFront;
+                },
+                [0x18C] = (msg) =>
+                {
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 4) return;
+                    var raw = BitConverter.ToInt32(payload, 0);
+                    cart.ActualThRightFront = raw >= 16384
+                        ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasRightFront
+                        : raw / 4096f / 5f * 360 - cart.ThBiasRightFront;
+                    //DLog.Log($"RF raw=0x{raw:X8}({raw}) angle={cart.ActualThRightFront:F2}", "0x18C");
+                },
+                [0x18D] = (msg) =>
+                {
+                    //Console.WriteLine($"Received 0x18D CAN Message {DateTime.Now:yyyy-MM-dd HH:mm:ss.ffffff}"); 
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 4) return;  
+                    cart.ActualThLeftRear = BitConverter.ToInt32(payload, 0);
+                    cart.ActualThLeftRear = cart.ActualThLeftRear >= 16384
+                        ? (cart.ActualThLeftRear - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftRear
+                        : cart.ActualThLeftRear / 4096f / 5f * 360 - cart.ThBiasLeftRear;
+                },
+                [0x18E] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x18E CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 4) return;
+                    cart.ActualThRightRear = BitConverter.ToInt32(payload, 0);
+                    cart.ActualThRightRear = cart.ActualThRightRear >= 16384
+                        ? (cart.ActualThRightRear - 98303) / 4096f / 5f * 360 - cart.ThBiasRightRear
+                        : cart.ActualThRightRear / 4096f / 5f * 360 - cart.ThBiasRightRear;
+                },
+
+                // 远程帧
+                [0x701] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x701 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 1) return;
+                    cart.LFLRemoteCode = payload[0];
+                },
+                [0x702] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x702 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 1) return;
+                    cart.LFRRemoteCode = payload[0];
+                },
+                [0x703] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x703 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 1) return;
+                    cart.RFLRemoteCode = payload[0];
+                },
+                [0x704] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x704 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 1) return;
+                    cart.RFRRemoteCode = payload[0];
+                },
+                [0x705] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x705 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 1) return;
+                    cart.LRLRemoteCode = payload[0];
+                },
+                [0x706] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x706 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 1) return;
+                    cart.LRRRemoteCode = payload[0];
+                },
+                [0x707] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x707 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 1) return;
+                    cart.RRLRemoteCode = payload[0];
+                },
+                [0x708] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x708 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 1) return;
+                    cart.RRRRemoteCode = payload[0];
+                },
+                [0x709] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x709 CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 1) return;
+                    cart.LArmRemoteCode = payload[0];
+                },
+                [0x70A] = (msg) =>
+                {
+                    //Console.WriteLine("Received 0x70A CAN Message");
+                    var payload = msg.Payload;
+                    if (payload == null || payload.Length < 1) return;
+                    cart.RArmRemoteCode = payload[0];
+                },
+            };
+        }
+
+    }
+}

MedullaAdapter/MCUSerialBridgeCLR.cs

@@ -0,0 +1,884 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Runtime.InteropServices;
+
+namespace MCUSerialBridgeCLR
+{
+    /// <summary>
+    /// 辅助方法与内部 C 结构体封装，用于 P/Invoke 交互
+    /// </summary>
+    internal static class PortStructHelper
+    {
+        /// <summary>
+        /// 将任意结构体序列化为字节数组
+        /// </summary>
+        /// <typeparam name="T">结构体类型</typeparam>
+        /// <param name="str">要序列化的结构体</param>
+        /// <returns>返回结构体对应的字节数组</returns>
+        /// <remarks>使用 Marshal 分配内存并复制内容</remarks>
+        public static byte[] StructToBytes<T>(T str)
+            where T : struct
+        {
+            int size = Marshal.SizeOf<T>();
+            byte[] arr = new byte[size];
+            IntPtr ptr = Marshal.AllocHGlobal(size);
+            try
+            {
+                Marshal.StructureToPtr(str, ptr, false);
+                Marshal.Copy(ptr, arr, 0, size);
+            }
+            finally
+            {
+                Marshal.FreeHGlobal(ptr);
+            }
+            return arr;
+        }
+
+        /// <summary>
+        /// 串口端口配置的原生结构体（与 MCU C 层对应）
+        /// </summary>
+        [StructLayout(LayoutKind.Sequential, Pack = 1)]
+        public struct SerialPortConfigC
+        {
+            /// <summary>端口类型（0x01 = Serial）</summary>
+            public byte port_type;
+
+            /// <summary>波特率</summary>
+            public uint baud;
+
+            /// <summary>接收帧时间间隔</summary>
+            public uint receive_frame_ms;
+
+            /// <summary>保留字节，填 0</summary>
+            [MarshalAs(UnmanagedType.ByValArray, SizeConst = 7)]
+            public byte[] reserved;
+        }
+
+        /// <summary>
+        /// CAN 端口配置的原生结构体（与 MCU C 层对应）
+        /// </summary>
+        [StructLayout(LayoutKind.Sequential, Pack = 1)]
+        public struct CANPortConfigC
+        {
+            /// <summary>端口类型（0x02 = CAN）</summary>
+            public byte port_type;
+
+            /// <summary>波特率</summary>
+            public uint baud;
+
+            /// <summary>最大重发时间</summary>
+            public uint retry_time_ms;
+
+            /// <summary>保留字节，填 0</summary>
+            [MarshalAs(UnmanagedType.ByValArray, SizeConst = 7)]
+            public byte[] reserved;
+        }
+    }
+
+    /// <summary>
+    /// MCU 固件版本信息结构体
+    /// </summary>
+    [StructLayout(LayoutKind.Sequential, CharSet = CharSet.Ansi)]
+    public struct VersionInfo
+    {
+        /// <summary>产品型号</summary>
+        [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 16)]
+        public string ProductionName;
+
+        /// <summary>Git 标签</summary>
+        [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 8)]
+        public string GitTag;
+
+        /// <summary>Git commit 哈希值</summary>
+        [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 8)]
+        public string GitCommit;
+
+        /// <summary>编译时间（字符串）</summary>
+        [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 24)]
+        public string BuildTime;
+
+        /// <summary>
+        /// 转换为可读字符串
+        /// </summary>
+        /// <returns>返回包含产品、Tag、Commit、BuildTime 的字符串</returns>
+        public override string ToString()
+        {
+            return $"Product: {ProductionName}, Tag: {GitTag}, Commit: {GitCommit}, Built: {BuildTime}";
+        }
+    }
+
+    /// <summary>
+    /// MCU 当前运行状态
+    /// </summary>
+    [StructLayout(LayoutKind.Explicit)]
+    public struct MCUState
+    {
+        /// <summary>原始 32 位状态值</summary>
+        [FieldOffset(0)]
+        public uint RawValue;
+
+        /// <summary>状态子字段 0</summary>
+        [FieldOffset(0)]
+        public byte Substate0;
+
+        /// <summary>状态子字段 1</summary>
+        [FieldOffset(1)]
+        public byte Substate1;
+
+        /// <summary>状态子字段 2</summary>
+        [FieldOffset(2)]
+        public byte Substate2;
+
+        /// <summary>高字节模式标志</summary>
+        [FieldOffset(3)]
+        public byte Mode;
+
+        /// <summary>是否处于 Bridge 模式</summary>
+        public bool IsBridge => (Mode & 0x80) == 0;
+
+        /// <summary>是否处于 DIVER 模式</summary>
+        public bool IsDIVER => (Mode & 0x80) != 0;
+
+        /// <summary>
+        /// 返回可读的状态字符串
+        /// </summary>
+        /// <returns>例如 "Bridge: Running" 或 "DIVER: Error"</returns>
+        public override string ToString()
+        {
+            string modeStr = IsBridge ? "Bridge" : "DIVER";
+            uint substate = (uint)(Substate0 | (Substate1 << 8) | (Substate2 << 16));
+
+            string subStr = substate switch
+            {
+                0x00000000 => "Idle",
+                0x0000000F => "Running",
+                0x000000FF => "Error",
+                0x00000001 => "Configured", // DIVER specific
+                0x8000000F => "Running",
+                0x800000FF => "Error",
+                _ => $"Unknown (0x{substate:X6})",
+            };
+
+            return $"{modeStr}: {subStr}";
+        }
+    }
+
+    /// <summary>
+    /// 抽象端口配置基类
+    /// </summary>
+    public abstract class PortConfig
+    {
+        /// <summary>端口类型（由子类实现）</summary>
+        public abstract byte PortType { get; }
+
+        /// <summary>序列化端口配置为字节数组（供 P/Invoke 使用）</summary>
+        /// <returns>返回固定长度字节数组（16 bytes）</returns>
+        public abstract byte[] ToBytes();
+    }
+
+    /// <summary>
+    /// 串口配置
+    /// </summary>
+    /// <remarks>
+    /// 构造函数
+    /// </remarks>
+    /// <param name="baud">波特率</param>
+    /// <param name="receiveFrameMs">接收帧间隔</param>
+    public class SerialPortConfig(uint baud, uint receiveFrameMs) : PortConfig
+    {
+        /// <summary>Serial 类型</summary>
+        public override byte PortType => 0x01;
+
+        /// <summary>波特率</summary>
+        public uint Baud { get; set; } = baud;
+
+        /// <summary>接收帧间隔</summary>
+        public uint ReceiveFrameMs { get; set; } = receiveFrameMs;
+
+        /// <summary>
+        /// 转换为字节数组
+        /// </summary>
+        /// <returns>16 字节数组</returns>
+        public override byte[] ToBytes()
+        {
+            var c = new PortStructHelper.SerialPortConfigC
+            {
+                port_type = PortType,
+                baud = Baud,
+                receive_frame_ms = ReceiveFrameMs,
+                reserved = new byte[7],
+            };
+            return PortStructHelper.StructToBytes(c);
+        }
+    }
+
+    /// <summary>
+    /// CAN 端口配置
+    /// </summary>
+    /// <remarks>
+    /// 构造函数
+    /// </remarks>
+    /// <param name="baud">波特率</param>
+    /// <param name="retryTimeMs">重发间隔</param>
+    public class CANPortConfig(uint baud, uint retryTimeMs) : PortConfig
+    {
+        /// <summary>CAN 类型</summary>
+        public override byte PortType => 0x02;
+
+        /// <summary>波特率</summary>
+        public uint Baud { get; set; } = baud;
+
+        /// <summary>重发间隔</summary>
+        public uint RetryTimeMs { get; set; } = retryTimeMs;
+
+        /// <summary>
+        /// 转换为字节数组
+        /// </summary>
+        /// <returns>16 字节数组</returns>
+        public override byte[] ToBytes()
+        {
+            var c = new PortStructHelper.CANPortConfigC
+            {
+                port_type = PortType,
+                baud = Baud,
+                retry_time_ms = RetryTimeMs,
+                reserved = new byte[7],
+            };
+            return PortStructHelper.StructToBytes(c);
+        }
+    }
+
+    /// <summary>
+    /// CAN 帧结构（标准帧 11-bit ID + 1-bit RTR + 4-bit DLC + Payload）
+    /// </summary>
+    public class CANMessage
+    {
+        /// <summary>标准帧 ID（0~0x7FF，11 位）</summary>
+        public ushort ID { get; set; }
+
+        /// <summary>远程帧标志：false = 数据帧，true = 远程帧</summary>
+        public bool RTR { get; set; }
+
+        /// <summary>数据长度码：0~8</summary>
+        public byte DLC { get; set; }
+
+        /// <summary>数据负载，长度必须严格等于 DLC（DLC=0 时可为 null）</summary>
+        public byte[] Payload { get; set; }
+
+        /// <summary>
+        /// 序列化为 MCU 协议字节流
+        /// </summary>
+        /// <returns>返回字节数组：2 bytes header + Payload</returns>
+        /// <exception cref="ArgumentOutOfRangeException">如果 DLC > 8</exception>
+        /// <exception cref="ArgumentException">如果 Payload 长度 != DLC</exception>
+        public byte[] ToBytes()
+        {
+            if (DLC > 8)
+                throw new ArgumentOutOfRangeException(nameof(DLC), "DLC must be 0-8");
+            if (DLC > 0 && (Payload == null || Payload.Length != DLC))
+                throw new ArgumentException("Payload length must equal DLC");
+
+            // 构造 2 字节 header
+            ushort header = 0;
+            header |= (ushort)(ID & 0x7FF); // bits 0-10
+            if (RTR)
+                header |= (1 << 11); // bit 11
+            header |= (ushort)((DLC & 0xF) << 12); // bits 12-15
+
+            byte[] result = new byte[2 + DLC];
+            byte[] headerBytes = BitConverter.GetBytes(header); // 小端序
+            result[0] = headerBytes[0];
+            result[1] = headerBytes[1];
+
+            if (DLC > 0)
+                Buffer.BlockCopy(Payload, 0, result, 2, DLC);
+            return result;
+        }
+
+        /// <summary>
+        /// 反序列化 MCU 协议字节流为 CANMessage
+        /// </summary>
+        /// <param name="data">原始字节数组</param>
+        /// <param name="length">实际有效长度</param>
+        /// <returns>CANMessage 实例</returns>
+        /// <exception cref="ArgumentException">数据长度错误</exception>
+        public static CANMessage FromBytes(byte[] data, uint length)
+        {
+            if (data == null || length > data.Length || length < 2)
+                throw new ArgumentException("Data must be at least 2 bytes");
+
+            ushort header = BitConverter.ToUInt16(data, 0);
+
+            var msg = new CANMessage
+            {
+                ID = (ushort)(header & 0x7FF),
+                RTR = (header & (1 << 11)) != 0,
+                DLC = (byte)((header >> 12) & 0xF),
+            };
+
+            if (msg.DLC > 0)
+            {
+                if (length < 2 + msg.DLC)
+                    throw new ArgumentException("Data length less than DLC");
+                msg.Payload = new byte[msg.DLC];
+                Buffer.BlockCopy(data, 2, msg.Payload, 0, msg.DLC);
+            }
+            else
+            {
+                msg.Payload = Array.Empty<byte>();
+            }
+
+            return msg;
+        }
+
+        public override string ToString()
+        {
+            string payloadStr =
+                (Payload == null || Payload.Length == 0)
+                    ? "[]"
+                    : "0x[" + string.Join(" ", Payload.Select(b => $"{b:X2}")) + "]";
+
+            return $"CANMessage(ID=0x{ID:X3}, RTR={RTR}, DLC={DLC}, Payload={payloadStr})";
+        }
+    }
+
+    /// <summary>
+    /// 内部 P/Invoke 层，直接映射 C DLL 函数
+    /// </summary>
+    internal static class MCUSerialBridgeCoreAPI
+    {
+        private const string DLL = @"mcu_serial_bridge.dll";
+
+        [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
+        internal static extern MCUSerialBridgeError msb_open(
+            out IntPtr handle,
+            [MarshalAs(UnmanagedType.LPStr)] string port,
+            uint baud
+        );
+
+        [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
+        internal static extern MCUSerialBridgeError msb_close(IntPtr handle);
+
+        [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
+        internal static extern MCUSerialBridgeError msb_reset(IntPtr handle, uint timeout_ms);
+
+        [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
+        public static extern MCUSerialBridgeError msb_version(
+            IntPtr handle,
+            out VersionInfo version,
+            uint timeout_ms
+        );
+
+        [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
+        public static extern MCUSerialBridgeError mcu_state(
+            IntPtr handle,
+            out MCUState state,
+            uint timeout
+        );
+
+        [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
+        internal static extern MCUSerialBridgeError msb_configure(
+            IntPtr handle,
+            uint num_ports,
+            IntPtr ports,
+            uint timeout
+        );
+
+        [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
+        internal static extern MCUSerialBridgeError msb_read_input(
+            IntPtr handle,
+            [Out] byte[] inputs,
+            uint timeout_ms
+        );
+
+        [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
+        internal static extern MCUSerialBridgeError msb_write_output(
+            IntPtr handle,
+            [In] byte[] outputs,
+            uint timeout_ms
+        );
+
+        [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
+        internal static extern MCUSerialBridgeError msb_read_port(
+            IntPtr handle,
+            byte port_index,
+            [Out] byte[] dst_data,
+            uint dst_capacity,
+            out uint out_length,
+            uint timeout_ms
+        );
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        internal delegate void msb_on_port_data_callback_function_t(
+            IntPtr dst_data,
+            uint dst_data_size,
+            IntPtr user_ctx
+        );
+
+        [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
+        internal static extern MCUSerialBridgeError msb_register_port_data_callback(
+            IntPtr handle,
+            byte port_index,
+            msb_on_port_data_callback_function_t callback,
+            IntPtr user_ctx
+        );
+
+        [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
+        internal static extern MCUSerialBridgeError msb_write_port(
+            IntPtr handle,
+            byte port_index,
+            [In] byte[] src_data,
+            uint src_data_len,
+            uint timeout_ms
+        );
+    }
+
+    /// <summary>
+    /// MCU 串口/端口操作托管封装类
+    /// 实现 IDisposable 管理底层句柄生命周期
+    /// </summary>
+    public class MCUSerialBridge : IDisposable
+    {
+        public static uint MaxPortNumber = 16;
+
+        private IntPtr nativeHandle = IntPtr.Zero;
+
+        /// <summary>判断是否已打开</summary>
+        public bool IsOpen => nativeHandle != IntPtr.Zero;
+
+        /// <summary>构造函数，初始化对象</summary>
+        public MCUSerialBridge()
+        {
+            nativeHandle = IntPtr.Zero;
+        }
+
+        /// <summary>析构函数</summary>
+        ~MCUSerialBridge()
+        {
+            Dispose(false);
+        }
+
+        /// <summary>显式释放资源</summary>
+        public void Dispose()
+        {
+            Dispose(true);
+            GC.SuppressFinalize(this);
+        }
+
+        /// <summary>内部释放资源方法</summary>
+        /// <param name="disposing">true 表示手动释放，false 表示析构释放</param>
+        private void Dispose(bool disposing)
+        {
+            if (nativeHandle != IntPtr.Zero)
+            {
+                MCUSerialBridgeCoreAPI.msb_close(nativeHandle);
+                nativeHandle = IntPtr.Zero;
+            }
+        }
+
+        /// <summary>打开串口</summary>
+        /// <param name="portName">串口名，如 "COM3"</param>
+        /// <param name="baud">波特率</param>
+        /// <returns>错误码</returns>
+        public MCUSerialBridgeError Open(string portName, uint baud)
+        {
+            return MCUSerialBridgeCoreAPI.msb_open(out nativeHandle, portName, baud);
+        }
+
+        /// <summary>关闭串口</summary>
+        /// <returns>错误码</returns>
+        public MCUSerialBridgeError Close()
+        {
+            if (nativeHandle == IntPtr.Zero)
+                return MCUSerialBridgeError.Win_HandleNotFound;
+
+            MCUSerialBridgeError error = MCUSerialBridgeCoreAPI.msb_close(nativeHandle);
+            nativeHandle = IntPtr.Zero;
+            return error;
+        }
+
+        /// <summary>MCU 复位</summary>
+        /// <returns>错误码</returns>
+        public MCUSerialBridgeError Reset(uint timeout = 200)
+        {
+            if (nativeHandle == IntPtr.Zero)
+                return MCUSerialBridgeError.Win_HandleNotFound;
+
+            return MCUSerialBridgeCoreAPI.msb_reset(nativeHandle, timeout);
+        }
+
+        /// <summary>获取固件版本</summary>
+        /// <param name="version">输出版本信息</param>
+        /// <param name="timeout">超时时间（ms）</param>
+        /// <returns>错误码</returns>
+        public MCUSerialBridgeError GetVersion(out VersionInfo version, uint timeout = 200)
+        {
+            version = new VersionInfo();
+            if (nativeHandle == IntPtr.Zero)
+                return MCUSerialBridgeError.Win_HandleNotFound;
+
+            return MCUSerialBridgeCoreAPI.msb_version(nativeHandle, out version, timeout);
+        }
+
+        /// <summary>获取 MCU 当前状态</summary>
+        /// <param name="state">输出状态</param>
+        /// <param name="timeout">超时时间（ms）</param>
+        /// <returns>错误码</returns>
+        public MCUSerialBridgeError GetState(out MCUState state, uint timeout = 200)
+        {
+            state = new MCUState();
+            if (nativeHandle == IntPtr.Zero)
+                return MCUSerialBridgeError.Win_HandleNotFound;
+
+            return MCUSerialBridgeCoreAPI.mcu_state(nativeHandle, out state, timeout);
+        }
+
+        /// <summary>配置端口</summary>
+        /// <param name="ports">端口集合</param>
+        /// <param name="timeout">超时时间（ms）</param>
+        /// <returns>错误码</returns>
+        public MCUSerialBridgeError Configure(IEnumerable<PortConfig> ports, uint timeout = 200)
+        {
+            if (nativeHandle == IntPtr.Zero)
+                return MCUSerialBridgeError.Win_HandleNotFound;
+
+            if (ports == null)
+                return MCUSerialBridgeError.Win_InvalidParam;
+
+            // 转换成数组
+            PortConfig[] portArray = ports as PortConfig[] ?? ports.ToArray();
+            int count = portArray.Length;
+
+            // 分配连续原生内存
+            int structSize = 16; // 每个 PortConfig 固定 16 字节
+            IntPtr nativePorts = Marshal.AllocHGlobal(structSize * count);
+
+            try
+            {
+                for (int i = 0; i < count; i++)
+                {
+                    byte[] bytes = portArray[i].ToBytes();
+                    if (bytes.Length != structSize)
+                        return MCUSerialBridgeError.Win_InvalidParam;
+
+                    Marshal.Copy(bytes, 0, nativePorts + i * structSize, structSize);
+                }
+
+                // 调用底层 API
+                return MCUSerialBridgeCoreAPI.msb_configure(
+                    nativeHandle,
+                    (uint)count,
+                    nativePorts,
+                    timeout
+                );
+            }
+            catch
+            {
+                return MCUSerialBridgeError.Win_InvalidParam;
+            }
+            finally
+            {
+                Marshal.FreeHGlobal(nativePorts);
+            }
+        }
+
+        /// <summary>读取输入（4 字节）</summary>
+        /// <param name="inputs">输出数组</param>
+        /// <param name="timeout">超时（ms）</param>
+        /// <returns>错误码</returns>
+        public MCUSerialBridgeError ReadInput(out byte[] inputs, uint timeout = 100)
+        {
+            inputs = new byte[4];
+            if (nativeHandle == IntPtr.Zero)
+                return MCUSerialBridgeError.Win_HandleNotFound;
+
+            return MCUSerialBridgeCoreAPI.msb_read_input(nativeHandle, inputs, timeout);
+        }
+
+        /// <summary>写输出（4 字节）</summary>
+        /// <param name="outputs">数据数组</param>
+        /// <param name="timeout">超时（ms）</param>
+        /// <returns>错误码</returns>
+        public MCUSerialBridgeError WriteOutput(byte[] outputs, uint timeout = 100)
+        {
+            if (nativeHandle == IntPtr.Zero)
+                return MCUSerialBridgeError.Win_HandleNotFound;
+
+            return MCUSerialBridgeCoreAPI.msb_write_output(nativeHandle, outputs, timeout);
+        }
+
+        /// <summary>
+        /// 读取 Serial 端口的一帧数据。
+        /// Serial 上报的数据按帧入队；本接口每次调用最多读取一帧。
+        /// 注意，如果不及时调用该接口，数据可能会丢失。
+        /// </summary>
+        /// <param name="portIndex">Serial 端口索引</param>
+        /// <param name="buffer">接收到的数据</param>
+        /// <param name="timeout">
+        /// 超时时间（毫秒）
+        ///  - 0：不等待，有数据立即返回，没有数据立即返回 MSB_Error_NoData
+        ///  - >0：若当前无数据，最多等待 timeout，期间有新帧到达则立即返回
+        /// </param>
+        /// <remarks>
+        /// 如果已经注册回调，本函数将始终返回 MSB_Error_NoData
+        /// </remarks>
+        /// <returns>
+        /// 错误码 MCUSerialBridgeError
+        ///  - OK               成功读取一帧
+        ///  - NoData           当前无可读数据（仅在 timeout == 0 或等待超时）
+        ///  - Win_InvalidParam 参数错误
+        /// </returns>
+        public MCUSerialBridgeError ReadSerial(byte portIndex, out byte[] buffer, uint timeout)
+        {
+            buffer = Array.Empty<byte>();
+
+            if (nativeHandle == IntPtr.Zero)
+                return MCUSerialBridgeError.Win_HandleNotFound;
+
+            const int MAX_PORT_FRAME = 2048;
+            byte[] tmp = new byte[MAX_PORT_FRAME];
+
+            var err = MCUSerialBridgeCoreAPI.msb_read_port(
+                nativeHandle,
+                portIndex,
+                tmp,
+                (uint)tmp.Length,
+                out uint outLen,
+                timeout
+            );
+
+            if (err != MCUSerialBridgeError.OK)
+                return err;
+
+            buffer = new byte[outLen];
+            Buffer.BlockCopy(tmp, 0, buffer, 0, (int)outLen);
+
+            return MCUSerialBridgeError.OK;
+        }
+
+        /// <summary>
+        /// 写 Serial 端口数据。
+        /// 注意：对于单个Serial端口，不支持多线程并行发送，不要在上一条数据没有发送完成之前调用该函数，否则有可能导致数据错误。
+        /// 注意：超时时间一定要大于波特率和数据长度综合得出的帧时间
+        /// </summary>
+        /// <param name="portIndex">Serial 端口索引</param>
+        /// <param name="data">待发送数据</param>
+        /// <param name="timeout">超时时间（毫秒）</param>
+        /// <returns>
+        /// 错误码 MCUSerialBridgeError
+        ///  - OK                   成功发送
+        ///  - 其他错误请查看 MCUSerialBridgeError
+        /// </returns>
+        public MCUSerialBridgeError WriteSerial(byte portIndex, byte[] data, uint timeout)
+        {
+            if (nativeHandle == IntPtr.Zero)
+                return MCUSerialBridgeError.Win_HandleNotFound;
+
+            if (data == null || data.Length == 0)
+                return MCUSerialBridgeError.Win_InvalidParam;
+
+            return MCUSerialBridgeCoreAPI.msb_write_port(
+                nativeHandle,
+                portIndex,
+                data,
+                (uint)data.Length,
+                timeout
+            );
+        }
+
+        /// <summary>
+        /// 读取 CAN 端口的一帧数据。
+        /// CAN 上报的数据按帧入队；本接口每次调用最多读取一帧。
+        /// 注意，如果不及时调用该接口，数据可能会丢失。
+        /// </summary>
+        /// <param name="portIndex">CAN 端口索引</param>
+        /// <param name="message">输出 CAN 消息对象</param>
+        /// <param name="timeout">超时时间（毫秒）</param>
+        /// <remarks>
+        /// 如果已经注册回调，本函数将始终返回 MSB_Error_NoData
+        /// </remarks>
+        /// <returns>
+        /// 错误码 MCUSerialBridgeError
+        ///  - OK                   成功读取一帧
+        ///  - NoData               当前无可读数据（仅在 timeout == 0 或等待超时）
+        ///  - Win_InvalidParam     参数错误
+        ///  - CAN_DataError        CAN数据错误
+        ///  - Win_HandleNotFound   句柄无效
+        /// </returns>
+        public MCUSerialBridgeError ReadCAN(byte portIndex, out CANMessage message, uint timeout)
+        {
+            message = null;
+
+            if (nativeHandle == IntPtr.Zero)
+                return MCUSerialBridgeError.Win_HandleNotFound;
+
+            const int MAX_FRAME = 16;
+            byte[] tmp = new byte[MAX_FRAME];
+
+            var err = MCUSerialBridgeCoreAPI.msb_read_port(
+                nativeHandle,
+                portIndex,
+                tmp,
+                (uint)tmp.Length,
+                out uint outLen,
+                timeout
+            );
+            if (err != MCUSerialBridgeError.OK)
+                return err;
+
+            try
+            {
+                message = CANMessage.FromBytes(tmp, outLen);
+            }
+            catch
+            {
+                return MCUSerialBridgeError.CAN_DataError;
+            }
+            return MCUSerialBridgeError.OK;
+        }
+
+        /// <summary>
+        /// 写 CAN 端口数据。
+        /// 注意：CAN 支持多线程发送，最多可同时发送 16 个消息。
+        /// 但是，多个CAN消息会进入排队队列等待发送，如果消息太多，可能引起超时。
+        /// </summary>
+        /// <param name="portIndex">CAN 端口索引</param>
+        /// <param name="message">待发送 CAN 消息对象</param>
+        /// <param name="timeout">超时时间（毫秒），默认 500ms</param>
+        /// <returns>
+        /// 错误码 MCUSerialBridgeError
+        ///  - OK                   成功发送
+        ///  - Win_InvalidParam     参数错误
+        ///  - CAN_DataError        CAN 数据错误
+        ///  - Win_HandleNotFound   句柄无效
+        /// </returns>
+        public MCUSerialBridgeError WriteCAN(byte portIndex, CANMessage message, uint timeout)
+        {
+            if (nativeHandle == IntPtr.Zero)
+                return MCUSerialBridgeError.Win_HandleNotFound;
+            if (message == null)
+                return MCUSerialBridgeError.Win_InvalidParam;
+            try
+            {
+                byte[] buffer = message.ToBytes();
+                return MCUSerialBridgeCoreAPI.msb_write_port(
+                    nativeHandle,
+                    portIndex,
+                    buffer,
+                    (uint)buffer.Length,
+                    timeout
+                );
+            }
+            catch
+            {
+                return MCUSerialBridgeError.CAN_DataError;
+            }
+        }
+
+        private readonly Dictionary<
+            byte,
+            MCUSerialBridgeCoreAPI.msb_on_port_data_callback_function_t
+        > _portCallbacks = [];
+
+        /// <summary>
+        /// 注册指定端口(Serial)的回调函数
+        /// </summary>
+        /// <param name="portIndex">端口索引</param>
+        /// <param name="callback">接收数据回调，byte[] 为接收到的原始数据</param>
+        /// <returns>错误码</returns>
+        /// <remarks>
+        /// 注意事项：
+        /// 1. 回调会在底层 C 层线程中直接调用，请**不要在回调内阻塞**，例如等待 I/O 或 Sleep。
+        /// 2. 回调内**不能调用 WriteSerial/WriteCAN 等发送函数**，否则可能导致死锁或丢帧。
+        /// 3. 回调内只能做轻量级操作，例如简单解析、统计或打标记。
+        /// 4. 若需要复杂处理（例如长时间解析、解码、存储数据库等），请**将数据入队到另一个线程**，再在后台处理。
+        /// 5. 数据可能随时到来，请保证回调尽快返回，避免影响后续帧接收。
+        /// 6. 不要把其他类型的端口注册到这个接口，接口不对 portIndex 做类型检查。
+        /// </remarks>
+        public MCUSerialBridgeError RegisterSerialPortCallback(
+            byte portIndex,
+            Action<byte[]> callback
+        )
+        {
+            if (callback == null)
+                return MCUSerialBridgeError.Win_InvalidParam;
+
+            if (portIndex > MaxPortNumber)
+                return MCUSerialBridgeError.Config_PortNumOver;
+
+            // 包装 C# 回调为 P/Invoke 委托
+            void del(IntPtr dst_data, uint dst_data_size, IntPtr user_ctx)
+            {
+                byte[] data = new byte[dst_data_size];
+                Marshal.Copy(dst_data, data, 0, (int)dst_data_size);
+                callback(data);
+            }
+
+            // 保存引用，防止 GC 回收
+            _portCallbacks[portIndex] = del;
+
+            // 调用 C 层注册
+            return MCUSerialBridgeCoreAPI.msb_register_port_data_callback(
+                nativeHandle,
+                portIndex,
+                _portCallbacks[portIndex],
+                IntPtr.Zero
+            );
+        }
+
+        /// <summary>
+        /// 注册指定端口(CAN)的回调函数
+        /// </summary>
+        /// <param name="portIndex">端口索引</param>
+        /// <param name="callback">接收数据回调，CANMessage 为接收到的原始数据</param>
+        /// <returns>错误码</returns>
+        /// <remarks>
+        /// 注意事项：
+        /// 1. 回调会在底层 C 层线程中直接调用，请**不要在回调内阻塞**，例如等待 I/O 或 Sleep。
+        /// 2. 回调内**不能调用 WriteSerial/WriteCAN 等发送函数**，否则可能导致死锁或丢帧。
+        /// 3. 回调内只能做轻量级操作，例如简单解析、统计或打标记。
+        /// 4. 若需要复杂处理（例如长时间解析、解码、存储数据库等），请**将数据入队到另一个线程**，再在后台处理。
+        /// 5. 数据可能随时到来，请保证回调尽快返回，避免影响后续帧接收。
+        /// 6. 不要把其他类型的端口注册到这个接口，接口不对 portIndex 做类型检查。
+        /// </remarks>
+        public MCUSerialBridgeError RegisterCANPortCallback(
+            byte portIndex,
+            Action<CANMessage> callback
+        )
+        {
+            if (callback == null)
+                return MCUSerialBridgeError.Win_InvalidParam;
+
+            if (portIndex > MaxPortNumber)
+                return MCUSerialBridgeError.Config_PortNumOver;
+
+            // 包装 C# 回调为 P/Invoke 委托
+            void del(IntPtr dst_data, uint dst_data_size, IntPtr user_ctx)
+            {
+                try
+                {
+                    byte[] data = new byte[dst_data_size];
+                    Marshal.Copy(dst_data, data, 0, (int)dst_data_size);
+                    CANMessage msg = CANMessage.FromBytes(data, dst_data_size);
+                    callback(msg);
+                }
+                catch
+                {
+                    // 解析失败直接忽略，保证回调不会抛异常阻塞 C 层线程
+                }
+            }
+
+            // 保存引用，防止 GC 回收
+            _portCallbacks[portIndex] = del;
+
+            // 调用 C 层注册
+            return MCUSerialBridgeCoreAPI.msb_register_port_data_callback(
+                nativeHandle,
+                portIndex,
+                _portCallbacks[portIndex],
+                IntPtr.Zero
+            );
+        }
+    }
+}

MedullaAdapter/MCUSerialBridgeError.cs

@@ -0,0 +1,97 @@
+using System;
+
+namespace MCUSerialBridgeCLR
+{
+    public enum MCUSerialBridgeError : uint
+    {
+        OK = 0x00000000, // Success
+        NoData = 0x00000001, // Port no new data
+        Win_Unknown = 0x80000001, // Unknown Windows error
+        Win_InvalidParam = 0x80000002, // Invalid parameter
+        Win_AllocFail = 0x80000003, // Memory allocation failed
+        Win_HandleNotFound = 0x80000004, // Handle not found
+        Win_ResourceBusy = 0x80000005, // Resource busy
+        Win_BufferFull = 0x80000006, // Buffer is full
+        Win_UserBufferTooSmall = 0x80000007, // Buffer is too small
+        Win_CannotOpenPort = 0x80000010, // Cannot open port
+        Win_CannotGetCommState = 0x80000011, // Cannot get comm state
+        Win_CannotSetCommState = 0x80000012, // Cannot set comm state
+        Win_CannotCreateThread = 0x80000013, // Cannot create thread
+        Proto_Invalid = 0xE0000001, // Protocol invalid
+        Proto_Checksum = 0xE0000002, // CRC check failed
+        Proto_Timeout = 0xE0000003, // Protocol timeout
+        Proto_FrameTooLong = 0xE0000004, // Frame too long
+        Proto_UnknownCommand = 0xE0000005, // Unknown command
+        Proto_InvalidPayload = 0xE0000006, // Invalid payload
+        State_NotRunning = 0xF0000000, // Not running, configure
+        State_Running = 0xF0000001, // Can not configure, already running
+        Config_PortNumOver = 0xC0000000, // Port number over
+        Config_SerialNumOver = 0xC0000001, // Serial port number over
+        Config_CANNumOver = 0xC0000002, // CAN number over
+        Config_UnknownPortType = 0xC0000010, // Unknown Port Type
+        Serial_OpenFail = 0x01000001, // Serial open failed
+        Serial_NotOpen = 0x01000002, // Serial not open
+        Serial_ReadFail = 0x01000003, // Serial read failed
+        Serial_WriteFail = 0x01000004, // Serial write failed
+        Serial_Busy = 0x01000005, // Serial is busy
+        CAN_DataError = 0x02000000, // CAN data error
+        CAN_SendFail = 0x02000001, // CAN send failed
+        CAN_RecvFail = 0x02000002, // CAN receive failed
+        CAN_BufferFull = 0x02000003, // CAN buffer full
+        CAN_NotInit = 0x02000004, // CAN not initialized
+        Port_WriteBusy = 0x10000001, // Port is busy now
+        MCU_Unknown = 0x00010001, // MCU unknown error
+        MCU_IOSizeError = 0x00010002, // IO Should be 4 bytes
+        MCU_OverTemperature = 0x00010010, // MCU over temperature
+    }
+
+    public static class MCUSerialBridgeErrorExtensions
+    {
+        public static string ToDescription(this MCUSerialBridgeError err)
+        {
+            return err switch
+            {
+                MCUSerialBridgeError.OK => "OK|Success",
+                MCUSerialBridgeError.NoData => "NoData|Port no new data",
+                MCUSerialBridgeError.Win_Unknown => "Win_Unknown|Unknown Windows error",
+                MCUSerialBridgeError.Win_InvalidParam => "Win_InvalidParam|Invalid parameter",
+                MCUSerialBridgeError.Win_AllocFail => "Win_AllocFail|Memory allocation failed",
+                MCUSerialBridgeError.Win_HandleNotFound => "Win_HandleNotFound|Handle not found",
+                MCUSerialBridgeError.Win_ResourceBusy => "Win_ResourceBusy|Resource busy",
+                MCUSerialBridgeError.Win_BufferFull => "Win_BufferFull|Buffer is full",
+                MCUSerialBridgeError.Win_UserBufferTooSmall => "Win_UserBufferTooSmall|Buffer is too small",
+                MCUSerialBridgeError.Win_CannotOpenPort => "Win_CannotOpenPort|Cannot open port",
+                MCUSerialBridgeError.Win_CannotGetCommState => "Win_CannotGetCommState|Cannot get comm state",
+                MCUSerialBridgeError.Win_CannotSetCommState => "Win_CannotSetCommState|Cannot set comm state",
+                MCUSerialBridgeError.Win_CannotCreateThread => "Win_CannotCreateThread|Cannot create thread",
+                MCUSerialBridgeError.Proto_Invalid => "Proto_Invalid|Protocol invalid",
+                MCUSerialBridgeError.Proto_Checksum => "Proto_Checksum|CRC check failed",
+                MCUSerialBridgeError.Proto_Timeout => "Proto_Timeout|Protocol timeout",
+                MCUSerialBridgeError.Proto_FrameTooLong => "Proto_FrameTooLong|Frame too long",
+                MCUSerialBridgeError.Proto_UnknownCommand => "Proto_UnknownCommand|Unknown command",
+                MCUSerialBridgeError.Proto_InvalidPayload => "Proto_InvalidPayload|Invalid payload",
+                MCUSerialBridgeError.State_NotRunning => "State_NotRunning|Not running, configure",
+                MCUSerialBridgeError.State_Running => "State_Running|Can not configure, already running",
+                MCUSerialBridgeError.Config_PortNumOver => "Config_PortNumOver|Port number over",
+                MCUSerialBridgeError.Config_SerialNumOver => "Config_SerialNumOver|Serial port number over",
+                MCUSerialBridgeError.Config_CANNumOver => "Config_CANNumOver|CAN number over",
+                MCUSerialBridgeError.Config_UnknownPortType => "Config_UnknownPortType|Unknown Port Type",
+                MCUSerialBridgeError.Serial_OpenFail => "Serial_OpenFail|Serial open failed",
+                MCUSerialBridgeError.Serial_NotOpen => "Serial_NotOpen|Serial not open",
+                MCUSerialBridgeError.Serial_ReadFail => "Serial_ReadFail|Serial read failed",
+                MCUSerialBridgeError.Serial_WriteFail => "Serial_WriteFail|Serial write failed",
+                MCUSerialBridgeError.Serial_Busy => "Serial_Busy|Serial is busy",
+                MCUSerialBridgeError.CAN_DataError => "CAN_DataError|CAN data error",
+                MCUSerialBridgeError.CAN_SendFail => "CAN_SendFail|CAN send failed",
+                MCUSerialBridgeError.CAN_RecvFail => "CAN_RecvFail|CAN receive failed",
+                MCUSerialBridgeError.CAN_BufferFull => "CAN_BufferFull|CAN buffer full",
+                MCUSerialBridgeError.CAN_NotInit => "CAN_NotInit|CAN not initialized",
+                MCUSerialBridgeError.Port_WriteBusy => "Port_WriteBusy|Port is busy now",
+                MCUSerialBridgeError.MCU_Unknown => "MCU_Unknown|MCU unknown error",
+                MCUSerialBridgeError.MCU_IOSizeError => "MCU_IOSizeError|IO Should be 4 bytes",
+                MCUSerialBridgeError.MCU_OverTemperature => "MCU_OverTemperature|MCU over temperature",
+                _ => "Unknown Error",
+            };
+        }
+    }
+}

MedullaAdapter/MedullaAdapter.csproj

@@ -0,0 +1,44 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+  <PropertyGroup>
+    <TargetFramework>netstandard2.0</TargetFramework>
+	<LangVersion>latest</LangVersion>
+  </PropertyGroup>
+	
+  <ItemGroup>
+	  <Compile Remove="EmbeddedCommunication.cs" />
+  </ItemGroup>
+	
+  <ItemGroup>
+    <PackageReference Include="Fody" Version="6.9.3">
+      <PrivateAssets>all</PrivateAssets>
+      <IncludeAssets>runtime; build; native; contentfiles; analyzers; buildtransitive</IncludeAssets>
+    </PackageReference>
+    <PackageReference Include="Newtonsoft.Json" Version="13.0.4" />
+    <PackageReference Include="System.IO.Ports" Version="6.0.0" />
+  </ItemGroup>
+
+  <ItemGroup>
+    <Reference Include="CommonUsage">
+      <HintPath>ref\CommonUsage.dll</HintPath>
+    </Reference>
+    <Reference Include="CycleGUI">
+      <HintPath>ref\CycleGUI.dll</HintPath>
+    </Reference>
+    <Reference Include="MDCSToolBox">
+      <HintPath>ref\MDCSToolBox.dll</HintPath>
+    </Reference>
+    <Reference Include="CartActivator">
+      <HintPath>ref\RefCartActivator.dll</HintPath>
+    </Reference>
+    <Reference Include="FundamentalLib">
+      <HintPath>ref\RefFundamentalLib.dll</HintPath>
+    </Reference>
+    <Reference Include="MedullaCore">
+      <HintPath>ref\RefMedullaCore.dll</HintPath>
+    </Reference>
+  </ItemGroup>
+	<ItemGroup>
+		<WeaverFiles Include="deps\DiverCompiler.exe" />
+	</ItemGroup>
+</Project>

MedullaAdapter/MotorRoutine.cs

@@ -0,0 +1,233 @@
+using CartActivator;
+using CommonUsage.Mathematics;
+using FundamentalLib;
+using MDCSToolBox.Commons;
+using MDCSToolBox.Commons.Controllers;
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using static MDCSToolBox.Medulla.Chassis.BasicCartDefinition;
+
+namespace MedullaAdapter
+{
+    public class MotorRoutine:LadderLogic<DiverCartDefinition>
+    {
+        private DateTime _lastMoveTime = DateTime.Now;
+        private DateTime _transmitterFleetDbgLastLog = DateTime.MinValue;
+        private DateTime _transmitterFleetResetLastLog = DateTime.MinValue;
+
+        private void LogTransmitterFleet(string branch, bool force = false)
+        {
+            var now = DateTime.Now;
+            if (!force && (now - _transmitterFleetDbgLastLog).TotalMilliseconds < 200) return;
+            _transmitterFleetDbgLastLog = now;
+            DLog.Log(
+                $"branch={branch} connected={cart.TransmitterConnected} ctrl={cart.TransmitterControlEnable} " +
+                $"SA={cart.Transmitter_SA} SC={cart.Transmitter_SC} SB={cart.Transmitter_SB} SD={cart.Transmitter_SD} car={cart.CarNum} " +
+                $"joyLx={cart.TransmitterLeftJoystickValX:0.000} joyRy={cart.TransmitterRightJoystickValY:0.000} joyRx={cart.TransmitterRightJoystickValX:0.000} " +
+                $"rawLx={cart.TransmitterLeftJoystickValXRaw} rawRy={cart.TransmitterRightJoystickValYRaw} " +
+                $"remotePanel={cart.MultiVehicleRemoteManualEnabled} outEn={cart.MultiVehicleManualEnabled} outMode={cart.MultiVehicleManualMode} " +
+                $"outVx={cart.MultiVehicleManualVx:0.000} outVy={cart.MultiVehicleManualVy:0.000} outVth={cart.MultiVehicleManualVth:0.000} hold={cart.MultiVehicleHold}",
+                "TransmitterFleetDbg");
+        }
+
+        private void LogTransmitterFleetReset(string reason)
+        {
+            var now = DateTime.Now;
+            if ((now - _transmitterFleetResetLastLog).TotalMilliseconds < 500) return;
+            _transmitterFleetResetLastLog = now;
+            DLog.Log(
+                $"reset reason={reason} remotePanel={cart.MultiVehicleRemoteManualEnabled} beforeEn={cart.MultiVehicleManualEnabled} " +
+                $"beforeMode={cart.MultiVehicleManualMode} beforeVx={cart.MultiVehicleManualVx:0.000} beforeVy={cart.MultiVehicleManualVy:0.000} " +
+                $"beforeVth={cart.MultiVehicleManualVth:0.000} hold={cart.MultiVehicleHold}",
+                "TransmitterFleetDbg");
+        }
+
+        public override void Operation(int iteration)
+        {
+            void ResetMultiVehicle(string reason)
+            {
+                if (cart.MultiVehicleRemoteManualEnabled)
+                {
+                    LogTransmitterFleet("reset-skipped-remote-panel");
+                    return;
+                }
+
+                if (cart.MultiVehicleManualEnabled || cart.MultiVehicleManualVx != 0 || cart.MultiVehicleManualVy != 0 ||
+                    cart.MultiVehicleManualVth != 0 || cart.MultiVehicleHold)
+                    LogTransmitterFleetReset(reason);
+
+                cart.MultiVehicleManualEnabled = false;
+                cart.MultiVehicleManualVx = 0;
+                cart.MultiVehicleManualVy = 0;
+                cart.MultiVehicleManualVth = 0;
+                cart.MultiVehicleHold = false;
+            }
+
+            TriggerOnce(cart.TransmitterConnected && cart.Transmitter_SA, 700,
+                () =>
+                {
+                    Console.WriteLine("遥控器使能!");
+                    cart.TransmitterControlEnable = true;
+                    cart.TransmitterLastTime = DateTime.Now;
+                    LogTransmitterFleet("enable", true);
+                });
+            TriggerOnce(cart.TransmitterConnected && !cart.Transmitter_SA, 700,
+                () =>
+                {
+                    Console.WriteLine("遥控器断使能!");
+                    cart.TransmitterControlEnable = false;
+                    LogTransmitterFleet("disable", true);
+                });
+
+            if (cart.TransmitterControlEnable && cart.Transmitter_SC == cart.CarNum)
+            {
+                CartDefinition.testPriority(5, "TransmitterMode");
+                ResetMultiVehicle("single-car-control");
+                LogTransmitterFleet("single-car-control");
+                TransmitterChassisControl();
+                cart.TransmitterLastTime = DateTime.Now;
+                cart.CarStatu = "Transmitter控制模式使能";
+            }
+            else if (cart.TransmitterControlEnable && cart.Transmitter_SC == 3)
+            {
+                cart.CarStatu = "Transmitter双车联动模式使能";
+                cart.MultiVehicleManualEnabled = true;
+                var fleetMode = 0;
+                if (cart.Transmitter_SB == TransmitterState.Mode0) fleetMode = 0;
+                else if (cart.Transmitter_SB == TransmitterState.Mode1) fleetMode = 1;
+                else if (cart.Transmitter_SB == TransmitterState.Mode2) fleetMode = 2;
+                cart.ApplyFleetManualCommand(fleetMode, -cart.TransmitterLeftJoystickValX,
+                    cart.TransmitterRightJoystickValY, 1f);
+
+                // hold multi-vehicle sync, make speed 0
+                cart.MultiVehicleHold = cart.Transmitter_SD == TransmitterState.Mode1;
+                LogTransmitterFleet("fleet-control");
+            }
+            else
+            {
+                ResetMultiVehicle("idle-or-sc-mismatch");
+                LogTransmitterFleet("idle-or-sc-mismatch");
+                cart.CarStatu = "正常运行";
+            }
+
+            cart.ClumsyControl = true;
+            cart.LeftFrontPid.ChangeParameters(cart.DiffSteerKp, cart.DiffSteerKi, cart.DiffSteerKd, cart.DiffSteerMaxI,
+                cart.DiffSteerDeadZone, cart.DiffSteerThresh, cart.DiffSteerSpeedAcc);
+            cart.LeftRearPid.ChangeParameters(cart.DiffSteerKp, cart.DiffSteerKi, cart.DiffSteerKd, cart.DiffSteerMaxI,
+                cart.DiffSteerDeadZone, cart.DiffSteerThresh, cart.DiffSteerSpeedAcc);
+            cart.RightFrontPid.ChangeParameters(cart.DiffSteerKp, cart.DiffSteerKi, cart.DiffSteerKd, cart.DiffSteerMaxI,
+                cart.DiffSteerDeadZone, cart.DiffSteerThresh, cart.DiffSteerSpeedAcc);
+            cart.RightRearPid.ChangeParameters(cart.DiffSteerKp, cart.DiffSteerKi, cart.DiffSteerKd, cart.DiffSteerMaxI,
+                cart.DiffSteerDeadZone, cart.DiffSteerThresh, cart.DiffSteerSpeedAcc);
+            var diffLf = cart.LeftFrontPid.GetResponse(cart.ThLeftFront, false, false, "LF");
+            var diffRf = cart.RightFrontPid.GetResponse(cart.ThRightFront, false, false, "RF");
+            var diffLr = cart.LeftRearPid.GetResponse(cart.ThLeftRear, false, false, "LR");
+            var diffRr = cart.RightRearPid.GetResponse(cart.ThRightRear, false, false, "RR");
+            // Hedingben.ToastText($"LF:{diffLf:0.00},LR:{diffLr:0.00},RF:{diffRf:0.00},RR:{diffRr:0.00}", "wheel-pid");
+            DLog.Log($"[wheel pid] {cart.ActualThLeftFront:0.00}->{cart.ThLeftFront:0.00} diff:{CommonMath.ThDiff(cart.ThLeftFront, cart.ActualThLeftFront):0.00} pid:{diffLf:0.00}");
+            cart.SpeedLFL = cart.SpeedLeftFrontLeft - diffLf;
+            cart.SpeedLFR = cart.SpeedLeftFrontRight + diffLf;
+            cart.SpeedRFL = cart.SpeedRightFrontLeft - diffRf;
+            cart.SpeedRFR = cart.SpeedRightFrontRight + diffRf;
+            cart.SpeedLRL = cart.SpeedLeftRearLeft - diffLr;
+            cart.SpeedLRR = cart.SpeedLeftRearRight + diffLr;
+            cart.SpeedRRL = cart.SpeedRightRearLeft - diffRr;
+            cart.SpeedRRR = cart.SpeedRightRearRight + diffRr;
+            List<float> wheelSpeed = new float[]
+            {
+                Math.Abs(cart.SpeedLFL), Math.Abs(cart.SpeedLFR), Math.Abs(cart.SpeedRFL),
+                Math.Abs(cart.SpeedRFR), Math.Abs(cart.SpeedLRL),Math.Abs(cart.SpeedLRR), Math.Abs(cart.SpeedRRL),
+                Math.Abs(cart.SpeedRRR)
+            }.ToList();
+            var wheelMaxSpeed = wheelSpeed.Max();
+            var speedSign = Math.Sign(cart.ThresSpeed - cart.SendThresSpeed);
+            var acc = Math.Abs(cart.ThresSpeed) > Math.Abs(cart.SendThresSpeed) ? cart.Chassis.AccPerSecond : cart.Chassis.DeAccPerSecond;
+            cart.SendThresSpeed += speedSign * Math.Min(Math.Abs(cart.ThresSpeed - cart.SendThresSpeed),
+                acc * (float)(DateTime.Now - _lastMoveTime).TotalSeconds);
+            if (cart.ThresSpeed < wheelMaxSpeed && cart.SendThresSpeed > wheelMaxSpeed)
+                cart.SendThresSpeed = Math.Min(cart.SendThresSpeed, wheelMaxSpeed);
+            _lastMoveTime = DateTime.Now;
+
+            //灯光控制
+            if (cart.AlarmLevel == 2)
+            {
+                //故障红灯常亮
+                cart.LightMode = 2;
+            }
+            else if (cart.ThresSpeed != 1)
+            {
+                //避障黄灯常亮
+                cart.LightMode = 3;
+            }
+            else if (!cart.WheelAbleState)
+            {
+                FlipFlop(ref cart.LightMode, 500, 0, 3);
+            }
+            else if (cart.Soc < cart.LowBatteryAlarmThreshold && cart.ElectricCurrent >= -10)
+            {
+                cart.LightMode = 3;
+            }
+            else if (cart.ElectricCurrent < -10)
+            {
+                FlipFlop(ref cart.LightMode, 500, 0, 1);
+            }
+            else
+            {
+                FlipFlop(ref cart.LightMode, 500, 0, 1);
+            }
+        }
+        public void TransmitterChassisControl()
+        {
+            TriggerOnce(cart.Transmitter_SB == TransmitterState.Mode0, 100,
+                () => cart.TransmitterControlMode = DiverCartDefinition.ManualControlMode.Normal);
+            TriggerOnce(cart.Transmitter_SB == TransmitterState.Mode1, 100,
+                () => cart.TransmitterControlMode = DiverCartDefinition.ManualControlMode.Crab);
+            TriggerOnce(cart.Transmitter_SB == TransmitterState.Mode2, 100,
+                () => cart.TransmitterControlMode = DiverCartDefinition.ManualControlMode.Spin);
+
+            Hedingben.ToastText($"X方向速度: {cart.TransmitterLeftJoystickValX}  Y方向速度:  {cart.TransmitterRightJoystickValY}");
+
+            if (cart.Transmitter_SD == TransmitterState.Mode1)
+            {
+                if(cart.Transmitter_T3 == 1694)
+                    cart.WheelReset();
+                else if(cart.Transmitter_T3 == 151)
+                    cart.WheelDisable();
+            }
+            if (cart.Transmitter_SD == TransmitterState.Mode1 && cart.TransmitterLeftJoystickValXRaw == 353)
+            {
+                TriggerOnce(cart.TransmitterRightJoystickValYRaw == 353, 100, () => cart.TransmitterSpeed += 0.1f);
+                TriggerOnce(cart.TransmitterRightJoystickValYRaw == 1694, 100, () => cart.TransmitterSpeed -= 0.1f);
+            }
+            cart.TransmitterSpeed = Math.Max(Math.Min(cart.TransmitterSpeed, cart.TransmitterSpeedUpperLimit), cart.TransmitterSpeedLowerLimit);
+            //cart.TransmitterSpeed = Math.Max(cart.TransmitterSpeed, cart.TransmitterSpeedLowerLimit);
+            if (!cart.Transmitter_SA)
+            {
+                cart.ManualControl(cart.TransmitterControlMode, 0,
+                    0, 0, cart.TransmitterSpeed,
+                    DateTime.Now - cart.TransmitterLastTime);
+                cart.SpeedLeftArm = 0;
+                cart.SpeedRightArm = 0;
+            }
+            //摇杆控制底盘
+            else if (cart.Transmitter_SD == TransmitterState.Mode0)
+            {
+                cart.ManualControl(cart.TransmitterControlMode, cart.TransmitterLeftJoystickValX,
+                    cart.TransmitterRightJoystickValY, 0, cart.TransmitterSpeed,
+                    DateTime.Now - cart.TransmitterLastTime);
+                cart.SpeedLeftArm = 0;
+                cart.SpeedRightArm = 0;
+            }
+            //右摇杆同时控制左右夹臂
+            else if (cart.Transmitter_SD == TransmitterState.Mode1 && cart.TransmitterLeftJoystickValX == 0)
+            {
+                cart.ManualControl(cart.TransmitterControlMode, 0,
+                    0, 0, cart.TransmitterSpeed,
+                    DateTime.Now - cart.TransmitterLastTime);
+                cart.SpeedLeftArm = cart.TransmitterRightJoystickValX * cart.ManualArmSpeedFac;
+                cart.SpeedRightArm = cart.TransmitterRightJoystickValX * cart.ManualArmSpeedFac;
+            }
+        }
+    }
+}

MedullaAdapter/PackageStruct.cs

@@ -0,0 +1,168 @@
+using System;
+
+namespace MedullaAdapter
+{
+    public class BasePack
+    {
+        private static ushort CalculateCRC16(byte[] data)
+        {
+            const ushort polynomial = 0xA001; // CRC-16-IBM多项式
+            ushort crc = 0xFFFF; // 初始值
+
+            foreach (byte b in data)
+            {
+                crc ^= b; // 将数据字节与CRC寄存器按位异或
+
+                for (int i = 0; i < 8; i++)
+                {
+                    if ((crc & 0x0001) != 0)
+                    {
+                        crc >>= 1;
+                        crc ^= polynomial;
+                    }
+                    else
+                    {
+                        crc >>= 1;
+                    }
+                }
+            }
+            return crc;
+        }
+
+        private byte[] _data;
+
+        public void SetData(byte[] data)
+        {
+            _data = data;
+        }
+
+        public byte[] GetPack()
+        {
+            var pack = new byte[_data.Length+7];
+            pack[0] = 0xBB;
+            pack[1] = 0xAA;
+            var length = BitConverter.GetBytes(_data.Length);
+            pack[2] = length[0];
+            pack[3] = length[1];
+            for (int i = 0; i < _data.Length; i++)
+            {
+                pack[4+i] = _data[i];
+            }
+
+            var crcByes = new byte[pack.Length - 5];
+            Array.Copy(pack,2,crcByes,0,crcByes.Length);
+            var crc = BitConverter.GetBytes(CalculateCRC16(crcByes));
+            pack[pack.Length-1] = 0xEE;
+            pack[pack.Length-2] = crc[1];
+            pack[pack.Length-3] = crc[0];
+            return pack;
+        }
+    }
+
+    public class ControlPack : BasePack
+    {
+        public ControlPack(byte controlCode)
+        {
+            byte[] controlData = new byte[3]{0x01,0x00,controlCode};
+            SetData(controlData);
+        }
+    }
+
+    public class SetConfigPack : BasePack
+    {
+        public SetConfigPack(int can1BaudRate, int can2BaudRate,int modbus0BaudRate, int modbus1BaudRate, int modbus2BaudRate,
+            int serialBaudRate,int upperSize = 1024,int lowerSize =1024 )
+        {
+            var upper = BitConverter.GetBytes(upperSize);
+            var lower = BitConverter.GetBytes(lowerSize);
+            var can = BitConverter.GetBytes(can1BaudRate);
+            var can2 = BitConverter.GetBytes(can2BaudRate);
+            var modbus0 = BitConverter.GetBytes(modbus0BaudRate);
+            var modbus1 = BitConverter.GetBytes(modbus1BaudRate);
+            var modbus2 = BitConverter.GetBytes(modbus2BaudRate);
+            var serial = BitConverter.GetBytes(serialBaudRate);
+            var canBuffer = BitConverter.GetBytes(128);
+            var mbBuffer = BitConverter.GetBytes(512);
+            var serialBuffer = BitConverter.GetBytes(1024);
+            byte[] configData = new byte[]
+            {
+                0x01, 0x10, 0x02, upper[0], upper[1], upper[2], upper[3], lower[0], lower[1],
+                lower[2], lower[3], 0x06, 0x00, 0x00, can[0], can[1], can[2], can[3], canBuffer[0], canBuffer[1], 0x00,
+                can2[0], can2[1], can2[2], can2[3], canBuffer[0], canBuffer[1], 0x10, modbus0[0],
+                modbus0[1], modbus0[2], modbus0[3], mbBuffer[0], mbBuffer[1], 0x10, modbus1[0],
+                modbus1[1], modbus1[2], modbus1[3], mbBuffer[0], mbBuffer[1], 0x10, modbus2[0],
+                modbus2[1], modbus2[2], modbus2[3], mbBuffer[0], mbBuffer[1], 0x20, serial[0], serial[1], serial[2],
+                serial[3], serialBuffer[0], serialBuffer[1]
+            };
+            SetData(configData);
+        }
+    }
+
+    public class ReadConfigPack : BasePack
+    {
+        public ReadConfigPack()
+        {
+            var readConfigData = new byte[] { 0x01, 0x10, 0x00 };
+            SetData(readConfigData);
+        }
+    }
+
+    public class DownloadCodePack : BasePack
+    {
+        public DownloadCodePack(int totalLength, int offset, int currentLength, byte[] codeBytes)
+        {
+            var codeData = new byte[12 + codeBytes.Length];
+            codeData[0] = 0x01;
+            codeData[1] = 0x11;
+            var totalLengthBytes = BitConverter.GetBytes(totalLength);
+            for (int i = 0; i < 4; i++)
+            {
+                codeData[2 + i] = totalLengthBytes[i];
+            }
+            var offsetBytes = BitConverter.GetBytes(offset);
+            for (int i = 0; i < 4; i++)
+            {
+                codeData[6 + i] = offsetBytes[i];
+            }
+            var curLengthBytes = BitConverter.GetBytes(currentLength);
+            for (int i = 0; i < 2; i++)
+            {
+                codeData[10 + i] = curLengthBytes[i];
+            }
+
+            for (int i = 0; i < codeBytes.Length; i++)
+            {
+                codeData[12+i] = codeBytes[i];
+            }
+            SetData(codeData);
+        }
+
+    }
+
+    public class HeartBeatPack : BasePack
+    {
+        public HeartBeatPack(int state, int error)
+        {
+            var errorBytes = BitConverter.GetBytes(error);
+            var heartBearData = new byte[] { 0x01, 0xF0, (byte)state, errorBytes[0], errorBytes[1] };
+            SetData(heartBearData);
+        }
+    }
+
+    public class UpperIOPack : BasePack
+    {
+        public UpperIOPack(byte[] data,int size)
+        {
+            byte[] UpperData = new byte[6 + size];
+            UpperData[0] = 0x01;
+            UpperData[1] = 0x20;
+            var length = BitConverter.GetBytes(size);
+            for (int i = 0; i < 4; i++)
+            {
+                UpperData[2+i] = length[i];
+            }
+            Array.Copy(data,0,UpperData,6,size);
+            SetData(UpperData);
+        }
+    }
+}

MedullaAdapter/Remote.cs

@@ -0,0 +1,47 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+using CartActivator;
+using MDCSToolBox.Medulla.Chassis.MultiWheel;
+
+namespace MedullaAdapter
+{
+    public class Remote:MultiWheelRemote<DiverCartDefinition>
+    {
+        [AsControlItem(name = "夹抱速度", LayoutRow = 0, LayoutCol = 4)]
+        public Throttle ArmSpeed;
+
+        [AsControlItem(name = "夹抱打开", LayoutRow = 1, LayoutCol = 0)]
+        public Button Open;
+
+        [AsControlItem(name = "夹抱关闭", LayoutRow = 1, LayoutCol = 2)]
+        public Button Close;
+
+        public override void MultiVehicleModeChassisLogic()
+        {
+            // cart.MultiVehicleRemoteManualEnabled = true;
+            // cart.MultiVehicleManualEnabled = true;
+            // cart.MultiVehicleManualVx = SpeedPad.y * SpeedThreshold.val;
+            // cart.MultiVehicleManualVth = -(float)Math.Pow(Math.Abs(SpeedPad.x), cart.ManualThetaPow) * Math.Sign(SpeedPad.x);
+        }
+
+        public override void CustomOperation()
+        {
+            if (Open.pressed)
+            {
+                cart.SpeedLeftArm = -ArmSpeed.val * cart.ManualArmSpeedFac;
+                cart.SpeedRightArm = -ArmSpeed.val * cart.ManualArmSpeedFac;
+            }
+            else if (Close.pressed)
+            {
+                cart.SpeedLeftArm = ArmSpeed.val * cart.ManualArmSpeedFac;
+                cart.SpeedRightArm = ArmSpeed.val * cart.ManualArmSpeedFac;
+            }
+            else
+            {
+                cart.SpeedLeftArm = 0;
+                cart.SpeedRightArm = 0;
+            }
+        }
+    }
+}

MedullaAdapter/RunOnMCU.cs

@@ -0,0 +1,47 @@
+using System;
+using System.Collections.Generic;
+
+namespace CartActivator
+{
+    public class LogicRunOnMCUAttribute:Attribute
+    {
+        public string mcu_url = "default";
+        public int scanInterval = 50;
+    }
+      
+    public class RunOnMCU
+    {
+        // if return null: not data, or return payload data excluding CRC
+        public static byte[] ReadEvent(int port, int event_id) => default;
+
+        public static void WriteEvent(byte[] payload, int port, int event_id) { }
+
+        // if return null: not data. 
+        public static byte[] ReadStream(int port) => default; 
+
+        public static void WriteStream(byte[] payload, int port){} 
+
+        // always have the same sized data.
+        public static byte[] ReadSnapshot() => default;
+
+        public static void WriteSnapshot(byte[] payload)
+        {
+        }
+
+        public static T BytesToStruct<T>() where T: struct => default;
+
+        public static byte[] StructToBytes<T>(T what) where T : struct => default;
+
+        public static int GetMillisFromStart() => default;
+
+        public static T Iterate<T>(IEnumerable<T> ie) => default;
+    }
+
+    public class MCUManager
+    {
+        public static void Use<T>()
+        {
+
+        }
+    }
+}

MedullaAdapter/bin/Debug/netstandard2.0/MedullaAdapter.deps.json

@@ -0,0 +1,352 @@
+{
+  "runtimeTarget": {
+    "name": ".NETStandard,Version=v2.0/",
+    "signature": ""
+  },
+  "compilationOptions": {},
+  "targets": {
+    ".NETStandard,Version=v2.0": {},
+    ".NETStandard,Version=v2.0/": {
+      "MedullaAdapter/1.0.0": {
+        "dependencies": {
+          "Fody": "6.9.3",
+          "NETStandard.Library": "2.0.3",
+          "Newtonsoft.Json": "13.0.4",
+          "System.IO.Ports": "6.0.0",
+          "CommonUsage": "1.0.0.0",
+          "CycleGUI": "1.0.0.0",
+          "MDCSToolBox": "1.0.0.0",
+          "RefCartActivator": "1.0.0.0",
+          "RefFundamentalLib": "0.0.0.0",
+          "RefMedullaCore": "1.0.0.0"
+        },
+        "runtime": {
+          "MedullaAdapter.dll": {}
+        }
+      },
+      "Fody/6.9.3": {},
+      "Microsoft.NETCore.Platforms/1.1.0": {},
+      "Microsoft.Win32.Registry/5.0.0": {
+        "dependencies": {
+          "System.Buffers": "4.5.1",
+          "System.Memory": "4.5.4",
+          "System.Security.AccessControl": "5.0.0",
+          "System.Security.Principal.Windows": "5.0.0"
+        },
+        "runtime": {
+          "lib/netstandard2.0/Microsoft.Win32.Registry.dll": {
+            "assemblyVersion": "5.0.0.0",
+            "fileVersion": "5.0.20.51904"
+          }
+        }
+      },
+      "NETStandard.Library/2.0.3": {
+        "dependencies": {
+          "Microsoft.NETCore.Platforms": "1.1.0"
+        }
+      },
+      "Newtonsoft.Json/13.0.4": {
+        "runtime": {
+          "lib/netstandard2.0/Newtonsoft.Json.dll": {
+            "assemblyVersion": "13.0.0.0",
+            "fileVersion": "13.0.4.30916"
+          }
+        }
+      },
+      "runtime.linux-arm.runtime.native.System.IO.Ports/6.0.0": {},
+      "runtime.linux-arm64.runtime.native.System.IO.Ports/6.0.0": {},
+      "runtime.linux-x64.runtime.native.System.IO.Ports/6.0.0": {},
+      "runtime.native.System.IO.Ports/6.0.0": {
+        "dependencies": {
+          "runtime.linux-arm.runtime.native.System.IO.Ports": "6.0.0",
+          "runtime.linux-arm64.runtime.native.System.IO.Ports": "6.0.0",
+          "runtime.linux-x64.runtime.native.System.IO.Ports": "6.0.0",
+          "runtime.osx-arm64.runtime.native.System.IO.Ports": "6.0.0",
+          "runtime.osx-x64.runtime.native.System.IO.Ports": "6.0.0"
+        }
+      },
+      "runtime.osx-arm64.runtime.native.System.IO.Ports/6.0.0": {},
+      "runtime.osx-x64.runtime.native.System.IO.Ports/6.0.0": {},
+      "System.Buffers/4.5.1": {
+        "runtime": {
+          "lib/netstandard2.0/System.Buffers.dll": {
+            "assemblyVersion": "4.0.3.0",
+            "fileVersion": "4.6.28619.1"
+          }
+        }
+      },
+      "System.IO.Ports/6.0.0": {
+        "dependencies": {
+          "Microsoft.Win32.Registry": "5.0.0",
+          "System.Memory": "4.5.4",
+          "runtime.native.System.IO.Ports": "6.0.0"
+        },
+        "runtime": {
+          "lib/netstandard2.0/System.IO.Ports.dll": {
+            "assemblyVersion": "6.0.0.0",
+            "fileVersion": "6.0.21.52210"
+          }
+        }
+      },
+      "System.Memory/4.5.4": {
+        "dependencies": {
+          "System.Buffers": "4.5.1",
+          "System.Numerics.Vectors": "4.4.0",
+          "System.Runtime.CompilerServices.Unsafe": "4.5.3"
+        },
+        "runtime": {
+          "lib/netstandard2.0/System.Memory.dll": {
+            "assemblyVersion": "4.0.1.1",
+            "fileVersion": "4.6.28619.1"
+          }
+        }
+      },
+      "System.Numerics.Vectors/4.4.0": {
+        "runtime": {
+          "lib/netstandard2.0/System.Numerics.Vectors.dll": {
+            "assemblyVersion": "4.1.3.0",
+            "fileVersion": "4.6.25519.3"
+          }
+        }
+      },
+      "System.Runtime.CompilerServices.Unsafe/4.5.3": {
+        "runtime": {
+          "lib/netstandard2.0/System.Runtime.CompilerServices.Unsafe.dll": {
+            "assemblyVersion": "4.0.4.1",
+            "fileVersion": "4.6.28619.1"
+          }
+        }
+      },
+      "System.Security.AccessControl/5.0.0": {
+        "dependencies": {
+          "System.Security.Principal.Windows": "5.0.0"
+        },
+        "runtime": {
+          "lib/netstandard2.0/System.Security.AccessControl.dll": {
+            "assemblyVersion": "5.0.0.0",
+            "fileVersion": "5.0.20.51904"
+          }
+        }
+      },
+      "System.Security.Principal.Windows/5.0.0": {
+        "runtime": {
+          "lib/netstandard2.0/System.Security.Principal.Windows.dll": {
+            "assemblyVersion": "5.0.0.0",
+            "fileVersion": "5.0.20.51904"
+          }
+        }
+      },
+      "CommonUsage/1.0.0.0": {
+        "runtime": {
+          "CommonUsage.dll": {
+            "assemblyVersion": "1.0.0.0",
+            "fileVersion": "1.0.0.0"
+          }
+        }
+      },
+      "CycleGUI/1.0.0.0": {
+        "runtime": {
+          "CycleGUI.dll": {
+            "assemblyVersion": "1.0.0.0",
+            "fileVersion": "1.0.0.0"
+          }
+        }
+      },
+      "MDCSToolBox/1.0.0.0": {
+        "runtime": {
+          "MDCSToolBox.dll": {
+            "assemblyVersion": "1.0.0.0",
+            "fileVersion": "1.0.0.0"
+          }
+        }
+      },
+      "RefCartActivator/1.0.0.0": {
+        "runtime": {
+          "RefCartActivator.dll": {
+            "assemblyVersion": "1.0.0.0",
+            "fileVersion": "0.0.0.0"
+          }
+        }
+      },
+      "RefFundamentalLib/0.0.0.0": {
+        "runtime": {
+          "RefFundamentalLib.dll": {
+            "assemblyVersion": "0.0.0.0",
+            "fileVersion": "0.0.0.0"
+          }
+        }
+      },
+      "RefMedullaCore/1.0.0.0": {
+        "runtime": {
+          "RefMedullaCore.dll": {
+            "assemblyVersion": "1.0.0.0",
+            "fileVersion": "0.0.0.0"
+          }
+        }
+      }
+    }
+  },
+  "libraries": {
+    "MedullaAdapter/1.0.0": {
+      "type": "project",
+      "serviceable": false,
+      "sha512": ""
+    },
+    "Fody/6.9.3": {
+      "type": "package",
+      "serviceable": true,
+      "sha512": "sha512-1CUGgFdyECDKgi5HaUBhdv6k+VG9Iy4OCforGfHyar3xQXAJypZkzymgKtWj/4SPd6nSG0Qi7NH71qHrDSZLaA==",
+      "path": "fody/6.9.3",
+      "hashPath": "fody.6.9.3.nupkg.sha512"
+    },
+    "Microsoft.NETCore.Platforms/1.1.0": {
+      "type": "package",
+      "serviceable": true,
+      "sha512": "sha512-kz0PEW2lhqygehI/d6XsPCQzD7ff7gUJaVGPVETX611eadGsA3A877GdSlU0LRVMCTH/+P3o2iDTak+S08V2+A==",
+      "path": "microsoft.netcore.platforms/1.1.0",
+      "hashPath": "microsoft.netcore.platforms.1.1.0.nupkg.sha512"
+    },
+    "Microsoft.Win32.Registry/5.0.0": {
+      "type": "package",
+      "serviceable": true,
+      "sha512": "sha512-dDoKi0PnDz31yAyETfRntsLArTlVAVzUzCIvvEDsDsucrl33Dl8pIJG06ePTJTI3tGpeyHS9Cq7Foc/s4EeKcg==",
+      "path": "microsoft.win32.registry/5.0.0",
+      "hashPath": "microsoft.win32.registry.5.0.0.nupkg.sha512"
+    },
+    "NETStandard.Library/2.0.3": {
+      "type": "package",
+      "serviceable": true,
+      "sha512": "sha512-st47PosZSHrjECdjeIzZQbzivYBJFv6P2nv4cj2ypdI204DO+vZ7l5raGMiX4eXMJ53RfOIg+/s4DHVZ54Nu2A==",
+      "path": "netstandard.library/2.0.3",
+      "hashPath": "netstandard.library.2.0.3.nupkg.sha512"
+    },
+    "Newtonsoft.Json/13.0.4": {
+      "type": "package",
+      "serviceable": true,
+      "sha512": "sha512-pdgNNMai3zv51W5aq268sujXUyx7SNdE2bj1wZcWjAQrKMFZV260lbqYop1d2GM67JI1huLRwxo9ZqnfF/lC6A==",
+      "path": "newtonsoft.json/13.0.4",
+      "hashPath": "newtonsoft.json.13.0.4.nupkg.sha512"
+    },
+    "runtime.linux-arm.runtime.native.System.IO.Ports/6.0.0": {
+      "type": "package",
+      "serviceable": true,
+      "sha512": "sha512-75q52H7CSpgIoIDwXb9o833EvBZIXJ0mdPhz1E6jSisEXUBlSCPalC29cj3EXsjpuDwr0dj1LRXZepIQH/oL4Q==",
+      "path": "runtime.linux-arm.runtime.native.system.io.ports/6.0.0",
+      "hashPath": "runtime.linux-arm.runtime.native.system.io.ports.6.0.0.nupkg.sha512"
+    },
+    "runtime.linux-arm64.runtime.native.System.IO.Ports/6.0.0": {
+      "type": "package",
+      "serviceable": true,
+      "sha512": "sha512-xn2bMThmXr3CsvOYmS8ex2Yz1xo+kcnhVg2iVhS9PlmqjZPAkrEo/I40wjrBZH/tU4kvH0s1AE8opAvQ3KIS8g==",
+      "path": "runtime.linux-arm64.runtime.native.system.io.ports/6.0.0",
+      "hashPath": "runtime.linux-arm64.runtime.native.system.io.ports.6.0.0.nupkg.sha512"
+    },
+    "runtime.linux-x64.runtime.native.System.IO.Ports/6.0.0": {
+      "type": "package",
+      "serviceable": true,
+      "sha512": "sha512-16nbNXwv0sC+gLGIuecri0skjuh6R1maIJggsaNP7MQBcbVcEfWFUOkEnsnvoLEjy0XerfibuRptfQ8AmdIcWA==",
+      "path": "runtime.linux-x64.runtime.native.system.io.ports/6.0.0",
+      "hashPath": "runtime.linux-x64.runtime.native.system.io.ports.6.0.0.nupkg.sha512"
+    },
+    "runtime.native.System.IO.Ports/6.0.0": {
+      "type": "package",
+      "serviceable": true,
+      "sha512": "sha512-KaaXlpOcuZjMdmyF5wzzx3b+PRKIzt6A5Ax9dKenPDQbVJAFpev+casD0BIig1pBcbs3zx7CqWemzUJKAeHdSQ==",
+      "path": "runtime.native.system.io.ports/6.0.0",
+      "hashPath": "runtime.native.system.io.ports.6.0.0.nupkg.sha512"
+    },
+    "runtime.osx-arm64.runtime.native.System.IO.Ports/6.0.0": {
+      "type": "package",
+      "serviceable": true,
+      "sha512": "sha512-fXG12NodG1QrCdoaeSQ1gVnk/koi4WYY4jZtarMkZeQMyReBm1nZlSRoPnUjLr2ZR36TiMjpcGnQfxymieUe7w==",
+      "path": "runtime.osx-arm64.runtime.native.system.io.ports/6.0.0",
+      "hashPath": "runtime.osx-arm64.runtime.native.system.io.ports.6.0.0.nupkg.sha512"
+    },
+    "runtime.osx-x64.runtime.native.System.IO.Ports/6.0.0": {
+      "type": "package",
+      "serviceable": true,
+      "sha512": "sha512-/As+zPY49+dSUXkh+fTUbyPhqrdGN//evLxo4Vue88pfh1BHZgF7q4kMblTkxYvwR6Vi03zSYxysSFktO8/SDQ==",
+      "path": "runtime.osx-x64.runtime.native.system.io.ports/6.0.0",
+      "hashPath": "runtime.osx-x64.runtime.native.system.io.ports.6.0.0.nupkg.sha512"
+    },
+    "System.Buffers/4.5.1": {
+      "type": "package",
+      "serviceable": true,
+      "sha512": "sha512-Rw7ijyl1qqRS0YQD/WycNst8hUUMgrMH4FCn1nNm27M4VxchZ1js3fVjQaANHO5f3sN4isvP4a+Met9Y4YomAg==",
+      "path": "system.buffers/4.5.1",
+      "hashPath": "system.buffers.4.5.1.nupkg.sha512"
+    },
+    "System.IO.Ports/6.0.0": {
+      "type": "package",
+      "serviceable": true,
+      "sha512": "sha512-dRyGI7fUESar5ZLIpiBOaaNLW7YyOBGftjj5Of+xcduC/Rjl7RjhEnWDvvNBmHuF3d0tdXoqdVI/yrVA8f00XA==",
+      "path": "system.io.ports/6.0.0",
+      "hashPath": "system.io.ports.6.0.0.nupkg.sha512"
+    },
+    "System.Memory/4.5.4": {
+      "type": "package",
+      "serviceable": true,
+      "sha512": "sha512-1MbJTHS1lZ4bS4FmsJjnuGJOu88ZzTT2rLvrhW7Ygic+pC0NWA+3hgAen0HRdsocuQXCkUTdFn9yHJJhsijDXw==",
+      "path": "system.memory/4.5.4",
+      "hashPath": "system.memory.4.5.4.nupkg.sha512"
+    },
+    "System.Numerics.Vectors/4.4.0": {
+      "type": "package",
+      "serviceable": true,
+      "sha512": "sha512-UiLzLW+Lw6HLed1Hcg+8jSRttrbuXv7DANVj0DkL9g6EnnzbL75EB7EWsw5uRbhxd/4YdG8li5XizGWepmG3PQ==",
+      "path": "system.numerics.vectors/4.4.0",
+      "hashPath": "system.numerics.vectors.4.4.0.nupkg.sha512"
+    },
+    "System.Runtime.CompilerServices.Unsafe/4.5.3": {
+      "type": "package",
+      "serviceable": true,
+      "sha512": "sha512-3TIsJhD1EiiT0w2CcDMN/iSSwnNnsrnbzeVHSKkaEgV85txMprmuO+Yq2AdSbeVGcg28pdNDTPK87tJhX7VFHw==",
+      "path": "system.runtime.compilerservices.unsafe/4.5.3",
+      "hashPath": "system.runtime.compilerservices.unsafe.4.5.3.nupkg.sha512"
+    },
+    "System.Security.AccessControl/5.0.0": {
+      "type": "package",
+      "serviceable": true,
+      "sha512": "sha512-dagJ1mHZO3Ani8GH0PHpPEe/oYO+rVdbQjvjJkBRNQkX4t0r1iaeGn8+/ybkSLEan3/slM0t59SVdHzuHf2jmw==",
+      "path": "system.security.accesscontrol/5.0.0",
+      "hashPath": "system.security.accesscontrol.5.0.0.nupkg.sha512"
+    },
+    "System.Security.Principal.Windows/5.0.0": {
+      "type": "package",
+      "serviceable": true,
+      "sha512": "sha512-t0MGLukB5WAVU9bO3MGzvlGnyJPgUlcwerXn1kzBRjwLKixT96XV0Uza41W49gVd8zEMFu9vQEFlv0IOrytICA==",
+      "path": "system.security.principal.windows/5.0.0",
+      "hashPath": "system.security.principal.windows.5.0.0.nupkg.sha512"
+    },
+    "CommonUsage/1.0.0.0": {
+      "type": "reference",
+      "serviceable": false,
+      "sha512": ""
+    },
+    "CycleGUI/1.0.0.0": {
+      "type": "reference",
+      "serviceable": false,
+      "sha512": ""
+    },
+    "MDCSToolBox/1.0.0.0": {
+      "type": "reference",
+      "serviceable": false,
+      "sha512": ""
+    },
+    "RefCartActivator/1.0.0.0": {
+      "type": "reference",
+      "serviceable": false,
+      "sha512": ""
+    },
+    "RefFundamentalLib/0.0.0.0": {
+      "type": "reference",
+      "serviceable": false,
+      "sha512": ""
+    },
+    "RefMedullaCore/1.0.0.0": {
+      "type": "reference",
+      "serviceable": false,
+      "sha512": ""
+    }
+  }
+}

MedullaAdapter/build/Clumsy/ClumsyLite.runtimeconfig.json

@@ -0,0 +1,13 @@
+{
+  "runtimeOptions": {
+    "tfm": "net8.0",
+    "framework": {
+      "name": "Microsoft.NETCore.App",
+      "version": "8.0.0"
+    },
+    "configProperties": {
+      "System.Reflection.Metadata.MetadataUpdater.IsSupported": false,
+      "System.Runtime.Serialization.EnableUnsafeBinaryFormatterSerialization": false
+    }
+  }
+}

MedullaAdapter/build/Clumsy_AGV2/ClumsyLite.runtimeconfig.json

@@ -0,0 +1,13 @@
+{
+  "runtimeOptions": {
+    "tfm": "net8.0",
+    "framework": {
+      "name": "Microsoft.NETCore.App",
+      "version": "8.0.0"
+    },
+    "configProperties": {
+      "System.Reflection.Metadata.MetadataUpdater.IsSupported": false,
+      "System.Runtime.Serialization.EnableUnsafeBinaryFormatterSerialization": false
+    }
+  }
+}

MedullaAdapter/native/code.c

@@ -0,0 +1,79 @@
+#define i1 char
+#define u1 unsigned char
+#define i2 short
+#define u2 unsigned short
+#define i4 int
+#define u4 unsigned int
+#define r4 float
+
+// function begins
+
+r4 cfun0(r4 arg0 ){
+//stack_vars:
+r4 stack_0_r4;;
+r4 stack_1_r4;;
+ //local_vars:
+r4 var0;
+ L_0001: stack_0_r4=(arg0); //IL_0001: ldarg.0: s_0, pop0, push1
+L_0002: stack_1_r4=(10.5f); //IL_0002: ldc.r4 10.5: s_1, pop0, push1
+L_0007: stack_0_r4=((stack_0_r4)/(stack_1_r4)); //IL_0007: div: s_2, pop2, push1
+L_0008: stack_1_r4=(60.0f); //IL_0008: ldc.r4 60: s_1, pop0, push1
+L_000d: stack_0_r4=((stack_0_r4)/(stack_1_r4)); //IL_000d: div: s_2, pop2, push1
+L_000e: stack_0_r4=(stack_0_r4); //IL_000e: conv.r8: s_1, pop1, push1
+L_000f: stack_1_r4=(3.1f); //IL_000f: ldc.r8 3.14159265358979: s_1, pop0, push1
+L_0018: stack_0_r4=((stack_0_r4)*(stack_1_r4)); //IL_0018: mul: s_2, pop2, push1
+L_0019: stack_1_r4=(85.0f); //IL_0019: ldc.r8 85: s_1, pop0, push1
+L_0022: stack_0_r4=((stack_0_r4)*(stack_1_r4)); //IL_0022: mul: s_2, pop2, push1
+L_0023: stack_1_r4=(1000.0f); //IL_0023: ldc.r8 1000: s_1, pop0, push1
+L_002c: stack_0_r4=((stack_0_r4)/(stack_1_r4)); //IL_002c: div: s_2, pop2, push1
+L_002d: stack_0_r4=(stack_0_r4); //IL_002d: conv.r4: s_1, pop1, push1
+L_002e: var0=stack_0_r4; //IL_002e: stloc.0: s_1, pop1, push0
+L_002f: goto L_0031; //IL_002f: br.s IL_0031: s_0, pop0, push0
+L_0031: stack_0_r4=(var0); //IL_0031: ldloc.0: s_0, pop0, push1
+L_0032: return stack_0_r4; //IL_0032: ret: s_1, pop1, push0
+}
+
+r4 cfun1(void* arg0, r4 arg1 ){
+//stack_vars:
+r4 stack_0_r4;;
+r4 stack_1_r4;;
+ //local_vars:
+r4 var0;
+ L_0001: stack_0_r4=(arg1); //IL_0001: ldarg.1: s_0, pop0, push1
+L_0002: stack_1_r4=(10.5f); //IL_0002: ldc.r4 10.5: s_1, pop0, push1
+L_0007: stack_0_r4=((stack_0_r4)/(stack_1_r4)); //IL_0007: div: s_2, pop2, push1
+L_0008: stack_0_r4=(stack_0_r4); //IL_0008: conv.r8: s_1, pop1, push1
+L_0009: stack_1_r4=(3.1f); //IL_0009: ldc.r8 3.14159265358979: s_1, pop0, push1
+L_0012: stack_0_r4=((stack_0_r4)*(stack_1_r4)); //IL_0012: mul: s_2, pop2, push1
+L_0013: stack_1_r4=(85.0f); //IL_0013: ldc.r8 85: s_1, pop0, push1
+L_001c: stack_0_r4=((stack_0_r4)*(stack_1_r4)); //IL_001c: mul: s_2, pop2, push1
+L_001d: stack_0_r4=(stack_0_r4); //IL_001d: conv.r4: s_1, pop1, push1
+L_001e: var0=stack_0_r4; //IL_001e: stloc.0: s_1, pop1, push0
+L_001f: goto L_0021; //IL_001f: br.s IL_0021: s_0, pop0, push0
+L_0021: stack_0_r4=(var0); //IL_0021: ldloc.0: s_0, pop0, push1
+L_0022: return stack_0_r4; //IL_0022: ret: s_1, pop1, push0
+}
+
+r4 cfun2(r4 arg0 ){
+//stack_vars:
+r4 stack_0_r4;;
+r4 stack_1_r4;;
+ //local_vars:
+r4 var0;
+ L_0001: stack_0_r4=(arg0); //IL_0001: ldarg.0: s_0, pop0, push1
+L_0002: stack_0_r4=(stack_0_r4); //IL_0002: conv.r8: s_1, pop1, push1
+L_0003: stack_1_r4=(267.0f); //IL_0003: ldc.r8 267.035375555132: s_1, pop0, push1
+L_000c: stack_0_r4=((stack_0_r4)/(stack_1_r4)); //IL_000c: div: s_2, pop2, push1
+L_000d: stack_1_r4=(10.5f); //IL_000d: ldc.r8 10.5: s_1, pop0, push1
+L_0016: stack_0_r4=((stack_0_r4)*(stack_1_r4)); //IL_0016: mul: s_2, pop2, push1
+L_0017: stack_1_r4=(60.0f); //IL_0017: ldc.r8 60: s_1, pop0, push1
+L_0020: stack_0_r4=((stack_0_r4)*(stack_1_r4)); //IL_0020: mul: s_2, pop2, push1
+L_0021: stack_1_r4=(1000.0f); //IL_0021: ldc.r8 1000: s_1, pop0, push1
+L_002a: stack_0_r4=((stack_0_r4)*(stack_1_r4)); //IL_002a: mul: s_2, pop2, push1
+L_002b: stack_0_r4=(stack_0_r4); //IL_002b: conv.r4: s_1, pop1, push1
+L_002c: var0=stack_0_r4; //IL_002c: stloc.0: s_1, pop1, push0
+L_002d: goto L_002f; //IL_002d: br.s IL_002f: s_0, pop0, push0
+L_002f: stack_0_r4=(var0); //IL_002f: ldloc.0: s_0, pop0, push1
+L_0030: return stack_0_r4; //IL_0030: ret: s_1, pop1, push0
+}
+

MedullaAdapter/obj/Debug/netstandard2.0/.NETStandard,Version=v2.0.AssemblyAttributes.cs

@@ -0,0 +1,4 @@
+// <autogenerated />
+using System;
+using System.Reflection;
+[assembly: global::System.Runtime.Versioning.TargetFrameworkAttribute(".NETStandard,Version=v2.0", FrameworkDisplayName = ".NET Standard 2.0")]

MedullaAdapter/obj/Debug/netstandard2.0/MedullaAdapter.AssemblyInfo.cs

@@ -0,0 +1,23 @@
+//------------------------------------------------------------------------------
+// <auto-generated>
+//     此代码由工具生成。
+//     运行时版本:4.0.30319.42000
+//
+//     对此文件的更改可能会导致不正确的行为，并且如果
+//     重新生成代码，这些更改将会丢失。
+// </auto-generated>
+//------------------------------------------------------------------------------
+
+using System;
+using System.Reflection;
+
+[assembly: System.Reflection.AssemblyCompanyAttribute("MedullaAdapter")]
+[assembly: System.Reflection.AssemblyConfigurationAttribute("Debug")]
+[assembly: System.Reflection.AssemblyFileVersionAttribute("1.0.0.0")]
+[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0")]
+[assembly: System.Reflection.AssemblyProductAttribute("MedullaAdapter")]
+[assembly: System.Reflection.AssemblyTitleAttribute("MedullaAdapter")]
+[assembly: System.Reflection.AssemblyVersionAttribute("1.0.0.0")]
+
+// 由 MSBuild WriteCodeFragment 类生成。
+

MedullaAdapter/obj/Debug/netstandard2.0/MedullaAdapter.AssemblyInfoInputs.cache

@@ -0,0 +1 @@
+2eac81d674c264ce890b4225c419fa343e662bbba0825694002aaadff6e734b7

MedullaAdapter/obj/Debug/netstandard2.0/MedullaAdapter.GeneratedMSBuildEditorConfig.editorconfig

@@ -0,0 +1,8 @@
+is_global = true
+build_property.RootNamespace = MedullaAdapter
+build_property.ProjectDir = D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\MedullaAdapter\
+build_property.EnableComHosting = 
+build_property.EnableGeneratedComInterfaceComImportInterop = 
+build_property.CsWinRTUseWindowsUIXamlProjections = false
+build_property.EffectiveAnalysisLevelStyle = 
+build_property.EnableCodeStyleSeverity =