增加原地旋转动作

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),