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

Initial commit from MyParking project

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-04 10:29:21 +08:00
commit d9432bd529
138 changed files with 16957 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MedullaAdapter/DiverCartDefinition.cs
+532
View File
@@ -0,0 +1,532 @@
// 定义车型、上下层IO、参数和MCU初始化
using CartActivator;
using MCUSerialBridgeCLR;
using MDCSToolBox.Medulla.Chassis.MultiWheel;
using Medulla.Types;
using System;
using System.Collections.Generic;
using System.Threading;
using MyParking.Shared;
namespace MedullaAdapter
{
[UseLadderLogic(logic = typeof(AlarmRoutine), scanInterval = 50)]
[UseLadderLogic(logic = typeof(MotorRoutine), scanInterval = 50)]
[UseLadderLogic(logic = typeof(MCURoutine), scanInterval = 20)]
[UseManualController(manualController = typeof(Remote))]
public class DiverCartDefinition : MultiWheelCartDefinition
{
#region 基本成员
public MCUSerialBridge Bridge;
internal enum ManualControlMode
{
Normal = 0, // 正常模式
Crab = 1, // 螃蟹模式
Spin = 2, // 自旋模式
}
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
[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 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;
#endregion
#region 初始参数
[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 = "遥控转弯舵角同步限速宽度，单位为度")]
public float ManualSteeringAlignmentSigmaDegrees = 8.0f;
[AsInitParam(desc = "自转最大角速度，单位deg/s")]
public float MaxSpinAngularSpeedDegreesPerSecond = 30f;
[AsInitParam(desc = "轮速诊断日志相对目录")]
public string WheelSpeedDiagnosticDirectory =
@"logs\wheel-speed";
[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
#region 监控参数
[IOObjectMonitor(desc = "从M上复位")] public bool ResetFromM;
[IOObjectMonitor(desc = "从M将驱动轮下使能")] public bool DisableFromM;
[IOObjectMonitor(desc = "左前左轮PID修正后速度")] public float SpeedLFL;
[IOObjectMonitor(desc = "左前右轮PID修正后速度")] public float SpeedLFR;
[IOObjectMonitor(desc = "右前左轮PID修正后速度")] public float SpeedRFL;
[IOObjectMonitor(desc = "右前右轮PID修正后速度")] public float SpeedRFR;
[IOObjectMonitor(desc = "左后左轮PID修正后速度")] public float SpeedLRL;
[IOObjectMonitor(desc = "左后右轮PID修正后速度")] public float SpeedLRR;
[IOObjectMonitor(desc = "右后左轮PID修正后速度")] public float SpeedRRL;
[IOObjectMonitor(desc = "右后右轮PID修正后速度")] public float SpeedRRR;
[IOObjectMonitor(desc = "左前舵轮转向PID输出")] public float DiffSteerOutputLeftFront;
[IOObjectMonitor(desc = "左后舵轮转向PID输出")] public float DiffSteerOutputLeftRear;
[IOObjectMonitor(desc = "右前舵轮转向PID输出")] public float DiffSteerOutputRightFront;
[IOObjectMonitor(desc = "右后舵轮转向PID输出")] public float DiffSteerOutputRightRear;
[IOObjectMonitor(desc = "灯光模式")] public int LightMode = 0;
[IOObjectMonitor(desc = "实体遥控器当前速度倍率")] public float TransmitterSpeed = 0.3f;
[IOObjectMonitor(desc = "轮速诊断记录已启用")]
public bool WheelSpeedDiagnosticEnabled;
[IOObjectMonitor(desc = "轮速诊断记录状态")]
public string WheelSpeedDiagnosticStatus = "未启动";
[IOObjectMonitor(desc = "左前左驱动器远程帧701")] public byte LFLRemoteCode = 0;
[IOObjectMonitor(desc = "左前右驱动器远程帧702")] public byte LFRRemoteCode = 0;
[IOObjectMonitor(desc = "右前左驱动器远程帧703")] public byte RFLRemoteCode = 0;
[IOObjectMonitor(desc = "右前右驱动器远程帧704")] public byte RFRRemoteCode = 0;
[IOObjectMonitor(desc = "左后左驱动器远程帧705")] public byte LRLRemoteCode = 0;
[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 操作按钮
// M层单车硬件：向驱动轮发送复位请求。
[IOObjectUtility]
public void WheelReset()
{
ResetFromM = true;
}
// M层单车硬件：向驱动轮发送下使能请求。
[IOObjectUtility]
public void WheelDisable()
{
DisableFromM = true;
}
// M层诊断：请求开始保存CAN轮速事件和底盘周期快照。
[IOObjectUtility]
public void StartWheelSpeedDiagnostic()
{
WheelSpeedDiagnosticEnabled = true;
WheelSpeedDiagnosticStatus = "等待创建日志文件";
}
// M层诊断：请求停止轮速记录并刷新CSV文件。
[IOObjectUtility]
public void StopWheelSpeedDiagnostic()
{
WheelSpeedDiagnosticEnabled = false;
WheelSpeedDiagnosticStatus = "等待停止并刷新日志";
}
#endregion
public override void CommunicationInit()
{
if (GhostMode) return;
State = -1;
Bridge = new MCUSerialBridge();
//Step1:打开指定串口连接
var err = Bridge.Open(MCUPort, 1000000u);
if (err != MCUSerialBridgeError.OK)
{
Console.WriteLine($"MCU Open FAILED: {err.ToDescription()}");
return;
}
else
{
Console.WriteLine("MCU Open OK");
}
//Step2:远程复位MCU
err = Bridge.Reset();
if (err != MCUSerialBridgeError.OK)
{
Console.WriteLine($"MCU Reset FAILED: {err.ToDescription()}");
return;
}
else
{
Thread.Sleep(500);
Console.WriteLine("MCU Reset OK");
}
//Step3:获取MCU版本号
err = Bridge.GetVersion(out var version, 100);
if (err != MCUSerialBridgeError.OK)
{
Console.WriteLine($"MCU GetVersion FAILED: {err.ToDescription()}");
return;
}
else
{
Console.WriteLine($"MCU GetVersion OK: {version}");
}
//Step4:获取MCU状态
err = Bridge.GetState(out var state, 100);
if (err != MCUSerialBridgeError.OK)
{
Console.WriteLine($"MCU GetState FAILED: {err.ToDescription()}");
return;
}
else
{
Console.WriteLine($"MCU GetState OK: {state}");
}
//Step5:串口/CAN配置
try
{
var ports = new List<PortConfig>();
for (int i = 0; i < 1; i++)
ports.Add(new CANPortConfig(500000, 10));
for (int i = 0; i < 3; i++)
ports.Add(new SerialPortConfig(9600, 10));
Console.WriteLine("=== Port Configuration ===");
for (int i = 0; i < ports.Count; i++)
{
if (ports[i] is SerialPortConfig s)
Console.WriteLine($"Port {i}: Serial, Baud={s.Baud}, ReceiveFrameMs={s.ReceiveFrameMs}");
else if (ports[i] is CANPortConfig c)
Console.WriteLine($"Port {i}: CAN, Baud={c.Baud}, RetryTimeMs={c.RetryTimeMs}");
}
var ret = Bridge.Configure(ports, 200);
if (ret != MCUSerialBridgeError.OK)
{
Console.WriteLine($"MCU Configure FAILED: {ret.ToDescription()}");
return;
}
else
{
Console.WriteLine("MCU Configure OK");
}
Console.WriteLine("MCU Configure {0}", ret == MCUSerialBridgeError.OK ? "OK" : $"FAILED: 0x{(uint)ret:X8}");
}
catch (Exception ex)
{
Console.WriteLine($"Configure Exception: {ex.Message}");
return;
}
State = 0;
}
internal void ManualControl(
ManualControlMode mode,
float x,
float y,
float frontDirection,
float speedThreshold,
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 normalizedSteering =
(float)Math.Pow(
Math.Abs(x),
ManualThetaPow) *
Math.Sign(x);
var steeringDegrees =
-normalizedSteering * MaxManualTheta;
var frontTh = steeringDegrees;
var rearTh = -steeringDegrees;
ManualMode = (int)mode;
switch (mode)
{
case ManualControlMode.Normal:
// 普通模式统一使用车体速度命令：
// X向前，行驶中连续改变角速度时舵轮边转、车辆边走。
// SendBodyCommand(
// vx: speed,
// vy: 0.0,
// omegaRadiansPerSecond: omega,
// interval);
Chassis.SendMotion(
speed,
frontTh,
rearTh,
interval);
break;
case ManualControlMode.Crab:
// 舵轮机械范围为[-120°,120°]。
// 蟹行后虚拟轴距由原车宽度决定，比正常模式轴距短。
// 按几何比例缩小转角，使相同摇杆输入获得接近一致的曲率。
var normalSteeringRadians =
AngleMath.DegreesToRadians(steeringDegrees);
var geometryRatio =
adapter.HalfTrackWidthMeters /
adapter.HalfWheelBaseMeters;
// +90°运动坐标系已经把虚拟左侧映射为车体后方，
// 此处保持普通模式的转向符号，避免再次取反导致左右颠倒。
var crabSteeringRadians =
Math.Atan(
geometryRatio *
Math.Tan(
normalSteeringRadians));
// 蟹行转角最终限制为±30°，为±120°机械舵角保留余量。
var maximumCrabSteeringRadians =
AngleMath.DegreesToRadians(30.0);
crabSteeringRadians = Math.Max(
-maximumCrabSteeringRadians,
Math.Min(
maximumCrabSteeringRadians,
crabSteeringRadians));
// 将车体左侧作为虚拟阿克曼车头，并在该运动坐标系中
// 复用与普通模式相同的SendMotion前后控制点解算。
if (!adapter.SendVirtualAckermannMotion(
motionDirectionRadians:
Math.PI / 2.0,
speedMetersPerSecond:
speed,
steeringRadians:
crabSteeringRadians,
interval))
{
adapter.StopImmediately();
Console.WriteLine(
"蟹行SendMotion命令分解失败，车辆已经停车：" +
adapter.LastFailureReason);
}
break;
case ManualControlMode.Spin:
// 摇杆处于中位时只清零驱动速度，保持已经准备好的
// 自转舵角；下次推动摇杆时仍会重新检查实际舵角。
if (Math.Abs(speed) < 1e-6f)
{
adapter
.StopXYThDrivePreserveSteeringState();
break;
}
// 自转时speed表示最外侧舵轮中心的目标切向速度，
// 根据v=omega*r换算为SendXYThSpeed需要的角速度。
var requestedSpinOmegaRadiansPerSecond =
speed /
adapter.MaximumWheelRadiusMeters;
// 对半径换算结果做正负对称限幅，防止遥控速度参数误设后自转过快。
var maximumSpinOmegaRadiansPerSecond =
AngleMath.DegreesToRadians(
Math.Max(
0f,
MaxSpinAngularSpeedDegreesPerSecond));
var spinOmegaRadiansPerSecond =
Math.Max(
-maximumSpinOmegaRadiansPerSecond,
Math.Min(
maximumSpinOmegaRadiansPerSecond,
requestedSpinOmegaRadiansPerSecond));
// 普通安全版SendXYThSpeed只下发角速度，
// 四轮实际舵角未到位时不会开放驱动速度。
if (!adapter.Send(
new ChassisCommand(
CarNum,
new Twist2D(
0.0,
0.0,
spinOmegaRadiansPerSecond)),
interval))
{
adapter.StopImmediately();
Console.WriteLine(
"SendXYThSpeed原地自转命令分解失败，车辆已经停车：" +
adapter.LastFailureReason);
}
break;
default:
ManualMode = -1;
adapter.StopImmediately();
break;
}
}
// 停车后切换模式：先预转舵轮，实际角度到位后才允许发送运动命令。
private bool EnsureManualModeReady(
ManualControlMode mode,
TimeSpan? interval)
{
var adapter = GetChassisAdapter();
if (adapter == null)
return false;
// 当前模式已经完成准备，可以直接接受运动命令。
if (_activeManualMode == mode &&
_pendingManualMode == null)
{
return true;
}
// 第一次收到新模式时，停车并下发一次舵轮准备姿态。
if (_pendingManualMode != mode)
{
adapter.StopImmediately();
// 所有模式的准备角度均按真实机械舵角表达；
// 先退出上一模式的虚拟运动坐标系，再执行预对齐。
adapter.ResetToBodyFrame();
_activeManualMode = null;
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();
var toleranceRadians =
AngleMath.DegreesToRadians(2.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;
if (mode == ManualControlMode.Spin)
{
// 四轮实际舵角确认到位后只交接一次，保留PrepareSpin
// 选定的机械舵角和轮速方向，避免首条XYTh命令重新选角。
if (!adapter.AdoptPreparedSpinForXYTh(
toleranceRadians))
{
return false;
}
}
// 蟹行轮子在真实车体系中到达机械+90°后，
// 再将车体左侧激活为SendMotion的虚拟X正方向。
else if (mode == ManualControlMode.Crab)
{
adapter.ActivateMotionFrame(
Math.PI / 2.0);
}
else
{
adapter.ResetToBodyFrame();
}
_activeManualMode = mode;
_pendingManualMode = null;
return true;
}
private MultiWheelChassisAdapter _chassisAdapter;
private MultiWheelChassisAdapter GetChassisAdapter()
{
if (Chassis == null)
return null;
if (_chassisAdapter == null ||
_chassisAdapter.VehicleId != CarNum)
{
_chassisAdapter =
new MultiWheelChassisAdapter(Chassis, CarNum);
}
_chassisAdapter.SteeringAlignmentSigmaDegrees =
Math.Max(
ManualSteeringAlignmentSigmaDegrees,
0.1f);
return _chassisAdapter;
}
#region MCURoutine兼容参数（暂保留原硬件协议）
// 保存MCU读取到的原始输入字节，供M层监控和硬件排查使用。
[AsLowerIO(desc = "MCU原始输入字节")]
public float test;
// 保留原MCU灯光分支；单车默认值-1表示使用本车LightMode。
[AsUpperIO(desc = "多车灯光同步兼容值，-1使用本车灯光")]
public int MultiVehicleLightSync = -1;
#endregion
}
}