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-22 17:37:36 +08:00
parent 6a3d1414dc
commit 580a936a83
5 changed files with 1386 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MedullaAdapter/AlarmRoutine.cs
+14 -1
View File
@@ -1 +1,14 @@
// 驱动器、急停、夹臂等安全报警
// 驱动器、急停、夹臂等安全报警
using MDCSToolBox.Medulla.Chassis.MultiWheel;
namespace MedullaAdapter
{
public class AlarmRoutine : MultiWheelAlarmRoutine<DiverCartDefinition>
{
// M层单车安全：汇总停车机器人夹臂等自定义报警状态。
public override void SetOtherAlarms()
{
// 当前阶段不使用夹臂，暂时没有自定义报警。
}
}
}
MedullaAdapter/DiverCartDefinition.cs
+129 -6
View File
@@ -9,6 +9,10 @@ using System.Threading;
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 基本成员
@@ -17,16 +21,14 @@ namespace MedullaAdapter
internal enum ManualControlMode
{
Normal = 0, // 正常模式
Spin = 1, // 自旋模式
Crab = 2, // 螃蟹模式
Crab = 1, // 螃蟹模式
Spin = 2, // 自旋模式
}
internal ManualControlMode TransmitterControlMode = ManualControlMode.Normal;
internal DateTime TransmitterLastTime; // 物理遥控器计算两次实体遥控器指令之间的时间间隔
internal DateTime TransmitterLastTime = DateTime.Now; // 物理遥控器计算两次实体遥控器指令之间的时间间隔
#endregion
#region AsUpperIO
[AsUpperIO(desc = "从M上复位")] public bool ResetFromM;
[AsUpperIO(desc = "从M将驱动轮下使能")] public bool DisableFromM;
[AsUpperIO(desc = "从C上复位")] public bool ResetFromC;
[AsUpperIO(desc = "从C将驱动轮下使能")] public bool DisableFromC;
#endregion
@@ -53,6 +55,8 @@ namespace MedullaAdapter
#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;
@@ -175,5 +179,124 @@ namespace MedullaAdapter
State = 0;
}
internal void ManualControl(
ManualControlMode mode,
float x,
float y,
float frontDirection,
float speedThreshold,
TimeSpan? interval = null)
{
if (Chassis == null) 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;
ManualMode = (int)mode;
switch (mode)
{
case ManualControlMode.Normal:
SetChassisDirection(frontDirection);
Chassis.SendMotion(speed, frontTh, rearTh, interval);
break;
case ManualControlMode.Crab:
SetChassisDirection(90);
Chassis.SendMotion(speed, frontTh, rearTh, interval);
break;
case ManualControlMode.Spin:
Chassis.SendRotateMotion(speed * MaxAngularSpeed, interval);
break;
default:
ManualMode = -1;
Chassis.PredefinedDriveStop();
break;
}
}
private void SetChassisDirection(float direction)
{
var origin = Chassis.GetOriginBias();
// 方向没有变化时不重复计算轮组几何关系。
if (Math.Abs(origin.Z - direction) < 0.001f)
return;
Chassis.SetOriginBias(
origin.X,
origin.Y,
direction);
// 坐标系改变后重新等待四个舵轮对齐。
Chassis.AfterDirectionChanged();
}
#region MCURoutine兼容参数（暂保留原硬件协议）
// 保存MCU读取到的原始输入字节，供M层监控和硬件排查使用。
[AsLowerIO(desc = "MCU原始输入字节")]
public float test;
// 保留原MCU灯光分支；单车默认值-1表示使用本车LightMode。
[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/MCURoutine.cs
+897 -1
View File
@@ -1 +1,897 @@
// 实际CAN协议、反馈解析、IO、电池、急停
// 实际CAN协议、反馈解析、IO、电池、急停
using CartActivator;
using FundamentalLib;
using MCUSerialBridgeCLR;
using System;
using System.Collections.Generic;
namespace MedullaAdapter
{
public class MCURoutine : LadderLogic<DiverCartDefinition>
{
private int _lastIteration = 0;
private int _count = 0;
private int _operationTime = 0;
private bool _driversDisabled;
private bool _canCallbackRegistered;
private bool _serialCallbackRegistered;
private byte _resetCode = 0x86;
private byte _enableCode1 = 0x06;
private byte _enableCode2 = 0x07;
private byte _enableCode3 = 0x0F;
private bool io_bit0 = false;//继电器
private bool io_bit1 = false;//抱闸
private bool io_bit2 = false;//红灯
private bool io_bit3 = false;//绿灯
private bool io_bit4 = false;//黄灯
private const byte BatteryPortIndex = 3;
private static readonly byte[] BatteryRequest = BuildBatteryRequest();
// M层单车底盘：将车轮线速度换算为驱动电机转速。
private static float ConvertMps2Rpm(float mps)
{
return (float)(mps / (Math.PI * 85f) * 10.5f * 60f * 1000f);
}
// M层单车底盘：将驱动电机转速换算为车轮线速度。
private static float ConvertRpm2Mps(float rpm)
{
return (float)(rpm / 10.5f / 60f * Math.PI * 85f / 1000f);
}
// M层单车底盘：将夹臂或执行器转数换算为毫米位移。
private float ConvertR2MM(float r)
{
return (float)(r / 10.5f * Math.PI * 85);
}
// M层CAN解析：从驱动器报文中解码有符号转速。
private static float DecodeRpmFromPayload(byte[] payload, int offset = 4)
{
return BitConverter.ToInt32(payload, offset) * 1875f / 512f / 10000f;
}
// M层硬件主循环：交换IO、发送轮组指令并更新车辆反馈状态。
public override void Operation(int iteration)
{
if (_lastIteration != iteration)
{
_lastIteration = iteration;
_count = 0;
}
else
{
_count++;
}
if (_count >= 15)
{
cart.AlarmLevel = 2;
Console.WriteLine("mcu lost connection \n");
}
if (_count >= 30)
{
return;
}
if (cart?.Bridge == null)
{
return;
}
//注册CAN回调
EnsureCanCallbacksRegistered();
//注册串口回调
EnsureSerialCallbacksRegistered();
PollBatterySerial(iteration);
#region io部分
var ioReadBuffer = new byte[4];
var readInputErr = cart.Bridge.ReadInput(out ioReadBuffer, 20);
if (readInputErr == MCUSerialBridgeError.OK)
{
cart.test = ioReadBuffer[0];
cart.Start = (ioReadBuffer[0] & (1 << 0)) != 0;
cart.ResetPressed = (ioReadBuffer[0] & (1 << 1)) != 0;
cart.ChassisMode = ((ioReadBuffer[0] & (1 << 2)) == 0) ? 0 : 1;
cart.BrakeEnable = (ioReadBuffer[0] & (1 << 3)) != 0;
if ((ioReadBuffer[0] & (1 << 4)) == 0)
{
cart.EmergencyPressed = 1;
}
}
else
{
Console.WriteLine("IO Read FAILED");
}
var ioWriteBuffer = new byte[4];
io_bit0 = cart.ChargePort ? true : false;
io_bit1 = cart.EmergencyPressed == 1 ? false : true;
if (cart.MultiVehicleLightSync == 0)
{
io_bit2 = false;
io_bit3 = false;
io_bit4 = false;
}
else if (cart.MultiVehicleLightSync == 1)
{
io_bit2 = false;
io_bit3 = true;
io_bit4 = false;
}
else
{
if (cart.LightMode == 2)
{
io_bit2 = true;//红
io_bit3 = false;//绿
io_bit4 = false;//黄
}
else if (cart.LightMode == 3)
{
io_bit2 = false;
io_bit3 = false;
io_bit4 = true;
}
else if (cart.LightMode == 1)
{
io_bit2 = false;
io_bit3 = true;
io_bit4 = false;
}
else
{
io_bit2 = false;
io_bit3 = false;
io_bit4 = false;
}
}
ioWriteBuffer[0] = (byte)((io_bit0 ? 1 << 0 : 0) | //继电器
(io_bit1 ? 1 << 1 : 0) | //抱闸
(io_bit2 ? 1 << 2 : 0) | //红灯
(io_bit3 ? 1 << 3 : 0) | //绿灯
(io_bit4 ? 1 << 4 : 0)); //黄灯
if (cart.Bridge.WriteOutput(ioWriteBuffer, 20) != MCUSerialBridgeError.OK)
{
Console.WriteLine("IO Write FAILED");
}
#endregion
#region 驱动部分
cart.ActualSpeedLeftFront = (cart.ActualSpeedLeftFrontLeft + cart.ActualSpeedLeftFrontRight) / 2;
cart.ActualSpeedLeftRear = (cart.ActualSpeedLeftRearLeft + cart.ActualSpeedLeftRearRight) / 2;
cart.ActualSpeedRightFront = (cart.ActualSpeedRightFrontLeft + cart.ActualSpeedRightFrontRight) / 2;
cart.ActualSpeedRightRear = (cart.ActualSpeedRightRearLeft + cart.ActualSpeedRightRearRight) / 2;
// M层CAN辅助：封装本周期驱动器CAN发送参数。
MCUSerialBridgeError SendCan(byte port, ushort standardId, byte[] payload, bool RTR = false, uint timeout = 2)
{
var canSend = new CANMessage
{
ID = standardId,
RTR = RTR,
DLC = !RTR ? (byte)8 : (byte)0,
Payload = payload ?? new byte[] { }
};
return cart.Bridge.WriteCAN(port, canSend, timeout);
}
var resetPayload = new byte[8] { 0xFD, _resetCode, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
var enable1Payload = new byte[8] { 0xFD, _enableCode1, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
var enable2Payload = new byte[8] { 0xFD, _enableCode2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
var enable3Payload = new byte[8] { 0xFD, _enableCode3, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
Hedingben.ToastText($"_operationTime:{_operationTime}", "OperationTime");
if (_operationTime == 0)
{
SendCan(0, 0x00, new byte[] { 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 });
_operationTime++;
return;
}
else if (_operationTime == 1)
{
if (cart.LeftFrontLeftErrorCode != 0)
{
SendCan(0, 0x201, enable1Payload);
SendCan(0, 0x201, resetPayload);
}
if (cart.LeftFrontRightErrorCode != 0)
{
SendCan(0, 0x202, enable1Payload);
SendCan(0, 0x202, resetPayload);
}
if (cart.RightFrontLeftErrorCode != 0)
{
SendCan(0, 0x203, enable1Payload);
SendCan(0, 0x203, resetPayload);
}
if (cart.RightFrontRightErrorCode != 0)
{
SendCan(0, 0x204, enable1Payload);
SendCan(0, 0x204, resetPayload);
}
if (cart.LeftRearLeftErrorCode != 0)
{
SendCan(0, 0x205, enable1Payload);
SendCan(0, 0x205, resetPayload);
}
if (cart.LeftRearRightErrorCode != 0)
{
SendCan(0, 0x206, enable1Payload);
SendCan(0, 0x206, resetPayload);
}
if (cart.RightRearLeftErrorCode != 0)
{
SendCan(0, 0x207, enable1Payload);
SendCan(0, 0x207, resetPayload);
}
if (cart.RightRearRightErrorCode != 0)
{
SendCan(0, 0x208, enable1Payload);
SendCan(0, 0x208, resetPayload);
}
if (cart.LeftArmErrorCode != 0)
{
SendCan(0, 0x209, enable1Payload);
SendCan(0, 0x209, resetPayload);
}
if (cart.RightArmErrorCode != 0)
{
SendCan(0, 0x20A, enable1Payload);
SendCan(0, 0x20A, resetPayload);
}
_operationTime++;
return;
}
else if (_operationTime == 2)
{
//SendNodeGuardRequests(SendCan);
//if (AreAllNodesOperational())
//{
// _operationTime++;
//}
_operationTime++;
return;
}
//检查错误是否被消除 若没被消除就得重新返回上一步发复位 若消除了就继续往下
else if (_operationTime == 3)
{
if (cart.LeftFrontLeftErrorCode != 0 || cart.LeftFrontRightErrorCode != 0 ||
cart.LeftRearLeftErrorCode != 0 || cart.LeftRearRightErrorCode != 0
|| cart.RightFrontLeftErrorCode != 0 || cart.RightFrontRightErrorCode != 0 ||
cart.RightRearLeftErrorCode != 0 || cart.RightRearRightErrorCode != 0 ||
cart.LeftArmErrorCode != 0 || cart.RightArmErrorCode != 0)
{
_operationTime = 1;
return;
}
else
{
_operationTime++;
return;
}
}
// 没有错误 没有节点保护 就发06 07 0F使能
else if (_operationTime == 4)
{
SendCan(0, 0x201, enable1Payload);
SendCan(0, 0x202, enable1Payload);
SendCan(0, 0x203, enable1Payload);
SendCan(0, 0x204, enable1Payload);
SendCan(0, 0x205, enable1Payload);
SendCan(0, 0x206, enable1Payload);
SendCan(0, 0x207, enable1Payload);
SendCan(0, 0x208, enable1Payload);
SendCan(0, 0x209, enable1Payload);
SendCan(0, 0x20A, enable1Payload);
_operationTime++;
return;
}
else if (_operationTime == 5)
{
SendCan(0, 0x201, enable2Payload);
SendCan(0, 0x202, enable2Payload);
SendCan(0, 0x203, enable2Payload);
SendCan(0, 0x204, enable2Payload);
SendCan(0, 0x205, enable2Payload);
SendCan(0, 0x206, enable2Payload);
SendCan(0, 0x207, enable2Payload);
SendCan(0, 0x208, enable2Payload);
SendCan(0, 0x209, enable2Payload);
SendCan(0, 0x20A, enable2Payload);
_operationTime++;
return;
}
else if (_operationTime == 6)
{
SendCan(0, 0x201, enable3Payload);
SendCan(0, 0x202, enable3Payload);
SendCan(0, 0x203, enable3Payload);
SendCan(0, 0x204, enable3Payload);
SendCan(0, 0x205, enable3Payload);
SendCan(0, 0x206, enable3Payload);
SendCan(0, 0x207, enable3Payload);
SendCan(0, 0x208, enable3Payload);
SendCan(0, 0x209, enable3Payload);
SendCan(0, 0x20A, enable3Payload);
if (cart.LeftFrontLeftErrorCode != 0 || cart.LeftFrontRightErrorCode != 0 ||
cart.LeftRearLeftErrorCode != 0 || cart.LeftRearRightErrorCode != 0
|| cart.RightFrontLeftErrorCode != 0 || cart.RightFrontRightErrorCode != 0 ||
cart.RightRearLeftErrorCode != 0 || cart.RightRearRightErrorCode != 0 ||
cart.LeftArmErrorCode != 0 || cart.RightArmErrorCode != 0)
{
_operationTime = 1;
return;
}
else
{
_operationTime++;
return;
}
}
if (cart.ResetPressed || cart.ResetFromM || cart.ResetFromC)
{
cart.AlarmLevel = -1;
cart.EmergencyPressed = 0;
_count = 0;
_operationTime = 0;
_driversDisabled = false;
cart.WheelAbleState = true;
cart.ResetFromM = false;
}
if (cart.DisableFromM || cart.DisableFromC)
{
_driversDisabled = true;
cart.WheelAbleState = false;
cart.DisableFromM = false;
}
var lfl = Math.Sign(cart.SpeedLFL) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedLFL));
var lfr = Math.Sign(cart.SpeedLFR) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedLFR));
var rfl = Math.Sign(cart.SpeedRFL) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedRFL));
var rfr = Math.Sign(cart.SpeedRFR) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedRFR));
var lrl = Math.Sign(cart.SpeedLRL) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedLRL));
var lrr = Math.Sign(cart.SpeedLRR) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedLRR));
var rrl = Math.Sign(cart.SpeedRRL) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedRRL));
var rrr = Math.Sign(cart.SpeedRRR) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedRRR));
var v1 = ConvertMps2Rpm(lfl);
var v2 = ConvertMps2Rpm(-lfr);
var v3 = ConvertMps2Rpm(rfl);
var v4 = ConvertMps2Rpm(-rfr);
var v5 = ConvertMps2Rpm(lrl);
var v6 = ConvertMps2Rpm(-lrr);
var v7 = ConvertMps2Rpm(rrl);
var v8 = ConvertMps2Rpm(-rrr);
var v9 = ConvertMps2Rpm(cart.SpeedLeftArm);
var v10 = ConvertMps2Rpm(cart.SpeedRightArm);
if (cart.AlarmLevel == 2 || cart.WaitStart || cart.PreparaStart)
{
v1 = v2 = v3 = v4 = v5 = v6 = v7 = v8 = v9 = v10 = 0;
}
if ((cart.ActualPosLeftArm <= cart.LeftArmLowerPos && cart.SpeedLeftArm < 0) || (cart.ActualPosLeftArm >= cart.LeftArmUpperPos && cart.SpeedLeftArm > 0))
{
v9 = 0;
}
if ((cart.ActualPosRightArm <= cart.RightArmLowerPos && cart.SpeedRightArm < 0) || (cart.ActualPosRightArm >= cart.RightArmUpperPos && cart.SpeedRightArm > 0))
{
v10 = 0;
}
var sendLFL = BitConverter.GetBytes((int)Math.Round(v1 * 512f * 10000f / 1875f));
var sendLFR = BitConverter.GetBytes((int)Math.Round(v2 * 512f * 10000f / 1875f));
var sendRFL = BitConverter.GetBytes((int)Math.Round(v3 * 512f * 10000f / 1875f));
var sendRFR = BitConverter.GetBytes((int)Math.Round(v4 * 512f * 10000f / 1875f));
var sendLRL = BitConverter.GetBytes((int)Math.Round(v5 * 512f * 10000f / 1875f));
var sendLRR = BitConverter.GetBytes((int)Math.Round(v6 * 512f * 10000f / 1875f));
var sendRRL = BitConverter.GetBytes((int)Math.Round(v7 * 512f * 10000f / 1875f));
var sendRRR = BitConverter.GetBytes((int)Math.Round(v8 * 512f * 10000f / 1875f));
var sendLArm = BitConverter.GetBytes((int)Math.Round(v9 * 512f * 10000f / 1875f));
var sendRArm = BitConverter.GetBytes((int)Math.Round(v10 * 512f * 10000f / 1875f));
if (iteration % 2 == 0)
{
//SendNodeGuardRequests(SendCan);
}
if (_driversDisabled)
{
SendCan(0, 0x201, enable1Payload);
SendCan(0, 0x202, enable1Payload);
SendCan(0, 0x203, enable1Payload);
SendCan(0, 0x204, enable1Payload);
SendCan(0, 0x205, enable1Payload);
SendCan(0, 0x206, enable1Payload);
SendCan(0, 0x207, enable1Payload);
SendCan(0, 0x208, enable1Payload);
}
else
{
SendCan(0, 0x201, new byte[] { 0xFD, _enableCode3, 0x00, sendLFL[0], sendLFL[1], sendLFL[2], sendLFL[3], 0x00 });
SendCan(0, 0x202, new byte[] { 0xFD, _enableCode3, 0x00, sendLFR[0], sendLFR[1], sendLFR[2], sendLFR[3], 0x00 });
SendCan(0, 0x203, new byte[] { 0xFD, _enableCode3, 0x00, sendRFL[0], sendRFL[1], sendRFL[2], sendRFL[3], 0x00 });
SendCan(0, 0x204, new byte[] { 0xFD, _enableCode3, 0x00, sendRFR[0], sendRFR[1], sendRFR[2], sendRFR[3], 0x00 });
SendCan(0, 0x205, new byte[] { 0xFD, _enableCode3, 0x00, sendLRL[0], sendLRL[1], sendLRL[2], sendLRL[3], 0x00 });
SendCan(0, 0x206, new byte[] { 0xFD, _enableCode3, 0x00, sendLRR[0], sendLRR[1], sendLRR[2], sendLRR[3], 0x00 });
SendCan(0, 0x207, new byte[] { 0xFD, _enableCode3, 0x00, sendRRL[0], sendRRL[1], sendRRL[2], sendRRL[3], 0x00 });
SendCan(0, 0x208, new byte[] { 0xFD, _enableCode3, 0x00, sendRRR[0], sendRRR[1], sendRRR[2], sendRRR[3], 0x00 });
}
SendCan(0, 0x209, new byte[] { 0xFD, _enableCode3, 0x00, sendLArm[0], sendLArm[1], sendLArm[2], sendLArm[3], 0x00 });
SendCan(0, 0x20A, new byte[] { 0xFD, _enableCode3, 0x00, sendRArm[0], sendRArm[1], sendRArm[2], sendRArm[3], 0x00 });
if (cart.LeftFrontLeftErrorCode != 0 || cart.LeftFrontRightErrorCode != 0 ||
cart.LeftRearLeftErrorCode != 0 || cart.LeftRearRightErrorCode != 0
|| cart.RightFrontLeftErrorCode != 0 || cart.RightFrontRightErrorCode != 0 ||
cart.RightRearLeftErrorCode != 0 || cart.RightRearRightErrorCode != 0)
{
//_errorCount++;
}
#endregion
}
// M层CAN安全：判断单个驱动节点是否处于可运行状态。
private static bool IsNodeOperational(byte remoteCode)
{
return remoteCode == 5 || remoteCode == 133;
}
// M层CAN安全：检查全部车轮和夹臂节点是否已上线。
private bool AreAllNodesOperational()
{
return IsNodeOperational(cart.LFLRemoteCode)
&& IsNodeOperational(cart.LFRRemoteCode)
&& IsNodeOperational(cart.RFLRemoteCode)
&& IsNodeOperational(cart.RFRRemoteCode)
&& IsNodeOperational(cart.LRLRemoteCode)
&& IsNodeOperational(cart.LRRRemoteCode)
&& IsNodeOperational(cart.RRLRemoteCode)
&& IsNodeOperational(cart.RRRRemoteCode)
&& IsNodeOperational(cart.LArmRemoteCode)
&& IsNodeOperational(cart.RArmRemoteCode);
}
// M层CAN维护：轮询所有驱动节点的在线状态。
private static void SendNodeGuardRequests(Func<byte, ushort, byte[], bool, uint, MCUSerialBridgeError> sendCan)
{
sendCan(0, 0x709, Array.Empty<byte>(), true, 2);
sendCan(0, 0x70A, Array.Empty<byte>(), true, 2);
sendCan(0, 0x701, Array.Empty<byte>(), true, 2);
sendCan(0, 0x702, Array.Empty<byte>(), true, 2);
sendCan(0, 0x703, Array.Empty<byte>(), true, 2);
sendCan(0, 0x704, Array.Empty<byte>(), true, 2);
sendCan(0, 0x705, Array.Empty<byte>(), true, 2);
sendCan(0, 0x706, Array.Empty<byte>(), true, 2);
sendCan(0, 0x707, Array.Empty<byte>(), true, 2);
sendCan(0, 0x708, Array.Empty<byte>(), true, 2);
}
// M层CAN通信：按需注册驱动器反馈报文回调。
private void EnsureCanCallbacksRegistered()
{
if (_canCallbackRegistered)
{
return;
}
if (cart?.Bridge == null)
{
return;
}
var dispatch = BuildCanDispatchTable();
var err0 = cart.Bridge.RegisterCANPortCallback(0, msg =>
{
if (msg == null) return;
if (dispatch.TryGetValue(msg.ID, out var handler))
{
handler(msg);
}
});
_canCallbackRegistered = true;
}
// M层串口通信：按需注册电池等串口设备回调。
private void EnsureSerialCallbacksRegistered()
{
if (_serialCallbackRegistered)
{
return;
}
if (cart?.Bridge == null)
{
return;
}
var err1 = cart.Bridge.RegisterSerialPortCallback(1, msg =>
{
if (msg == null) return;
Hedingben.ToastText($"Callback Serial 1 Callback Received: {BitConverter.ToString(msg)}", "Serial 1");
});
Hedingben.ToastText($"Register Serial 1: {err1}", "Register Serial");
_serialCallbackRegistered = err1 == MCUSerialBridgeError.OK;
}
// M层单车电源：周期发送Modbus电池状态查询。
private void PollBatterySerial(int iteration)
{
if (cart?.Bridge == null) return;
if (iteration % 20 != 0) return;
var writeErr = cart.Bridge.WriteSerial(BatteryPortIndex, BatteryRequest, 60);
if (writeErr != MCUSerialBridgeError.OK)
{
Hedingben.ToastText($"Battery write err: {writeErr}", "Serial 2");
return;
}
var readErr = cart.Bridge.ReadSerial(BatteryPortIndex, out var msg, 40);
if (readErr == MCUSerialBridgeError.NoData) return;
if (readErr != MCUSerialBridgeError.OK)
{
Hedingben.ToastText($"Battery read err: {readErr}", "Serial 2");
return;
}
if (msg == null || msg.Length == 0) return;
Hedingben.ToastText($"Serial 2 RX: {BitConverter.ToString(msg)}", "Serial 2");
TryUpdateBatteryData(msg);
}
// M层单车电源：校验并解析电池Modbus响应。
private void TryUpdateBatteryData(byte[] msg)
{
// Modbus RTU response: [id,03,08,data(8),crc(2)]
if (msg.Length < 13 || msg[0] != 0x01 || msg[1] != 0x03 || msg[2] < 8) return;
ushort calc = ComputeModbusCrc(msg, msg.Length - 2);
ushort recv = (ushort)(msg[msg.Length - 2] | (msg[msg.Length - 1] << 8));
if (calc != recv) return;
cart.Voltage = ReadInt16BE(msg, 3) * 0.1f;
cart.Soc = ReadUInt16BE(msg, 5);
cart.SOH = ReadUInt16BE(msg, 7);
cart.ElectricCurrent = -ReadInt16BE(msg, 9) * 0.1f;
}
// M层单车电源：构造读取电池寄存器的Modbus请求。
private static byte[] BuildBatteryRequest()
{
// 01 03 00 03 00 04 CRC (读取寄存器 03~06)
byte[] req = new byte[] { 0x01, 0x03, 0x00, 0x03, 0x00, 0x04, 0x00, 0x00 };
ushort crc = ComputeModbusCrc(req, 6);
req[6] = (byte)(crc & 0xFF);
req[7] = (byte)((crc >> 8) & 0xFF);
return req;
}
// M层协议辅助：计算Modbus RTU的CRC16校验值。
private static ushort ComputeModbusCrc(byte[] data, int length)
{
ushort crc = 0xFFFF;
for (int i = 0; i < length; i++)
{
crc ^= data[i];
for (int j = 0; j < 8; j++)
{
bool lsb = (crc & 0x0001) != 0;
crc >>= 1;
if (lsb) crc ^= 0xA001;
}
}
return crc;
}
// M层协议辅助：按大端序读取有符号16位数值。
private static short ReadInt16BE(byte[] data, int offset)
{
return (short)((data[offset] << 8) | data[offset + 1]);
}
// M层协议辅助：按大端序读取无符号16位数值。
private static ushort ReadUInt16BE(byte[] data, int offset)
{
return (ushort)((data[offset] << 8) | data[offset + 1]);
}
// M层CAN解析：建立驱动器报文标识符到处理函数的分发表。
private Dictionary<ushort, Action<CANMessage>> BuildCanDispatchTable()
{
return new Dictionary<ushort, Action<CANMessage>>
{
// 速度/位置反馈 0x281~0x28A
[0x281] = (msg) =>
{
//Console.WriteLine("Received 0x281 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
cart.ActualSpeedLeftFrontLeft = ConvertRpm2Mps(rpm);
cart.LFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
},
[0x282] = (msg) =>
{
//Console.WriteLine("Received 0x282 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
cart.ActualSpeedLeftFrontRight = -ConvertRpm2Mps(rpm);
cart.LFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
},
[0x283] = (msg) =>
{
//Console.WriteLine("Received 0x283 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
cart.ActualSpeedRightFrontLeft = ConvertRpm2Mps(rpm);
cart.RFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
},
[0x284] = (msg) =>
{
//Console.WriteLine("Received 0x284 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
cart.ActualSpeedRightFrontRight = -ConvertRpm2Mps(rpm);
cart.RFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
},
[0x285] = (msg) =>
{
//Console.WriteLine("Received 0x285 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
cart.ActualSpeedLeftRearLeft = ConvertRpm2Mps(rpm);
cart.LRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
},
[0x286] = (msg) =>
{
//Console.WriteLine("Received 0x286 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
cart.ActualSpeedLeftRearRight = -ConvertRpm2Mps(rpm);
cart.LRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
},
[0x287] = (msg) =>
{
//Console.WriteLine("Received 0x287 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
cart.ActualSpeedRightRearLeft = ConvertRpm2Mps(rpm);
cart.RRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
},
[0x288] = (msg) =>
{
//Console.WriteLine("Received 0x288 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
cart.ActualSpeedRightRearRight = -ConvertRpm2Mps(rpm);
cart.RRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
},
[0x289] = (msg) =>
{
//Console.WriteLine("Received 0x289 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
cart.ActualSpeedLeftArm = ConvertRpm2Mps(rpm);
cart.ActualPosLeftArm = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
},
[0x28A] = (msg) =>
{
//Console.WriteLine("Received 0x28A CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
cart.ActualSpeedRightArm = ConvertRpm2Mps(rpm);
cart.ActualPosRightArm = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
},
// 状态/错误码 0x181~0x18A
[0x181] = (msg) =>
{
//Console.WriteLine("Received 0x181 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 4) return;
cart.LeftFrontLeftStateCode = BitConverter.ToInt16(payload, 0);
cart.LeftFrontLeftErrorCode = BitConverter.ToInt16(payload, 2);
cart.LeftFrontLeftElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
},
[0x182] = (msg) =>
{
//Console.WriteLine("Received 0x182 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 4) return;
cart.LeftFrontRightStateCode = BitConverter.ToInt16(payload, 0);
cart.LeftFrontRightErrorCode = BitConverter.ToInt16(payload, 2);
cart.LeftFrontRightElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
},
[0x183] = (msg) =>
{
//Console.WriteLine("Received 0x183 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 4) return;
cart.RightFrontLeftStateCode = BitConverter.ToInt16(payload, 0);
cart.RightFrontLeftErrorCode = BitConverter.ToInt16(payload, 2);
cart.RightFrontLeftElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
},
[0x184] = (msg) =>
{
//Console.WriteLine("Received 0x184 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 4) return;
cart.RightFrontRightStateCode = BitConverter.ToInt16(payload, 0);
cart.RightFrontRightErrorCode = BitConverter.ToInt16(payload, 2);
cart.RightFrontRightElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
},
[0x185] = (msg) =>
{
//Console.WriteLine("Received 0x185 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 4) return;
cart.LeftRearLeftStateCode = BitConverter.ToInt16(payload, 0);
cart.LeftRearLeftErrorCode = BitConverter.ToInt16(payload, 2);
cart.LeftRearLeftElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
},
[0x186] = (msg) =>
{
//Console.WriteLine("Received 0x186 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 4) return;
cart.LeftRearRightStateCode = BitConverter.ToInt16(payload, 0);
cart.LeftRearRightErrorCode = BitConverter.ToInt16(payload, 2);
cart.LeftRearRightElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
},
[0x187] = (msg) =>
{
//Console.WriteLine("Received 0x187 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 4) return;
cart.RightRearLeftStateCode = BitConverter.ToInt16(payload, 0);
cart.RightRearLeftErrorCode = BitConverter.ToInt16(payload, 2);
cart.RightRearLeftElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
},
[0x188] = (msg) =>
{
//Console.WriteLine("Received 0x188 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 4) return;
cart.RightRearRightStateCode = BitConverter.ToInt16(payload, 0);
cart.RightRearRightErrorCode = BitConverter.ToInt16(payload, 2);
cart.RightRearRightElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
},
[0x189] = (msg) =>
{
//Console.WriteLine("Received 0x189 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 4) return;
cart.LeftArmStateCode = BitConverter.ToInt16(payload, 0);
cart.LeftArmErrorCode = BitConverter.ToInt16(payload, 2);
cart.LeftArmElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
},
[0x18A] = (msg) =>
{
//Console.WriteLine("Received 0x18A CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 4) return;
cart.RightArmStateCode = BitConverter.ToInt16(payload, 0);
cart.RightArmErrorCode = BitConverter.ToInt16(payload, 2);
cart.RightArmElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
},
// 舵角 0x18B~0x18E
[0x18B] = (msg) =>
{
//Console.WriteLine("Received 0x18B CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 4) return;
cart.ActualThLeftFront = BitConverter.ToInt32(payload, 0);
cart.ActualThLeftFront = cart.ActualThLeftFront >= 16384
? (cart.ActualThLeftFront - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftFront
: cart.ActualThLeftFront / 4096f / 5f * 360 - cart.ThBiasLeftFront;
},
[0x18C] = (msg) =>
{
var payload = msg.Payload;
if (payload == null || payload.Length < 4) return;
var raw = BitConverter.ToInt32(payload, 0);
cart.ActualThRightFront = raw >= 16384
? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasRightFront
: raw / 4096f / 5f * 360 - cart.ThBiasRightFront;
//DLog.Log($"RF raw=0x{raw:X8}({raw}) angle={cart.ActualThRightFront:F2}", "0x18C");
},
[0x18D] = (msg) =>
{
//Console.WriteLine($"Received 0x18D CAN Message {DateTime.Now:yyyy-MM-dd HH:mm:ss.ffffff}");
var payload = msg.Payload;
if (payload == null || payload.Length < 4) return;
cart.ActualThLeftRear = BitConverter.ToInt32(payload, 0);
cart.ActualThLeftRear = cart.ActualThLeftRear >= 16384
? (cart.ActualThLeftRear - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftRear
: cart.ActualThLeftRear / 4096f / 5f * 360 - cart.ThBiasLeftRear;
},
[0x18E] = (msg) =>
{
//Console.WriteLine("Received 0x18E CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 4) return;
cart.ActualThRightRear = BitConverter.ToInt32(payload, 0);
cart.ActualThRightRear = cart.ActualThRightRear >= 16384
? (cart.ActualThRightRear - 98303) / 4096f / 5f * 360 - cart.ThBiasRightRear
: cart.ActualThRightRear / 4096f / 5f * 360 - cart.ThBiasRightRear;
},
// 远程帧
[0x701] = (msg) =>
{
//Console.WriteLine("Received 0x701 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 1) return;
cart.LFLRemoteCode = payload[0];
},
[0x702] = (msg) =>
{
//Console.WriteLine("Received 0x702 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 1) return;
cart.LFRRemoteCode = payload[0];
},
[0x703] = (msg) =>
{
//Console.WriteLine("Received 0x703 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 1) return;
cart.RFLRemoteCode = payload[0];
},
[0x704] = (msg) =>
{
//Console.WriteLine("Received 0x704 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 1) return;
cart.RFRRemoteCode = payload[0];
},
[0x705] = (msg) =>
{
//Console.WriteLine("Received 0x705 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 1) return;
cart.LRLRemoteCode = payload[0];
},
[0x706] = (msg) =>
{
//Console.WriteLine("Received 0x706 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 1) return;
cart.LRRRemoteCode = payload[0];
},
[0x707] = (msg) =>
{
//Console.WriteLine("Received 0x707 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 1) return;
cart.RRLRemoteCode = payload[0];
},
[0x708] = (msg) =>
{
//Console.WriteLine("Received 0x708 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 1) return;
cart.RRRRemoteCode = payload[0];
},
[0x709] = (msg) =>
{
//Console.WriteLine("Received 0x709 CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 1) return;
cart.LArmRemoteCode = payload[0];
},
[0x70A] = (msg) =>
{
//Console.WriteLine("Received 0x70A CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 1) return;
cart.RArmRemoteCode = payload[0];
},
};
}
}
}
MedullaAdapter/MotorRoutine.cs
+331 -1
View File
@@ -1 +1,331 @@
// 计算8个驱动电机的目标速度和舵角PID
// 计算8个驱动电机的目标速度和舵角PID
using CartActivator;
using CommonUsage.Mathematics;
using FundamentalLib;
using MDCSToolBox.Commons;
using MDCSToolBox.Commons.Controllers;
using System;
using System.Collections.Generic;
using System.Linq;
using static MDCSToolBox.Medulla.Chassis.BasicCartDefinition;
namespace MedullaAdapter
{
public class MotorRoutine : LadderLogic<DiverCartDefinition>
{
private bool _wasTransmitterControlling;
private DateTime _lastMoveTime = DateTime.Now;
public override void Operation(int iteration)
{
if (!cart.GhostMode && cart.State == -1) return;
// SA稳定打开后，使能实体遥控器。
TriggerOnce(
cart.TransmitterConnected && cart.Transmitter_SA,
300,
() =>
{
cart.TransmitterControlEnable = true;
cart.TransmitterLastTime = DateTime.Now;
});
// 遥控器断连或SA关闭时，立即撤销遥控使能。
if (!cart.TransmitterConnected || !cart.Transmitter_SA)
cart.TransmitterControlEnable = false;
var transmitterSelected =
cart.TransmitterConnected &&
cart.TransmitterControlEnable &&
cart.Transmitter_SA &&
cart.Transmitter_SC == cart.CarNum;
var transmitterControlling =
transmitterSelected &&
CartDefinition.testPriority(5, "TransmitterMode");
if (transmitterControlling)
{
TransmitterChassisControl();
cart.TransmitterLastTime = DateTime.Now;
cart.CarStatu = "实体遥控器控制";
}
else
{
// 只在遥控器刚刚退出时发送一次停车，
// 不能每周期停车，否则会覆盖C层轨迹控制。
if (_wasTransmitterControlling)
{
cart.ManualControl(
cart.TransmitterControlMode,
0, 0, 0,
cart.TransmitterSpeed,
DateTime.Now - cart.TransmitterLastTime);
cart.TransmitterLastTime = DateTime.Now;
}
cart.CarStatu = "正常运行";
}
_wasTransmitterControlling = transmitterControlling;
// 当前是否由C层控制。
cart.ClumsyControl = CartDefinition.currentPriority == 0;
// 计算四个舵轮PID和8个驱动电机最终速度。
UpdateDiffSteerWheelSpeeds();
// 平滑更新硬件速度限制。
UpdateSendSpeedLimit();
// 更新红黄绿灯状态。
UpdateLightMode();
}
// 物理遥控器设置
public void TransmitterChassisControl()
{
var interval = DateTime.Now - cart.TransmitterLastTime;
switch (cart.Transmitter_SB)
{
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);
return;
}
// SA关闭后立即停车。
if (!cart.Transmitter_SA)
{
cart.ManualControl(
cart.TransmitterControlMode,
0, 0, 0,
cart.TransmitterSpeed,
interval);
return;
}
// 限制实体遥控器的最大速度。
cart.TransmitterSpeed = Math.Max(
cart.TransmitterSpeedLowerLimit,
Math.Min(
cart.TransmitterSpeed,
cart.TransmitterSpeedUpperLimit));
// SD的Mode0作为底盘驾驶档。
if (cart.Transmitter_SD == TransmitterState.Mode0)
{
cart.ManualControl(
cart.TransmitterControlMode,
cart.TransmitterLeftJoystickValX,
cart.TransmitterRightJoystickValY,
0,
cart.TransmitterSpeed,
interval);
return;
}
// 非驾驶档必须主动停车，防止上一条运动指令残留。
cart.ManualControl(
cart.TransmitterControlMode,
0, 0, 0,
cart.TransmitterSpeed,
interval);
}
// M层单车底盘：根据四个舵轮的目标角度和实际角度修正8个驱动电机速度。
private void UpdateDiffSteerWheelSpeeds()
{
if (cart.LeftFrontPid == null ||
cart.LeftRearPid == null ||
cart.RightFrontPid == null ||
cart.RightRearPid == null)
{
cart.SpeedLFL = 0;
cart.SpeedLFR = 0;
cart.SpeedRFL = 0;
cart.SpeedRFR = 0;
cart.SpeedLRL = 0;
cart.SpeedLRR = 0;
cart.SpeedRRL = 0;
cart.SpeedRRR = 0;
return;
}
// 更新左前舵轮PID参数。
cart.LeftFrontPid.ChangeParameters(
cart.DiffSteerKp,
cart.DiffSteerKi,
cart.DiffSteerKd,
cart.DiffSteerMaxI,
cart.DiffSteerDeadZone,
cart.DiffSteerThresh,
cart.DiffSteerSpeedAcc);
// 更新左后舵轮PID参数。
cart.LeftRearPid.ChangeParameters(
cart.DiffSteerKp,
cart.DiffSteerKi,
cart.DiffSteerKd,
cart.DiffSteerMaxI,
cart.DiffSteerDeadZone,
cart.DiffSteerThresh,
cart.DiffSteerSpeedAcc);
// 更新右前舵轮PID参数。
cart.RightFrontPid.ChangeParameters(
cart.DiffSteerKp,
cart.DiffSteerKi,
cart.DiffSteerKd,
cart.DiffSteerMaxI,
cart.DiffSteerDeadZone,
cart.DiffSteerThresh,
cart.DiffSteerSpeedAcc);
// 更新右后舵轮PID参数。
cart.RightRearPid.ChangeParameters(
cart.DiffSteerKp,
cart.DiffSteerKi,
cart.DiffSteerKd,
cart.DiffSteerMaxI,
cart.DiffSteerDeadZone,
cart.DiffSteerThresh,
cart.DiffSteerSpeedAcc);
// 根据实际舵角计算四条腿的差速修正量。
var diffLf = cart.LeftFrontPid.GetResponse(
cart.ThLeftFront, false, false, "LF");
var diffLr = cart.LeftRearPid.GetResponse(
cart.ThLeftRear, false, false, "LR");
var diffRf = cart.RightFrontPid.GetResponse(
cart.ThRightFront, false, false, "RF");
var diffRr = cart.RightRearPid.GetResponse(
cart.ThRightRear, false, false, "RR");
// 左前腿：左右电机施加方向相反的PID修正量。
cart.SpeedLFL = cart.SpeedLeftFrontLeft - diffLf;
cart.SpeedLFR = cart.SpeedLeftFrontRight + diffLf;
// 左后腿。
cart.SpeedLRL = cart.SpeedLeftRearLeft - diffLr;
cart.SpeedLRR = cart.SpeedLeftRearRight + diffLr;
// 右前腿。
cart.SpeedRFL = cart.SpeedRightFrontLeft - diffRf;
cart.SpeedRFR = cart.SpeedRightFrontRight + diffRf;
// 右后腿。
cart.SpeedRRL = cart.SpeedRightRearLeft - diffRr;
cart.SpeedRRR = cart.SpeedRightRearRight + diffRr;
}
// M层单车限速：按照加速度和减速度平滑更新实际下发速度上限。
private void UpdateSendSpeedLimit()
{
if (cart.Chassis == null)
{
cart.SendThresSpeed = 0;
return;
}
var now = DateTime.Now;
var elapsedSeconds = (float)(now - _lastMoveTime).TotalSeconds;
_lastMoveTime = now;
// 防止调试暂停或线程卡顿后，一次产生过大的速度跳变。
elapsedSeconds = Math.Clamp(elapsedSeconds, 0f, 0.2f);
// 限速值不允许小于零。
var targetLimit = Math.Max(0f, cart.ThresSpeed);
var currentLimit = Math.Max(0f, cart.SendThresSpeed);
// 增大速度上限时用加速度，减小时用减速度。
var speedChangingRate =
targetLimit > currentLimit
? cart.Chassis.AccPerSecond
: cart.Chassis.DeAccPerSecond;
speedChangingRate = Math.Max(0f, speedChangingRate);
var maxChange = speedChangingRate * elapsedSeconds;
var speedDifference = targetLimit - currentLimit;
if (Math.Abs(speedDifference) <= maxChange)
{
currentLimit = targetLimit;
}
else
{
currentLimit += Math.Sign(speedDifference) * maxChange;
}
// 计算当前8个电机目标速度中的最大绝对值。
var wheelMaxSpeed = 0f;
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLFL));
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLFR));
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRFL));
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRFR));
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLRL));
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLRR));
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRRL));
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRRR));
// 没有必要让平滑限速值高于当前所有车轮需要的速度。
if (targetLimit < wheelMaxSpeed &&
currentLimit > wheelMaxSpeed)
{
currentLimit = wheelMaxSpeed;
}
cart.SendThresSpeed = currentLimit;
}
// M层单车灯光：根据报警、驱动器、限速和电量状态生成红黄绿灯模式。
private void UpdateLightMode()
{
// 二级报警：红灯常亮。
if (cart.AlarmLevel == 2)
{
cart.LightMode = 2;
return;
}
// 一级报警：黄灯常亮。
if (cart.AlarmLevel == 1)
{
cart.LightMode = 3;
return;
}
// 驱动轮未使能：黄灯闪烁。
if (!cart.WheelAbleState)
{
FlipFlop(ref cart.LightMode, 500, 0, 3);
return;
}
// C层正在进行限速：黄灯常亮。
if (Math.Abs(cart.ThresSpeed - 1f) > 0.001f)
{
cart.LightMode = 3;
return;
}
// 电量低于预警值：黄灯常亮。
if (cart.Soc <= cart.LowBatteryAlarmThreshold)
{
cart.LightMode = 3;
return;
}
// 正常运行：绿灯闪烁。
FlipFlop(ref cart.LightMode, 500, 0, 1);
}
}
}
MedullaAdapter/Remote.cs
+15 -1
View File
@@ -1 +1,15 @@
// Medulla虚拟遥控器和夹臂控制
// Medulla虚拟遥控器和夹臂控制
using MDCSToolBox.Medulla.Chassis.MultiWheel;
namespace MedullaAdapter
{
// M层单车虚拟遥控器：使用父类提供的底盘控制界面。
public class Remote : MultiWheelRemote<DiverCartDefinition>
{
// 单车不支持多车联动，误打开多车开关时主动停车。
public override void MultiVehicleModeChassisLogic()
{
cart.Chassis?.PredefinedDriveStop();
}
}
}