update 删除一些没有用到的ref

.codex/skills/commit/SKILL.md

@@ -1,77 +0,0 @@
----
-name: commit
-description: 自动生成中文 git commit 信息并提交推送。读取当前改动，用简洁的中文一句话概括改动内容，然后自动执行 git add、commit、push。当用户说"提交""commit""提交代码""推送"时使用。
-allowed-tools: Bash(git status:*), Bash(git diff:*), Bash(git add:*), Bash(git commit:*), Bash(git push:*), Bash(git log:*), Bash(git branch:*)
----
-
-# 自动 commit 并 push
-
-读取当前 git 改动，生成简洁的中文 commit 信息，然后自动提交并推送。
-
-## 执行步骤
-
-### 1. 查看当前状态
-
-先了解仓库当前情况：
-
-```bash
-git status
-git diff --stat        # 看改动了哪些文件、改动量
-git diff               # 看未暂存的具体改动
-git diff --staged      # 看已暂存的具体改动
-git log --oneline -5   # 看最近几次提交风格，保持一致
-```
-
-### 2. 分析改动
-
-基于 diff 内容，理解这次改动**实际做了什么**：
-
-- 新增了什么功能/文件
-- 修改/修复了什么
-- 删除/重构了什么
-- 是文档、配置还是代码改动
-
-**不要凭文件名猜测，要看实际 diff 内容。**
-
-### 3. 生成 commit 信息
-
-要求：
-
-- **中文**，简洁，**一句话**概括这次改动的核心内容
-- **不要前缀**（不用 feat/fix/docs 这种 Conventional Commits 前缀）
-- 直接描述做了什么，动词开头，如"添加 ALNS 自适应大邻域搜索算法"、"修复 POX 交叉中的索引越界问题"、"重构 FJSP 解码逻辑去掉 AGV 部分"
-- 如果一次改动包含多个不相关的事情，提示用户是否要分开提交（但默认仍按一条处理）
-- 长度控制在一行能看完，不写冗长描述
-
-### 4. 自动提交并推送
-
-确认 commit 信息后，依次执行：
-
-```bash
-git add -A                          # 暂存所有改动
-git commit -m "生成的中文commit信息"
-git push                            # 推送到当前分支的远程
-```
-
-### 5. 处理常见情况
-
-- **没有改动**：如果 `git status` 显示没有改动，告知用户无需提交，停止
-- **push 失败**：
-  - 如果是因为远程有新提交（需要先 pull），告知用户，建议先 `git pull` 或 `git pull --rebase`，**不要自动强推**
-  - 如果是没有配置远程或没有 upstream 分支，提示用户，给出 `git push -u origin <分支名>` 的建议命令
-  - 如果是认证问题，告知用户检查凭证
-- **当前在重要分支**（如 main/master）：正常执行，但在输出里提示一下当前分支名，让用户心里有数
-
-### 6. 输出
-
-完成后简要报告：
-
-- 生成的 commit 信息
-- 提交到了哪个分支
-- push 是否成功
-
-## 注意事项
-
-- commit 信息必须如实反映 diff 内容，不编造
-- push 失败时不要用 `--force` 强推，交给用户决定
-- 如果改动很大很杂，主动提示用户考虑拆分提交，但不强制

.codex/skills/readme/SKILL.md

@@ -1,92 +0,0 @@
----
-name: readme
-description: 为当前项目生成适配 Gitee / 公司内部代码仓库的中英文双语 README。默认生成 README.md（中文，Gitee 默认展示）和 README_en.md（英文）两个文件，顶部互相链接切换语言。适用于公司项目、算法项目、机器人项目、工程代码仓库。当用户说“写个README”“生成项目介绍”“生成Gitee README”“make a readme”时使用。
----
-
-# Gitee 双语 README 生成
-
-为当前项目生成两个互相链接的 README 文件：
-
-- `README.md`：简体中文，作为 Gitee 默认展示文件
-- `README_en.md`：英文版，供中英文切换使用
-
-如果项目中已经存在 `README_zh.md`、`Readme_zh.md`、`Readme_en.md` 等命名，先读取已有文件，并尽量沿用当前仓库已有命名规范；如果没有明确规范，默认使用 `README.md` + `README_en.md`。
-
-## 执行目标
-
-生成符合公司内部 Gitee 仓库风格的 README，不写成 GitHub 开源宣传页。
-
-README 应该让新同事或项目参与者快速知道：
-
-- 项目是什么
-- 面向什么设备 / 平台 / 场景
-- 软件架构大概是什么
-- 如何安装依赖
-- 如何编译 / 运行 / 启动
-- 代码目录怎么组织
-- 如何按公司流程参与开发
-
-## 执行步骤
-
-### 1. 调研项目
-
-先充分了解项目，不要凭空编造内容。
-
-必须优先读取和分析：
-
-- 项目根目录结构
-- 已有 README / 文档
-- 主入口脚本
-- 启动脚本
-- `CMakeLists.txt`
-- `package.xml`
-- `requirements.txt`
-- `pyproject.toml`
-- `package.json`
-- `docker-compose.yml`
-- `Dockerfile`
-- 配置文件
-- launch 文件
-- ROS / ROS2 相关目录
-- 核心源码目录
-- 设备通信、底盘控制、导航、感知、驱动相关代码
-
-需要识别：
-
-- 项目名称
-- 项目用途
-- 运行平台
-- 技术栈
-- 编程语言
-- ROS / ROS2 版本（如果存在）
-- 构建方式
-- 启动方式
-- 主要模块
-- 依赖项
-- 是否有实际设备、仿真环境、域控一体机、阿克曼底盘、CAN、串口、网络通信等内容
-
-**重要：只写代码和文档中真实存在的内容。**
-
-不要编造：
-
-- 未确认的算法
-- 未确认的性能指标
-- 未确认的硬件型号
-- 未确认的 ROS 版本
-- 未确认的启动命令
-- 未确认的部署流程
-- 未确认的许可证
-
-如果信息不足，用“待补充”明确标注，不要用通用模板假装完整。
-
----
-
-## 2. 文件命名与语言切换
-
-### 默认文件
-
-生成：
-
-```text
-README.md
-README_en.md

.cursor/rules/karpathy-guidelines.mdc

@@ -1,70 +0,0 @@
----
-description: Behavioral guidelines to reduce common LLM coding mistakes. Use when writing, reviewing, or refactoring code to avoid overcomplication, make surgical changes, surface assumptions, and define verifiable success criteria.
-alwaysApply: true
----
-
-# Karpathy behavioral guidelines
-
-Behavioral guidelines to reduce common LLM coding mistakes. Merge with project-specific instructions as needed.
-
-**Tradeoff:** These guidelines bias toward caution over speed. For trivial tasks, use judgment.
-
-## 1. Think Before Coding
-
-**Don't assume. Don't hide confusion. Surface tradeoffs.**
-
-Before implementing:
-- State your assumptions explicitly. If uncertain, ask.
-- If multiple interpretations exist, present them - don't pick silently.
-- If a simpler approach exists, say so. Push back when warranted.
-- If something is unclear, stop. Name what's confusing. Ask.
-
-## 2. Simplicity First
-
-**Minimum code that solves the problem. Nothing speculative.**
-
-- No features beyond what was asked.
-- No abstractions for single-use code.
-- No "flexibility" or "configurability" that wasn't requested.
-- No error handling for impossible scenarios.
-- If you write 200 lines and it could be 50, rewrite it.
-
-Ask yourself: "Would a senior engineer say this is overcomplicated?" If yes, simplify.
-
-## 3. Surgical Changes
-
-**Touch only what you must. Clean up only your own mess.**
-
-When editing existing code:
-- Don't "improve" adjacent code, comments, or formatting.
-- Don't refactor things that aren't broken.
-- Match existing style, even if you'd do it differently.
-- If you notice unrelated dead code, mention it - don't delete it.
-
-When your changes create orphans:
-- Remove imports/variables/functions that YOUR changes made unused.
-- Don't remove pre-existing dead code unless asked.
-
-The test: Every changed line should trace directly to the user's request.
-
-## 4. Goal-Driven Execution
-
-**Define success criteria. Loop until verified.**
-
-Transform tasks into verifiable goals:
-- "Add validation" → "Write tests for invalid inputs, then make them pass"
-- "Fix the bug" → "Write a test that reproduces it, then make it pass"
-- "Refactor X" → "Ensure tests pass before and after"
-
-For multi-step tasks, state a brief plan:
-```
-1. [Step] → verify: [check]
-2. [Step] → verify: [check]
-3. [Step] → verify: [check]
-```
-
-Strong success criteria let you loop independently. Weak criteria ("make it work") require constant clarification.
-
----
-
-**These guidelines are working if:** fewer unnecessary changes in diffs, fewer rewrites due to overcomplication, and clarifying questions come before implementation rather than after mistakes.

.gitattributes

@@ -0,0 +1,2 @@
+*.cs text eol=crlf
+.gitattributes text eol=lf

.gitignore

@@ -1,90 +1,56 @@
-##################################################
-# Visual Studio
-##################################################
-
-# Visual Studio 工作区缓存
+# Build results
+[Bb]in/
+[Oo]bj/
+build/
+artifacts/
+**/bin/
+**/obj/
+# Visual Studio / Rider / VS Code
 .vs/
-**/.vs/
-
-# 用户配置
+.idea/
+.vscode/
 *.user
 *.suo
+*.rsuser
 *.userosscache
 *.sln.docstates
 
-##################################################
-# Build 输出
-##################################################
+# .NET generated files
+project.assets.json
+project.nuget.cache
+*.nuget.g.props
+*.nuget.g.targets
+*.AssemblyInfo.cs
+*.GeneratedMSBuildEditorConfig.editorconfig
+*.GlobalUsings.g.cs
+*.FileListAbsolute.txt
+*.CoreCompileInputs.cache
+*.AssemblyReference.cache
+*.assets.cache
 
-# 编译输出目录
-bin/
-obj/
-**/bin/
-**/obj/
-
-##################################################
-# Rider / VS Code
-##################################################
-
-.idea/
-.vscode/
-
-##################################################
-# NuGet
-##################################################
-
-*.nupkg
-packages/
-
-##################################################
-# 日志
-##################################################
+# Test and coverage output
+TestResults/
+coverage/
+*.coverage
+*.coveragexml
+coverage*.json
 
+# Logs, diagnostics, and temporary files
 *.log
-
-##################################################
-# 临时文件
-##################################################
-
+*.tlog
+*.binlog
+*.pdb
+*.cache
 *.tmp
 *.temp
+*.swp
+*.bak
 
-##################################################
-# 测试结果
-##################################################
-
-TestResults/
-
-##################################################
-# 发布目录
-##################################################
-
-publish/
-
-##################################################
-# Windows
-##################################################
+# Local runtime configuration
+cartparams.json
+appsettings.Development.json
+*.local.json
 
+# OS files
 Thumbs.db
-Desktop.ini
-
-##################################################
-# JetBrains
-##################################################
-
-_ReSharper*/
-*.DotSettings.user
-
-##################################################
-# 缓存
-##################################################
-
-*.cache
-
-##################################################
-# 数据库（如果有）
-##################################################
-
-*.db
-*.sqlite
-*.sqlite3
+.DS_Store

ClumsyPilot/AGV.cs

@@ -1,21 +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()
-                => new ChassisController().Get();
-        public override MultiWheelMagTracker GetMagController()
-            => new MultiWheelMagTracker();
-        public override NaiveMagnetController GetNaiveMagnetController()
-            => new NaiveMagnetController();
-
-        public void Sleep(float seconds)
         {
-            new DriveTask(new Sleep { Second = seconds }.Get()).Wait();
+            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

@@ -1,5 +1,9 @@
+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;
@@ -10,7 +14,8 @@ public class ChassisController : MovementDefinition<MultiWheelGeometricControlle
 {
     public float BaseSpeed = Configuration.conf.basicSpeed;
 
-    // 创建单车几何跟踪控制器（直接控本车底盘，不走多车 Auto 通道）
+    private DateTime _sendMotionDbgLast = DateTime.MinValue;
+
     public override MultiWheelGeometricController Get()
     {
         return new MultiWheelGeometricController
@@ -40,6 +45,56 @@ public class ChassisController : MovementDefinition<MultiWheelGeometricControlle
             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

@@ -2,24 +2,15 @@
 
   <PropertyGroup>
     <TargetFramework>netstandard2.0</TargetFramework>
-    <LangVersion>10</LangVersion>
-    <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
-    <AssemblyName>ClumsyPilot</AssemblyName>
-    <RootNamespace>MultiWheelC</RootNamespace>
-    <AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
-    <OutputPath>build\Clumsy\</OutputPath>
+	<LangVersion>10</LangVersion>
+	<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
   </PropertyGroup>
 
   <ItemGroup>
-    <PackageReference Include="Newtonsoft.Json" Version="13.0.3" />
+    <PackageReference Include="Newtonsoft.Json" Version="13.0.4" />
     <PackageReference Include="System.Numerics.Vectors" Version="4.6.1" />
   </ItemGroup>
 
-  <ItemGroup>
-  <Compile Include="..\Shared\*.cs"
-           Link="Shared\%(Filename)%(Extension)" />
-</ItemGroup>
-
   <ItemGroup>
     <Reference Include="CommonUsage">
       <HintPath>ref\CommonUsage.dll</HintPath>
@@ -40,5 +31,5 @@
       <HintPath>ref\RefFundamentalLib.dll</HintPath>
     </Reference>
   </ItemGroup>
-
+	
 </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

@@ -1,126 +1,608 @@
+using System;
+using System.Collections.Generic;
+using System.Numerics;
 using ClumsyCore;
-using ClumsyCore.DTools;
 using ClumsyCore.Interfaces;
 using ClumsyCore.Pilot;
-using MDCSToolBox.Commons.Controllers;
-using System;
-using System.Numerics;
+using FundamentalLib;
+using CommonUsage.Chassis;
+using CommonUsage.Mathematics;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+using MDCSToolBox.Clumsy.Tracks;
 
-namespace MultiWheelC
+namespace MultiWheelC;
+
+public class MultiForwardTest : MovementDefinition
 {
-    public abstract class DstTrackerTestBase : MovementTest
+    public float Speed = 0.2f;
+    public float DurationSeconds = 2f;
+
+    public override IEnumerable<bool> Get()
     {
-        public bool UseInteractivePick = true;
-        public float srcX;
-        public float srcY;
-        public float dstX;
-        public float dstY;
-        public float carDirectionBias;
-
-        private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
-        private DriveTask _dt;
-
-        protected DstTrackerTestBase(float defaultCarDirectionBias)
+        var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
+        chassis.SetOriginBias(0, 0, 0);
+        var end = DateTime.Now.AddSeconds(DurationSeconds);
+        while (DateTime.Now < end)
         {
-            carDirectionBias = defaultCarDirectionBias;
+            chassis.SendMotion(Speed, 0, 0);
+            yield return true;
         }
 
-        public override void TestStop()
+        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)
         {
-            _dt?.Stop();
-            _painter?.Clear();
+            chassis.SendRotateMotion(0);
+            if (WheelsAligned(chassis, WheelAlignDeg)) break;
+            yield return true;
         }
 
-        public override void Test()
+        // 阶段二：旋转到目标世界朝向，到位即停。
+        var target = CommonMath.RoundTh(TargetWorldDeg);
+        while (true)
         {
-            Vector2 p1;
-            Vector2 p2;
-            if (UseInteractivePick)
+            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))
             {
-                p1 = UI.GetPoint("point1");
-                p2 = UI.GetPoint("point2");
+                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
             {
-                p1 = new Vector2(srcX, srcY);
-                p2 = new Vector2(dstX, dstY);
+                // 无定位：按时长估算，无法测角，目标维持巡航速度到估算时长（仅缓启动整形）。
+                desiredMag = maxOmega;
+                if (elapsed >= estDuration) { stopReason = "estDuration"; break; }
             }
 
-            _painter.Clear();
-            _dt = new DriveTask(new DstTracker
+            // 缓启动：只对“加速(目标>当前)”按角加速度限斜率，让起步平滑爬升；
+            // “减速(目标<当前)”跟随上面的减速曲线立即下调，保证及时刹车不超调。
+            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)
             {
-                Src = p1,
-                Dst = p2,
-                CarDirectionBias = carDirectionBias,
-            }.Get());
-            _dt.Wait();
+                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;
         }
-    }
 
-    [MovementTest(name = "测试终点跟踪动作-前进")]
-    public sealed class DstTrackerForward : DstTrackerTestBase
-    {
-        public DstTrackerForward() : base(0f) { }
-    }
+        // 到位：角速度先归零，保持脚本使能让 TickMultiVehicle 的 PI 把队形稳住一小段时间再撤离。
+        self.MultiVehicleScriptVth = 0;
+        var settleEnd = DateTime.Now.AddSeconds(Math.Max(0, SettleSec));
+        while (DateTime.Now < settleEnd)
+            yield return true;
 
-    [MovementTest(name = "测试终点跟踪动作-后退")]
-    public sealed class DstTrackerBackward : DstTrackerTestBase
-    {
-        public DstTrackerBackward() : base(180f) { }
-    }
-
-    [MovementTest(name = "底盘旋转测试")]
-    public class RotateToAngleTest : MovementTest
-    {
-        private DriveTask _dt;
-
-        // 停止当前正在执行的底盘原地旋转任务。
-        public override void TestStop()
+        ClearScript();
+        DLog.Log(
+            $"DONE reason={stopReason} 累计转角={accumulated:0.0}° 目标={TargetDeltaDeg:0.0}° " +
+            $"用时={(DateTime.Now - start).TotalSeconds:0.00}s useDetourHeading={hasPos}",
+            "FleetRotateDbg");
+        if (centerTracking)
         {
-            _dt?.Stop();
-        }
-
-        // 交互输入目标角度后执行底盘原地旋转测试。
-        public override void Test()
-        {
-            var input = UI.GetInput("输入旋转角度：");
-            if (!float.TryParse(input, out var angleTarget))
-            {
-                Console.WriteLine(
-                    $"旋转测试输入无效：{input}");
-                return;
-            }
-            // 防止重复启动测试时，上一项旋转任务仍在运行。
-            _dt?.Stop();
-            var task = new DriveTask(
-                new MultiWheelRotateInPlace
-                {
-                    AngleTarget = angleTarget,
-
-                    PidparamsRead = () => new PIDParams
-                    {
-                        Kp = PilotDefinition.Conf.InPlaceRotateKp,
-                        Ki = PilotDefinition.Conf.InPlaceRotateKi,
-                        Kd = PilotDefinition.Conf.InPlaceRotateKd,
-                        DeadZone = PilotDefinition.Conf.InPlaceRotateArriveDeg,
-                        SpeedAccPerSec = PilotDefinition.Conf.InPlaceRotateAcc,
-                        OutputUpperThreshold = PilotDefinition.Conf.InPlaceRotateMaxSpeed,
-                        MaxI = PilotDefinition.Conf.InPlaceRotateMaxI,
-                    }
-                }.Get());
-            _dt = task;
-            try
-            {
-                task.Wait();
-            }
-            finally
-            {
-                // 防止旧任务结束时，错误清除后来启动的新任务。
-                if (ReferenceEquals(_dt, task))
-                {
-                    _dt = null;
-                }
-            }
+            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

@@ -1,77 +1,34 @@
-using ClumsyCore;
+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 DstTracker : MovementDefinition
-    {
-        public Vector2 Src;
-        public Vector2 Dst;
-        public float CarDirectionBias = 0f;
-        public Painter Painter = UI.GetPainter("DstTracker");
-        public override IEnumerable<bool> Get()
-        {
-            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
-            DriveTask task = null;
-            try
-            {
-                Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
-                Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
-
-                var tracker = new ChassisController().Get();
-                var linePath = new LineTrack(Src, Dst)
-                {
-                    CarDirectionBias = CarDirectionBias,
-                    Speed = PilotDefinition.Conf.DstTrackerMaxSpeed
-                };
-                tracker.AddTrack(linePath);
-                task = new DriveTask(tracker.Track());
-                task.Wait();
-                yield return false;
-            }
-            finally
-            {
-                task?.Stop();
-                chassis.SendXYThSpeed(0f, 0f, 0f);
-            }
-        }
-
-    }
-
-    public class Sleep : MovementDefinition
-    {
-        public float Second = 2f;
-
-        public override IEnumerable<bool> Get()
-        {
-            if (Second <= 0)
-            {
-                yield return false;
-                yield break;
-            }
-
-            var endTime = DateTime.UtcNow.AddSeconds(Second);
-            while (DateTime.UtcNow < endTime)
-            {
-                Thread.Sleep(50);
-                yield return true;
-            }
-
-            yield return false;
-        }
-    }
-
     public class MultiWheelRotateInPlace : MovementDefinition
     {
         /// <summary>
@@ -89,40 +46,281 @@ namespace MultiWheelC
 
         public PIDController thPid;
 
-        // 将角度归一化到零到三百六十度范围内。
         private static float RangeAngle(float theta)
         {
             return (float)(theta - Math.Round(theta / 360.0f) * 360);
         }
 
-        // 使用 PID 控制原地旋转到目标角度。
         public override IEnumerable<bool> Get()
         {
-            try
-            {
-                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);
+            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);
 
-                while (true)
-                {
-                    var s = thPid.GetResponse(targetAngle, true);
-                    Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
-                    Chassis.SendXYThSpeed(0, 0, s);
-                    if (thPid.IsArrived()) break;
-                    yield return true;
-                }
-
-                Console.WriteLine($"final rotate to {targetAngle}");
-            }
-            finally
+            DateTime lastTime = DateTime.Now;
+            while (true)
             {
-                Chassis.SendXYThSpeed(0, 0, 0);
+                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

@@ -6,19 +6,108 @@ namespace MultiWheelC;
 
 public class PilotConfig : MultiWheelPilotConfig
 {
-    #region 单车-轨迹跟踪
+    [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 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 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 = "终点跟踪:速度")] public float DstTrackerMaxSpeed = 0.3f;
-    #endregion
+    [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;
 
-    #region 单车-原地旋转
     [FieldMember(desc = "原地旋转：目标朝向(世界坐标系, deg)")]
     public float InPlaceRotateTargetWorldDeg = 90f;
 
@@ -34,210 +123,8 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "原地旋转：旋转过程中舵轮偏差重对齐阈值(deg)")]
     public float InPlaceRotateActiveWheelAlignDeg = 10f;
 
-    #endregion
-
-    #region 单车-临时
-    [FieldMember(desc = "原地旋转Kp")]
-    public float InPlaceRotateKp = 0.05f;
-
-    [FieldMember(desc = "原地旋转Ki")]
-    public float InPlaceRotateKi = 0.01f;
-
-    [FieldMember(desc = "原地旋转Kd")]
-    public float InPlaceRotateKd = 0f;
-
-    [FieldMember(desc = "原地旋转积分限幅")]
-    public float InPlaceRotateMaxI = 0.01f;
-
-    [FieldMember(desc = "原地旋转最大角速度(deg/s)")]
-    public float InPlaceRotateMaxSpeed = 30f;
-
-    [FieldMember(desc = "原地旋转角加速度(deg/s²)")]
-    public float InPlaceRotateAcc = 30f;
-
-    [FieldMember(desc = "原地旋转超时(s)")]
-    public float InPlaceRotateTimeoutSec = 15f;
-    #endregion
-
-
-
-
-    #region 单车-钻车与夹抱
-    [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;
-
-    [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 = "轮胎跟踪:切换至盲走距离")] 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 TireFollowingReleaseDistance = 1600;
-
-    [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;
-
-    [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;
-    #endregion
-#if false
-    #region 多车-编队与遥控
-    [FieldMember(desc = "联动时转向角爬升加速度")] public float SyncThAccPerSec = 30f;
-    [FieldMember(desc = "两车间距 (mm)")] public float TestCarSyncDistance = 2400f;
-    [FieldMember(desc = "编队排布偏角")] public float TestCarSyncTh = 0f;
-    // Fleet manual remote IO values are normalized joystick ratios. Keep all speed/angle scaling here.
-    [FieldMember(desc = "车队遥控最大线速度")] public float FleetManualMaxSpeed = 0.3f;
-    [FieldMember(desc = "常规模式满杆舵角")] public float FleetManualMaxSteerAngleDeg = 45f;
-    [FieldMember(desc = "蟹行满杆舵角")] public float FleetManualMaxCrabAngleDeg = 60f;
-    [FieldMember(desc = "旋转满杆角速度")] public float FleetManualMaxRotateOmegaDegPerSec = 45f;
-    [FieldMember(desc = "蟹行舵角上限（对齐 ±120）")] public float MultiVehicleCrabSteerLimitDeg = 120f;
-    [FieldMember(desc = "互识别检测中心偏移")] public float DeltaDetectCenter = 350f;
-    #endregion
-
-    #region 多车-通信
-    [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 = "";
-    [FieldMember(desc = "多车联动：自动速度命令超时(ms，0=auto)")] public int MultiVehicleAutoCmdTimeoutMs = 0;
-    [FieldMember(desc = "多车联动：成员存活TTL(ms，0=auto)")] public int MultiVehicleMemberTtlMs = 0;
-
-    [JsonProperty("MultiVehicleMasterIp")]
-    private string LegacyMasterIpSetter
-    {
-        set
-        {
-            if (string.IsNullOrEmpty(value) || value == "/") return;
-            if (MultiVehicleMasterEndpoint == "/")
-                MultiVehicleMasterEndpoint = value.Contains(":") ? value : $"{value}:8008";
-        }
-    }
-
-    #endregion
-
-    #region 多车-队形补偿
-    [FieldMember(desc = "定位是否参与车队内姿态纠正(不影响整队姿态计算)")] public bool MultiVehicleSyncUseDetour = false;
-    [FieldMember(desc = "手动联动是否启用定位姿态纠正(默认关闭)")] public bool MultiVehicleManualUseDetourCorrection = false;
-    [FieldMember(desc = "多车联动：启用互识别纠正")] public bool MultiVehicleUseDetect = false;
-    [FieldMember(desc = "多车联动：自动模式按理想中心前馈(弧线)")] public bool MultiVehicleAutoUseIdealCenter = true;
-    [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;
-    #endregion
-
-    #region 多车-旋转补偿
-    [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;
-    [FieldMember(desc = "原地旋转纠偏：生效的最小角速度阈值(deg/s)")] public float MultiVehicleRotateActiveOmega = 0.5f;
-    [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;
-    #endregion
-
-    #region 多车-联动动作
+    // ===== 车队联动-原地旋转动作（FleetRotateInPlace / 对应 FleetRemote 原地旋转模式）=====
+    // 通过 Clumsy 内部脚本字段驱动 TickMultiVehicle 的 mode2 旋转（绕车队中心 + PI 纠偏），需主车运行。
     [FieldMember(desc = "车队原地旋转：角速度大小(deg/s，方向由目标角符号决定)")]
     public float FleetRotateOmega = 15f;
 
@@ -259,9 +146,14 @@ public class PilotConfig : MultiWheelPilotConfig
     [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;
 
@@ -307,6 +199,7 @@ public class PilotConfig : MultiWheelPilotConfig
     [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;
 
@@ -324,10 +217,118 @@ public class PilotConfig : MultiWheelPilotConfig
 
     [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
 
-#endif
-
-
-
+    [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/build/Clumsy/ClumsyPilot.deps.json

@@ -1,163 +0,0 @@
-{
-  "runtimeTarget": {
-    "name": ".NETStandard,Version=v2.0/",
-    "signature": ""
-  },
-  "compilationOptions": {},
-  "targets": {
-    ".NETStandard,Version=v2.0": {},
-    ".NETStandard,Version=v2.0/": {
-      "ClumsyPilot/1.0.0": {
-        "dependencies": {
-          "NETStandard.Library": "2.0.3",
-          "Newtonsoft.Json": "13.0.3",
-          "System.Numerics.Vectors": "4.6.1",
-          "CommonUsage": "1.0.0.0",
-          "LessokajiWeaverUtilities": "1.0.0.0",
-          "MDCSToolBox": "1.0.0.0",
-          "RefClumsyCore": "0.0.0.0",
-          "RefClumsyDance": "0.0.0.0",
-          "RefFundamentalLib": "0.0.0.0"
-        },
-        "runtime": {
-          "ClumsyPilot.dll": {}
-        }
-      },
-      "Microsoft.NETCore.Platforms/1.1.0": {},
-      "NETStandard.Library/2.0.3": {
-        "dependencies": {
-          "Microsoft.NETCore.Platforms": "1.1.0"
-        }
-      },
-      "Newtonsoft.Json/13.0.3": {
-        "runtime": {
-          "lib/netstandard2.0/Newtonsoft.Json.dll": {
-            "assemblyVersion": "13.0.0.0",
-            "fileVersion": "13.0.3.27908"
-          }
-        }
-      },
-      "System.Numerics.Vectors/4.6.1": {
-        "runtime": {
-          "lib/netstandard2.0/System.Numerics.Vectors.dll": {
-            "assemblyVersion": "4.1.3.0",
-            "fileVersion": "4.600.125.16908"
-          }
-        }
-      },
-      "CommonUsage/1.0.0.0": {
-        "runtime": {
-          "CommonUsage.dll": {
-            "assemblyVersion": "1.0.0.0",
-            "fileVersion": "1.0.0.0"
-          }
-        }
-      },
-      "LessokajiWeaverUtilities/1.0.0.0": {
-        "runtime": {
-          "LessokajiWeaverUtilities.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"
-          }
-        }
-      },
-      "RefClumsyCore/0.0.0.0": {
-        "runtime": {
-          "RefClumsyCore.dll": {
-            "assemblyVersion": "0.0.0.0",
-            "fileVersion": "0.0.0.0"
-          }
-        }
-      },
-      "RefClumsyDance/0.0.0.0": {
-        "runtime": {
-          "RefClumsyDance.dll": {
-            "assemblyVersion": "0.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"
-          }
-        }
-      }
-    }
-  },
-  "libraries": {
-    "ClumsyPilot/1.0.0": {
-      "type": "project",
-      "serviceable": false,
-      "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"
-    },
-    "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.3": {
-      "type": "package",
-      "serviceable": true,
-      "sha512": "sha512-HrC5BXdl00IP9zeV+0Z848QWPAoCr9P3bDEZguI+gkLcBKAOxix/tLEAAHC+UvDNPv4a2d18lOReHMOagPa+zQ==",
-      "path": "newtonsoft.json/13.0.3",
-      "hashPath": "newtonsoft.json.13.0.3.nupkg.sha512"
-    },
-    "System.Numerics.Vectors/4.6.1": {
-      "type": "package",
-      "serviceable": true,
-      "sha512": "sha512-sQxefTnhagrhoq2ReR0D/6K0zJcr9Hrd6kikeXsA1I8kOCboTavcUC4r7TSfpKFeE163uMuxZcyfO1mGO3EN8Q==",
-      "path": "system.numerics.vectors/4.6.1",
-      "hashPath": "system.numerics.vectors.4.6.1.nupkg.sha512"
-    },
-    "CommonUsage/1.0.0.0": {
-      "type": "reference",
-      "serviceable": false,
-      "sha512": ""
-    },
-    "LessokajiWeaverUtilities/1.0.0.0": {
-      "type": "reference",
-      "serviceable": false,
-      "sha512": ""
-    },
-    "MDCSToolBox/1.0.0.0": {
-      "type": "reference",
-      "serviceable": false,
-      "sha512": ""
-    },
-    "RefClumsyCore/0.0.0.0": {
-      "type": "reference",
-      "serviceable": false,
-      "sha512": ""
-    },
-    "RefClumsyDance/0.0.0.0": {
-      "type": "reference",
-      "serviceable": false,
-      "sha512": ""
-    },
-    "RefFundamentalLib/0.0.0.0": {
-      "type": "reference",
-      "serviceable": false,
-      "sha512": ""
-    }
-  }
-}

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

@@ -1,84 +0,0 @@
-{
-  "format": 1,
-  "restore": {
-    "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj": {}
-  },
-  "projects": {
-    "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj": {
-      "version": "1.0.0",
-      "restore": {
-        "projectUniqueName": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
-        "projectName": "ClumsyPilot",
-        "projectPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
-        "packagesPath": "C:\\Users\\admin\\.nuget\\packages\\",
-        "outputPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\obj\\",
-        "projectStyle": "PackageReference",
-        "fallbackFolders": [
-          "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages"
-        ],
-        "configFilePaths": [
-          "C:\\Users\\admin\\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.3, )"
-            },
-            "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.316\\RuntimeIdentifierGraph.json"
-        }
-      }
-    }
-  }
-}

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

@@ -1,16 +0,0 @@
-<?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\admin\.nuget\packages\;C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages</NuGetPackageFolders>
-    <NuGetProjectStyle Condition=" '$(NuGetProjectStyle)' == '' ">PackageReference</NuGetProjectStyle>
-    <NuGetToolVersion Condition=" '$(NuGetToolVersion)' == '' ">6.14.3</NuGetToolVersion>
-  </PropertyGroup>
-  <ItemGroup Condition=" '$(ExcludeRestorePackageImports)' != 'true' ">
-    <SourceRoot Include="C:\Users\admin\.nuget\packages\" />
-    <SourceRoot Include="C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages\" />
-  </ItemGroup>
-</Project>

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

@@ -1,6 +0,0 @@
-<?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

@@ -1,4 +0,0 @@
-// <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

@@ -1,22 +0,0 @@
-//------------------------------------------------------------------------------
-// <auto-generated>
-//     This code was generated by a tool.
-//
-//     Changes to this file may cause incorrect behavior and will be lost if
-//     the code is regenerated.
-// </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+580a936a830dcb7a25ef327cf553341405264033")]
-[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

@@ -1 +0,0 @@
-038d57c5b1714403d308c9343b385ef762951607b63a9fb275f4d7d2b8fd29cb

ClumsyPilot/obj/Debug/ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig

@@ -1,8 +0,0 @@
-is_global = true
-build_property.RootNamespace = MultiWheelC
-build_property.ProjectDir = d:\MyParking\ClumsyPilot\
-build_property.EnableComHosting = 
-build_property.EnableGeneratedComInterfaceComImportInterop = 
-build_property.CsWinRTUseWindowsUIXamlProjections = false
-build_property.EffectiveAnalysisLevelStyle = 
-build_property.EnableCodeStyleSeverity = 

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

@@ -1 +0,0 @@
-3920568398d269aea7b71fb4581ad111777c15ca3f2f2bb272ee9e0a989215c4

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

@@ -1,34 +0,0 @@
-D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\ClumsyPilot.deps.json
-D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\ClumsyPilot.dll
-D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\ClumsyPilot.pdb
-D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\CommonUsage.dll
-D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\LessokajiWeaverUtilities.dll
-D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\MDCSToolBox.dll
-D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\RefClumsyCore.dll
-D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\RefClumsyDance.dll
-D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\RefFundamentalLib.dll
-D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.csproj.AssemblyReference.cache
-D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig
-D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.AssemblyInfoInputs.cache
-D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.AssemblyInfo.cs
-D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.csproj.CoreCompileInputs.cache
-D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPi.5EF10E9F.Up2Date
-D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.dll
-D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.pdb
-D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\ClumsyPilot.deps.json
-D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\ClumsyPilot.dll
-D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\ClumsyPilot.pdb
-D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\CommonUsage.dll
-D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\LessokajiWeaverUtilities.dll
-D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\MDCSToolBox.dll
-D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\RefClumsyCore.dll
-D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\RefClumsyDance.dll
-D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\RefFundamentalLib.dll
-D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.csproj.AssemblyReference.cache
-D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig
-D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.AssemblyInfoInputs.cache
-D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.AssemblyInfo.cs
-D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.csproj.CoreCompileInputs.cache
-D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPi.5EF10E9F.Up2Date
-D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.dll
-D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.pdb

ClumsyPilot/obj/project.assets.json

@@ -1,341 +0,0 @@
-{
-  "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.3": {
-        "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.3": {
-      "sha512": "HrC5BXdl00IP9zeV+0Z848QWPAoCr9P3bDEZguI+gkLcBKAOxix/tLEAAHC+UvDNPv4a2d18lOReHMOagPa+zQ==",
-      "type": "package",
-      "path": "newtonsoft.json/13.0.3",
-      "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.3.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.3",
-      "System.Numerics.Vectors >= 4.6.1"
-    ]
-  },
-  "packageFolders": {
-    "C:\\Users\\admin\\.nuget\\packages\\": {},
-    "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages": {}
-  },
-  "project": {
-    "version": "1.0.0",
-    "restore": {
-      "projectUniqueName": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
-      "projectName": "ClumsyPilot",
-      "projectPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
-      "packagesPath": "C:\\Users\\admin\\.nuget\\packages\\",
-      "outputPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\obj\\",
-      "projectStyle": "PackageReference",
-      "fallbackFolders": [
-        "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages"
-      ],
-      "configFilePaths": [
-        "C:\\Users\\admin\\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.3, )"
-          },
-          "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.316\\RuntimeIdentifierGraph.json"
-      }
-    }
-  }
-}

ClumsyPilot/obj/project.nuget.cache

@@ -1,13 +0,0 @@
-{
-  "version": 2,
-  "dgSpecHash": "YBvImiCcgSo=",
-  "success": true,
-  "projectFilePath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
-  "expectedPackageFiles": [
-    "C:\\Users\\admin\\.nuget\\packages\\microsoft.netcore.platforms\\1.1.0\\microsoft.netcore.platforms.1.1.0.nupkg.sha512",
-    "C:\\Users\\admin\\.nuget\\packages\\netstandard.library\\2.0.3\\netstandard.library.2.0.3.nupkg.sha512",
-    "C:\\Users\\admin\\.nuget\\packages\\newtonsoft.json\\13.0.3\\newtonsoft.json.13.0.3.nupkg.sha512",
-    "C:\\Users\\admin\\.nuget\\packages\\system.numerics.vectors\\4.6.1\\system.numerics.vectors.4.6.1.nupkg.sha512"
-  ],
-  "logs": []
-}

MedullaAdapter/AlarmRoutine.cs

@@ -1,15 +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>
     {
-        // M层单车安全：汇总停车机器人夹臂等自定义报警状态。
         public override void SetOtherAlarms()
         {
             AddAlarm("左夹臂驱动报警", 2, () => cart.LeftArmErrorCode != 0);
             AddAlarm("右夹臂驱动报警", 2, () => cart.RightArmErrorCode != 0);
+            //AddAlarm("夹臂不同步报警", 2, () => cart.ClampOutOfSync);
         }
     }
-}
+}

MedullaAdapter/DiverCartDefinition.cs

@@ -1,12 +1,13 @@
-// 定义车型、上下层IO、参数和MCU初始化
-using CartActivator;
+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;
-using MyParking.Shared;
+
 
 namespace MedullaAdapter
 {
@@ -14,32 +15,49 @@ namespace MedullaAdapter
     [UseLadderLogic(logic = typeof(MotorRoutine), scanInterval = 50)]
     [UseLadderLogic(logic = typeof(MCURoutine), scanInterval = 20)]
     [UseManualController(manualController = typeof(Remote))]
-    public class DiverCartDefinition : MultiWheelCartDefinition
+    public partial class DiverCartDefinition : MultiWheelCartDefinition
     {
-        #region 基本成员
+        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;
 
-        internal enum ManualControlMode
-        {
-            Normal = 0, // 正常模式
-            Crab = 1, // 螃蟹模式
-            Spin = 2, // 自旋模式
-        }
-        internal ManualControlMode TransmitterControlMode = ManualControlMode.Normal;
-        internal DateTime TransmitterLastTime = DateTime.Now; // 物理遥控器计算两次实体遥控器指令之间的时间间隔
-        private ManualControlMode? _pendingManualMode;
-        private ManualControlMode? _activeManualMode;
         #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;
 
-        #region AsUpperIO
-        [AsUpperIO(desc = "从C上复位")] public bool ResetFromC;
-        [AsUpperIO(desc = "从C将驱动轮下使能")] public bool DisableFromC;
-        [AsUpperIO(desc = "左夹臂下发速度", timeOutReset = true)] public float SpeedLeftArm;
-        [AsUpperIO(desc = "右夹臂下发速度", timeOutReset = true)] public float SpeedRightArm;
-        [AsUpperIO(desc = "夹臂不同步报警")] public bool ClampOutOfSync;
-        #endregion
-
-        #region AsLowerIO
         [AsLowerIO(desc = "左前左轮实际位置")] public float LFLActualPos;
         [AsLowerIO(desc = "左前右轮实际位置")] public float LFRActualPos;
         [AsLowerIO(desc = "右前左轮实际位置")] public float RFLActualPos;
@@ -48,8 +66,11 @@ namespace MedullaAdapter
         [AsLowerIO(desc = "左后右轮实际位置")] public float LRRActualPos;
         [AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
         [AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
-        [AsLowerIO(desc = "左夹臂实际速度")] public float ActualSpeedLeftArm;
-        [AsLowerIO(desc = "右夹臂实际速度")] public float ActualSpeedRightArm;
+
+        [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;
@@ -58,85 +79,86 @@ namespace MedullaAdapter
         [AsLowerIO(desc = "右夹臂电流")] public float RightArmElectric;
         [AsLowerIO(desc = "左夹臂实际位置")] public float ActualPosLeftArm;
         [AsLowerIO(desc = "右夹臂实际位置")] public float ActualPosRightArm;
-        [AsLowerIO(desc = "驱动轮使能状态")] public bool WheelAbleState = true;
-        [AsLowerIO(desc = "电池健康状态")] public float SOH;
-        [AsInitParam(desc = "车号")][AsLowerIO] public int CarNum = 1;
-        #endregion
+        [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;
 
-        #region  初始参数
-        [AsInitParam(desc = "MCU端口号")] public string MCUPort = "COM4";
-        [AsInitParam(desc = "遥控器速度上限")] public float TransmitterSpeedUpperLimit = 1.0f;
-        [AsInitParam(desc = "遥控器速度下限")] public float TransmitterSpeedLowerLimit = 0.0f;
         [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";
 
-        #endregion
+        [AsLowerIO(desc = "陀螺仪角度")] public float GyrosTh;
+        [AsLowerIO(desc = "电池健康状态")] public float SOH;
 
-        #region 监控参数
-        [IOObjectMonitor(desc = "从M上复位")] public bool ResetFromM;
-        [IOObjectMonitor(desc = "从M将驱动轮下使能")] public bool DisableFromM;
-        [IOObjectMonitor(desc = "左前左轮PID修正后速度")] public float SpeedLFL;
-        [IOObjectMonitor(desc = "左前右轮PID修正后速度")] public float SpeedLFR;
-        [IOObjectMonitor(desc = "右前左轮PID修正后速度")] public float SpeedRFL;
-        [IOObjectMonitor(desc = "右前右轮PID修正后速度")] public float SpeedRFR;
-        [IOObjectMonitor(desc = "左后左轮PID修正后速度")] public float SpeedLRL;
-        [IOObjectMonitor(desc = "左后右轮PID修正后速度")] public float SpeedLRR;
-        [IOObjectMonitor(desc = "右后左轮PID修正后速度")] public float SpeedRRL;
-        [IOObjectMonitor(desc = "右后右轮PID修正后速度")] public float SpeedRRR;
-        [IOObjectMonitor(desc = "灯光模式")] public int LightMode = 0;
-        [IOObjectMonitor(desc = "实体遥控器当前速度倍率")] public float TransmitterSpeed = 0.3f;
-        [IOObjectMonitor(desc = "左前左驱动器远程帧701")] public byte LFLRemoteCode = 0;
-        [IOObjectMonitor(desc = "左前右驱动器远程帧702")] public byte LFRRemoteCode = 0;
-        [IOObjectMonitor(desc = "右前左驱动器远程帧703")] public byte RFLRemoteCode = 0;
-        [IOObjectMonitor(desc = "右前右驱动器远程帧704")] public byte RFRRemoteCode = 0;
-        [IOObjectMonitor(desc = "左后左驱动器远程帧705")] public byte LRLRemoteCode = 0;
-        [IOObjectMonitor(desc = "左后右驱动器远程帧706")] public byte LRRRemoteCode = 0;
-        [IOObjectMonitor(desc = "右后左驱动器远程帧707")] public byte RRLRemoteCode = 0;
-        [IOObjectMonitor(desc = "右后右驱动器远程帧708")] public byte RRRRemoteCode = 0;
-        [IOObjectMonitor(desc = "左夹臂驱动器远程帧709")] public byte LArmRemoteCode = 0;
-        [IOObjectMonitor(desc = "右夹臂驱动器远程帧70A")] public byte RArmRemoteCode = 0;
-        #endregion
+        [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;
 
-        #region  操作按钮
-        // M层单车硬件：向驱动轮发送复位请求。
         [IOObjectUtility]
         public void WheelReset()
         {
             ResetFromM = true;
         }
-        // M层单车硬件：向驱动轮发送下使能请求。
+
         [IOObjectUtility]
         public void WheelDisable()
         {
             DisableFromM = true;
         }
-        #endregion
 
         public override void CommunicationInit()
         {
             if (GhostMode) return;
-            State = -1;
+
             Bridge = new MCUSerialBridge();
             //Step1:打开指定串口连接
             var err = Bridge.Open(MCUPort, 1000000u);
             if (err != MCUSerialBridgeError.OK)
             {
-                Console.WriteLine($"MCU Open FAILED: {err.ToDescription()}");
+                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: {err.ToDescription()}");
+                Console.WriteLine("MCU Reset FAILED: {0}", err.ToDescription());
                 return;
             }
             else
@@ -144,28 +166,31 @@ namespace MedullaAdapter
                 Thread.Sleep(500);
                 Console.WriteLine("MCU Reset OK");
             }
-            //Step3:获取MCU版本号
+
+            //Step3:GetVersion
             err = Bridge.GetVersion(out var version, 100);
             if (err != MCUSerialBridgeError.OK)
             {
-                Console.WriteLine($"MCU GetVersion FAILED: {err.ToDescription()}");
+                Console.WriteLine("MCU GetVersion FAILED: {0}", err.ToDescription());
                 return;
             }
             else
             {
-                Console.WriteLine($"MCU GetVersion OK: {version}");
+                Console.WriteLine("MCU GetVersion OK: {0}", version.ToString());
             }
-            //Step4:获取MCU状态
+
+            //Step4:GetState
             err = Bridge.GetState(out var state, 100);
             if (err != MCUSerialBridgeError.OK)
             {
-                Console.WriteLine($"MCU GetState FAILED: {err.ToDescription()}");
+                Console.WriteLine("MCU GetState FAILED: {0}", err.ToDescription());
                 return;
             }
             else
             {
-                Console.WriteLine($"MCU GetState OK: {state}");
+                Console.WriteLine("MCU GetState OK: {0}", state.ToString());
             }
+
             //Step5:串口/CAN配置
             try
             {
@@ -178,15 +203,18 @@ namespace MedullaAdapter
                 for (int i = 0; i < ports.Count; i++)
                 {
                     if (ports[i] is SerialPortConfig s)
-                        Console.WriteLine($"Port {i}: Serial, Baud={s.Baud}, ReceiveFrameMs={s.ReceiveFrameMs}");
+                        Console.WriteLine("Port {0}: Serial, Baud={1}, ReceiveFrameMs={2}",
+                            i,
+                            s.Baud,
+                            s.ReceiveFrameMs);
                     else if (ports[i] is CANPortConfig c)
-                        Console.WriteLine($"Port {i}: CAN, Baud={c.Baud}, RetryTimeMs={c.RetryTimeMs}");
+                        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: {ret.ToDescription()}");
+                    Console.WriteLine("MCU Configure FAILED: {0}", ret.ToDescription());
                     return;
                 }
                 else
@@ -197,221 +225,54 @@ namespace MedullaAdapter
             }
             catch (Exception ex)
             {
-                Console.WriteLine($"Configure Exception: {ex.Message}");
-                return;
+                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)
+        internal void ManualControl(ManualControlMode mode, float x, float y, float frontDirection,
+                    float speedThreshold, TimeSpan? interval = null)
         {
             if (Chassis == null) return;
 
-            var adapter = GetChassisAdapter();
-            if (adapter == null) return;
-
-            // 模式变化时先停车并下发舵轮准备角度；
-            // 在实际舵角到位之前，不开放驱动速度。
-            if (!EnsureManualModeReady(mode, interval))
-            {
-                adapter.StopImmediately();
-                return;
-            }
-
             var speed = speedThreshold * y;
-            var omega = CalculateManualOmega(speed, x);
-            ManualMode = (int)mode;
-
+            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:
-                    SendBodyCommand(vx: speed, vy: 0.0, omegaRadiansPerSecond: omega, interval);
+                    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:
-                    SendBodyCommand(vx: 0.0, vy: speed, omegaRadiansPerSecond: omega, interval);
+                    ManualMode = 2;
+                    Chassis.DirectionAngle = 90;
+                    Chassis.SendMotion(speed, frontTh, rearTh, interval);
                     break;
+
                 case ManualControlMode.Spin:
-                    var spinOmega =
-                        speed * MaxAngularSpeed *
-                        Math.PI / 180.0;
-
-                    SendBodyCommand(
-                        vx: 0.0,
-                        vy: 0.0,
-                        omegaRadiansPerSecond: spinOmega,
-                        interval);
-                    break;
-                default:
-                    ManualMode = -1;
-                    adapter.StopImmediately();
+                    ManualMode = 3;
+                    Chassis.SendRotateMotion(speed * MaxAngularSpeed, interval);
                     break;
             }
         }
-
-        // 停车后切换模式：先预转舵轮，实际角度到位后才允许发送运动命令。
-        private bool EnsureManualModeReady(
-            ManualControlMode mode,
-            TimeSpan? interval)
+       
+        internal enum ManualControlMode
         {
-            var adapter = GetChassisAdapter();
-            if (adapter == null)
-                return false;
-
-            // 当前模式已经完成准备，可以直接接受运动命令。
-            if (_activeManualMode == mode &&
-                _pendingManualMode == null)
-            {
-                return true;
-            }
-
-            // 第一次收到新模式时，停车并下发一次舵轮准备姿态。
-            if (_pendingManualMode != mode)
-            {
-                adapter.StopImmediately();
-
-                var preparationAccepted = mode switch
-                {
-                    ManualControlMode.Normal =>
-                        adapter.PrepareParallelDirection(0.0),
-
-                    ManualControlMode.Crab =>
-                        adapter.PrepareParallelDirection(
-                            Math.PI / 2.0),
-
-                    ManualControlMode.Spin =>
-                        adapter.PrepareSpin(interval),
-
-                    _ => false
-                };
-
-                if (!preparationAccepted)
-                {
-                    _pendingManualMode = null;
-                    return false;
-                }
-
-                _pendingManualMode = mode;
-                return false;
-            }
-
-            // 后续控制周期保持停车，并读取实际舵角判断是否到位。
-            adapter.StopImmediately();
-
-            const double toleranceRadians =
-                2.0 * Math.PI / 180.0;
-
-            bool aligned;
-
-            if (mode == ManualControlMode.Spin)
-            {
-                // 自转的四个舵轮目标角不同，等待期间持续刷新其目标。
-                var preparationAccepted =
-                    adapter.PrepareSpin(interval);
-
-                aligned =
-                    preparationAccepted &&
-                    adapter.AreSpinWheelsAligned;
-            }
-            else
-            {
-                var targetDirection = mode ==
-                    ManualControlMode.Crab
-                        ? Math.PI / 2.0
-                        : 0.0;
-
-                aligned =
-                    adapter.AreParallelWheelsAligned(
-                        targetDirection,
-                        toleranceRadians);
-            }
-
-            if (!aligned)
-                return false;
-
-            _activeManualMode = mode;
-            _pendingManualMode = null;
-            return true;
+            Normal = 0,
+            Spin = 1,
+            Crab = 2,
+            Sway = 3
         }
-
-        private double CalculateManualOmega(
-            float speed,
-            float steeringInput)
-        {
-            var normalizedSteering =
-                (float)Math.Pow(
-                    Math.Abs(steeringInput),
-                    ManualThetaPow) *
-                Math.Sign(steeringInput);
-
-            var steeringDegrees =
-                -normalizedSteering * MaxManualTheta;
-
-            var steeringRadians =
-                steeringDegrees * Math.PI / 180.0;
-
-            // CommonUsage中的ControlPointRadius单位为毫米。
-            var halfWheelBaseMeters =
-                Math.Max(
-                    Chassis.ControlPointRadius / 1000.0,
-                    0.01);
-
-            return speed * Math.Tan(steeringRadians) / halfWheelBaseMeters;
-        }
-
-        internal void SendBodyCommand(double vx, double vy, double omegaRadiansPerSecond, TimeSpan? interval = null)
-        {
-            var adapter = GetChassisAdapter();
-
-            if (adapter == null)
-                return;
-
-            var command = new ChassisCommand(
-                CarNum,
-                new Twist2D(vx, vy, omegaRadiansPerSecond));
-
-            if (!adapter.Send(command, interval))
-            {
-                adapter.StopImmediately();
-
-                Console.WriteLine(
-                    "底盘命令分解失败，车辆已经停车：" +
-                    adapter.LastFailureReason);
-            }
-        }
-        private MultiWheelChassisAdapter _chassisAdapter;
-
-        private MultiWheelChassisAdapter GetChassisAdapter()
-        {
-            if (Chassis == null)
-                return null;
-
-            if (_chassisAdapter == null ||
-                _chassisAdapter.VehicleId != CarNum)
-            {
-                _chassisAdapter =
-                    new MultiWheelChassisAdapter(Chassis, CarNum);
-            }
-
-            return _chassisAdapter;
-        }
-
-        #region MCURoutine兼容参数（暂保留原硬件协议）
-
-        // 保存MCU读取到的原始输入字节，供M层监控和硬件排查使用。
-        [AsLowerIO(desc = "MCU原始输入字节")]
-        public float test;
-
-        // 保留原MCU灯光分支；单车默认值-1表示使用本车LightMode。
-        [AsUpperIO(desc = "多车灯光同步兼容值，-1使用本车灯光")]
-        public int MultiVehicleLightSync = -1;
-
-        #endregion
-
     }
 }

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

@@ -1,5 +1,4 @@
-// 实际CAN协议、反馈解析、IO、电池、急停
-using CartActivator;
+using CartActivator;
 using FundamentalLib;
 using MCUSerialBridgeCLR;
 using System;
@@ -7,7 +6,8 @@ using System.Collections.Generic;
 
 namespace MedullaAdapter
 {
-    public class MCURoutine : LadderLogic<DiverCartDefinition>
+    //[LogicRunOnMCU(scanInterval = 20)]
+    public class MCURoutine:LadderLogic<DiverCartDefinition>
     {
         private int _lastIteration = 0;
         private int _count = 0;
@@ -27,27 +27,22 @@ namespace MedullaAdapter
         private const byte BatteryPortIndex = 3;
         private static readonly byte[] BatteryRequest = BuildBatteryRequest();
 
-        // M层单车底盘：将车轮线速度换算为驱动电机转速。
         private static float ConvertMps2Rpm(float mps)
         {
             return (float)(mps / (Math.PI * 85f) * 10.5f * 60f * 1000f);
         }
-        // M层单车底盘：将驱动电机转速换算为车轮线速度。
         private static float ConvertRpm2Mps(float rpm)
         {
             return (float)(rpm / 10.5f / 60f * Math.PI * 85f / 1000f);
         }
-        // M层单车底盘：将夹臂或执行器转数换算为毫米位移。
         private float ConvertR2MM(float r)
         {
             return (float)(r / 10.5f * Math.PI * 85);
         }
-        // M层CAN解析：从驱动器报文中解码有符号转速。
         private static float DecodeRpmFromPayload(byte[] payload, int offset = 4)
         {
             return BitConverter.ToInt32(payload, offset) * 1875f / 512f / 10000f;
         }
-        // M层硬件主循环：交换IO、发送轮组指令并更新车辆反馈状态。
         public override void Operation(int iteration)
         {
             if (_lastIteration != iteration)
@@ -158,7 +153,6 @@ namespace MedullaAdapter
             cart.ActualSpeedLeftRear = (cart.ActualSpeedLeftRearLeft + cart.ActualSpeedLeftRearRight) / 2;
             cart.ActualSpeedRightFront = (cart.ActualSpeedRightFrontLeft + cart.ActualSpeedRightFrontRight) / 2;
             cart.ActualSpeedRightRear = (cart.ActualSpeedRightRearLeft + cart.ActualSpeedRightRearRight) / 2;
-            // M层CAN辅助：封装本周期驱动器CAN发送参数。
             MCUSerialBridgeError SendCan(byte port, ushort standardId, byte[] payload, bool RTR = false, uint timeout = 2)
             {
                 var canSend = new CANMessage
@@ -423,18 +417,16 @@ namespace MedullaAdapter
             {
                 //_errorCount++;
             }
-
+            
             #endregion
 
         }
 
-        // M层CAN安全：判断单个驱动节点是否处于可运行状态。
         private static bool IsNodeOperational(byte remoteCode)
         {
             return remoteCode == 5 || remoteCode == 133;
         }
 
-        // M层CAN安全：检查全部车轮和夹臂节点是否已上线。
         private bool AreAllNodesOperational()
         {
             return IsNodeOperational(cart.LFLRemoteCode)
@@ -449,7 +441,6 @@ namespace MedullaAdapter
                 && IsNodeOperational(cart.RArmRemoteCode);
         }
 
-        // M层CAN维护：轮询所有驱动节点的在线状态。
         private static void SendNodeGuardRequests(Func<byte, ushort, byte[], bool, uint, MCUSerialBridgeError> sendCan)
         {
             sendCan(0, 0x709, Array.Empty<byte>(), true, 2);
@@ -464,7 +455,6 @@ namespace MedullaAdapter
             sendCan(0, 0x708, Array.Empty<byte>(), true, 2);
         }
 
-        // M层CAN通信：按需注册驱动器反馈报文回调。
         private void EnsureCanCallbacksRegistered()
         {
             if (_canCallbackRegistered)
@@ -488,7 +478,6 @@ namespace MedullaAdapter
             _canCallbackRegistered = true;
         }
 
-        // M层串口通信：按需注册电池等串口设备回调。
         private void EnsureSerialCallbacksRegistered()
         {
             if (_serialCallbackRegistered)
@@ -508,7 +497,6 @@ namespace MedullaAdapter
             _serialCallbackRegistered = err1 == MCUSerialBridgeError.OK;
         }
 
-        // M层单车电源：周期发送Modbus电池状态查询。
         private void PollBatterySerial(int iteration)
         {
             if (cart?.Bridge == null) return;
@@ -534,7 +522,6 @@ namespace MedullaAdapter
             TryUpdateBatteryData(msg);
         }
 
-        // M层单车电源：校验并解析电池Modbus响应。
         private void TryUpdateBatteryData(byte[] msg)
         {
             // Modbus RTU response: [id,03,08,data(8),crc(2)]
@@ -549,7 +536,6 @@ namespace MedullaAdapter
             cart.ElectricCurrent = -ReadInt16BE(msg, 9) * 0.1f;
         }
 
-        // M层单车电源：构造读取电池寄存器的Modbus请求。
         private static byte[] BuildBatteryRequest()
         {
             // 01 03 00 03 00 04 CRC (读取寄存器 03~06)
@@ -560,7 +546,6 @@ namespace MedullaAdapter
             return req;
         }
 
-        // M层协议辅助：计算Modbus RTU的CRC16校验值。
         private static ushort ComputeModbusCrc(byte[] data, int length)
         {
             ushort crc = 0xFFFF;
@@ -577,19 +562,16 @@ namespace MedullaAdapter
             return crc;
         }
 
-        // M层协议辅助：按大端序读取有符号16位数值。
         private static short ReadInt16BE(byte[] data, int offset)
         {
             return (short)((data[offset] << 8) | data[offset + 1]);
         }
 
-        // M层协议辅助：按大端序读取无符号16位数值。
         private static ushort ReadUInt16BE(byte[] data, int offset)
         {
             return (ushort)((data[offset] << 8) | data[offset + 1]);
         }
 
-        // M层CAN解析：建立驱动器报文标识符到处理函数的分发表。
         private Dictionary<ushort, Action<CANMessage>> BuildCanDispatchTable()
         {
             return new Dictionary<ushort, Action<CANMessage>>
@@ -802,7 +784,7 @@ namespace MedullaAdapter
                 {
                     //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;
+                    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

MedullaAdapter/MCUSerialBridgeCLR.cs

@@ -1,5 +1,3 @@
-// C#调用MCU通信桥
-
 using System;
 using System.Collections.Generic;
 using System.Linq;
@@ -104,7 +102,6 @@ namespace MCUSerialBridgeCLR
         /// 转换为可读字符串
         /// </summary>
         /// <returns>返回包含产品、Tag、Commit、BuildTime 的字符串</returns>
-        // M层MCU适配：格式化固件版本信息便于日志显示。
         public override string ToString()
         {
             return $"Product: {ProductionName}, Tag: {GitTag}, Commit: {GitCommit}, Built: {BuildTime}";
@@ -147,7 +144,6 @@ namespace MCUSerialBridgeCLR
         /// 返回可读的状态字符串
         /// </summary>
         /// <returns>例如 "Bridge: Running" 或 "DIVER: Error"</returns>
-        // M层MCU适配：格式化MCU运行状态便于日志显示。
         public override string ToString()
         {
             string modeStr = IsBridge ? "Bridge" : "DIVER";
@@ -178,7 +174,6 @@ namespace MCUSerialBridgeCLR
 
         /// <summary>序列化端口配置为字节数组（供 P/Invoke 使用）</summary>
         /// <returns>返回固定长度字节数组（16 bytes）</returns>
-        // M层MCU适配：将端口配置序列化为原生接口字节。
         public abstract byte[] ToBytes();
     }
 
@@ -190,7 +185,6 @@ namespace MCUSerialBridgeCLR
     /// </remarks>
     /// <param name="baud">波特率</param>
     /// <param name="receiveFrameMs">接收帧间隔</param>
-    // M层MCU适配：创建串口通信参数配置。
     public class SerialPortConfig(uint baud, uint receiveFrameMs) : PortConfig
     {
         /// <summary>Serial 类型</summary>
@@ -206,7 +200,6 @@ namespace MCUSerialBridgeCLR
         /// 转换为字节数组
         /// </summary>
         /// <returns>16 字节数组</returns>
-        // M层MCU适配：序列化串口波特率和组帧时间。
         public override byte[] ToBytes()
         {
             var c = new PortStructHelper.SerialPortConfigC
@@ -228,7 +221,6 @@ namespace MCUSerialBridgeCLR
     /// </remarks>
     /// <param name="baud">波特率</param>
     /// <param name="retryTimeMs">重发间隔</param>
-    // M层MCU适配：创建CAN通信参数配置。
     public class CANPortConfig(uint baud, uint retryTimeMs) : PortConfig
     {
         /// <summary>CAN 类型</summary>
@@ -244,7 +236,6 @@ namespace MCUSerialBridgeCLR
         /// 转换为字节数组
         /// </summary>
         /// <returns>16 字节数组</returns>
-        // M层MCU适配：序列化CAN波特率和重试时间。
         public override byte[] ToBytes()
         {
             var c = new PortStructHelper.CANPortConfigC
@@ -281,7 +272,6 @@ namespace MCUSerialBridgeCLR
         /// <returns>返回字节数组：2 bytes header + Payload</returns>
         /// <exception cref="ArgumentOutOfRangeException">如果 DLC > 8</exception>
         /// <exception cref="ArgumentException">如果 Payload 长度 != DLC</exception>
-        // M层CAN适配：将标准或扩展CAN帧序列化为原生布局。
         public byte[] ToBytes()
         {
             if (DLC > 8)
@@ -313,7 +303,6 @@ namespace MCUSerialBridgeCLR
         /// <param name="length">实际有效长度</param>
         /// <returns>CANMessage 实例</returns>
         /// <exception cref="ArgumentException">数据长度错误</exception>
-        // M层CAN适配：从原生缓冲区还原CAN消息。
         public static CANMessage FromBytes(byte[] data, uint length)
         {
             if (data == null || length > data.Length || length < 2)
@@ -343,7 +332,6 @@ namespace MCUSerialBridgeCLR
             return msg;
         }
 
-        // M层CAN诊断：格式化CAN标识符和数据内容。
         public override string ToString()
         {
             string payloadStr =
@@ -363,7 +351,6 @@ namespace MCUSerialBridgeCLR
         private const string DLL = @"mcu_serial_bridge.dll";
 
         [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
-        // M层原生接口：打开MCU串口桥设备。
         internal static extern MCUSerialBridgeError msb_open(
             out IntPtr handle,
             [MarshalAs(UnmanagedType.LPStr)] string port,
@@ -371,15 +358,12 @@ namespace MCUSerialBridgeCLR
         );
 
         [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
-        // M层原生接口：关闭MCU串口桥设备。
         internal static extern MCUSerialBridgeError msb_close(IntPtr handle);
 
         [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
-        // M层原生接口：复位MCU串口桥。
         internal static extern MCUSerialBridgeError msb_reset(IntPtr handle, uint timeout_ms);
 
         [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
-        // M层原生接口：读取MCU固件版本。
         public static extern MCUSerialBridgeError msb_version(
             IntPtr handle,
             out VersionInfo version,
@@ -387,7 +371,6 @@ namespace MCUSerialBridgeCLR
         );
 
         [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
-        // M层原生接口：读取MCU当前运行状态。
         public static extern MCUSerialBridgeError mcu_state(
             IntPtr handle,
             out MCUState state,
@@ -395,7 +378,6 @@ namespace MCUSerialBridgeCLR
         );
 
         [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
-        // M层原生接口：下发串口桥端口配置。
         internal static extern MCUSerialBridgeError msb_configure(
             IntPtr handle,
             uint num_ports,
@@ -404,7 +386,6 @@ namespace MCUSerialBridgeCLR
         );
 
         [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
-        // M层原生接口：读取MCU数字输入。
         internal static extern MCUSerialBridgeError msb_read_input(
             IntPtr handle,
             [Out] byte[] inputs,
@@ -412,7 +393,6 @@ namespace MCUSerialBridgeCLR
         );
 
         [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
-        // M层原生接口：写入MCU数字输出。
         internal static extern MCUSerialBridgeError msb_write_output(
             IntPtr handle,
             [In] byte[] outputs,
@@ -420,7 +400,6 @@ namespace MCUSerialBridgeCLR
         );
 
         [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
-        // M层原生接口：从指定串口或CAN端口读取数据。
         internal static extern MCUSerialBridgeError msb_read_port(
             IntPtr handle,
             byte port_index,
@@ -431,7 +410,6 @@ namespace MCUSerialBridgeCLR
         );
 
         [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
-        // M层硬件桥接：定义串口或CAN端口收到原生数据时的回调签名。
         internal delegate void msb_on_port_data_callback_function_t(
             IntPtr dst_data,
             uint dst_data_size,
@@ -439,7 +417,6 @@ namespace MCUSerialBridgeCLR
         );
 
         [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
-        // M层原生接口：注册端口数据到达回调。
         internal static extern MCUSerialBridgeError msb_register_port_data_callback(
             IntPtr handle,
             byte port_index,
@@ -448,7 +425,6 @@ namespace MCUSerialBridgeCLR
         );
 
         [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
-        // M层原生接口：向指定串口或CAN端口写入数据。
         internal static extern MCUSerialBridgeError msb_write_port(
             IntPtr handle,
             byte port_index,
@@ -472,21 +448,18 @@ namespace MCUSerialBridgeCLR
         public bool IsOpen => nativeHandle != IntPtr.Zero;
 
         /// <summary>构造函数，初始化对象</summary>
-        // M层MCU适配：创建串口桥包装器并固定原生回调委托。
         public MCUSerialBridge()
         {
             nativeHandle = IntPtr.Zero;
         }
 
         /// <summary>析构函数</summary>
-        // M层MCU适配：对象回收时兜底释放原生串口桥句柄。
         ~MCUSerialBridge()
         {
             Dispose(false);
         }
 
         /// <summary>显式释放资源</summary>
-        // M层MCU适配：释放串口桥句柄和非托管资源。
         public void Dispose()
         {
             Dispose(true);
@@ -495,7 +468,6 @@ namespace MCUSerialBridgeCLR
 
         /// <summary>内部释放资源方法</summary>
         /// <param name="disposing">true 表示手动释放，false 表示析构释放</param>
-        // M层MCU适配：按托管或终结路径关闭原生句柄。
         private void Dispose(bool disposing)
         {
             if (nativeHandle != IntPtr.Zero)
@@ -509,7 +481,6 @@ namespace MCUSerialBridgeCLR
         /// <param name="portName">串口名，如 "COM3"</param>
         /// <param name="baud">波特率</param>
         /// <returns>错误码</returns>
-        // M层单车通信：按端口名和波特率连接MCU串口桥。
         public MCUSerialBridgeError Open(string portName, uint baud)
         {
             return MCUSerialBridgeCoreAPI.msb_open(out nativeHandle, portName, baud);
@@ -517,7 +488,6 @@ namespace MCUSerialBridgeCLR
 
         /// <summary>关闭串口</summary>
         /// <returns>错误码</returns>
-        // M层单车通信：关闭当前MCU串口桥连接。
         public MCUSerialBridgeError Close()
         {
             if (nativeHandle == IntPtr.Zero)
@@ -530,7 +500,6 @@ namespace MCUSerialBridgeCLR
 
         /// <summary>MCU 复位</summary>
         /// <returns>错误码</returns>
-        // M层单车通信：请求MCU复位并等待结果。
         public MCUSerialBridgeError Reset(uint timeout = 200)
         {
             if (nativeHandle == IntPtr.Zero)
@@ -543,7 +512,6 @@ namespace MCUSerialBridgeCLR
         /// <param name="version">输出版本信息</param>
         /// <param name="timeout">超时时间（ms）</param>
         /// <returns>错误码</returns>
-        // M层MCU诊断：读取串口桥固件版本。
         public MCUSerialBridgeError GetVersion(out VersionInfo version, uint timeout = 200)
         {
             version = new VersionInfo();
@@ -557,7 +525,6 @@ namespace MCUSerialBridgeCLR
         /// <param name="state">输出状态</param>
         /// <param name="timeout">超时时间（ms）</param>
         /// <returns>错误码</returns>
-        // M层MCU诊断：读取串口桥运行状态。
         public MCUSerialBridgeError GetState(out MCUState state, uint timeout = 200)
         {
             state = new MCUState();
@@ -571,7 +538,6 @@ namespace MCUSerialBridgeCLR
         /// <param name="ports">端口集合</param>
         /// <param name="timeout">超时时间（ms）</param>
         /// <returns>错误码</returns>
-        // M层MCU适配：批量配置CAN和串口通道参数。
         public MCUSerialBridgeError Configure(IEnumerable<PortConfig> ports, uint timeout = 200)
         {
             if (nativeHandle == IntPtr.Zero)
@@ -621,7 +587,6 @@ namespace MCUSerialBridgeCLR
         /// <param name="inputs">输出数组</param>
         /// <param name="timeout">超时（ms）</param>
         /// <returns>错误码</returns>
-        // M层单车IO：读取MCU数字输入状态。
         public MCUSerialBridgeError ReadInput(out byte[] inputs, uint timeout = 100)
         {
             inputs = new byte[4];
@@ -635,7 +600,6 @@ namespace MCUSerialBridgeCLR
         /// <param name="outputs">数据数组</param>
         /// <param name="timeout">超时（ms）</param>
         /// <returns>错误码</returns>
-        // M层单车IO：写入继电器、灯光等数字输出状态。
         public MCUSerialBridgeError WriteOutput(byte[] outputs, uint timeout = 100)
         {
             if (nativeHandle == IntPtr.Zero)
@@ -665,7 +629,6 @@ namespace MCUSerialBridgeCLR
         ///  - NoData           当前无可读数据（仅在 timeout == 0 或等待超时）
         ///  - Win_InvalidParam 参数错误
         /// </returns>
-        // M层串口通信：同步读取指定MCU串口的数据。
         public MCUSerialBridgeError ReadSerial(byte portIndex, out byte[] buffer, uint timeout)
         {
             buffer = Array.Empty<byte>();
@@ -707,7 +670,6 @@ namespace MCUSerialBridgeCLR
         ///  - OK                   成功发送
         ///  - 其他错误请查看 MCUSerialBridgeError
         /// </returns>
-        // M层串口通信：向指定MCU串口发送数据。
         public MCUSerialBridgeError WriteSerial(byte portIndex, byte[] data, uint timeout)
         {
             if (nativeHandle == IntPtr.Zero)
@@ -744,7 +706,6 @@ namespace MCUSerialBridgeCLR
         ///  - CAN_DataError        CAN数据错误
         ///  - Win_HandleNotFound   句柄无效
         /// </returns>
-        // M层CAN通信：同步读取指定CAN通道的一帧消息。
         public MCUSerialBridgeError ReadCAN(byte portIndex, out CANMessage message, uint timeout)
         {
             message = null;
@@ -792,7 +753,6 @@ namespace MCUSerialBridgeCLR
         ///  - CAN_DataError        CAN 数据错误
         ///  - Win_HandleNotFound   句柄无效
         /// </returns>
-        // M层CAN通信：向指定CAN通道发送一帧消息。
         public MCUSerialBridgeError WriteCAN(byte portIndex, CANMessage message, uint timeout)
         {
             if (nativeHandle == IntPtr.Zero)
@@ -836,7 +796,6 @@ namespace MCUSerialBridgeCLR
         /// 5. 数据可能随时到来，请保证回调尽快返回，避免影响后续帧接收。
         /// 6. 不要把其他类型的端口注册到这个接口，接口不对 portIndex 做类型检查。
         /// </remarks>
-        // M层串口通信：注册指定串口的异步接收回调。
         public MCUSerialBridgeError RegisterSerialPortCallback(
             byte portIndex,
             Action<byte[]> callback
@@ -849,7 +808,6 @@ namespace MCUSerialBridgeCLR
                 return MCUSerialBridgeError.Config_PortNumOver;
 
             // 包装 C# 回调为 P/Invoke 委托
-            // M层串口回调：复制原生缓存并转交托管回调处理。
             void del(IntPtr dst_data, uint dst_data_size, IntPtr user_ctx)
             {
                 byte[] data = new byte[dst_data_size];
@@ -884,7 +842,6 @@ namespace MCUSerialBridgeCLR
         /// 5. 数据可能随时到来，请保证回调尽快返回，避免影响后续帧接收。
         /// 6. 不要把其他类型的端口注册到这个接口，接口不对 portIndex 做类型检查。
         /// </remarks>
-        // M层CAN通信：注册指定CAN通道的异步接收回调。
         public MCUSerialBridgeError RegisterCANPortCallback(
             byte portIndex,
             Action<CANMessage> callback
@@ -897,7 +854,6 @@ namespace MCUSerialBridgeCLR
                 return MCUSerialBridgeError.Config_PortNumOver;
 
             // 包装 C# 回调为 P/Invoke 委托
-            // M层CAN回调：还原原生CAN帧并转交托管回调处理。
             void del(IntPtr dst_data, uint dst_data_size, IntPtr user_ctx)
             {
                 try

MedullaAdapter/MCUSerialBridgeError.cs

@@ -1,4 +1,4 @@
-// MCU通信错误码
+using System;
 
 namespace MCUSerialBridgeCLR
 {
@@ -47,7 +47,6 @@ namespace MCUSerialBridgeCLR
 
     public static class MCUSerialBridgeErrorExtensions
     {
-        // M层MCU适配：把串口桥错误码转换为便于诊断的说明。
         public static string ToDescription(this MCUSerialBridgeError err)
         {
             return err switch

MedullaAdapter/MedullaAdapter.csproj

@@ -1,57 +1,44 @@
 <Project Sdk="Microsoft.NET.Sdk">
 
   <PropertyGroup>
-    <TargetFramework>net8.0</TargetFramework>
-    <ImplicitUsings>enable</ImplicitUsings>
-    <Nullable>disable</Nullable>
-    <Platforms>AnyCPU</Platforms>
-
-    <AssemblyName>MedullaAdapter</AssemblyName>
-
-    <AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
-    <OutputPath>build\Medulla\plugins\</OutputPath>
+    <TargetFramework>netstandard2.0</TargetFramework>
+	<LangVersion>latest</LangVersion>
   </PropertyGroup>
-
+	
   <ItemGroup>
-    <Reference Include="CartActivator">
-      <HintPath>ref\RefCartActivator.dll</HintPath>
-      <Private>false</Private>
-    </Reference>
-    <Reference Include="MedullaCore">
-      <HintPath>ref\RefMedullaCore.dll</HintPath>
-      <Private>false</Private>
-    </Reference>
-
-    <Reference Include="FundamentalLib">
-      <HintPath>ref\RefFundamentalLib.dll</HintPath>
-      <Private>false</Private>
-    </Reference>
-
-    <Reference Include="CommonUsage">
-      <HintPath>ref\CommonUsage.dll</HintPath>
-      <Private>false</Private>
-    </Reference>
-
-    <Reference Include="MDCSToolBox">
-      <HintPath>ref\MDCSToolBox.dll</HintPath>
-      <Private>false</Private>
-    </Reference>
-
-    <Reference Include="CycleGUI">
-      <HintPath>ref\CycleGUI.dll</HintPath>
-      <Private>false</Private>
-    </Reference>
+	  <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>
-  <Compile Include="..\Shared\ChassisCommand.cs"
-            Link="Shared\ChassisCommand.cs" />
-
-  <Compile Include="..\Shared\FrameTransform2D.cs"
-            Link="Shared\FrameTransform2D.cs" />
-
-  <Compile Include="..\Shared\MultiWheelChassisAdapter.cs"
-            Link="Shared\MultiWheelChassisAdapter.cs" />
-</ItemGroup>
-
+    <Reference Include="CommonUsage">
+      <HintPath>parkingRobotRef\CommonUsage.dll</HintPath>
+    </Reference>
+    <Reference Include="CycleGUI">
+      <HintPath>ref\CycleGUI.dll</HintPath>
+    </Reference>
+    <Reference Include="MDCSToolBox">
+      <HintPath>parkingRobotRef\MDCSToolBox.dll</HintPath>
+    </Reference>
+    <Reference Include="CartActivator">
+      <HintPath>parkingRobotRef\RefCartActivator.dll</HintPath>
+    </Reference>
+    <Reference Include="FundamentalLib">
+      <HintPath>parkingRobotRef\RefFundamentalLib.dll</HintPath>
+    </Reference>
+    <Reference Include="MedullaCore">
+      <HintPath>parkingRobotRef\RefMedullaCore.dll</HintPath>
+    </Reference>
+  </ItemGroup>
+	<ItemGroup>
+		<WeaverFiles Include="deps\DiverCompiler.exe" />
+	</ItemGroup>
 </Project>

MedullaAdapter/MotorRoutine.cs

@@ -1,5 +1,4 @@
-// 计算8个驱动电机的目标速度和舵角PID
-using CartActivator;
+using CartActivator;
 using CommonUsage.Mathematics;
 using FundamentalLib;
 using MDCSToolBox.Commons;
@@ -11,352 +10,224 @@ using static MDCSToolBox.Medulla.Chassis.BasicCartDefinition;
 
 namespace MedullaAdapter
 {
-    public class MotorRoutine : LadderLogic<DiverCartDefinition>
+    public class MotorRoutine:LadderLogic<DiverCartDefinition>
     {
-        private bool _wasTransmitterControlling;
         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)
         {
-            if (!cart.GhostMode && cart.State == -1) return;
-            // SA稳定打开后，使能实体遥控器。
-            TriggerOnce(
-                cart.TransmitterConnected && cart.Transmitter_SA,
-                300,
-                () =>
-                {
-                    cart.TransmitterControlEnable = true;
-                    cart.TransmitterLastTime = DateTime.Now;
-                });
-            // 遥控器断连或SA关闭时，立即撤销遥控使能。
-            if (!cart.TransmitterConnected || !cart.Transmitter_SA)
-                cart.TransmitterControlEnable = false;
-            var transmitterSelected =
-                cart.TransmitterConnected &&
-                cart.TransmitterControlEnable &&
-                cart.Transmitter_SA &&
-                cart.Transmitter_SC == cart.CarNum;
-            var transmitterControlling =
-                transmitterSelected &&
-                CartDefinition.testPriority(5, "TransmitterMode");
-            if (transmitterControlling)
+            void ResetMultiVehicle(string reason)
             {
-                TransmitterChassisControl();
-                cart.TransmitterLastTime = DateTime.Now;
-                cart.CarStatu = "实体遥控器控制";
-            }
-            else
-            {
-                // 只在遥控器刚刚退出时发送一次停车，
-                // 不能每周期停车，否则会覆盖C层轨迹控制。
-                if (_wasTransmitterControlling)
+                if (cart.MultiVehicleRemoteManualEnabled)
                 {
-                    cart.ManualControl(
-                        cart.TransmitterControlMode,
-                        0, 0, 0,
-                        cart.TransmitterSpeed,
-                        DateTime.Now - cart.TransmitterLastTime);
-
-                    StopClampArms();
-                    cart.TransmitterLastTime = DateTime.Now;
+                    LogTransmitterFleet("reset-skipped-remote-panel");
+                    return;
                 }
 
-                cart.CarStatu = "正常运行";
-            }
-            _wasTransmitterControlling = transmitterControlling;
-            // 当前是否由C层控制。
-            cart.ClumsyControl = CartDefinition.currentPriority == 0;
-            // 计算四个舵轮PID和8个驱动电机最终速度。
-            UpdateDiffSteerWheelSpeeds();
-            // 平滑更新硬件速度限制。
-            UpdateSendSpeedLimit();
-            // 更新红黄绿灯状态。
-            UpdateLightMode();
-        }
-        // 物理遥控器设置
-        public void TransmitterChassisControl()
-        {
-            var interval = DateTime.Now - cart.TransmitterLastTime;
-            switch (cart.Transmitter_SB)
-            {
-                case TransmitterState.Mode0:
-                    cart.TransmitterControlMode =
-                    DiverCartDefinition.ManualControlMode.Normal;
-                    break;
-                case TransmitterState.Mode1:
-                    cart.TransmitterControlMode =
-                        DiverCartDefinition.ManualControlMode.Crab;
-                    break;
-                case TransmitterState.Mode2:
-                    cart.TransmitterControlMode =
-                        DiverCartDefinition.ManualControlMode.Spin;
-                    break;
-                default:
-                    cart.ManualControl(
-                        cart.TransmitterControlMode,
-                        0, 0, 0,
-                        cart.TransmitterSpeed,
-                        interval);
-                    StopClampArms();
-                    return;
-            }
-            // SA关闭后立即停车。
-            if (!cart.Transmitter_SA)
-            {
-                cart.ManualControl(
-                    cart.TransmitterControlMode,
-                    0, 0, 0,
-                    cart.TransmitterSpeed,
-                    interval);
-                StopClampArms();
-                return;
-            }
-            // 限制实体遥控器的最大速度。
-            cart.TransmitterSpeed = Math.Max(
-                cart.TransmitterSpeedLowerLimit,
-                Math.Min(
-                    cart.TransmitterSpeed,
-                    cart.TransmitterSpeedUpperLimit));
-            // SD的Mode0作为底盘驾驶档。
-            if (cart.Transmitter_SD == TransmitterState.Mode0)
-            {
-                // 底盘驾驶档不允许保留上一周期的夹臂速度。
-                StopClampArms();
+                if (cart.MultiVehicleManualEnabled || cart.MultiVehicleManualVx != 0 || cart.MultiVehicleManualVy != 0 ||
+                    cart.MultiVehicleManualVth != 0 || cart.MultiVehicleHold)
+                    LogTransmitterFleetReset(reason);
 
-                cart.ManualControl(
-                    cart.TransmitterControlMode,
-                    cart.TransmitterLeftJoystickValX,
-                    cart.TransmitterRightJoystickValY,
-                    0,
-                    cart.TransmitterSpeed,
-                    interval);
-
-                return;
-            }
-            if (cart.Transmitter_SD == TransmitterState.Mode1)
-            {
-                // 切换到夹臂档时，先确保底盘停止。
-                cart.ManualControl(
-                    cart.TransmitterControlMode,
-                    0, 0, 0,
-                    cart.TransmitterSpeed,
-                    interval);
-
-                var armSpeed =
-                    cart.TransmitterRightJoystickValX *
-                    cart.ManualArmSpeedFac;
-
-                cart.SpeedLeftArm = armSpeed;
-                cart.SpeedRightArm = armSpeed;
-                return;
-            }
-            // 非驾驶档必须主动停车，防止上一条运动指令残留。
-            cart.ManualControl(
-                cart.TransmitterControlMode,
-                0, 0, 0,
-                cart.TransmitterSpeed,
-                interval);
-            StopClampArms();
-        }
-
-        // M层单车夹臂安全：清除物理遥控器留下的左右夹臂速度命令。
-        private void StopClampArms()
-        {
-            cart.SpeedLeftArm = 0;
-            cart.SpeedRightArm = 0;
-        }
-
-        // M层单车底盘：根据四个舵轮的目标角度和实际角度修正8个驱动电机速度。
-        private void UpdateDiffSteerWheelSpeeds()
-        {
-            if (cart.LeftFrontPid == null ||
-                cart.LeftRearPid == null ||
-                cart.RightFrontPid == null ||
-                cart.RightRearPid == null)
-            {
-                cart.SpeedLFL = 0;
-                cart.SpeedLFR = 0;
-                cart.SpeedRFL = 0;
-                cart.SpeedRFR = 0;
-                cart.SpeedLRL = 0;
-                cart.SpeedLRR = 0;
-                cart.SpeedRRL = 0;
-                cart.SpeedRRR = 0;
-                return;
+                cart.MultiVehicleManualEnabled = false;
+                cart.MultiVehicleManualVx = 0;
+                cart.MultiVehicleManualVy = 0;
+                cart.MultiVehicleManualVth = 0;
+                cart.MultiVehicleHold = false;
             }
 
-            // 更新左前舵轮PID参数。
-            cart.LeftFrontPid.ChangeParameters(
-                cart.DiffSteerKp,
-                cart.DiffSteerKi,
-                cart.DiffSteerKd,
-                cart.DiffSteerMaxI,
-                cart.DiffSteerDeadZone,
-                cart.DiffSteerThresh,
-                cart.DiffSteerSpeedAcc);
+            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);
+                });
 
-            // 更新左后舵轮PID参数。
-            cart.LeftRearPid.ChangeParameters(
-                cart.DiffSteerKp,
-                cart.DiffSteerKi,
-                cart.DiffSteerKd,
-                cart.DiffSteerMaxI,
-                cart.DiffSteerDeadZone,
-                cart.DiffSteerThresh,
-                cart.DiffSteerSpeedAcc);
-
-            // 更新右前舵轮PID参数。
-            cart.RightFrontPid.ChangeParameters(
-                cart.DiffSteerKp,
-                cart.DiffSteerKi,
-                cart.DiffSteerKd,
-                cart.DiffSteerMaxI,
-                cart.DiffSteerDeadZone,
-                cart.DiffSteerThresh,
-                cart.DiffSteerSpeedAcc);
-
-            // 更新右后舵轮PID参数。
-            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 diffLr = cart.LeftRearPid.GetResponse(
-                cart.ThLeftRear, false, false, "LR");
-
-            var diffRf = cart.RightFrontPid.GetResponse(
-                cart.ThRightFront, false, false, "RF");
-
-            var diffRr = cart.RightRearPid.GetResponse(
-                cart.ThRightRear, false, false, "RR");
-
-            // 左前腿：左右电机施加方向相反的PID修正量。
-            cart.SpeedLFL = cart.SpeedLeftFrontLeft - diffLf;
-            cart.SpeedLFR = cart.SpeedLeftFrontRight + diffLf;
-
-            // 左后腿。
-            cart.SpeedLRL = cart.SpeedLeftRearLeft - diffLr;
-            cart.SpeedLRR = cart.SpeedLeftRearRight + diffLr;
-
-            // 右前腿。
-            cart.SpeedRFL = cart.SpeedRightFrontLeft - diffRf;
-            cart.SpeedRFR = cart.SpeedRightFrontRight + diffRf;
-
-            // 右后腿。
-            cart.SpeedRRL = cart.SpeedRightRearLeft - diffRr;
-            cart.SpeedRRR = cart.SpeedRightRearRight + diffRr;
-        }
-
-        // M层单车限速：按照加速度和减速度平滑更新实际下发速度上限。
-        private void UpdateSendSpeedLimit()
-        {
-            if (cart.Chassis == null)
+            if (cart.TransmitterControlEnable && cart.Transmitter_SC == cart.CarNum)
             {
-                cart.SendThresSpeed = 0;
-                return;
+                CartDefinition.testPriority(5, "TransmitterMode");
+                ResetMultiVehicle("single-car-control");
+                LogTransmitterFleet("single-car-control");
+                TransmitterChassisControl();
+                cart.TransmitterLastTime = DateTime.Now;
+                cart.CarStatu = "Transmitter控制模式使能";
             }
-
-            var now = DateTime.Now;
-            var elapsedSeconds = (float)(now - _lastMoveTime).TotalSeconds;
-            _lastMoveTime = now;
-
-            // 防止调试暂停或线程卡顿后，一次产生过大的速度跳变。
-            elapsedSeconds = Math.Clamp(elapsedSeconds, 0f, 0.2f);
-
-            // 限速值不允许小于零。
-            var targetLimit = Math.Max(0f, cart.ThresSpeed);
-            var currentLimit = Math.Max(0f, cart.SendThresSpeed);
-
-            // 增大速度上限时用加速度，减小时用减速度。
-            var speedChangingRate =
-                targetLimit > currentLimit
-                    ? cart.Chassis.AccPerSecond
-                    : cart.Chassis.DeAccPerSecond;
-
-            speedChangingRate = Math.Max(0f, speedChangingRate);
-
-            var maxChange = speedChangingRate * elapsedSeconds;
-            var speedDifference = targetLimit - currentLimit;
-
-            if (Math.Abs(speedDifference) <= maxChange)
+            else if (cart.TransmitterControlEnable && cart.Transmitter_SC == 3)
             {
-                currentLimit = targetLimit;
+                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
             {
-                currentLimit += Math.Sign(speedDifference) * maxChange;
+                ResetMultiVehicle("idle-or-sc-mismatch");
+                LogTransmitterFleet("idle-or-sc-mismatch");
+                cart.CarStatu = "正常运行";
             }
 
-            // 计算当前8个电机目标速度中的最大绝对值。
-            var wheelMaxSpeed = 0f;
-
-            wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLFL));
-            wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLFR));
-            wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRFL));
-            wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRFR));
-            wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLRL));
-            wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLRR));
-            wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRRL));
-            wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRRR));
-
-            // 没有必要让平滑限速值高于当前所有车轮需要的速度。
-            if (targetLimit < wheelMaxSpeed &&
-                currentLimit > wheelMaxSpeed)
+            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[]
             {
-                currentLimit = wheelMaxSpeed;
-            }
+                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;
 
-            cart.SendThresSpeed = currentLimit;
-        }
-
-        // M层单车灯光：根据报警、驱动器、限速和电量状态生成红黄绿灯模式。
-        private void UpdateLightMode()
-        {
-            // 二级报警：红灯常亮。
+            //灯光控制
             if (cart.AlarmLevel == 2)
             {
+                //故障红灯常亮
                 cart.LightMode = 2;
-                return;
             }
-
-            // 一级报警：黄灯常亮。
-            if (cart.AlarmLevel == 1)
+            else if (cart.ThresSpeed != 1)
             {
+                //避障黄灯常亮
                 cart.LightMode = 3;
-                return;
             }
-
-            // 驱动轮未使能：黄灯闪烁。
-            if (!cart.WheelAbleState)
+            else if (!cart.WheelAbleState)
             {
                 FlipFlop(ref cart.LightMode, 500, 0, 3);
-                return;
             }
-
-            // C层正在进行限速：黄灯常亮。
-            if (Math.Abs(cart.ThresSpeed - 1f) > 0.001f)
+            else if (cart.Soc < cart.LowBatteryAlarmThreshold && cart.ElectricCurrent >= -10)
             {
                 cart.LightMode = 3;
-                return;
             }
-
-            // 电量低于预警值：黄灯常亮。
-            if (cart.Soc <= cart.LowBatteryAlarmThreshold)
+            else if (cart.ElectricCurrent < -10)
             {
-                cart.LightMode = 3;
-                return;
+                FlipFlop(ref cart.LightMode, 500, 0, 1);
+            }
+            else
+            {
+                FlipFlop(ref cart.LightMode, 500, 0, 1);
             }
-
-            // 正常运行：绿灯闪烁。
-            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

@@ -1,51 +1,13 @@
-// Medulla虚拟遥控器和夹臂控制
-using MDCSToolBox.Medulla.Chassis.MultiWheel;
+using System;
+using System.Collections.Generic;
+using System.Text;
 using CartActivator;
+using MDCSToolBox.Medulla.Chassis.MultiWheel;
 
 namespace MedullaAdapter
 {
-    // M层单车虚拟遥控器：使用父类提供的底盘控制界面。
-    public class Remote : MultiWheelRemote<DiverCartDefinition>
+    public class Remote:MultiWheelRemote<DiverCartDefinition>
     {
-        // 将父类虚拟遥控器界面输入统一转发到本车的ManualControl。
-        public override void ChassisOperation()
-        {
-            if (MultiVehicleMode.on)
-            {
-                MultiVehicleModeChassisLogic();
-                statusText = "单车版本不支持多车联动遥控";
-                return;
-            }
-
-            // 当前单车适配层只定义Normal、Crab和Spin三种模式。
-            // 禁止这些旧按钮绕过适配层直接修改底盘坐标偏置。
-            if (AckermannMode.on ||
-                SwayMode.on ||
-                XYThMode.on)
-            {
-                cart.Chassis?.PredefinedDriveStop();
-                statusText = "当前单车版本暂不支持阿克曼、斜行或全向模式";
-                return;
-            }
-
-            var mode = SpinMode.on
-                ? DiverCartDefinition.ManualControlMode.Spin
-                : CrabMode.on
-                    ? DiverCartDefinition.ManualControlMode.Crab
-                    : DiverCartDefinition.ManualControlMode.Normal;
-
-            cart.ManualControl(
-                mode,
-                SpeedPad.x,
-                SpeedPad.y,
-                FrontDirection.dval * 180,
-                SpeedThreshold.val);
-
-            statusText =
-                $"{mode}, x={SpeedPad.x:0.00}, " +
-                $"y={SpeedPad.y:0.00}, " +
-                $"speed={SpeedThreshold.val:0.00}";
-        }
         [AsControlItem(name = "夹抱速度", LayoutRow = 0, LayoutCol = 4)]
         public Throttle ArmSpeed;
 
@@ -54,22 +16,26 @@ namespace MedullaAdapter
 
         [AsControlItem(name = "夹抱关闭", LayoutRow = 1, LayoutCol = 2)]
         public Button Close;
-        // M层虚拟遥控器：控制左右夹臂同步打开或关闭。
+
+        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;
+                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;
+                cart.SpeedLeftArm = ArmSpeed.val * cart.ManualArmSpeedFac;
+                cart.SpeedRightArm = ArmSpeed.val * cart.ManualArmSpeedFac;
             }
             else
             {
@@ -77,10 +43,5 @@ namespace MedullaAdapter
                 cart.SpeedRightArm = 0;
             }
         }
-        // 单车不支持多车联动，误打开多车开关时主动停车。
-        public override void MultiVehicleModeChassisLogic()
-        {
-            cart.Chassis?.PredefinedDriveStop();
-        }
     }
 }

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/build/Medulla/plugins/MedullaAdapter.deps.json

@@ -1,23 +0,0 @@
-{
-  "runtimeTarget": {
-    "name": ".NETCoreApp,Version=v8.0",
-    "signature": ""
-  },
-  "compilationOptions": {},
-  "targets": {
-    ".NETCoreApp,Version=v8.0": {
-      "MedullaAdapter/1.0.0": {
-        "runtime": {
-          "MedullaAdapter.dll": {}
-        }
-      }
-    }
-  },
-  "libraries": {
-    "MedullaAdapter/1.0.0": {
-      "type": "project",
-      "serviceable": false,
-      "sha512": ""
-    }
-  }
-}

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/MedullaAdapter.csproj.nuget.dgspec.json

@@ -1,73 +0,0 @@
-{
-  "format": 1,
-  "restore": {
-    "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj": {}
-  },
-  "projects": {
-    "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj": {
-      "version": "1.0.0",
-      "restore": {
-        "projectUniqueName": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj",
-        "projectName": "MedullaAdapter",
-        "projectPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj",
-        "packagesPath": "C:\\Users\\CodexSandboxOffline\\.nuget\\packages\\",
-        "outputPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\obj\\",
-        "projectStyle": "PackageReference",
-        "fallbackFolders": [
-          "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages"
-        ],
-        "configFilePaths": [
-          "C:\\Users\\admin\\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": [
-          "net8.0"
-        ],
-        "sources": {
-          "C:\\Program Files (x86)\\Microsoft SDKs\\NuGetPackages\\": {},
-          "https://api.nuget.org/v3/index.json": {}
-        },
-        "frameworks": {
-          "net8.0": {
-            "targetAlias": "net8.0",
-            "projectReferences": {}
-          }
-        },
-        "warningProperties": {
-          "warnAsError": [
-            "NU1605"
-          ]
-        },
-        "restoreAuditProperties": {
-          "enableAudit": "true",
-          "auditLevel": "low",
-          "auditMode": "direct"
-        },
-        "SdkAnalysisLevel": "9.0.300"
-      },
-      "frameworks": {
-        "net8.0": {
-          "targetAlias": "net8.0",
-          "imports": [
-            "net461",
-            "net462",
-            "net47",
-            "net471",
-            "net472",
-            "net48",
-            "net481"
-          ],
-          "assetTargetFallback": true,
-          "warn": true,
-          "frameworkReferences": {
-            "Microsoft.NETCore.App": {
-              "privateAssets": "all"
-            }
-          },
-          "runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.316/PortableRuntimeIdentifierGraph.json"
-        }
-      }
-    }
-  }
-}

MedullaAdapter/obj/MedullaAdapter.csproj.nuget.g.props

@@ -1,16 +0,0 @@
-<?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)' == '' ">C:\Users\CodexSandboxOffline\.nuget\packages\</NuGetPackageRoot>
-    <NuGetPackageFolders Condition=" '$(NuGetPackageFolders)' == '' ">C:\Users\CodexSandboxOffline\.nuget\packages\;C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages</NuGetPackageFolders>
-    <NuGetProjectStyle Condition=" '$(NuGetProjectStyle)' == '' ">PackageReference</NuGetProjectStyle>
-    <NuGetToolVersion Condition=" '$(NuGetToolVersion)' == '' ">6.14.3</NuGetToolVersion>
-  </PropertyGroup>
-  <ItemGroup Condition=" '$(ExcludeRestorePackageImports)' != 'true' ">
-    <SourceRoot Include="C:\Users\CodexSandboxOffline\.nuget\packages\" />
-    <SourceRoot Include="C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages\" />
-  </ItemGroup>
-</Project>

MedullaAdapter/obj/MedullaAdapter.csproj.nuget.g.targets

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="utf-8" standalone="no"?>
-<Project ToolsVersion="14.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003" />

MedullaAdapter/obj/project.assets.json

@@ -1,79 +0,0 @@
-{
-  "version": 3,
-  "targets": {
-    "net8.0": {}
-  },
-  "libraries": {},
-  "projectFileDependencyGroups": {
-    "net8.0": []
-  },
-  "packageFolders": {
-    "C:\\Users\\CodexSandboxOffline\\.nuget\\packages\\": {},
-    "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages": {}
-  },
-  "project": {
-    "version": "1.0.0",
-    "restore": {
-      "projectUniqueName": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj",
-      "projectName": "MedullaAdapter",
-      "projectPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj",
-      "packagesPath": "C:\\Users\\CodexSandboxOffline\\.nuget\\packages\\",
-      "outputPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\obj\\",
-      "projectStyle": "PackageReference",
-      "fallbackFolders": [
-        "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages"
-      ],
-      "configFilePaths": [
-        "C:\\Users\\admin\\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": [
-        "net8.0"
-      ],
-      "sources": {
-        "C:\\Program Files (x86)\\Microsoft SDKs\\NuGetPackages\\": {},
-        "https://api.nuget.org/v3/index.json": {}
-      },
-      "frameworks": {
-        "net8.0": {
-          "targetAlias": "net8.0",
-          "projectReferences": {}
-        }
-      },
-      "warningProperties": {
-        "warnAsError": [
-          "NU1605"
-        ]
-      },
-      "restoreAuditProperties": {
-        "enableAudit": "true",
-        "auditLevel": "low",
-        "auditMode": "direct"
-      },
-      "SdkAnalysisLevel": "9.0.300"
-    },
-    "frameworks": {
-      "net8.0": {
-        "targetAlias": "net8.0",
-        "imports": [
-          "net461",
-          "net462",
-          "net47",
-          "net471",
-          "net472",
-          "net48",
-          "net481"
-        ],
-        "assetTargetFallback": true,
-        "warn": true,
-        "frameworkReferences": {
-          "Microsoft.NETCore.App": {
-            "privateAssets": "all"
-          }
-        },
-        "runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.316/PortableRuntimeIdentifierGraph.json"
-      }
-    }
-  }
-}

MedullaAdapter/obj/project.nuget.cache

@@ -1,8 +0,0 @@
-{
-  "version": 2,
-  "dgSpecHash": "b9v8vkN2ac8=",
-  "success": true,
-  "projectFilePath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj",
-  "expectedPackageFiles": [],
-  "logs": []
-}

README.md

@@ -1,169 +0,0 @@
-# MyParking 停车机器人
-
-[简体中文](README.md) | [English](README_en.md)
-
-## 重写说明
-
-本仓库是停车机器人控制软件的重写版本。当前不以一次性恢复全部旧功能为目标，而是按以下顺序重新建立可验证、可维护的能力：
-
-1. 先实现单台停车机器人小车的基本功能；
-2. 在单车闭环稳定后逐步增加停车作业功能；
-3. 再优化轨迹跟踪方法及其稳定性；
-4. 最后评估并实现多车通信、编队与协同控制。
-
-**当前处于第 1 阶段，开发重点是单车基本功能。** `PilotConfig.cs` 中由 `#if false` 包围的钻车、夹抱和多车参数仅作为历史代码或设计参考，不参与当前编译，也不表示相关功能已经可用。
-
-## 项目简介
-
-MyParking 是一个面向多轮停车机器人底盘的 C# 控制工程。仓库包含上层运动控制插件 `ClumsyPilot` 和下层硬件适配插件 `MedullaAdapter`，用于建立从单车运动指令到 MCU 串口桥、CAN/串口端口的基础链路。
-
-当前代码已经包含单车终点直线跟踪、前进/后退测试、PID 原地旋转、停止保护，以及 MCU 串口桥的托管封装和初始化流程。完整停车作业所需的驱动器协议、反馈解析、安全报警、夹抱执行和感知流程仍待实现或接入。
-
-## 当前范围
-
-| 范围 | 状态 | 说明 |
-| --- | --- | --- |
-| 单车几何控制器 | 已接入 | 根据公共配置创建 `MultiWheelGeometricController` |
-| 单车终点跟踪 | 已实现基础版本 | 使用直线路径，可测试前进和后退，到达或退出时发送零速度 |
-| 单车原地旋转 | 已实现基础版本 | 读取定位朝向并通过 PID 输出角速度，到位后停车 |
-| MCU 串口桥 | 已封装 | 支持打开、复位、状态/版本查询、端口配置、IO、串口与 CAN 收发/回调 |
-| MCU 初始化 | 已实现基础流程 | 默认使用 `COM4`、1 路 CAN 和 3 路串口配置 |
-| 驱动反馈与安全链路 | 待实现 | 驱动协议、轮速/舵角反馈、电池、急停和报警例程目前没有实际逻辑 |
-| 停车作业功能 | 待实现 | 钻车、轮胎识别、夹抱等旧参数当前被条件编译禁用 |
-| 多车能力 | 暂不实施 | 多车参数当前被条件编译禁用，待单车及跟踪能力稳定后再设计 |
-
-## 软件结构
-
-```text
-上层 Clumsy 运行环境
-        │
-        ▼
-ClumsyPilot：单车动作、轨迹跟踪、测试入口
-        │ 底盘速度指令
-        ▼
-Medulla 运行环境 / MedullaAdapter
-        │ P/Invoke
-        ▼
-mcu_serial_bridge.dll → MCU → CAN / Serial / Digital IO
-```
-
-两个工程均生成类库，仓库中没有独立的可执行入口：
-
-- `ClumsyPilot`：上层单车运动控制，目标框架为 .NET Standard 2.0；
-- `MedullaAdapter`：下层 MCU 和底盘适配，目标框架为 .NET 8.0。
-
-## 目录说明
-
-```text
-MyParking/
-├── ParkingRobot.sln                 # Visual Studio 解决方案
-├── ClumsyPilot/
-│   ├── AGV.cs                       # 上层 AGV 接口
-│   ├── ChassisController.cs         # 单车几何控制器配置
-│   ├── Movements.cs                 # 终点跟踪、等待、原地旋转动作
-│   ├── MovementTests.cs             # Clumsy 环境中的人工动作测试
-│   ├── PilotConfig.cs               # 当前单车配置及禁用的历史/预研配置
-│   ├── PilotDefinition.cs           # 车型尺寸和车号定义
-│   └── ref/                          # 上层框架引用程序集
-└── MedullaAdapter/
-    ├── DiverCartDefinition.cs       # 车型 IO、监控字段及 MCU 初始化
-    ├── MCUSerialBridgeCLR.cs        # 原生 MCU 串口桥的 C# 封装
-    ├── MCUSerialBridgeError.cs      # 错误码与诊断说明
-    ├── AlarmRoutine.cs              # 安全报警例程占位
-    ├── MCURoutine.cs                # MCU IO/反馈例程占位
-    ├── MotorRoutine.cs              # 电机控制例程占位
-    ├── Remote.cs                    # 遥控例程占位
-    └── ref/                          # 下层框架引用程序集
-```
-
-## 开发环境与依赖
-
-- Windows 开发/运行环境；当前硬件接入使用 COM 端口和原生 DLL；
-- Visual Studio 2022，或能够构建 .NET 8.0 与 .NET Standard 2.0 项目的 .NET SDK；
-- NuGet 包：`Newtonsoft.Json 13.0.3`、`System.Numerics.Vectors 4.6.1`；
-- `ClumsyPilot/ref` 和 `MedullaAdapter/ref` 中的内部框架程序集；
-- 实机运行所需的 `mcu_serial_bridge.dll`；该文件当前未包含在仓库中；
-- 能够加载 `ClumsyPilot.dll` 与 `MedullaAdapter.dll` 的 Clumsy/Medulla 宿主环境；宿主程序和部署配置当前未包含在仓库中。
-
-仓库中未发现 ROS/ROS 2、Docker 或独立仿真启动配置。
-
-## 编译
-
-在仓库根目录执行：
-
-```powershell
-dotnet restore ParkingRobot.sln
-dotnet build ParkingRobot.sln -c Debug
-```
-
-主要输出位置：
-
-```text
-ClumsyPilot/build/Clumsy/ClumsyPilot.dll
-MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll
-```
-
-当前源码已通过解决方案编译。现有警告为 `DiverCartDefinition.TransmitterLastTime` 尚未赋值，不影响程序集生成。
-
-## 运行与配置
-
-本仓库只生成插件类库，不能通过 `dotnet run` 独立启动。需要由匹配版本的 Clumsy/Medulla 宿主加载上述程序集。具体宿主版本、目录复制方式、配置文件位置和启动命令尚未随仓库提供，待补充。
-
-当前能够从代码确认的 MCU 默认初始化参数如下：
-
-| 参数 | 默认值 |
-| --- | --- |
-| MCU 端口 | `COM4` |
-| MCU 连接波特率 | `1000000` |
-| CAN 通道 | 1 路，`500000 bit/s`，重试时间 `10 ms` |
-| 串口通道 | 3 路，`9600 bit/s`，接收帧时间 `10 ms` |
-
-实机启动前应在宿主参数界面或对应配置中确认端口和硬件参数。首次调试建议架空驱动轮或使用安全测试区域，并准备可靠的急停手段；当前安全报警与急停反馈逻辑尚未完成，不能将软件停车作为唯一安全措施。
-
-## 单车功能验证
-
-`MovementTests.cs` 向 Clumsy 测试界面注册了以下人工测试：
-
-- `测试终点跟踪动作-前进`：选取起点和终点后执行前向直线跟踪；
-- `测试终点跟踪动作-后退`：以 180° 车身方向偏置执行后退跟踪；
-- `底盘旋转测试`：输入世界坐标系目标角度后执行 PID 原地旋转。
-
-这些是宿主环境内的动作测试，并非 `dotnet test` 自动化测试。实机验证前需要先确认定位数据、底盘速度接口、舵轮方向、速度单位和急停链路。
-
-## 开发路线
-
-### 阶段 1：单车基本功能（当前）
-
-- 打通上层动作、底盘控制、Medulla 适配和 MCU 通信链路；
-- 完成单车启停、直线前进/后退、原地旋转和停止保护；
-- 补齐驱动器命令、轮速与舵角反馈、IO、电池、急停和报警处理；
-- 建立可重复的仿真/台架/实车验证方法。
-
-### 阶段 2：增加停车作业功能
-
-- 在单车基础控制稳定后，逐项接入遥控、感知、钻车、夹抱和退出车辆等功能；
-- 每项功能分别完成参数定义、异常处理和实车验证，不直接启用旧的禁用代码。
-
-### 阶段 3：优化跟踪方法
-
-- 根据单车测试数据改进路径跟踪、速度规划、转向控制和到位判定；
-- 完善曲线、倒车、低速近目标等工况，并补充可复现的回归测试；
-- 在安全性、稳定性和可诊断性达到要求后冻结单车接口。
-
-### 阶段 4：考虑多车场景
-
-- 在单车接口稳定的前提下设计车辆身份、通信、心跳、超时和失联降级；
-- 再实现编队、同步动作、车间位姿校正和多车安全策略；
-- 多车预研参数需要重新评审，不以当前 `#if false` 代码作为完成依据。
-
-## 参与开发
-
-1. 修改前确认所属阶段，当前提交优先服务于单车基本功能；
-2. 保持 `ClumsyPilot` 与 `MedullaAdapter` 的职责边界，避免在上层动作中直接实现硬件协议；
-3. 新增硬件参数时注明单位、默认值、适用车型和安全范围；
-4. 提交前至少执行 `dotnet build ParkingRobot.sln`，并记录宿主测试或实车测试条件；
-5. 分支、代码评审和发布流程待项目团队补充。
-
-## 许可证
-
-仓库中暂未提供许可证文件。使用和分发范围请遵循公司内部规定。

README_en.md

@@ -1,169 +0,0 @@
-# MyParking Parking Robot
-
-[简体中文](README.md) | [English](README_en.md)
-
-## Rewrite Notice
-
-This repository is a rewrite of the parking-robot control software. The goal is not to restore every legacy feature at once. Development follows this sequence:
-
-1. Implement the basic functions of one parking robot first;
-2. Add parking-operation features after the single-robot loop is stable;
-3. Improve the tracking method and its robustness;
-4. Evaluate and implement multi-robot communication, formation, and coordination last.
-
-**The project is currently in Stage 1 and focuses on basic single-robot functions.** The vehicle-entry, clamping, and multi-robot parameters enclosed by `#if false` in `PilotConfig.cs` are retained only as legacy or design references. They are excluded from the current build and do not indicate available features.
-
-## Overview
-
-MyParking is a C# control project for a multi-wheel parking-robot chassis. It contains an upper-layer motion-control plugin, `ClumsyPilot`, and a lower-layer hardware adapter, `MedullaAdapter`. Together, they establish the basic path from single-robot motion commands to an MCU serial bridge and its CAN, serial, and digital-I/O ports.
-
-The current code includes basic straight-line destination tracking, forward and reverse tests, PID-based in-place rotation, stop-on-exit handling, a managed wrapper for the MCU bridge, and its initialization sequence. Driver protocols, feedback parsing, safety alarms, clamping actuators, perception, and the complete parking workflow still need to be implemented or integrated.
-
-## Current Scope
-
-| Area | Status | Notes |
-| --- | --- | --- |
-| Single-robot geometric controller | Integrated | Creates a `MultiWheelGeometricController` from shared configuration |
-| Destination tracking | Basic version implemented | Tracks a straight path forward or backward and sends zero speed when finished or interrupted |
-| In-place rotation | Basic version implemented | Reads the localization heading and produces angular speed through a PID controller |
-| MCU serial bridge | Wrapped | Supports open/reset, state/version queries, port configuration, I/O, serial, CAN, and callbacks |
-| MCU initialization | Basic flow implemented | Defaults to `COM4`, one CAN channel, and three serial channels |
-| Driver feedback and safety chain | To be implemented | Driver protocol, wheel/steering feedback, battery, emergency-stop, and alarm routines contain no operational logic yet |
-| Parking-operation features | To be implemented | Vehicle entry, tire recognition, and clamping parameters are currently excluded by conditional compilation |
-| Multi-robot features | Deferred | Multi-robot parameters are excluded and will be reconsidered after single-robot tracking is stable |
-
-## Software Structure
-
-```text
-Upper-layer Clumsy runtime
-        │
-        ▼
-ClumsyPilot: single-robot actions, tracking, and test entries
-        │ chassis velocity commands
-        ▼
-Medulla runtime / MedullaAdapter
-        │ P/Invoke
-        ▼
-mcu_serial_bridge.dll → MCU → CAN / Serial / Digital IO
-```
-
-Both projects build as libraries; this repository contains no standalone executable entry point:
-
-- `ClumsyPilot`: upper-layer single-robot motion control targeting .NET Standard 2.0;
-- `MedullaAdapter`: lower-layer MCU and chassis adapter targeting .NET 8.0.
-
-## Repository Layout
-
-```text
-MyParking/
-├── ParkingRobot.sln                 # Visual Studio solution
-├── ClumsyPilot/
-│   ├── AGV.cs                       # Upper-layer AGV interface
-│   ├── ChassisController.cs         # Single-robot geometric-controller setup
-│   ├── Movements.cs                 # Destination tracking, delay, and rotation actions
-│   ├── MovementTests.cs             # Manual action tests in the Clumsy runtime
-│   ├── PilotConfig.cs               # Active single-robot and disabled legacy/R&D settings
-│   ├── PilotDefinition.cs           # Vehicle dimensions and vehicle-number definition
-│   └── ref/                          # Upper-layer framework assemblies
-└── MedullaAdapter/
-    ├── DiverCartDefinition.cs       # Vehicle I/O, monitoring fields, and MCU initialization
-    ├── MCUSerialBridgeCLR.cs        # C# wrapper for the native MCU serial bridge
-    ├── MCUSerialBridgeError.cs      # Error codes and diagnostic descriptions
-    ├── AlarmRoutine.cs              # Placeholder for safety and alarm routines
-    ├── MCURoutine.cs                # Placeholder for MCU I/O and feedback routines
-    ├── MotorRoutine.cs              # Placeholder for motor-control routines
-    ├── Remote.cs                    # Placeholder for remote-control routines
-    └── ref/                          # Lower-layer framework assemblies
-```
-
-## Development Environment and Dependencies
-
-- Windows development/runtime environment; current hardware access uses a COM port and a native DLL;
-- Visual Studio 2022, or a .NET SDK capable of building .NET 8.0 and .NET Standard 2.0 projects;
-- NuGet packages: `Newtonsoft.Json 13.0.3` and `System.Numerics.Vectors 4.6.1`;
-- Internal framework assemblies under `ClumsyPilot/ref` and `MedullaAdapter/ref`;
-- `mcu_serial_bridge.dll` for physical-hardware operation; it is not currently included in this repository;
-- A compatible Clumsy/Medulla host capable of loading `ClumsyPilot.dll` and `MedullaAdapter.dll`. The host application and deployment configuration are not included.
-
-No ROS/ROS 2, Docker, or standalone simulation launch configuration was found in the repository.
-
-## Build
-
-Run from the repository root:
-
-```powershell
-dotnet restore ParkingRobot.sln
-dotnet build ParkingRobot.sln -c Debug
-```
-
-Primary output locations:
-
-```text
-ClumsyPilot/build/Clumsy/ClumsyPilot.dll
-MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll
-```
-
-The current source builds successfully. The remaining warning reports that `DiverCartDefinition.TransmitterLastTime` is never assigned; it does not prevent assembly generation.
-
-## Runtime and Configuration
-
-This repository produces plugin libraries and cannot be started independently with `dotnet run`. A compatible Clumsy/Medulla host must load the assemblies above. The exact host version, copy locations, configuration-file paths, and startup command have not been provided and remain to be documented.
-
-MCU defaults confirmed from the current source are:
-
-| Setting | Default |
-| --- | --- |
-| MCU port | `COM4` |
-| MCU connection baud rate | `1000000` |
-| CAN channels | One at `500000 bit/s`, with a `10 ms` retry time |
-| Serial channels | Three at `9600 bit/s`, with a `10 ms` receive-frame time |
-
-Confirm the port and hardware parameters in the host configuration before physical operation. For initial tests, lift the drive wheels or use a controlled safety area and provide a reliable physical emergency stop. The alarm and emergency-stop feedback logic is incomplete, so software stop commands must not be the only safety measure.
-
-## Single-Robot Validation
-
-`MovementTests.cs` registers these manual tests in the Clumsy test interface:
-
-- `测试终点跟踪动作-前进`: select a source and destination for forward straight-line tracking;
-- `测试终点跟踪动作-后退`: track backward with a 180-degree vehicle-direction offset;
-- `底盘旋转测试`: enter a target world-frame heading and run PID-based in-place rotation.
-
-These are host-integrated action tests, not an automated `dotnet test` suite. Before physical testing, verify localization data, the chassis velocity interface, steering direction, speed units, and the emergency-stop chain.
-
-## Development Roadmap
-
-### Stage 1: Basic Single-Robot Functions (Current)
-
-- Connect upper-layer actions, chassis control, the Medulla adapter, and MCU communication;
-- Complete single-robot start/stop, straight forward/reverse motion, in-place rotation, and stop protection;
-- Implement driver commands, wheel and steering feedback, I/O, battery, emergency-stop, and alarm handling;
-- Establish repeatable simulation, bench, and physical-vehicle validation procedures.
-
-### Stage 2: Add Parking-Operation Features
-
-- After basic single-robot control is stable, integrate remote control, perception, vehicle entry, clamping, and vehicle-exit functions one at a time;
-- Define parameters, exception handling, and physical validation for each feature instead of directly enabling legacy disabled code.
-
-### Stage 3: Improve Tracking
-
-- Use single-robot test data to improve path tracking, speed planning, steering control, and arrival detection;
-- Cover curves, reverse motion, and low-speed near-target conditions, with reproducible regression tests;
-- Freeze the single-robot interfaces only after safety, stability, and diagnostics meet project requirements.
-
-### Stage 4: Consider Multi-Robot Scenarios
-
-- Once the single-robot interfaces are stable, design vehicle identity, communication, heartbeats, timeouts, and disconnect fallback behavior;
-- Then implement formation control, synchronized actions, relative-pose correction, and multi-robot safety policies;
-- Re-review all legacy multi-robot parameters; the current `#if false` block is not evidence of completed functionality.
-
-## Contributing
-
-1. Confirm the applicable roadmap stage before making a change; current contributions should prioritize basic single-robot functions.
-2. Preserve the boundary between `ClumsyPilot` and `MedullaAdapter`; hardware protocols should not be implemented directly in upper-layer actions.
-3. Document units, defaults, applicable vehicle types, and safe ranges for new hardware parameters.
-4. Run at least `dotnet build ParkingRobot.sln` before submitting and record the host or physical-test conditions used.
-5. The team still needs to document its branch, code-review, and release processes.
-
-## License
-
-No license file is currently included. Use and distribution must follow internal company policy.

Shared/ChassisCommand.cs

@@ -1,183 +0,0 @@
-// 纯数据层：只描述坐标、速度和命令
-// 定义二维坐标、位姿、速度、车队布局和单车底盘命令。
-// Shared层统一使用SI单位：位置m、线速度m/s、角度rad、角速度rad/s。
-// 车体坐标系采用右手系：X向前、Y向左、逆时针角度和角速度为正。
-// 命名约定：XxxInYyy表示Xxx在Yyy坐标系中的表达。
-
-namespace MyParking.Shared
-{
-    /// <summary>
-    /// 二维坐标点，X、Y单位均为米。
-    /// </summary>
-    public readonly struct Point2D
-    {
-        public Point2D(double xMeters, double yMeters)
-        {
-            XMeters = xMeters;
-            YMeters = yMeters;
-        }
-
-        public double XMeters { get; }
-
-        public double YMeters { get; }
-
-        public static Point2D Zero => new Point2D(0.0, 0.0);
-    }
-
-    /// <summary>
-    /// 二维局部坐标系在父坐标系中的位姿。
-    /// 位置单位为米，朝向单位为弧度，逆时针为正。
-    /// 具体父子关系由变量名称说明，例如RadarPoseInBody。
-    /// </summary>
-    public readonly struct Pose2D
-    {
-        public Pose2D(
-            double xMeters,
-            double yMeters,
-            double yawRadians)
-        {
-            XMeters = xMeters;
-            YMeters = yMeters;
-            YawRadians = yawRadians;
-        }
-
-        public double XMeters { get; }
-
-        public double YMeters { get; }
-
-        public double YawRadians { get; }
-
-        public Point2D Position =>
-            new Point2D(XMeters, YMeters);
-
-        public static Pose2D Identity =>
-            new Pose2D(0.0, 0.0, 0.0);
-    }
-
-    /// <summary>
-    /// 二维刚体速度。
-    /// 线速度单位为m/s，角速度单位为rad/s。
-    /// 速度所属坐标系由持有该Twist2D的外层类型或变量名称确定。
-    /// </summary>
-    public readonly struct Twist2D
-    {
-        public Twist2D(
-            double vxMetersPerSecond,
-            double vyMetersPerSecond,
-            double omegaRadiansPerSecond)
-        {
-            VxMetersPerSecond = vxMetersPerSecond;
-            VyMetersPerSecond = vyMetersPerSecond;
-            OmegaRadiansPerSecond = omegaRadiansPerSecond;
-        }
-
-        public double VxMetersPerSecond { get; }
-
-        public double VyMetersPerSecond { get; }
-
-        public double OmegaRadiansPerSecond { get; }
-
-        public static Twist2D Zero =>
-            new Twist2D(0.0, 0.0, 0.0);
-    }
-
-    /// <summary>
-    /// 发送给单辆车的车体坐标系速度命令。
-    /// </summary>
-    public readonly struct ChassisCommand
-    {
-        public ChassisCommand(
-            int vehicleId,
-            Twist2D bodyTwist)
-        {
-            VehicleId = vehicleId;
-            BodyTwist = bodyTwist;
-        }
-
-        public int VehicleId { get; }
-
-        /// <summary>
-        /// 单车车体坐标系速度：X向前、Y向左、逆时针旋转为正。
-        /// </summary>
-        public Twist2D BodyTwist { get; }
-
-        /// <summary>
-        /// 创建指定车辆的停止命令。
-        /// </summary>
-        public static ChassisCommand Stop(int vehicleId)
-        {
-            return new ChassisCommand(
-                vehicleId,
-                Twist2D.Zero);
-        }
-    }
-
-    /// <summary>
-    /// 单辆车的车体坐标系在车队坐标系中的位姿。
-    /// </summary>
-    public readonly struct VehicleLayout
-    {
-        public VehicleLayout(
-            int vehicleId,
-            Pose2D poseInFleet)
-        {
-            VehicleId = vehicleId;
-            PoseInFleet = poseInFleet;
-        }
-
-        public int VehicleId { get; }
-
-        public Pose2D PoseInFleet { get; }
-    }
-
-    /// <summary>
-    /// 车队整体运动命令，速度分量均在车队坐标系中表达。
-    /// </summary>
-    public readonly struct FleetMotionCommand
-    {
-        public FleetMotionCommand(
-            Point2D referencePointInFleet,
-            Twist2D twistAtReferencePoint)
-        {
-            ReferencePointInFleet = referencePointInFleet;
-            TwistAtReferencePoint = twistAtReferencePoint;
-        }
-
-        /// <summary>
-        /// 速度命令对应的参考点，也可作为自定义旋转中心。
-        /// </summary>
-        public Point2D ReferencePointInFleet { get; }
-
-        /// <summary>
-        /// 参考点处的车队速度。
-        /// </summary>
-        public Twist2D TwistAtReferencePoint { get; }
-
-        /// <summary>
-        /// 创建绕指定中心原地旋转的车队命令。
-        /// </summary>
-        public static FleetMotionCommand RotateAround(
-            Point2D rotationCenterInFleet,
-            double omegaRadiansPerSecond)
-        {
-            return new FleetMotionCommand(
-                rotationCenterInFleet,
-                new Twist2D(
-                    0.0,
-                    0.0,
-                    omegaRadiansPerSecond));
-        }
-
-        /// <summary>
-        /// 创建车队停止命令。
-        /// </summary>
-        public static FleetMotionCommand Stop()
-        {
-            return new FleetMotionCommand(
-                Point2D.Zero,
-                Twist2D.Zero);
-        }
-    }
-
-
-}

Shared/FleetKinematics.cs

@@ -1 +0,0 @@
-// 把车队整体速度分解为每辆车的局部速度

Shared/FrameTransform2D.cs

@@ -1,177 +0,0 @@
-// 车体、运动、车队坐标系之间的转换
-using System;
-
-namespace MyParking.Shared
-{
-    /// <summary>
-    /// 提供二维刚体坐标系之间的点、向量、位姿和速度变换。
-    /// 坐标系采用X向前、Y向左、逆时针为正的右手系。
-    /// </summary>
-    public static class FrameTransform2D
-    {
-        private const double TwoPi = 2.0 * Math.PI;
-        /// <summary>
-        /// 将角度归一化到[-π, π)范围。
-        /// </summary>
-        public static double NormalizeAngle(double angleRadians)
-        {
-
-            if (double.IsNaN(angleRadians) || double.IsInfinity(angleRadians))
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(angleRadians),
-                    "角度必须是有限数值。");
-            }
-            angleRadians %= TwoPi;
-            if (angleRadians >= Math.PI)
-                angleRadians -= TwoPi;
-            if (angleRadians < -Math.PI)
-                angleRadians += TwoPi;
-            return angleRadians;
-
-        }
-
-        /// <summary>
-        /// 计算从current到target的最短角度差。
-        /// 返回正值表示逆时针旋转。
-        /// </summary>
-        public static double ShortestAngleDifference(
-            double targetRadians,
-            double currentRadians)
-        {
-            return NormalizeAngle(targetRadians - currentRadians);
-        }
-
-        /// <summary>
-        /// 将源坐标系中的点变换到目标坐标系。
-        /// sourcePoseInTarget表示源坐标系在目标坐标系中的位姿。
-        /// </summary>
-        public static Point2D TransformPoint(
-            Pose2D sourcePoseInTarget,
-            Point2D pointInSource)
-        {
-            var cos = Math.Cos(sourcePoseInTarget.YawRadians);
-            var sin = Math.Sin(sourcePoseInTarget.YawRadians);
-
-            return new Point2D(
-                sourcePoseInTarget.XMeters +
-                cos * pointInSource.XMeters -
-                sin * pointInSource.YMeters,
-
-                sourcePoseInTarget.YMeters +
-                sin * pointInSource.XMeters +
-                cos * pointInSource.YMeters);
-        }
-
-        /// <summary>
-        /// 将目标坐标系中的点反向变换到源坐标系。
-        /// </summary>
-        public static Point2D InverseTransformPoint(
-            Pose2D sourcePoseInTarget,
-            Point2D pointInTarget)
-        {
-            var dx = pointInTarget.XMeters - sourcePoseInTarget.XMeters;
-
-            var dy = pointInTarget.YMeters - sourcePoseInTarget.YMeters;
-
-            var cos = Math.Cos(sourcePoseInTarget.YawRadians);
-            var sin = Math.Sin(sourcePoseInTarget.YawRadians);
-
-            return new Point2D(
-                cos * dx + sin * dy,
-                -sin * dx + cos * dy);
-        }
-
-        /// <summary>
-        /// 将源坐标系中的向量旋转到目标坐标系。
-        /// 向量没有位置，因此不叠加平移量。
-        /// </summary>
-        public static Point2D TransformVector(
-            Pose2D sourcePoseInTarget,
-            Point2D vectorInSource)
-        {
-            var cos = Math.Cos(sourcePoseInTarget.YawRadians);
-            var sin = Math.Sin(sourcePoseInTarget.YawRadians);
-
-            return new Point2D(
-                cos * vectorInSource.XMeters -
-                sin * vectorInSource.YMeters,
-
-                sin * vectorInSource.XMeters +
-                cos * vectorInSource.YMeters);
-        }
-
-        /// <summary>
-        /// 组合两级坐标变换。
-        /// parentFromMiddle表示middle在parent中的位姿；
-        /// middleFromChild表示child在middle中的位姿；
-        /// 返回child在parent中的位姿。
-        /// </summary>
-        public static Pose2D Compose(
-            Pose2D parentFromMiddle,
-            Pose2D middleFromChild)
-        {
-            var childPositionInParent = TransformPoint(
-                parentFromMiddle,
-                middleFromChild.Position);
-
-            return new Pose2D(
-                childPositionInParent.XMeters,
-                childPositionInParent.YMeters,
-                NormalizeAngle(
-                    parentFromMiddle.YawRadians +
-                    middleFromChild.YawRadians));
-        }
-
-        /// <summary>
-        /// 对坐标变换求逆。
-        /// 输入child在parent中的位姿，返回parent在child中的位姿。
-        /// </summary>
-        public static Pose2D Inverse(Pose2D childPoseInParent)
-        {
-            var cos = Math.Cos(childPoseInParent.YawRadians);
-            var sin = Math.Sin(childPoseInParent.YawRadians);
-
-            return new Pose2D(
-                -cos * childPoseInParent.XMeters -
-                sin * childPoseInParent.YMeters,
-
-                sin * childPoseInParent.XMeters -
-                cos * childPoseInParent.YMeters,
-
-                NormalizeAngle(
-                    -childPoseInParent.YawRadians));
-        }
-
-
-        /// <summary>
-        /// 将源坐标系中的位姿变换到目标坐标系。
-        /// </summary>
-        public static Pose2D TransformPose(
-            Pose2D sourcePoseInTarget,
-            Pose2D poseInSource)
-        {
-            return Compose(sourcePoseInTarget, poseInSource);
-        }
-
-        /// <summary>
-        /// 转换同一物理参考点处的速度表达坐标系。
-        /// 只旋转线速度，角速度保持不变。
-        /// </summary>
-        public static Twist2D TransformTwistAtSamePoint(
-            Pose2D sourcePoseInTarget,
-            Twist2D twistInSource)
-        {
-            var linearVelocityInTarget = TransformVector(
-                sourcePoseInTarget,
-                new Point2D(
-                    twistInSource.VxMetersPerSecond,
-                    twistInSource.VyMetersPerSecond));
-
-            return new Twist2D(
-                linearVelocityInTarget.XMeters,
-                linearVelocityInTarget.YMeters,
-                twistInSource.OmegaRadiansPerSecond);
-        }
-    }
-}

Shared/MultiWheelChassisAdapter.cs

@@ -1,289 +0,0 @@
-// 将统一命令转换为原 Chassis API 调用
-using System;
-using CommonUsage.Chassis;
-
-namespace MyParking.Shared
-{
-    /// <summary>
-    /// 将统一的单车车体速度命令转换为旧版MultiWheelChassis调用。
-    /// 车体坐标系固定为X向前、Y向左、逆时针为正。
-    /// </summary>
-    public sealed class MultiWheelChassisAdapter
-    {
-        #region 辅助内容
-        private const double RadiansToDegrees = 180.0 / Math.PI;
-        private const float BiasTolerance = 0.001f;
-        private readonly MultiWheelChassis _chassis;
-        /// <summary>
-        /// 当前适配器对应的车辆编号。
-        /// </summary>
-        public int VehicleId { get; }
-
-        /// <summary>
-        /// 检查旧底盘是否仍处于无偏置的真实车体坐标系。
-        /// </summary>
-        private void EnsureBodyFrameIsActive()
-        {
-            var bias = _chassis.GetOriginBias();
-
-            if (Math.Abs(bias.X) <= BiasTolerance &&
-                Math.Abs(bias.Y) <= BiasTolerance &&
-                Math.Abs(bias.Z) <= BiasTolerance)
-            {
-                return;
-            }
-
-            throw new InvalidOperationException(
-                "MultiWheelChassis的坐标偏置在适配器创建后被修改。" +
-                $"当前偏置为X={bias.X}, Y={bias.Y}, Th={bias.Z}°。" +
-                "请不要再调用DirectionAngle或SetOriginBias控制蟹行。");
-        }
-        /// <summary>
-        /// 检查底盘命令是否包含无效数值。
-        /// </summary>
-        private static void ValidateTwist(Twist2D twist)
-        {
-            ValidateFinite(
-                twist.VxMetersPerSecond,
-                nameof(twist.VxMetersPerSecond));
-
-            ValidateFinite(
-                twist.VyMetersPerSecond,
-                nameof(twist.VyMetersPerSecond));
-
-            ValidateFinite(
-                twist.OmegaRadiansPerSecond,
-                nameof(twist.OmegaRadiansPerSecond));
-        }
-        /// <summary>
-        /// 检查数值是否为有限值。
-        /// </summary>
-        private static void ValidateFinite(
-            double value,
-            string parameterName)
-        {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "底盘速度命令不能是NaN或无穷大。");
-            }
-
-            if (value > float.MaxValue ||
-                value < -float.MaxValue)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "底盘速度命令超过float可表示范围。");
-            }
-        }
-
-        /// <summary>
-        /// 获取最近一次底盘运动分解失败原因。
-        /// </summary>
-        public string LastFailureReason =>
-            _chassis.LastMotionDecomposeFailureReason;
-        #endregion
-
-        /// <summary>
-        /// 将旧底盘的原点偏置恢复为真实单车车体坐标系。
-        /// </summary>
-        public void ResetToBodyFrame()
-        {
-            _chassis.SetOriginBias(
-                x: 0.0f,
-                y: 0.0f,
-                th: 0.0f);
-        }
-        public MultiWheelChassisAdapter(MultiWheelChassis chassis, int vehicleId)
-        {
-            _chassis = chassis ?? throw new ArgumentNullException(nameof(chassis));
-            if (vehicleId <= 0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(vehicleId),
-                    "车辆编号必须大于零。");
-            }
-            VehicleId = vehicleId;
-#pragma warning disable CS0612, CS0618
-            var wheels = _chassis.GetSteerWheels();
-#pragma warning restore CS0612, CS0618
-
-            if (wheels.Count == 0)
-            {
-                throw new InvalidOperationException(
-                    "MultiWheelChassis尚未完成舵轮初始化，" +
-                    "不能创建底盘适配器。");
-            }
-            // 禁用旧版DirectionAngle/ZeroDirection坐标偏置，
-            // 保证SendXYThSpeed直接使用真实车体坐标系。
-            ResetToBodyFrame();
-        }
-
-
-        /// <summary>
-        /// 将车体坐标系速度命令发送给多舵轮底盘。
-        /// </summary>
-        public bool Send(ChassisCommand command, TimeSpan? interval = null)
-        {
-            if (command.VehicleId != VehicleId)
-            {
-                throw new InvalidOperationException(
-                    $"命令车辆编号{command.VehicleId}与适配器车辆编号" +
-                    $"{VehicleId}不一致。");
-            }
-            ValidateTwist(command.BodyTwist);
-            // 防止其他旧逻辑再次调用DirectionAngle或
-            // SetOriginBias改变底盘坐标语义。
-            EnsureBodyFrameIsActive();
-            var vxMetersPerSecond =
-                            (float)command.BodyTwist.VxMetersPerSecond;
-            var vyMetersPerSecond =
-                (float)command.BodyTwist.VyMetersPerSecond;
-            var omegaDegreesPerSecond =
-                (float)(
-                    command.BodyTwist.OmegaRadiansPerSecond *
-                    RadiansToDegrees);
-            var success = _chassis.SendXYThSpeed(
-                                vxMetersPerSecond,
-                                vyMetersPerSecond,
-                                omegaDegreesPerSecond,
-                                interval);
-            if (!success)
-            {
-                // 防止分解失败后继续执行上一条运动命令。
-                _chassis.PredefinedDriveStop();
-            }
-            return success;
-        }
-        /// <summary>
-        /// 按底盘减速度配置平滑停车，需要在控制周期中持续调用。
-        /// </summary>
-        public void RampStop(TimeSpan? interval = null)
-        {
-            _chassis.RampStop(interval);
-        }
-        /// <summary>
-        /// 立即将所有驱动轮速度下发为零。
-        /// </summary>
-        public void StopImmediately()
-        {
-            _chassis.PredefinedDriveStop();
-        }
-
-        /// <summary>
-        /// 停车并将所有舵轮转到指定的车体角度。
-        /// 只调整舵轮角度，不产生车辆线速度。
-        /// </summary>
-        public bool PrepareParallelDirection(
-            double directionRadians)
-        {
-            EnsureBodyFrameIsActive();
-            var targetDegrees = (float)(FrameTransform2D.NormalizeAngle(directionRadians) *
-                                    RadiansToDegrees);
-
-#pragma warning disable CS0612, CS0618
-            var wheels = _chassis.GetSteerWheels();
-#pragma warning restore CS0612, CS0618
-
-            // 没有舵轮时不能认为预对齐成功。
-            if (wheels.Count == 0)
-            {
-                return false;
-            }
-
-            // 先检查所有舵轮能否到达目标机械角度。
-            foreach (var wheel in wheels)
-            {
-                if (targetDegrees < wheel.AngleLowerLimit ||
-                    targetDegrees > wheel.AngleUpperLimit)
-                {
-                    return false;
-                }
-            }
-
-            // 模式切换前立即停止驱动轮。
-            _chassis.PredefinedDriveStop();
-            // 检查完成后再统一下发，避免只转动一部分舵轮。
-            foreach (var wheel in wheels)
-            {
-                wheel.WriteAngle(targetDegrees);
-            }
-
-            return true;
-        }
-
-        /// <summary>
-        /// 检查所有舵轮是否已经对准给定方向。
-        /// </summary>
-        public bool AreParallelWheelsAligned(
-            double directionRadians,
-            double toleranceRadians)
-        {
-            if (double.IsNaN(toleranceRadians) ||
-        double.IsInfinity(toleranceRadians) ||
-        toleranceRadians < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(toleranceRadians),
-                    "舵轮到位容差必须是非负有限值。");
-            }
-
-            EnsureBodyFrameIsActive();
-            var targetDegrees = (float)(
-                FrameTransform2D.NormalizeAngle(directionRadians) *
-                180.0 / Math.PI);
-
-            var toleranceDegrees = (float)(
-                Math.Abs(toleranceRadians) *
-                180.0 / Math.PI);
-
-#pragma warning disable CS0612, CS0618
-            var wheels = _chassis.GetSteerWheels();
-#pragma warning restore CS0612, CS0618
-
-            foreach (var wheel in wheels)
-            {
-                var angleErrorDegrees = targetDegrees - wheel.ReadAngle();
-
-                if (Math.Abs(angleErrorDegrees) >
-                    toleranceDegrees)
-                {
-                    return false;
-                }
-            }
-
-            return true;
-        }
-
-        /// <summary>
-        /// 停车并将舵轮预对齐到原地自转方向。
-        /// 返回是否成功生成舵轮目标。
-        /// </summary>
-        public bool PrepareSpin(
-            TimeSpan? interval = null)
-        {
-            EnsureBodyFrameIsActive();
-            _chassis.PredefinedDriveStop();
-
-            var success =
-                _chassis.SendRotateMotion(
-                    0.0f,
-                    interval);
-
-            if (!success)
-            {
-                _chassis.PredefinedDriveStop();
-            }
-            return success;
-        }
-        /// <summary>
-        /// 所有舵轮是否已对齐到原地自转方向。
-        /// </summary>
-        public bool AreSpinWheelsAligned => _chassis.LastRotateAligned;
-
-
-
-    }
-}

Shared/SteeringConstraintManager.cs

@@ -1,225 +0,0 @@
-// 然后因为我这个车的舵轮是有正负120°限制的，所以有这种情况，比如我给了+90°让它进入蟹行模式，然后我在蟹行模式我去追一个相对于车左侧方向+40°的轨迹，那这时候就超过限制了，那我这是要做一个什么措施吗，比如我能想到的就是达到这个限位要么就停车打印说明到达限位，安全措施；要么就是我在追这个轨迹的过程中知道我大概可能超过限位，然后我提前停下，自转，调整为车头朝向为轨迹的方向，也就是自转调整到我车当前模式的初始姿态方向这种；你有什么好的建议吗，其他的建议吗，或者有什么可行的方法，因为实际的过程还是需要考虑这个限位的问题的
-
-
-
-
-
-
-// 9:49
-
-
-
-
-
-
-// 这个问题必须考虑，但“目标轮角超过 +120°”不一定意味着命令无法执行。对于舵轮，首先要利用“轮角反向、轮速反向”的等价关系，然后才考虑停车和车身重定向。
-// 1. +130° 不一定超过限位后就无法运动
-// 你的例子是：
-// 蟹行基准方向：+90°
-// 轨迹又向该方向增加：+40°
-// 期望运动方向：+130°
-// 舵轮限制：[-120°, +120°]
-// 直接使用：
-// 轮角：+130°
-// 轮速：+0.3 m/s
-// 确实超过了 +120°。
-// 但它有一个完全等价的表达：
-// 轮角：-50°
-// 轮速：-0.3 m/s
-// 因为：
-// (θ, v) ≡ (θ - 180°, -v)
-// 即：
-// (+130°, +0.3) ≡ (-50°, -0.3)
-// 虽然轮子的机械朝向和转速符号变了，但轮胎与地面的速度向量完全相同。
-// 所以第一层措施应当是：
-// 先尝试期望角度
-// 如果超过限位
-//     尝试角度±180°
-//     同时将轮速取反
-// 如果等价角度满足限位
-//     使用等价解
-// 否则才判定无法执行
-// 你当前的 CommonUsage.dll 中已经有类似的舵角反解逻辑，SendXYThSpeed() 返回 bool 也是为了告诉上层运动分解是否成功。
-// 2. 你的±120°结构在静态上覆盖了所有运动方向
-// 因为：
-// 可转范围总宽度 = 240°
-// 大于 180°。
-// 只要驱动轮允许正反转，那么对于任意二维速度方向，通常都能在下面两种表达中找到一个合法值：
-// (θ, +v)
-// (θ ± 180°, -v)
-// 因此单纯从“某一时刻目标速度方向”看，+130° 可以用 -50° 表达，不一定需要旋转车身。
-// 真正困难的是动态过程：
-// 当前轮角：+90°
-// 新目标等价轮角：-50°
-// 需要转动：-140°
-// 虽然最终角度合法，但舵轮需要转过很大的角度。在舵轮没有到位前，不能继续以正常速度驱动车辆，否则实际运动方向会与轨迹命令不一致。
-// 所以核心问题不是“有没有合法终态”，而是：
-// 从当前轮角切换到合法目标轮角的过程是否安全、连续。
-
-// 3. 建议采用四层处理策略
-// 第一层：等价舵角选择
-// 为每一个舵轮计算速度向量：
-// Vix = Vx - ω·yi
-// Viy = Vy + ω·xi
-// 然后：
-// θi = atan2(Viy, Vix)
-// si = sqrt(Vix² + Viy²)
-// 尝试候选解：
-// 候选1：(θi, si)
-// 候选2：(θi + 180°, -si)
-// 候选3：(θi - 180°, -si)
-// 过滤掉超过 [-120°, +120°] 的候选，再选择相对当前机械轮角转动最小的候选。
-// 这里计算机械转角距离时，不要简单把差值归一化成最短圆周角，因为舵轮不能无限旋转。对于有限机械区间，应当直接比较：
-// Math.Abs(candidateAngle - currentMechanicalAngle)
-// 第二层：软限位
-// 不要把 ±120° 当作正常工作的边界，建议设置软限位，例如：
-// 物理硬限位：[-120°, +120°]
-// 软件工作限位：[-105°, +105°]
-// 预警区域：[105°, 120°]
-// 具体余量需要实车标定，初期可以保留 10°～20°。
-// 状态可以分成：
-// |θ| < 100°       正常
-// 100°～105°       接近限位，开始规划
-// 105°～120°       限速或停止切换
-// >120°            禁止下发并报警
-// 软限位的意义是给控制器留出制动、舵轮响应和误差空间。
-// 第三层：舵轮转向期间限制驱动速度
-// 如果新的合法舵角与当前舵角相差很大，例如：
-// 当前：+90°
-// 目标：-50°
-// 差值：140°
-// 不应当马上正常行驶，应进入：
-// 减速
-//   ↓
-// 驱动速度降为0
-//   ↓
-// 舵轮转到-50°
-//   ↓
-// 确认所有舵轮到位
-//   ↓
-// 轮速使用负值重新起步
-// 可以设置阈值：
-// 舵角误差 < 2°：允许正常起步
-// 舵角误差 2°～10°：低速或保持
-// 舵角误差 > 10°：驱动速度为0
-// 实际阈值根据舵向响应速度标定。
-// 需要注意：把所有 Vx、Vy、Omega 同比例减小，只能降低运动速度，通常不会改变目标舵轮角度。因此“减速”能让切换更安全，但不一定能让超限角度重新变得合法。
-// 4. 车身重定向是更平滑的高级方案
-// 你提出的：
-// 提前停车，然后自转，调整车头朝向，再继续跟踪。
-
-// 这是正确而且很实用的方案，尤其适合连续轨迹跟踪。
-// 但不一定要把车头完全转到轨迹方向，应当根据当前驾驶模式选择车身朝向。
-// 假设轨迹方向相对当前车体为：
-// +130°
-// 如果希望继续保持蟹行模式，舵轮的优选方向是 +90°，那么只需要让车体逆时针自转：
-// 130° - 90° = 40°
-// 车体旋转 +40° 后：
-// 世界轨迹方向不变
-// 轨迹在新车体坐标系中的方向：+90°
-// 舵轮回到标准蟹行角度：+90°
-// 不需要让车头旋转完整的 130°。
-// 如果准备切换成正常前进模式，优选舵角是 0°，才需要让车头基本对准轨迹：
-// 130° - 0° = 130°
-// 因此可以使用下面的通用公式：
-// 期望车身朝向 =
-// 世界轨迹切线方向 - 当前模式的优选舵轮方向
-// 其中：
-// 正常模式优选角：0°
-// 左蟹行优选角：+90°
-// 右蟹行优选角：-90°
-// 对于蟹行轨迹，车身朝向参考值为：
-// yawReference =
-//     pathTangentWorldRadians -
-//     Math.PI / 2.0;
-// 这样车体会逐渐调整自身朝向，使轨迹方向始终落在舵轮舒适区，而不是一直顶着 ±120° 工作。
-// 5. 不建议到达硬限位才突然自转
-// 更好的方案是向前预测。
-// 轨迹跟踪本身通常有前视点，可以提前计算未来一段轨迹对应的目标舵角：
-// 当前时刻目标轮角
-// 未来0.2秒目标轮角
-// 未来0.5秒目标轮角
-// 未来1.0秒目标轮角
-// 一旦预测到未来会进入软限位，就提前触发车身重定向：
-// 正常跟踪
-//   ↓
-// 预测舵角接近软限位
-//   ↓
-// 逐渐降低线速度
-//   ↓
-// 停车
-//   ↓
-// 车身原地旋转
-//   ↓
-// 舵轮调整到模式初始方向
-//   ↓
-// 重新捕获轨迹
-//   ↓
-// 继续跟踪
-// 可以定义状态机：
-// internal enum TrackingConstraintState
-// {
-//     Tracking,
-//     ApproachingSteeringLimit,
-//     Stopping,
-//     ReorientingBody,
-//     AligningWheels,
-//     Resuming,
-//     Fault
-// }
-// 不要在一个控制周期内同时完成停车、车身旋转和恢复跟踪。
-// 6. LQR和限位处理应该分层
-// 以后写 LQR 时，不建议让基础 LQR 直接承担全部舵轮限位逻辑。
-// 推荐结构：
-// 轨迹参考
-//    ↓
-// LQR轨迹控制器
-// 输出期望 Vx、Vy、Omega
-//    ↓
-// 舵轮约束管理器
-// 等价角选择、软限位、预测、车身重定向
-//    ↓
-// ChassisCommand
-//    ↓
-// MultiWheelChassisAdapter
-//    ↓
-// SendXYThSpeed
-// 可以以后增加一个文件：
-// Shared/SteeringConstraintManager.cs
-// 它负责：
-// 根据 Vx、Vy、Omega 预测每个舵轮目标角。
-// 选择“舵角反向、轮速反向”的等价解。
-// 检查硬限位和软限位。
-// 判断是否需要停车对齐。
-// 给出是否需要重新规划车身朝向。
-// 记录哪个舵轮导致不可执行。
-// 而 FrameTransform2D、ChassisCommand 不应该包含这些车辆机械约束。
-// 7. 当前阶段建议先做到什么
-// 现在先实现最小安全版本：
-// 使用 SendXYThSpeed()，让底层优先选择等价舵角。
-// 检查 adapter.Send(command) 的返回值。
-// 返回 false 时立即停车，不能继续沿用上一条速度。
-// 打印每个舵轮的当前角度、目标角度和上下限。
-// 模式切换时先停车、预转舵轮、确认到位，再开放摇杆。
-// 先用较低速度测试 +90° → +110° → +120° → +130° 的过程。
-// 最低限度的发送逻辑应当是：
-// var success = adapter.Send(command, interval);
-
-// if (!success)
-// {
-//     adapter.Stop(interval);
-
-//     Console.WriteLine(
-//         "底盘命令分解失败，已停车：" +
-//         "可能存在舵角限制或舵轮无法完成方向切换。");
-// }
-// 然后再实现预测重定向。
-// 推荐的最终策略
-// 对于你的停车机器人，我建议采用：
-// 优先级1：使用θ±180°、轮速反向的等价解
-// 优先级2：选择机械转角变化较小的等价解
-// 优先级3：接近软限位时降低速度
-// 优先级4：提前规划车身朝向，保持舵轮在舒适角度
-// 优先级5：无法连续切换时停车、自转、重新对齐
-// 优先级6：仍不可行时保持停车并报警
-// 所以你的 +130° 示例不应该一开始就判故障。它可以先使用 -50° + 反向轮速。如果从当前 +90° 切换到 -50° 的动态过程不安全，再停车调整。若轨迹允许改变车身姿态，更理想的办法是让车身提前旋转 +40°，从而继续以标准 +90° 蟹行姿态跟踪，而不是一直逼近机械硬限位。

Simulation/Commands/BuiltInSimulationActions.cs

@@ -1,121 +0,0 @@
-using MyParking.Simulation.Core;
-
-namespace MyParking.Simulation.Commands;
-
-/// <summary>
-/// 内置离线测试动作；新增带特性的方法后网页会自动生成按钮。
-/// </summary>
-public static class BuiltInSimulationActions
-{
-    [SimulationAction(
-        "mode-normal",
-        "正常",
-        "舵轮模式",
-        10)]
-    public static bool NormalMode(
-        SimulationVehicle vehicle)
-    {
-        return vehicle.SetMode("Normal");
-    }
-
-    [SimulationAction(
-        "mode-crab-left",
-        "左蟹行",
-        "舵轮模式",
-        20)]
-    public static bool CrabLeftMode(
-        SimulationVehicle vehicle)
-    {
-        return vehicle.SetMode("CrabLeft");
-    }
-
-    [SimulationAction(
-        "mode-crab-right",
-        "右蟹行",
-        "舵轮模式",
-        30)]
-    public static bool CrabRightMode(
-        SimulationVehicle vehicle)
-    {
-        return vehicle.SetMode("CrabRight");
-    }
-
-    [SimulationAction(
-        "mode-spin",
-        "自转",
-        "舵轮模式",
-        40)]
-    public static bool SpinMode(
-        SimulationVehicle vehicle)
-    {
-        return vehicle.SetMode("Spin");
-    }
-
-    [SimulationAction(
-        "forward",
-        "前进",
-        "运动测试",
-        10)]
-    public static bool Forward(
-        SimulationVehicle vehicle)
-    {
-        return vehicle.Move(1.0);
-    }
-
-    [SimulationAction(
-        "turn-left",
-        "左转",
-        "运动测试",
-        20)]
-    public static bool TurnLeft(
-        SimulationVehicle vehicle)
-    {
-        return vehicle.Turn(1.0);
-    }
-
-    [SimulationAction(
-        "stop",
-        "停止",
-        "运动测试",
-        30)]
-    public static bool Stop(
-        SimulationVehicle vehicle)
-    {
-        vehicle.Stop();
-        return true;
-    }
-
-    [SimulationAction(
-        "turn-right",
-        "右转",
-        "运动测试",
-        40)]
-    public static bool TurnRight(
-        SimulationVehicle vehicle)
-    {
-        return vehicle.Turn(-1.0);
-    }
-
-    [SimulationAction(
-        "backward",
-        "后退",
-        "运动测试",
-        50)]
-    public static bool Backward(
-        SimulationVehicle vehicle)
-    {
-        return vehicle.Move(-1.0);
-    }
-
-    [SimulationAction(
-        "reset",
-        "单车复位",
-        "维护",
-        10)]
-    public static bool Reset(
-        SimulationVehicle vehicle)
-    {
-        vehicle.Reset();
-        return true;
-    }
-}

Simulation/Commands/MySimulationTests.cs

@@ -1,31 +0,0 @@
-using MyParking.Shared;
-using MyParking.Simulation.Core;
-
-namespace MyParking.Simulation.Commands;
-
-/// <summary>
-/// 我自己增加的停车机器人仿真测试动作。
-/// </summary>
-public static class MySimulationTests
-{
-    /// <summary>
-    /// 测试车辆以0.2m/s向车体左侧运动。
-    /// </summary>
-    // [SimulationAction(
-    //     key: "move-left-020",
-    //     displayName: "向左移动0.2m/s",
-    //     group: "我的测试",
-    //     order: 10)]
-    // public static bool MoveLeft(
-    //     SimulationVehicle vehicle)
-    // {
-    //     var command = new ChassisCommand(
-    //         vehicle.VehicleId,
-    //         new Twist2D(
-    //             vxMetersPerSecond: 0.0,
-    //             vyMetersPerSecond: 0.2,
-    //             omegaRadiansPerSecond: 0.0));
-
-    //     return vehicle.ApplyCommand(command);
-    // }
-}

Simulation/Commands/SimulationActionAttribute.cs

@@ -1,38 +0,0 @@
-namespace MyParking.Simulation.Commands;
-
-/// <summary>
-/// 将一个静态仿真测试方法自动注册为网页按钮。
-/// 方法签名必须为bool Xxx(SimulationVehicle vehicle)。
-/// </summary>
-[AttributeUsage(AttributeTargets.Method)]
-public sealed class SimulationActionAttribute : Attribute
-{
-    public SimulationActionAttribute(
-        string key,
-        string displayName,
-        string group,
-        int order = 0)
-    {
-        Key = key;
-        DisplayName = displayName;
-        Group = group;
-        Order = order;
-    }
-
-    public string Key { get; }
-
-    public string DisplayName { get; }
-
-    public string Group { get; }
-
-    public int Order { get; }
-}
-
-/// <summary>
-/// 网页生成测试按钮所需的命令元数据。
-/// </summary>
-public sealed record SimulationActionDescriptor(
-    string Key,
-    string DisplayName,
-    string Group,
-    int Order);

Simulation/Commands/SimulationCommandDispatcher.cs

@@ -1,150 +0,0 @@
-using System.Reflection;
-using MyParking.Simulation.Core;
-
-namespace MyParking.Simulation.Commands;
-
-/// <summary>
-/// 自动发现带SimulationAction特性的测试方法并分发网页命令。
-/// </summary>
-public sealed class SimulationCommandDispatcher
-{
-    private readonly SimulationWorld _world;
-    private readonly IReadOnlyDictionary<string, RegisteredAction> _actions;
-
-    public SimulationCommandDispatcher(
-        SimulationWorld world)
-    {
-        _world = world;
-        _actions = DiscoverActions();
-    }
-
-    /// <summary>
-    /// 返回网页动态生成按钮所需的全部命令。
-    /// </summary>
-    public IReadOnlyList<SimulationActionDescriptor> GetActions()
-    {
-        return _actions.Values
-            .Select(action => action.Descriptor)
-            .OrderBy(action => action.Group)
-            .ThenBy(action => action.Order)
-            .ToArray();
-    }
-
-    /// <summary>
-    /// 对指定车辆执行一个已注册的测试方法。
-    /// </summary>
-    public CommandResult Execute(
-        int vehicleId,
-        string command)
-    {
-        if (!_actions.TryGetValue(
-                command,
-                out var registeredAction))
-        {
-            return new CommandResult(
-                false,
-                $"未注册仿真命令：{command}。");
-        }
-
-        try
-        {
-            return _world.WithVehicle(vehicleId, vehicle =>
-            {
-                var success = registeredAction.Handler(vehicle);
-                var message = success
-                    ? $"车辆{vehicleId}已执行：{registeredAction.Descriptor.DisplayName}。"
-                    : $"车辆{vehicleId}暂时无法执行：{registeredAction.Descriptor.DisplayName}。";
-
-                return new CommandResult(success, message);
-            });
-        }
-        catch (KeyNotFoundException exception)
-        {
-            return new CommandResult(
-                false,
-                exception.Message);
-        }
-    }
-
-    private static IReadOnlyDictionary<string, RegisteredAction>
-        DiscoverActions()
-    {
-        var actions = new Dictionary<string, RegisteredAction>(
-            StringComparer.OrdinalIgnoreCase);
-
-        var methods = Assembly.GetExecutingAssembly()
-            .GetTypes()
-            .SelectMany(type => type.GetMethods(
-                BindingFlags.Public |
-                BindingFlags.NonPublic |
-                BindingFlags.Static));
-
-        foreach (var method in methods)
-        {
-            var attribute =
-                method.GetCustomAttribute<SimulationActionAttribute>();
-
-            if (attribute == null)
-                continue;
-
-            ValidateMethod(method, attribute);
-
-            var handler =
-                (Func<SimulationVehicle, bool>)method.CreateDelegate(
-                    typeof(Func<SimulationVehicle, bool>));
-
-            var descriptor = new SimulationActionDescriptor(
-                attribute.Key,
-                attribute.DisplayName,
-                attribute.Group,
-                attribute.Order);
-
-            if (!actions.TryAdd(
-                    attribute.Key,
-                    new RegisteredAction(descriptor, handler)))
-            {
-                throw new InvalidOperationException(
-                    $"仿真命令Key重复：{attribute.Key}。");
-            }
-        }
-
-        return actions;
-    }
-
-    private static void ValidateMethod(
-        MethodInfo method,
-        SimulationActionAttribute attribute)
-    {
-        var parameters = method.GetParameters();
-
-        if (method.ReturnType != typeof(bool) ||
-            parameters.Length != 1 ||
-            parameters[0].ParameterType !=
-            typeof(SimulationVehicle))
-        {
-            throw new InvalidOperationException(
-                $"[{nameof(SimulationActionAttribute)}]方法" +
-                $"{method.DeclaringType?.FullName}.{method.Name}" +
-                "必须是static bool Xxx(SimulationVehicle vehicle)。");
-        }
-
-        if (string.IsNullOrWhiteSpace(attribute.Key) ||
-            string.IsNullOrWhiteSpace(attribute.DisplayName) ||
-            string.IsNullOrWhiteSpace(attribute.Group))
-        {
-            throw new InvalidOperationException(
-                $"仿真命令{method.Name}的特性参数不能为空。");
-        }
-    }
-
-    private sealed record RegisteredAction(
-        SimulationActionDescriptor Descriptor,
-        Func<SimulationVehicle, bool> Handler);
-}
-
-/// <summary>
-/// 网页测试命令的执行结果。
-/// </summary>
-public sealed record CommandResult(
-    bool Success,
-    string Message);

Simulation/Core/SimulationClock.cs

@@ -1,38 +0,0 @@
-using System.Diagnostics;
-
-namespace MyParking.Simulation.Core;
-
-/// <summary>
-/// 以固定周期推进离线仿真世界。
-/// </summary>
-public sealed class SimulationClock(
-    SimulationWorld world,
-    ILogger<SimulationClock> logger) : BackgroundService
-{
-    private static readonly TimeSpan TickInterval =
-        TimeSpan.FromMilliseconds(20);
-
-    protected override async Task ExecuteAsync(
-        CancellationToken stoppingToken)
-    {
-        logger.LogInformation(
-            "停车机器人离线仿真时钟已启动，周期{Period}ms。",
-            TickInterval.TotalMilliseconds);
-
-        using var timer = new PeriodicTimer(TickInterval);
-        var stopwatch = Stopwatch.StartNew();
-        var previousSeconds = stopwatch.Elapsed.TotalSeconds;
-
-        while (await timer.WaitForNextTickAsync(stoppingToken))
-        {
-            var currentSeconds = stopwatch.Elapsed.TotalSeconds;
-            var deltaTimeSeconds = Math.Clamp(
-                currentSeconds - previousSeconds,
-                0.001,
-                0.1);
-
-            previousSeconds = currentSeconds;
-            world.Step(deltaTimeSeconds);
-        }
-    }
-}

Simulation/Core/SimulationVehicle.cs

@@ -1,382 +0,0 @@
-using MyParking.Shared;
-using MyParking.Simulation.Models;
-
-namespace MyParking.Simulation.Core;
-
-/// <summary>
-/// 保存单辆四舵轮停车机器人的离线仿真状态。
-/// </summary>
-public sealed class SimulationVehicle
-{
-    public const double BodyLengthMeters = 1.472;
-    public const double BodyWidthMeters = 0.948;
-
-    // 当前MDCSToolBox.dll的MultiWheelChassisInitializer运行时轮位：
-    // X=±750mm、Y=±500mm，舵角机械限位为±120°。
-    private const double WheelX = 0.75;
-    private const double WheelY = 0.5;
-    private const double MaximumBodyAcceleration = 0.6;
-    private const double MaximumAngularAcceleration = 0.8;
-
-    private readonly List<VirtualSteerWheel> _wheels;
-    private readonly double _initialX;
-    private readonly double _initialY;
-    private readonly double _initialYaw;
-
-    private Twist2D _targetBodyTwist = Twist2D.Zero;
-    private double _actualVx;
-    private double _actualVy;
-    private double _actualOmega;
-
-    public SimulationVehicle(
-        int vehicleId,
-        double initialX,
-        double initialY,
-        double initialYaw)
-    {
-        VehicleId = vehicleId;
-        _initialX = initialX;
-        _initialY = initialY;
-        _initialYaw = initialYaw;
-
-        _wheels =
-        [
-            new VirtualSteerWheel("左前", WheelX, WheelY),
-            new VirtualSteerWheel("右前", WheelX, -WheelY),
-            new VirtualSteerWheel("左后", -WheelX, WheelY),
-            new VirtualSteerWheel("右后", -WheelX, -WheelY)
-        ];
-
-        Reset();
-    }
-
-    public int VehicleId { get; }
-
-    public string Mode { get; private set; } = "Normal";
-
-    public double XMeters { get; private set; }
-
-    public double YMeters { get; private set; }
-
-    public double YawRadians { get; private set; }
-
-    public bool ModeReady => _wheels.All(wheel => wheel.IsAligned);
-
-    /// <summary>
-    /// 停车并切换舵轮准备模式。
-    /// </summary>
-    public bool SetMode(string mode)
-    {
-        Stop();
-
-        var success = mode switch
-        {
-            "Normal" => PrepareParallelDirection(0.0),
-            "CrabLeft" => PrepareParallelDirection(90.0),
-            "CrabRight" => PrepareParallelDirection(-90.0),
-            "Spin" => PrepareSpinDirection(),
-            _ => false
-        };
-
-        if (success)
-            Mode = mode;
-
-        return success;
-    }
-
-    /// <summary>
-    /// 按当前模式发送前进或后退命令。
-    /// </summary>
-    public bool Move(double directionSign)
-    {
-        if (!ModeReady)
-            return false;
-
-        const double linearSpeed = 0.35;
-        const double angularSpeed = 0.45;
-
-        var command = Mode switch
-        {
-            "Normal" => new Twist2D(
-                directionSign * linearSpeed, 0.0, 0.0),
-            "CrabLeft" => new Twist2D(
-                0.0, directionSign * linearSpeed, 0.0),
-            "CrabRight" => new Twist2D(
-                0.0, -directionSign * linearSpeed, 0.0),
-            "Spin" => new Twist2D(
-                0.0, 0.0, directionSign * angularSpeed),
-            _ => Twist2D.Zero
-        };
-
-        return ApplyCommand(
-            new ChassisCommand(VehicleId, command));
-    }
-
-    /// <summary>
-    /// 在当前运动模式下增加逆时针或顺时针转动。
-    /// </summary>
-    public bool Turn(double directionSign)
-    {
-        if (!ModeReady || Mode == "Spin")
-            return false;
-
-        var command = new Twist2D(
-            _targetBodyTwist.VxMetersPerSecond,
-            _targetBodyTwist.VyMetersPerSecond,
-            directionSign * 0.28);
-
-        return ApplyCommand(
-            new ChassisCommand(VehicleId, command));
-    }
-
-    /// <summary>
-    /// 将网页虚拟遥控器的油门和转向组合为连续车体速度命令。
-    /// </summary>
-    public bool ManualDrive(
-        double throttle,
-        double steering,
-        double speedScale,
-        double steeringScale)
-    {
-        if (!AreFinite(
-                throttle,
-                steering,
-                speedScale,
-                steeringScale))
-        {
-            return false;
-        }
-
-        throttle = Math.Clamp(throttle, -1.0, 1.0);
-        steering = Math.Clamp(steering, -1.0, 1.0);
-        speedScale = Math.Clamp(speedScale, 0.0, 1.0);
-        steeringScale = Math.Clamp(steeringScale, 0.0, 1.0);
-
-        if (Math.Abs(throttle) < 0.001 &&
-            Math.Abs(steering) < 0.001)
-        {
-            Stop();
-            return true;
-        }
-
-        if (!ModeReady)
-            return false;
-
-        const double maximumLinearSpeed = 0.6;
-        const double maximumAngularSpeed = 0.7;
-
-        var linearSpeed =
-            throttle * maximumLinearSpeed * speedScale;
-        var angularSpeed =
-            steering * maximumAngularSpeed * steeringScale;
-
-        var twist = Mode switch
-        {
-            "Normal" => new Twist2D(
-                linearSpeed,
-                0.0,
-                angularSpeed),
-            "CrabLeft" => new Twist2D(
-                0.0,
-                linearSpeed,
-                angularSpeed),
-            "CrabRight" => new Twist2D(
-                0.0,
-                -linearSpeed,
-                angularSpeed),
-            "Spin" => new Twist2D(
-                0.0,
-                0.0,
-                angularSpeed),
-            _ => Twist2D.Zero
-        };
-
-        return ApplyCommand(
-            new ChassisCommand(VehicleId, twist));
-    }
-
-    /// <summary>
-    /// 应用统一车体速度命令并分解为四个舵轮速度向量。
-    /// </summary>
-    public bool ApplyCommand(ChassisCommand command)
-    {
-        if (command.VehicleId != VehicleId)
-            return false;
-
-        var twist = command.BodyTwist;
-        var wheelCommands = _wheels.Select(wheel =>
-        {
-            var wheelVx =
-                twist.VxMetersPerSecond -
-                twist.OmegaRadiansPerSecond * wheel.YMeters;
-
-            var wheelVy =
-                twist.VyMetersPerSecond +
-                twist.OmegaRadiansPerSecond * wheel.XMeters;
-
-            return (Wheel: wheel, Vx: wheelVx, Vy: wheelVy);
-        }).ToArray();
-
-        foreach (var item in wheelCommands)
-        {
-            if (!item.Wheel.SetVelocityVector(
-                    item.Vx,
-                    item.Vy))
-            {
-                Stop();
-                return false;
-            }
-        }
-
-        _targetBodyTwist = twist;
-        return true;
-    }
-
-    /// <summary>
-    /// 将车辆目标速度设置为零并保持当前舵轮角度。
-    /// </summary>
-    public void Stop()
-    {
-        _targetBodyTwist = Twist2D.Zero;
-        foreach (var wheel in _wheels)
-            wheel.Stop();
-    }
-
-    /// <summary>
-    /// 更新舵轮反馈和车辆世界位姿。
-    /// </summary>
-    public void Step(double deltaTimeSeconds)
-    {
-        foreach (var wheel in _wheels)
-            wheel.Step(deltaTimeSeconds);
-
-        var canMove = _wheels.All(wheel => wheel.IsAligned);
-        var targetVx = canMove
-            ? _targetBodyTwist.VxMetersPerSecond
-            : 0.0;
-        var targetVy = canMove
-            ? _targetBodyTwist.VyMetersPerSecond
-            : 0.0;
-        var targetOmega = canMove
-            ? _targetBodyTwist.OmegaRadiansPerSecond
-            : 0.0;
-
-        _actualVx = MoveTowards(
-            _actualVx,
-            targetVx,
-            MaximumBodyAcceleration * deltaTimeSeconds);
-
-        _actualVy = MoveTowards(
-            _actualVy,
-            targetVy,
-            MaximumBodyAcceleration * deltaTimeSeconds);
-
-        _actualOmega = MoveTowards(
-            _actualOmega,
-            targetOmega,
-            MaximumAngularAcceleration * deltaTimeSeconds);
-
-        var cos = Math.Cos(YawRadians);
-        var sin = Math.Sin(YawRadians);
-
-        var worldVx = cos * _actualVx - sin * _actualVy;
-        var worldVy = sin * _actualVx + cos * _actualVy;
-
-        XMeters += worldVx * deltaTimeSeconds;
-        YMeters += worldVy * deltaTimeSeconds;
-        YawRadians = FrameTransform2D.NormalizeAngle(
-            YawRadians + _actualOmega * deltaTimeSeconds);
-    }
-
-    /// <summary>
-    /// 恢复车辆初始位置和舵轮状态。
-    /// </summary>
-    public void Reset()
-    {
-        XMeters = _initialX;
-        YMeters = _initialY;
-        YawRadians = _initialYaw;
-        Mode = "Normal";
-        _targetBodyTwist = Twist2D.Zero;
-        _actualVx = 0.0;
-        _actualVy = 0.0;
-        _actualOmega = 0.0;
-
-        foreach (var wheel in _wheels)
-            wheel.Reset();
-    }
-
-    /// <summary>
-    /// 创建供网页读取的不可变状态快照。
-    /// </summary>
-    public VehicleStateDto GetSnapshot()
-    {
-        return new VehicleStateDto(
-            VehicleId,
-            XMeters,
-            YMeters,
-            YawRadians,
-            BodyLengthMeters,
-            BodyWidthMeters,
-            Mode,
-            ModeReady,
-            new TwistStateDto(
-                _targetBodyTwist.VxMetersPerSecond,
-                _targetBodyTwist.VyMetersPerSecond,
-                _targetBodyTwist.OmegaRadiansPerSecond),
-            new TwistStateDto(
-                _actualVx,
-                _actualVy,
-                _actualOmega),
-            _wheels.Select(wheel =>
-                new WheelStateDto(
-                    wheel.Name,
-                    wheel.XMeters,
-                    wheel.YMeters,
-                    wheel.TargetAngleDegrees,
-                    wheel.ActualAngleDegrees,
-                    wheel.TargetSpeedMetersPerSecond,
-                    wheel.ActualSpeedMetersPerSecond,
-                    wheel.IsAligned)).ToArray());
-    }
-
-    private bool PrepareParallelDirection(double targetAngleDegrees)
-    {
-        return _wheels.All(wheel =>
-            wheel.PrepareDirection(targetAngleDegrees));
-    }
-
-    private bool PrepareSpinDirection()
-    {
-        var success = true;
-
-        foreach (var wheel in _wheels)
-        {
-            var vx = -wheel.YMeters;
-            var vy = wheel.XMeters;
-            success &= wheel.SetVelocityVector(vx, vy);
-            wheel.Stop();
-        }
-
-        return success;
-    }
-
-    private static double MoveTowards(
-        double current,
-        double target,
-        double maximumChange)
-    {
-        var difference = target - current;
-        if (Math.Abs(difference) <= maximumChange)
-            return target;
-
-        return current + Math.Sign(difference) * maximumChange;
-    }
-
-    private static bool AreFinite(params double[] values)
-    {
-        return values.All(value =>
-            !double.IsNaN(value) &&
-            !double.IsInfinity(value));
-    }
-}

Simulation/Core/SimulationWorld.cs

@@ -1,205 +0,0 @@
-using MyParking.Shared;
-using MyParking.Simulation.Models;
-
-namespace MyParking.Simulation.Core;
-
-/// <summary>
-/// 管理离线仿真车辆、车队中心和成员布局。
-/// </summary>
-public sealed class SimulationWorld
-{
-    private readonly object _syncRoot = new();
-    private Dictionary<int, SimulationVehicle> _vehicles = new();
-    private SimulationConfigurationDto _configuration;
-
-    public SimulationWorld()
-    {
-        _configuration = CreateDefaultConfiguration();
-        ApplyConfigurationCore(_configuration);
-    }
-
-    public T WithVehicle<T>(
-        int vehicleId,
-        Func<SimulationVehicle, T> action)
-    {
-        lock (_syncRoot)
-        {
-            if (!_vehicles.TryGetValue(
-                    vehicleId,
-                    out var vehicle))
-            {
-                throw new KeyNotFoundException(
-                    $"不存在车辆{vehicleId}。");
-            }
-
-            return action(vehicle);
-        }
-    }
-
-    public void Step(double deltaTimeSeconds)
-    {
-        lock (_syncRoot)
-        {
-            foreach (var vehicle in _vehicles.Values)
-                vehicle.Step(deltaTimeSeconds);
-        }
-    }
-
-    public IReadOnlyList<VehicleStateDto> GetSnapshot()
-    {
-        lock (_syncRoot)
-        {
-            return _vehicles.Values
-                .OrderBy(vehicle => vehicle.VehicleId)
-                .Select(vehicle => vehicle.GetSnapshot())
-                .ToArray();
-        }
-    }
-
-    public SimulationConfigurationDto GetConfiguration()
-    {
-        lock (_syncRoot)
-        {
-            return CloneConfiguration(_configuration);
-        }
-    }
-
-    public void ApplyConfiguration(
-        SimulationConfigurationDto configuration)
-    {
-        ValidateConfiguration(configuration);
-
-        lock (_syncRoot)
-        {
-            _configuration = CloneConfiguration(configuration);
-            ApplyConfigurationCore(_configuration);
-        }
-    }
-
-    public void Reset()
-    {
-        lock (_syncRoot)
-        {
-            ApplyConfigurationCore(_configuration);
-        }
-    }
-
-    private void ApplyConfigurationCore(
-        SimulationConfigurationDto configuration)
-    {
-        var fleetPoseInWorld = new Pose2D(
-            configuration.FleetCenter.XMeters,
-            configuration.FleetCenter.YMeters,
-            configuration.FleetCenter.YawRadians);
-
-        _vehicles = configuration.Vehicles
-            .Take(configuration.VehicleCount)
-            .Select(layout =>
-            {
-                var bodyPoseInFleet = new Pose2D(
-                    layout.XMeters,
-                    layout.YMeters,
-                    layout.YawRadians);
-
-                var bodyPoseInWorld =
-                    FrameTransform2D.Compose(
-                        fleetPoseInWorld,
-                        bodyPoseInFleet);
-
-                return new SimulationVehicle(
-                    layout.VehicleId,
-                    bodyPoseInWorld.XMeters,
-                    bodyPoseInWorld.YMeters,
-                    bodyPoseInWorld.YawRadians);
-            })
-            .ToDictionary(vehicle => vehicle.VehicleId);
-    }
-
-    private static void ValidateConfiguration(
-        SimulationConfigurationDto configuration)
-    {
-        if (configuration.VehicleCount is < 1 or > 8)
-        {
-            throw new ArgumentOutOfRangeException(
-                nameof(configuration.VehicleCount),
-                "仿真车辆数量必须在1到8之间。");
-        }
-
-        if (configuration.FleetCenter == null)
-        {
-            throw new ArgumentException(
-                "必须提供车队中心位姿。",
-                nameof(configuration));
-        }
-
-        if (configuration.Vehicles == null ||
-            configuration.Vehicles.Count <
-            configuration.VehicleCount)
-        {
-            throw new ArgumentException(
-                "成员布局数量不能少于车辆数量。",
-                nameof(configuration));
-        }
-
-        var selectedLayouts = configuration.Vehicles
-            .Take(configuration.VehicleCount)
-            .ToArray();
-
-        if (selectedLayouts.Any(layout =>
-                layout.VehicleId <= 0) ||
-            selectedLayouts
-                .Select(layout => layout.VehicleId)
-                .Distinct()
-                .Count() != selectedLayouts.Length)
-        {
-            throw new ArgumentException(
-                "车辆编号必须大于零且不能重复。",
-                nameof(configuration));
-        }
-
-        var values = new[]
-        {
-            configuration.FleetCenter.XMeters,
-            configuration.FleetCenter.YMeters,
-            configuration.FleetCenter.YawRadians
-        }.Concat(selectedLayouts.SelectMany(layout => new[]
-        {
-            layout.XMeters,
-            layout.YMeters,
-            layout.YawRadians
-        }));
-
-        if (values.Any(value =>
-                double.IsNaN(value) ||
-                double.IsInfinity(value)))
-        {
-            throw new ArgumentException(
-                "车队和车辆布局不能包含NaN或无穷大。",
-                nameof(configuration));
-        }
-    }
-
-    private static SimulationConfigurationDto
-        CreateDefaultConfiguration()
-    {
-        return new SimulationConfigurationDto(
-            1,
-            new FleetCenterDto(0.0, 0.0, 0.0),
-            new[]
-            {
-                new VehicleLayoutDto(1, 0.0, 0.0, 0.0)
-            });
-    }
-
-    private static SimulationConfigurationDto
-        CloneConfiguration(
-            SimulationConfigurationDto configuration)
-    {
-        return new SimulationConfigurationDto(
-            configuration.VehicleCount,
-            configuration.FleetCenter with { },
-            configuration.Vehicles
-                .Select(layout => layout with { })
-                .ToArray());
-    }
-}

Simulation/Core/VirtualSteerWheel.cs

@@ -1,174 +0,0 @@
-namespace MyParking.Simulation.Core;
-
-/// <summary>
-/// 模拟单个舵轮的转向和驱动响应，不包含真实电机物理模型。
-/// </summary>
-public sealed class VirtualSteerWheel
-{
-    private const double AngleLowerLimitDegrees = -120.0;
-    private const double AngleUpperLimitDegrees = 120.0;
-    private const double MaximumSteeringRateDegreesPerSecond = 90.0;
-    private const double MaximumDriveAccelerationMetersPerSecondSquared = 0.8;
-    private const double AlignmentToleranceDegrees = 1.5;
-
-    public VirtualSteerWheel(
-        string name,
-        double xMeters,
-        double yMeters)
-    {
-        Name = name;
-        XMeters = xMeters;
-        YMeters = yMeters;
-    }
-
-    public string Name { get; }
-
-    public double XMeters { get; }
-
-    public double YMeters { get; }
-
-    public double TargetAngleDegrees { get; private set; }
-
-    public double ActualAngleDegrees { get; private set; }
-
-    public double TargetSpeedMetersPerSecond { get; private set; }
-
-    public double ActualSpeedMetersPerSecond { get; private set; }
-
-    public bool IsAligned =>
-        Math.Abs(TargetAngleDegrees - ActualAngleDegrees) <=
-        AlignmentToleranceDegrees;
-
-    /// <summary>
-    /// 设置期望轮胎速度向量，并在机械限位内选择等价舵角。
-    /// </summary>
-    public bool SetVelocityVector(
-        double vxMetersPerSecond,
-        double vyMetersPerSecond)
-    {
-        var speed = Math.Sqrt(
-            vxMetersPerSecond * vxMetersPerSecond +
-            vyMetersPerSecond * vyMetersPerSecond);
-
-        if (speed < 1e-6)
-        {
-            TargetSpeedMetersPerSecond = 0.0;
-            return true;
-        }
-
-        var desiredAngleDegrees =
-            Math.Atan2(vyMetersPerSecond, vxMetersPerSecond) *
-            180.0 / Math.PI;
-
-        return SetDirectionAndSpeed(
-            desiredAngleDegrees,
-            speed);
-    }
-
-    /// <summary>
-    /// 停车时设置舵轮预对齐方向。
-    /// </summary>
-    public bool PrepareDirection(double targetAngleDegrees)
-    {
-        TargetSpeedMetersPerSecond = 0.0;
-        return SetDirectionAndSpeed(
-            targetAngleDegrees,
-            0.0);
-    }
-
-    /// <summary>
-    /// 将驱动目标设置为零，并保持当前舵轮方向。
-    /// </summary>
-    public void Stop()
-    {
-        TargetSpeedMetersPerSecond = 0.0;
-    }
-
-    /// <summary>
-    /// 按固定转向速度和驱动加速度更新虚拟反馈。
-    /// </summary>
-    public void Step(double deltaTimeSeconds)
-    {
-        ActualAngleDegrees = MoveTowards(
-            ActualAngleDegrees,
-            TargetAngleDegrees,
-            MaximumSteeringRateDegreesPerSecond *
-            deltaTimeSeconds);
-
-        var allowedTargetSpeed =
-            IsAligned ? TargetSpeedMetersPerSecond : 0.0;
-
-        ActualSpeedMetersPerSecond = MoveTowards(
-            ActualSpeedMetersPerSecond,
-            allowedTargetSpeed,
-            MaximumDriveAccelerationMetersPerSecondSquared *
-            deltaTimeSeconds);
-    }
-
-    /// <summary>
-    /// 恢复舵轮初始状态。
-    /// </summary>
-    public void Reset()
-    {
-        TargetAngleDegrees = 0.0;
-        ActualAngleDegrees = 0.0;
-        TargetSpeedMetersPerSecond = 0.0;
-        ActualSpeedMetersPerSecond = 0.0;
-    }
-
-    private bool SetDirectionAndSpeed(
-        double desiredAngleDegrees,
-        double desiredSpeedMetersPerSecond)
-    {
-        var candidates = new[]
-        {
-            (Angle: NormalizeDegrees(desiredAngleDegrees),
-                Speed: desiredSpeedMetersPerSecond),
-            (Angle: NormalizeDegrees(desiredAngleDegrees + 180.0),
-                Speed: -desiredSpeedMetersPerSecond),
-            (Angle: NormalizeDegrees(desiredAngleDegrees - 180.0),
-                Speed: -desiredSpeedMetersPerSecond)
-        };
-
-        var validCandidates = candidates
-            .Where(candidate =>
-                candidate.Angle >= AngleLowerLimitDegrees &&
-                candidate.Angle <= AngleUpperLimitDegrees)
-            .OrderBy(candidate =>
-                Math.Abs(candidate.Angle - ActualAngleDegrees))
-            .ToArray();
-
-        if (validCandidates.Length == 0)
-        {
-            TargetSpeedMetersPerSecond = 0.0;
-            return false;
-        }
-
-        var selected = validCandidates[0];
-        TargetAngleDegrees = selected.Angle;
-        TargetSpeedMetersPerSecond = selected.Speed;
-        return true;
-    }
-
-    private static double MoveTowards(
-        double current,
-        double target,
-        double maximumChange)
-    {
-        var difference = target - current;
-        if (Math.Abs(difference) <= maximumChange)
-            return target;
-
-        return current + Math.Sign(difference) * maximumChange;
-    }
-
-    private static double NormalizeDegrees(double angleDegrees)
-    {
-        angleDegrees %= 360.0;
-        if (angleDegrees >= 180.0)
-            angleDegrees -= 360.0;
-        if (angleDegrees < -180.0)
-            angleDegrees += 360.0;
-        return angleDegrees;
-    }
-}

Simulation/Models/ManualControlInputDto.cs

@@ -1,10 +0,0 @@
-namespace MyParking.Simulation.Models;
-
-/// <summary>
-/// 网页虚拟遥控器输入，所有输入范围均为-1到1。
-/// </summary>
-public sealed record ManualControlInputDto(
-    double Throttle,
-    double Steering,
-    double SpeedScale,
-    double SteeringScale);

Simulation/Models/SimulationConfigurationDto.cs

@@ -1,26 +0,0 @@
-namespace MyParking.Simulation.Models;
-
-/// <summary>
-/// 离线仿真的车辆数量、车队中心和成员相对布局。
-/// </summary>
-public sealed record SimulationConfigurationDto(
-    int VehicleCount,
-    FleetCenterDto FleetCenter,
-    IReadOnlyList<VehicleLayoutDto> Vehicles);
-
-/// <summary>
-/// 车队中心在世界坐标系中的位姿。
-/// </summary>
-public sealed record FleetCenterDto(
-    double XMeters,
-    double YMeters,
-    double YawRadians);
-
-/// <summary>
-/// 单车车体坐标系在车队中心坐标系中的位姿。
-/// </summary>
-public sealed record VehicleLayoutDto(
-    int VehicleId,
-    double XMeters,
-    double YMeters,
-    double YawRadians);

Simulation/Models/VehicleStateDto.cs

@@ -1,25 +0,0 @@
-namespace MyParking.Simulation.Models;
-
-/// <summary>
-/// 网页绘制单辆停车机器人所需的状态快照。
-/// </summary>
-public sealed record VehicleStateDto(
-    int VehicleId,
-    double XMeters,
-    double YMeters,
-    double YawRadians,
-    double BodyLengthMeters,
-    double BodyWidthMeters,
-    string Mode,
-    bool ModeReady,
-    TwistStateDto TargetBodyTwist,
-    TwistStateDto ActualBodyTwist,
-    IReadOnlyList<WheelStateDto> Wheels);
-
-/// <summary>
-/// 网页显示的二维车体速度快照。
-/// </summary>
-public sealed record TwistStateDto(
-    double VxMetersPerSecond,
-    double VyMetersPerSecond,
-    double OmegaRadiansPerSecond);

Simulation/Models/WheelStateDto.cs

@@ -1,14 +0,0 @@
-namespace MyParking.Simulation.Models;
-
-/// <summary>
-/// 网页绘制单个舵轮所需的状态快照。
-/// </summary>
-public sealed record WheelStateDto(
-    string Name,
-    double XMeters,
-    double YMeters,
-    double TargetAngleDegrees,
-    double ActualAngleDegrees,
-    double TargetSpeedMetersPerSecond,
-    double ActualSpeedMetersPerSecond,
-    bool IsAligned);

Simulation/MyParking.Simulation.csproj

@@ -1,17 +0,0 @@
-<Project Sdk="Microsoft.NET.Sdk.Web">
-
-  <PropertyGroup>
-    <TargetFramework>net8.0</TargetFramework>
-    <Nullable>enable</Nullable>
-    <ImplicitUsings>enable</ImplicitUsings>
-    <RootNamespace>MyParking.Simulation</RootNamespace>
-  </PropertyGroup>
-
-  <ItemGroup>
-    <Compile Include="..\Shared\ChassisCommand.cs"
-             Link="Shared\ChassisCommand.cs" />
-    <Compile Include="..\Shared\FrameTransform2D.cs"
-             Link="Shared\FrameTransform2D.cs" />
-  </ItemGroup>
-
-</Project>