From b1c50a0916da74c5a1b3674728efb34014c4991b Mon Sep 17 00:00:00 2001
From: "shuai.li" <1248399577@qq.com>
Date: Wed, 1 Jul 2026 22:47:59 +0800
Subject: [PATCH] update

---
 MultiWheel/MultiWheelC/AGV.cs       |  7 ++---
 MultiWheel/MultiWheelC/Movements.cs | 41 ++++++++++++++++++++++-------
 2 files changed, 35 insertions(+), 13 deletions(-)

diff --git a/MultiWheel/MultiWheelC/AGV.cs b/MultiWheel/MultiWheelC/AGV.cs
index 10fb797..2852273 100644
--- a/MultiWheel/MultiWheelC/AGV.cs
+++ b/MultiWheel/MultiWheelC/AGV.cs
@@ -1,4 +1,5 @@
 using ClumsyCore;
+using ClumsyCore.Interfaces;
 using FundamentalLib;
 using MDCSToolBox.Clumsy.AgvInterfaces;
 using MDCSToolBox.Clumsy.MotionControllers;
@@ -132,7 +133,7 @@ namespace MultiWheelC
                     0)
                 },
             };
-            DLog.Log($"¼ì²âÆ÷ÊýÁ¿Îª{detectors.Count}", "TireFollowing");
+            DLog.Log($"×ê̥Ϊ{tireNum}", "TireFollowing");
             var following = new TireFollowing()
             {
                 GetController = () => new ChassisController().Get(),
@@ -141,7 +142,7 @@ namespace MultiWheelC
                 detectors = detectors,
                 SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
                 MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
-                TireNum = detectors.Count,
+                TireNum = tireNum,
                 CarDirection = frontLidarDetect ? 0f : 180f,
                 WalkBlindTh = frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarWalkBlindTh : PilotDefinition.Conf.TireFollowingBackLidarWalkBlindTh,
             };
@@ -272,7 +273,7 @@ namespace MultiWheelC
                     if (setLocationRes != null && setLocationRes.l_step == 2) break;
                 }
             });
-
         }
+        public float baseSpeed = 0;
     }
 }
diff --git a/MultiWheel/MultiWheelC/Movements.cs b/MultiWheel/MultiWheelC/Movements.cs
index a9da6f0..21b7bad 100644
--- a/MultiWheel/MultiWheelC/Movements.cs
+++ b/MultiWheel/MultiWheelC/Movements.cs
@@ -181,24 +181,45 @@ namespace MultiWheelC
         public Action<int> LeaveSrcFunction = null;
         private PIDController pid;
 
+        // GhostMode 虚拟里程计
+        private float _ghostDistance;
+        private DateTime _lastTick;
+
         public override IEnumerable<bool> Get()
         {
-            pid = new PIDController(() => (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2, Kp, Ki, Kd, 0,
-                    DeadZone, MaxSpeed)
+            bool isGhost = PilotDefinition.Self.GhostMode;
+            if (isGhost)
+            {
+                _ghostDistance = 0f;
+                _lastTick = DateTime.Now;
+            }
+
+            pid = new PIDController(() =>
+                isGhost ? _ghostDistance : (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+                Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
             { SpeedAccPerSec = MaxSpeed / 2f };
+
             var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
             while (true)
             {
                 var speed = pid.GetResponse(Target);
-                Console.WriteLine($"output: {speed}  current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
-                var current = (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2;
+
+                if (isGhost)
+                {
+                    var now = DateTime.Now;
+                    float dt = (float)(now - _lastTick).TotalSeconds;
+                    _ghostDistance += speed * dt * 1000f;
+                    _lastTick = now;
+                    Console.WriteLine($"[Ghost] output: {speed:F2}  current: {_ghostDistance:F2}");
+                }
+                else
+                {
+                    Console.WriteLine($"output: {speed}  current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
+                }
+
+                var current = isGhost ? _ghostDistance : (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2;
                 chassis.SendXYThSpeed(speed, 0, 0);
-                //if (Math.Abs(current - Target) < pid.DeadZone)
-                //{
-                //    chassis.SendXYThSpeed(0f, 0f, 0f);
-                //    Console.WriteLine($"调整退出:当�({current:f2}) ,目标:({Target:f2})");
-                //    break;
-                //}
+
                 if (pid.IsArrived()) break;
                 yield return true;
             }