880 lines
39 KiB
C#
880 lines
39 KiB
C#
using CartActivator;
|
|
using FundamentalLib;
|
|
using MCUSerialBridgeCLR;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
|
|
namespace MedullaAdapter
|
|
{
|
|
//[LogicRunOnMCU(scanInterval = 20)]
|
|
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();
|
|
|
|
private static float ConvertMps2Rpm(float mps)
|
|
{
|
|
return (float)(mps / (Math.PI * 85f) * 10.5f * 60f * 1000f);
|
|
}
|
|
private static float ConvertRpm2Mps(float rpm)
|
|
{
|
|
return (float)(rpm / 10.5f / 60f * Math.PI * 85f / 1000f);
|
|
}
|
|
private float ConvertR2MM(float r)
|
|
{
|
|
return (float)(r / 10.5f * Math.PI * 85);
|
|
}
|
|
private static float DecodeRpmFromPayload(byte[] payload, int offset = 4)
|
|
{
|
|
return BitConverter.ToInt32(payload, offset) * 1875f / 512f / 10000f;
|
|
}
|
|
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;
|
|
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
|
|
|
|
}
|
|
|
|
private static bool IsNodeOperational(byte remoteCode)
|
|
{
|
|
return remoteCode == 5 || remoteCode == 133;
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
private static short ReadInt16BE(byte[] data, int offset)
|
|
{
|
|
return (short)((data[offset] << 8) | data[offset + 1]);
|
|
}
|
|
|
|
private static ushort ReadUInt16BE(byte[] data, int offset)
|
|
{
|
|
return (ushort)((data[offset] << 8) | data[offset + 1]);
|
|
}
|
|
|
|
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];
|
|
},
|
|
};
|
|
}
|
|
|
|
}
|
|
}
|