using CartActivator; using MCUSerialBridgeCLR; using MDCSToolBox.Medulla.Chassis.MultiWheel; using Medulla.Types; using System; using System.Collections.Generic; using System.Runtime.InteropServices; using System.Text; 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 partial class DiverCartDefinition : MultiWheelCartDefinition { public DiverCommunication Embedded; public MCUSerialBridge Bridge; [AsUpperIO(desc = "转弯半径")] public float Radius; #region MultiVehicleCoordination [AsInitParam(desc = "遥控器最大线速度")] public float MaxManualSpeed = 0.3f; [AsInitParam(desc = "遥控器最大角速度")] public float MaxManualAngularSpeed = 45f; [AsLowerIO(desc = "启用（手动）多车联动")] public bool MultiVehicleManualEnabled = false; [AsLowerIO(desc = "（手动）多车联动模式")] public int MultiVehicleManualMode = 0; [AsUpperIO(desc = "多车联动：声光同步，-1未启动，0灭，1亮")] public int MultiVehicleLightSync = -1; [AsLowerIO(desc = "多车联动模式-暂停")] public bool MultiVehicleHold = false; [AsLowerIO(desc = "多车联动：遥控器Vx")] public float MultiVehicleManualVx = 0f; [AsLowerIO(desc = "多车联动：蟹行方向比例")] public float MultiVehicleManualVy = 0f; [AsLowerIO(desc = "多车联动：遥控器Vth")] public float MultiVehicleManualVth = 0f; #endregion [IOObjectMonitor(desc = "左前左轮下发速度")] public float SpeedLFL; [IOObjectMonitor(desc = "左前右轮下发速度")] public float SpeedLFR; [IOObjectMonitor(desc = "右前左轮下发速度")] public float SpeedRFL; [IOObjectMonitor(desc = "右前右轮下发速度")] public float SpeedRFR; [IOObjectMonitor(desc = "左后左轮下发速度")] public float SpeedLRL; [IOObjectMonitor(desc = "左后右轮下发速度")] public float SpeedLRR; [IOObjectMonitor(desc = "右后左轮下发速度")] public float SpeedRRL; [IOObjectMonitor(desc = "右后右轮下发速度")] public float SpeedRRR; //[AsUpperIO(desc = "左前左轮下发速度", timeOutReset = true)] public float SpeedLFL; //[AsUpperIO(desc = "左前右轮下发速度", timeOutReset = true)] public float SpeedLFR; //[AsUpperIO(desc = "右前左轮下发速度", timeOutReset = true)] public float SpeedRFL; //[AsUpperIO(desc = "右前右轮下发速度", timeOutReset = true)] public float SpeedRFR; //[AsUpperIO(desc = "左后左轮下发速度", timeOutReset = true)] public float SpeedLRL; //[AsUpperIO(desc = "左后右轮下发速度", timeOutReset = true)] public float SpeedLRR; //[AsUpperIO(desc = "右后左轮下发速度", timeOutReset = true)] public float SpeedRRL; //[AsUpperIO(desc = "右后右轮下发速度", timeOutReset = true)] public float SpeedRRR; [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; [AsUpperIO(desc = "左夹臂下发速度", timeOutReset = true)] public float SpeedLeftArm; [AsUpperIO(desc = "右夹臂下发速度", timeOutReset = true)] public float SpeedRightArm; [AsUpperIO(desc = "左夹臂实际速度")] public float ActualSpeedLeftArm; [AsUpperIO(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 = "test")] public float test; [AsLowerIO(desc = "test1")] public byte test1; [AsLowerIO(desc = "test2")] public byte test2; [AsInitParam(desc = "test3")] public int test3=24; [IOObjectMonitor(desc = "灯光模式")] public int LightMode = 0; [AsInitParam(desc = "触发模式")] public int trigger = 0; [AsInitParam(desc = "手动控制夹臂速度系数")] public float ManualArmSpeedFac = 1.0f; [AsInitParam(desc = "车号")][AsLowerIO] public int CarNum = 1; [AsInitParam(desc = "初始音量")] public int MusicVolume = 5; [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; [AsInitParam(desc = "MCU端口号")] public string MCUPort = "COM4"; [AsLowerIO(desc = "陀螺仪角度")] public float GyrosTh; [AsLowerIO(desc = "电池健康状态")] public float SOH; [AsLowerIO(desc = "左前左驱动器远程帧701")] public byte LFLRemoteCode = 0; [AsLowerIO(desc = "左前右驱动器远程帧702")] public byte LFRRemoteCode = 0; [AsLowerIO(desc = "右前左驱动器远程帧703")] public byte RFLRemoteCode = 0; [AsLowerIO(desc = "右前右驱动器远程帧704")] public byte RFRRemoteCode = 0; [AsLowerIO(desc = "左后左驱动器远程帧705")] public byte LRLRemoteCode = 0; [AsLowerIO(desc = "左后右驱动器远程帧706")] public byte LRRRemoteCode = 0; [AsLowerIO(desc = "右后左驱动器远程帧707")] public byte RRLRemoteCode = 0; [AsLowerIO(desc = "右后右驱动器远程帧708")] public byte RRRRemoteCode = 0; [AsLowerIO(desc = "左夹臂驱动器远程帧709")] public byte LArmRemoteCode = 0; [AsLowerIO(desc = "右夹臂驱动器远程帧70A")] public byte RArmRemoteCode = 0; [AsUpperIO(desc = "从M上复位")] public bool ResetFromM; [AsUpperIO(desc = "从M将驱动轮下使能")] public bool DisableFromM; [AsUpperIO(desc = "从C上复位")] public bool ResetFromC; [AsUpperIO(desc = "从C将驱动轮下使能")] public bool DisableFromC; [AsUpperIO(desc = "夹臂不同步报警")] public bool ClampOutOfSync; internal ManualControlMode TransmitterControlMode = ManualControlMode.Normal; [IOObjectMonitor] public float TransmitterSpeed = 0.3f; [AsLowerIO(desc = "驱动轮使能状态")] public bool WheelAbleState = true; [AsInitParam(desc = "遥控器速度上限")] public float TransmitterSpeedUpperLimit = 1.0f; [AsInitParam(desc = "遥控器速度下限")] public float TransmitterSpeedLowerLimit = 0.0f; internal DateTime TransmitterLastTime; [IOObjectUtility] public void WheelReset() { ResetFromM = true; } [IOObjectUtility] public void WheelDisable() { DisableFromM = true; } public override void CommunicationInit() { if (GhostMode) return; Bridge = new MCUSerialBridge(); //Step1:打开指定串口连接 var err = Bridge.Open(MCUPort, 1000000u); if (err != MCUSerialBridgeError.OK) { Console.WriteLine("MCU Open FAILED: {0}", err.ToDescription()); return; } else { Console.WriteLine("MCU Open OK"); } //Step2:远程复位MCU err = Bridge.Reset(); if (err != MCUSerialBridgeError.OK) { Console.WriteLine("MCU Reset FAILED: {0}", err.ToDescription()); return; } else { Thread.Sleep(500); Console.WriteLine("MCU Reset OK"); } //Step3:GetVersion err = Bridge.GetVersion(out var version, 100); if (err != MCUSerialBridgeError.OK) { Console.WriteLine("MCU GetVersion FAILED: {0}", err.ToDescription()); return; } else { Console.WriteLine("MCU GetVersion OK: {0}", version.ToString()); } //Step4:GetState err = Bridge.GetState(out var state, 100); if (err != MCUSerialBridgeError.OK) { Console.WriteLine("MCU GetState FAILED: {0}", err.ToDescription()); return; } else { Console.WriteLine("MCU GetState OK: {0}", state.ToString()); } //Step5:串口/CAN配置 try { var ports = new List(); 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 {0}: Serial, Baud={1}, ReceiveFrameMs={2}", i, s.Baud, s.ReceiveFrameMs); else if (ports[i] is CANPortConfig c) Console.WriteLine("Port {0}: CAN, Baud={1}, RetryTimeMs={2}", i, c.Baud, c.RetryTimeMs); } var ret = Bridge.Configure(ports, 200); if (ret != MCUSerialBridgeError.OK) { Console.WriteLine("MCU Configure FAILED: {0}", 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: {0}", ex.Message); } //MCUInterface.Start(this); 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; switch (mode) { case ManualControlMode.Normal: ManualMode = 0; Chassis.DirectionAngle = frontDirection; Chassis.SendMotion(speed, frontTh, rearTh, interval); break; case ManualControlMode.Sway: ManualMode = 1; Chassis.DirectionAngle = frontDirection; Chassis.SendMotion(speed, frontTh, -rearTh, interval); break; case ManualControlMode.Crab: ManualMode = 2; Chassis.DirectionAngle = 90; Chassis.SendMotion(speed, frontTh, rearTh, interval); break; case ManualControlMode.Spin: ManualMode = 3; Chassis.SendRotateMotion(speed * MaxAngularSpeed, interval); break; } } internal enum ManualControlMode { Normal = 0, Spin = 1, Crab = 2, Sway = 3 } } }