179 lines
7.6 KiB
C#
179 lines
7.6 KiB
C#
// 定义车型、上下层IO、参数和MCU初始化
|
|
using CartActivator;
|
|
using MCUSerialBridgeCLR;
|
|
using MDCSToolBox.Medulla.Chassis.MultiWheel;
|
|
using Medulla.Types;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Threading;
|
|
|
|
namespace MedullaAdapter
|
|
{
|
|
public class DiverCartDefinition : MultiWheelCartDefinition
|
|
{
|
|
#region 基本成员
|
|
public MCUSerialBridge Bridge;
|
|
|
|
internal enum ManualControlMode
|
|
{
|
|
Normal = 0, // 正常模式
|
|
Spin = 1, // 自旋模式
|
|
Crab = 2, // 螃蟹模式
|
|
}
|
|
internal ManualControlMode TransmitterControlMode = ManualControlMode.Normal;
|
|
internal DateTime TransmitterLastTime; // 物理遥控器计算两次实体遥控器指令之间的时间间隔
|
|
#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
|
|
|
|
#region AsLowerIO
|
|
[AsLowerIO(desc = "左前左轮实际位置")] public float LFLActualPos;
|
|
[AsLowerIO(desc = "左前右轮实际位置")] public float LFRActualPos;
|
|
[AsLowerIO(desc = "右前左轮实际位置")] public float RFLActualPos;
|
|
[AsLowerIO(desc = "右前右轮实际位置")] public float RFRActualPos;
|
|
[AsLowerIO(desc = "左后左轮实际位置")] public float LRLActualPos;
|
|
[AsLowerIO(desc = "左后右轮实际位置")] public float LRRActualPos;
|
|
[AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
|
|
[AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
|
|
[AsLowerIO(desc = "驱动轮使能状态")] public bool WheelAbleState = true;
|
|
[AsLowerIO(desc = "电池健康状态")] public float SOH;
|
|
[AsInitParam(desc = "车号")][AsLowerIO] public int CarNum = 1;
|
|
#endregion
|
|
|
|
#region 初始参数
|
|
[AsInitParam(desc = "MCU端口号")] public string MCUPort = "COM4";
|
|
[AsInitParam(desc = "遥控器速度上限")] public float TransmitterSpeedUpperLimit = 1.0f;
|
|
[AsInitParam(desc = "遥控器速度下限")] public float TransmitterSpeedLowerLimit = 0.0f;
|
|
|
|
#endregion
|
|
|
|
#region 监控参数
|
|
[IOObjectMonitor(desc = "左前左轮PID修正后速度")] public float SpeedLFL;
|
|
[IOObjectMonitor(desc = "左前右轮PID修正后速度")] public float SpeedLFR;
|
|
[IOObjectMonitor(desc = "右前左轮PID修正后速度")] public float SpeedRFL;
|
|
[IOObjectMonitor(desc = "右前右轮PID修正后速度")] public float SpeedRFR;
|
|
[IOObjectMonitor(desc = "左后左轮PID修正后速度")] public float SpeedLRL;
|
|
[IOObjectMonitor(desc = "左后右轮PID修正后速度")] public float SpeedLRR;
|
|
[IOObjectMonitor(desc = "右后左轮PID修正后速度")] public float SpeedRRL;
|
|
[IOObjectMonitor(desc = "右后右轮PID修正后速度")] public float SpeedRRR;
|
|
[IOObjectMonitor(desc = "灯光模式")] public int LightMode = 0;
|
|
[IOObjectMonitor(desc = "实体遥控器当前速度倍率")] public float TransmitterSpeed = 0.3f;
|
|
[IOObjectMonitor(desc = "左前左驱动器远程帧701")] public byte LFLRemoteCode = 0;
|
|
[IOObjectMonitor(desc = "左前右驱动器远程帧702")] public byte LFRRemoteCode = 0;
|
|
[IOObjectMonitor(desc = "右前左驱动器远程帧703")] public byte RFLRemoteCode = 0;
|
|
[IOObjectMonitor(desc = "右前右驱动器远程帧704")] public byte RFRRemoteCode = 0;
|
|
[IOObjectMonitor(desc = "左后左驱动器远程帧705")] public byte LRLRemoteCode = 0;
|
|
[IOObjectMonitor(desc = "左后右驱动器远程帧706")] public byte LRRRemoteCode = 0;
|
|
[IOObjectMonitor(desc = "右后左驱动器远程帧707")] public byte RRLRemoteCode = 0;
|
|
[IOObjectMonitor(desc = "右后右驱动器远程帧708")] public byte RRRRemoteCode = 0;
|
|
#endregion
|
|
|
|
#region 操作按钮
|
|
// M层单车硬件：向驱动轮发送复位请求。
|
|
[IOObjectUtility]
|
|
public void WheelReset()
|
|
{
|
|
ResetFromM = true;
|
|
}
|
|
// M层单车硬件：向驱动轮发送下使能请求。
|
|
[IOObjectUtility]
|
|
public void WheelDisable()
|
|
{
|
|
DisableFromM = true;
|
|
}
|
|
#endregion
|
|
|
|
public override void CommunicationInit()
|
|
{
|
|
if (GhostMode) return;
|
|
State = -1;
|
|
Bridge = new MCUSerialBridge();
|
|
//Step1:打开指定串口连接
|
|
var err = Bridge.Open(MCUPort, 1000000u);
|
|
if (err != MCUSerialBridgeError.OK)
|
|
{
|
|
Console.WriteLine($"MCU Open FAILED: {err.ToDescription()}");
|
|
return;
|
|
}
|
|
else
|
|
{
|
|
Console.WriteLine("MCU Open OK");
|
|
}
|
|
//Step2:远程复位MCU
|
|
err = Bridge.Reset();
|
|
if (err != MCUSerialBridgeError.OK)
|
|
{
|
|
Console.WriteLine($"MCU Reset FAILED: {err.ToDescription()}");
|
|
return;
|
|
}
|
|
else
|
|
{
|
|
Thread.Sleep(500);
|
|
Console.WriteLine("MCU Reset OK");
|
|
}
|
|
//Step3:获取MCU版本号
|
|
err = Bridge.GetVersion(out var version, 100);
|
|
if (err != MCUSerialBridgeError.OK)
|
|
{
|
|
Console.WriteLine($"MCU GetVersion FAILED: {err.ToDescription()}");
|
|
return;
|
|
}
|
|
else
|
|
{
|
|
Console.WriteLine($"MCU GetVersion OK: {version}");
|
|
}
|
|
//Step4:获取MCU状态
|
|
err = Bridge.GetState(out var state, 100);
|
|
if (err != MCUSerialBridgeError.OK)
|
|
{
|
|
Console.WriteLine($"MCU GetState FAILED: {err.ToDescription()}");
|
|
return;
|
|
}
|
|
else
|
|
{
|
|
Console.WriteLine($"MCU GetState OK: {state}");
|
|
}
|
|
//Step5:串口/CAN配置
|
|
try
|
|
{
|
|
var ports = new List<PortConfig>();
|
|
for (int i = 0; i < 1; i++)
|
|
ports.Add(new CANPortConfig(500000, 10));
|
|
for (int i = 0; i < 3; i++)
|
|
ports.Add(new SerialPortConfig(9600, 10));
|
|
Console.WriteLine("=== Port Configuration ===");
|
|
for (int i = 0; i < ports.Count; i++)
|
|
{
|
|
if (ports[i] is SerialPortConfig s)
|
|
Console.WriteLine($"Port {i}: Serial, Baud={s.Baud}, ReceiveFrameMs={s.ReceiveFrameMs}");
|
|
else if (ports[i] is CANPortConfig c)
|
|
Console.WriteLine($"Port {i}: CAN, Baud={c.Baud}, RetryTimeMs={c.RetryTimeMs}");
|
|
}
|
|
|
|
var ret = Bridge.Configure(ports, 200);
|
|
if (ret != MCUSerialBridgeError.OK)
|
|
{
|
|
Console.WriteLine($"MCU Configure FAILED: {ret.ToDescription()}");
|
|
return;
|
|
}
|
|
else
|
|
{
|
|
Console.WriteLine("MCU Configure OK");
|
|
}
|
|
Console.WriteLine("MCU Configure {0}", ret == MCUSerialBridgeError.OK ? "OK" : $"FAILED: 0x{(uint)ret:X8}");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Console.WriteLine($"Configure Exception: {ex.Message}");
|
|
return;
|
|
}
|
|
State = 0;
|
|
}
|
|
|
|
}
|
|
} |