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

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

Browse Source
This commit is contained in:
yuxiang.shen
2026-07-27 17:47:50 +08:00
parent 580a936a83
commit e6b99c45b3
47 changed files with 4238 additions and 122 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MedullaAdapter/AlarmRoutine.cs
+2 -1
View File
@@ -8,7 +8,8 @@ namespace MedullaAdapter
// M层单车安全：汇总停车机器人夹臂等自定义报警状态。
public override void SetOtherAlarms()
{
// 当前阶段不使用夹臂，暂时没有自定义报警。
AddAlarm("左夹臂驱动报警", 2, () => cart.LeftArmErrorCode != 0);
AddAlarm("右夹臂驱动报警", 2, () => cart.RightArmErrorCode != 0);
}
}
}
MedullaAdapter/DiverCartDefinition.cs
+193 -78
View File
@@ -6,6 +6,7 @@ using Medulla.Types;
using System;
using System.Collections.Generic;
using System.Threading;
using MyParking.Shared;
namespace MedullaAdapter
{
@@ -26,11 +27,16 @@ namespace MedullaAdapter
}
internal ManualControlMode TransmitterControlMode = ManualControlMode.Normal;
internal DateTime TransmitterLastTime = DateTime.Now; // 物理遥控器计算两次实体遥控器指令之间的时间间隔
private ManualControlMode? _pendingManualMode;
private ManualControlMode? _activeManualMode;
#endregion
#region AsUpperIO
[AsUpperIO(desc = "从C上复位")] public bool ResetFromC;
[AsUpperIO(desc = "从C将驱动轮下使能")] public bool DisableFromC;
[AsUpperIO(desc = "左夹臂下发速度", timeOutReset = true)] public float SpeedLeftArm;
[AsUpperIO(desc = "右夹臂下发速度", timeOutReset = true)] public float SpeedRightArm;
[AsUpperIO(desc = "夹臂不同步报警")] public bool ClampOutOfSync;
#endregion
#region AsLowerIO
@@ -42,6 +48,16 @@ namespace MedullaAdapter
[AsLowerIO(desc = "左后右轮实际位置")] public float LRRActualPos;
[AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
[AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
[AsLowerIO(desc = "左夹臂实际速度")] public float ActualSpeedLeftArm;
[AsLowerIO(desc = "右夹臂实际速度")] public float ActualSpeedRightArm;
[AsLowerIO(desc = "左夹臂状态字")] public int LeftArmStateCode;
[AsLowerIO(desc = "右夹臂状态字")] public int RightArmStateCode;
[AsLowerIO(desc = "左夹臂错误字")] public int LeftArmErrorCode;
[AsLowerIO(desc = "右夹臂错误字")] public int RightArmErrorCode;
[AsLowerIO(desc = "左夹臂电流")] public float LeftArmElectric;
[AsLowerIO(desc = "右夹臂电流")] public float RightArmElectric;
[AsLowerIO(desc = "左夹臂实际位置")] public float ActualPosLeftArm;
[AsLowerIO(desc = "右夹臂实际位置")] public float ActualPosRightArm;
[AsLowerIO(desc = "驱动轮使能状态")] public bool WheelAbleState = true;
[AsLowerIO(desc = "电池健康状态")] public float SOH;
[AsInitParam(desc = "车号")][AsLowerIO] public int CarNum = 1;
@@ -51,6 +67,12 @@ 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 = "手动控制夹臂速度系数")] public float ManualArmSpeedFac = 1.0f;
[AsInitParam(desc = "左夹臂低限位")][AsLowerIO] public int LeftArmLowerPos = -10000;
[AsInitParam(desc = "左夹臂高限位")][AsLowerIO] public int LeftArmUpperPos = 5927610;
[AsInitParam(desc = "右夹臂低限位")][AsLowerIO] public int RightArmLowerPos = -17295;
[AsInitParam(desc = "右夹臂高限位")][AsLowerIO] public int RightArmUpperPos = 5927610;
#endregion
@@ -75,6 +97,8 @@ namespace MedullaAdapter
[IOObjectMonitor(desc = "左后右驱动器远程帧706")] public byte LRRRemoteCode = 0;
[IOObjectMonitor(desc = "右后左驱动器远程帧707")] public byte RRLRemoteCode = 0;
[IOObjectMonitor(desc = "右后右驱动器远程帧708")] public byte RRRRemoteCode = 0;
[IOObjectMonitor(desc = "左夹臂驱动器远程帧709")] public byte LArmRemoteCode = 0;
[IOObjectMonitor(desc = "右夹臂驱动器远程帧70A")] public byte RArmRemoteCode = 0;
#endregion
#region 操作按钮
@@ -188,43 +212,193 @@ namespace MedullaAdapter
TimeSpan? interval = null)
{
if (Chassis == null) return;
var adapter = GetChassisAdapter();
if (adapter == null) return;
// 模式变化时先停车并下发舵轮准备角度；
// 在实际舵角到位之前，不开放驱动速度。
if (!EnsureManualModeReady(mode, interval))
{
adapter.StopImmediately();
return;
}
var speed = speedThreshold * y;
var thPow = (float)Math.Pow(Math.Abs(x), ManualThetaPow) * Math.Sign(x);
var frontTh = -thPow * MaxManualTheta;
var rearTh = thPow * MaxManualTheta;
var omega = CalculateManualOmega(speed, x);
ManualMode = (int)mode;
switch (mode)
{
case ManualControlMode.Normal:
SetChassisDirection(frontDirection);
Chassis.SendMotion(speed, frontTh, rearTh, interval);
SendBodyCommand(vx: speed, vy: 0.0, omegaRadiansPerSecond: omega, interval);
break;
case ManualControlMode.Crab:
SetChassisDirection(90);
Chassis.SendMotion(speed, frontTh, rearTh, interval);
SendBodyCommand(vx: 0.0, vy: speed, omegaRadiansPerSecond: omega, interval);
break;
case ManualControlMode.Spin:
Chassis.SendRotateMotion(speed * MaxAngularSpeed, interval);
var spinOmega =
speed * MaxAngularSpeed *
Math.PI / 180.0;
SendBodyCommand(
vx: 0.0,
vy: 0.0,
omegaRadiansPerSecond: spinOmega,
interval);
break;
default:
ManualMode = -1;
Chassis.PredefinedDriveStop();
adapter.StopImmediately();
break;
}
}
private void SetChassisDirection(float direction)
// 停车后切换模式：先预转舵轮，实际角度到位后才允许发送运动命令。
private bool EnsureManualModeReady(
ManualControlMode mode,
TimeSpan? interval)
{
var origin = Chassis.GetOriginBias();
// 方向没有变化时不重复计算轮组几何关系。
if (Math.Abs(origin.Z - direction) < 0.001f)
var adapter = GetChassisAdapter();
if (adapter == null)
return false;
// 当前模式已经完成准备，可以直接接受运动命令。
if (_activeManualMode == mode &&
_pendingManualMode == null)
{
return true;
}
// 第一次收到新模式时，停车并下发一次舵轮准备姿态。
if (_pendingManualMode != mode)
{
adapter.StopImmediately();
var preparationAccepted = mode switch
{
ManualControlMode.Normal =>
adapter.PrepareParallelDirection(0.0),
ManualControlMode.Crab =>
adapter.PrepareParallelDirection(
Math.PI / 2.0),
ManualControlMode.Spin =>
adapter.PrepareSpin(interval),
_ => false
};
if (!preparationAccepted)
{
_pendingManualMode = null;
return false;
}
_pendingManualMode = mode;
return false;
}
// 后续控制周期保持停车，并读取实际舵角判断是否到位。
adapter.StopImmediately();
const double toleranceRadians =
2.0 * Math.PI / 180.0;
bool aligned;
if (mode == ManualControlMode.Spin)
{
// 自转的四个舵轮目标角不同，等待期间持续刷新其目标。
var preparationAccepted =
adapter.PrepareSpin(interval);
aligned =
preparationAccepted &&
adapter.AreSpinWheelsAligned;
}
else
{
var targetDirection = mode ==
ManualControlMode.Crab
? Math.PI / 2.0
: 0.0;
aligned =
adapter.AreParallelWheelsAligned(
targetDirection,
toleranceRadians);
}
if (!aligned)
return false;
_activeManualMode = mode;
_pendingManualMode = null;
return true;
}
private double CalculateManualOmega(
float speed,
float steeringInput)
{
var normalizedSteering =
(float)Math.Pow(
Math.Abs(steeringInput),
ManualThetaPow) *
Math.Sign(steeringInput);
var steeringDegrees =
-normalizedSteering * MaxManualTheta;
var steeringRadians =
steeringDegrees * Math.PI / 180.0;
// CommonUsage中的ControlPointRadius单位为毫米。
var halfWheelBaseMeters =
Math.Max(
Chassis.ControlPointRadius / 1000.0,
0.01);
return speed * Math.Tan(steeringRadians) / halfWheelBaseMeters;
}
internal void SendBodyCommand(double vx, double vy, double omegaRadiansPerSecond, TimeSpan? interval = null)
{
var adapter = GetChassisAdapter();
if (adapter == null)
return;
Chassis.SetOriginBias(
origin.X,
origin.Y,
direction);
// 坐标系改变后重新等待四个舵轮对齐。
Chassis.AfterDirectionChanged();
var command = new ChassisCommand(
CarNum,
new Twist2D(vx, vy, omegaRadiansPerSecond));
if (!adapter.Send(command, interval))
{
adapter.StopImmediately();
Console.WriteLine(
"底盘命令分解失败，车辆已经停车：" +
adapter.LastFailureReason);
}
}
private MultiWheelChassisAdapter _chassisAdapter;
private MultiWheelChassisAdapter GetChassisAdapter()
{
if (Chassis == null)
return null;
if (_chassisAdapter == null ||
_chassisAdapter.VehicleId != CarNum)
{
_chassisAdapter =
new MultiWheelChassisAdapter(Chassis, CarNum);
}
return _chassisAdapter;
}
#region MCURoutine兼容参数（暂保留原硬件协议）
@@ -237,65 +411,6 @@ namespace MedullaAdapter
[AsUpperIO(desc = "多车灯光同步兼容值，-1使用本车灯光")]
public int MultiVehicleLightSync = -1;
// 以下夹臂字段用于兼容原MCU的0x209、0x20A等CAN协议；当前不使用时目标速度保持为0。
[AsUpperIO(desc = "左夹臂目标速度", timeOutReset = true)]
public float SpeedLeftArm;
[AsUpperIO(desc = "右夹臂目标速度", timeOutReset = true)]
public float SpeedRightArm;
[AsLowerIO(desc = "左夹臂实际速度")]
public float ActualSpeedLeftArm;
[AsLowerIO(desc = "右夹臂实际速度")]
public float ActualSpeedRightArm;
[AsLowerIO(desc = "左夹臂实际位置")]
public float ActualPosLeftArm;
[AsLowerIO(desc = "右夹臂实际位置")]
public float ActualPosRightArm;
[AsLowerIO(desc = "左夹臂状态字")]
public int LeftArmStateCode;
[AsLowerIO(desc = "右夹臂状态字")]
public int RightArmStateCode;
[AsLowerIO(desc = "左夹臂错误字")]
public int LeftArmErrorCode;
[AsLowerIO(desc = "右夹臂错误字")]
public int RightArmErrorCode;
[AsLowerIO(desc = "左夹臂电流")]
public float LeftArmElectric;
[AsLowerIO(desc = "右夹臂电流")]
public float RightArmElectric;
[AsInitParam(desc = "左夹臂低限位")]
[AsLowerIO]
public int LeftArmLowerPos = -10000;
[AsInitParam(desc = "左夹臂高限位")]
[AsLowerIO]
public int LeftArmUpperPos = 5927610;
[AsInitParam(desc = "右夹臂低限位")]
[AsLowerIO]
public int RightArmLowerPos = -17295;
[AsInitParam(desc = "右夹臂高限位")]
[AsLowerIO]
public int RightArmUpperPos = 5927610;
[AsLowerIO(desc = "左夹臂驱动器远程帧709")]
public byte LArmRemoteCode;
[AsLowerIO(desc = "右夹臂驱动器远程帧70A")]
public byte RArmRemoteCode;
#endregion
}
MedullaAdapter/MedullaAdapter.csproj
+11 -1
View File
@@ -17,7 +17,6 @@
<HintPath>ref\RefCartActivator.dll</HintPath>
<Private>false</Private>
</Reference>
<Reference Include="MedullaCore">
<HintPath>ref\RefMedullaCore.dll</HintPath>
<Private>false</Private>
@@ -44,4 +43,15 @@
</Reference>
</ItemGroup>
<ItemGroup>
<Compile Include="..\Shared\ChassisCommand.cs"
Link="Shared\ChassisCommand.cs" />
<Compile Include="..\Shared\FrameTransform2D.cs"
Link="Shared\FrameTransform2D.cs" />
<Compile Include="..\Shared\MultiWheelChassisAdapter.cs"
Link="Shared\MultiWheelChassisAdapter.cs" />
</ItemGroup>
</Project>
MedullaAdapter/MotorRoutine.cs
+32 -1
View File
@@ -56,6 +56,7 @@ namespace MedullaAdapter
cart.TransmitterSpeed,
DateTime.Now - cart.TransmitterLastTime);
StopClampArms();
cart.TransmitterLastTime = DateTime.Now;
}
@@ -95,6 +96,7 @@ namespace MedullaAdapter
0, 0, 0,
cart.TransmitterSpeed,
interval);
StopClampArms();
return;
}
// SA关闭后立即停车。
@@ -105,6 +107,7 @@ namespace MedullaAdapter
0, 0, 0,
cart.TransmitterSpeed,
interval);
StopClampArms();
return;
}
// 限制实体遥控器的最大速度。
@@ -116,6 +119,9 @@ namespace MedullaAdapter
// SD的Mode0作为底盘驾驶档。
if (cart.Transmitter_SD == TransmitterState.Mode0)
{
// 底盘驾驶档不允许保留上一周期的夹臂速度。
StopClampArms();
cart.ManualControl(
cart.TransmitterControlMode,
cart.TransmitterLeftJoystickValX,
@@ -126,12 +132,37 @@ namespace MedullaAdapter
return;
}
if (cart.Transmitter_SD == TransmitterState.Mode1)
{
// 切换到夹臂档时，先确保底盘停止。
cart.ManualControl(
cart.TransmitterControlMode,
0, 0, 0,
cart.TransmitterSpeed,
interval);
var armSpeed =
cart.TransmitterRightJoystickValX *
cart.ManualArmSpeedFac;
cart.SpeedLeftArm = armSpeed;
cart.SpeedRightArm = armSpeed;
return;
}
// 非驾驶档必须主动停车，防止上一条运动指令残留。
cart.ManualControl(
cart.TransmitterControlMode,
0, 0, 0,
cart.TransmitterSpeed,
interval);
StopClampArms();
}
// M层单车夹臂安全：清除物理遥控器留下的左右夹臂速度命令。
private void StopClampArms()
{
cart.SpeedLeftArm = 0;
cart.SpeedRightArm = 0;
}
// M层单车底盘：根据四个舵轮的目标角度和实际角度修正8个驱动电机速度。
@@ -328,4 +359,4 @@ namespace MedullaAdapter
}
}
}
}
MedullaAdapter/Remote.cs
+72 -1
View File
@@ -1,15 +1,86 @@
// Medulla虚拟遥控器和夹臂控制
using MDCSToolBox.Medulla.Chassis.MultiWheel;
using CartActivator;
namespace MedullaAdapter
{
// M层单车虚拟遥控器：使用父类提供的底盘控制界面。
public class Remote : MultiWheelRemote<DiverCartDefinition>
{
// 将父类虚拟遥控器界面输入统一转发到本车的ManualControl。
public override void ChassisOperation()
{
if (MultiVehicleMode.on)
{
MultiVehicleModeChassisLogic();
statusText = "单车版本不支持多车联动遥控";
return;
}
// 当前单车适配层只定义Normal、Crab和Spin三种模式。
// 禁止这些旧按钮绕过适配层直接修改底盘坐标偏置。
if (AckermannMode.on ||
SwayMode.on ||
XYThMode.on)
{
cart.Chassis?.PredefinedDriveStop();
statusText = "当前单车版本暂不支持阿克曼、斜行或全向模式";
return;
}
var mode = SpinMode.on
? DiverCartDefinition.ManualControlMode.Spin
: CrabMode.on
? DiverCartDefinition.ManualControlMode.Crab
: DiverCartDefinition.ManualControlMode.Normal;
cart.ManualControl(
mode,
SpeedPad.x,
SpeedPad.y,
FrontDirection.dval * 180,
SpeedThreshold.val);
statusText =
$"{mode}, x={SpeedPad.x:0.00}, " +
$"y={SpeedPad.y:0.00}, " +
$"speed={SpeedThreshold.val:0.00}";
}
[AsControlItem(name = "夹抱速度", LayoutRow = 0, LayoutCol = 4)]
public Throttle ArmSpeed;
[AsControlItem(name = "夹抱打开", LayoutRow = 1, LayoutCol = 0)]
public Button Open;
[AsControlItem(name = "夹抱关闭", LayoutRow = 1, LayoutCol = 2)]
public Button Close;
// M层虚拟遥控器：控制左右夹臂同步打开或关闭。
public override void CustomOperation()
{
if (Open.pressed)
{
cart.SpeedLeftArm =
-ArmSpeed.val * cart.ManualArmSpeedFac;
cart.SpeedRightArm =
-ArmSpeed.val * cart.ManualArmSpeedFac;
}
else if (Close.pressed)
{
cart.SpeedLeftArm =
ArmSpeed.val * cart.ManualArmSpeedFac;
cart.SpeedRightArm =
ArmSpeed.val * cart.ManualArmSpeedFac;
}
else
{
cart.SpeedLeftArm = 0;
cart.SpeedRightArm = 0;
}
}
// 单车不支持多车联动，误打开多车开关时主动停车。
public override void MultiVehicleModeChassisLogic()
{
cart.Chassis?.PredefinedDriveStop();
}
}
}
}
MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll
BIN
View File
Binary file not shown.
MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb
BIN
View File
Binary file not shown.