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/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
}