添加单车底盘仿真平台并完善运动控制与夹臂功能

2026-07-27 17:47:50 +08:00
parent 580a936a83
commit e6b99c45b3
47 changed files with 4238 additions and 122 deletions
@@ -72,30 +72,55 @@ namespace MultiWheelC
    [MovementTest(name = "底盘旋转测试")]
    public class RotateToAngleTest : MovementTest
    {
-        // 底盘旋转测试不支持停止操作。
+        private DriveTask _dt;
+
+        // 停止当前正在执行的底盘原地旋转任务。
        public override void TestStop()
        {
-            throw new NotImplementedException();
+            _dt?.Stop();
        }

        // 交互输入目标角度后执行底盘原地旋转测试。
        public override void Test()
        {
-            var target = UI.GetInput("输入旋转角度：");
-            new DriveTask(new MultiWheelRotateInPlace()
+            var input = UI.GetInput("输入旋转角度：");
+            if (!float.TryParse(input, out var angleTarget))
            {
-                AngleTarget = float.Parse(target),
-                PidparamsRead = () => new PIDParams()
+                Console.WriteLine(
+                    $"旋转测试输入无效：{input}");
+                return;
+            }
+            // 防止重复启动测试时，上一项旋转任务仍在运行。
+            _dt?.Stop();
+            var task = new DriveTask(
+                new MultiWheelRotateInPlace
                {
-                    Kp = PilotDefinition.Conf.InPlaceRotateKp,
-                    Ki = PilotDefinition.Conf.InPlaceRotateKi,
-                    Kd = PilotDefinition.Conf.InPlaceRotateKd,
-                    DeadZone = PilotDefinition.Conf.InPlaceRotateArriveDeg,
-                    SpeedAccPerSec = PilotDefinition.Conf.InPlaceRotateAcc,
-                    OutputUpperThreshold = PilotDefinition.Conf.InPlaceRotateMaxSpeed,
-                    MaxI = PilotDefinition.Conf.InPlaceRotateMaxI,
+                    AngleTarget = angleTarget,
+
+                    PidparamsRead = () => new PIDParams
+                    {
+                        Kp = PilotDefinition.Conf.InPlaceRotateKp,
+                        Ki = PilotDefinition.Conf.InPlaceRotateKi,
+                        Kd = PilotDefinition.Conf.InPlaceRotateKd,
+                        DeadZone = PilotDefinition.Conf.InPlaceRotateArriveDeg,
+                        SpeedAccPerSec = PilotDefinition.Conf.InPlaceRotateAcc,
+                        OutputUpperThreshold = PilotDefinition.Conf.InPlaceRotateMaxSpeed,
+                        MaxI = PilotDefinition.Conf.InPlaceRotateMaxI,
+                    }
+                }.Get());
+            _dt = task;
+            try
+            {
+                task.Wait();
+            }
+            finally
+            {
+                // 防止旧任务结束时，错误清除后来启动的新任务。
+                if (ReferenceEquals(_dt, task))
+                {
+                    _dt = null;
                }
-            }.Get()).Wait();
+            }
        }
    }
 }