集中停车控制配置并完善终点逼近和周期诊断

2026-08-11 17:46:52 +08:00
parent 33a33af710
commit 3043febd91
30 changed files with 2946 additions and 30 deletions
@@ -0,0 +1,91 @@
+using System;
+using System.Collections.Generic;
+using ClumsyCore.Pilot;
+using MDCSToolBox.Commons.Controllers;
+
+namespace MultiWheelC
+{
+    public class ClampToTarget : MovementDefinition
+    {
+        public float LeftClampTarget;
+        public float RightClampTarget;
+        public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed;
+        public float ClampKp = PilotDefinition.Conf.ClampControlKp;
+        public float ClampKi = PilotDefinition.Conf.ClampControlKi;
+        public float ClampKd = PilotDefinition.Conf.ClampControlKd;
+        public float ClampMaxI = PilotDefinition.Conf.ClampControlMaxI;
+        public float ClampSpeedAcc = PilotDefinition.Conf.ClampControlSpeedAcc;
+        public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
+        public float TimeoutSeconds = 30f;
+        private PIDController leftpid, rightpid;
+
+        // C层单车业务：驱动左右夹臂运动到夹紧或松开目标。
+        public override IEnumerable<bool> Get()
+        {
+            try
+            {
+                leftpid = new PIDController(
+                    () => PilotDefinition.Self.ActualPosLeftArm,
+                    ClampKp, ClampKi, ClampKd, ClampMaxI,
+                    ClampDeadZone, MaxClampSpeed)
+                {
+                    SpeedAccPerSec = ClampSpeedAcc
+                };
+
+                rightpid = new PIDController(
+                    () => PilotDefinition.Self.ActualPosRightArm,
+                    ClampKp, ClampKi, ClampKd, ClampMaxI,
+                    ClampDeadZone, MaxClampSpeed)
+                {
+                    SpeedAccPerSec = ClampSpeedAcc
+                };
+
+                var startTime = DateTime.UtcNow;
+                while (true)
+                {
+                    if (TimeoutSeconds > 0f &&
+                        (DateTime.UtcNow - startTime).TotalSeconds >
+                        TimeoutSeconds)
+                    {
+                        Console.WriteLine(
+                            $"夹臂运动超时({TimeoutSeconds:F1}s)，" +
+                            "停止左右夹臂。");
+                        yield break;
+                    }
+
+                    var leftspeed =
+                        leftpid.GetResponse(LeftClampTarget);
+                    var rightspeed =
+                        rightpid.GetResponse(RightClampTarget);
+                    Console.WriteLine(
+                        $"left arm speed:{leftspeed}  " +
+                        $"right arm speed:{rightspeed}");
+
+                    PilotDefinition.Self.SpeedLeftArm = leftspeed;
+                    PilotDefinition.Self.SpeedRightArm = rightspeed;
+
+                    var leftArrived = leftpid.IsArrived();
+                    var rightArrived = rightpid.IsArrived();
+                    if (leftArrived)
+                        PilotDefinition.Self.SpeedLeftArm = 0f;
+                    if (rightArrived)
+                        PilotDefinition.Self.SpeedRightArm = 0f;
+
+                    if (leftArrived && rightArrived)
+                        break;
+
+                    yield return true;
+                }
+
+                Console.WriteLine(
+                    $"left clamp to target:{LeftClampTarget}  " +
+                    $"right clamp to target:{RightClampTarget}");
+            }
+            finally
+            {
+                PilotDefinition.Self.SpeedLeftArm = 0f;
+                PilotDefinition.Self.SpeedRightArm = 0f;
+            }
+        }
+    }
+}