增加原地旋转动作

MultiWheel/MultiWheelC/AGV.cs

@@ -69,6 +69,25 @@ namespace MultiWheelC
                 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:后雷达识别
@@ -192,7 +211,7 @@ namespace MultiWheelC
                 },
                 CarDirection = 180f,
                 SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
-                MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
+                MaxSpeed = 0.25f,
                 EnableHandover = true,
                 HandoverDistance = 200f,
                 HandoverSpeed = 0.3f,
@@ -246,6 +265,9 @@ namespace MultiWheelC
                     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;
@@ -253,6 +275,12 @@ namespace MultiWheelC
                 }
                 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),

MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs

@@ -520,6 +520,35 @@ namespace MultiWheelC
         }
     }
 
+    [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)

MultiWheel/MultiWheelC/PilotConfig.cs

@@ -306,6 +306,14 @@ public class PilotConfig : MultiWheelPilotConfig
     [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;
+    [FieldMember(desc = "轮胎跟踪:角度调整kd")] public float TireFollowingThkd = 0f;
+    [FieldMember(desc = "轮胎跟踪:角度调整SpeedAcc")] public float TireFollowingThSpeedAccPerSec = 1f;
+    [FieldMember(desc = "轮胎跟踪:角度调整Thresh")] public float TireFollowingThThresh = 0.1f;
+    [FieldMember(desc = "轮胎跟踪:角度调整DeadZone")] public float TireFollowingThDeadZone = 5f;
+    [FieldMember(desc = "轮胎跟踪:角度调整MaxI")] public float TireFollowingThMaxI = 0.01f;
 }

MultiWheel/MultiWheelC/TireFollowing.cs

@@ -297,7 +297,8 @@ namespace MultiWheelC
                     if (EnableHandover)
                     {
                         controller.SlowDistance = float.MinValue;
-                        controller.FinishDistance = 20;
+                        controller.FinishSpeed = 0.2f;
+                        controller.FinishDistance = 50;
                     }
                     (WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh) = GetCurrentPos2Dst(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
                     var walkBlindPathEnd = Tuple.Create(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
@@ -416,6 +417,14 @@ namespace MultiWheelC
                     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)
                     {
@@ -423,11 +432,11 @@ namespace MultiWheelC
                         if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
                         {
                             WalkBlindStage1 = true;
-                            if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
+                            //if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
-                            {
+                            //{
-                                detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
+                            //    detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
-                                DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
+                            //    DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
-                            }
+                            //}
                             WalkBlindCarPathDstX = trackDst.X;
                             WalkBlindCarPathDstY = trackDst.Y;
                             WalkBlindCarPathDstTh = angle2target + WalkBlindTh;