diff --git a/CommonUsage-MultiVehicleSync/commonusage/Chassis/MultiWheelChassis.cs b/CommonUsage-MultiVehicleSync/commonusage/Chassis/MultiWheelChassis.cs index 1576213..e82f753 100644 --- a/CommonUsage-MultiVehicleSync/commonusage/Chassis/MultiWheelChassis.cs +++ b/CommonUsage-MultiVehicleSync/commonusage/Chassis/MultiWheelChassis.cs @@ -1190,7 +1190,8 @@ namespace CommonUsage.Chassis float vx, float vy, float vth, - TimeSpan? deltaTime = null) + TimeSpan? deltaTime = null, + bool enableDifferentialSteerFeedforward = false) { const string operationName = "SendXYThSpeed"; @@ -1313,13 +1314,42 @@ namespace CommonUsage.Chassis var driveScale = _xyThWheelsAligned ? alignmentSpeedScale : 0f; + #region 新增虚拟旋转中心 + // vth传入单位为deg/s，这里换算成rad/s。 + var omegaRadiansPerSecond = + vth * (float)Math.PI / 180f; + + // 只有调用者主动开启并且存在旋转运动时， + // 才启用差速舵轮左右轮的几何速度前馈。 + var useDifferentialSteerFeedforward = + enableDifferentialSteerFeedforward && + Math.Abs(omegaRadiansPerSecond) > 1e-4f; + + var rotationCenter = Vector2.Zero; + + if (useDifferentialSteerFeedforward) + { + // 根据车体速度场： + // vx(point) = vx - omega * y + // vy(point) = vy + omega * x + // 计算车体坐标系中的瞬时旋转中心。 + // + // vx、vy单位为m/s，计算结果原本是m； + // 舵轮Position使用mm，因此乘以1000。 + rotationCenter = new Vector2( + -vy / omegaRadiansPerSecond * 1000f, + vx / omegaRadiansPerSecond * 1000f); + } + #endregion + + for (var i = 0; i < _steerWheels.Count; i++) AccumulateSpeed( i, driveScale * sendSpeed[i], - false, - new Vector2(0f, 0f), + useDifferentialSteerFeedforward, + rotationCenter, deltaTime); if (writeDiagnostics) diff --git a/MedullaAdapter/DiverCartDefinition.cs b/MedullaAdapter/DiverCartDefinition.cs index 49e7c2d..0e09751 100644 --- a/MedullaAdapter/DiverCartDefinition.cs +++ b/MedullaAdapter/DiverCartDefinition.cs @@ -67,6 +67,8 @@ namespace MedullaAdapter [AsInitParam(desc = "MCU端口号")] public string MCUPort = "COM4"; [AsInitParam(desc = "遥控器速度上限")] public float TransmitterSpeedUpperLimit = 1.0f; [AsInitParam(desc = "遥控器速度下限")] public float TransmitterSpeedLowerLimit = 0.0f; + [AsInitParam(desc = "实体遥控器SB模式防抖时间，单位ms")] + public int TransmitterModeDebounceMilliseconds = 200; [AsInitParam(desc = "手动控制夹臂速度系数")] public float ManualArmSpeedFac = 1.0f; [AsInitParam(desc = "遥控转弯舵角同步限速宽度，单位为度")] public float ManualSteeringAlignmentSigmaDegrees = 8.0f; diff --git a/MedullaAdapter/MotorRoutine.cs b/MedullaAdapter/MotorRoutine.cs index 5ca2cea..9c57c77 100644 --- a/MedullaAdapter/MotorRoutine.cs +++ b/MedullaAdapter/MotorRoutine.cs @@ -11,6 +11,10 @@ namespace MedullaAdapter { private bool _wasTransmitterControlling; private DateTime _lastMoveTime = DateTime.Now; + private DiverCartDefinition.ManualControlMode? + _pendingTransmitterControlMode; + private DateTime _pendingTransmitterControlModeSince = + DateTime.MinValue; public override void Operation(int iteration) { if (!cart.GhostMode && cart.State == -1) return; @@ -68,33 +72,81 @@ namespace MedullaAdapter // 更新红黄绿灯状态。 UpdateLightMode(); } + // M层物理遥控器：只有SB档位连续稳定指定时间后才确认模式切换。 + private bool TryGetStableTransmitterControlMode( + out DiverCartDefinition.ManualControlMode stableMode) + { + stableMode = cart.TransmitterControlMode; + + DiverCartDefinition.ManualControlMode? requestedMode; + + switch (cart.Transmitter_SB) + { + case TransmitterState.Mode0: + requestedMode = + DiverCartDefinition.ManualControlMode.Normal; + break; + + case TransmitterState.Mode1: + requestedMode = + DiverCartDefinition.ManualControlMode.Crab; + break; + + case TransmitterState.Mode2: + requestedMode = + DiverCartDefinition.ManualControlMode.Spin; + break; + + default: + _pendingTransmitterControlMode = null; + _pendingTransmitterControlModeSince = + DateTime.MinValue; + return false; + } + + if (_pendingTransmitterControlMode != requestedMode) + { + _pendingTransmitterControlMode = requestedMode; + _pendingTransmitterControlModeSince = DateTime.Now; + return false; + } + + var debounceMilliseconds = Math.Max( + 0, + cart.TransmitterModeDebounceMilliseconds); + + if ((DateTime.Now - + _pendingTransmitterControlModeSince) + .TotalMilliseconds < debounceMilliseconds) + { + return false; + } + + stableMode = requestedMode.Value; + return true; + } + // 物理遥控器设置 public void TransmitterChassisControl() { var interval = DateTime.Now - cart.TransmitterLastTime; - switch (cart.Transmitter_SB) + + if (!TryGetStableTransmitterControlMode( + out var stableControlMode)) { - case TransmitterState.Mode0: - cart.TransmitterControlMode = - DiverCartDefinition.ManualControlMode.Normal; - break; - case TransmitterState.Mode1: - cart.TransmitterControlMode = - DiverCartDefinition.ManualControlMode.Crab; - break; - case TransmitterState.Mode2: - cart.TransmitterControlMode = - DiverCartDefinition.ManualControlMode.Spin; - break; - default: - cart.ManualControl( - cart.TransmitterControlMode, - 0, 0, 0, - cart.TransmitterSpeed, - interval); - StopClampArms(); - return; + // SB处于中间档或尚未稳定时立即停车， + // 保持当前模式，不下发新的模式目标角。 + cart.ManualControl( + cart.TransmitterControlMode, + 0, 0, 0, + cart.TransmitterSpeed, + interval); + StopClampArms(); + return; } + + cart.TransmitterControlMode = stableControlMode; + // SA关闭后立即停车。 if (!cart.Transmitter_SA) { diff --git a/ParkingRobot.sln b/ParkingRobot.sln new file mode 100644 index 0000000..811a7c3 --- /dev/null +++ b/ParkingRobot.sln @@ -0,0 +1,70 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 17 +VisualStudioVersion = 17.0.31903.59 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "CommonUsage-MultiVehicleSync", "CommonUsage-MultiVehicleSync", "{2291F6EE-0AD9-D010-97BA-674682470F91}" +EndProject +Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "commonusage", "commonusage", "{0A043828-FC50-B2F1-03BD-49EF4E42C81C}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "CommonUsage", "CommonUsage-MultiVehicleSync\commonusage\CommonUsage.csproj", "{DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "MedullaAdapter", "MedullaAdapter\MedullaAdapter.csproj", "{95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "MultiWheelC", "MultiWheelC\MultiWheelC.csproj", "{29520219-FA63-4B59-AA80-91B4FFA0D9B1}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Debug|Any CPU.Build.0 = Debug|Any CPU + {DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Debug|x64.ActiveCfg = Debug|Any CPU + {DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Debug|x64.Build.0 = Debug|Any CPU + {DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Debug|x86.ActiveCfg = Debug|Any CPU + {DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Debug|x86.Build.0 = Debug|Any CPU + {DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Release|Any CPU.ActiveCfg = Release|Any CPU + {DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Release|Any CPU.Build.0 = Release|Any CPU + {DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Release|x64.ActiveCfg = Release|Any CPU + {DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Release|x64.Build.0 = Release|Any CPU + {DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Release|x86.ActiveCfg = Release|Any CPU + {DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41}.Release|x86.Build.0 = Release|Any CPU + {95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Debug|Any CPU.Build.0 = Debug|Any CPU + {95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Debug|x64.ActiveCfg = Debug|Any CPU + {95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Debug|x64.Build.0 = Debug|Any CPU + {95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Debug|x86.ActiveCfg = Debug|Any CPU + {95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Debug|x86.Build.0 = Debug|Any CPU + {95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Release|Any CPU.ActiveCfg = Release|Any CPU + {95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Release|Any CPU.Build.0 = Release|Any CPU + {95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Release|x64.ActiveCfg = Release|Any CPU + {95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Release|x64.Build.0 = Release|Any CPU + {95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Release|x86.ActiveCfg = Release|Any CPU + {95F7BD98-C801-4E4E-B8A6-C0EFE3535C5C}.Release|x86.Build.0 = Release|Any CPU + {29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Debug|Any CPU.Build.0 = Debug|Any CPU + {29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Debug|x64.ActiveCfg = Debug|Any CPU + {29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Debug|x64.Build.0 = Debug|Any CPU + {29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Debug|x86.ActiveCfg = Debug|Any CPU + {29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Debug|x86.Build.0 = Debug|Any CPU + {29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Release|Any CPU.ActiveCfg = Release|Any CPU + {29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Release|Any CPU.Build.0 = Release|Any CPU + {29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Release|x64.ActiveCfg = Release|Any CPU + {29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Release|x64.Build.0 = Release|Any CPU + {29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Release|x86.ActiveCfg = Release|Any CPU + {29520219-FA63-4B59-AA80-91B4FFA0D9B1}.Release|x86.Build.0 = Release|Any CPU + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(NestedProjects) = preSolution + {0A043828-FC50-B2F1-03BD-49EF4E42C81C} = {2291F6EE-0AD9-D010-97BA-674682470F91} + {DAAF1A42-FA55-4FC0-8083-F5A2FEFE0A41} = {0A043828-FC50-B2F1-03BD-49EF4E42C81C} + EndGlobalSection +EndGlobal diff --git a/README.md b/README.md index 51625be..0139fcb 100644 --- a/README.md +++ b/README.md @@ -89,6 +89,7 @@ MCU ─► CAN / Serial / IO ──┘ ```text MyParking/ +├── ParkingRobot.sln # 根解决方案（CommonUsage / M / C） ├── build-and-package.ps1 # 官方构建与 M/C 打包脚本 ├── AGENTS.md # 协作与代码规范 ├── MultiWheelC/ # C 层动作、跟踪、测试和实验记录 @@ -110,7 +111,7 @@ MyParking/ └── C/ # MultiWheelC.dll + CommonUsage.dll ``` -仓库内没有根级 `ParkingRobot.sln`。唯一 solution 文件位于 `CommonUsage-MultiVehicleSync/commonusage/CommonUsageSln.sln`。 +根目录 `ParkingRobot.sln` 包含 `CommonUsage`、`MedullaAdapter` 和 `MultiWheelC`，便于在 Visual Studio 中打开整仓。`Shared` 无独立项目，由 M/C 编译引入。部署打包仍以 `build-and-package.ps1` 为准。 ## 开发环境与依赖 diff --git a/README_en.md b/README_en.md index dfb490b..c52af29 100644 --- a/README_en.md +++ b/README_en.md @@ -89,6 +89,7 @@ MCU ─► CAN / Serial / IO ──┘ ```text MyParking/ +├── ParkingRobot.sln # Root solution (CommonUsage / M / C) ├── build-and-package.ps1 # Official build and M/C packaging script ├── AGENTS.md # Collaboration and coding rules ├── MultiWheelC/ # C-layer actions, tracking, tests, and recording @@ -110,7 +111,7 @@ MyParking/ └── C/ # MultiWheelC.dll + CommonUsage.dll ``` -There is no root-level `ParkingRobot.sln`. The only solution file is `CommonUsage-MultiVehicleSync/commonusage/CommonUsageSln.sln`. +The root `ParkingRobot.sln` includes `CommonUsage`, `MedullaAdapter`, and `MultiWheelC` for opening the repo in Visual Studio. `Shared` has no standalone project and is compiled into the M/C projects. Packaging still uses `build-and-package.ps1`. ## Development Environment and Dependencies diff --git a/Shared/Chassis/MultiWheelChassisAdapter.cs b/Shared/Chassis/MultiWheelChassisAdapter.cs index 0c12308..8a73a54 100644 --- a/Shared/Chassis/MultiWheelChassisAdapter.cs +++ b/Shared/Chassis/MultiWheelChassisAdapter.cs @@ -291,7 +291,8 @@ namespace MyParking.Shared vxMetersPerSecond, vyMetersPerSecond, omegaDegreesPerSecond, - interval); + interval, + enableDifferentialSteerFeedforward: true); if (!success) { // 防止分解失败后继续执行上一条运动命令。 diff --git a/docs/cartparams.json b/docs/cartparams.json new file mode 100644 index 0000000..3ef5a58 --- /dev/null +++ b/docs/cartparams.json @@ -0,0 +1,95 @@ +{ + "MaxManualSpeed": 0.3, + "MaxManualAngularSpeed": 45.0, + "test3": 24, + "trigger": 0, + "ManualArmSpeedFac": 5.0, + "CarNum": 1, + "MusicVolume": 5, + "LeftArmLowerPos": -76358, + "LeftArmUpperPos": 38224, + "RightArmLowerPos": -41976, + "RightArmUpperPos": 73519, + "MCUPort": "COM3", + "TransmitterSpeedUpperLimit": 1.0, + "TransmitterSpeedLowerLimit": 0.0, + "ThBiasLeftFront": -342.0, + "ThBiasLeftRear": -185.35, + "ThBiasRightFront": -58.5, + "ThBiasRightRear": -1198.85, + "TurnAccSpeed": 1000.0, + "MaxManualTheta": 45.0, + "IsDiffSteer": true, + "ManualThetaPow": 2.0, + "DiffSteerKp": 0.003, + "DiffSteerKi": 0.0, + "DiffSteerKd": 0.0, + "DiffSteerMaxI": 0.0, + "DiffSteerThresh": 0.3, + "DiffSteerDeadZone": 0.1, + "DiffSteerSpeedAcc": 1.0, + "Ghost_ActualSpeedLeftFront_GhostPIDUpdater_kp": 0.5, + "Ghost_ActualSpeedLeftFront_GhostPIDUpdater_threshold": 200.0, + "Ghost_ActualSpeedLeftRear_GhostPIDUpdater_kp": 0.5, + "Ghost_ActualSpeedLeftRear_GhostPIDUpdater_threshold": 200.0, + "Ghost_ActualSpeedRightFront_GhostPIDUpdater_kp": 0.5, + "Ghost_ActualSpeedRightFront_GhostPIDUpdater_threshold": 200.0, + "Ghost_ActualSpeedRightRear_GhostPIDUpdater_kp": 0.5, + "Ghost_ActualSpeedRightRear_GhostPIDUpdater_threshold": 200.0, + "Ghost_ActualSpeedLeftFrontLeft_GhostPIDUpdater_kp": 0.5, + "Ghost_ActualSpeedLeftFrontLeft_GhostPIDUpdater_threshold": 200.0, + "Ghost_ActualSpeedLeftFrontRight_GhostPIDUpdater_kp": 0.5, + "Ghost_ActualSpeedLeftFrontRight_GhostPIDUpdater_threshold": 200.0, + "Ghost_ActualSpeedLeftRearLeft_GhostPIDUpdater_kp": 0.5, + "Ghost_ActualSpeedLeftRearLeft_GhostPIDUpdater_threshold": 200.0, + "Ghost_ActualSpeedLeftRearRight_GhostPIDUpdater_kp": 0.5, + "Ghost_ActualSpeedLeftRearRight_GhostPIDUpdater_threshold": 200.0, + "Ghost_ActualSpeedRightFrontLeft_GhostPIDUpdater_kp": 0.5, + "Ghost_ActualSpeedRightFrontLeft_GhostPIDUpdater_threshold": 200.0, + "Ghost_ActualSpeedRightFrontRight_GhostPIDUpdater_kp": 0.5, + "Ghost_ActualSpeedRightFrontRight_GhostPIDUpdater_threshold": 200.0, + "Ghost_ActualSpeedRightRearLeft_GhostPIDUpdater_kp": 0.5, + "Ghost_ActualSpeedRightRearLeft_GhostPIDUpdater_threshold": 200.0, + "Ghost_ActualSpeedRightRearRight_GhostPIDUpdater_kp": 0.5, + "Ghost_ActualSpeedRightRearRight_GhostPIDUpdater_threshold": 200.0, + "Ghost_ActualThLeftFront_GhostPIDUpdater_kp": 0.5, + "Ghost_ActualThLeftFront_GhostPIDUpdater_threshold": 5.0, + "Ghost_ActualThLeftRear_GhostPIDUpdater_kp": 0.5, + "Ghost_ActualThLeftRear_GhostPIDUpdater_threshold": 5.0, + "Ghost_ActualThRightFront_GhostPIDUpdater_kp": 0.5, + "Ghost_ActualThRightFront_GhostPIDUpdater_threshold": 5.0, + "Ghost_ActualThRightRear_GhostPIDUpdater_kp": 0.5, + "Ghost_ActualThRightRear_GhostPIDUpdater_threshold": 5.0, + "WheelAngleChangeLimit": 20.0, + "ElectricDebugMode": false, + "PostGPSToDetour": false, + "GPSBaseLongitude": 0.0, + "GPSBaseLatitude": 0.0, + "GPSInCarX": 0.0, + "GPSInCarY": 0.0, + "GPSInCarTh": 0.0, + "LowBatteryAlarmThreshold": 15.0, + "LowBatteryStopThreshold": 10.0, + "GhostMode": false, + "QrMode": false, + "GhostEnableSimBattery": false, + "GhostTimeBatteryFac": 600.0, + "ReductionRatio": 1.74, + "WheelDiameter": 165.0, + "CloseMusic": false, + "IOObstaclSlowFac": 0.5, + "ManualSpeed": 0.2, + "ManualRotateSpeed": 10.0, + "GamepadRotateSpeed": 1.0, + "RecoveryTime": 2000.0, + "ManualModeArea": 0, + "ManualModeIOArea": 1, + "UseLessTag": true, + "MaxAngularSpeed": 75.0, + "SafetyRelayShielded": false, + "SafetyRelayShieldedRleaseTime": 30, + "RotateIoArea": 16, + "ChargeMaxThreshold": 95.0, + "TransmitterCOMNum": "COM5", + "UseTransmitter": true +} \ No newline at end of file