First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

添加差速舵轮旋转中心前馈与遥控器模式防抖，并补充ParkingRobot解决方案

Browse Source 
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
yuxiang.shen
2026-08-04 18:04:17 +08:00
co-authored by Cursor
parent 31ec941b07
commit 7447317812
8 changed files with 279 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CommonUsage-MultiVehicleSync/commonusage/Chassis/MultiWheelChassis.cs
+33 -3
View File
@@ -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)
MedullaAdapter/DiverCartDefinition.cs
+2
View File
@@ -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;
MedullaAdapter/MotorRoutine.cs
+73 -21
View File
@@ -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)
{
ParkingRobot.sln
+70
View File
@@ -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
README.md
+2 -1
View File
@@ -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` 为准。
## 开发环境与依赖
README_en.md
+2 -1
View File
@@ -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
Shared/Chassis/MultiWheelChassisAdapter.cs
+2 -1
View File
@@ -291,7 +291,8 @@ namespace MyParking.Shared
vxMetersPerSecond,
vyMetersPerSecond,
omegaDegreesPerSecond,
interval);
interval,
enableDifferentialSteerFeedforward: true);
if (!success)
{
// 防止分解失败后继续执行上一条运动命令。
docs/cartparams.json
+95
View File
@@ -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
}