增加原地旋转动作

2026-07-07 20:00:57 +08:00
parent 68dd6fc061
commit 6ca401cf68
4 changed files with 82 additions and 8 deletions
@@ -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)