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

2026-07-27 17:47:50 +08:00
parent 580a936a83
commit e6b99c45b3
47 changed files with 4238 additions and 122 deletions
@@ -15,6 +15,11 @@
    <PackageReference Include="System.Numerics.Vectors" Version="4.6.1" />
  </ItemGroup>

+  <ItemGroup>
+  <Compile Include="..\Shared\*.cs"
+           Link="Shared\%(Filename)%(Extension)" />
+</ItemGroup>
+
  <ItemGroup>
    <Reference Include="CommonUsage">
      <HintPath>ref\CommonUsage.dll</HintPath>
@@ -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();
+            }
        }
    }
 }
@@ -124,4 +124,5 @@ namespace MultiWheelC
        }
    }

+
 }
@@ -60,7 +60,7 @@ public class PilotConfig : MultiWheelPilotConfig
    #endregion


-#if false
+

    #region 单车-钻车与夹抱
    [FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")]
@@ -165,10 +165,8 @@ public class PilotConfig : MultiWheelPilotConfig
    [FieldMember(desc = "抱夹控制pid:Thresh")] public float ClampControlThresh = 0.2f;
    [FieldMember(desc = "抱夹控制pid:DeadZone")] public float ClampControlDeadZone = 5f;
    [FieldMember(desc = "抱夹最大速度")] public float MaxClampSpeed = 1.5f;
-
-
    #endregion
-
+#if false
    #region 多车-编队与遥控
    [FieldMember(desc = "联动时转向角爬升加速度")] public float SyncThAccPerSec = 30f;
    [FieldMember(desc = "两车间距 (mm)")] public float TestCarSyncDistance = 2400f;
@@ -330,24 +328,6 @@ public class PilotConfig : MultiWheelPilotConfig

 #endif

-    #region 仿真-Playground
-    [FieldMember(desc = "Playground WebAPI 基地址")]
-    public string PlaygroundWebApiUrl = "http://localhost:18090";
-
-    [FieldMember(desc = "Playground 小车名称（场景 robots[].name）")]
-    public string PlaygroundRobotName = "agv_multi_1";
-
-    [FieldMember(desc = "Playground 邻车名称（仅主车用于原地旋转位姿诊断）")]
-    public string PlaygroundNeighborRobotName = "agv_multi_2";
-
-    [FieldMember(desc = "旋转位姿诊断开关(simulation only)")]
-    public bool MultiVehicleRotatePoseWebApiDiagEnabled = false;
-    [FieldMember(desc = "WebAPI 平移测试：平移距离(mm)")]
-    public float WebApiTranslateMm = 100f;
-
-    [FieldMember(desc = "WebAPI 旋转测试：旋转角度(deg)")]
-    public float WebApiRotateDeg = 5f;
-    #endregion


 }
@@ -11,4 +11,34 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
    public new float CarWidth = 948f;
    [AsLowerIO(desc = "车号")] public int CarNum = 1;
    public override void StandardInit() { }
+
+    #region 夹臂变量
+    [AsUpperIO(desc = "左夹臂下发速度")] public float SpeedLeftArm;
+
+    [AsUpperIO(desc = "右夹臂下发速度")] public float SpeedRightArm;
+
+    [AsLowerIO(desc = "左夹臂实际位置")] public float ActualPosLeftArm;
+
+    [AsLowerIO(desc = "右夹臂实际位置")] public float ActualPosRightArm;
+
+    [AsUpperIO(desc = "夹臂不同步报警")] public bool ClampOutOfSync = false;
+    #endregion
+
+    [AsLowerIO(desc = "左前左轮实际位置")] public float LFLActualPos;
+    [AsLowerIO(desc = "左前右轮实际位置")] public float LFRActualPos;
+    [AsLowerIO(desc = "右前左轮实际位置")] public float RFLActualPos;
+    [AsLowerIO(desc = "右前右轮实际位置")] public float RFRActualPos;
+    [AsLowerIO(desc = "左后左轮实际位置")] public float LRLActualPos;
+    [AsLowerIO(desc = "左后右轮实际位置")] public float LRRActualPos;
+    [AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
+    [AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
+
+    [AsLowerIO(desc = "左夹臂低限位")] public float LeftArmLowerPos;
+    [AsLowerIO(desc = "左夹臂高限位")] public float LeftArmUpperPos;
+    [AsLowerIO(desc = "右夹臂低限位")] public float RightArmLowerPos;
+    [AsLowerIO(desc = "右夹臂高限位")] public float RightArmUpperPos;
+
+    [AsUpperIO(desc = "从C往驱动器下使能")] public bool DisableFromC = false;
+    [AsUpperIO(desc = "从C上复位")] public bool ResetFromC = false;
+    [AsLowerIO(desc = "驱动轮使能状态")] public bool WheelAbleState = true;
 }
@@ -13,7 +13,7 @@ using System.Reflection;
 [assembly: System.Reflection.AssemblyCompanyAttribute("ClumsyPilot")]
 [assembly: System.Reflection.AssemblyConfigurationAttribute("Debug")]
 [assembly: System.Reflection.AssemblyFileVersionAttribute("1.0.0.0")]
-[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0")]
+[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0+580a936a830dcb7a25ef327cf553341405264033")]
 [assembly: System.Reflection.AssemblyProductAttribute("ClumsyPilot")]
 [assembly: System.Reflection.AssemblyTitleAttribute("ClumsyPilot")]
 [assembly: System.Reflection.AssemblyVersionAttribute("1.0.0.0")]
@@ -1 +1 @@
-5d77794fa0720c6591db5b06ac60427413c18989a6f7b64420ccb07d122d85bc
+038d57c5b1714403d308c9343b385ef762951607b63a9fb275f4d7d2b8fd29cb
@@ -1,6 +1,6 @@
 is_global = true
 build_property.RootNamespace = MultiWheelC
-build_property.ProjectDir = d:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\
+build_property.ProjectDir = d:\MyParking\ClumsyPilot\
 build_property.EnableComHosting = 
 build_property.EnableGeneratedComInterfaceComImportInterop = 
 build_property.CsWinRTUseWindowsUIXamlProjections = false
@@ -1 +1 @@
-cbba0ac806b0617cc6f7209f71f12565d8a56471fa82abdd118fca6c967d6a5a
+3920568398d269aea7b71fb4581ad111777c15ca3f2f2bb272ee9e0a989215c4