添加单车底盘仿真平台并完善运动控制与夹臂功能

.codex/skills/commit/SKILL.md

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

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

@@ -0,0 +1,70 @@
+---
+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.

.vs/ProjectSettings.json

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

.vs/VSWorkspaceState.json

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

ClumsyPilot/AGV.cs

@@ -1,534 +1,21 @@
 using ClumsyCore;
-using ClumsyCore.Interfaces;
-using ClumsyCore.Sensors;
-using CommonUsage.Chassis;
-using CommonUsage.Mathematics;
-using FundamentalLib;
 using MDCSToolBox.Clumsy.AgvInterfaces;
-using MDCSToolBox.Clumsy.Calibration;
 using MDCSToolBox.Clumsy.MotionControllers;
-using MDCSToolBox.Clumsy.Tracks;
-using MDCSToolBox.Commons.Controllers;
-using Newtonsoft.Json;
-using System;
-using System.Collections.Generic;
-using System.Net.Http;
-using System.Numerics;
-using System.Security.Cryptography;
-using System.Threading;
-using System.Threading.Tasks;
-using static ClumsyCore.DTools.Painter;
 
 namespace MultiWheelC
 {
-    public class SetLocationRes
-    {
-        public float x, y, th;
-        public int l_step;
-        public long tick;
-        public string error;
-    }
     public class AGV : MultiWheelInterface
     {
         public override AbstractGeometricController GetController()
-        {
-            return new ChassisController().Get();
-        }
-
+                => new ChassisController().Get();
         public override MultiWheelMagTracker GetMagController()
-        {
-            return new MultiWheelMagTracker();
-        }
-
+            => new MultiWheelMagTracker();
         public override NaiveMagnetController GetNaiveMagnetController()
-        {
-            return new NaiveMagnetController();
-        }
+            => new NaiveMagnetController();
 
-        public void Sleep(float s)
+        public void Sleep(float seconds)
         {
-            new DriveTask(new Sleep() { Second = s }.Get()).Wait();
+            new DriveTask(new Sleep { Second = seconds }.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,9 +1,5 @@
-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;
@@ -14,8 +10,7 @@ public class ChassisController : MovementDefinition<MultiWheelGeometricControlle
 {
     public float BaseSpeed = Configuration.conf.basicSpeed;
 
-    private DateTime _sendMotionDbgLast = DateTime.MinValue;
-
+    // 创建单车几何跟踪控制器（直接控本车底盘，不走多车 Auto 通道）
     public override MultiWheelGeometricController Get()
     {
         return new MultiWheelGeometricController
@@ -45,56 +40,6 @@ 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

@@ -4,15 +4,22 @@
     <TargetFramework>netstandard2.0</TargetFramework>
     <LangVersion>10</LangVersion>
     <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
+    <AssemblyName>ClumsyPilot</AssemblyName>
+    <RootNamespace>MultiWheelC</RootNamespace>
     <AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
-	<OutputPath>..\..\build\Clumsy\</OutputPath>
+    <OutputPath>build\Clumsy\</OutputPath>
   </PropertyGroup>
 
   <ItemGroup>
-    <PackageReference Include="Newtonsoft.Json" Version="13.0.4" />
+    <PackageReference Include="Newtonsoft.Json" Version="13.0.3" />
     <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>
@@ -34,8 +41,4 @@
     </Reference>
   </ItemGroup>
 
-  <Target Name="PostBuild" AfterTargets="PostBuildEvent">
-	  <Exec Command="if not exist $(SolutionDir)build\Clumsy mkdir $(SolutionDir)build\Clumsy&#xD;&#xA;if not exist $(SolutionDir)build\Clumsy_AGV2 mkdir $(SolutionDir)build\Clumsy_AGV2&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.exe $(SolutionDir)build\Clumsy\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.deps.json $(SolutionDir)build\Clumsy\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.runtimeconfig.json $(SolutionDir)build\Clumsy\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\RefClumsyCore.dll $(SolutionDir)build\Clumsy\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\RefClumsyDance.dll $(SolutionDir)build\Clumsy\&#xD;&#xA;copy /Y D:\MDCS\Release\MDCSToolBox.dll $(SolutionDir)build\Clumsy\&#xD;&#xA;copy /Y D:\MDCS\Release\CommonUsage.dll $(SolutionDir)build\Clumsy\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.exe $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.deps.json $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.runtimeconfig.json $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\RefClumsyCore.dll $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\RefClumsyDance.dll $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y D:\MDCS\Release\MDCSToolBox.dll $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y D:\MDCS\Release\CommonUsage.dll $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y $(TargetDir)$(TargetName).dll $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y $(TargetDir)$(TargetName).pdb $(SolutionDir)build\Clumsy_AGV2\" />
-  </Target>
-	
 </Project>

ClumsyPilot/FleetCrabWalk.cs

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

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

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

@@ -1,136 +0,0 @@
-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,608 +1,126 @@
-using System;
-using System.Collections.Generic;
-using System.Numerics;
 using ClumsyCore;
+using ClumsyCore.DTools;
 using ClumsyCore.Interfaces;
 using ClumsyCore.Pilot;
-using FundamentalLib;
-using CommonUsage.Chassis;
-using CommonUsage.Mathematics;
-using MDCSToolBox.Clumsy.Movements;
-using MDCSToolBox.Clumsy.Pilot;
-using MDCSToolBox.Clumsy.Tracks;
+using MDCSToolBox.Commons.Controllers;
+using System;
+using System.Numerics;
 
-namespace MultiWheelC;
-
-public class MultiForwardTest : MovementDefinition
+namespace MultiWheelC
 {
-    public float Speed = 0.2f;
-    public float DurationSeconds = 2f;
+    public abstract class DstTrackerTestBase : MovementTest
+    {
+        public bool UseInteractivePick = true;
+        public float srcX;
+        public float srcY;
+        public float dstX;
+        public float dstY;
+        public float carDirectionBias;
 
-    public override IEnumerable<bool> Get()
+        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)
-        {
-            chassis.SendMotion(Speed, 0, 0);
-            yield return true;
+            carDirectionBias = defaultCarDirectionBias;
         }
 
-        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()
+        public override void TestStop()
         {
-        var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
-        chassis.SetOriginBias(0, 0, 0);
-
-        // 阶段一：仅把舵轮打到原地旋转所需角度（下发 0 速度，只对齐不旋转）。
-        while (true)
-        {
-            chassis.SendRotateMotion(0);
-            if (WheelsAligned(chassis, WheelAlignDeg)) break;
-            yield return true;
+            _dt?.Stop();
+            _painter?.Clear();
         }
 
-        // 阶段二：旋转到目标世界朝向，到位即停。
-        var target = CommonMath.RoundTh(TargetWorldDeg);
-        while (true)
-        {
-            var cur = CommonMath.RoundTh((float)DetourInterface.getCartLocation().th);
-            var diff = CommonMath.ThDiff(target, cur); // 逆时针为正
-            if (Math.Abs(diff) <= ArriveDeg) break;
-            chassis.SendRotateMotion(Math.Sign(diff) * RotSpeed);
-            yield return true;
-        }
-
-        chassis.PredefinedDriveStop();
-    }
-
-    private static bool WheelsAligned(MultiWheelChassis chassis, float tolDeg)
-    {
-#pragma warning disable CS0612, CS0618
-        var wheels = chassis.GetSteerWheels();
-#pragma warning restore CS0612, CS0618
-        foreach (var sw in wheels)
-            if (Math.Abs(CommonMath.ThDiff(sw.ReadAngle(), sw.GetSendAngle())) > tolDeg)
-                return false;
-        return true;
-    }
-}
-
-[MovementTest(name = "多舵轮-前进2秒")]
-public class MultiForwardMovementTest : MovementTest
-{
-    private DriveTask _task;
-
         public override void Test()
         {
-        _task = new DriveTask(new MultiForwardTest().Get());
-        _task.Wait();
+            Vector2 p1;
+            Vector2 p2;
+            if (UseInteractivePick)
+            {
+                p1 = UI.GetPoint("point1");
+                p2 = UI.GetPoint("point2");
+            }
+            else
+            {
+                p1 = new Vector2(srcX, srcY);
+                p2 = new Vector2(dstX, dstY);
             }
 
-    public override void TestStop() => _task?.Stop();
-}
-
-[MovementTest(name = "多舵轮-原地旋转到目标角度")]
-public class MultiRotateMovementTest : MovementTest
-{
-    private DriveTask _task;
-
-    public override void Test()
+            _painter.Clear();
+            _dt = new DriveTask(new DstTracker
             {
-        _task = new DriveTask(new MultiRotateToWorldAngle
-        {
-            TargetWorldDeg = PilotDefinition.Conf.InPlaceRotateTargetWorldDeg,
-            RotSpeed = PilotDefinition.Conf.InPlaceRotateSpeed,
-            ArriveDeg = PilotDefinition.Conf.InPlaceRotateArriveDeg,
-            WheelAlignDeg = PilotDefinition.Conf.InPlaceRotateWheelAlignDeg
+                Src = p1,
+                Dst = p2,
+                CarDirectionBias = carDirectionBias,
             }.Get());
-        _task.Wait();
+            _dt.Wait();
+        }
     }
 
+    [MovementTest(name = "测试终点跟踪动作-前进")]
+    public sealed class DstTrackerForward : DstTrackerTestBase
+    {
+        public DstTrackerForward() : base(0f) { }
+    }
+
+    [MovementTest(name = "测试终点跟踪动作-后退")]
+    public sealed class DstTrackerBackward : DstTrackerTestBase
+    {
+        public DstTrackerBackward() : base(180f) { }
+    }
+
+    [MovementTest(name = "底盘旋转测试")]
+    public class RotateToAngleTest : MovementTest
+    {
+        private DriveTask _dt;
+
+        // 停止当前正在执行的底盘原地旋转任务。
         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;
+            _dt?.Stop();
         }
 
-    public void Stop() => ClearScript();
-
-    public override IEnumerable<bool> Get()
-    {
-        var self = PilotDefinition.Self;
-        var conf = PilotDefinition.Conf;
-
-        if (conf.MultiVehicleMasterEndpoint != "/")
-        {
-            Hedingben.ToastText("车队原地旋转需在主车(主车端点=\"/\")运行", "FleetRotate");
-            yield break;
-        }
-
-        var dir = Math.Sign(TargetDeltaDeg);
-        if (dir == 0) dir = 1;
-        var maxOmega = Math.Abs(Omega);
-        var minOmega = Math.Min(Math.Abs(MinOmega), maxOmega); // 最小不超过最大
-        var slowDeg = Math.Max(1e-3f, SlowDeg);                // 减速区宽度
-        var accel = AccelDegPerSec2;                           // 缓启动角加速度，仅作用于起步，<=0 关闭
-        var targetMag = Math.Abs(TargetDeltaDeg);
-
-        var hasPos = UseDetourHeading;
-        var prevTh = hasPos ? (float)DetourInterface.getCartLocation().th : 0f;
-        var startTh = prevTh;
-        var accumulated = 0f; // 累计带符号转角(deg)
-        var start = DateTime.Now;
-        var lastTime = start;
-        var lastLog = DateTime.MinValue;
-        var lastCenterLog = DateTime.MinValue;
-        var cmdMag = 0f;      // 当前实际下发角速度大小(deg/s)，缓启动从 0 斜坡爬升
-        var centerTracking = false;
-        float centerStartX = 0, centerStartY = 0, centerStartTh = 0;
-        float centerLastX = 0, centerLastY = 0, centerLastTh = 0, centerMaxDrift = 0;
-        // 无定位按时长估算时，补上缓启动斜坡少转的等效时间(≈ maxOmega/(2·accel))，使时长更接近目标角。
-        var estDuration = maxOmega > 1e-3 ? targetMag / maxOmega : 0;
-        if (accel > 1e-3) estDuration += maxOmega / (2 * accel);
-
-        DLog.Log(
-            $"REQUEST target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} accel={accel:0.0} " +
-            $"slowDeg={slowDeg:0.0} minOmega={minOmega:0.0} useDetourHeading={hasPos} startTh={startTh:0.00} " +
-            $"estDuration={estDuration:0.00}s syncUseDetour={conf.MultiVehicleSyncUseDetour}",
-            "FleetRotateDbg");
-
-        // 使能脚本驱动的原地旋转（mode2）。TickMultiVehicle 后台循环据此执行旋转并广播给从车。
-        // 起步从 0 角速度开始，由缓启动斜坡爬升，避免阶跃下发导致队形骤偏/抖动。
-        self.MultiVehicleScriptVx = 0;
-        self.MultiVehicleScriptVy = 0;
-        self.MultiVehicleScriptMode = 2;
-        self.MultiVehicleScriptVth = 0;
-        self.MultiVehicleScriptEnabled = true;
-        self.MultiVehicleRotateWheelsReady = false;
-        self.MultiVehicleRotateFleetReady = false;
-
-        DLog.Log("WAIT_ALIGN fleet rotate wheels", "FleetRotateDbg");
-        while (!self.MultiVehicleRotateFleetReady)
-        {
-            self.MultiVehicleScriptVx = 0;
-            self.MultiVehicleScriptVy = 0;
-            self.MultiVehicleScriptMode = 2;
-            self.MultiVehicleScriptVth = 0;
-            self.MultiVehicleScriptEnabled = true;
-            Hedingben.ToastText("车队原地旋转舵轮预对齐中", "FleetRotate");
-            yield return true;
-        }
-
-        float centerStartCarX = 0, centerStartCarY = 0, centerStartCarTh = 0;
-        if (hasPos)
-        {
-            var startPos = DetourInterface.getCartLocation();
-            centerStartCarX = (float)startPos.x;
-            centerStartCarY = (float)startPos.y;
-            centerStartCarTh = (float)startPos.th;
-            prevTh = centerStartCarTh;
-            startTh = prevTh;
-            if (self.TryGetFleetCenterFromPose(centerStartCarX, centerStartCarY, centerStartCarTh,
-                    out centerStartX, out centerStartY, out centerStartTh))
-            {
-                centerLastX = centerStartX;
-                centerLastY = centerStartY;
-                centerLastTh = centerStartTh;
-                centerMaxDrift = 0;
-                centerTracking = true;
-            }
-        }
-        accumulated = 0f;
-        start = DateTime.Now;
-        lastTime = start;
-        lastLog = DateTime.MinValue;
-        lastCenterLog = DateTime.MinValue;
-        cmdMag = 0f;
-        DLog.Log(
-            $"START target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} startTh={startTh:0.00} " +
-            $"fleetAligned={self.MultiVehicleRotateFleetReady}",
-            "FleetRotateDbg");
-        if (centerTracking)
-        {
-            DLog.Log(
-                $"START center=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
-                $"car=({centerStartCarX:0.0},{centerStartCarY:0.0},{centerStartCarTh:0.00}) " +
-                $"target={TargetDeltaDeg:0.0} omega={maxOmega:0.0}",
-                "FleetRotateCenterDbg");
-        }
-
-        var stopReason = "stop()";
-        while (true)
-        {
-            var now = DateTime.Now;
-            var dt = (float)Math.Min(0.2, Math.Max(0, (now - lastTime).TotalSeconds));
-            lastTime = now;
-            var elapsed = (now - start).TotalSeconds;
-
-            float desiredMag;
-            float curTh = 0f, remaining = 0f, actualRate = 0f;
-            if (hasPos)
-            {
-                var carPos = DetourInterface.getCartLocation();
-                curTh = (float)carPos.th;
-                var step = (float)CommonMath.ThDiff(curTh, prevTh); // 本帧实际转角(逆时针为正)
-                accumulated += step;
-                actualRate = dt > 1e-3 ? step / dt : 0f;            // 实际角速率(deg/s)，用于对比指令
-                prevTh = curTh;
-
-                remaining = targetMag - Math.Abs(accumulated);
-                if (remaining <= ArriveDeg) { stopReason = "arrived"; break; }
-
-                // 减速区：剩余角度 < SlowDeg 时，目标角速度按剩余比例线性降到 MinOmega，
-                // 使切断指令瞬间残余动量足够小，抑制惯性滑行造成的超调。宽度直观、便于现场调试。
-                desiredMag = remaining < slowDeg
-                    ? Math.Max(minOmega, maxOmega * (remaining / slowDeg))
-                    : maxOmega;
-
-                if (centerTracking &&
-                    self.TryGetFleetCenterFromPose((float)carPos.x, (float)carPos.y, (float)carPos.th,
-                        out centerLastX, out centerLastY, out centerLastTh))
-                {
-                    var centerDx = centerLastX - centerStartX;
-                    var centerDy = centerLastY - centerStartY;
-                    var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
-                    centerMaxDrift = Math.Max(centerMaxDrift, centerDrift);
-                    var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
-                    if ((now - lastCenterLog).TotalMilliseconds >= 250)
-                    {
-                        lastCenterLog = now;
-                        DLog.Log(
-                            $"ACTION t={elapsed:0.00}s center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
-                            $"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
-                            $"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
-                            $"cmdW={dir * cmdMag:0.000} actualW={actualRate:0.000} acc={accumulated:0.0} remain={remaining:0.0} " +
-                            $"wheelReady={self.MultiVehicleRotateWheelsReady} fleetReady={self.MultiVehicleRotateFleetReady}",
-                            "FleetRotateCenterDbg");
-                    }
-                }
-            }
-            else
-            {
-                // 无定位：按时长估算，无法测角，目标维持巡航速度到估算时长（仅缓启动整形）。
-                desiredMag = maxOmega;
-                if (elapsed >= estDuration) { stopReason = "estDuration"; break; }
-            }
-
-            // 缓启动：只对“加速(目标>当前)”按角加速度限斜率，让起步平滑爬升；
-            // “减速(目标<当前)”跟随上面的减速曲线立即下调，保证及时刹车不超调。
-            if (accel > 1e-3 && desiredMag > cmdMag)
-                cmdMag = Math.Min(desiredMag, cmdMag + accel * dt);
-            else
-                cmdMag = desiredMag;
-
-            self.MultiVehicleScriptVth = dir * cmdMag;
-
-            // 落盘诊断(节流~150ms)：实际航向/累计转角/实际角速率 vs 指令角速率，定位"开环转速不足"。
-            if ((now - lastLog).TotalMilliseconds >= 150)
-            {
-                lastLog = now;
-                DLog.Log(
-                    hasPos
-                        ? $"t={elapsed:0.00}s curTh={curTh:0.00} acc={accumulated:0.0} remain={remaining:0.0} " +
-                          $"cmdW={dir * cmdMag:0.0} actualW={actualRate:0.0} (实际/指令={(Math.Abs(cmdMag) > 1e-3 ? actualRate / (dir * cmdMag) : 0):0.00})"
-                        : $"t={elapsed:0.00}s/{estDuration:0.00}s (无航向反馈,开环按时长) cmdW={dir * cmdMag:0.0}",
-                    "FleetRotateDbg");
-            }
-
-            Hedingben.ToastText(
-                hasPos
-                    ? $"车队原地旋转 目标{TargetDeltaDeg:0.0}° 已转{accumulated:0.0}° 余{targetMag - Math.Abs(accumulated):0.0}° ω={cmdMag:0.0}"
-                    : $"车队原地旋转(无定位,按时长) {elapsed:0.0}/{estDuration:0.0}s ω={cmdMag:0.0}",
-                "FleetRotate");
-
-            yield return true;
-        }
-
-        // 到位：角速度先归零，保持脚本使能让 TickMultiVehicle 的 PI 把队形稳住一小段时间再撤离。
-        self.MultiVehicleScriptVth = 0;
-        var settleEnd = DateTime.Now.AddSeconds(Math.Max(0, SettleSec));
-        while (DateTime.Now < settleEnd)
-            yield return true;
-
-        ClearScript();
-        DLog.Log(
-            $"DONE reason={stopReason} 累计转角={accumulated:0.0}° 目标={TargetDeltaDeg:0.0}° " +
-            $"用时={(DateTime.Now - start).TotalSeconds:0.00}s useDetourHeading={hasPos}",
-            "FleetRotateDbg");
-        if (centerTracking)
-        {
-            var centerDx = centerLastX - centerStartX;
-            var centerDy = centerLastY - centerStartY;
-            var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
-            var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
-            DLog.Log(
-                $"DONE reason={stopReason} center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
-                $"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
-                $"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
-                $"acc={accumulated:0.0} target={TargetDeltaDeg:0.0}",
-                "FleetRotateCenterDbg");
-        }
-        Hedingben.ToastText($"车队原地旋转完成({stopReason}) 累计{accumulated:0.0}°", "FleetRotate");
-    }
-}
-
-[MovementTest(name = "车队联动-原地旋转")]
-public class FleetRotateInPlaceTest : MovementTest
-{
-    private FleetRotateInPlace _proc;
-    private DriveTask _task;
-
+        // 交互输入目标角度后执行底盘原地旋转测试。
         public override void Test()
         {
-        _proc = new FleetRotateInPlace
+            var input = UI.GetInput("输入旋转角度：");
+            if (!float.TryParse(input, out var angleTarget))
             {
-            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");
+                Console.WriteLine(
+                    $"旋转测试输入无效：{input}");
                 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)
+            // 防止重复启动测试时，上一项旋转任务仍在运行。
+            _dt?.Stop();
+            var task = new DriveTask(
+                new MultiWheelRotateInPlace
                 {
-            Speed = PilotDefinition.Conf.FleetCurveSpeed,
-            CarDirectionBias = 0f
-        };
+                    AngleTarget = angleTarget,
 
-        _proc = new FleetCurveWalk
+                    PidparamsRead = () => new PIDParams
                     {
-            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();
+                        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,
                     }
-
-    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()
+                }.Get());
+            _dt = task;
+            try
             {
+                task.Wait();
+            }
+            finally
+            {
+                // 防止旧任务结束时，错误清除后来启动的新任务。
+                if (ReferenceEquals(_dt, task))
+                {
+                    _dt = null;
+                }
+            }
+        }
     }
 }

ClumsyPilot/Movements.cs

@@ -1,34 +1,77 @@
-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>
@@ -46,281 +89,40 @@ 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);
+                thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
+                    p.OutputUpperThreshold, p.SpeedAccPerSec);
 
-            DateTime lastTime = DateTime.Now;
                 while (true)
                 {
                     var s = thPid.GetResponse(targetAngle, true);
                     Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
                     Chassis.SendXYThSpeed(0, 0, s);
-                lastTime = DateTime.Now;
                     if (thPid.IsArrived()) break;
                     yield return true;
                 }
-            Chassis.SendXYThSpeed(0, 0, 0);
+
                 Console.WriteLine($"final rotate to {targetAngle}");
             }
-    }
-
-
-    public class ClampToTarget : MovementDefinition
+            finally
             {
-        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;
+                Chassis.SendXYThSpeed(0, 0, 0);
             }
-
-            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,108 +6,19 @@ namespace MultiWheelC;
 
 public class PilotConfig : MultiWheelPilotConfig
 {
-    [FieldMember(desc = "[sync] steering angle acceleration(deg/s^2)")] public float SyncThAccPerSec = 30f;
-    [FieldMember(desc = "[sync] fleet member distance(mm)")] public float TestCarSyncDistance = 2400f;
-    [FieldMember(desc = "[sync] fleet layout bias angle(deg)")] public float TestCarSyncTh = 0f;
-    // Fleet manual remote IO values are normalized joystick ratios. Keep all speed/angle scaling here.
-    [FieldMember(desc = "[sync] fleet manual max linear speed(m/s)")] public float FleetManualMaxSpeed = 0.3f;
-    [FieldMember(desc = "[sync] fleet manual normal-mode full-stick steering angle(deg)")] public float FleetManualMaxSteerAngleDeg = 45f;
-    [FieldMember(desc = "[sync] fleet manual crab-mode full-stick steering angle(deg)")] public float FleetManualMaxCrabAngleDeg = 60f;
-    [FieldMember(desc = "[sync] fleet manual rotate-mode full-stick angular speed(deg/s)")] public float FleetManualMaxRotateOmegaDegPerSec = 45f;
-    [FieldMember(desc = "[sync] 蟹行舵角上限(deg，应与Medulla舵轮角度限制匹配，默认120)")] public float MultiVehicleCrabSteerLimitDeg = 120f;
-    [FieldMember(desc = "[sync] 检测中心偏移(mm)")] public float DeltaDetectCenter = 350f;
-    // 仅控制"车队内姿态纠正"(POS 补偿)是否使用 Detour 的 SLAM 位姿，不影响"整个车队姿态的计算"。
-    // 默认 false：定位不参与车队内姿态纠正（各车按编队几何/互识别保持队形，不做 SLAM 逐车纠偏）。
-    // 为 true：额外用 getCartLocation() 反推每台车相对编队中心的偏差并做 POS 补偿。
-    // 注意：无论该开关如何，自动模式下整队姿态(反推/广播车队中心、SLAM 间距、自动安全门)始终依赖 Detour 全局定位；
-    // 主车自动模式必调用 getCartLocation()，若无有效全局定位该调用会阻塞 → 联动线程阻塞不下发速度（安全停车）。
-    [FieldMember(desc = "[sync] 定位是否参与车队内姿态纠正(不影响整队姿态计算)")] public bool MultiVehicleSyncUseDetour = false;
-    // 手动外部遥控联动默认只走 2 腿检测/几何同步，避免 Detour getCartLocation 阻塞导致遥控和检测可视化变慢。
-    [FieldMember(desc = "[sync] 手动联动是否启用定位姿态纠正(默认关闭)")] public bool MultiVehicleManualUseDetourCorrection = false;
+    #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 = "直线行走距离")] 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 = "多车联动：本车回连端点 ip:port，供主车 notify 回连，空=127.0.0.1:本车port")] public string MultiVehicleSelfEndpoint = "";
-
-    [JsonProperty("MultiVehicleMasterIp")]
-    private string LegacyMasterIpSetter
-    {
-        set
-        {
-            if (string.IsNullOrEmpty(value) || value == "/") return;
-            if (MultiVehicleMasterEndpoint == "/")
-                MultiVehicleMasterEndpoint = value.Contains(":") ? value : $"{value}:8008";
-        }
-    }
-
-    [FieldMember(desc = "多车联动：启用互识别纠正")] public bool MultiVehicleUseDetect = false;
-
-    // B: 自动速度命令新鲜度(ms)。主车超过此时长未从路径控制器收到新速度命令(路径结束/早退/卡顿)，
-    //    即视为失效并清零下发速度，避免车队按末速度滑行。0 表示自动取 max(200, interval*4)。
-    [FieldMember(desc = "多车联动：自动速度命令超时(ms，0=auto)")] public int MultiVehicleAutoCmdTimeoutMs = 0;
-    // C: fleet 成员存活 TTL(ms)。主车剔除超过此时长未 register/刷新的从车；编队就绪要求所有成员新鲜。
-    //    0 表示自动取 max(500, interval*6)。
-    [FieldMember(desc = "多车联动：成员存活TTL(ms，0=auto)")] public int MultiVehicleMemberTtlMs = 0;
-    // D: 自动模式下用主车路径控制器的理想车队中心(idealPos/idealAngle)作为各车 layout 目标，
-    //    弧线路径上做 per-car 前馈而非仅共用 frontTh/rearTh 事后纠偏。
-    [FieldMember(desc = "多车联动：自动模式按理想中心前馈(弧线)")] public bool MultiVehicleAutoUseIdealCenter = true;
-    // H: 自动模式必须有有效车队中心(SLAM 可反推)，全程定位丢失时停车，避免纯 SLAM 下盲跑。
-    [FieldMember(desc = "多车联动：自动模式要求有效车队中心")] public bool MultiVehicleAutoRequireFleetCenter = true;
-    [FieldMember(desc = "多车联动：SLAM X补偿系数")] public float MultiVehiclePosBiasXFac = 0.5f;
-    [FieldMember(desc = "多车联动：SLAM Y补偿系数")] public float MultiVehiclePosBiasYFac = 0.5f;
-    [FieldMember(desc = "多车联动：SLAM Th补偿系数")] public float MultiVehiclePosBiasThFac = 0.5f;
-    [FieldMember(desc = "多车联动：X补偿阈值(mm)")] public float MultiVehiclePosBiasXThreshold = 50f;
-    [FieldMember(desc = "多车联动：Y补偿阈值(mm)")] public float MultiVehiclePosBiasYThreshold = 50f;
-    [FieldMember(desc = "多车联动：Th补偿阈值(deg)")] public float MultiVehiclePosBiasThThreshold = 5f;
-
-    [FieldMember(desc = "多车联动：互识别 X补偿系数")] public float MultiVehicleDetectBiasXFac = 0.5f;
-    [FieldMember(desc = "多车联动：互识别 Y补偿系数")] public float MultiVehicleDetectBiasYFac = 0.5f;
-    [FieldMember(desc = "多车联动：互识别 Th补偿系数")] public float MultiVehicleDetectBiasThFac = 0.5f;
-    [FieldMember(desc = "多车联动：互识别 X补偿阈值(mm)")] public float MultiVehicleDetectBiasXThreshold = 50f;
-    [FieldMember(desc = "多车联动：互识别 Y补偿阈值(mm)")] public float MultiVehicleDetectBiasYThreshold = 50f;
-    [FieldMember(desc = "多车联动：互识别 Th补偿阈值(deg)")] public float MultiVehicleDetectBiasThThreshold = 5f;
-
-    // 原地旋转(mode2)闭环纠偏(PI)：把"本车应移动到的位置(dx,dy,mm)/应转角(dth,deg)"作为误差，
-    // 用 PI 控制器换算成车体系修正速度叠加到绕队心旋转上。纯 P 对抗恒定横向滑移扰动有稳态残差，
-    // 加积分项把稳态误差拉到 0；积分带限幅(抗 windup)，总输出限幅在 Max 内防过冲/振荡。
-    // Fac=比例增益(mm/s per mm、deg/s per deg)；IFac=积分增益(mm/s per mm·s、deg/s per deg·s)；Max=总输出上限。
-    [FieldMember(desc = "原地旋转纠偏：平移比例增益P(mm/s per mm)")] public float MultiVehicleRotateCompXyFac = 1.2f;
-    [FieldMember(desc = "原地旋转纠偏：平移积分增益I(mm/s per mm·s)")] public float MultiVehicleRotateCompXyIFac = 0.8f;
-    [FieldMember(desc = "原地旋转纠偏：平移速度上限(mm/s)")] public float MultiVehicleRotateCompXyMax = 150f;
-    [FieldMember(desc = "原地旋转纠偏：转向比例增益P(deg/s per deg)")] public float MultiVehicleRotateCompThFac = 0.8f;
-    [FieldMember(desc = "原地旋转纠偏：转向积分增益I(deg/s per deg·s)")] public float MultiVehicleRotateCompThIFac = 0.8f;
-    [FieldMember(desc = "原地旋转纠偏：转向速度上限(deg/s)")] public float MultiVehicleRotateCompThMax = 15f;
-    // 仅当车队实际被指令旋转(|fleetOmega|超过此阈值)时才运行纠偏 PI；否则清零并复位积分，
-    // 避免松开摇杆后积分残留持续驱动车辆"自行旋转停不下来"。
-    [FieldMember(desc = "原地旋转纠偏：生效的最小角速度阈值(deg/s)")] public float MultiVehicleRotateActiveOmega = 0.5f;
-    // 安全网：每轮纠偏速度幅值 <= 该比例 * 本轮旋转切向速度，限制合速度相对纯切向的最大偏角。
-    // 旧配置若仍为 <0，运行时按安全默认 0.10 处理；确需放宽时可在主车显式调大并同步给从车。
-    [FieldMember(desc = "原地旋转纠偏：纠偏/旋转切向比例硬上限，<0使用安全默认0.10")] public float MultiVehicleRotateCompTangentFrac = 0.10f;
-
-    [FieldMember(desc = "单车同步 xy 精度(mm)")] public float SingleCarSyncPrecisionXy = 10f;
-    [FieldMember(desc = "单车同步 th 精度(deg)")] public float SingleCarSyncPrecisionTh = 0.2f;
-
-    [FieldMember(desc = "Playground WebAPI 基地址")]
-    public string PlaygroundWebApiUrl = "http://localhost:18090";
-
-    [FieldMember(desc = "MultiVehicle rotate pose WebAPI diagnostics (simulation only)")]
-    public bool MultiVehicleRotatePoseWebApiDiagEnabled = false;
-
-    [FieldMember(desc = "Playground 小车名称（场景 robots[].name）")]
-    public string PlaygroundRobotName = "agv_multi_1";
-
-    [FieldMember(desc = "Playground 邻车名称（仅主车用于原地旋转位姿诊断）")]
-    public string PlaygroundNeighborRobotName = "agv_multi_2";
-
-    [FieldMember(desc = "WebAPI 平移测试：平移距离(mm)")]
-    public float WebApiTranslateMm = 100f;
-
-    [FieldMember(desc = "WebAPI 旋转测试：旋转角度(deg)")]
-    public float WebApiRotateDeg = 5f;
+    [FieldMember(desc = "终点跟踪:速度")] public float DstTrackerMaxSpeed = 0.3f;
+    #endregion
 
+    #region 单车-原地旋转
     [FieldMember(desc = "原地旋转：目标朝向(世界坐标系, deg)")]
     public float InPlaceRotateTargetWorldDeg = 90f;
 
@@ -123,102 +34,35 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "原地旋转：旋转过程中舵轮偏差重对齐阈值(deg)")]
     public float InPlaceRotateActiveWheelAlignDeg = 10f;
 
-    // ===== 车队联动-原地旋转动作（FleetRotateInPlace / 对应 FleetRemote 原地旋转模式）=====
-    // 通过 Clumsy 内部脚本字段驱动 TickMultiVehicle 的 mode2 旋转（绕车队中心 + PI 纠偏），需主车运行。
-    [FieldMember(desc = "车队原地旋转：角速度大小(deg/s，方向由目标角符号决定)")]
-    public float FleetRotateOmega = 15f;
+    #endregion
 
-    [FieldMember(desc = "车队原地旋转：目标相对转角(deg，+逆时针)")]
-    public float FleetRotateTargetDeltaDeg = 90f;
+    #region 单车-临时
+    [FieldMember(desc = "原地旋转Kp")]
+    public float InPlaceRotateKp = 0.05f;
 
-    [FieldMember(desc = "车队原地旋转：到位角度精度(deg)")]
-    public float FleetRotateArriveDeg = 1.5f;
+    [FieldMember(desc = "原地旋转Ki")]
+    public float InPlaceRotateKi = 0.01f;
 
-    [FieldMember(desc = "车队原地旋转：减速区宽度(deg)，抑制收尾惯性超调")]
-    public float FleetRotateSlowDeg = 25f;
+    [FieldMember(desc = "原地旋转Kd")]
+    public float InPlaceRotateKd = 0f;
 
-    [FieldMember(desc = "车队原地旋转：减速区末段最小角速度(deg/s)")]
-    public float FleetRotateMinOmega = 3f;
+    [FieldMember(desc = "原地旋转积分限幅")]
+    public float InPlaceRotateMaxI = 0.01f;
 
-    [FieldMember(desc = "车队原地旋转：起步缓启动角加速度(deg/s²，<=0关闭)")]
-    public float FleetRotateAccel = 20f;
+    [FieldMember(desc = "原地旋转最大角速度(deg/s)")]
+    public float InPlaceRotateMaxSpeed = 30f;
 
-    [FieldMember(desc = "车队原地旋转：到位后安定时长(s)")]
-    public float FleetRotateSettleSec = 0.5f;
+    [FieldMember(desc = "原地旋转角加速度(deg/s²)")]
+    public float InPlaceRotateAcc = 30f;
 
-    // 与 MultiVehicleSyncUseDetour 解耦：转到指定角度需航向反馈，默认 true 读主车 SLAM 航向闭环判停。
-    // false 时退化为按估算时长开环停止（实际转速≠指令时不精确，易出现"没转到目标就停"）。
-    [FieldMember(desc = "车队原地旋转：用Detour主车航向闭环判停(默认true，false=按时长开环)")]
-    public bool FleetRotateUseDetourHeading = true;
+    [FieldMember(desc = "原地旋转超时(s)")]
+    public float InPlaceRotateTimeoutSec = 15f;
+    #endregion
 
-    // ===== 车队联动-自动蟹行（FleetCrabWalk）=====
-    // 以当前车队中心为起点，构造一条直线路径；MovementTest 中车身保持启动朝向追踪该路径。
-    // 动作侧参考几何控制器的路径跟踪思路，直接写入 MultiVehicleAuto... 字段，不再复用脚本手动链路。
-    [FieldMember(desc = "车队蟹行：路径方向相对启动时车队朝向夹角(deg，逆时针为正；路径在车右侧x度时填-x)")]
-    public float FleetCrabAngleDeg = 45f;
 
-    [FieldMember(desc = "车队蟹行：AGV入口使用的车队世界系目标朝向(deg)")]
-    public float FleetCrabBodyWorldHeadingDeg = 0f;
 
-    [FieldMember(desc = "车队蟹行：路径长度(mm)")]
-    public float FleetCrabLengthMm = 2000f;
 
-    [FieldMember(desc = "车队蟹行：行驶速度(m/s)")]
-    public float FleetCrabSpeed = 0.2f;
-
-    [FieldMember(desc = "车队蟹行：速度命令加速度限制(m/s^2，<=0表示不限制)")]
-    public float FleetCrabAccel = 0.2f;
-
-    [FieldMember(desc = "车队蟹行：预对齐后正式下发速度前5秒加速度(m/s^2，<=0表示不限制)")]
-    public float FleetCrabStartAccel = 0.01f;
-
-    [FieldMember(desc = "车队蟹行：末端开始减速距离(mm)")]
-    public float FleetCrabSlowDistance = 2000f;
-
-    [FieldMember(desc = "车队蟹行：完成距离(mm)，低于该剩余距离结束动作")]
-    public float FleetCrabFinishDistance = 20f;
-
-    [FieldMember(desc = "车队蟹行：末端最低速度(m/s)")]
-    public float FleetCrabFinishSpeed = 0.02f;
-
-    [FieldMember(desc = "车队蟹行：末端减速曲线指数")]
-    public float FleetCrabSlowingPow = 0.8f;
-
-    [FieldMember(desc = "车队蟹行：GCP舵角修正上限(deg)")]
-    public float FleetCrabGcpThetaThreshold = 95f;
-
-    [FieldMember(desc = "车队蟹行：headingErr角度纠偏比例系数")]
-    public float FleetCrabDthLinearFac = 1f;
-
-    [FieldMember(desc = "车队蟹行：headingErr角度纠偏舵角限幅(deg)")]
-    public float FleetCrabDthLinearThreshold = 10f;
-
-    [FieldMember(desc = "FleetCrab startup sync timeout(s)")]
-    public float FleetCrabStartSyncTimeoutSec = 8f;
-
-    [FieldMember(desc = "FleetCrab startup wheel alignment tolerance(deg)")]
-    public float FleetCrabStartWheelAlignDeg = 2f;
-
-    // ===== Fleet linked Bezier curve walk =====
-    [FieldMember(desc = "FleetCurve MovementTest Bezier control point count")]
-    public int FleetCurveTestControlPointCount = 4;
-
-    [FieldMember(desc = "FleetCurve speed(m/s)")]
-    public float FleetCurveSpeed = 0.2f;
-
-    [FieldMember(desc = "FleetCurve slow distance(mm)")]
-    public float FleetCurveSlowDistance = 2000f;
-
-    [FieldMember(desc = "FleetCurve finish distance(mm)")]
-    public float FleetCurveFinishDistance = 20f;
-
-    [FieldMember(desc = "FleetCurve finish speed(m/s)")]
-    public float FleetCurveFinishSpeed = 0.02f;
-
-    [FieldMember(desc = "FleetCurve slowing curve exponent")]
-    public float FleetCurveSlowingPow = 0.8f;
-
-    // ===== 2腿检测（单线雷达识别两腿托盘 / 轮胎）=====
+    #region 单车-钻车与夹抱
     [FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")]
     public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1";
 
@@ -264,7 +108,6 @@ public class PilotConfig : MultiWheelPilotConfig
     [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;
@@ -284,22 +127,6 @@ public class PilotConfig : MultiWheelPilotConfig
     [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;
@@ -320,9 +147,7 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "轮胎跟踪:距离过近角度忽略阈值")] public float TireFollowingAngleIgnoreThr = 0.2f;
     [FieldMember(desc = "轮胎跟踪:Y最大平均数")] public int TireFollowingYAverageFrameCount = 5;
 
-    [FieldMember(desc = "终点跟踪:速度")] public float DstTrackerMaxSpeed = 0.3f;
     [FieldMember(desc = "轮胎跟踪:释放锁点距离")] public float TireFollowingReleaseDistance = 1600;
-    #endregion
 
     [FieldMember(desc = "轮胎跟踪:角度调整kp")] public float TireFollowingThkp = 0.05f;
     [FieldMember(desc = "轮胎跟踪:角度调整ki")] public float TireFollowingThki = 0.01f;
@@ -331,4 +156,178 @@ public class PilotConfig : MultiWheelPilotConfig
     [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 多车-联动动作
+    [FieldMember(desc = "车队原地旋转：角速度大小(deg/s，方向由目标角符号决定)")]
+    public float FleetRotateOmega = 15f;
+
+    [FieldMember(desc = "车队原地旋转：目标相对转角(deg，+逆时针)")]
+    public float FleetRotateTargetDeltaDeg = 90f;
+
+    [FieldMember(desc = "车队原地旋转：到位角度精度(deg)")]
+    public float FleetRotateArriveDeg = 1.5f;
+
+    [FieldMember(desc = "车队原地旋转：减速区宽度(deg)，抑制收尾惯性超调")]
+    public float FleetRotateSlowDeg = 25f;
+
+    [FieldMember(desc = "车队原地旋转：减速区末段最小角速度(deg/s)")]
+    public float FleetRotateMinOmega = 3f;
+
+    [FieldMember(desc = "车队原地旋转：起步缓启动角加速度(deg/s²，<=0关闭)")]
+    public float FleetRotateAccel = 20f;
+
+    [FieldMember(desc = "车队原地旋转：到位后安定时长(s)")]
+    public float FleetRotateSettleSec = 0.5f;
+
+    [FieldMember(desc = "车队原地旋转：用Detour主车航向闭环判停(默认true，false=按时长开环)")]
+    public bool FleetRotateUseDetourHeading = true;
+
+    [FieldMember(desc = "车队蟹行：路径方向相对启动时车队朝向夹角(deg，逆时针为正；路径在车右侧x度时填-x)")]
+    public float FleetCrabAngleDeg = 45f;
+
+    [FieldMember(desc = "车队蟹行：AGV入口使用的车队世界系目标朝向(deg)")]
+    public float FleetCrabBodyWorldHeadingDeg = 0f;
+
+    [FieldMember(desc = "车队蟹行：路径长度(mm)")]
+    public float FleetCrabLengthMm = 2000f;
+
+    [FieldMember(desc = "车队蟹行：行驶速度(m/s)")]
+    public float FleetCrabSpeed = 0.2f;
+
+    [FieldMember(desc = "车队蟹行：速度命令加速度限制(m/s^2，<=0表示不限制)")]
+    public float FleetCrabAccel = 0.2f;
+
+    [FieldMember(desc = "车队蟹行：预对齐后正式下发速度前5秒加速度(m/s^2，<=0表示不限制)")]
+    public float FleetCrabStartAccel = 0.01f;
+
+    [FieldMember(desc = "车队蟹行：末端开始减速距离(mm)")]
+    public float FleetCrabSlowDistance = 2000f;
+
+    [FieldMember(desc = "车队蟹行：完成距离(mm)，低于该剩余距离结束动作")]
+    public float FleetCrabFinishDistance = 20f;
+
+    [FieldMember(desc = "车队蟹行：末端最低速度(m/s)")]
+    public float FleetCrabFinishSpeed = 0.02f;
+
+    [FieldMember(desc = "车队蟹行：末端减速曲线指数")]
+    public float FleetCrabSlowingPow = 0.8f;
+
+    [FieldMember(desc = "车队蟹行：GCP舵角修正上限(deg)")]
+    public float FleetCrabGcpThetaThreshold = 95f;
+
+    [FieldMember(desc = "车队蟹行：headingErr角度纠偏比例系数")]
+    public float FleetCrabDthLinearFac = 1f;
+
+    [FieldMember(desc = "车队蟹行：headingErr角度纠偏舵角限幅(deg)")]
+    public float FleetCrabDthLinearThreshold = 10f;
+
+    [FieldMember(desc = "FleetCrab startup sync timeout(s)")]
+    public float FleetCrabStartSyncTimeoutSec = 8f;
+
+    [FieldMember(desc = "FleetCrab startup wheel alignment tolerance(deg)")]
+    public float FleetCrabStartWheelAlignDeg = 2f;
+
+    [FieldMember(desc = "FleetCurve MovementTest Bezier control point count")]
+    public int FleetCurveTestControlPointCount = 4;
+
+    [FieldMember(desc = "FleetCurve speed(m/s)")]
+    public float FleetCurveSpeed = 0.2f;
+
+    [FieldMember(desc = "FleetCurve slow distance(mm)")]
+    public float FleetCurveSlowDistance = 2000f;
+
+    [FieldMember(desc = "FleetCurve finish distance(mm)")]
+    public float FleetCurveFinishDistance = 20f;
+
+    [FieldMember(desc = "FleetCurve finish speed(m/s)")]
+    public float FleetCurveFinishSpeed = 0.02f;
+
+    [FieldMember(desc = "FleetCurve slowing curve exponent")]
+    public float FleetCurveSlowingPow = 0.8f;
+    #endregion
+
+#endif
+
+
+
 }

ClumsyPilot/PlaygroundWebApi.cs

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

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

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

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

@@ -0,0 +1,163 @@
+{
+  "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,23 +1,23 @@
 {
   "format": 1,
   "restore": {
-    "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj": {}
+    "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj": {}
   },
   "projects": {
-    "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj": {
+    "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj": {
       "version": "1.0.0",
       "restore": {
-        "projectUniqueName": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
+        "projectUniqueName": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
         "projectName": "ClumsyPilot",
-        "projectPath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
-        "packagesPath": "C:\\Users\\Fairyland\\.nuget\\packages\\",
-        "outputPath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\obj\\",
+        "projectPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
+        "packagesPath": "C:\\Users\\admin\\.nuget\\packages\\",
+        "outputPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\obj\\",
         "projectStyle": "PackageReference",
         "fallbackFolders": [
-          "D:\\VS2022\\Shared\\NuGetPackages"
+          "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages"
         ],
         "configFilePaths": [
-          "C:\\Users\\Fairyland\\AppData\\Roaming\\NuGet\\NuGet.Config",
+          "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"
         ],
@@ -58,7 +58,7 @@
             },
             "Newtonsoft.Json": {
               "target": "Package",
-              "version": "[13.0.4, )"
+              "version": "[13.0.3, )"
             },
             "System.Numerics.Vectors": {
               "target": "Package",
@@ -76,7 +76,7 @@
           ],
           "assetTargetFallback": true,
           "warn": true,
-          "runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.308\\RuntimeIdentifierGraph.json"
+          "runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.316\\RuntimeIdentifierGraph.json"
         }
       }
     }

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

@@ -5,12 +5,12 @@
     <RestoreTool Condition=" '$(RestoreTool)' == '' ">NuGet</RestoreTool>
     <ProjectAssetsFile Condition=" '$(ProjectAssetsFile)' == '' ">$(MSBuildThisFileDirectory)project.assets.json</ProjectAssetsFile>
     <NuGetPackageRoot Condition=" '$(NuGetPackageRoot)' == '' ">$(UserProfile)\.nuget\packages\</NuGetPackageRoot>
-    <NuGetPackageFolders Condition=" '$(NuGetPackageFolders)' == '' ">C:\Users\Fairyland\.nuget\packages\;D:\VS2022\Shared\NuGetPackages</NuGetPackageFolders>
+    <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.2</NuGetToolVersion>
+    <NuGetToolVersion Condition=" '$(NuGetToolVersion)' == '' ">6.14.3</NuGetToolVersion>
   </PropertyGroup>
   <ItemGroup Condition=" '$(ExcludeRestorePackageImports)' != 'true' ">
-    <SourceRoot Include="C:\Users\Fairyland\.nuget\packages\" />
-    <SourceRoot Include="D:\VS2022\Shared\NuGetPackages\" />
+    <SourceRoot Include="C:\Users\admin\.nuget\packages\" />
+    <SourceRoot Include="C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages\" />
   </ItemGroup>
 </Project>

ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfo.cs

@@ -1,10 +1,9 @@
 //------------------------------------------------------------------------------
 // <auto-generated>
-//     此代码由工具生成。
-//     运行时版本:4.0.30319.42000
+//     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>
 //------------------------------------------------------------------------------
 
@@ -14,7 +13,7 @@ using System.Reflection;
 [assembly: System.Reflection.AssemblyCompanyAttribute("ClumsyPilot")]
 [assembly: System.Reflection.AssemblyConfigurationAttribute("Debug")]
 [assembly: System.Reflection.AssemblyFileVersionAttribute("1.0.0.0")]
-[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0")]
+[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0+580a936a830dcb7a25ef327cf553341405264033")]
 [assembly: System.Reflection.AssemblyProductAttribute("ClumsyPilot")]
 [assembly: System.Reflection.AssemblyTitleAttribute("ClumsyPilot")]
 [assembly: System.Reflection.AssemblyVersionAttribute("1.0.0.0")]

ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfoInputs.cache

@@ -1 +1 @@
-5d77794fa0720c6591db5b06ac60427413c18989a6f7b64420ccb07d122d85bc
+038d57c5b1714403d308c9343b385ef762951607b63a9fb275f4d7d2b8fd29cb

ClumsyPilot/obj/Debug/ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig

@@ -1,6 +1,6 @@
 is_global = true
-build_property.RootNamespace = ClumsyPilot
-build_property.ProjectDir = D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\
+build_property.RootNamespace = MultiWheelC
+build_property.ProjectDir = d:\MyParking\ClumsyPilot\
 build_property.EnableComHosting = 
 build_property.EnableGeneratedComInterfaceComImportInterop = 
 build_property.CsWinRTUseWindowsUIXamlProjections = false

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

@@ -1 +1 @@
-622a214d5f25f6580d0c98fb28e188cba923d74f280b70d75d03c4009c18c55b
+3920568398d269aea7b71fb4581ad111777c15ca3f2f2bb272ee9e0a989215c4

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

@@ -1,11 +1,34 @@
-D:\@FariyLandTask\@FRLD-GitProject\build\Clumsy\ClumsyPilot.deps.json
-D:\@FariyLandTask\@FRLD-GitProject\build\Clumsy\ClumsyPilot.dll
-D:\@FariyLandTask\@FRLD-GitProject\build\Clumsy\ClumsyPilot.pdb
-D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.csproj.AssemblyReference.cache
-D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig
-D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.AssemblyInfoInputs.cache
-D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.AssemblyInfo.cs
-D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.csproj.CoreCompileInputs.cache
-D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPi.5EF10E9F.Up2Date
-D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.dll
-D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.pdb
+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/Debug/netstandard2.0/.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/netstandard2.0/ClumsyPilot.AssemblyInfo.cs

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

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

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

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

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

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

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

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

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

ClumsyPilot/obj/project.assets.json

@@ -26,7 +26,7 @@
           "build/netstandard2.0/NETStandard.Library.targets": {}
         }
       },
-      "Newtonsoft.Json/13.0.4": {
+      "Newtonsoft.Json/13.0.3": {
         "type": "package",
         "compile": {
           "lib/netstandard2.0/Newtonsoft.Json.dll": {
@@ -199,10 +199,10 @@
         "netstandard.library.nuspec"
       ]
     },
-    "Newtonsoft.Json/13.0.4": {
-      "sha512": "pdgNNMai3zv51W5aq268sujXUyx7SNdE2bj1wZcWjAQrKMFZV260lbqYop1d2GM67JI1huLRwxo9ZqnfF/lC6A==",
+    "Newtonsoft.Json/13.0.3": {
+      "sha512": "HrC5BXdl00IP9zeV+0Z848QWPAoCr9P3bDEZguI+gkLcBKAOxix/tLEAAHC+UvDNPv4a2d18lOReHMOagPa+zQ==",
       "type": "package",
-      "path": "newtonsoft.json/13.0.4",
+      "path": "newtonsoft.json/13.0.3",
       "files": [
         ".nupkg.metadata",
         ".signature.p7s",
@@ -224,7 +224,7 @@
         "lib/netstandard1.3/Newtonsoft.Json.xml",
         "lib/netstandard2.0/Newtonsoft.Json.dll",
         "lib/netstandard2.0/Newtonsoft.Json.xml",
-        "newtonsoft.json.13.0.4.nupkg.sha512",
+        "newtonsoft.json.13.0.3.nupkg.sha512",
         "newtonsoft.json.nuspec",
         "packageIcon.png"
       ]
@@ -254,28 +254,28 @@
   "projectFileDependencyGroups": {
     ".NETStandard,Version=v2.0": [
       "NETStandard.Library >= 2.0.3",
-      "Newtonsoft.Json >= 13.0.4",
+      "Newtonsoft.Json >= 13.0.3",
       "System.Numerics.Vectors >= 4.6.1"
     ]
   },
   "packageFolders": {
-    "C:\\Users\\Fairyland\\.nuget\\packages\\": {},
-    "D:\\VS2022\\Shared\\NuGetPackages": {}
+    "C:\\Users\\admin\\.nuget\\packages\\": {},
+    "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages": {}
   },
   "project": {
     "version": "1.0.0",
     "restore": {
-      "projectUniqueName": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
+      "projectUniqueName": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
       "projectName": "ClumsyPilot",
-      "projectPath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
-      "packagesPath": "C:\\Users\\Fairyland\\.nuget\\packages\\",
-      "outputPath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\obj\\",
+      "projectPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
+      "packagesPath": "C:\\Users\\admin\\.nuget\\packages\\",
+      "outputPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\obj\\",
       "projectStyle": "PackageReference",
       "fallbackFolders": [
-        "D:\\VS2022\\Shared\\NuGetPackages"
+        "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages"
       ],
       "configFilePaths": [
-        "C:\\Users\\Fairyland\\AppData\\Roaming\\NuGet\\NuGet.Config",
+        "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"
       ],
@@ -316,7 +316,7 @@
           },
           "Newtonsoft.Json": {
             "target": "Package",
-            "version": "[13.0.4, )"
+            "version": "[13.0.3, )"
           },
           "System.Numerics.Vectors": {
             "target": "Package",
@@ -334,7 +334,7 @@
         ],
         "assetTargetFallback": true,
         "warn": true,
-        "runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.308\\RuntimeIdentifierGraph.json"
+        "runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.316\\RuntimeIdentifierGraph.json"
       }
     }
   }

ClumsyPilot/obj/project.nuget.cache

@@ -1,13 +1,13 @@
 {
   "version": 2,
-  "dgSpecHash": "gHGYwM2i+So=",
+  "dgSpecHash": "YBvImiCcgSo=",
   "success": true,
-  "projectFilePath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
+  "projectFilePath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
   "expectedPackageFiles": [
-    "C:\\Users\\Fairyland\\.nuget\\packages\\microsoft.netcore.platforms\\1.1.0\\microsoft.netcore.platforms.1.1.0.nupkg.sha512",
-    "C:\\Users\\Fairyland\\.nuget\\packages\\netstandard.library\\2.0.3\\netstandard.library.2.0.3.nupkg.sha512",
-    "C:\\Users\\Fairyland\\.nuget\\packages\\newtonsoft.json\\13.0.4\\newtonsoft.json.13.0.4.nupkg.sha512",
-    "C:\\Users\\Fairyland\\.nuget\\packages\\system.numerics.vectors\\4.6.1\\system.numerics.vectors.4.6.1.nupkg.sha512"
+    "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,21 +1,15 @@
-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,13 +1,12 @@
-using CartActivator;
+// 定义车型、上下层IO、参数和MCU初始化
+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
 {
@@ -15,49 +14,32 @@ namespace MedullaAdapter
     [UseLadderLogic(logic = typeof(MotorRoutine), scanInterval = 50)]
     [UseLadderLogic(logic = typeof(MCURoutine), scanInterval = 20)]
     [UseManualController(manualController = typeof(Remote))]
-    public partial class DiverCartDefinition : MultiWheelCartDefinition
+    public class DiverCartDefinition : MultiWheelCartDefinition
     {
-        public DiverCommunication Embedded;
+        #region 基本成员
         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;
@@ -66,11 +48,8 @@ namespace MedullaAdapter
         [AsLowerIO(desc = "左后右轮实际位置")] public float LRRActualPos;
         [AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
         [AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
-
-        [AsUpperIO(desc = "左夹臂下发速度", timeOutReset = true)] public float SpeedLeftArm;
-        [AsUpperIO(desc = "右夹臂下发速度", timeOutReset = true)] public float SpeedRightArm;
-        [AsUpperIO(desc = "左夹臂实际速度")] public float ActualSpeedLeftArm;
-        [AsUpperIO(desc = "右夹臂实际速度")] public float ActualSpeedRightArm;
+        [AsLowerIO(desc = "左夹臂实际速度")] public float ActualSpeedLeftArm;
+        [AsLowerIO(desc = "右夹臂实际速度")] public float ActualSpeedRightArm;
         [AsLowerIO(desc = "左夹臂状态字")] public int LeftArmStateCode;
         [AsLowerIO(desc = "右夹臂状态字")] public int RightArmStateCode;
         [AsLowerIO(desc = "左夹臂错误字")] public int LeftArmErrorCode;
@@ -79,86 +58,85 @@ namespace MedullaAdapter
         [AsLowerIO(desc = "右夹臂电流")] public float RightArmElectric;
         [AsLowerIO(desc = "左夹臂实际位置")] public float ActualPosLeftArm;
         [AsLowerIO(desc = "右夹臂实际位置")] public float ActualPosRightArm;
-        [AsLowerIO(desc = "test")] public float test;
-        [AsLowerIO(desc = "test1")] public byte test1;
-        [AsLowerIO(desc = "test2")] public byte test2;
-        [AsInitParam(desc = "test3")] public int test3=24;
-        [IOObjectMonitor(desc = "灯光模式")] public int LightMode = 0;
-        [AsInitParam(desc = "触发模式")] public int trigger = 0;
-
-        [AsInitParam(desc = "手动控制夹臂速度系数")] public float ManualArmSpeedFac = 1.0f;
+        [AsLowerIO(desc = "驱动轮使能状态")] public bool WheelAbleState = true;
+        [AsLowerIO(desc = "电池健康状态")] public float SOH;
         [AsInitParam(desc = "车号")][AsLowerIO] public int CarNum = 1;
-        [AsInitParam(desc = "初始音量")] public int MusicVolume = 5;
+        #endregion
 
+        #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 LeftArmLowerPos = -10000;
         [AsInitParam(desc = "左夹臂高限位")][AsLowerIO] public int LeftArmUpperPos = 5927610;
         [AsInitParam(desc = "右夹臂低限位")][AsLowerIO] public int RightArmLowerPos = -17295;
         [AsInitParam(desc = "右夹臂高限位")][AsLowerIO] public int RightArmUpperPos = 5927610;
 
-        [AsInitParam(desc = "MCU端口号")] public string MCUPort = "COM4";
 
-        [AsLowerIO(desc = "陀螺仪角度")] public float GyrosTh;
-        [AsLowerIO(desc = "电池健康状态")] public float SOH;
+        #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 监控参数
+        [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
 
+        #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: {0}", err.ToDescription());
+                Console.WriteLine($"MCU Open FAILED: {err.ToDescription()}");
                 return;
             }
             else
             {
                 Console.WriteLine("MCU Open OK");
             }
-
             //Step2:远程复位MCU
             err = Bridge.Reset();
             if (err != MCUSerialBridgeError.OK)
             {
-                Console.WriteLine("MCU Reset FAILED: {0}", err.ToDescription());
+                Console.WriteLine($"MCU Reset FAILED: {err.ToDescription()}");
                 return;
             }
             else
@@ -166,31 +144,28 @@ namespace MedullaAdapter
                 Thread.Sleep(500);
                 Console.WriteLine("MCU Reset OK");
             }
-
-            //Step3:GetVersion
+            //Step3:获取MCU版本号
             err = Bridge.GetVersion(out var version, 100);
             if (err != MCUSerialBridgeError.OK)
             {
-                Console.WriteLine("MCU GetVersion FAILED: {0}", err.ToDescription());
+                Console.WriteLine($"MCU GetVersion FAILED: {err.ToDescription()}");
                 return;
             }
             else
             {
-                Console.WriteLine("MCU GetVersion OK: {0}", version.ToString());
+                Console.WriteLine($"MCU GetVersion OK: {version}");
             }
-
-            //Step4:GetState
+            //Step4:获取MCU状态
             err = Bridge.GetState(out var state, 100);
             if (err != MCUSerialBridgeError.OK)
             {
-                Console.WriteLine("MCU GetState FAILED: {0}", err.ToDescription());
+                Console.WriteLine($"MCU GetState FAILED: {err.ToDescription()}");
                 return;
             }
             else
             {
-                Console.WriteLine("MCU GetState OK: {0}", state.ToString());
+                Console.WriteLine($"MCU GetState OK: {state}");
             }
-
             //Step5:串口/CAN配置
             try
             {
@@ -203,18 +178,15 @@ namespace MedullaAdapter
                 for (int i = 0; i < ports.Count; i++)
                 {
                     if (ports[i] is SerialPortConfig s)
-                        Console.WriteLine("Port {0}: Serial, Baud={1}, ReceiveFrameMs={2}",
-                            i,
-                            s.Baud,
-                            s.ReceiveFrameMs);
+                        Console.WriteLine($"Port {i}: Serial, Baud={s.Baud}, ReceiveFrameMs={s.ReceiveFrameMs}");
                     else if (ports[i] is CANPortConfig c)
-                        Console.WriteLine("Port {0}: CAN, Baud={1}, RetryTimeMs={2}", i, c.Baud, c.RetryTimeMs);
+                        Console.WriteLine($"Port {i}: CAN, Baud={c.Baud}, RetryTimeMs={c.RetryTimeMs}");
                 }
 
                 var ret = Bridge.Configure(ports, 200);
                 if (ret != MCUSerialBridgeError.OK)
                 {
-                    Console.WriteLine("MCU Configure FAILED: {0}", ret.ToDescription());
+                    Console.WriteLine($"MCU Configure FAILED: {ret.ToDescription()}");
                     return;
                 }
                 else
@@ -225,54 +197,221 @@ namespace MedullaAdapter
             }
             catch (Exception ex)
             {
-                Console.WriteLine("Configure Exception: {0}", ex.Message);
+                Console.WriteLine($"Configure Exception: {ex.Message}");
+                return;
             }
-            //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 thPow = (float)Math.Pow(Math.Abs(x), ManualThetaPow) * Math.Sign(x);
-            var frontTh = -thPow * MaxManualTheta;
-            var rearTh = thPow * MaxManualTheta;
+            var omega = CalculateManualOmega(speed, x);
+            ManualMode = (int)mode;
+
             switch (mode)
             {
                 case ManualControlMode.Normal:
-                    ManualMode = 0;
-                    Chassis.DirectionAngle = frontDirection;
-                    Chassis.SendMotion(speed, frontTh, rearTh, interval);
+                    SendBodyCommand(vx: speed, vy: 0.0, omegaRadiansPerSecond: omega, interval);
                     break;
-
-                case ManualControlMode.Sway:
-                    ManualMode = 1;
-                    Chassis.DirectionAngle = frontDirection;
-                    Chassis.SendMotion(speed, frontTh, -rearTh, interval);
-                    break;
-
                 case ManualControlMode.Crab:
-                    ManualMode = 2;
-                    Chassis.DirectionAngle = 90;
-                    Chassis.SendMotion(speed, frontTh, rearTh, interval);
+                    SendBodyCommand(vx: 0.0, vy: speed, omegaRadiansPerSecond: omega, interval);
                     break;
-
                 case ManualControlMode.Spin:
-                    ManualMode = 3;
-                    Chassis.SendRotateMotion(speed * MaxAngularSpeed, interval);
+                    var spinOmega =
+                        speed * MaxAngularSpeed *
+                        Math.PI / 180.0;
+
+                    SendBodyCommand(
+                        vx: 0.0,
+                        vy: 0.0,
+                        omegaRadiansPerSecond: spinOmega,
+                        interval);
+                    break;
+                default:
+                    ManualMode = -1;
+                    adapter.StopImmediately();
                     break;
             }
         }
 
-        internal enum ManualControlMode
+        // 停车后切换模式：先预转舵轮，实际角度到位后才允许发送运动命令。
+        private bool EnsureManualModeReady(
+            ManualControlMode mode,
+            TimeSpan? interval)
         {
-            Normal = 0,
-            Spin = 1,
-            Crab = 2,
-            Sway = 3
+            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;
+        }
+
+        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

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

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

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

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

MedullaAdapter/FodyWeavers.xsd

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

@@ -1,291 +0,0 @@
-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,4 +1,5 @@
-using CartActivator;
+// 实际CAN协议、反馈解析、IO、电池、急停
+using CartActivator;
 using FundamentalLib;
 using MCUSerialBridgeCLR;
 using System;
@@ -6,8 +7,7 @@ using System.Collections.Generic;
 
 namespace MedullaAdapter
 {
-    //[LogicRunOnMCU(scanInterval = 20)]
-    public class MCURoutine:LadderLogic<DiverCartDefinition>
+    public class MCURoutine : LadderLogic<DiverCartDefinition>
     {
         private int _lastIteration = 0;
         private int _count = 0;
@@ -27,22 +27,27 @@ 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)
@@ -153,6 +158,7 @@ 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
@@ -422,11 +428,13 @@ namespace MedullaAdapter
 
         }
 
+        // M层CAN安全：判断单个驱动节点是否处于可运行状态。
         private static bool IsNodeOperational(byte remoteCode)
         {
             return remoteCode == 5 || remoteCode == 133;
         }
 
+        // M层CAN安全：检查全部车轮和夹臂节点是否已上线。
         private bool AreAllNodesOperational()
         {
             return IsNodeOperational(cart.LFLRemoteCode)
@@ -441,6 +449,7 @@ 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);
@@ -455,6 +464,7 @@ namespace MedullaAdapter
             sendCan(0, 0x708, Array.Empty<byte>(), true, 2);
         }
 
+        // M层CAN通信：按需注册驱动器反馈报文回调。
         private void EnsureCanCallbacksRegistered()
         {
             if (_canCallbackRegistered)
@@ -478,6 +488,7 @@ namespace MedullaAdapter
             _canCallbackRegistered = true;
         }
 
+        // M层串口通信：按需注册电池等串口设备回调。
         private void EnsureSerialCallbacksRegistered()
         {
             if (_serialCallbackRegistered)
@@ -497,6 +508,7 @@ namespace MedullaAdapter
             _serialCallbackRegistered = err1 == MCUSerialBridgeError.OK;
         }
 
+        // M层单车电源：周期发送Modbus电池状态查询。
         private void PollBatterySerial(int iteration)
         {
             if (cart?.Bridge == null) return;
@@ -522,6 +534,7 @@ namespace MedullaAdapter
             TryUpdateBatteryData(msg);
         }
 
+        // M层单车电源：校验并解析电池Modbus响应。
         private void TryUpdateBatteryData(byte[] msg)
         {
             // Modbus RTU response: [id,03,08,data(8),crc(2)]
@@ -536,6 +549,7 @@ namespace MedullaAdapter
             cart.ElectricCurrent = -ReadInt16BE(msg, 9) * 0.1f;
         }
 
+        // M层单车电源：构造读取电池寄存器的Modbus请求。
         private static byte[] BuildBatteryRequest()
         {
             // 01 03 00 03 00 04 CRC (读取寄存器 03~06)
@@ -546,6 +560,7 @@ namespace MedullaAdapter
             return req;
         }
 
+        // M层协议辅助：计算Modbus RTU的CRC16校验值。
         private static ushort ComputeModbusCrc(byte[] data, int length)
         {
             ushort crc = 0xFFFF;
@@ -562,16 +577,19 @@ 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>>

MedullaAdapter/MCUSerialBridgeCLR.cs

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

MedullaAdapter/MedullaAdapter.csproj

@@ -1,44 +1,57 @@
 <Project Sdk="Microsoft.NET.Sdk">
 
   <PropertyGroup>
-    <TargetFramework>netstandard2.0</TargetFramework>
-	<LangVersion>latest</LangVersion>
+    <TargetFramework>net8.0</TargetFramework>
+    <ImplicitUsings>enable</ImplicitUsings>
+    <Nullable>disable</Nullable>
+    <Platforms>AnyCPU</Platforms>
+
+    <AssemblyName>MedullaAdapter</AssemblyName>
+
+    <AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
+    <OutputPath>build\Medulla\plugins\</OutputPath>
   </PropertyGroup>
 
   <ItemGroup>
-	  <Compile Remove="EmbeddedCommunication.cs" />
-  </ItemGroup>
-	
-  <ItemGroup>
-    <PackageReference Include="Fody" Version="6.9.3">
-      <PrivateAssets>all</PrivateAssets>
-      <IncludeAssets>runtime; build; native; contentfiles; analyzers; buildtransitive</IncludeAssets>
-    </PackageReference>
-    <PackageReference Include="Newtonsoft.Json" Version="13.0.4" />
-    <PackageReference Include="System.IO.Ports" Version="6.0.0" />
-  </ItemGroup>
-
-  <ItemGroup>
-    <Reference Include="CommonUsage">
-      <HintPath>ref\CommonUsage.dll</HintPath>
-    </Reference>
-    <Reference Include="CycleGUI">
-      <HintPath>ref\CycleGUI.dll</HintPath>
-    </Reference>
-    <Reference Include="MDCSToolBox">
-      <HintPath>ref\MDCSToolBox.dll</HintPath>
-    </Reference>
     <Reference Include="CartActivator">
       <HintPath>ref\RefCartActivator.dll</HintPath>
-    </Reference>
-    <Reference Include="FundamentalLib">
-      <HintPath>ref\RefFundamentalLib.dll</HintPath>
+      <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>
   </ItemGroup>
+
   <ItemGroup>
-		<WeaverFiles Include="deps\DiverCompiler.exe" />
-	</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>
+
 </Project>

MedullaAdapter/MotorRoutine.cs

@@ -1,4 +1,5 @@
-using CartActivator;
+// 计算8个驱动电机的目标速度和舵角PID
+using CartActivator;
 using CommonUsage.Mathematics;
 using FundamentalLib;
 using MDCSToolBox.Commons;
@@ -10,224 +11,352 @@ 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)
         {
-            void ResetMultiVehicle(string reason)
+            if (!cart.GhostMode && cart.State == -1) return;
+            // SA稳定打开后，使能实体遥控器。
+            TriggerOnce(
+                cart.TransmitterConnected && cart.Transmitter_SA,
+                300,
+                () =>
                 {
-                if (cart.MultiVehicleRemoteManualEnabled)
+                    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)
             {
-                    LogTransmitterFleet("reset-skipped-remote-panel");
+                TransmitterChassisControl();
+                cart.TransmitterLastTime = DateTime.Now;
+                cart.CarStatu = "实体遥控器控制";
+            }
+            else
+            {
+                // 只在遥控器刚刚退出时发送一次停车，
+                // 不能每周期停车，否则会覆盖C层轨迹控制。
+                if (_wasTransmitterControlling)
+                {
+                    cart.ManualControl(
+                        cart.TransmitterControlMode,
+                        0, 0, 0,
+                        cart.TransmitterSpeed,
+                        DateTime.Now - cart.TransmitterLastTime);
+
+                    StopClampArms();
+                    cart.TransmitterLastTime = DateTime.Now;
+                }
+
+                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();
+
+                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;
             }
 
-                if (cart.MultiVehicleManualEnabled || cart.MultiVehicleManualVx != 0 || cart.MultiVehicleManualVy != 0 ||
-                    cart.MultiVehicleManualVth != 0 || cart.MultiVehicleHold)
-                    LogTransmitterFleetReset(reason);
+            // 更新左前舵轮PID参数。
+            cart.LeftFrontPid.ChangeParameters(
+                cart.DiffSteerKp,
+                cart.DiffSteerKi,
+                cart.DiffSteerKd,
+                cart.DiffSteerMaxI,
+                cart.DiffSteerDeadZone,
+                cart.DiffSteerThresh,
+                cart.DiffSteerSpeedAcc);
 
-                cart.MultiVehicleManualEnabled = false;
-                cart.MultiVehicleManualVx = 0;
-                cart.MultiVehicleManualVy = 0;
-                cart.MultiVehicleManualVth = 0;
-                cart.MultiVehicleHold = false;
-            }
+            // 更新左后舵轮PID参数。
+            cart.LeftRearPid.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.RightFrontPid.ChangeParameters(
+                cart.DiffSteerKp,
+                cart.DiffSteerKi,
+                cart.DiffSteerKd,
+                cart.DiffSteerMaxI,
+                cart.DiffSteerDeadZone,
+                cart.DiffSteerThresh,
+                cart.DiffSteerSpeedAcc);
 
-            if (cart.TransmitterControlEnable && cart.Transmitter_SC == cart.CarNum)
-            {
-                CartDefinition.testPriority(5, "TransmitterMode");
-                ResetMultiVehicle("single-car-control");
-                LogTransmitterFleet("single-car-control");
-                TransmitterChassisControl();
-                cart.TransmitterLastTime = DateTime.Now;
-                cart.CarStatu = "Transmitter控制模式使能";
-            }
-            else if (cart.TransmitterControlEnable && cart.Transmitter_SC == 3)
-            {
-                cart.CarStatu = "Transmitter双车联动模式使能";
-                cart.MultiVehicleManualEnabled = true;
-                var fleetMode = 0;
-                if (cart.Transmitter_SB == TransmitterState.Mode0) fleetMode = 0;
-                else if (cart.Transmitter_SB == TransmitterState.Mode1) fleetMode = 1;
-                else if (cart.Transmitter_SB == TransmitterState.Mode2) fleetMode = 2;
-                cart.ApplyFleetManualCommand(fleetMode, -cart.TransmitterLeftJoystickValX,
-                    cart.TransmitterRightJoystickValY, 1f);
+            // 更新右后舵轮PID参数。
+            cart.RightRearPid.ChangeParameters(
+                cart.DiffSteerKp,
+                cart.DiffSteerKi,
+                cart.DiffSteerKd,
+                cart.DiffSteerMaxI,
+                cart.DiffSteerDeadZone,
+                cart.DiffSteerThresh,
+                cart.DiffSteerSpeedAcc);
 
-                // hold multi-vehicle sync, make speed 0
-                cart.MultiVehicleHold = cart.Transmitter_SD == TransmitterState.Mode1;
-                LogTransmitterFleet("fleet-control");
-            }
-            else
-            {
-                ResetMultiVehicle("idle-or-sc-mismatch");
-                LogTransmitterFleet("idle-or-sc-mismatch");
-                cart.CarStatu = "正常运行";
-            }
+            // 根据实际舵角计算四条腿的差速修正量。
+            var diffLf = cart.LeftFrontPid.GetResponse(
+                cart.ThLeftFront, false, false, "LF");
 
-            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}");
+            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.SpeedRFL = cart.SpeedRightFrontLeft - diffRf;
-            cart.SpeedRFR = cart.SpeedRightFrontRight + diffRf;
+
+            // 左后腿。
             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;
-            List<float> wheelSpeed = new float[]
-            {
-                Math.Abs(cart.SpeedLFL), Math.Abs(cart.SpeedLFR), Math.Abs(cart.SpeedRFL),
-                Math.Abs(cart.SpeedRFR), Math.Abs(cart.SpeedLRL),Math.Abs(cart.SpeedLRR), Math.Abs(cart.SpeedRRL),
-                Math.Abs(cart.SpeedRRR)
-            }.ToList();
-            var wheelMaxSpeed = wheelSpeed.Max();
-            var speedSign = Math.Sign(cart.ThresSpeed - cart.SendThresSpeed);
-            var acc = Math.Abs(cart.ThresSpeed) > Math.Abs(cart.SendThresSpeed) ? cart.Chassis.AccPerSecond : cart.Chassis.DeAccPerSecond;
-            cart.SendThresSpeed += speedSign * Math.Min(Math.Abs(cart.ThresSpeed - cart.SendThresSpeed),
-                acc * (float)(DateTime.Now - _lastMoveTime).TotalSeconds);
-            if (cart.ThresSpeed < wheelMaxSpeed && cart.SendThresSpeed > wheelMaxSpeed)
-                cart.SendThresSpeed = Math.Min(cart.SendThresSpeed, wheelMaxSpeed);
-            _lastMoveTime = DateTime.Now;
+        }
 
-            //灯光控制
-            if (cart.AlarmLevel == 2)
+        // M层单车限速：按照加速度和减速度平滑更新实际下发速度上限。
+        private void UpdateSendSpeedLimit()
         {
-                //故障红灯常亮
-                cart.LightMode = 2;
+            if (cart.Chassis == null)
+            {
+                cart.SendThresSpeed = 0;
+                return;
             }
-            else if (cart.ThresSpeed != 1)
+
+            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)
             {
-                //避障黄灯常亮
-                cart.LightMode = 3;
-            }
-            else if (!cart.WheelAbleState)
-            {
-                FlipFlop(ref cart.LightMode, 500, 0, 3);
-            }
-            else if (cart.Soc < cart.LowBatteryAlarmThreshold && cart.ElectricCurrent >= -10)
-            {
-                cart.LightMode = 3;
-            }
-            else if (cart.ElectricCurrent < -10)
-            {
-                FlipFlop(ref cart.LightMode, 500, 0, 1);
+                currentLimit = targetLimit;
             }
             else
             {
+                currentLimit += Math.Sign(speedDifference) * maxChange;
+            }
+
+            // 计算当前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)
+            {
+                currentLimit = wheelMaxSpeed;
+            }
+
+            cart.SendThresSpeed = currentLimit;
+        }
+
+        // M层单车灯光：根据报警、驱动器、限速和电量状态生成红黄绿灯模式。
+        private void UpdateLightMode()
+        {
+            // 二级报警：红灯常亮。
+            if (cart.AlarmLevel == 2)
+            {
+                cart.LightMode = 2;
+                return;
+            }
+
+            // 一级报警：黄灯常亮。
+            if (cart.AlarmLevel == 1)
+            {
+                cart.LightMode = 3;
+                return;
+            }
+
+            // 驱动轮未使能：黄灯闪烁。
+            if (!cart.WheelAbleState)
+            {
+                FlipFlop(ref cart.LightMode, 500, 0, 3);
+                return;
+            }
+
+            // C层正在进行限速：黄灯常亮。
+            if (Math.Abs(cart.ThresSpeed - 1f) > 0.001f)
+            {
+                cart.LightMode = 3;
+                return;
+            }
+
+            // 电量低于预警值：黄灯常亮。
+            if (cart.Soc <= cart.LowBatteryAlarmThreshold)
+            {
+                cart.LightMode = 3;
+                return;
+            }
+
+            // 正常运行：绿灯闪烁。
             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

@@ -1,168 +0,0 @@
-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,13 +1,51 @@
-using System;
-using System.Collections.Generic;
-using System.Text;
-using CartActivator;
+// Medulla虚拟遥控器和夹臂控制
 using MDCSToolBox.Medulla.Chassis.MultiWheel;
+using CartActivator;
 
 namespace MedullaAdapter
 {
-    public class Remote:MultiWheelRemote<DiverCartDefinition>
+    // M层单车虚拟遥控器：使用父类提供的底盘控制界面。
+    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;
 
@@ -16,26 +54,22 @@ namespace MedullaAdapter
 
         [AsControlItem(name = "夹抱关闭", LayoutRow = 1, LayoutCol = 2)]
         public Button Close;
-
-        public override void MultiVehicleModeChassisLogic()
-        {
-            // cart.MultiVehicleRemoteManualEnabled = true;
-            // cart.MultiVehicleManualEnabled = true;
-            // cart.MultiVehicleManualVx = SpeedPad.y * SpeedThreshold.val;
-            // cart.MultiVehicleManualVth = -(float)Math.Pow(Math.Abs(SpeedPad.x), cart.ManualThetaPow) * Math.Sign(SpeedPad.x);
-        }
-
+        // M层虚拟遥控器：控制左右夹臂同步打开或关闭。
         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
             {
@@ -43,5 +77,10 @@ namespace MedullaAdapter
                 cart.SpeedRightArm = 0;
             }
         }
+        // 单车不支持多车联动，误打开多车开关时主动停车。
+        public override void MultiVehicleModeChassisLogic()
+        {
+            cart.Chassis?.PredefinedDriveStop();
+        }
     }
 }

MedullaAdapter/RunOnMCU.cs

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

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

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

MedullaAdapter/build/Clumsy/ClumsyLite.runtimeconfig.json

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

MedullaAdapter/build/Clumsy_AGV2/ClumsyLite.runtimeconfig.json

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

MedullaAdapter/build/Medulla/plugins/MedullaAdapter.deps.json

@@ -0,0 +1,23 @@
+{
+  "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

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

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

@@ -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")]

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

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

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

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

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

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