update
This commit is contained in:
shuai.li
@@ -0,0 +1,278 @@
|
||||
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<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 {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<DiverCartDefinition>.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
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user