update 离车时SetOriginBias(0, 0, 0)

MultiWheel/MultiWheelC/AGV.cs

@@ -5,6 +5,7 @@ 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;
@@ -164,7 +165,8 @@ namespace MultiWheelC
             }
             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(),
@@ -230,6 +232,8 @@ namespace MultiWheelC
             {
                 Thread.Sleep(50);
             }
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            chassis.SetOriginBias(0, 0, 0);
             DLog.Log($"锁点{dstId}完成", "TireFollowing");
             IEnumerable<bool> TrackThenFollow()
             {