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()
             {

MultiWheel/MultiWheelC/Movements.cs

@@ -211,12 +211,14 @@ namespace MultiWheelC
 
         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)
             {