diff --git a/MedullaAdapter/AlarmRoutine.cs b/MedullaAdapter/AlarmRoutine.cs index 5450f59..3706845 100644 --- a/MedullaAdapter/AlarmRoutine.cs +++ b/MedullaAdapter/AlarmRoutine.cs @@ -1 +1,14 @@ -// 驱动器、急停、夹臂等安全报警 \ No newline at end of file +// 驱动器、急停、夹臂等安全报警 +using MDCSToolBox.Medulla.Chassis.MultiWheel; + +namespace MedullaAdapter +{ + public class AlarmRoutine : MultiWheelAlarmRoutine + { + // M层单车安全：汇总停车机器人夹臂等自定义报警状态。 + public override void SetOtherAlarms() + { + // 当前阶段不使用夹臂，暂时没有自定义报警。 + } + } +} \ No newline at end of file diff --git a/MedullaAdapter/DiverCartDefinition.cs b/MedullaAdapter/DiverCartDefinition.cs index bd92bad..b427d3d 100644 --- a/MedullaAdapter/DiverCartDefinition.cs +++ b/MedullaAdapter/DiverCartDefinition.cs @@ -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 + } -} \ No newline at end of file +} diff --git a/MedullaAdapter/MCURoutine.cs b/MedullaAdapter/MCURoutine.cs index decc969..ab9171f 100644 --- a/MedullaAdapter/MCURoutine.cs +++ b/MedullaAdapter/MCURoutine.cs @@ -1 +1,897 @@ -// 实际CAN协议、反馈解析、IO、电池、急停 \ No newline at end of file +// 实际CAN协议、反馈解析、IO、电池、急停 +using CartActivator; +using FundamentalLib; +using MCUSerialBridgeCLR; +using System; +using System.Collections.Generic; + +namespace MedullaAdapter +{ + public class MCURoutine : LadderLogic + { + 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 sendCan) + { + sendCan(0, 0x709, Array.Empty(), true, 2); + sendCan(0, 0x70A, Array.Empty(), true, 2); + sendCan(0, 0x701, Array.Empty(), true, 2); + sendCan(0, 0x702, Array.Empty(), true, 2); + sendCan(0, 0x703, Array.Empty(), true, 2); + sendCan(0, 0x704, Array.Empty(), true, 2); + sendCan(0, 0x705, Array.Empty(), true, 2); + sendCan(0, 0x706, Array.Empty(), true, 2); + sendCan(0, 0x707, Array.Empty(), true, 2); + sendCan(0, 0x708, Array.Empty(), 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> BuildCanDispatchTable() + { + return new Dictionary> + { + // 速度/位置反馈 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]; + }, + }; + } + + } +} diff --git a/MedullaAdapter/MotorRoutine.cs b/MedullaAdapter/MotorRoutine.cs index b2f8794..9725a05 100644 --- a/MedullaAdapter/MotorRoutine.cs +++ b/MedullaAdapter/MotorRoutine.cs @@ -1 +1,331 @@ -// 计算8个驱动电机的目标速度和舵角PID \ No newline at end of file +// 计算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 + { + 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); + } + + } +} \ No newline at end of file diff --git a/MedullaAdapter/Remote.cs b/MedullaAdapter/Remote.cs index f2fece5..842d426 100644 --- a/MedullaAdapter/Remote.cs +++ b/MedullaAdapter/Remote.cs @@ -1 +1,15 @@ -// Medulla虚拟遥控器和夹臂控制 \ No newline at end of file +// Medulla虚拟遥控器和夹臂控制 +using MDCSToolBox.Medulla.Chassis.MultiWheel; + +namespace MedullaAdapter +{ + // M层单车虚拟遥控器：使用父类提供的底盘控制界面。 + public class Remote : MultiWheelRemote + { + // 单车不支持多车联动，误打开多车开关时主动停车。 + public override void MultiVehicleModeChassisLogic() + { + cart.Chassis?.PredefinedDriveStop(); + } + } +} \ No newline at end of file