初始化 shenyuxiang 分支
This commit is contained in:
Binary file not shown.
@@ -1,3 +0,0 @@
|
||||
{
|
||||
"CurrentProjectSetting": null
|
||||
}
|
||||
@@ -1,6 +0,0 @@
|
||||
{
|
||||
"ExpandedNodes": [
|
||||
""
|
||||
],
|
||||
"PreviewInSolutionExplorer": false
|
||||
}
|
||||
Binary file not shown.
+6
-519
@@ -1,534 +1,21 @@
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Sensors;
|
||||
using CommonUsage.Chassis;
|
||||
using CommonUsage.Mathematics;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.AgvInterfaces;
|
||||
using MDCSToolBox.Clumsy.Calibration;
|
||||
using MDCSToolBox.Clumsy.MotionControllers;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using Newtonsoft.Json;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Net.Http;
|
||||
using System.Numerics;
|
||||
using System.Security.Cryptography;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
using static ClumsyCore.DTools.Painter;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public class SetLocationRes
|
||||
{
|
||||
public float x, y, th;
|
||||
public int l_step;
|
||||
public long tick;
|
||||
public string error;
|
||||
}
|
||||
public class AGV : MultiWheelInterface
|
||||
{
|
||||
public override AbstractGeometricController GetController()
|
||||
{
|
||||
return new ChassisController().Get();
|
||||
}
|
||||
|
||||
=> new ChassisController().Get();
|
||||
public override MultiWheelMagTracker GetMagController()
|
||||
{
|
||||
return new MultiWheelMagTracker();
|
||||
}
|
||||
|
||||
=> new MultiWheelMagTracker();
|
||||
public override NaiveMagnetController GetNaiveMagnetController()
|
||||
=> new NaiveMagnetController();
|
||||
|
||||
public void Sleep(float seconds)
|
||||
{
|
||||
return new NaiveMagnetController();
|
||||
new DriveTask(new Sleep { Second = seconds }.Get()).Wait();
|
||||
}
|
||||
|
||||
public void Sleep(float s)
|
||||
{
|
||||
new DriveTask(new Sleep() { Second = s }.Get()).Wait();
|
||||
}
|
||||
|
||||
public void ControlChargePort(bool open)
|
||||
{
|
||||
DLog.Log($"call ControlChargePort({open})");
|
||||
PilotDefinition.Self.OpenChargeByClumsy = open;
|
||||
}
|
||||
|
||||
public void SwitchLidarArea(int area)
|
||||
{
|
||||
DLog.Log($"call SwitchLidarArea({area})");
|
||||
PilotDefinition.Self.AreaChoose = area;
|
||||
}
|
||||
|
||||
public void SwitchIoArea(int area)
|
||||
{
|
||||
if (area != -1)
|
||||
{
|
||||
PilotDefinition.Self.IOObstacleArea = area;
|
||||
}
|
||||
}
|
||||
public void RotateToTarget(float target)
|
||||
{
|
||||
//if (!needrotate) return;
|
||||
var dl = new DriveTask(new MultiWheelRotateInPlace()
|
||||
{
|
||||
AngleTarget = target,
|
||||
PidparamsRead = () => new PIDParams()
|
||||
{
|
||||
Kp = PilotDefinition.Conf.TireFollowingThkp,
|
||||
Ki = PilotDefinition.Conf.TireFollowingThki,
|
||||
Kd = PilotDefinition.Conf.TireFollowingThkd,
|
||||
DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone,
|
||||
SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec,
|
||||
OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh,
|
||||
MaxI = PilotDefinition.Conf.TireFollowingThMaxI,
|
||||
}
|
||||
}.Get());
|
||||
dl.Wait();
|
||||
}
|
||||
|
||||
//参数1：tireNum 需要钻过的轮胎对数量
|
||||
//参数2：frontLidarDetect true:前雷达识别 false:后雷达识别
|
||||
public void TireFollowing(int tireNum, bool frontLidarDetect, int srcId, int dstId)
|
||||
{
|
||||
while (!TryLock(dstId))
|
||||
{
|
||||
Thread.Sleep(50);
|
||||
}
|
||||
DLog.Log($"锁点{dstId}完成", "TireFollowing");
|
||||
var lidarName = frontLidarDetect ? "前雷达" : "后雷达";
|
||||
DLog.Log($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎", "TireFollowing");
|
||||
if (tireNum != 1 && tireNum != 2)
|
||||
{
|
||||
DLog.Log($"TireNum必须是1或2 (当前输入:{tireNum})", "TireFollowing");
|
||||
return;
|
||||
}
|
||||
|
||||
if (PilotDefinition.Self.GhostMode)
|
||||
{
|
||||
while (!TryLock(dstId))
|
||||
{
|
||||
Console.WriteLine("等待锁取货点中...");
|
||||
Thread.Sleep(200);
|
||||
}
|
||||
Console.WriteLine($"锁点{dstId}完成");
|
||||
Thread.Sleep(1000);
|
||||
Console.WriteLine($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎");
|
||||
Thread.Sleep(1000);
|
||||
Leave(srcId);
|
||||
Console.WriteLine($"开始第一段盲走，此时释放预取货点{srcId}");
|
||||
Thread.Sleep(2000);
|
||||
//Leave(dstId);
|
||||
//Console.WriteLine($"结束第一段盲走，此时释放取货点{dstId}");
|
||||
Thread.Sleep(2000);
|
||||
Console.WriteLine($"结束钻车动作");
|
||||
return;
|
||||
}
|
||||
|
||||
var detectors = new List<TireFollowing.DetectorDefinition>()
|
||||
{
|
||||
new TireFollowing.DetectorDefinition()
|
||||
{
|
||||
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, frontLidarDetect),
|
||||
StartGuessingX = frontLidarDetect ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX,
|
||||
StartGuessingY = 0,
|
||||
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
|
||||
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||||
PathTransformation = new Tuple<float, float, float>(
|
||||
frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
|
||||
frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||
0),
|
||||
LeaveSrcFunction = Leave,
|
||||
SrcId = srcId,
|
||||
DstId = dstId,
|
||||
},
|
||||
new TireFollowing.DetectorDefinition()
|
||||
{
|
||||
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, frontLidarDetect),
|
||||
StartGuessingX = frontLidarDetect ? PilotDefinition.Conf.TireFollowingStage2GuessX : -PilotDefinition.Conf.TireFollowingStage2GuessX,
|
||||
StartGuessingY = 0,
|
||||
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
|
||||
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||||
PathTransformation = new Tuple<float, float, float>(
|
||||
frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
|
||||
frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||
0)
|
||||
},
|
||||
};
|
||||
DLog.Log($"钻胎为{tireNum}", "TireFollowing");
|
||||
var following = new TireFollowing()
|
||||
{
|
||||
GetController = () => new ChassisController().Get(),
|
||||
GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
|
||||
GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
|
||||
detectors = detectors,
|
||||
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
|
||||
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
|
||||
TireNum = tireNum,
|
||||
CarDirection = frontLidarDetect ? 0f : 180f,
|
||||
WalkBlindTh = frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarWalkBlindTh : PilotDefinition.Conf.TireFollowingBackLidarWalkBlindTh,
|
||||
};
|
||||
var _dt = new DriveTask(following.Get());
|
||||
_dt.Wait();
|
||||
DLog.Log("钻车动作结束", "TireFollowing");
|
||||
}
|
||||
|
||||
//离车一定是后雷达识别一个轮胎
|
||||
public void LeaveCar(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
|
||||
{
|
||||
while (!TryLock(dstId))
|
||||
{
|
||||
Thread.Sleep(50);
|
||||
}
|
||||
DLog.Log($"锁点{dstId}完成", "TireFollowing");
|
||||
DLog.Log($"开始钻车动作，通过后雷达识别结果钻1对轮胎", "TireFollowing");
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
chassis.SetOriginBias(0, 0, 0);
|
||||
var following = new TireFollowing()
|
||||
{
|
||||
GetController = () => new ChassisController().Get(),
|
||||
GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
|
||||
GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
|
||||
detectors = new List<TireFollowing.DetectorDefinition>()
|
||||
{
|
||||
new TireFollowing.DetectorDefinition()
|
||||
{
|
||||
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
|
||||
StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
|
||||
StartGuessingY = 0,
|
||||
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingLeaveCarWalkBlindSwitchingDistance,
|
||||
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||||
PathTransformation = new Tuple<float, float, float>(
|
||||
PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
|
||||
PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||
0),
|
||||
LeaveSrcFunction = Leave,
|
||||
SrcId = srcId,
|
||||
DstId = dstId,
|
||||
},
|
||||
},
|
||||
CarDirection = 180f,
|
||||
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
|
||||
MaxSpeed = 0.25f,
|
||||
EnableHandover = true,
|
||||
HandoverDistance = 200f,
|
||||
HandoverSpeed = 0.3f,
|
||||
WalkBlindTh = 0,
|
||||
TireNum = 1
|
||||
};
|
||||
|
||||
IEnumerable<bool> LeaveThenFollow()
|
||||
{
|
||||
foreach (var running in following.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
|
||||
DLog.Log("离车TireFollowing结束，开始DstTracker", "TireFollowing");
|
||||
|
||||
foreach (var running in new DstTracker()
|
||||
{
|
||||
Src = new Vector2(srcX, srcY),
|
||||
Dst = new Vector2(dstX, dstY),
|
||||
CarDirectionBias = 180f,
|
||||
InitialSendSpeed = 0.3f
|
||||
}.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
yield return false;
|
||||
}
|
||||
|
||||
var _dt = new DriveTask(LeaveThenFollow());
|
||||
_dt.Wait();
|
||||
DLog.Log("离车动作1结束", "TireFollowing");
|
||||
}
|
||||
|
||||
public void LineTracking(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
|
||||
{
|
||||
while (!TryLock(dstId))
|
||||
{
|
||||
Thread.Sleep(50);
|
||||
}
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
chassis.SetOriginBias(0, 0, 0);
|
||||
DLog.Log($"锁点{dstId}完成", "TireFollowing");
|
||||
IEnumerable<bool> TrackThenFollow()
|
||||
{
|
||||
foreach (var running in new LineTracking()
|
||||
{
|
||||
Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
|
||||
LeaveSrcFunction = Leave,
|
||||
SrcId = srcId,
|
||||
EnableHandover = true,
|
||||
HandoverDistance = 200,
|
||||
HandoverSpeed = 0.3f,
|
||||
}.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
|
||||
DLog.Log("离车LineTracking结束，开始DstTracker", "TireFollowing");
|
||||
while (!TryLock(426))
|
||||
{
|
||||
Thread.Sleep(20);
|
||||
}
|
||||
Leave(dstId);
|
||||
DLog.Log($"释放锁点{dstId}完成", "TireFollowing");
|
||||
foreach (var running in new DstTracker()
|
||||
{
|
||||
Src = new Vector2(srcX, srcY),
|
||||
Dst = new Vector2(dstX, dstY),
|
||||
InitialSendSpeed = 0.3f
|
||||
}.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
var _dt = new DriveTask(TrackThenFollow());
|
||||
_dt.Wait();
|
||||
DLog.Log("离车动作2结束", "TireFollowing");
|
||||
}
|
||||
|
||||
//驱动器上使能
|
||||
public void DriverAble()
|
||||
{
|
||||
var dl = new DriveTask(new DriverAble() { }.Get());
|
||||
dl.Wait();
|
||||
DLog.Log("驱动器上使能完成", "TireFollowing");
|
||||
}
|
||||
|
||||
//驱动器下使能
|
||||
public void DriverDisable()
|
||||
{
|
||||
var dl = new DriveTask(new DriverDisable() { }.Get());
|
||||
dl.Wait();
|
||||
DLog.Log("驱动器下使能完成", "TireFollowing");
|
||||
}
|
||||
|
||||
// 夹抱：close 为 true 时关闭夹抱，否则打开夹抱。
|
||||
public void ClamptoTarget(bool close)
|
||||
{
|
||||
if (PilotDefinition.Self.GhostMode)
|
||||
{
|
||||
Thread.Sleep(2000);
|
||||
Console.WriteLine("夹抱完成");
|
||||
return;
|
||||
}
|
||||
new DriveTask(new ClampToTarget()
|
||||
{
|
||||
LeftClampTarget = close ? PilotDefinition.Self.LeftArmUpperPos : PilotDefinition.Self.LeftArmLowerPos,
|
||||
RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos
|
||||
}.Get()).Wait();
|
||||
}
|
||||
// Fleet crab walk: convert scheduler src/dst into the same relative crab-walk path used by MovementTest.
|
||||
public void FleetCrabWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
|
||||
float speed)
|
||||
{
|
||||
var dx = dstX - srcX;
|
||||
var dy = dstY - srcY;
|
||||
var pathLength = (float)Math.Sqrt(dx * dx + dy * dy);
|
||||
|
||||
if (pathLength <= 1f)
|
||||
{
|
||||
DLog.Log("FleetCrabWalk abort: path length is too short.", "FleetCrabDbg");
|
||||
return;
|
||||
}
|
||||
|
||||
var self = PilotDefinition.Self;
|
||||
if (!self.TryGetFleetCenterFromMembers(out var centerX, out var centerY, out var centerTh) &&
|
||||
!self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
|
||||
{
|
||||
DLog.Log("FleetCrabWalk abort: failed to read fleet center.", "FleetCrabDbg");
|
||||
Hedingben.ToastText("FleetCrab requires master localization", "FleetCrab");
|
||||
return;
|
||||
}
|
||||
|
||||
var pathAngle = (float)CommonMath.RoundTh((float)(Math.Atan2(dy, dx) / Math.PI * 180.0));
|
||||
var crabAngle = (float)CommonMath.ThDiff(pathAngle, centerTh);
|
||||
var targetBodyWorldHeading = (float)CommonMath.RoundTh(PilotDefinition.Conf.FleetCrabBodyWorldHeadingDeg);
|
||||
var bodyToPathAngle = (float)CommonMath.ThDiff(pathAngle, targetBodyWorldHeading);
|
||||
DLog.Log(
|
||||
$"call FleetCrabWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
|
||||
$"len={pathLength:0.0}, speed={speed:0.000}, pathAngle={pathAngle:0.0}, " +
|
||||
$"center=({centerX:0},{centerY:0},{centerTh:0.0}), crabAngle={crabAngle:0.0}, " +
|
||||
$"targetBodyWorld={targetBodyWorldHeading:0.0}, bodyToPath={bodyToPathAngle:0.0})",
|
||||
"FleetCrabDbg");
|
||||
|
||||
if (dstId != -1)
|
||||
{
|
||||
while (!TryLock(dstId))
|
||||
{
|
||||
Thread.Sleep(50);
|
||||
}
|
||||
DLog.Log($"锁点{dstId}完成", "FleetCrabDbg");
|
||||
}
|
||||
|
||||
var action = new MultiWheelC.FleetCrabWalk
|
||||
{
|
||||
CrabAngleDeg = crabAngle,
|
||||
BodyToPathAngleDeg = bodyToPathAngle,
|
||||
CrabLengthMm = pathLength,
|
||||
CrabSpeed = speed,
|
||||
FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
|
||||
FleetCrabStartAccel = PilotDefinition.Conf.FleetCrabStartAccel,
|
||||
FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
|
||||
FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
|
||||
FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,
|
||||
FleetCrabSlowingPow = PilotDefinition.Conf.FleetCrabSlowingPow,
|
||||
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
|
||||
};
|
||||
|
||||
try
|
||||
{
|
||||
new DriveTask(action.Get()).Wait();
|
||||
}
|
||||
finally
|
||||
{
|
||||
if (srcId != -1)
|
||||
{
|
||||
Leave(srcId);
|
||||
DLog.Log($"释放放车点{srcId}", "FleetCrabDbg");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void FleetCurveWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
|
||||
float speed, params float[] trackTypeInfo)
|
||||
{
|
||||
if (trackTypeInfo == null || trackTypeInfo.Length < 2)
|
||||
{
|
||||
DLog.Log("FleetCurveWalk abort: invalid trackTypeInfo, expected Bezier type info.", "FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve invalid trackTypeInfo", "FleetCurve");
|
||||
return;
|
||||
}
|
||||
|
||||
var trackType = (int)trackTypeInfo[0];
|
||||
if (trackType != 2)
|
||||
{
|
||||
DLog.Log($"FleetCurveWalk abort: unsupported trackType={trackType}, only Bezier(type=2) is supported.",
|
||||
"FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve only supports Bezier trackType=2", "FleetCurve");
|
||||
return;
|
||||
}
|
||||
|
||||
var controlPointNum = (int)trackTypeInfo[1];
|
||||
var expectedLength = 2 + controlPointNum * 2;
|
||||
if (controlPointNum < 3 || trackTypeInfo.Length < expectedLength)
|
||||
{
|
||||
DLog.Log(
|
||||
$"FleetCurveWalk abort: invalid Bezier trackTypeInfo. controlPointNum={controlPointNum}, " +
|
||||
$"length={trackTypeInfo.Length}, expected>={expectedLength}.",
|
||||
"FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve invalid Bezier trackTypeInfo", "FleetCurve");
|
||||
return;
|
||||
}
|
||||
|
||||
BezierTrack track;
|
||||
try
|
||||
{
|
||||
track = ProcessTrackTypeInfo(srcX, srcY, dstX, dstY, trackTypeInfo) as BezierTrack;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
DLog.Log($"FleetCurveWalk abort: failed to process trackTypeInfo. {ex.Message}", "FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve failed to process track", "FleetCurve");
|
||||
return;
|
||||
}
|
||||
|
||||
if (track == null)
|
||||
{
|
||||
DLog.Log("FleetCurveWalk abort: ProcessTrackTypeInfo did not return BezierTrack.", "FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve requires BezierTrack", "FleetCurve");
|
||||
return;
|
||||
}
|
||||
|
||||
track.Speed = speed;
|
||||
track.CarDirectionBias = 0f;
|
||||
|
||||
DLog.Log(
|
||||
$"call FleetCurveWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
|
||||
$"speed={speed:0.000}, trackType={trackType}, controls={controlPointNum}, track={track.GetType().Name}, " +
|
||||
$"carDirectionBias=0.0)",
|
||||
"FleetCurveDbg");
|
||||
|
||||
if (dstId != -1)
|
||||
{
|
||||
while (!TryLock(dstId))
|
||||
{
|
||||
Thread.Sleep(50);
|
||||
}
|
||||
DLog.Log($"閿佺偣{dstId}瀹屾垚", "FleetCurveDbg");
|
||||
}
|
||||
|
||||
var action = new MultiWheelC.FleetCurveWalk
|
||||
{
|
||||
Track = track,
|
||||
CurveSpeed = speed,
|
||||
CarDirectionBias = 0f,
|
||||
SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance,
|
||||
FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance,
|
||||
FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed,
|
||||
SlowingPow = PilotDefinition.Conf.FleetCurveSlowingPow,
|
||||
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
|
||||
StartSyncTimeoutSec = PilotDefinition.Conf.FleetCrabStartSyncTimeoutSec
|
||||
};
|
||||
|
||||
try
|
||||
{
|
||||
new DriveTask(action.Get()).Wait();
|
||||
}
|
||||
finally
|
||||
{
|
||||
if (srcId != -1)
|
||||
{
|
||||
Leave(srcId);
|
||||
DLog.Log($"release srcId={srcId}", "FleetCurveDbg");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void ChangeAvoidanceDistance(float stopDistance, float slowDistance)
|
||||
{
|
||||
DLog.Log($"call ChangeAvoidanceDistance({stopDistance},{slowDistance})");
|
||||
PilotDefinition.Self.SlowDistance = slowDistance;
|
||||
PilotDefinition.Self.StopDistance = stopDistance;
|
||||
}
|
||||
public void ChangeAvoidanceParam(float length = -1, float width = -1)
|
||||
{
|
||||
|
||||
PilotDefinition.Self.CarLength = length;
|
||||
PilotDefinition.Self.CarWidth = width;
|
||||
|
||||
}
|
||||
public void SetLocation(float x, float y, float th)
|
||||
{
|
||||
DLog.Log($"call SetLocation({x},{y},{th})");
|
||||
Console.WriteLine($"call SetLocation({x},{y},{th})");
|
||||
Queue(() =>
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
var str1 = new HttpClient()
|
||||
.GetStringAsync(
|
||||
$"http://127.0.0.1:4321/setLocation?x={x}&y={y}&th={th}")
|
||||
.Result;
|
||||
Thread.Sleep(500);
|
||||
Console.WriteLine($"SetLocation str={str1}");
|
||||
var setLocationRes = JsonConvert.DeserializeObject<SetLocationRes>(str1);
|
||||
Console.WriteLine(setLocationRes.l_step);
|
||||
if (setLocationRes != null && setLocationRes.l_step == 2) break;
|
||||
}
|
||||
});
|
||||
}
|
||||
public float baseSpeed = 0;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,9 +1,5 @@
|
||||
using System;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.MotionControllers;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
@@ -14,8 +10,7 @@ public class ChassisController : MovementDefinition<MultiWheelGeometricControlle
|
||||
{
|
||||
public float BaseSpeed = Configuration.conf.basicSpeed;
|
||||
|
||||
private DateTime _sendMotionDbgLast = DateTime.MinValue;
|
||||
|
||||
// 创建单车几何跟踪控制器（直接控本车底盘，不走多车 Auto 通道）
|
||||
public override MultiWheelGeometricController Get()
|
||||
{
|
||||
return new MultiWheelGeometricController
|
||||
@@ -45,56 +40,6 @@ public class ChassisController : MovementDefinition<MultiWheelGeometricControlle
|
||||
DthLinearThreshold = PilotDefinition.Conf.DthLinearThreshold,
|
||||
BiasFac = PilotDefinition.Conf.BiasFac,
|
||||
BiasThreshold = PilotDefinition.Conf.BiasThreshold,
|
||||
|
||||
MultiVehicleSendMotion = (speed, frontTh, rearTh, idealPos, idealAngle) =>
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
// A: 用固定锁对象（不再锁会被替换的字段引用）。
|
||||
int fleetCnt;
|
||||
lock (self.FleetLock)
|
||||
fleetCnt = self.MultiVehicleFleet.Count;
|
||||
|
||||
// 诊断（节流 ~300ms）：确认回调被调用、编队是否就绪、是否因数量不符提前 return（导致不下发速度）。
|
||||
if ((DateTime.Now - _sendMotionDbgLast).TotalMilliseconds >= 300)
|
||||
{
|
||||
_sendMotionDbgLast = DateTime.Now;
|
||||
DLog.Log(
|
||||
$"SENDMOTION speed={speed:0.000} fTh={frontTh:0.0} rTh={rearTh:0.0} " +
|
||||
$"ideal=({idealPos.X:0},{idealPos.Y:0},{idealAngle:0.0}) " +
|
||||
$"editCnt={fleetCnt}/{PilotDefinition.Conf.MultiVehicleFleetNum} " +
|
||||
$"earlyReturn={fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum}",
|
||||
"FleetCrabDbg");
|
||||
}
|
||||
|
||||
if (fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum)
|
||||
return;
|
||||
|
||||
self.MultiVehicleAutoVx = speed;
|
||||
self.MultiVehicleAutoFrontTh = frontTh;
|
||||
self.MultiVehicleAutoRearTh = rearTh;
|
||||
// D: 透传路径控制器算出的理想车队中心位姿（此前被丢弃），供各车按 layout 做前馈。
|
||||
self.MultiVehicleAutoIdealX = idealPos.X;
|
||||
self.MultiVehicleAutoIdealY = idealPos.Y;
|
||||
self.MultiVehicleAutoIdealTh = idealAngle;
|
||||
self.MultiVehicleAutoHasIdeal = true;
|
||||
// B: 标记命令新鲜度。路径结束/早退/卡顿不再刷新此时刻 → 主车超时后清零速度，避免滑行。
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
},
|
||||
|
||||
// G: 读取车队中心原子快照，避免跨线程读到撕裂的 x/y/th 组合。
|
||||
MultiVehicleGetFleetPos = () =>
|
||||
{
|
||||
var snap = PilotDefinition.Self.GetFleetCenterSnapshot();
|
||||
return new Location
|
||||
{
|
||||
x = snap.X,
|
||||
y = snap.Y,
|
||||
th = snap.Th,
|
||||
l_step = 1,
|
||||
tick = DateTime.Now.Ticks
|
||||
};
|
||||
}
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -2,14 +2,16 @@
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>netstandard2.0</TargetFramework>
|
||||
<LangVersion>10</LangVersion>
|
||||
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
|
||||
<AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
|
||||
<OutputPath>..\..\build\Clumsy\</OutputPath>
|
||||
<LangVersion>10</LangVersion>
|
||||
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
|
||||
<AssemblyName>ClumsyPilot</AssemblyName>
|
||||
<RootNamespace>MultiWheelC</RootNamespace>
|
||||
<AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
|
||||
<OutputPath>build\Clumsy\</OutputPath>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<PackageReference Include="Newtonsoft.Json" Version="13.0.4" />
|
||||
<PackageReference Include="Newtonsoft.Json" Version="13.0.3" />
|
||||
<PackageReference Include="System.Numerics.Vectors" Version="4.6.1" />
|
||||
</ItemGroup>
|
||||
|
||||
@@ -33,9 +35,5 @@
|
||||
<HintPath>ref\RefFundamentalLib.dll</HintPath>
|
||||
</Reference>
|
||||
</ItemGroup>
|
||||
|
||||
<Target Name="PostBuild" AfterTargets="PostBuildEvent">
|
||||
<Exec Command="if not exist $(SolutionDir)build\Clumsy mkdir $(SolutionDir)build\Clumsy
if not exist $(SolutionDir)build\Clumsy_AGV2 mkdir $(SolutionDir)build\Clumsy_AGV2
copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.exe $(SolutionDir)build\Clumsy\
copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.deps.json $(SolutionDir)build\Clumsy\
copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.runtimeconfig.json $(SolutionDir)build\Clumsy\
copy /Y D:\MDCS\Release\Clumsy\RefClumsyCore.dll $(SolutionDir)build\Clumsy\
copy /Y D:\MDCS\Release\Clumsy\RefClumsyDance.dll $(SolutionDir)build\Clumsy\
copy /Y D:\MDCS\Release\MDCSToolBox.dll $(SolutionDir)build\Clumsy\
copy /Y D:\MDCS\Release\CommonUsage.dll $(SolutionDir)build\Clumsy\
copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.exe $(SolutionDir)build\Clumsy_AGV2\
copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.deps.json $(SolutionDir)build\Clumsy_AGV2\
copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.runtimeconfig.json $(SolutionDir)build\Clumsy_AGV2\
copy /Y D:\MDCS\Release\Clumsy\RefClumsyCore.dll $(SolutionDir)build\Clumsy_AGV2\
copy /Y D:\MDCS\Release\Clumsy\RefClumsyDance.dll $(SolutionDir)build\Clumsy_AGV2\
copy /Y D:\MDCS\Release\MDCSToolBox.dll $(SolutionDir)build\Clumsy_AGV2\
copy /Y D:\MDCS\Release\CommonUsage.dll $(SolutionDir)build\Clumsy_AGV2\
copy /Y $(TargetDir)$(TargetName).dll $(SolutionDir)build\Clumsy_AGV2\
copy /Y $(TargetDir)$(TargetName).pdb $(SolutionDir)build\Clumsy_AGV2\" />
|
||||
</Target>
|
||||
|
||||
|
||||
</Project>
|
||||
|
||||
@@ -1,526 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using CommonUsage.Chassis;
|
||||
using CommonUsage.Mathematics;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
// ===== 车队联动-自动蟹行动作 =====
|
||||
// 以当前车队中心为起点，构造指定方向和长度的直线路径；
|
||||
// 执行侧直接写 MultiVehicleAuto...，由 TickMultiVehicle 自动分支统一下发。
|
||||
//
|
||||
// 控制思路参考 MDCSToolbox 几何控制器，但实现收在 MultiWheelC 内：
|
||||
// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度、横向偏差和车身目标朝向偏差；
|
||||
// 2) 根据横向偏差给前后 GCP 同向修正，根据车身目标朝向偏差给前后 GCP 反向修正；
|
||||
// 3) 根据终点距离减速，并发布 ideal fleet center 给从车做前馈。
|
||||
//
|
||||
// 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
|
||||
public class FleetCrabWalk : MovementDefinition
|
||||
{
|
||||
/// <summary>路径方向相对启动时车队朝向的夹角(deg，逆时针为正)。</summary>
|
||||
public float CrabAngleDeg = 45f;
|
||||
|
||||
/// <summary>路径方向相对车身目标朝向的夹角(deg，逆时针为正)。MovementTest 会设为 CrabAngleDeg，以保持启动时车身朝向。</summary>
|
||||
public float BodyToPathAngleDeg = 45f;
|
||||
|
||||
/// <summary>路径长度(mm)。</summary>
|
||||
public float CrabLengthMm = 2000f;
|
||||
|
||||
/// <summary>行驶速度(m/s)。</summary>
|
||||
public float CrabSpeed = 0.2f;
|
||||
|
||||
/// <summary>速度命令加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
|
||||
public float FleetCrabAccel = 0.2f;
|
||||
|
||||
/// <summary>预对齐后正式下发速度前 5 秒加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
|
||||
public float FleetCrabStartAccel = 0.01f;
|
||||
|
||||
/// <summary>末端开始减速距离(mm)。</summary>
|
||||
public float FleetCrabSlowDistance = 2000f;
|
||||
|
||||
/// <summary>完成距离(mm)，低于该剩余距离结束动作。</summary>
|
||||
public float FleetCrabFinishDistance = 20f;
|
||||
|
||||
/// <summary>末端最低速度(m/s)。</summary>
|
||||
public float FleetCrabFinishSpeed = 0.02f;
|
||||
|
||||
/// <summary>末端减速曲线指数。</summary>
|
||||
public float FleetCrabSlowingPow = 0.8f;
|
||||
|
||||
/// <summary>前后 GCP 舵角修正上限(deg)。</summary>
|
||||
public float GcpThetaThreshold = 95f;
|
||||
|
||||
private bool _stopping;
|
||||
|
||||
private void Cleanup()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = 0;
|
||||
self.MultiVehicleAutoRearTh = 0;
|
||||
self.MultiVehicleAutoHasIdeal = false;
|
||||
self.MultiVehicleAutoEnabled = false;
|
||||
}
|
||||
|
||||
public void Stop()
|
||||
{
|
||||
_stopping = true;
|
||||
Cleanup();
|
||||
}
|
||||
|
||||
private static float Clamp(float value, float min, float max)
|
||||
{
|
||||
if (value < min) return min;
|
||||
if (value > max) return max;
|
||||
return value;
|
||||
}
|
||||
|
||||
private static float ClampAbs(float value, float limit)
|
||||
{
|
||||
var absLimit = Math.Abs(limit);
|
||||
if (absLimit <= 0) return value;
|
||||
if (value > absLimit) return absLimit;
|
||||
if (value < -absLimit) return -absLimit;
|
||||
return value;
|
||||
}
|
||||
|
||||
private static float Slew(float current, float target, float maxDelta)
|
||||
{
|
||||
if (maxDelta <= 0) return target;
|
||||
if (target > current + maxDelta) return current + maxDelta;
|
||||
if (target < current - maxDelta) return current - maxDelta;
|
||||
return target;
|
||||
}
|
||||
|
||||
private static float AverageAngle(float frontTh, float rearTh)
|
||||
{
|
||||
var diff = (float)CommonMath.ThDiff(frontTh, rearTh);
|
||||
return (float)CommonMath.RoundTh(rearTh + diff / 2f);
|
||||
}
|
||||
|
||||
private static void ResolveCrabDriveEquivalent(float speed, float rawFrontTh, float rawRearTh, float steerLimit,
|
||||
out float driveSpeed, out float frontTh, out float rearTh, out bool reverseEquivalent, out float rawBaseTh)
|
||||
{
|
||||
var limit = Math.Min(179f, Math.Max(1f, Math.Abs(steerLimit)));
|
||||
rawBaseTh = AverageAngle(rawFrontTh, rawRearTh);
|
||||
driveSpeed = speed;
|
||||
frontTh = rawFrontTh;
|
||||
rearTh = rawRearTh;
|
||||
reverseEquivalent = false;
|
||||
|
||||
if (rawBaseTh > limit)
|
||||
{
|
||||
frontTh = (float)CommonMath.RoundTh(frontTh - 180f);
|
||||
rearTh = (float)CommonMath.RoundTh(rearTh - 180f);
|
||||
driveSpeed = -driveSpeed;
|
||||
reverseEquivalent = true;
|
||||
}
|
||||
else if (rawBaseTh < -limit)
|
||||
{
|
||||
frontTh = (float)CommonMath.RoundTh(frontTh + 180f);
|
||||
rearTh = (float)CommonMath.RoundTh(rearTh + 180f);
|
||||
driveSpeed = -driveSpeed;
|
||||
reverseEquivalent = true;
|
||||
}
|
||||
|
||||
frontTh = ClampAbs(frontTh, limit);
|
||||
rearTh = ClampAbs(rearTh, limit);
|
||||
}
|
||||
|
||||
private static float ProbeSpeed(float speed)
|
||||
{
|
||||
return Math.Abs(speed) > 1e-4f ? speed : 1f;
|
||||
}
|
||||
|
||||
private static bool TryGetMotionYawSign(float frontTh, float rearTh, float driveSpeed, float controlRadius,
|
||||
out float yawSign)
|
||||
{
|
||||
yawSign = 0f;
|
||||
if (Math.Abs(CommonMath.ThDiff(frontTh, rearTh)) <= 1e-3f)
|
||||
return false;
|
||||
|
||||
var radius = Math.Max(1f, Math.Abs(controlRadius));
|
||||
Vector2 pFront = new(radius, 0), pRear = new(-radius, 0),
|
||||
normFront = CommonMath.Transform2D(pFront, frontTh + 90f, Vector2.UnitX),
|
||||
normRear = CommonMath.Transform2D(pRear, rearTh + 90f, Vector2.UnitX);
|
||||
var (intersect, center) = CommonMath.TwoLinesIntersection(pFront, normFront, pRear, normRear);
|
||||
if (!intersect)
|
||||
return false;
|
||||
|
||||
// Match MultiWheelChassis.SendMotion: the tangent side is selected by
|
||||
// rotCenter.Y > 1, and reverse-equivalent motion flips the yaw direction.
|
||||
var tangentSign = center.Y > 1f ? 1f : -1f;
|
||||
var speedSign = driveSpeed >= 0f ? 1f : -1f;
|
||||
yawSign = speedSign * tangentSign;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static float GetYawSplitSign(float baseTh, float speed, float steerLimit, float controlRadius)
|
||||
{
|
||||
const float probeDth = 1f;
|
||||
ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + probeDth, baseTh - probeDth, steerLimit,
|
||||
out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
|
||||
return TryGetMotionYawSign(probeFrontTh, probeRearTh, probeSpeed, controlRadius, out var yawSign)
|
||||
? yawSign
|
||||
: 1f;
|
||||
}
|
||||
|
||||
private static float EstimateLateralVelocity(float bodyTh, float frontTh, float rearTh, float driveSpeed,
|
||||
Vector2 pathLeft)
|
||||
{
|
||||
var motionTh = (float)CommonMath.RoundTh(bodyTh + AverageAngle(frontTh, rearTh));
|
||||
var rad = motionTh / 180f * Math.PI;
|
||||
var dir = new Vector2((float)Math.Cos(rad), (float)Math.Sin(rad));
|
||||
if (driveSpeed < 0f)
|
||||
dir = -dir;
|
||||
return Vector2.Dot(dir, pathLeft);
|
||||
}
|
||||
|
||||
private static float ScoreBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
|
||||
float lateral, float biasProbe)
|
||||
{
|
||||
ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + biasProbe, baseTh + biasProbe, steerLimit,
|
||||
out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
|
||||
var lateralVelocity = EstimateLateralVelocity(bodyTh, probeFrontTh, probeRearTh, probeSpeed, pathLeft);
|
||||
return -Math.Sign(lateral) * lateralVelocity;
|
||||
}
|
||||
|
||||
private static float GetLateralBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
|
||||
float lateral)
|
||||
{
|
||||
if (Math.Abs(lateral) <= 1e-3f)
|
||||
return 1f;
|
||||
|
||||
const float probeBias = 1f;
|
||||
var positiveScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, probeBias);
|
||||
var negativeScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, -probeBias);
|
||||
return positiveScore >= negativeScore ? 1f : -1f;
|
||||
}
|
||||
|
||||
private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
|
||||
out float centerTh, out string source)
|
||||
{
|
||||
if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
|
||||
{
|
||||
source = "fleet";
|
||||
return true;
|
||||
}
|
||||
|
||||
if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
|
||||
{
|
||||
source = "slam";
|
||||
return true;
|
||||
}
|
||||
|
||||
source = "none";
|
||||
return false;
|
||||
}
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
var conf = PilotDefinition.Conf;
|
||||
var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
|
||||
if (chassis == null)
|
||||
{
|
||||
DLog.Log("ABORT: FleetCrabWalk requires MultiWheelChassis.", "FleetCrabDbg");
|
||||
yield break;
|
||||
}
|
||||
_stopping = false;
|
||||
|
||||
DLog.Log(
|
||||
$"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " +
|
||||
$"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " +
|
||||
$"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
|
||||
$"autoFields=true pathMode=relative pathAngle={CrabAngleDeg:0.0} " +
|
||||
$"bodyToPath={BodyToPathAngleDeg:0.0} gcpLimit={GcpThetaThreshold:0.0} " +
|
||||
$"biasFac={conf.BiasFac:0.00} fleetCrabDthFac={conf.FleetCrabDthLinearFac:0.00}",
|
||||
"FleetCrabDbg");
|
||||
|
||||
if (conf.MultiVehicleMasterEndpoint != "/")
|
||||
{
|
||||
DLog.Log($"ABORT: 非主车 (endpoint={conf.MultiVehicleMasterEndpoint})", "FleetCrabDbg");
|
||||
Hedingben.ToastText("车队蟹行需在主车(主车端点=\"/\")运行", "FleetCrab");
|
||||
yield break;
|
||||
}
|
||||
|
||||
// 注意：getCartLocation() 在无有效 Detour 定位时会阻塞——若卡在这里且后面看不到 CENTER 日志，即定位未就绪。
|
||||
DLog.Log("主车校验通过，开始读取车队中心 (getCartLocation 无定位会阻塞)…", "FleetCrabDbg");
|
||||
if (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
|
||||
{
|
||||
DLog.Log("ABORT: TryGetFleetCenterFromSlam 返回 false (无定位)", "FleetCrabDbg");
|
||||
Hedingben.ToastText("车队蟹行需要主车 Detour 定位", "FleetCrab");
|
||||
yield break;
|
||||
}
|
||||
DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
|
||||
|
||||
DLog.Log($"CENTER_SOURCE source={initialCenterSource} center=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
|
||||
|
||||
var pathStart = new Vector2(x0, y0);
|
||||
var pathLengthMm = CrabLengthMm;
|
||||
var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
|
||||
var dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
|
||||
var targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
|
||||
var phiRad = phi / 180.0 * Math.PI;
|
||||
var pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
|
||||
var pathLeft = new Vector2(-pathDir.Y, pathDir.X);
|
||||
|
||||
DLog.Log(
|
||||
$"START center=({x0:0},{y0:0},{theta:0.0}) pathMode=relative " +
|
||||
$"src=({pathStart.X:0},{pathStart.Y:0}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
|
||||
$"phi={phi:0.0} targetBody={targetBodyTh:0.0} " +
|
||||
$"len={pathLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000} startAccel={FleetCrabStartAccel:0.000} accel={FleetCrabAccel:0.000} " +
|
||||
$"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " +
|
||||
$"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}",
|
||||
"FleetCrabDbg");
|
||||
|
||||
var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
|
||||
var controlRadius = Math.Max(1f, Math.Abs(conf.TestCarSyncDistance) / 2f);
|
||||
ResolveCrabDriveEquivalent(0f, (float)CommonMath.ThDiff(phi, theta),
|
||||
(float)CommonMath.ThDiff(phi, theta), gcpLimit, out _, out var holdFrontTh, out var holdRearTh,
|
||||
out _, out _);
|
||||
var warmStart = DateTime.Now;
|
||||
var warmSeqBaseline = self.BeginFleetMotionWarmup();
|
||||
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = holdFrontTh;
|
||||
self.MultiVehicleAutoRearTh = holdRearTh;
|
||||
self.MultiVehicleAutoIdealX = pathStart.X;
|
||||
self.MultiVehicleAutoIdealY = pathStart.Y;
|
||||
self.MultiVehicleAutoIdealTh = targetBodyTh;
|
||||
self.MultiVehicleAutoHasIdeal = true;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
self.PrimeMasterAutoFromSlam();
|
||||
DLog.Log(
|
||||
$"WARMUP auto fields enabled, waiting for fleet startup sync seqBase={warmSeqBaseline} " +
|
||||
$"hold=({holdFrontTh:0.00},{holdRearTh:0.00})",
|
||||
"FleetCrabDbg");
|
||||
|
||||
var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, conf.FleetCrabStartSyncTimeoutSec));
|
||||
var warmIter = 0;
|
||||
var warmReady = false;
|
||||
var warmDetail = "";
|
||||
while (!_stopping && DateTime.Now < warmEnd)
|
||||
{
|
||||
warmIter++;
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = holdFrontTh;
|
||||
self.MultiVehicleAutoRearTh = holdRearTh;
|
||||
self.MultiVehicleAutoIdealX = pathStart.X;
|
||||
self.MultiVehicleAutoIdealY = pathStart.Y;
|
||||
self.MultiVehicleAutoIdealTh = targetBodyTh;
|
||||
self.MultiVehicleAutoHasIdeal = true;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
self.PrimeMasterAutoFromSlam();
|
||||
var snap = self.GetFleetCenterSnapshot();
|
||||
int cnt;
|
||||
lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
|
||||
if (warmIter % 5 == 0)
|
||||
DLog.Log(
|
||||
$"WARMUP#{warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) tick={snap.Tick} " +
|
||||
$"autoEn={self.MultiVehicleAutoEnabled} scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum} " +
|
||||
$"detail={warmDetail}",
|
||||
"FleetCrabDbg");
|
||||
if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
|
||||
conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
|
||||
{
|
||||
warmReady = true;
|
||||
DLog.Log(
|
||||
$"WARMUP done iter={warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt} detail={warmDetail}",
|
||||
"FleetCrabDbg");
|
||||
break;
|
||||
}
|
||||
yield return true;
|
||||
}
|
||||
if (!warmReady)
|
||||
{
|
||||
DLog.Log($"WARMUP timeout: fleet startup sync failed, abort action. detail={warmDetail}",
|
||||
"FleetCrabDbg");
|
||||
Hedingben.ToastText("车队蟹行启动同步超时，已取消", "FleetCrab");
|
||||
Cleanup();
|
||||
yield break;
|
||||
}
|
||||
|
||||
Hedingben.ToastText($"车队蟹行 路径{phi:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{pathLengthMm:0}mm", "FleetCrab");
|
||||
|
||||
if (warmReady && self.TryGetFleetCenterFromMembers(out var warmX, out var warmY, out var warmTh))
|
||||
{
|
||||
x0 = warmX;
|
||||
y0 = warmY;
|
||||
theta = warmTh;
|
||||
pathStart = new Vector2(x0, y0);
|
||||
phi = CommonMath.RoundTh(theta + CrabAngleDeg);
|
||||
dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
|
||||
targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
|
||||
phiRad = phi / 180.0 * Math.PI;
|
||||
pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
|
||||
pathLeft = new Vector2(-pathDir.Y, pathDir.X);
|
||||
self.MultiVehicleAutoIdealX = pathStart.X;
|
||||
self.MultiVehicleAutoIdealY = pathStart.Y;
|
||||
self.MultiVehicleAutoIdealTh = targetBodyTh;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
DLog.Log(
|
||||
$"WARMUP_REBASE source=fleet center=({x0:0},{y0:0},{theta:0.0}) phi={phi:0.0} targetBody={targetBodyTh:0.0} dst=({dst.X:0},{dst.Y:0})",
|
||||
"FleetCrabDbg");
|
||||
}
|
||||
|
||||
var iter = 0;
|
||||
var lastLog = DateTime.MinValue;
|
||||
var finishDistance = Math.Max(0f, FleetCrabFinishDistance);
|
||||
var slowDistance = Math.Max(finishDistance + 1f, FleetCrabSlowDistance);
|
||||
var baseSpeed = Math.Abs(CrabSpeed);
|
||||
var finishSpeed = Math.Min(baseSpeed, Math.Abs(FleetCrabFinishSpeed));
|
||||
var slowingPow = Math.Max(0.01f, FleetCrabSlowingPow);
|
||||
var accel = Math.Abs(FleetCrabAccel);
|
||||
var startAccel = Math.Abs(FleetCrabStartAccel);
|
||||
var cmdSpeed = 0f;
|
||||
var lastTick = DateTime.Now;
|
||||
var speedRampStart = DateTime.Now;
|
||||
var stopReason = "done";
|
||||
|
||||
while (!_stopping)
|
||||
{
|
||||
iter++;
|
||||
|
||||
if (!TryGetControlFleetCenter(self, out var cx, out var cy, out var cth, out var centerSource))
|
||||
{
|
||||
stopReason = "fleet center invalid";
|
||||
DLog.Log("ABORT: TryGetControlFleetCenter returned false during auto crab.", "FleetCrabDbg");
|
||||
break;
|
||||
}
|
||||
var delta = new Vector2(cx - pathStart.X, cy - pathStart.Y);
|
||||
var along = Vector2.Dot(delta, pathDir);
|
||||
var lateral = Vector2.Dot(delta, pathLeft);
|
||||
var remain = pathLengthMm - along;
|
||||
if (remain <= finishDistance)
|
||||
break;
|
||||
|
||||
var targetSpeed = baseSpeed;
|
||||
var slowRatio = 1f;
|
||||
if (remain < slowDistance)
|
||||
{
|
||||
slowRatio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow);
|
||||
targetSpeed = slowRatio * (baseSpeed - finishSpeed) + finishSpeed;
|
||||
}
|
||||
var now = DateTime.Now;
|
||||
var dt = Math.Max(0.001f, (float)(now - lastTick).TotalSeconds);
|
||||
lastTick = now;
|
||||
var rampElapsed = (now - speedRampStart).TotalSeconds;
|
||||
var activeAccel = rampElapsed < 5.0 ? startAccel : accel;
|
||||
var speed = activeAccel > 0 ? Slew(cmdSpeed, targetSpeed, activeAccel * dt) : targetSpeed;
|
||||
cmdSpeed = speed;
|
||||
|
||||
var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
|
||||
var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
|
||||
var headingErrReverse = (float)CommonMath.ThDiff(cth, targetBodyTh);
|
||||
var targetBodyToPath = (float)CommonMath.ThDiff(phi, targetBodyTh);
|
||||
var rawBiasMagnitude = (float)(Math.Atan(conf.BiasFac * Math.Abs(lateral) / 1000f /
|
||||
Math.Max(speed, 0.3f)) / Math.PI * 180.0);
|
||||
var biasSign = GetLateralBiasSign(baseCrabTh, cth, speed, gcpLimit, pathLeft, lateral);
|
||||
var rawBiasItem = rawBiasMagnitude * biasSign;
|
||||
var biasItem = ClampAbs(rawBiasItem, conf.BiasThreshold);
|
||||
var yawSplitSign = GetYawSplitSign(baseCrabTh + biasItem, speed, gcpLimit, controlRadius);
|
||||
var rawDthItem = conf.FleetCrabDthLinearFac * headingErr * yawSplitSign;
|
||||
var dthItem = ClampAbs(rawDthItem, conf.FleetCrabDthLinearThreshold);
|
||||
var rawFrontTh = baseCrabTh + biasItem + dthItem;
|
||||
var rawRearTh = baseCrabTh + biasItem - dthItem;
|
||||
ResolveCrabDriveEquivalent(speed, rawFrontTh, rawRearTh, gcpLimit, out var driveSpeed,
|
||||
out var frontTh, out var rearTh, out var reverseEquivalent, out var rawBaseTh);
|
||||
holdFrontTh = frontTh;
|
||||
holdRearTh = rearTh;
|
||||
var idealAlong = Clamp(along, 0f, pathLengthMm);
|
||||
var ideal = pathStart + pathDir * idealAlong;
|
||||
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = driveSpeed;
|
||||
self.MultiVehicleAutoFrontTh = frontTh;
|
||||
self.MultiVehicleAutoRearTh = rearTh;
|
||||
self.MultiVehicleAutoIdealX = ideal.X;
|
||||
self.MultiVehicleAutoIdealY = ideal.Y;
|
||||
self.MultiVehicleAutoIdealTh = targetBodyTh;
|
||||
self.MultiVehicleAutoHasIdeal = true;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
|
||||
if ((DateTime.Now - lastLog).TotalMilliseconds >= 300)
|
||||
{
|
||||
lastLog = DateTime.Now;
|
||||
var snap = self.GetFleetCenterSnapshot();
|
||||
int fleetCnt;
|
||||
lock (self.FleetLock) fleetCnt = self.MultiVehicleFleet.Count;
|
||||
DLog.Log(
|
||||
$"ITER#{iter} centerSrc={centerSource} center=({cx:0},{cy:0},{cth:0.0}) snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
|
||||
$"along={along:0} lateral={lateral:0} remain={remain:0} headingErr={headingErr:0.0} " +
|
||||
$"baseTh={baseCrabTh:0.0} bias={biasItem:0.0} dth={dthItem:0.0} " +
|
||||
$"slowRatio={slowRatio:0.000} targetV={targetSpeed:0.000} rampT={rampElapsed:0.0} accel={activeAccel:0.000} auto=(vx:{driveSpeed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " +
|
||||
$"ideal=({ideal.X:0},{ideal.Y:0},{targetBodyTh:0.0}) scriptEn={self.MultiVehicleScriptEnabled} " +
|
||||
$"cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
|
||||
"FleetCrabDbg");
|
||||
DLog.Log(
|
||||
$"CTRL iter={iter} centerSrc:{centerSource} phi:{phi:0.00} targetBody:{targetBodyTh:0.00} startTheta:{theta:0.00} " +
|
||||
$"cth:{cth:0.00} crabAngle:{CrabAngleDeg:0.00} bodyToPathCfg:{BodyToPathAngleDeg:0.00} " +
|
||||
$"targetBodyToPath:{targetBodyToPath:0.00} bodyToPathNow:{baseCrabTh:0.00} " +
|
||||
$"headingErr(target-current):{headingErr:0.00} reverse(current-target):{headingErrReverse:0.00} yawSign:{yawSplitSign:0} " +
|
||||
$"fleetCrabDthFac:{conf.FleetCrabDthLinearFac:0.000} rawDth:{rawDthItem:0.00} dth:{dthItem:0.00} dthLimit:{conf.FleetCrabDthLinearThreshold:0.00} " +
|
||||
$"lateral:{lateral:0.0} biasFac:{conf.BiasFac:0.000} biasSign:{biasSign:0} rawBias:{rawBiasItem:0.00} bias:{biasItem:0.00} biasLimit:{conf.BiasThreshold:0.00} " +
|
||||
$"baseTh:{baseCrabTh:0.00} rawBase:{rawBaseTh:0.00} rawOut(f:{rawFrontTh:0.00},r:{rawRearTh:0.00}) " +
|
||||
$"out(f:{frontTh:0.00},r:{rearTh:0.00}) gcpLimit:{gcpLimit:0.00} revEq:{reverseEquivalent} " +
|
||||
$"speedRaw:{speed:0.000} speed:{driveSpeed:0.000} rampT:{rampElapsed:0.0} accel:{activeAccel:0.000} along:{along:0.0} remain:{remain:0.0} ideal=({ideal.X:0.0},{ideal.Y:0.0},{targetBodyTh:0.00})",
|
||||
"FleetCrabHeadingDbg");
|
||||
}
|
||||
yield return true;
|
||||
}
|
||||
|
||||
if (_stopping)
|
||||
stopReason = "stop";
|
||||
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = holdFrontTh;
|
||||
self.MultiVehicleAutoRearTh = holdRearTh;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
DLog.Log(
|
||||
$"STOP_HOLD iter={iter} reason={stopReason} hold=(fTh:{holdFrontTh:0.0},rTh:{holdRearTh:0.0}) cmdSpeed={cmdSpeed:0.000}",
|
||||
"FleetCrabDbg");
|
||||
var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
|
||||
while (!_stopping && DateTime.Now < settleEnd)
|
||||
{
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = holdFrontTh;
|
||||
self.MultiVehicleAutoRearTh = holdRearTh;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
Cleanup();
|
||||
Hedingben.ToastText("车队蟹行完成", "FleetCrab");
|
||||
DLog.Log($"DONE iter={iter} reason={stopReason}", "FleetCrabDbg");
|
||||
}
|
||||
}
|
||||
@@ -1,411 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Globalization;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using CommonUsage.Chassis;
|
||||
using CommonUsage.Mathematics;
|
||||
using MDCSToolBox.Clumsy.MotionControllers;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class FleetCurveWalk : MovementDefinition
|
||||
{
|
||||
public BezierTrack Track;
|
||||
public List<Vector2> ControlPoints = new();
|
||||
public float CurveSpeed = 0.2f;
|
||||
public float CarDirectionBias = 0f;
|
||||
public int BezierResolution = 100;
|
||||
public float SlowDistance = 2000f;
|
||||
public float FinishDistance = 20f;
|
||||
public float FinishSpeed = 0.02f;
|
||||
public float SlowingPow = 0.8f;
|
||||
public float GcpThetaThreshold = 95f;
|
||||
public float StartSyncTimeoutSec = 8f;
|
||||
|
||||
private bool _stopping;
|
||||
private MultiWheelGeometricController _controller;
|
||||
private MultiWheelChassis _chassis;
|
||||
private bool _savedControlPoints;
|
||||
private float _savedControlRadius;
|
||||
private Vector2 _savedGcp0;
|
||||
private Vector2 _savedGcp1;
|
||||
|
||||
public void Stop()
|
||||
{
|
||||
_stopping = true;
|
||||
if (_controller != null)
|
||||
_controller.BreakAndHold = true;
|
||||
Cleanup();
|
||||
}
|
||||
|
||||
public static bool TryParsePointList(string text, out List<Vector2> points, out string error)
|
||||
{
|
||||
points = new List<Vector2>();
|
||||
error = "";
|
||||
if (string.IsNullOrWhiteSpace(text))
|
||||
{
|
||||
error = "empty control point list";
|
||||
return false;
|
||||
}
|
||||
|
||||
var segments = text.Split(new[] { ';', '|' }, StringSplitOptions.RemoveEmptyEntries);
|
||||
for (var i = 0; i < segments.Length; i++)
|
||||
{
|
||||
var pair = segments[i].Split(new[] { ',', ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
|
||||
if (pair.Length != 2)
|
||||
{
|
||||
error = $"invalid point #{i + 1}: {segments[i]}";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!TryParseFloat(pair[0], out var x) || !TryParseFloat(pair[1], out var y))
|
||||
{
|
||||
error = $"invalid number in point #{i + 1}: {segments[i]}";
|
||||
return false;
|
||||
}
|
||||
points.Add(new Vector2(x, y));
|
||||
}
|
||||
|
||||
if (points.Count < 3)
|
||||
{
|
||||
error = "Bezier curve requires at least 3 control points";
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public static List<Vector2> BuildRelativeControlPoints(Vector2 start, float startTh, List<Vector2> relativePoints)
|
||||
{
|
||||
var source = relativePoints ?? new List<Vector2>();
|
||||
var normalized = new List<Vector2>();
|
||||
if (source.Count == 0 || Vector2.Distance(source[0], Vector2.Zero) > 1f)
|
||||
normalized.Add(Vector2.Zero);
|
||||
for (var i = 0; i < source.Count; i++)
|
||||
normalized.Add(source[i]);
|
||||
if (normalized.Count < 2)
|
||||
normalized.Add(new Vector2(1000f, 0f));
|
||||
if (normalized.Count < 3)
|
||||
normalized.Add(new Vector2(2000f, 0f));
|
||||
|
||||
var result = new List<Vector2>();
|
||||
for (var i = 0; i < normalized.Count; i++)
|
||||
result.Add(CommonMath.Transform2D(start, startTh, normalized[i]));
|
||||
return result;
|
||||
}
|
||||
|
||||
public static List<Vector2> BuildAgvControlPoints(float srcX, float srcY, float dstX, float dstY,
|
||||
params float[] controlPointCoords)
|
||||
{
|
||||
var src = new Vector2(srcX, srcY);
|
||||
var dst = new Vector2(dstX, dstY);
|
||||
var result = new List<Vector2>();
|
||||
if (controlPointCoords == null || controlPointCoords.Length == 0)
|
||||
{
|
||||
result.Add(src);
|
||||
result.Add((src + dst) / 2f);
|
||||
result.Add(dst);
|
||||
return result;
|
||||
}
|
||||
|
||||
if (controlPointCoords.Length % 2 != 0)
|
||||
throw new ArgumentException("FleetCurve controlPointCoords must contain x,y pairs.");
|
||||
|
||||
var supplied = new List<Vector2>();
|
||||
for (var i = 0; i < controlPointCoords.Length; i += 2)
|
||||
supplied.Add(new Vector2(controlPointCoords[i], controlPointCoords[i + 1]));
|
||||
|
||||
if (supplied.Count >= 3 &&
|
||||
Vector2.Distance(supplied[0], src) <= 10f &&
|
||||
Vector2.Distance(supplied[supplied.Count - 1], dst) <= 10f)
|
||||
return supplied;
|
||||
|
||||
result.Add(src);
|
||||
for (var i = 0; i < supplied.Count; i++)
|
||||
result.Add(supplied[i]);
|
||||
result.Add(dst);
|
||||
if (result.Count < 3)
|
||||
result.Insert(1, (src + dst) / 2f);
|
||||
return result;
|
||||
}
|
||||
|
||||
private static bool TryParseFloat(string text, out float value)
|
||||
{
|
||||
return float.TryParse(text, NumberStyles.Float, CultureInfo.InvariantCulture, out value) ||
|
||||
float.TryParse(text, out value);
|
||||
}
|
||||
|
||||
private static float ClampAbs(float value, float limit)
|
||||
{
|
||||
var absLimit = Math.Abs(limit);
|
||||
if (absLimit <= 0) return value;
|
||||
if (value > absLimit) return absLimit;
|
||||
if (value < -absLimit) return -absLimit;
|
||||
return value;
|
||||
}
|
||||
|
||||
private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
|
||||
out float centerTh, out string source)
|
||||
{
|
||||
if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
|
||||
{
|
||||
source = "fleet";
|
||||
return true;
|
||||
}
|
||||
|
||||
if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
|
||||
{
|
||||
source = "slam";
|
||||
return true;
|
||||
}
|
||||
|
||||
source = "none";
|
||||
return false;
|
||||
}
|
||||
|
||||
private void Cleanup()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = 0;
|
||||
self.MultiVehicleAutoRearTh = 0;
|
||||
self.MultiVehicleAutoHasIdeal = false;
|
||||
self.MultiVehicleAutoEnabled = false;
|
||||
RestoreControlPointRadius();
|
||||
}
|
||||
|
||||
private void ApplyFleetControlPointRadius(MultiWheelChassis chassis, float radius)
|
||||
{
|
||||
if (!_savedControlPoints)
|
||||
{
|
||||
_chassis = chassis;
|
||||
_savedControlRadius = chassis.ControlPointRadius;
|
||||
var gcps = chassis.GetGeometricControlPoints();
|
||||
if (gcps.Count >= 2)
|
||||
{
|
||||
_savedGcp0 = gcps[0].Position;
|
||||
_savedGcp1 = gcps[1].Position;
|
||||
}
|
||||
_savedControlPoints = true;
|
||||
}
|
||||
|
||||
chassis.ControlPointRadius = radius;
|
||||
var points = chassis.GetGeometricControlPoints();
|
||||
if (points.Count >= 2)
|
||||
{
|
||||
points[0].Position = new Vector2(radius, 0);
|
||||
points[1].Position = new Vector2(-radius, 0);
|
||||
}
|
||||
}
|
||||
|
||||
private void RestoreControlPointRadius()
|
||||
{
|
||||
if (!_savedControlPoints || _chassis == null)
|
||||
return;
|
||||
|
||||
_chassis.ControlPointRadius = _savedControlRadius;
|
||||
var points = _chassis.GetGeometricControlPoints();
|
||||
if (points.Count >= 2)
|
||||
{
|
||||
points[0].Position = _savedGcp0;
|
||||
points[1].Position = _savedGcp1;
|
||||
}
|
||||
_savedControlPoints = false;
|
||||
}
|
||||
|
||||
private static void WriteWarmupAuto(PilotDefinition self, Vector2 idealPos, float idealTh,
|
||||
float frontTh, float rearTh)
|
||||
{
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = frontTh;
|
||||
self.MultiVehicleAutoRearTh = rearTh;
|
||||
self.MultiVehicleAutoIdealX = idealPos.X;
|
||||
self.MultiVehicleAutoIdealY = idealPos.Y;
|
||||
self.MultiVehicleAutoIdealTh = idealTh;
|
||||
self.MultiVehicleAutoHasIdeal = true;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
}
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
var conf = PilotDefinition.Conf;
|
||||
var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
|
||||
_stopping = false;
|
||||
|
||||
if (chassis == null)
|
||||
{
|
||||
DLog.Log("ABORT: FleetCurveWalk requires MultiWheelChassis.", "FleetCurveDbg");
|
||||
yield break;
|
||||
}
|
||||
|
||||
if (conf.MultiVehicleMasterEndpoint != "/")
|
||||
{
|
||||
DLog.Log($"ABORT: FleetCurveWalk must run on master endpoint, endpoint={conf.MultiVehicleMasterEndpoint}",
|
||||
"FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve requires master vehicle", "FleetCurve");
|
||||
yield break;
|
||||
}
|
||||
|
||||
if (Track == null && (ControlPoints == null || ControlPoints.Count < 3))
|
||||
{
|
||||
DLog.Log("ABORT: FleetCurveWalk requires a BezierTrack or at least 3 control points.", "FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve requires track or >=3 control points", "FleetCurve");
|
||||
yield break;
|
||||
}
|
||||
|
||||
if (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
|
||||
{
|
||||
DLog.Log("ABORT: FleetCurveWalk failed to read fleet center.", "FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
|
||||
yield break;
|
||||
}
|
||||
|
||||
var baseSpeed = Math.Abs(CurveSpeed);
|
||||
if (baseSpeed <= 1e-4f)
|
||||
{
|
||||
DLog.Log("ABORT: FleetCurveWalk speed is zero.", "FleetCurveDbg");
|
||||
yield break;
|
||||
}
|
||||
|
||||
var resolution = Math.Max(2, BezierResolution);
|
||||
var speedFinish = Math.Min(baseSpeed, Math.Abs(FinishSpeed));
|
||||
var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
|
||||
var controlRadius = Math.Max(1f, Math.Abs(conf.TestCarSyncDistance) / 2f);
|
||||
|
||||
ApplyFleetControlPointRadius(chassis, controlRadius);
|
||||
|
||||
try
|
||||
{
|
||||
var track = Track;
|
||||
var trackSource = "external";
|
||||
if (track == null)
|
||||
{
|
||||
var points = new List<Vector2>(ControlPoints);
|
||||
track = new BezierTrack(points, resolution);
|
||||
trackSource = "controlPoints";
|
||||
}
|
||||
track.CarDirectionBias = CarDirectionBias;
|
||||
track.Speed = baseSpeed;
|
||||
|
||||
var center = new Vector2(x0, y0);
|
||||
var (idealPos, idealAngle, bias, pd) = track.QueryTangentPoint(center);
|
||||
var carDirection = (float)CommonMath.ThDiff(theta, CarDirectionBias);
|
||||
var holdTh = ClampAbs((float)CommonMath.ThDiff(idealAngle, carDirection), gcpLimit);
|
||||
var targetBodyTh = (float)CommonMath.RoundTh(idealAngle + CarDirectionBias);
|
||||
|
||||
DLog.Log(
|
||||
$"START center=({x0:0},{y0:0},{theta:0.0}) source={initialCenterSource} " +
|
||||
$"track={track.GetType().Name} trackSource={trackSource} controls={ControlPoints?.Count ?? 0} " +
|
||||
$"len={track.Length():0} speed={baseSpeed:0.000} bias={CarDirectionBias:0.0} " +
|
||||
$"query=({idealPos.X:0},{idealPos.Y:0}) tangent={idealAngle:0.0} targetBody={targetBodyTh:0.0} " +
|
||||
$"pathBias={bias:0.0} pd={pd:0.0} hold={holdTh:0.0} radius={controlRadius:0}",
|
||||
"FleetCurveDbg");
|
||||
|
||||
var warmStart = DateTime.Now;
|
||||
var warmSeqBaseline = self.BeginFleetMotionWarmup();
|
||||
WriteWarmupAuto(self, idealPos, targetBodyTh, holdTh, holdTh);
|
||||
self.PrimeMasterAutoFromSlam();
|
||||
|
||||
var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, StartSyncTimeoutSec));
|
||||
var warmIter = 0;
|
||||
var warmReady = false;
|
||||
var warmDetail = "";
|
||||
while (!_stopping && DateTime.Now < warmEnd)
|
||||
{
|
||||
warmIter++;
|
||||
WriteWarmupAuto(self, idealPos, targetBodyTh, holdTh, holdTh);
|
||||
self.PrimeMasterAutoFromSlam();
|
||||
|
||||
if (warmIter % 5 == 0)
|
||||
{
|
||||
var snap = self.GetFleetCenterSnapshot();
|
||||
int cnt;
|
||||
lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
|
||||
DLog.Log(
|
||||
$"WARMUP#{warmIter} snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
|
||||
$"cnt={cnt}/{conf.MultiVehicleFleetNum} detail={warmDetail}",
|
||||
"FleetCurveDbg");
|
||||
}
|
||||
|
||||
if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
|
||||
conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
|
||||
{
|
||||
warmReady = true;
|
||||
DLog.Log($"WARMUP done iter={warmIter} detail={warmDetail}", "FleetCurveDbg");
|
||||
break;
|
||||
}
|
||||
yield return true;
|
||||
}
|
||||
|
||||
if (!warmReady)
|
||||
{
|
||||
DLog.Log($"WARMUP timeout: fleet startup sync failed, abort curve action. detail={warmDetail}",
|
||||
"FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve startup sync timeout", "FleetCurve");
|
||||
Cleanup();
|
||||
yield break;
|
||||
}
|
||||
|
||||
_controller = new ChassisController { BaseSpeed = baseSpeed }.Get();
|
||||
_controller.MultiVehicleSync = true;
|
||||
_controller.BaseSpeed = baseSpeed;
|
||||
_controller.SlowDistance = Math.Max(FinishDistance + 1f, SlowDistance);
|
||||
_controller.FinishDistance = Math.Max(0f, FinishDistance);
|
||||
_controller.FinishSpeed = speedFinish;
|
||||
_controller.SlowingPow = Math.Max(0.01f, SlowingPow);
|
||||
_controller.GcpThetaThreshold = gcpLimit;
|
||||
_controller.AddTrack(track, "FleetCurve");
|
||||
|
||||
Hedingben.ToastText($"FleetCurve len {track.Length():0}mm speed {baseSpeed:0.00}", "FleetCurve");
|
||||
|
||||
foreach (var running in _controller.Track())
|
||||
{
|
||||
if (_stopping)
|
||||
break;
|
||||
if (!running)
|
||||
break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
if (!_stopping)
|
||||
{
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
|
||||
while (!_stopping && DateTime.Now < settleEnd)
|
||||
{
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
|
||||
DLog.Log($"DONE stopping={_stopping}", "FleetCurveDbg");
|
||||
}
|
||||
finally
|
||||
{
|
||||
Cleanup();
|
||||
_controller = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,606 +0,0 @@
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using ClumsyCore.Utilities;
|
||||
using ClumsyDance.ClumsyDance.Detectors;
|
||||
using ClumsyDance.ClumsyWalk.Detectors;
|
||||
using CommonUsage.Chassis;
|
||||
using CommonUsage.Mathematics;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Calibration;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using System.Security.Cryptography;
|
||||
using System.Threading;
|
||||
using LineSegment = ClumsyCore.Utilities.LineSegment;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
[MovementTest(name = "轮胎检测")]
|
||||
public class TireDetect : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
_running = false;
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_painter = UI.GetPainter("TwoLegDetectTest", false);
|
||||
_painter.Clear();
|
||||
|
||||
var frontlidar = UI.GetInput("1是用前雷达识别，2是用后雷达识别");
|
||||
var result = int.Parse(frontlidar.ToString());
|
||||
var lastDetectX = result == 1 ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX;
|
||||
var lastDetectY = 0f;
|
||||
while (_running)
|
||||
{
|
||||
var ld = Detect(lastDetectX, SetFilters(lastDetectX, lastDetectY), result == 1 ? true : false);
|
||||
if(ld == null)
|
||||
{
|
||||
//Console.WriteLine("ld == null");
|
||||
continue;
|
||||
}
|
||||
_painter.Clear();
|
||||
var center = (ld.Src + ld.Dst) / 2;
|
||||
var distanceToCarOrigin = Vector2.Distance(Vector2.Zero, center);
|
||||
var distanceLabelPos = center / 2;
|
||||
_painter.DrawLine(Color.Cyan, Vector2.Zero, center, width: 2);
|
||||
_painter.DrawText(Color.Yellow, $"{distanceToCarOrigin:F3}", distanceLabelPos.X, distanceLabelPos.Y);
|
||||
lastDetectX = center.X;
|
||||
lastDetectY = center.Y;
|
||||
Thread.Sleep(100);
|
||||
}
|
||||
}
|
||||
|
||||
public static LineSegment Detect(float guessX, List<DetectFilter> filters, bool frontlidar)
|
||||
{
|
||||
return new Lidar2dDetect2LegTray()
|
||||
{
|
||||
BlobDist = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegBlobDist : PilotDefinition.Conf.TireBackTwoLegBlobDist,
|
||||
BlobPtCount = frontlidar ? PilotDefinition.Conf.TireTwoLegBlobPtCount : PilotDefinition.Conf.TireTwoLegBlobPtCount,
|
||||
BlobSize = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegBlobSize : PilotDefinition.Conf.TireBackTwoLegBlobSize,
|
||||
CenterChange = Tuple.Create(frontlidar ? PilotDefinition.Conf.TireFrontTwoLegCenterChangeX : PilotDefinition.Conf.TireBackTwoLegCenterChangeX, 0f, 0f),
|
||||
LegWidth = PilotDefinition.Conf.TireTwoLegWidth,
|
||||
LegWidthErr = frontlidar ? PilotDefinition.Conf.TireTwoLegWidthErr : PilotDefinition.Conf.TireTwoLegWidthErr,
|
||||
Padding = frontlidar ? PilotDefinition.Conf.TireFrontPadding : PilotDefinition.Conf.TireBackPadding,
|
||||
PillarFindingScope = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegPillarFindingScope : PilotDefinition.Conf.TireBackTwoLegPillarFindingScope,
|
||||
SgnDir = PilotDefinition.Conf.TwoLegSgnDir,
|
||||
}.DetectWithGuess(frontlidar ? "frontlidar" : "leftlidar,rightlidar", new LineSegment(new Vector2(guessX, 0), Vector2.Zero),
|
||||
guessCoordinateSystem: CoordinateSystem.Car2D, outCoordinateSystem: CoordinateSystem.Car2D, filters);
|
||||
}
|
||||
|
||||
private List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
|
||||
{
|
||||
var painter = UI.GetPainter("GeneralFollowing.SetFilters", false);
|
||||
painter.Clear();
|
||||
painter.Clear(3000);
|
||||
|
||||
var box = new Vector2[]
|
||||
{
|
||||
new (guessCenterX - PilotDefinition.Conf.TireFilterLength / 2, guessCenterY - PilotDefinition.Conf.TireFilterWidth / 2),
|
||||
new (guessCenterX + PilotDefinition.Conf.TireFilterLength / 2, guessCenterY - PilotDefinition.Conf.TireFilterWidth / 2),
|
||||
new (guessCenterX + PilotDefinition.Conf.TireFilterLength / 2, guessCenterY + PilotDefinition.Conf.TireFilterWidth / 2),
|
||||
new (guessCenterX - PilotDefinition.Conf.TireFilterLength / 2, guessCenterY + PilotDefinition.Conf.TireFilterWidth / 2),
|
||||
};
|
||||
|
||||
for (var i = 0; i < box.Length; ++i)
|
||||
painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
|
||||
|
||||
// PC filter in car coordinate frame
|
||||
return new List<DetectFilter>()
|
||||
{
|
||||
new(CoordinateSystem.Car2D,
|
||||
p => LessMath.IsPointInPolygon4(
|
||||
box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))),
|
||||
};
|
||||
}
|
||||
|
||||
private Painter _painter;
|
||||
private bool _running = true;
|
||||
}
|
||||
|
||||
[MovementTest(name = "钻车测试")]
|
||||
public class FollowTire : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
var front = UI.GetInput("1是用前雷达识别，2是用后雷达识别");
|
||||
var result = int.Parse(front.ToString());
|
||||
var lidarname = result == 1 ? "前雷达" : "后雷达";
|
||||
DLog.Log($"开始钻车测试，用{lidarname}识别", "TireFollowing");
|
||||
|
||||
var following = new TireFollowing()
|
||||
{
|
||||
GetController = () => new ChassisController().Get(),
|
||||
GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
|
||||
GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
|
||||
detectors = new List<TireFollowing.DetectorDefinition>()
|
||||
{
|
||||
new TireFollowing.DetectorDefinition()
|
||||
{
|
||||
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, result == 1 ? true : false),
|
||||
StartGuessingX = result == 1 ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX,
|
||||
StartGuessingY = 0,
|
||||
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
|
||||
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||||
PathTransformation = new Tuple<float, float, float>(
|
||||
result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
|
||||
result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||
0)
|
||||
},
|
||||
new TireFollowing.DetectorDefinition()
|
||||
{
|
||||
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, result == 1 ? true : false),
|
||||
StartGuessingX = result == 1 ? PilotDefinition.Conf.TireFollowingStage2GuessX : -PilotDefinition.Conf.TireFollowingStage2GuessX,
|
||||
StartGuessingY = 0,
|
||||
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
|
||||
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||||
PathTransformation = new Tuple<float, float, float>(
|
||||
result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
|
||||
result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||
0)
|
||||
},
|
||||
},
|
||||
CarDirection = result == 1 ? 0f : 180f,
|
||||
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
|
||||
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
|
||||
TireNum = PilotDefinition.Conf.TireFollowingTireNum,
|
||||
WalkBlindTh = result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarWalkBlindTh : PilotDefinition.Conf.TireFollowingBackLidarWalkBlindTh,
|
||||
};
|
||||
_dt = new DriveTask(following.Get());
|
||||
_dt.Wait();
|
||||
DLog.Log($"结束钻车测试", "TireFollowing");
|
||||
}
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "离车测试")]
|
||||
public class LeaveCar : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
DLog.Log($"开始离车测试，用后雷达识别", "TireFollowing");
|
||||
var following = new TireFollowing()
|
||||
{
|
||||
GetController = () => new ChassisController().Get(),
|
||||
GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
|
||||
GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
|
||||
detectors = new List<TireFollowing.DetectorDefinition>()
|
||||
{
|
||||
new TireFollowing.DetectorDefinition()
|
||||
{
|
||||
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
|
||||
StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
|
||||
StartGuessingY = 0,
|
||||
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingLeaveCarWalkBlindSwitchingDistance,
|
||||
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||||
PathTransformation = new Tuple<float, float, float>(
|
||||
PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
|
||||
PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||
0)
|
||||
},
|
||||
},
|
||||
CarDirection = 180f,
|
||||
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
|
||||
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
|
||||
WalkBlindTh = 0,
|
||||
TireNum = 1
|
||||
};
|
||||
_dt = new DriveTask(following.Get());
|
||||
_dt.Wait();
|
||||
DLog.Log($"结束离车测试", "TireFollowing");
|
||||
}
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "抱夹关闭")]
|
||||
public class ClampTest1 : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
PilotDefinition.Self.SpeedLeftArm = 0;
|
||||
PilotDefinition.Self.SpeedRightArm = 0;
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_dt = new DriveTask(new ClampToTarget()
|
||||
{
|
||||
LeftClampTarget = PilotDefinition.Self.LeftArmUpperPos,
|
||||
RightClampTarget = PilotDefinition.Self.RightArmUpperPos
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "抱夹打开")]
|
||||
public class ClampTest2 : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
PilotDefinition.Self.SpeedLeftArm = 0;
|
||||
PilotDefinition.Self.SpeedRightArm = 0;
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_dt = new DriveTask(new ClampToTarget()
|
||||
{
|
||||
LeftClampTarget = PilotDefinition.Self.LeftArmLowerPos,
|
||||
RightClampTarget = PilotDefinition.Self.RightArmLowerPos
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试前进基于轮里程")]
|
||||
public class LineTrackingTest : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
}
|
||||
public override void Test()
|
||||
{
|
||||
_dt = new DriveTask(new LineTracking()
|
||||
{
|
||||
Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试后退基于轮里程")]
|
||||
public class ReverseLineTrackingTest : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
}
|
||||
public override void Test()
|
||||
{
|
||||
_dt = new DriveTask(new LineTracking()
|
||||
{
|
||||
Target = -PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试终点跟踪动作-前进")]
|
||||
public class DstTrackerForward : MovementTest
|
||||
{
|
||||
public bool UseInteractivePick = true;
|
||||
public float srcX;
|
||||
public float srcY;
|
||||
public float dstX;
|
||||
public float dstY;
|
||||
public float carDirectionBias = 0f;
|
||||
private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
_painter?.Clear();
|
||||
}
|
||||
public override void Test()
|
||||
{
|
||||
var p1 = UI.GetPoint("point1");
|
||||
var p2 = UI.GetPoint("point2");
|
||||
|
||||
_painter.Clear();
|
||||
_dt = new DriveTask(new DstTracker()
|
||||
{
|
||||
Src = p1,
|
||||
Dst = p2,
|
||||
CarDirectionBias = carDirectionBias,
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试终点跟踪动作-后退")]
|
||||
public class DstTrackerhoutui : MovementTest
|
||||
{
|
||||
public bool UseInteractivePick = true;
|
||||
public float srcX;
|
||||
public float srcY;
|
||||
public float dstX;
|
||||
public float dstY;
|
||||
public float carDirectionBias = 180f;
|
||||
private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
_painter?.Clear();
|
||||
}
|
||||
public override void Test()
|
||||
{
|
||||
var p1 = UI.GetPoint("point1");
|
||||
var p2 = UI.GetPoint("point2");
|
||||
|
||||
_painter.Clear();
|
||||
_dt = new DriveTask(new DstTracker()
|
||||
{
|
||||
Src = p1,
|
||||
Dst = p2,
|
||||
CarDirectionBias = carDirectionBias,
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试先直行再终点跟踪")]
|
||||
public class LineTrackThenDstTrackerTest : MovementTest
|
||||
{
|
||||
public float carDirectionBias = 0f;
|
||||
private readonly Painter _painter = UI.GetPainter("LineTrackThenDstTrackerTest");
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
_painter?.Clear();
|
||||
}
|
||||
public override void Test()
|
||||
{
|
||||
var src = UI.GetPoint("请在上位机选择起点(src)");
|
||||
var dst = UI.GetPoint("请在上位机选择终点(dst)");
|
||||
_painter.Clear();
|
||||
_painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
|
||||
_painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
|
||||
_painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
|
||||
_painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
|
||||
_painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
|
||||
IEnumerable<bool> TrackThenFollow()
|
||||
{
|
||||
foreach (var running in new LineTracking()
|
||||
{
|
||||
Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
|
||||
EnableHandover = true,
|
||||
HandoverDistance = 200f,
|
||||
HandoverSpeed = 0.3f,
|
||||
}.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
foreach (var running in new DstTracker()
|
||||
{
|
||||
Src = src,
|
||||
Dst = dst,
|
||||
CarDirectionBias = carDirectionBias,
|
||||
InitialSendSpeed = 0.3f
|
||||
}.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
_dt = new DriveTask(TrackThenFollow());
|
||||
_dt.Wait();
|
||||
}
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试先离车再终点跟踪")]
|
||||
public class LeaveCarThenDstTrackerTest : MovementTest
|
||||
{
|
||||
private readonly Painter _painter = UI.GetPainter("LeaveCarThenDstTrackerTest");
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
_painter?.Clear();
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
var src = UI.GetPoint("请在上位机选择离车后起点(src)");
|
||||
var dst = UI.GetPoint("请在上位机选择终点(dst)");
|
||||
|
||||
_painter.Clear();
|
||||
_painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
|
||||
_painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
|
||||
_painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
|
||||
_painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
|
||||
_painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
|
||||
|
||||
IEnumerable<bool> LeaveThenFollow()
|
||||
{
|
||||
var following = new TireFollowing()
|
||||
{
|
||||
GetController = () => new ChassisController().Get(),
|
||||
GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
|
||||
GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
|
||||
detectors = new List<TireFollowing.DetectorDefinition>()
|
||||
{
|
||||
new TireFollowing.DetectorDefinition()
|
||||
{
|
||||
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
|
||||
StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
|
||||
StartGuessingY = 0,
|
||||
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
|
||||
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||||
PathTransformation = new Tuple<float, float, float>(
|
||||
PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
|
||||
PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||
0),
|
||||
},
|
||||
},
|
||||
CarDirection = 180f,
|
||||
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
|
||||
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
|
||||
WalkBlindTh = 0,
|
||||
TireNum = 1
|
||||
};
|
||||
|
||||
foreach (var running in following.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
foreach (var running in new DstTracker()
|
||||
{
|
||||
Src = src,
|
||||
Dst = dst,
|
||||
CarDirectionBias = 180f,
|
||||
}.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
yield return false;
|
||||
}
|
||||
|
||||
_dt = new DriveTask(LeaveThenFollow());
|
||||
_dt.Wait();
|
||||
}
|
||||
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
|
||||
[MovementTest(name = "驱动器下使能测试")]
|
||||
public class DriverDisableTest : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
new DriveTask(new DriverDisable(){ }.Get()).Wait();
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "驱动器复位测试")]
|
||||
public class DriverAbleTest : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
new DriveTask(new DriverAble(){ }.Get()).Wait();
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "底盘旋转测试")]
|
||||
public class RotateToAngleTest : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
var target = UI.GetInput("输入旋转角度：");
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
new DriveTask(new MultiWheelRotateInPlace()
|
||||
{
|
||||
AngleTarget = float.Parse(target),
|
||||
PidparamsRead = () => new PIDParams()
|
||||
{
|
||||
Kp = PilotDefinition.Conf.TireFollowingThkp,
|
||||
Ki = PilotDefinition.Conf.TireFollowingThki,
|
||||
Kd = PilotDefinition.Conf.TireFollowingThkd,
|
||||
DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone,
|
||||
SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec,
|
||||
OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh,
|
||||
MaxI = PilotDefinition.Conf.TireFollowingThMaxI,
|
||||
}
|
||||
}.Get()).Wait();
|
||||
}
|
||||
}
|
||||
|
||||
public class utils
|
||||
{
|
||||
public static List<(float x, float y, float th)> RemoveOutliers(List<(float x, float y, float th)> data, float threshold = 2.0f)
|
||||
{
|
||||
var means = CalculateMean(data);
|
||||
var stdDevs = CalculateStandardDeviation(data, means);
|
||||
|
||||
return data.Where(point =>
|
||||
Math.Abs(point.x - means.x) <= threshold * stdDevs.x &&
|
||||
Math.Abs(point.y - means.y) <= threshold * stdDevs.y &&
|
||||
AngularDistance(point.th, means.th) <= threshold * stdDevs.th
|
||||
).ToList();
|
||||
}
|
||||
|
||||
public static (float x, float y, float th) CalculateMean(List<(float x, float y, float th)> data)
|
||||
{
|
||||
float meanX = data.Average(point => point.x);
|
||||
float meanY = data.Average(point => point.y);
|
||||
float sinSum = data.Sum(point => (float)Math.Sin(DegreeToRadian(point.th)));
|
||||
float cosSum = data.Sum(point => (float)Math.Cos(DegreeToRadian(point.th)));
|
||||
float meanTh = RadianToDegree((float)Math.Atan2(sinSum, cosSum));
|
||||
|
||||
return (meanX, meanY, meanTh);
|
||||
}
|
||||
|
||||
public static (float x, float y, float th) CalculateStandardDeviation(List<(float x, float y, float th)> data, (float x, float y, float th) means)
|
||||
{
|
||||
float varianceX = data.Average(point => (point.x - means.x) * (point.x - means.x));
|
||||
float varianceY = data.Average(point => (point.y - means.y) * (point.y - means.y));
|
||||
|
||||
// 计算角度的方差
|
||||
float varianceTh = data.Average(point => AngularDistance(point.th, means.th) * AngularDistance(point.th, means.th));
|
||||
|
||||
return ((float)Math.Sqrt(varianceX), (float)Math.Sqrt(varianceY), (float)Math.Sqrt(varianceTh));
|
||||
}
|
||||
|
||||
public static float DegreeToRadian(float degree)
|
||||
{
|
||||
return (float)(degree * Math.PI / 180.0);
|
||||
}
|
||||
|
||||
public static float RadianToDegree(float radian)
|
||||
{
|
||||
return (float)(radian * 180.0 / Math.PI);
|
||||
}
|
||||
|
||||
public static float AngularDistance(float angle1, float angle2)
|
||||
{
|
||||
return CommonMath.ThDiff(angle1, angle2);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -1,136 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using System.Threading;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Pilot;
|
||||
using ClumsyCore.Utilities;
|
||||
using ClumsyDance.ClumsyDance.Detectors;
|
||||
using ClumsyDance.ClumsyWalk.Detectors;
|
||||
using FundamentalLib;
|
||||
using LineSegment = ClumsyCore.Utilities.LineSegment;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
/// <summary>
|
||||
/// 2腿检测：用单线激光雷达识别两腿托盘/轮胎，按上一帧结果作为下一帧猜测做闭环检测。
|
||||
/// 从 StandardMultiWheelLifter 移植；参数全部走 PilotConfig（Fields 面板），雷达选择改为配置项而非阻塞输入。
|
||||
/// </summary>
|
||||
[MovementTest(name = "多舵轮-2腿检测")]
|
||||
public class TwoLegDetect : MovementTest
|
||||
{
|
||||
private Painter _painter;
|
||||
private bool _running = true;
|
||||
|
||||
public override void TestStop() => _running = false;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_running = true;
|
||||
_painter = UI.GetPainter("MultiWheelTwoLegDetect", false);
|
||||
_painter.Clear();
|
||||
|
||||
var lidar = PilotDefinition.Conf.TwoLegLidarName;
|
||||
var lastDetectX = PilotDefinition.Conf.TwoLegGuessX;
|
||||
var lastDetectY = 0f;
|
||||
|
||||
while (_running)
|
||||
{
|
||||
var ld = Detect(lidar, lastDetectX, SetFilters(lastDetectX, lastDetectY));
|
||||
if (ld == null)
|
||||
{
|
||||
Thread.Sleep(100);
|
||||
continue;
|
||||
}
|
||||
|
||||
var center = (ld.Src + ld.Dst) / 2;
|
||||
var distanceToCarOrigin = Vector2.Distance(Vector2.Zero, center);
|
||||
|
||||
_painter.Clear();
|
||||
_painter.DrawLine(Color.Cyan, Vector2.Zero, center, width: 2);
|
||||
_painter.DrawText(Color.Yellow, $"{distanceToCarOrigin:F1}", center.X / 2, center.Y / 2);
|
||||
|
||||
Hedingben.ToastText(
|
||||
$"[Test检测] lidar:{lidar} guess x:{lastDetectX:F0} y:{lastDetectY:F0} | " +
|
||||
$"中心 x:{center.X:F0} y:{center.Y:F0} dist:{distanceToCarOrigin:F0}",
|
||||
"MultiWheelTwoLegDetect-test");
|
||||
|
||||
// 用本帧中心作为下一帧猜测，实现闭环跟踪
|
||||
lastDetectX = center.X;
|
||||
lastDetectY = center.Y;
|
||||
Thread.Sleep(100);
|
||||
}
|
||||
|
||||
_painter.Clear();
|
||||
}
|
||||
|
||||
/// <summary>在车体坐标系下，按猜测位置检测两腿，返回连接两腿的线段（车体系）。</summary>
|
||||
public static LineSegment Detect(string lidarName, float guessX, List<DetectFilter> filters)
|
||||
{
|
||||
var conf = PilotDefinition.Conf;
|
||||
#pragma warning disable CS0612, CS0618
|
||||
var detector = new Lidar2dDetect2LegTray
|
||||
{
|
||||
BlobDist = conf.TwoLegBlobDist,
|
||||
BlobPtCount = conf.TwoLegBlobPtCount,
|
||||
BlobSize = conf.TwoLegBlobSize,
|
||||
CenterChange = Tuple.Create(conf.TwoLegCenterChangeX, 0f, 0f),
|
||||
LegWidth = conf.TwoLegWidth,
|
||||
LegWidthErr = conf.TwoLegWidthErr,
|
||||
Padding = conf.TwoLegPadding,
|
||||
PillarFindingScope = conf.TwoLegPillarFindingScope,
|
||||
SgnDir = conf.TwoLegSgnDir,
|
||||
};
|
||||
#pragma warning restore CS0612, CS0618
|
||||
|
||||
var result = detector.DetectWithGuess(
|
||||
lidarName,
|
||||
new LineSegment(new Vector2(guessX, 0), Vector2.Zero),
|
||||
guessCoordinateSystem: CoordinateSystem.Car2D,
|
||||
outCoordinateSystem: CoordinateSystem.Car2D,
|
||||
filters);
|
||||
return ApplyOutputBias(result);
|
||||
}
|
||||
|
||||
private static LineSegment ApplyOutputBias(LineSegment result)
|
||||
{
|
||||
if (result == null) return null;
|
||||
|
||||
var conf = PilotDefinition.Conf;
|
||||
if (Math.Abs(conf.TwoLegOutputBiasX) < 1e-6f && Math.Abs(conf.TwoLegOutputBiasY) < 1e-6f)
|
||||
return result;
|
||||
|
||||
var bias = new Vector2(conf.TwoLegOutputBiasX, conf.TwoLegOutputBiasY);
|
||||
return new LineSegment(result.Src + bias, result.Dst + bias);
|
||||
}
|
||||
|
||||
/// <summary>在猜测中心周围构造一个矩形 ROI，过滤掉框外点云，降低误识别。</summary>
|
||||
public static List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
|
||||
{
|
||||
var conf = PilotDefinition.Conf;
|
||||
var painter = UI.GetPainter("MultiWheelTwoLegDetect.Filter", false);
|
||||
painter.Clear();
|
||||
|
||||
var box = new[]
|
||||
{
|
||||
new Vector2(guessCenterX - conf.TwoLegFilterLength / 2, guessCenterY - conf.TwoLegFilterWidth / 2),
|
||||
new Vector2(guessCenterX + conf.TwoLegFilterLength / 2, guessCenterY - conf.TwoLegFilterWidth / 2),
|
||||
new Vector2(guessCenterX + conf.TwoLegFilterLength / 2, guessCenterY + conf.TwoLegFilterWidth / 2),
|
||||
new Vector2(guessCenterX - conf.TwoLegFilterLength / 2, guessCenterY + conf.TwoLegFilterWidth / 2),
|
||||
};
|
||||
|
||||
for (var i = 0; i < box.Length; ++i)
|
||||
painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
|
||||
|
||||
// 点云滤波在车体坐标系下进行
|
||||
return new List<DetectFilter>
|
||||
{
|
||||
new(CoordinateSystem.Car2D,
|
||||
p => LessMath.IsPointInPolygon4(
|
||||
box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))),
|
||||
};
|
||||
}
|
||||
}
|
||||
+77
-584
@@ -1,608 +1,101 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using CommonUsage.Chassis;
|
||||
using CommonUsage.Mathematics;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using System;
|
||||
using System.Numerics;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class MultiForwardTest : MovementDefinition
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public float Speed = 0.2f;
|
||||
public float DurationSeconds = 2f;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
public abstract class DstTrackerTestBase : MovementTest
|
||||
{
|
||||
var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
|
||||
chassis.SetOriginBias(0, 0, 0);
|
||||
var end = DateTime.Now.AddSeconds(DurationSeconds);
|
||||
while (DateTime.Now < end)
|
||||
public bool UseInteractivePick = true;
|
||||
public float srcX;
|
||||
public float srcY;
|
||||
public float dstX;
|
||||
public float dstY;
|
||||
public float carDirectionBias;
|
||||
|
||||
private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
|
||||
private DriveTask _dt;
|
||||
|
||||
protected DstTrackerTestBase(float defaultCarDirectionBias)
|
||||
{
|
||||
chassis.SendMotion(Speed, 0, 0);
|
||||
yield return true;
|
||||
carDirectionBias = defaultCarDirectionBias;
|
||||
}
|
||||
|
||||
chassis.SendMotion(0, 0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
// 原地旋转到指定世界坐标系朝向：先把舵轮打到旋转所需角度，对齐后再旋转，按目标角度停止（非固定时长）。
|
||||
public class MultiRotateToWorldAngle : MovementDefinition
|
||||
{
|
||||
/// <summary>目标朝向（世界坐标系，单位 deg）。</summary>
|
||||
public float TargetWorldDeg;
|
||||
|
||||
/// <summary>旋转角速度（deg/s，逆时针为正）。</summary>
|
||||
public float RotSpeed = 30f;
|
||||
|
||||
/// <summary>到位角度精度（deg）。</summary>
|
||||
public float ArriveDeg = 1f;
|
||||
|
||||
/// <summary>起转前舵轮对齐精度（deg）。</summary>
|
||||
public float WheelAlignDeg = 2f;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
|
||||
chassis.SetOriginBias(0, 0, 0);
|
||||
|
||||
// 阶段一：仅把舵轮打到原地旋转所需角度（下发 0 速度，只对齐不旋转）。
|
||||
while (true)
|
||||
public override void TestStop()
|
||||
{
|
||||
chassis.SendRotateMotion(0);
|
||||
if (WheelsAligned(chassis, WheelAlignDeg)) break;
|
||||
yield return true;
|
||||
_dt?.Stop();
|
||||
_painter?.Clear();
|
||||
}
|
||||
|
||||
// 阶段二：旋转到目标世界朝向，到位即停。
|
||||
var target = CommonMath.RoundTh(TargetWorldDeg);
|
||||
while (true)
|
||||
public override void Test()
|
||||
{
|
||||
var cur = CommonMath.RoundTh((float)DetourInterface.getCartLocation().th);
|
||||
var diff = CommonMath.ThDiff(target, cur); // 逆时针为正
|
||||
if (Math.Abs(diff) <= ArriveDeg) break;
|
||||
chassis.SendRotateMotion(Math.Sign(diff) * RotSpeed);
|
||||
yield return true;
|
||||
}
|
||||
|
||||
chassis.PredefinedDriveStop();
|
||||
}
|
||||
|
||||
private static bool WheelsAligned(MultiWheelChassis chassis, float tolDeg)
|
||||
{
|
||||
#pragma warning disable CS0612, CS0618
|
||||
var wheels = chassis.GetSteerWheels();
|
||||
#pragma warning restore CS0612, CS0618
|
||||
foreach (var sw in wheels)
|
||||
if (Math.Abs(CommonMath.ThDiff(sw.ReadAngle(), sw.GetSendAngle())) > tolDeg)
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-前进2秒")]
|
||||
public class MultiForwardMovementTest : MovementTest
|
||||
{
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_task = new DriveTask(new MultiForwardTest().Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop() => _task?.Stop();
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-原地旋转到目标角度")]
|
||||
public class MultiRotateMovementTest : MovementTest
|
||||
{
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_task = new DriveTask(new MultiRotateToWorldAngle
|
||||
{
|
||||
TargetWorldDeg = PilotDefinition.Conf.InPlaceRotateTargetWorldDeg,
|
||||
RotSpeed = PilotDefinition.Conf.InPlaceRotateSpeed,
|
||||
ArriveDeg = PilotDefinition.Conf.InPlaceRotateArriveDeg,
|
||||
WheelAlignDeg = PilotDefinition.Conf.InPlaceRotateWheelAlignDeg
|
||||
}.Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_task?.Stop();
|
||||
((MultiWheelChassis)BasicPilotBase.Chassis).PredefinedDriveStop();
|
||||
}
|
||||
}
|
||||
|
||||
// ===== 车队联动-原地旋转动作 =====
|
||||
// 等价于 FleetRemote 的「原地旋转」模式（已实测可用）：FleetRemote 通过 Medulla 手动 IO
|
||||
// (MultiVehicleManualEnabled + Mode=2 + Vth) 驱动 PilotDefinition.TickMultiVehicle 绕车队中心旋转。
|
||||
// 手动 IO 是 [AsLowerIO]（Medulla→Clumsy，每周期回写），Clumsy 侧动作直接写会被覆盖；
|
||||
// 因此本动作改用 Clumsy 内部脚本字段 MultiVehicleScript*（TickMultiVehicle 已将其作为手动等价输入），
|
||||
// 不写一行底盘指令——实际的 SendRotateMotion + PI 纠偏 + 向从车广播均由 TickMultiVehicle 完成。
|
||||
//
|
||||
// 前提：在「主车」(MultiVehicleMasterEndpoint == "/") 的 Clumsy 上运行，且从车已注册(编队就绪)。
|
||||
// 停止条件：主车 SLAM 朝向累计转过 |TargetDeltaDeg|（刚体原地旋转，整车朝向变化量 == 车队转角）；
|
||||
// 无定位时退化为按 |TargetDeltaDeg| / |Omega| 估算时长；并带安全超时。
|
||||
public class FleetRotateInPlace : MovementDefinition
|
||||
{
|
||||
/// <summary>角速度大小(deg/s)；实际方向由 TargetDeltaDeg 的符号决定。</summary>
|
||||
public float Omega = 15f;
|
||||
|
||||
/// <summary>目标相对转角(deg，带符号，+ 为逆时针)。</summary>
|
||||
public float TargetDeltaDeg = 90f;
|
||||
|
||||
/// <summary>到位角度精度(deg)。</summary>
|
||||
public float ArriveDeg = 1.5f;
|
||||
|
||||
/// <summary>减速区宽度(deg)：剩余角度小于此值时，角速度按剩余比例线性降到 MinOmega，抑制惯性超调。</summary>
|
||||
public float SlowDeg = 25f;
|
||||
|
||||
/// <summary>减速区末段最小角速度(deg/s)：避免越接近目标越慢、长尾停不下/到不了位。</summary>
|
||||
public float MinOmega = 3f;
|
||||
|
||||
/// <summary>缓启动角加速度(deg/s²)：起步时角速度从 0 按此斜率爬升到巡航值，抑制起步抖动/队形骤偏。仅作用于起步加速，<=0 关闭缓启动（阶跃起步）。</summary>
|
||||
public float AccelDegPerSec2 = 20f;
|
||||
|
||||
/// <summary>
|
||||
/// 是否用 Detour 主车航向闭环判停（读 getCartLocation().th 累计实际转角，到 |TargetDeltaDeg| 停）。
|
||||
/// 与 MultiVehicleSyncUseDetour 解耦：转到指定角度需要角度反馈，故默认 true。
|
||||
/// false 时退化为按估算时长开环停止（实际转速≠指令时不精确）。注意 true 时若无有效全局定位，
|
||||
/// getCartLocation() 会阻塞（与单车 MultiRotateToWorldAngle 行为一致）。
|
||||
/// </summary>
|
||||
public bool UseDetourHeading = true;
|
||||
|
||||
// 注：不设超时上限——旋转持续到到位（或无定位时按估算时长结束），或被 Stop()/TestStop() 主动中止。
|
||||
|
||||
/// <summary>到位后保持脚本使能、角速度归零的安定时长(s)，让纠偏把队形稳住再撤离。</summary>
|
||||
public float SettleSec = 0.5f;
|
||||
|
||||
private void ClearScript()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
}
|
||||
|
||||
public void Stop() => ClearScript();
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
var conf = PilotDefinition.Conf;
|
||||
|
||||
if (conf.MultiVehicleMasterEndpoint != "/")
|
||||
{
|
||||
Hedingben.ToastText("车队原地旋转需在主车(主车端点=\"/\")运行", "FleetRotate");
|
||||
yield break;
|
||||
}
|
||||
|
||||
var dir = Math.Sign(TargetDeltaDeg);
|
||||
if (dir == 0) dir = 1;
|
||||
var maxOmega = Math.Abs(Omega);
|
||||
var minOmega = Math.Min(Math.Abs(MinOmega), maxOmega); // 最小不超过最大
|
||||
var slowDeg = Math.Max(1e-3f, SlowDeg); // 减速区宽度
|
||||
var accel = AccelDegPerSec2; // 缓启动角加速度，仅作用于起步，<=0 关闭
|
||||
var targetMag = Math.Abs(TargetDeltaDeg);
|
||||
|
||||
var hasPos = UseDetourHeading;
|
||||
var prevTh = hasPos ? (float)DetourInterface.getCartLocation().th : 0f;
|
||||
var startTh = prevTh;
|
||||
var accumulated = 0f; // 累计带符号转角(deg)
|
||||
var start = DateTime.Now;
|
||||
var lastTime = start;
|
||||
var lastLog = DateTime.MinValue;
|
||||
var lastCenterLog = DateTime.MinValue;
|
||||
var cmdMag = 0f; // 当前实际下发角速度大小(deg/s)，缓启动从 0 斜坡爬升
|
||||
var centerTracking = false;
|
||||
float centerStartX = 0, centerStartY = 0, centerStartTh = 0;
|
||||
float centerLastX = 0, centerLastY = 0, centerLastTh = 0, centerMaxDrift = 0;
|
||||
// 无定位按时长估算时，补上缓启动斜坡少转的等效时间(≈ maxOmega/(2·accel))，使时长更接近目标角。
|
||||
var estDuration = maxOmega > 1e-3 ? targetMag / maxOmega : 0;
|
||||
if (accel > 1e-3) estDuration += maxOmega / (2 * accel);
|
||||
|
||||
DLog.Log(
|
||||
$"REQUEST target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} accel={accel:0.0} " +
|
||||
$"slowDeg={slowDeg:0.0} minOmega={minOmega:0.0} useDetourHeading={hasPos} startTh={startTh:0.00} " +
|
||||
$"estDuration={estDuration:0.00}s syncUseDetour={conf.MultiVehicleSyncUseDetour}",
|
||||
"FleetRotateDbg");
|
||||
|
||||
// 使能脚本驱动的原地旋转（mode2）。TickMultiVehicle 后台循环据此执行旋转并广播给从车。
|
||||
// 起步从 0 角速度开始，由缓启动斜坡爬升，避免阶跃下发导致队形骤偏/抖动。
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptMode = 2;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptEnabled = true;
|
||||
self.MultiVehicleRotateWheelsReady = false;
|
||||
self.MultiVehicleRotateFleetReady = false;
|
||||
|
||||
DLog.Log("WAIT_ALIGN fleet rotate wheels", "FleetRotateDbg");
|
||||
while (!self.MultiVehicleRotateFleetReady)
|
||||
{
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptMode = 2;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptEnabled = true;
|
||||
Hedingben.ToastText("车队原地旋转舵轮预对齐中", "FleetRotate");
|
||||
yield return true;
|
||||
}
|
||||
|
||||
float centerStartCarX = 0, centerStartCarY = 0, centerStartCarTh = 0;
|
||||
if (hasPos)
|
||||
{
|
||||
var startPos = DetourInterface.getCartLocation();
|
||||
centerStartCarX = (float)startPos.x;
|
||||
centerStartCarY = (float)startPos.y;
|
||||
centerStartCarTh = (float)startPos.th;
|
||||
prevTh = centerStartCarTh;
|
||||
startTh = prevTh;
|
||||
if (self.TryGetFleetCenterFromPose(centerStartCarX, centerStartCarY, centerStartCarTh,
|
||||
out centerStartX, out centerStartY, out centerStartTh))
|
||||
Vector2 p1;
|
||||
Vector2 p2;
|
||||
if (UseInteractivePick)
|
||||
{
|
||||
centerLastX = centerStartX;
|
||||
centerLastY = centerStartY;
|
||||
centerLastTh = centerStartTh;
|
||||
centerMaxDrift = 0;
|
||||
centerTracking = true;
|
||||
p1 = UI.GetPoint("point1");
|
||||
p2 = UI.GetPoint("point2");
|
||||
}
|
||||
}
|
||||
accumulated = 0f;
|
||||
start = DateTime.Now;
|
||||
lastTime = start;
|
||||
lastLog = DateTime.MinValue;
|
||||
lastCenterLog = DateTime.MinValue;
|
||||
cmdMag = 0f;
|
||||
DLog.Log(
|
||||
$"START target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} startTh={startTh:0.00} " +
|
||||
$"fleetAligned={self.MultiVehicleRotateFleetReady}",
|
||||
"FleetRotateDbg");
|
||||
if (centerTracking)
|
||||
{
|
||||
DLog.Log(
|
||||
$"START center=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
|
||||
$"car=({centerStartCarX:0.0},{centerStartCarY:0.0},{centerStartCarTh:0.00}) " +
|
||||
$"target={TargetDeltaDeg:0.0} omega={maxOmega:0.0}",
|
||||
"FleetRotateCenterDbg");
|
||||
}
|
||||
|
||||
var stopReason = "stop()";
|
||||
while (true)
|
||||
{
|
||||
var now = DateTime.Now;
|
||||
var dt = (float)Math.Min(0.2, Math.Max(0, (now - lastTime).TotalSeconds));
|
||||
lastTime = now;
|
||||
var elapsed = (now - start).TotalSeconds;
|
||||
|
||||
float desiredMag;
|
||||
float curTh = 0f, remaining = 0f, actualRate = 0f;
|
||||
if (hasPos)
|
||||
else
|
||||
{
|
||||
var carPos = DetourInterface.getCartLocation();
|
||||
curTh = (float)carPos.th;
|
||||
var step = (float)CommonMath.ThDiff(curTh, prevTh); // 本帧实际转角(逆时针为正)
|
||||
accumulated += step;
|
||||
actualRate = dt > 1e-3 ? step / dt : 0f; // 实际角速率(deg/s)，用于对比指令
|
||||
prevTh = curTh;
|
||||
p1 = new Vector2(srcX, srcY);
|
||||
p2 = new Vector2(dstX, dstY);
|
||||
}
|
||||
|
||||
remaining = targetMag - Math.Abs(accumulated);
|
||||
if (remaining <= ArriveDeg) { stopReason = "arrived"; break; }
|
||||
_painter.Clear();
|
||||
_dt = new DriveTask(new DstTracker
|
||||
{
|
||||
Src = p1,
|
||||
Dst = p2,
|
||||
CarDirectionBias = carDirectionBias,
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
}
|
||||
|
||||
// 减速区：剩余角度 < SlowDeg 时，目标角速度按剩余比例线性降到 MinOmega，
|
||||
// 使切断指令瞬间残余动量足够小，抑制惯性滑行造成的超调。宽度直观、便于现场调试。
|
||||
desiredMag = remaining < slowDeg
|
||||
? Math.Max(minOmega, maxOmega * (remaining / slowDeg))
|
||||
: maxOmega;
|
||||
[MovementTest(name = "测试终点跟踪动作-前进")]
|
||||
public sealed class DstTrackerForward : DstTrackerTestBase
|
||||
{
|
||||
public DstTrackerForward() : base(0f) { }
|
||||
}
|
||||
|
||||
if (centerTracking &&
|
||||
self.TryGetFleetCenterFromPose((float)carPos.x, (float)carPos.y, (float)carPos.th,
|
||||
out centerLastX, out centerLastY, out centerLastTh))
|
||||
[MovementTest(name = "测试终点跟踪动作-后退")]
|
||||
public sealed class DstTrackerBackward : DstTrackerTestBase
|
||||
{
|
||||
public DstTrackerBackward() : base(180f) { }
|
||||
}
|
||||
|
||||
[MovementTest(name = "底盘旋转测试")]
|
||||
public class RotateToAngleTest : MovementTest
|
||||
{
|
||||
// 底盘旋转测试不支持停止操作。
|
||||
public override void TestStop()
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
// 交互输入目标角度后执行底盘原地旋转测试。
|
||||
public override void Test()
|
||||
{
|
||||
var target = UI.GetInput("输入旋转角度：");
|
||||
new DriveTask(new MultiWheelRotateInPlace()
|
||||
{
|
||||
AngleTarget = float.Parse(target),
|
||||
PidparamsRead = () => new PIDParams()
|
||||
{
|
||||
var centerDx = centerLastX - centerStartX;
|
||||
var centerDy = centerLastY - centerStartY;
|
||||
var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
|
||||
centerMaxDrift = Math.Max(centerMaxDrift, centerDrift);
|
||||
var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
|
||||
if ((now - lastCenterLog).TotalMilliseconds >= 250)
|
||||
{
|
||||
lastCenterLog = now;
|
||||
DLog.Log(
|
||||
$"ACTION t={elapsed:0.00}s center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
|
||||
$"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
|
||||
$"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
|
||||
$"cmdW={dir * cmdMag:0.000} actualW={actualRate:0.000} acc={accumulated:0.0} remain={remaining:0.0} " +
|
||||
$"wheelReady={self.MultiVehicleRotateWheelsReady} fleetReady={self.MultiVehicleRotateFleetReady}",
|
||||
"FleetRotateCenterDbg");
|
||||
}
|
||||
Kp = PilotDefinition.Conf.InPlaceRotateKp,
|
||||
Ki = PilotDefinition.Conf.InPlaceRotateKi,
|
||||
Kd = PilotDefinition.Conf.InPlaceRotateKd,
|
||||
DeadZone = PilotDefinition.Conf.InPlaceRotateArriveDeg,
|
||||
SpeedAccPerSec = PilotDefinition.Conf.InPlaceRotateAcc,
|
||||
OutputUpperThreshold = PilotDefinition.Conf.InPlaceRotateMaxSpeed,
|
||||
MaxI = PilotDefinition.Conf.InPlaceRotateMaxI,
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 无定位：按时长估算，无法测角，目标维持巡航速度到估算时长（仅缓启动整形）。
|
||||
desiredMag = maxOmega;
|
||||
if (elapsed >= estDuration) { stopReason = "estDuration"; break; }
|
||||
}
|
||||
|
||||
// 缓启动：只对“加速(目标>当前)”按角加速度限斜率，让起步平滑爬升；
|
||||
// “减速(目标<当前)”跟随上面的减速曲线立即下调，保证及时刹车不超调。
|
||||
if (accel > 1e-3 && desiredMag > cmdMag)
|
||||
cmdMag = Math.Min(desiredMag, cmdMag + accel * dt);
|
||||
else
|
||||
cmdMag = desiredMag;
|
||||
|
||||
self.MultiVehicleScriptVth = dir * cmdMag;
|
||||
|
||||
// 落盘诊断(节流~150ms)：实际航向/累计转角/实际角速率 vs 指令角速率，定位"开环转速不足"。
|
||||
if ((now - lastLog).TotalMilliseconds >= 150)
|
||||
{
|
||||
lastLog = now;
|
||||
DLog.Log(
|
||||
hasPos
|
||||
? $"t={elapsed:0.00}s curTh={curTh:0.00} acc={accumulated:0.0} remain={remaining:0.0} " +
|
||||
$"cmdW={dir * cmdMag:0.0} actualW={actualRate:0.0} (实际/指令={(Math.Abs(cmdMag) > 1e-3 ? actualRate / (dir * cmdMag) : 0):0.00})"
|
||||
: $"t={elapsed:0.00}s/{estDuration:0.00}s (无航向反馈,开环按时长) cmdW={dir * cmdMag:0.0}",
|
||||
"FleetRotateDbg");
|
||||
}
|
||||
|
||||
Hedingben.ToastText(
|
||||
hasPos
|
||||
? $"车队原地旋转 目标{TargetDeltaDeg:0.0}° 已转{accumulated:0.0}° 余{targetMag - Math.Abs(accumulated):0.0}° ω={cmdMag:0.0}"
|
||||
: $"车队原地旋转(无定位,按时长) {elapsed:0.0}/{estDuration:0.0}s ω={cmdMag:0.0}",
|
||||
"FleetRotate");
|
||||
|
||||
yield return true;
|
||||
}.Get()).Wait();
|
||||
}
|
||||
|
||||
// 到位：角速度先归零，保持脚本使能让 TickMultiVehicle 的 PI 把队形稳住一小段时间再撤离。
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
var settleEnd = DateTime.Now.AddSeconds(Math.Max(0, SettleSec));
|
||||
while (DateTime.Now < settleEnd)
|
||||
yield return true;
|
||||
|
||||
ClearScript();
|
||||
DLog.Log(
|
||||
$"DONE reason={stopReason} 累计转角={accumulated:0.0}° 目标={TargetDeltaDeg:0.0}° " +
|
||||
$"用时={(DateTime.Now - start).TotalSeconds:0.00}s useDetourHeading={hasPos}",
|
||||
"FleetRotateDbg");
|
||||
if (centerTracking)
|
||||
{
|
||||
var centerDx = centerLastX - centerStartX;
|
||||
var centerDy = centerLastY - centerStartY;
|
||||
var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
|
||||
var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
|
||||
DLog.Log(
|
||||
$"DONE reason={stopReason} center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
|
||||
$"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
|
||||
$"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
|
||||
$"acc={accumulated:0.0} target={TargetDeltaDeg:0.0}",
|
||||
"FleetRotateCenterDbg");
|
||||
}
|
||||
Hedingben.ToastText($"车队原地旋转完成({stopReason}) 累计{accumulated:0.0}°", "FleetRotate");
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "车队联动-原地旋转")]
|
||||
public class FleetRotateInPlaceTest : MovementTest
|
||||
{
|
||||
private FleetRotateInPlace _proc;
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_proc = new FleetRotateInPlace
|
||||
{
|
||||
Omega = PilotDefinition.Conf.FleetRotateOmega,
|
||||
TargetDeltaDeg = PilotDefinition.Conf.FleetRotateTargetDeltaDeg,
|
||||
ArriveDeg = PilotDefinition.Conf.FleetRotateArriveDeg,
|
||||
SlowDeg = PilotDefinition.Conf.FleetRotateSlowDeg,
|
||||
MinOmega = PilotDefinition.Conf.FleetRotateMinOmega,
|
||||
AccelDegPerSec2 = PilotDefinition.Conf.FleetRotateAccel,
|
||||
SettleSec = PilotDefinition.Conf.FleetRotateSettleSec,
|
||||
UseDetourHeading = PilotDefinition.Conf.FleetRotateUseDetourHeading
|
||||
};
|
||||
_task = new DriveTask(_proc.Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_proc?.Stop();
|
||||
_task?.Stop();
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "车队联动-曲线行走")]
|
||||
public class FleetCurveWalkTest : MovementTest
|
||||
{
|
||||
private FleetCurveWalk _proc;
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
if (!self.TryGetFleetCenterFromMembers(out var x, out var y, out var th) &&
|
||||
!self.TryGetFleetCenterFromSlam(out x, out y, out th))
|
||||
{
|
||||
DLog.Log("FleetCurveWalkTest abort: failed to read fleet center.", "FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
|
||||
return;
|
||||
}
|
||||
|
||||
var pointCount = Math.Max(3, PilotDefinition.Conf.FleetCurveTestControlPointCount);
|
||||
var controlPoints = new List<Vector2>();
|
||||
for (var i = 0; i < pointCount; i++)
|
||||
controlPoints.Add(UI.GetPoint($"FleetCurve point {i + 1}/{pointCount}"));
|
||||
var fleetCenter = new Vector2(x, y);
|
||||
if (Vector2.Distance(fleetCenter, controlPoints[0]) >
|
||||
Vector2.Distance(fleetCenter, controlPoints[controlPoints.Count - 1]))
|
||||
controlPoints.Reverse();
|
||||
var track = new BezierTrack(controlPoints)
|
||||
{
|
||||
Speed = PilotDefinition.Conf.FleetCurveSpeed,
|
||||
CarDirectionBias = 0f
|
||||
};
|
||||
|
||||
_proc = new FleetCurveWalk
|
||||
{
|
||||
Track = track,
|
||||
CurveSpeed = PilotDefinition.Conf.FleetCurveSpeed,
|
||||
CarDirectionBias = 0f,
|
||||
SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance,
|
||||
FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance,
|
||||
FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed,
|
||||
SlowingPow = PilotDefinition.Conf.FleetCurveSlowingPow,
|
||||
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
|
||||
StartSyncTimeoutSec = PilotDefinition.Conf.FleetCrabStartSyncTimeoutSec
|
||||
};
|
||||
_task = new DriveTask(_proc.Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_proc?.Stop();
|
||||
_task?.Stop();
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "车队联动-自动蟹行")]
|
||||
public class FleetCrabWalkTest : MovementTest
|
||||
{
|
||||
private FleetCrabWalk _proc;
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_proc = new FleetCrabWalk
|
||||
{
|
||||
CrabAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
|
||||
BodyToPathAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
|
||||
CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm,
|
||||
CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed,
|
||||
FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
|
||||
FleetCrabStartAccel = PilotDefinition.Conf.FleetCrabStartAccel,
|
||||
FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
|
||||
FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
|
||||
FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,
|
||||
FleetCrabSlowingPow = PilotDefinition.Conf.FleetCrabSlowingPow,
|
||||
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
|
||||
};
|
||||
_task = new DriveTask(_proc.Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_proc?.Stop();
|
||||
_task?.Stop();
|
||||
}
|
||||
}
|
||||
|
||||
// ===== 调用 Playground WebAPI 瞬移小车（前移 / 左移 / 旋转）=====
|
||||
// 平移/旋转量在 Fields 面板配置：WebApiTranslateMm（默认100mm）、WebApiRotateDeg（默认5度）。
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI前移")]
|
||||
public class WebApiForwardMoveTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
var name = PilotDefinition.Conf.PlaygroundRobotName;
|
||||
var d = PilotDefinition.Conf.WebApiTranslateMm;
|
||||
|
||||
var pose = PlaygroundWebApi.GetPose(url, name);
|
||||
// 车体系前向 (d, 0) 变换到世界系：车头方向即朝向 yaw
|
||||
var dst = CommonMath.Transform2D(new Vector2(pose.X, pose.Y), pose.YawDeg, new Vector2(d, 0));
|
||||
PlaygroundWebApi.Move(url, name, dst.X, dst.Y, pose.YawDeg);
|
||||
Hedingben.ToastText($"前移 {d:f0}mm -> ({dst.X:f0},{dst.Y:f0})", "WebApiForward");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI左移")]
|
||||
public class WebApiLeftMoveTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
var name = PilotDefinition.Conf.PlaygroundRobotName;
|
||||
var d = PilotDefinition.Conf.WebApiTranslateMm;
|
||||
|
||||
var pose = PlaygroundWebApi.GetPose(url, name);
|
||||
// 车体系左向 (0, d) 变换到世界系（车体 +Y 即左侧）
|
||||
var dst = CommonMath.Transform2D(new Vector2(pose.X, pose.Y), pose.YawDeg, new Vector2(0, d));
|
||||
PlaygroundWebApi.Move(url, name, dst.X, dst.Y, pose.YawDeg);
|
||||
Hedingben.ToastText($"左移 {d:f0}mm -> ({dst.X:f0},{dst.Y:f0})", "WebApiLeft");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI旋转")]
|
||||
public class WebApiRotateTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
var name = PilotDefinition.Conf.PlaygroundRobotName;
|
||||
var deg = PilotDefinition.Conf.WebApiRotateDeg;
|
||||
|
||||
var pose = PlaygroundWebApi.GetPose(url, name);
|
||||
var ny = pose.YawDeg + deg; // 逆时针为正
|
||||
PlaygroundWebApi.Move(url, name, pose.X, pose.Y, ny);
|
||||
Hedingben.ToastText($"旋转 {deg:f1}° -> {ny:f1}°", "WebApiRotate");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI恢复运动")]
|
||||
public class WebApiMotionResumeTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
PlaygroundWebApi.ResumeMotion(url);
|
||||
Hedingben.ToastText("已恢复车辆运动", "WebApiMotion");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI暂停运动")]
|
||||
public class WebApiMotionPauseTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
PlaygroundWebApi.PauseMotion(url); // 默认 zero 模式：反馈归零
|
||||
Hedingben.ToastText("已暂停车辆运动 (zero)", "WebApiMotion");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
+75
-274
@@ -1,34 +1,77 @@
|
||||
using ClumsyCore;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using ClumsyCore.Sensors;
|
||||
using ClumsyCore.Utilities;
|
||||
using ClumsyDance.ClumsyWalk.Detectors;
|
||||
using ClumsyDance.Sensors;
|
||||
using CommonUsage.Chassis;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Calibration;
|
||||
using MDCSToolBox.Clumsy.HighLevelSecurity;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MDCSToolBox.Commons;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using Newtonsoft.Json;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using System.Net.Http;
|
||||
using System.Numerics;
|
||||
using System.Reflection;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
using static ClumsyCore.DTools.Painter;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public class DstTracker : MovementDefinition
|
||||
{
|
||||
public Vector2 Src;
|
||||
public Vector2 Dst;
|
||||
public float CarDirectionBias = 0f;
|
||||
public Painter Painter = UI.GetPainter("DstTracker");
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
DriveTask task = null;
|
||||
try
|
||||
{
|
||||
Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
|
||||
Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
|
||||
|
||||
var tracker = new ChassisController().Get();
|
||||
var linePath = new LineTrack(Src, Dst)
|
||||
{
|
||||
CarDirectionBias = CarDirectionBias,
|
||||
Speed = PilotDefinition.Conf.DstTrackerMaxSpeed
|
||||
};
|
||||
tracker.AddTrack(linePath);
|
||||
task = new DriveTask(tracker.Track());
|
||||
task.Wait();
|
||||
yield return false;
|
||||
}
|
||||
finally
|
||||
{
|
||||
task?.Stop();
|
||||
chassis.SendXYThSpeed(0f, 0f, 0f);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public class Sleep : MovementDefinition
|
||||
{
|
||||
public float Second = 2f;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
if (Second <= 0)
|
||||
{
|
||||
yield return false;
|
||||
yield break;
|
||||
}
|
||||
|
||||
var endTime = DateTime.UtcNow.AddSeconds(Second);
|
||||
while (DateTime.UtcNow < endTime)
|
||||
{
|
||||
Thread.Sleep(50);
|
||||
yield return true;
|
||||
}
|
||||
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
|
||||
public class MultiWheelRotateInPlace : MovementDefinition
|
||||
{
|
||||
/// <summary>
|
||||
@@ -46,281 +89,39 @@ namespace MultiWheelC
|
||||
|
||||
public PIDController thPid;
|
||||
|
||||
// 将角度归一化到零到三百六十度范围内。
|
||||
private static float RangeAngle(float theta)
|
||||
{
|
||||
return (float)(theta - Math.Round(theta / 360.0f) * 360);
|
||||
}
|
||||
|
||||
// 使用 PID 控制原地旋转到目标角度。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var targetAngle = RangeAngle(AngleTarget);
|
||||
var p = PidparamsRead();
|
||||
thPid = new PIDController(ThetaReader, p.Kp);
|
||||
thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone, p.OutputUpperThreshold, p.SpeedAccPerSec);
|
||||
|
||||
DateTime lastTime = DateTime.Now;
|
||||
while (true)
|
||||
try
|
||||
{
|
||||
var s = thPid.GetResponse(targetAngle, true);
|
||||
Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
|
||||
Chassis.SendXYThSpeed(0, 0, s);
|
||||
lastTime = DateTime.Now;
|
||||
if (thPid.IsArrived()) break;
|
||||
yield return true;
|
||||
}
|
||||
Chassis.SendXYThSpeed(0, 0, 0);
|
||||
Console.WriteLine($"final rotate to {targetAngle}");
|
||||
}
|
||||
}
|
||||
var targetAngle = RangeAngle(AngleTarget);
|
||||
var p = PidparamsRead();
|
||||
thPid = new PIDController(ThetaReader, p.Kp);
|
||||
thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
|
||||
p.OutputUpperThreshold, p.SpeedAccPerSec);
|
||||
|
||||
|
||||
public class ClampToTarget : MovementDefinition
|
||||
{
|
||||
public float LeftClampTarget;
|
||||
public float RightClampTarget;
|
||||
|
||||
public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed;
|
||||
public float ClampKp = PilotDefinition.Conf.ClampControlKp;
|
||||
public float ClampKi = PilotDefinition.Conf.ClampControlKi;
|
||||
public float ClampKd = PilotDefinition.Conf.ClampControlKd;
|
||||
public float ClampMaxI = PilotDefinition.Conf.ClampControlMaxI;
|
||||
public float ClampSpeedAcc = PilotDefinition.Conf.ClampControlSpeedAcc;
|
||||
public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
|
||||
private PIDController leftpid, rightpid;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
leftpid = new PIDController(() => PilotDefinition.Self.ActualPosLeftArm, ClampKp, ClampKi, ClampKd,
|
||||
ClampMaxI, ClampDeadZone, MaxClampSpeed)
|
||||
{ SpeedAccPerSec = ClampSpeedAcc };
|
||||
|
||||
rightpid = new PIDController(() => PilotDefinition.Self.ActualPosRightArm, ClampKp, ClampKi, ClampKd,
|
||||
ClampMaxI, ClampDeadZone, MaxClampSpeed)
|
||||
{ SpeedAccPerSec = ClampSpeedAcc };
|
||||
|
||||
while (true)
|
||||
{
|
||||
var leftspeed = leftpid.GetResponse(LeftClampTarget);
|
||||
var rightspeed = rightpid.GetResponse(RightClampTarget);
|
||||
Console.WriteLine($"left arm speed:{leftspeed} right arm speed:{rightspeed}");
|
||||
PilotDefinition.Self.SpeedLeftArm = leftspeed;
|
||||
PilotDefinition.Self.SpeedRightArm = rightspeed;
|
||||
if (leftpid.IsArrived()) PilotDefinition.Self.SpeedLeftArm = 0;
|
||||
if (rightpid.IsArrived()) PilotDefinition.Self.SpeedRightArm = 0;
|
||||
|
||||
if (leftpid.IsArrived() && rightpid.IsArrived()) break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
PilotDefinition.Self.SpeedLeftArm = 0;
|
||||
PilotDefinition.Self.SpeedRightArm = 0;
|
||||
Console.WriteLine($"left clamp to target:{LeftClampTarget} right clamp to target:{RightClampTarget}");
|
||||
}
|
||||
}
|
||||
|
||||
public class Sleep : MovementDefinition
|
||||
{
|
||||
public float Second = 2;
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var start = DateTime.Now;
|
||||
while ((DateTime.Now-start).TotalSeconds<Second)
|
||||
{
|
||||
yield return true;
|
||||
Thread.Sleep(1000);
|
||||
Console.WriteLine("Sleep");
|
||||
}
|
||||
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
|
||||
//直线行走基于detour
|
||||
public class LineTracking1 : MovementDefinition
|
||||
{
|
||||
public float LineDistance = 1000f;
|
||||
public int SrcId = -1;
|
||||
public int DstId = -1;
|
||||
public Action<int> LeaveSrcFunction = null;
|
||||
public Painter painter = UI.GetPainter("Line", false);
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var curpose = DetourInterface.getCartLocation();
|
||||
Console.WriteLine($"curpose.th:{curpose.th}");
|
||||
var src = new Vector2((float)curpose.x, (float)curpose.y);
|
||||
var dst = new Vector2((float)curpose.x + LineDistance * (float)Math.Cos(curpose.th),
|
||||
(float)curpose.y + LineDistance * (float)Math.Sin(curpose.th));
|
||||
Console.WriteLine($"src:{src.X} {src.Y}");
|
||||
Console.WriteLine($"dst:{dst.X} {dst.Y}");
|
||||
painter.DrawLine(Color.Green, src.X, src.Y, dst.X, dst.Y, width: 3);
|
||||
|
||||
var tracker = new ChassisController().Get();
|
||||
var linePath = new LineTrack(src, dst) { CarDirectionBias = LineDistance > 0 ? 0 : 180 };
|
||||
tracker.AddTrack(linePath);
|
||||
var _dt = new DriveTask(tracker.Track());
|
||||
_dt.Wait();
|
||||
if (SrcId != -1 && LeaveSrcFunction != null)
|
||||
{
|
||||
LeaveSrcFunction(SrcId);
|
||||
DLog.Log($"释放放车点{SrcId}", "TireFollowing");
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
|
||||
//在世界坐标系下，从路径起点追踪到终点并停车
|
||||
public class DstTracker : MovementDefinition
|
||||
{
|
||||
public Vector2 Src;
|
||||
public Vector2 Dst;
|
||||
public float CarDirectionBias = 0f;
|
||||
public Painter Painter = UI.GetPainter("DstTracker");
|
||||
|
||||
public float InitialSendSpeed = 0;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
|
||||
Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
|
||||
|
||||
var tracker = new ChassisController().Get();
|
||||
|
||||
if (InitialSendSpeed != 0)
|
||||
{
|
||||
tracker.SkipInitialRotate = true;
|
||||
tracker.InitialSendSpeed = InitialSendSpeed;
|
||||
}
|
||||
|
||||
var linePath = new LineTrack(Src, Dst) { CarDirectionBias = CarDirectionBias, Speed = PilotDefinition.Conf.DstTrackerMaxSpeed };
|
||||
tracker.AddTrack(linePath);
|
||||
var task = new DriveTask(tracker.Track());
|
||||
task.Wait();
|
||||
|
||||
// 到点后兜底停车
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
chassis.SendXYThSpeed(0f, 0f, 0f);
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
|
||||
//直线行走基于轮里程
|
||||
public class LineTracking : MovementDefinition
|
||||
{
|
||||
public float Target;
|
||||
public float MaxSpeed = PilotDefinition.Conf.LineTrackMaxSpeed;
|
||||
public float Kp = PilotDefinition.Conf.LineTrackKp;
|
||||
public float Ki = PilotDefinition.Conf.LineTrackKi;
|
||||
public float Kd = PilotDefinition.Conf.LineTrackKd;
|
||||
public float DeadZone = PilotDefinition.Conf.LineTrackDeadZone;
|
||||
public int SrcId = -1;
|
||||
public int DstId = -1;
|
||||
public Action<int> LeaveSrcFunction = null;
|
||||
private PIDController pid;
|
||||
// 末段衔接：接近目标后不再让 PID 把速度降到 0，保留一个接力速度给后续动作接管
|
||||
public bool EnableHandover = false;
|
||||
public float HandoverDistance = 80f; // mm
|
||||
public float HandoverSpeed = 0.15f; // m/s
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
|
||||
pid = new PIDController(() =>
|
||||
(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
|
||||
Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
|
||||
{ SpeedAccPerSec = MaxSpeed / 2f };
|
||||
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
//chassis.SetOriginBias(0, 0, 0);
|
||||
DLog.Log($"直线行驶距离:{Target}", "TireFollowing");
|
||||
while (true)
|
||||
{
|
||||
var current = (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2;
|
||||
var remain = Target - current;
|
||||
if (EnableHandover && Math.Abs(remain) <= Math.Max(1f, HandoverDistance))
|
||||
while (true)
|
||||
{
|
||||
var handoverSign = Math.Sign(remain);
|
||||
if (handoverSign == 0) handoverSign = 1;
|
||||
var handoverSpeed = Math.Abs(HandoverSpeed) * handoverSign;
|
||||
Console.WriteLine($"handover speed: {handoverSpeed:F3}, remain: {remain:F2}");
|
||||
chassis.SendXYThSpeed(handoverSpeed, 0, 0);
|
||||
// 保留一拍接力速度，让后续 DstTracker 无缝接管
|
||||
var s = thPid.GetResponse(targetAngle, true);
|
||||
Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
|
||||
Chassis.SendXYThSpeed(0, 0, s);
|
||||
if (thPid.IsArrived()) break;
|
||||
yield return true;
|
||||
break;
|
||||
}
|
||||
var speed = pid.GetResponse(Target);
|
||||
Console.WriteLine($"output: {speed} current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
|
||||
|
||||
chassis.SendXYThSpeed(speed, 0, 0);
|
||||
|
||||
if (pid.IsArrived()) break;
|
||||
yield return true;
|
||||
Console.WriteLine($"final rotate to {targetAngle}");
|
||||
}
|
||||
if (SrcId != -1 && LeaveSrcFunction != null)
|
||||
finally
|
||||
{
|
||||
LeaveSrcFunction(SrcId);
|
||||
DLog.Log($"释放放车点{SrcId}", "TireFollowing");
|
||||
Chassis.SendXYThSpeed(0, 0, 0);
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
|
||||
public class DriverAble : MovementDefinition
|
||||
{
|
||||
public int WaitTimeoutMs = 2000;
|
||||
public int PollIntervalMs = 50;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
Console.WriteLine("驱动器上使能");
|
||||
PilotDefinition.Self.ResetFromC = true;
|
||||
|
||||
var start = DateTime.Now;
|
||||
var timeoutMs = Math.Max(0, WaitTimeoutMs);
|
||||
var pollMs = Math.Max(1, PollIntervalMs);
|
||||
var success = PilotDefinition.Self.WheelAbleState;
|
||||
while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs)
|
||||
{
|
||||
Thread.Sleep(pollMs);
|
||||
success = PilotDefinition.Self.WheelAbleState;
|
||||
if (!success) yield return true;
|
||||
}
|
||||
|
||||
PilotDefinition.Self.ResetFromC = false;
|
||||
if (success)
|
||||
Console.WriteLine($"驱动器上使能完成，WheelAbleState={PilotDefinition.Self.WheelAbleState}");
|
||||
else
|
||||
Console.WriteLine($"驱动器上使能超时，WheelAbleState={PilotDefinition.Self.WheelAbleState}，等待{timeoutMs}ms");
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
|
||||
public class DriverDisable : MovementDefinition
|
||||
{
|
||||
public int WaitTimeoutMs = 3000;
|
||||
public int PollIntervalMs = 20;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
Console.WriteLine("驱动器下使能");
|
||||
PilotDefinition.Self.DisableFromC = true;
|
||||
|
||||
var start = DateTime.Now;
|
||||
var timeoutMs = Math.Max(0, WaitTimeoutMs);
|
||||
var pollMs = Math.Max(1, PollIntervalMs);
|
||||
var success = !PilotDefinition.Self.WheelAbleState;
|
||||
while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs)
|
||||
{
|
||||
Thread.Sleep(pollMs);
|
||||
success = !PilotDefinition.Self.WheelAbleState;
|
||||
if (!success) yield return true;
|
||||
}
|
||||
|
||||
PilotDefinition.Self.DisableFromC = false;
|
||||
if (success)
|
||||
Console.WriteLine($"驱动器下使能完成，WheelAbleState={PilotDefinition.Self.WheelAbleState}");
|
||||
else
|
||||
Console.WriteLine($"驱动器下使能超时，WheelAbleState={PilotDefinition.Self.WheelAbleState}，等待{timeoutMs}ms");
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
+223
-204
@@ -6,108 +6,19 @@ namespace MultiWheelC;
|
||||
|
||||
public class PilotConfig : MultiWheelPilotConfig
|
||||
{
|
||||
[FieldMember(desc = "[sync] steering angle acceleration(deg/s^2)")] public float SyncThAccPerSec = 30f;
|
||||
[FieldMember(desc = "[sync] fleet member distance(mm)")] public float TestCarSyncDistance = 2400f;
|
||||
[FieldMember(desc = "[sync] fleet layout bias angle(deg)")] public float TestCarSyncTh = 0f;
|
||||
// Fleet manual remote IO values are normalized joystick ratios. Keep all speed/angle scaling here.
|
||||
[FieldMember(desc = "[sync] fleet manual max linear speed(m/s)")] public float FleetManualMaxSpeed = 0.3f;
|
||||
[FieldMember(desc = "[sync] fleet manual normal-mode full-stick steering angle(deg)")] public float FleetManualMaxSteerAngleDeg = 45f;
|
||||
[FieldMember(desc = "[sync] fleet manual crab-mode full-stick steering angle(deg)")] public float FleetManualMaxCrabAngleDeg = 60f;
|
||||
[FieldMember(desc = "[sync] fleet manual rotate-mode full-stick angular speed(deg/s)")] public float FleetManualMaxRotateOmegaDegPerSec = 45f;
|
||||
[FieldMember(desc = "[sync] 蟹行舵角上限(deg，应与Medulla舵轮角度限制匹配，默认120)")] public float MultiVehicleCrabSteerLimitDeg = 120f;
|
||||
[FieldMember(desc = "[sync] 检测中心偏移(mm)")] public float DeltaDetectCenter = 350f;
|
||||
// 仅控制"车队内姿态纠正"(POS 补偿)是否使用 Detour 的 SLAM 位姿，不影响"整个车队姿态的计算"。
|
||||
// 默认 false：定位不参与车队内姿态纠正（各车按编队几何/互识别保持队形，不做 SLAM 逐车纠偏）。
|
||||
// 为 true：额外用 getCartLocation() 反推每台车相对编队中心的偏差并做 POS 补偿。
|
||||
// 注意：无论该开关如何，自动模式下整队姿态(反推/广播车队中心、SLAM 间距、自动安全门)始终依赖 Detour 全局定位；
|
||||
// 主车自动模式必调用 getCartLocation()，若无有效全局定位该调用会阻塞 → 联动线程阻塞不下发速度（安全停车）。
|
||||
[FieldMember(desc = "[sync] 定位是否参与车队内姿态纠正(不影响整队姿态计算)")] public bool MultiVehicleSyncUseDetour = false;
|
||||
// 手动外部遥控联动默认只走 2 腿检测/几何同步，避免 Detour getCartLocation 阻塞导致遥控和检测可视化变慢。
|
||||
[FieldMember(desc = "[sync] 手动联动是否启用定位姿态纠正(默认关闭)")] public bool MultiVehicleManualUseDetourCorrection = false;
|
||||
#region 单车-轨迹跟踪
|
||||
|
||||
[FieldMember(desc = "多车联动：总车数")] public int MultiVehicleFleetNum = 2;
|
||||
[FieldMember(desc = "联动线程周期(ms)")] public int MultiVehicleSyncInterval = 50;
|
||||
[FieldMember(desc = "多车联动：主车端点 ip:port，/ 表示本车为主车")] public string MultiVehicleMasterEndpoint = "/";
|
||||
[FieldMember(desc = "多车联动：本车同步 IP")] public string SimpleIp = "127.0.0.1";
|
||||
[FieldMember(desc = "直线行走距离")] public float LineTrackDistance = 1000f;
|
||||
[FieldMember(desc = "直线行走最大速度")] public float LineTrackMaxSpeed = 0.3f;
|
||||
[FieldMember(desc = "直线行走Kp")] public float LineTrackKp = 0.2f;
|
||||
[FieldMember(desc = "直线行走Ki")] public float LineTrackKi = 0f;
|
||||
[FieldMember(desc = "直线行走Kd")] public float LineTrackKd = 0f;
|
||||
[FieldMember(desc = "直线行走DeadZone")] public float LineTrackDeadZone = 50f;
|
||||
|
||||
[FieldMember(desc = "多车联动：本车回连端点 ip:port，供主车 notify 回连，空=127.0.0.1:本车port")] public string MultiVehicleSelfEndpoint = "";
|
||||
|
||||
[JsonProperty("MultiVehicleMasterIp")]
|
||||
private string LegacyMasterIpSetter
|
||||
{
|
||||
set
|
||||
{
|
||||
if (string.IsNullOrEmpty(value) || value == "/") return;
|
||||
if (MultiVehicleMasterEndpoint == "/")
|
||||
MultiVehicleMasterEndpoint = value.Contains(":") ? value : $"{value}:8008";
|
||||
}
|
||||
}
|
||||
|
||||
[FieldMember(desc = "多车联动：启用互识别纠正")] public bool MultiVehicleUseDetect = false;
|
||||
|
||||
// B: 自动速度命令新鲜度(ms)。主车超过此时长未从路径控制器收到新速度命令(路径结束/早退/卡顿)，
|
||||
// 即视为失效并清零下发速度，避免车队按末速度滑行。0 表示自动取 max(200, interval*4)。
|
||||
[FieldMember(desc = "多车联动：自动速度命令超时(ms，0=auto)")] public int MultiVehicleAutoCmdTimeoutMs = 0;
|
||||
// C: fleet 成员存活 TTL(ms)。主车剔除超过此时长未 register/刷新的从车；编队就绪要求所有成员新鲜。
|
||||
// 0 表示自动取 max(500, interval*6)。
|
||||
[FieldMember(desc = "多车联动：成员存活TTL(ms，0=auto)")] public int MultiVehicleMemberTtlMs = 0;
|
||||
// D: 自动模式下用主车路径控制器的理想车队中心(idealPos/idealAngle)作为各车 layout 目标，
|
||||
// 弧线路径上做 per-car 前馈而非仅共用 frontTh/rearTh 事后纠偏。
|
||||
[FieldMember(desc = "多车联动：自动模式按理想中心前馈(弧线)")] public bool MultiVehicleAutoUseIdealCenter = true;
|
||||
// H: 自动模式必须有有效车队中心(SLAM 可反推)，全程定位丢失时停车，避免纯 SLAM 下盲跑。
|
||||
[FieldMember(desc = "多车联动：自动模式要求有效车队中心")] public bool MultiVehicleAutoRequireFleetCenter = true;
|
||||
[FieldMember(desc = "多车联动：SLAM X补偿系数")] public float MultiVehiclePosBiasXFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：SLAM Y补偿系数")] public float MultiVehiclePosBiasYFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：SLAM Th补偿系数")] public float MultiVehiclePosBiasThFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：X补偿阈值(mm)")] public float MultiVehiclePosBiasXThreshold = 50f;
|
||||
[FieldMember(desc = "多车联动：Y补偿阈值(mm)")] public float MultiVehiclePosBiasYThreshold = 50f;
|
||||
[FieldMember(desc = "多车联动：Th补偿阈值(deg)")] public float MultiVehiclePosBiasThThreshold = 5f;
|
||||
|
||||
[FieldMember(desc = "多车联动：互识别 X补偿系数")] public float MultiVehicleDetectBiasXFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：互识别 Y补偿系数")] public float MultiVehicleDetectBiasYFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：互识别 Th补偿系数")] public float MultiVehicleDetectBiasThFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：互识别 X补偿阈值(mm)")] public float MultiVehicleDetectBiasXThreshold = 50f;
|
||||
[FieldMember(desc = "多车联动：互识别 Y补偿阈值(mm)")] public float MultiVehicleDetectBiasYThreshold = 50f;
|
||||
[FieldMember(desc = "多车联动：互识别 Th补偿阈值(deg)")] public float MultiVehicleDetectBiasThThreshold = 5f;
|
||||
|
||||
// 原地旋转(mode2)闭环纠偏(PI)：把"本车应移动到的位置(dx,dy,mm)/应转角(dth,deg)"作为误差，
|
||||
// 用 PI 控制器换算成车体系修正速度叠加到绕队心旋转上。纯 P 对抗恒定横向滑移扰动有稳态残差，
|
||||
// 加积分项把稳态误差拉到 0；积分带限幅(抗 windup)，总输出限幅在 Max 内防过冲/振荡。
|
||||
// Fac=比例增益(mm/s per mm、deg/s per deg)；IFac=积分增益(mm/s per mm·s、deg/s per deg·s)；Max=总输出上限。
|
||||
[FieldMember(desc = "原地旋转纠偏：平移比例增益P(mm/s per mm)")] public float MultiVehicleRotateCompXyFac = 1.2f;
|
||||
[FieldMember(desc = "原地旋转纠偏：平移积分增益I(mm/s per mm·s)")] public float MultiVehicleRotateCompXyIFac = 0.8f;
|
||||
[FieldMember(desc = "原地旋转纠偏：平移速度上限(mm/s)")] public float MultiVehicleRotateCompXyMax = 150f;
|
||||
[FieldMember(desc = "原地旋转纠偏：转向比例增益P(deg/s per deg)")] public float MultiVehicleRotateCompThFac = 0.8f;
|
||||
[FieldMember(desc = "原地旋转纠偏：转向积分增益I(deg/s per deg·s)")] public float MultiVehicleRotateCompThIFac = 0.8f;
|
||||
[FieldMember(desc = "原地旋转纠偏：转向速度上限(deg/s)")] public float MultiVehicleRotateCompThMax = 15f;
|
||||
// 仅当车队实际被指令旋转(|fleetOmega|超过此阈值)时才运行纠偏 PI；否则清零并复位积分，
|
||||
// 避免松开摇杆后积分残留持续驱动车辆"自行旋转停不下来"。
|
||||
[FieldMember(desc = "原地旋转纠偏：生效的最小角速度阈值(deg/s)")] public float MultiVehicleRotateActiveOmega = 0.5f;
|
||||
// 安全网：每轮纠偏速度幅值 <= 该比例 * 本轮旋转切向速度，限制合速度相对纯切向的最大偏角。
|
||||
// 旧配置若仍为 <0，运行时按安全默认 0.10 处理；确需放宽时可在主车显式调大并同步给从车。
|
||||
[FieldMember(desc = "原地旋转纠偏：纠偏/旋转切向比例硬上限，<0使用安全默认0.10")] public float MultiVehicleRotateCompTangentFrac = 0.10f;
|
||||
|
||||
[FieldMember(desc = "单车同步 xy 精度(mm)")] public float SingleCarSyncPrecisionXy = 10f;
|
||||
[FieldMember(desc = "单车同步 th 精度(deg)")] public float SingleCarSyncPrecisionTh = 0.2f;
|
||||
|
||||
[FieldMember(desc = "Playground WebAPI 基地址")]
|
||||
public string PlaygroundWebApiUrl = "http://localhost:18090";
|
||||
|
||||
[FieldMember(desc = "MultiVehicle rotate pose WebAPI diagnostics (simulation only)")]
|
||||
public bool MultiVehicleRotatePoseWebApiDiagEnabled = false;
|
||||
|
||||
[FieldMember(desc = "Playground 小车名称（场景 robots[].name）")]
|
||||
public string PlaygroundRobotName = "agv_multi_1";
|
||||
|
||||
[FieldMember(desc = "Playground 邻车名称（仅主车用于原地旋转位姿诊断）")]
|
||||
public string PlaygroundNeighborRobotName = "agv_multi_2";
|
||||
|
||||
[FieldMember(desc = "WebAPI 平移测试：平移距离(mm)")]
|
||||
public float WebApiTranslateMm = 100f;
|
||||
|
||||
[FieldMember(desc = "WebAPI 旋转测试：旋转角度(deg)")]
|
||||
public float WebApiRotateDeg = 5f;
|
||||
[FieldMember(desc = "终点跟踪:速度")] public float DstTrackerMaxSpeed = 0.3f;
|
||||
#endregion
|
||||
|
||||
#region 单车-原地旋转
|
||||
[FieldMember(desc = "原地旋转：目标朝向(世界坐标系, deg)")]
|
||||
public float InPlaceRotateTargetWorldDeg = 90f;
|
||||
|
||||
@@ -123,102 +34,35 @@ public class PilotConfig : MultiWheelPilotConfig
|
||||
[FieldMember(desc = "原地旋转：旋转过程中舵轮偏差重对齐阈值(deg)")]
|
||||
public float InPlaceRotateActiveWheelAlignDeg = 10f;
|
||||
|
||||
// ===== 车队联动-原地旋转动作（FleetRotateInPlace / 对应 FleetRemote 原地旋转模式）=====
|
||||
// 通过 Clumsy 内部脚本字段驱动 TickMultiVehicle 的 mode2 旋转（绕车队中心 + PI 纠偏），需主车运行。
|
||||
[FieldMember(desc = "车队原地旋转：角速度大小(deg/s，方向由目标角符号决定)")]
|
||||
public float FleetRotateOmega = 15f;
|
||||
#endregion
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：目标相对转角(deg，+逆时针)")]
|
||||
public float FleetRotateTargetDeltaDeg = 90f;
|
||||
#region 单车-临时
|
||||
[FieldMember(desc = "原地旋转Kp")]
|
||||
public float InPlaceRotateKp = 0.05f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：到位角度精度(deg)")]
|
||||
public float FleetRotateArriveDeg = 1.5f;
|
||||
[FieldMember(desc = "原地旋转Ki")]
|
||||
public float InPlaceRotateKi = 0.01f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：减速区宽度(deg)，抑制收尾惯性超调")]
|
||||
public float FleetRotateSlowDeg = 25f;
|
||||
[FieldMember(desc = "原地旋转Kd")]
|
||||
public float InPlaceRotateKd = 0f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：减速区末段最小角速度(deg/s)")]
|
||||
public float FleetRotateMinOmega = 3f;
|
||||
[FieldMember(desc = "原地旋转积分限幅")]
|
||||
public float InPlaceRotateMaxI = 0.01f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：起步缓启动角加速度(deg/s²，<=0关闭)")]
|
||||
public float FleetRotateAccel = 20f;
|
||||
[FieldMember(desc = "原地旋转最大角速度(deg/s)")]
|
||||
public float InPlaceRotateMaxSpeed = 30f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：到位后安定时长(s)")]
|
||||
public float FleetRotateSettleSec = 0.5f;
|
||||
[FieldMember(desc = "原地旋转角加速度(deg/s²)")]
|
||||
public float InPlaceRotateAcc = 30f;
|
||||
|
||||
// 与 MultiVehicleSyncUseDetour 解耦：转到指定角度需航向反馈，默认 true 读主车 SLAM 航向闭环判停。
|
||||
// false 时退化为按估算时长开环停止（实际转速≠指令时不精确，易出现"没转到目标就停"）。
|
||||
[FieldMember(desc = "车队原地旋转：用Detour主车航向闭环判停(默认true，false=按时长开环)")]
|
||||
public bool FleetRotateUseDetourHeading = true;
|
||||
[FieldMember(desc = "原地旋转超时(s)")]
|
||||
public float InPlaceRotateTimeoutSec = 15f;
|
||||
#endregion
|
||||
|
||||
// ===== 车队联动-自动蟹行（FleetCrabWalk）=====
|
||||
// 以当前车队中心为起点，构造一条直线路径；MovementTest 中车身保持启动朝向追踪该路径。
|
||||
// 动作侧参考几何控制器的路径跟踪思路，直接写入 MultiVehicleAuto... 字段，不再复用脚本手动链路。
|
||||
[FieldMember(desc = "车队蟹行：路径方向相对启动时车队朝向夹角(deg，逆时针为正；路径在车右侧x度时填-x)")]
|
||||
public float FleetCrabAngleDeg = 45f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：AGV入口使用的车队世界系目标朝向(deg)")]
|
||||
public float FleetCrabBodyWorldHeadingDeg = 0f;
|
||||
#if false
|
||||
|
||||
[FieldMember(desc = "车队蟹行：路径长度(mm)")]
|
||||
public float FleetCrabLengthMm = 2000f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：行驶速度(m/s)")]
|
||||
public float FleetCrabSpeed = 0.2f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：速度命令加速度限制(m/s^2，<=0表示不限制)")]
|
||||
public float FleetCrabAccel = 0.2f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：预对齐后正式下发速度前5秒加速度(m/s^2，<=0表示不限制)")]
|
||||
public float FleetCrabStartAccel = 0.01f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：末端开始减速距离(mm)")]
|
||||
public float FleetCrabSlowDistance = 2000f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：完成距离(mm)，低于该剩余距离结束动作")]
|
||||
public float FleetCrabFinishDistance = 20f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：末端最低速度(m/s)")]
|
||||
public float FleetCrabFinishSpeed = 0.02f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：末端减速曲线指数")]
|
||||
public float FleetCrabSlowingPow = 0.8f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：GCP舵角修正上限(deg)")]
|
||||
public float FleetCrabGcpThetaThreshold = 95f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：headingErr角度纠偏比例系数")]
|
||||
public float FleetCrabDthLinearFac = 1f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：headingErr角度纠偏舵角限幅(deg)")]
|
||||
public float FleetCrabDthLinearThreshold = 10f;
|
||||
|
||||
[FieldMember(desc = "FleetCrab startup sync timeout(s)")]
|
||||
public float FleetCrabStartSyncTimeoutSec = 8f;
|
||||
|
||||
[FieldMember(desc = "FleetCrab startup wheel alignment tolerance(deg)")]
|
||||
public float FleetCrabStartWheelAlignDeg = 2f;
|
||||
|
||||
// ===== Fleet linked Bezier curve walk =====
|
||||
[FieldMember(desc = "FleetCurve MovementTest Bezier control point count")]
|
||||
public int FleetCurveTestControlPointCount = 4;
|
||||
|
||||
[FieldMember(desc = "FleetCurve speed(m/s)")]
|
||||
public float FleetCurveSpeed = 0.2f;
|
||||
|
||||
[FieldMember(desc = "FleetCurve slow distance(mm)")]
|
||||
public float FleetCurveSlowDistance = 2000f;
|
||||
|
||||
[FieldMember(desc = "FleetCurve finish distance(mm)")]
|
||||
public float FleetCurveFinishDistance = 20f;
|
||||
|
||||
[FieldMember(desc = "FleetCurve finish speed(m/s)")]
|
||||
public float FleetCurveFinishSpeed = 0.02f;
|
||||
|
||||
[FieldMember(desc = "FleetCurve slowing curve exponent")]
|
||||
public float FleetCurveSlowingPow = 0.8f;
|
||||
|
||||
// ===== 2腿检测（单线雷达识别两腿托盘 / 轮胎）=====
|
||||
#region 单车-钻车与夹抱
|
||||
[FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")]
|
||||
public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1";
|
||||
|
||||
@@ -264,7 +108,6 @@ public class PilotConfig : MultiWheelPilotConfig
|
||||
[FieldMember(desc = "2腿检测：ROI滤波框宽(mm)")]
|
||||
public float TwoLegFilterWidth = 600f;
|
||||
|
||||
#region
|
||||
[FieldMember(desc = "轮胎识别:识别框长")] public float TireFilterLength = 1800f;
|
||||
[FieldMember(desc = "轮胎识别:识别框宽")] public float TireFilterWidth = 600f;
|
||||
[FieldMember(desc = "轮胎识别:轮胎间距")] public float TireTwoLegWidth = 800f;
|
||||
@@ -284,22 +127,6 @@ public class PilotConfig : MultiWheelPilotConfig
|
||||
[FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackTwoLegSgnDir = 1;
|
||||
[FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegCenterChangeX = 0;
|
||||
|
||||
[FieldMember(desc = "抱夹控制pid:Kp")] public float ClampControlKp = 0.1f;
|
||||
[FieldMember(desc = "抱夹控制pid:Ki")] public float ClampControlKi = 0f;
|
||||
[FieldMember(desc = "抱夹控制pid:Kd")] public float ClampControlKd = 0f;
|
||||
[FieldMember(desc = "抱夹控制pid:MaxI")] public float ClampControlMaxI = 0f;
|
||||
[FieldMember(desc = "抱夹控制pid:Acc")] public float ClampControlSpeedAcc = 1f;
|
||||
[FieldMember(desc = "抱夹控制pid:Thresh")] public float ClampControlThresh = 0.2f;
|
||||
[FieldMember(desc = "抱夹控制pid:DeadZone")] public float ClampControlDeadZone = 5f;
|
||||
[FieldMember(desc = "抱夹最大速度")] public float MaxClampSpeed = 1.5f;
|
||||
|
||||
[FieldMember(desc = "直线行走距离")] public float LineTrackDistance = 1000f;
|
||||
[FieldMember(desc = "直线行走最大速度")] public float LineTrackMaxSpeed = 0.3f;
|
||||
[FieldMember(desc = "直线行走Kp")] public float LineTrackKp = 0.2f;
|
||||
[FieldMember(desc = "直线行走Ki")] public float LineTrackKi = 0f;
|
||||
[FieldMember(desc = "直线行走Kd")] public float LineTrackKd = 0f;
|
||||
[FieldMember(desc = "直线行走DeadZone")] public float LineTrackDeadZone = 50f;
|
||||
|
||||
[FieldMember(desc = "轮胎跟踪:切换至盲走距离")] public float TireFollowingWalkBlindSwitchingDistance = 1200f;
|
||||
[FieldMember(desc = "轮胎跟踪:识别第一对轮胎的初始距离")] public float TireFollowingStage1GuessX = 2000f;
|
||||
[FieldMember(desc = "轮胎跟踪:识别第二对轮胎的初始距离")] public float TireFollowingStage2GuessX = 2475f;
|
||||
@@ -320,9 +147,7 @@ public class PilotConfig : MultiWheelPilotConfig
|
||||
[FieldMember(desc = "轮胎跟踪:距离过近角度忽略阈值")] public float TireFollowingAngleIgnoreThr = 0.2f;
|
||||
[FieldMember(desc = "轮胎跟踪:Y最大平均数")] public int TireFollowingYAverageFrameCount = 5;
|
||||
|
||||
[FieldMember(desc = "终点跟踪:速度")] public float DstTrackerMaxSpeed = 0.3f;
|
||||
[FieldMember(desc = "轮胎跟踪:释放锁点距离")] public float TireFollowingReleaseDistance = 1600;
|
||||
#endregion
|
||||
|
||||
[FieldMember(desc = "轮胎跟踪:角度调整kp")] public float TireFollowingThkp = 0.05f;
|
||||
[FieldMember(desc = "轮胎跟踪:角度调整ki")] public float TireFollowingThki = 0.01f;
|
||||
@@ -331,4 +156,198 @@ public class PilotConfig : MultiWheelPilotConfig
|
||||
[FieldMember(desc = "轮胎跟踪:角度调整Thresh")] public float TireFollowingThThresh = 0.1f;
|
||||
[FieldMember(desc = "轮胎跟踪:角度调整DeadZone")] public float TireFollowingThDeadZone = 5f;
|
||||
[FieldMember(desc = "轮胎跟踪:角度调整MaxI")] public float TireFollowingThMaxI = 0.01f;
|
||||
|
||||
[FieldMember(desc = "抱夹控制pid:Kp")] public float ClampControlKp = 0.1f;
|
||||
[FieldMember(desc = "抱夹控制pid:Ki")] public float ClampControlKi = 0f;
|
||||
[FieldMember(desc = "抱夹控制pid:Kd")] public float ClampControlKd = 0f;
|
||||
[FieldMember(desc = "抱夹控制pid:MaxI")] public float ClampControlMaxI = 0f;
|
||||
[FieldMember(desc = "抱夹控制pid:Acc")] public float ClampControlSpeedAcc = 1f;
|
||||
[FieldMember(desc = "抱夹控制pid:Thresh")] public float ClampControlThresh = 0.2f;
|
||||
[FieldMember(desc = "抱夹控制pid:DeadZone")] public float ClampControlDeadZone = 5f;
|
||||
[FieldMember(desc = "抱夹最大速度")] public float MaxClampSpeed = 1.5f;
|
||||
|
||||
|
||||
#endregion
|
||||
|
||||
#region 多车-编队与遥控
|
||||
[FieldMember(desc = "联动时转向角爬升加速度")] public float SyncThAccPerSec = 30f;
|
||||
[FieldMember(desc = "两车间距 (mm)")] public float TestCarSyncDistance = 2400f;
|
||||
[FieldMember(desc = "编队排布偏角")] public float TestCarSyncTh = 0f;
|
||||
// Fleet manual remote IO values are normalized joystick ratios. Keep all speed/angle scaling here.
|
||||
[FieldMember(desc = "车队遥控最大线速度")] public float FleetManualMaxSpeed = 0.3f;
|
||||
[FieldMember(desc = "常规模式满杆舵角")] public float FleetManualMaxSteerAngleDeg = 45f;
|
||||
[FieldMember(desc = "蟹行满杆舵角")] public float FleetManualMaxCrabAngleDeg = 60f;
|
||||
[FieldMember(desc = "旋转满杆角速度")] public float FleetManualMaxRotateOmegaDegPerSec = 45f;
|
||||
[FieldMember(desc = "蟹行舵角上限（对齐 ±120）")] public float MultiVehicleCrabSteerLimitDeg = 120f;
|
||||
[FieldMember(desc = "互识别检测中心偏移")] public float DeltaDetectCenter = 350f;
|
||||
#endregion
|
||||
|
||||
#region 多车-通信
|
||||
[FieldMember(desc = "多车联动：总车数")] public int MultiVehicleFleetNum = 2;
|
||||
[FieldMember(desc = "联动线程周期(ms)")] public int MultiVehicleSyncInterval = 50;
|
||||
[FieldMember(desc = "多车联动：主车端点 ip:port，/ 表示本车为主车")] public string MultiVehicleMasterEndpoint = "/";
|
||||
[FieldMember(desc = "多车联动：本车同步 IP")] public string SimpleIp = "127.0.0.1";
|
||||
|
||||
[FieldMember(desc = "多车联动：本车回连端点 ip:port，供主车 notify 回连，空=127.0.0.1:本车port")] public string MultiVehicleSelfEndpoint = "";
|
||||
[FieldMember(desc = "多车联动：自动速度命令超时(ms，0=auto)")] public int MultiVehicleAutoCmdTimeoutMs = 0;
|
||||
[FieldMember(desc = "多车联动：成员存活TTL(ms，0=auto)")] public int MultiVehicleMemberTtlMs = 0;
|
||||
|
||||
[JsonProperty("MultiVehicleMasterIp")]
|
||||
private string LegacyMasterIpSetter
|
||||
{
|
||||
set
|
||||
{
|
||||
if (string.IsNullOrEmpty(value) || value == "/") return;
|
||||
if (MultiVehicleMasterEndpoint == "/")
|
||||
MultiVehicleMasterEndpoint = value.Contains(":") ? value : $"{value}:8008";
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region 多车-队形补偿
|
||||
[FieldMember(desc = "定位是否参与车队内姿态纠正(不影响整队姿态计算)")] public bool MultiVehicleSyncUseDetour = false;
|
||||
[FieldMember(desc = "手动联动是否启用定位姿态纠正(默认关闭)")] public bool MultiVehicleManualUseDetourCorrection = false;
|
||||
[FieldMember(desc = "多车联动：启用互识别纠正")] public bool MultiVehicleUseDetect = false;
|
||||
[FieldMember(desc = "多车联动：自动模式按理想中心前馈(弧线)")] public bool MultiVehicleAutoUseIdealCenter = true;
|
||||
[FieldMember(desc = "多车联动：自动模式要求有效车队中心")] public bool MultiVehicleAutoRequireFleetCenter = true;
|
||||
[FieldMember(desc = "多车联动：SLAM X补偿系数")] public float MultiVehiclePosBiasXFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：SLAM Y补偿系数")] public float MultiVehiclePosBiasYFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：SLAM Th补偿系数")] public float MultiVehiclePosBiasThFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：X补偿阈值(mm)")] public float MultiVehiclePosBiasXThreshold = 50f;
|
||||
[FieldMember(desc = "多车联动：Y补偿阈值(mm)")] public float MultiVehiclePosBiasYThreshold = 50f;
|
||||
[FieldMember(desc = "多车联动：Th补偿阈值(deg)")] public float MultiVehiclePosBiasThThreshold = 5f;
|
||||
[FieldMember(desc = "多车联动：互识别 X补偿系数")] public float MultiVehicleDetectBiasXFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：互识别 Y补偿系数")] public float MultiVehicleDetectBiasYFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：互识别 Th补偿系数")] public float MultiVehicleDetectBiasThFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：互识别 X补偿阈值(mm)")] public float MultiVehicleDetectBiasXThreshold = 50f;
|
||||
[FieldMember(desc = "多车联动：互识别 Y补偿阈值(mm)")] public float MultiVehicleDetectBiasYThreshold = 50f;
|
||||
[FieldMember(desc = "多车联动：互识别 Th补偿阈值(deg)")] public float MultiVehicleDetectBiasThThreshold = 5f;
|
||||
#endregion
|
||||
|
||||
#region 多车-旋转补偿
|
||||
[FieldMember(desc = "原地旋转纠偏：平移比例增益P(mm/s per mm)")] public float MultiVehicleRotateCompXyFac = 1.2f;
|
||||
[FieldMember(desc = "原地旋转纠偏：平移积分增益I(mm/s per mm·s)")] public float MultiVehicleRotateCompXyIFac = 0.8f;
|
||||
[FieldMember(desc = "原地旋转纠偏：平移速度上限(mm/s)")] public float MultiVehicleRotateCompXyMax = 150f;
|
||||
[FieldMember(desc = "原地旋转纠偏：转向比例增益P(deg/s per deg)")] public float MultiVehicleRotateCompThFac = 0.8f;
|
||||
[FieldMember(desc = "原地旋转纠偏：转向积分增益I(deg/s per deg·s)")] public float MultiVehicleRotateCompThIFac = 0.8f;
|
||||
[FieldMember(desc = "原地旋转纠偏：转向速度上限(deg/s)")] public float MultiVehicleRotateCompThMax = 15f;
|
||||
[FieldMember(desc = "原地旋转纠偏：生效的最小角速度阈值(deg/s)")] public float MultiVehicleRotateActiveOmega = 0.5f;
|
||||
[FieldMember(desc = "原地旋转纠偏：纠偏/旋转切向比例硬上限，<0使用安全默认0.10")] public float MultiVehicleRotateCompTangentFrac = 0.10f;
|
||||
|
||||
[FieldMember(desc = "单车同步 xy 精度(mm)")] public float SingleCarSyncPrecisionXy = 10f;
|
||||
[FieldMember(desc = "单车同步 th 精度(deg)")] public float SingleCarSyncPrecisionTh = 0.2f;
|
||||
#endregion
|
||||
|
||||
#region 多车-联动动作
|
||||
[FieldMember(desc = "车队原地旋转：角速度大小(deg/s，方向由目标角符号决定)")]
|
||||
public float FleetRotateOmega = 15f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：目标相对转角(deg，+逆时针)")]
|
||||
public float FleetRotateTargetDeltaDeg = 90f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：到位角度精度(deg)")]
|
||||
public float FleetRotateArriveDeg = 1.5f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：减速区宽度(deg)，抑制收尾惯性超调")]
|
||||
public float FleetRotateSlowDeg = 25f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：减速区末段最小角速度(deg/s)")]
|
||||
public float FleetRotateMinOmega = 3f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：起步缓启动角加速度(deg/s²，<=0关闭)")]
|
||||
public float FleetRotateAccel = 20f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：到位后安定时长(s)")]
|
||||
public float FleetRotateSettleSec = 0.5f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：用Detour主车航向闭环判停(默认true，false=按时长开环)")]
|
||||
public bool FleetRotateUseDetourHeading = true;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：路径方向相对启动时车队朝向夹角(deg，逆时针为正；路径在车右侧x度时填-x)")]
|
||||
public float FleetCrabAngleDeg = 45f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：AGV入口使用的车队世界系目标朝向(deg)")]
|
||||
public float FleetCrabBodyWorldHeadingDeg = 0f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：路径长度(mm)")]
|
||||
public float FleetCrabLengthMm = 2000f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：行驶速度(m/s)")]
|
||||
public float FleetCrabSpeed = 0.2f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：速度命令加速度限制(m/s^2，<=0表示不限制)")]
|
||||
public float FleetCrabAccel = 0.2f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：预对齐后正式下发速度前5秒加速度(m/s^2，<=0表示不限制)")]
|
||||
public float FleetCrabStartAccel = 0.01f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：末端开始减速距离(mm)")]
|
||||
public float FleetCrabSlowDistance = 2000f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：完成距离(mm)，低于该剩余距离结束动作")]
|
||||
public float FleetCrabFinishDistance = 20f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：末端最低速度(m/s)")]
|
||||
public float FleetCrabFinishSpeed = 0.02f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：末端减速曲线指数")]
|
||||
public float FleetCrabSlowingPow = 0.8f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：GCP舵角修正上限(deg)")]
|
||||
public float FleetCrabGcpThetaThreshold = 95f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：headingErr角度纠偏比例系数")]
|
||||
public float FleetCrabDthLinearFac = 1f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：headingErr角度纠偏舵角限幅(deg)")]
|
||||
public float FleetCrabDthLinearThreshold = 10f;
|
||||
|
||||
[FieldMember(desc = "FleetCrab startup sync timeout(s)")]
|
||||
public float FleetCrabStartSyncTimeoutSec = 8f;
|
||||
|
||||
[FieldMember(desc = "FleetCrab startup wheel alignment tolerance(deg)")]
|
||||
public float FleetCrabStartWheelAlignDeg = 2f;
|
||||
|
||||
[FieldMember(desc = "FleetCurve MovementTest Bezier control point count")]
|
||||
public int FleetCurveTestControlPointCount = 4;
|
||||
|
||||
[FieldMember(desc = "FleetCurve speed(m/s)")]
|
||||
public float FleetCurveSpeed = 0.2f;
|
||||
|
||||
[FieldMember(desc = "FleetCurve slow distance(mm)")]
|
||||
public float FleetCurveSlowDistance = 2000f;
|
||||
|
||||
[FieldMember(desc = "FleetCurve finish distance(mm)")]
|
||||
public float FleetCurveFinishDistance = 20f;
|
||||
|
||||
[FieldMember(desc = "FleetCurve finish speed(m/s)")]
|
||||
public float FleetCurveFinishSpeed = 0.02f;
|
||||
|
||||
[FieldMember(desc = "FleetCurve slowing curve exponent")]
|
||||
public float FleetCurveSlowingPow = 0.8f;
|
||||
#endregion
|
||||
|
||||
#endif
|
||||
|
||||
#region 仿真-Playground
|
||||
[FieldMember(desc = "Playground WebAPI 基地址")]
|
||||
public string PlaygroundWebApiUrl = "http://localhost:18090";
|
||||
|
||||
[FieldMember(desc = "Playground 小车名称（场景 robots[].name）")]
|
||||
public string PlaygroundRobotName = "agv_multi_1";
|
||||
|
||||
[FieldMember(desc = "Playground 邻车名称（仅主车用于原地旋转位姿诊断）")]
|
||||
public string PlaygroundNeighborRobotName = "agv_multi_2";
|
||||
|
||||
[FieldMember(desc = "旋转位姿诊断开关(simulation only)")]
|
||||
public bool MultiVehicleRotatePoseWebApiDiagEnabled = false;
|
||||
[FieldMember(desc = "WebAPI 平移测试：平移距离(mm)")]
|
||||
public float WebApiTranslateMm = 100f;
|
||||
|
||||
[FieldMember(desc = "WebAPI 旋转测试：旋转角度(deg)")]
|
||||
public float WebApiRotateDeg = 5f;
|
||||
#endregion
|
||||
|
||||
|
||||
}
|
||||
|
||||
+1
-2115
File diff suppressed because it is too large
Load Diff
@@ -1,81 +0,0 @@
|
||||
using System;
|
||||
using System.Net.Http;
|
||||
using System.Text;
|
||||
using Newtonsoft.Json;
|
||||
using Newtonsoft.Json.Linq;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
/// <summary>
|
||||
/// Playground 仿真器 HTTP Web API 轻量客户端：查询小车位姿、瞬移小车。
|
||||
/// 服务端实现见 Playground/Web/PlaygroundWebApi.cs，默认监听 http://localhost:18090。
|
||||
/// 坐标单位 mm，朝向 yawDeg 单位为度，世界坐标系与场景 JSON 一致。
|
||||
/// </summary>
|
||||
public static class PlaygroundWebApi
|
||||
{
|
||||
// 禁用系统代理：本机 Playground 走 localhost，若经系统代理(如 127.0.0.1:7890)会连接失败。
|
||||
private static readonly HttpClient Http = new HttpClient(new HttpClientHandler { UseProxy = false })
|
||||
{
|
||||
Timeout = TimeSpan.FromSeconds(3)
|
||||
};
|
||||
|
||||
public struct Pose
|
||||
{
|
||||
public float X;
|
||||
public float Y;
|
||||
public float YawDeg;
|
||||
}
|
||||
|
||||
/// <summary>查询单台小车的世界位姿。GET /api/robots/{name}。</summary>
|
||||
public static Pose GetPose(string baseUrl, string robotName)
|
||||
{
|
||||
var url = $"{baseUrl.TrimEnd('/')}/api/robots/{Uri.EscapeDataString(robotName)}";
|
||||
var json = Http.GetStringAsync(url).GetAwaiter().GetResult();
|
||||
var o = JObject.Parse(json);
|
||||
return new Pose
|
||||
{
|
||||
X = o.Value<float>("x"),
|
||||
Y = o.Value<float>("y"),
|
||||
YawDeg = o.Value<float>("yawDeg")
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>将小车瞬移到目标世界位姿。POST /api/robots/{name}/move。</summary>
|
||||
public static void Move(string baseUrl, string robotName, float x, float y, float yawDeg)
|
||||
{
|
||||
var url = $"{baseUrl.TrimEnd('/')}/api/robots/{Uri.EscapeDataString(robotName)}/move";
|
||||
var body = JsonConvert.SerializeObject(new { x, y, yaw = yawDeg, stop = true });
|
||||
using var content = new StringContent(body, Encoding.UTF8, "application/json");
|
||||
var resp = Http.PostAsync(url, content).GetAwaiter().GetResult();
|
||||
resp.EnsureSuccessStatusCode();
|
||||
}
|
||||
|
||||
/// <summary>查询车辆运动是否启用（暂停时为 false）。GET /api/motion。</summary>
|
||||
public static bool MotionEnabled(string baseUrl)
|
||||
{
|
||||
var url = $"{baseUrl.TrimEnd('/')}/api/motion";
|
||||
var json = Http.GetStringAsync(url).GetAwaiter().GetResult();
|
||||
return JObject.Parse(json).Value<bool>("motionEnabled");
|
||||
}
|
||||
|
||||
/// <summary>恢复车辆运动。POST /api/motion/resume。</summary>
|
||||
public static void ResumeMotion(string baseUrl)
|
||||
{
|
||||
var url = $"{baseUrl.TrimEnd('/')}/api/motion/resume";
|
||||
var resp = Http.PostAsync(url, null).GetAwaiter().GetResult();
|
||||
resp.EnsureSuccessStatusCode();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 暂停车辆运动（仅冻结运动，不停止仿真；传感器继续扫描）。POST /api/motion/pause。
|
||||
/// feedback: "zero"(默认,反馈归零) / "none"(不上报) / "hold"(保留暂停瞬间值)。
|
||||
/// </summary>
|
||||
public static void PauseMotion(string baseUrl, string feedback = "zero")
|
||||
{
|
||||
var url = $"{baseUrl.TrimEnd('/')}/api/motion/pause";
|
||||
var body = JsonConvert.SerializeObject(new { feedback });
|
||||
using var content = new StringContent(body, Encoding.UTF8, "application/json");
|
||||
var resp = Http.PostAsync(url, content).GetAwaiter().GetResult();
|
||||
resp.EnsureSuccessStatusCode();
|
||||
}
|
||||
}
|
||||
@@ -1,511 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using System.Reflection;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using ClumsyCore.Utilities;
|
||||
using ClumsyDance.ClumsyWalk.Detectors;
|
||||
using CommonUsage.Chassis;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox;
|
||||
using MDCSToolBox.Clumsy.Calibration;
|
||||
using MDCSToolBox.Clumsy.MotionControllers;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using static ClumsyCore.DTools.Painter;
|
||||
using LineSegment = ClumsyCore.Utilities.LineSegment;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public class TireFollowing : MovementDefinition
|
||||
{
|
||||
public Func<AbstractGeometricController> GetController;
|
||||
|
||||
/// <summary>
|
||||
/// 车辆方向
|
||||
/// </summary>
|
||||
public float CarDirection = 0;
|
||||
|
||||
/// <summary>
|
||||
/// 停止距离
|
||||
/// </summary>
|
||||
//public float FinishDistance = 1000;
|
||||
|
||||
/// <summary>
|
||||
/// 减速距离
|
||||
/// </summary>
|
||||
public float SlowDistance = 1000;
|
||||
|
||||
/// <summary>
|
||||
/// 最大速度
|
||||
/// </summary>
|
||||
public float MaxSpeed = 0.3f;
|
||||
|
||||
// 末段衔接：接近盲走终点时给非零速度，供后续动作连续接管
|
||||
public bool EnableHandover = false;
|
||||
public float HandoverDistance = 200f; // mm
|
||||
public float HandoverSpeed = 0.2f; // m/s
|
||||
|
||||
/// <summary>
|
||||
/// 钻轮胎数量
|
||||
/// </summary>
|
||||
public int TireNum = 1;
|
||||
|
||||
/// <summary>
|
||||
/// 盲走角度偏移
|
||||
/// </summary>
|
||||
public float WalkBlindTh = -1f;
|
||||
|
||||
/// <summary>
|
||||
/// 是否检测到目标
|
||||
/// </summary>
|
||||
public bool NoTarget = false;
|
||||
|
||||
public float GuessRangeX;
|
||||
|
||||
public float GuessRangeY;
|
||||
|
||||
/// <summary>
|
||||
/// 检测器定义
|
||||
/// </summary>
|
||||
public class DetectorDefinition
|
||||
{
|
||||
/// <summary>
|
||||
/// 开始检测距离
|
||||
/// </summary>
|
||||
public float StartGuessingX;
|
||||
|
||||
/// <summary>
|
||||
/// 开始检测距离
|
||||
/// </summary>
|
||||
public float StartGuessingY;
|
||||
|
||||
/// <summary>
|
||||
/// 检测函数
|
||||
/// </summary>
|
||||
public Func<float, float, List<DetectFilter>, LineSegment> DetectFunction = null;
|
||||
|
||||
public Action<int> LeaveSrcFunction = null;
|
||||
|
||||
public int SrcId = -1;
|
||||
public int DstId = -1;
|
||||
|
||||
/// <summary>
|
||||
/// 路径偏移
|
||||
/// </summary>
|
||||
public Tuple<float, float, float> PathTransformation = Tuple.Create(0f, 0f, 0f);
|
||||
|
||||
public float PathTransformationAnchorDistance = 0f;
|
||||
|
||||
/// <summary>
|
||||
/// 切换条件
|
||||
/// </summary>
|
||||
public Func<float, bool> SwitchWalkBlindCondition = null;
|
||||
|
||||
/// <summary>
|
||||
/// 盲走停止距离
|
||||
/// </summary>
|
||||
public Func<float, bool> FinishWalkBlindCondition = null;
|
||||
}
|
||||
|
||||
public Func<bool> FinishCondition;
|
||||
|
||||
/// <summary>
|
||||
/// 多个检测器列表
|
||||
/// </summary>
|
||||
public List<DetectorDefinition> detectors = null;
|
||||
|
||||
private Painter _painter;
|
||||
private List<float> _remainDistanceList = new List<float>();
|
||||
private List<float> _remainAngleList = new List<float>();
|
||||
private List<float> _targetYList = new List<float>();
|
||||
|
||||
private List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
|
||||
{
|
||||
var painter = UI.GetPainter("GeneralFollowing.SetFilters", false);
|
||||
painter.Clear();
|
||||
painter.Clear(3000);
|
||||
|
||||
var box = new Vector2[]
|
||||
{
|
||||
new (guessCenterX - GuessRangeX, guessCenterY - GuessRangeY),
|
||||
new (guessCenterX + GuessRangeX, guessCenterY - GuessRangeY),
|
||||
new (guessCenterX + GuessRangeX, guessCenterY + GuessRangeY),
|
||||
new (guessCenterX - GuessRangeX, guessCenterY + GuessRangeY),
|
||||
};
|
||||
|
||||
for (var i = 0; i < box.Length; ++i)
|
||||
painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
|
||||
|
||||
// PC filter in car coordinate frame
|
||||
return new List<DetectFilter>()
|
||||
{
|
||||
new(CoordinateSystem.Car2D,
|
||||
p => LessMath.IsPointInPolygon4(
|
||||
box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))),
|
||||
};
|
||||
}
|
||||
|
||||
public void Stop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
///计算车体中心的位移和角度增量
|
||||
/// </summary>
|
||||
/// <param name="a">a轮在车体坐标系下位置</param>
|
||||
/// <param name="va">a轮在车体坐标系下位移增量</param>
|
||||
/// <param name="b">b轮在车体坐标系下位置</param>
|
||||
/// <param name="vb">b轮在车体坐标系下位移增量</param>
|
||||
/// <returns></returns>
|
||||
private static (float, float, float) CenterMoveFromPoints(Vector2 a,
|
||||
Vector2 aDelta,
|
||||
Vector2 b,
|
||||
Vector2 bDelta)
|
||||
{
|
||||
float th_x = 0, th_y = 0, th = 0, x = 0, y = 0;
|
||||
var eps = 0.0000001;
|
||||
if (Math.Abs(a.Y - b.Y) > eps)
|
||||
{
|
||||
th_x = (aDelta.X - bDelta.X) / (b.Y - a.Y);
|
||||
}
|
||||
if (Math.Abs(a.X - b.X) > eps)
|
||||
{
|
||||
th_y = (aDelta.Y - bDelta.Y) / (a.X - b.X);
|
||||
}
|
||||
th = th_x == 0 ? th_y : th_x;
|
||||
|
||||
x = (aDelta.X + bDelta.X) / 2f - (a.Y - b.Y) / 2f * th;
|
||||
|
||||
y = (aDelta.Y + bDelta.Y) / 2f + (a.X - b.X) / 2f * th;
|
||||
|
||||
return (x, y, th);
|
||||
}
|
||||
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
_painter = UI.GetPainter("GeneralFollowing", false);
|
||||
var lastDetectX = detectors[0].StartGuessingX;
|
||||
var lastDetectY = detectors[0].StartGuessingY;
|
||||
var detectorIndex = 0;
|
||||
|
||||
var controller = (MultiWheelGeometricController)GetController.Invoke();
|
||||
controller.BaseSpeed = MaxSpeed;
|
||||
controller.FinishDistance = float.MinValue;
|
||||
controller.FirstThAccuracy = 999;
|
||||
_dt = new DriveTask(controller.Track(true, CoordinateSystem.Car2D));
|
||||
void HardStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
((MultiWheelChassis)PilotDefinition.Chassis).DriveStop();
|
||||
DLog.Log($"Hard Stop!", "TireFollowing");
|
||||
}
|
||||
float WalkBlindCarPathDstX = -1f, WalkBlindCarPathDstY = -1f, WalkBlindCarPathDstTh = -1f;
|
||||
bool WalkBlindStage1 = false, WalkBlindStage2 = false;
|
||||
var angle2target = -1f;
|
||||
float _lastLFLEncoder = -1, _lastLFREncoder = -1, _lastRFLEncoder = -1, _lastRFREncoder = -1;
|
||||
float _lastLRLEncoder = -1, _lastLRREncoder = -1, _lastRRLEncoder = -1, _lastRRREncoder = -1;
|
||||
|
||||
(float, float, float) GetCurrentPos2Dst(float lastX, float lastY, float lastTh)
|
||||
{
|
||||
// Read current encoders
|
||||
var curLFLEncoder = PilotDefinition.Self.LFLActualPos;
|
||||
var curLFREncoder = PilotDefinition.Self.LFRActualPos;
|
||||
var curRFLEncoder = PilotDefinition.Self.RFLActualPos;
|
||||
var curRFREncoder = PilotDefinition.Self.RFRActualPos;
|
||||
var curLRLEncoder = PilotDefinition.Self.LRLActualPos;
|
||||
var curLRREncoder = PilotDefinition.Self.LRRActualPos;
|
||||
var curRRLEncoder = PilotDefinition.Self.RRLActualPos;
|
||||
var curRRREncoder = PilotDefinition.Self.RRRActualPos;
|
||||
|
||||
// Average delta per wheel pair (LF, LR, RF, RR)
|
||||
var lfDelta = (curLFLEncoder - _lastLFLEncoder + curLFREncoder - _lastLFREncoder) / 2f;
|
||||
var lrDelta = (curLRLEncoder - _lastLRLEncoder + curLRREncoder - _lastLRREncoder) / 2f;
|
||||
var rfDelta = (curRFLEncoder - _lastRFLEncoder + curRFREncoder - _lastRFREncoder) / 2f;
|
||||
var rrDelta = (curRRLEncoder - _lastRRLEncoder + curRRREncoder - _lastRRREncoder) / 2f;
|
||||
var deltaList = new List<float> { lfDelta, lrDelta, rfDelta, rrDelta };
|
||||
|
||||
var xs = new List<float>();
|
||||
var ys = new List<float>();
|
||||
var ths = new List<float>();
|
||||
var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
|
||||
var steerWheels = chassis.GetSteerWheels();
|
||||
for (var i = 0; i < steerWheels.Count; ++i)
|
||||
{
|
||||
var sw1 = steerWheels[i];
|
||||
var a = sw1.Position;
|
||||
var tha = sw1.ReadAngle() / 180f * (float)Math.PI;
|
||||
var deltaa = deltaList[i];
|
||||
var va = new Vector2(deltaa * (float)Math.Cos(tha), deltaa * (float)Math.Sin(tha));
|
||||
for (var j = i + 1; j < steerWheels.Count; ++j)
|
||||
{
|
||||
var sw2 = steerWheels[j];
|
||||
var b = sw2.Position;
|
||||
var thb = sw2.ReadAngle() / 180f * (float)Math.PI;
|
||||
var deltab = deltaList[j];
|
||||
var vb = new Vector2(deltab * (float)Math.Cos(thb), deltab * (float)Math.Sin(thb));
|
||||
var (tempx, tempy, tempth) = CenterMoveFromPoints(a, va, b, vb);
|
||||
Hedingben.ToastText($"{tempx:f2} {tempy:f2} {tempth / Math.PI * 180f:f2} ", $"{i}_{j}");
|
||||
xs.Add(tempx);
|
||||
ys.Add(tempy);
|
||||
ths.Add(tempth);
|
||||
}
|
||||
}
|
||||
var x = xs.Average();
|
||||
var y = ys.Average();
|
||||
var Th = ths.Average() / (float)Math.PI * 180;
|
||||
var moveTup = Tuple.Create(x, y, Th);
|
||||
var moved = MathTools.SolveTransform2D(MathTools.SolveTransform2D(Tuple.Create(lastX, lastY, lastTh), moveTup), Tuple.Create(0f, 0f, 0f));
|
||||
|
||||
_lastLFLEncoder = curLFLEncoder;
|
||||
_lastLFREncoder = curLFREncoder;
|
||||
_lastRFLEncoder = curRFLEncoder;
|
||||
_lastRFREncoder = curRFREncoder;
|
||||
_lastLRLEncoder = curLRLEncoder;
|
||||
_lastLRREncoder = curLRREncoder;
|
||||
_lastRRLEncoder = curRRLEncoder;
|
||||
_lastRRREncoder = curRRREncoder;
|
||||
return (moved.Item1, moved.Item2, moved.Item3);
|
||||
}
|
||||
|
||||
while (true)
|
||||
{
|
||||
if (detectorIndex > detectors.Count - 1)
|
||||
throw new Exception("detector index out of range!");
|
||||
|
||||
_painter.Clear();
|
||||
if (WalkBlindStage1 || WalkBlindStage2)
|
||||
{
|
||||
//第二次盲走时或只钻一个轮胎时
|
||||
if (WalkBlindStage2 || detectors.Count == 1 || TireNum == 1)
|
||||
{
|
||||
//controller.FinishDistance = 10f;
|
||||
controller.SlowDistance = SlowDistance;
|
||||
controller.SlowingPow = 0.7f;
|
||||
}
|
||||
if (EnableHandover)
|
||||
{
|
||||
controller.SlowDistance = float.MinValue;
|
||||
controller.FinishSpeed = 0.2f;
|
||||
controller.FinishDistance = 50;
|
||||
}
|
||||
(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh) = GetCurrentPos2Dst(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
|
||||
var walkBlindPathEnd = Tuple.Create(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
|
||||
var walkBlindPathStart = LessMath.Transform2D(walkBlindPathEnd, Tuple.Create(CarDirection == 0 ? -3000f : 3000f, 0f, 0f));
|
||||
var walkBlindPathDst = new Vector2(WalkBlindCarPathDstX, WalkBlindCarPathDstY);
|
||||
var walkBlindPathSrc = new Vector2(walkBlindPathStart.Item1, walkBlindPathStart.Item2);
|
||||
var walkBlindPath = new LineSegment(walkBlindPathSrc, walkBlindPathDst);
|
||||
|
||||
DLog.Log($"盲走目标点:{walkBlindPath.Src.X:F2} {walkBlindPath.Src.Y:F2} {walkBlindPath.Dst.X:F2} {walkBlindPath.Dst.Y:F2}", "TireFollowing");
|
||||
_painter.DrawDot(Color.Purple, walkBlindPathDst, sz: 3);
|
||||
_painter.DrawLine(Color.GreenYellow, walkBlindPath.Src, walkBlindPath.Dst, endArrow: true, width: 2);
|
||||
|
||||
var track = new LineTrack(walkBlindPath.Src, walkBlindPath.Dst);
|
||||
track.CarDirectionBias = CarDirection;
|
||||
controller.UpdateTracks(new List<AbstractTrack> { track });
|
||||
var rd = (float)LessMath.PerpendicularPosition(0, 0, walkBlindPath.Dst.X, walkBlindPath.Dst.Y,
|
||||
walkBlindPath.Src.X, walkBlindPath.Src.Y);
|
||||
_remainDistanceList.Add(rd);
|
||||
while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0);
|
||||
rd = _remainDistanceList.Average();
|
||||
DLog.Log($"盲走投影点剩余距离:{rd:0.0} ", "TireFollowing");
|
||||
// 检查是否达到盲走结束条件
|
||||
if (detectors[detectorIndex].FinishWalkBlindCondition(rd))
|
||||
{
|
||||
if (WalkBlindStage1)
|
||||
{
|
||||
DLog.Log("达到第一次盲走停止距离，停下或开始钻第二对轮胎", "TireFollowing");
|
||||
//if (detectors[detectorIndex].DstId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
|
||||
//{
|
||||
// detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].DstId);
|
||||
// DLog.Log($"释放取车点{detectors[detectorIndex].DstId}", "TireFollowing");
|
||||
//}
|
||||
WalkBlindStage1 = false;
|
||||
_remainAngleList.Clear();
|
||||
_remainDistanceList.Clear();
|
||||
detectorIndex++;
|
||||
if ((detectors.Count == 1 || TireNum == 1) && !EnableHandover)
|
||||
{
|
||||
HardStop();
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
else if (WalkBlindStage2)
|
||||
{
|
||||
DLog.Log("达到第二对轮胎处，停止移动", "TireFollowing");
|
||||
if (!EnableHandover)
|
||||
{
|
||||
HardStop();
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
yield return true;
|
||||
continue;
|
||||
}
|
||||
|
||||
var target = detectors[detectorIndex].DetectFunction(CarDirection, lastDetectX,
|
||||
SetFilters(lastDetectX, lastDetectY));
|
||||
|
||||
if (target == null)
|
||||
{
|
||||
DLog.Log("无目标，等待下一帧", "TireFollowing");
|
||||
controller.FirstRotateMaxSpeed = 0;
|
||||
yield return true;
|
||||
continue;
|
||||
}
|
||||
else controller.FirstRotateMaxSpeed = 5;
|
||||
|
||||
var targetAngle = CalculateAngle2YAxis(target.Src, target.Dst);
|
||||
var targetPos = new Vector2((target.Src.X + target.Dst.X) / 2f, (target.Src.Y + target.Dst.Y) / 2f);
|
||||
var dis2target = (float)Math.Sqrt(Math.Pow(targetPos.X, 2) + Math.Pow(targetPos.Y, 2));
|
||||
//距离较近以后角度容易跳变
|
||||
if (dis2target < PilotDefinition.Conf.TireFollowingCloseDistance && Math.Abs(targetAngle) > PilotDefinition.Conf.TireFollowingAngleIgnoreThr)
|
||||
{
|
||||
yield return true;
|
||||
continue;
|
||||
}
|
||||
else _remainAngleList.Add(targetAngle);
|
||||
while (_remainAngleList.Count > 10) _remainAngleList.RemoveAt(0);
|
||||
angle2target = _remainAngleList.Average();
|
||||
var distanceLabelPos = targetPos / 2f;
|
||||
_painter.DrawLine(Color.Cyan, Vector2.Zero, targetPos, width: 2);
|
||||
_painter.DrawText(Color.Yellow, $"{dis2target:F3}", distanceLabelPos.X, distanceLabelPos.Y);
|
||||
|
||||
var path = DetectorHelper.GetApproachPath(target, CoordinateSystem.Car2D, pathLen: 3000,
|
||||
bias: detectors[detectorIndex].PathTransformation,
|
||||
biasAnchorDistance: detectors[detectorIndex].PathTransformationAnchorDistance);
|
||||
|
||||
if (path == null)
|
||||
{
|
||||
DLog.Log("no path!", "TireFollowing");
|
||||
NoTarget = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
lastDetectX = ((target.Src + target.Dst) / 2f).X;
|
||||
lastDetectY = ((target.Src + target.Dst) / 2f).Y;
|
||||
|
||||
var currentY = path.CarPath.Dst.Y;
|
||||
if (Math.Abs(targetAngle) < PilotDefinition.Conf.TireFollowingAngleIgnoreThr &&
|
||||
dis2target < PilotDefinition.Conf.TireFollowingCloseDistance)
|
||||
{
|
||||
_targetYList.Add(currentY);
|
||||
while (_targetYList.Count > PilotDefinition.Conf.TireFollowingYAverageFrameCount) _targetYList.RemoveAt(0);
|
||||
}
|
||||
|
||||
var trackDstY = _targetYList.Count > 0 ? _targetYList.Average() : currentY;
|
||||
Hedingben.ToastText($"target Y:{_targetYList.Count} {trackDstY}", "target Y");
|
||||
|
||||
var trackDst = new Vector2(path.CarPath.Dst.X, trackDstY);
|
||||
_painter.DrawLine(Color.GreenYellow, path.CarPath.Src, trackDst, endArrow: true);
|
||||
|
||||
var rd = (float)LessMath.PerpendicularPosition(0, 0, trackDst.X, trackDst.Y,
|
||||
path.CarPath.Src.X, path.CarPath.Src.Y);
|
||||
_remainDistanceList.Add(rd);
|
||||
while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0);
|
||||
rd = _remainDistanceList.Average();
|
||||
_painter.DrawText(Color.Green, $"{rd:F3}", distanceLabelPos.X, distanceLabelPos.Y - 200);
|
||||
if(rd < PilotDefinition.Conf.TireFollowingReleaseDistance)
|
||||
{
|
||||
if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
|
||||
{
|
||||
detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
|
||||
DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
|
||||
}
|
||||
}
|
||||
|
||||
if (detectorIndex < detectors.Count - 1)
|
||||
{
|
||||
controller.SlowDistance = 1;
|
||||
if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
|
||||
{
|
||||
WalkBlindStage1 = true;
|
||||
//if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
|
||||
//{
|
||||
// detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
|
||||
// DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
|
||||
//}
|
||||
WalkBlindCarPathDstX = trackDst.X;
|
||||
WalkBlindCarPathDstY = trackDst.Y;
|
||||
WalkBlindCarPathDstTh = angle2target + WalkBlindTh;
|
||||
DLog.Log($"切换至第一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
|
||||
$"{WalkBlindCarPathDstY:F2} " +
|
||||
$"{WalkBlindCarPathDstTh:F2}", "TireFollowing");
|
||||
_lastLFLEncoder = PilotDefinition.Self.LFLActualPos;
|
||||
_lastLFREncoder = PilotDefinition.Self.LFRActualPos;
|
||||
_lastRFLEncoder = PilotDefinition.Self.RFLActualPos;
|
||||
_lastRFREncoder = PilotDefinition.Self.RFRActualPos;
|
||||
_lastLRLEncoder = PilotDefinition.Self.LRLActualPos;
|
||||
_lastLRREncoder = PilotDefinition.Self.LRRActualPos;
|
||||
_lastRRLEncoder = PilotDefinition.Self.RRLActualPos;
|
||||
_lastRRREncoder = PilotDefinition.Self.RRRActualPos;
|
||||
_remainDistanceList.Clear();
|
||||
_targetYList.Clear();
|
||||
lastDetectX = detectors[detectorIndex + 1].StartGuessingX;
|
||||
lastDetectY = detectors[detectorIndex + 1].StartGuessingY;
|
||||
continue;
|
||||
}
|
||||
}
|
||||
else if (detectorIndex == detectors.Count - 1)
|
||||
{
|
||||
if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
|
||||
{
|
||||
WalkBlindStage2 = true;
|
||||
WalkBlindCarPathDstX = trackDst.X;
|
||||
WalkBlindCarPathDstY = trackDst.Y;
|
||||
WalkBlindCarPathDstTh = angle2target + WalkBlindTh;
|
||||
DLog.Log($"切换至最后一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
|
||||
$"{WalkBlindCarPathDstY:F2} " +
|
||||
$"{WalkBlindCarPathDstTh:F2}", "TireFollowing");
|
||||
if (detectors.Count == 1)
|
||||
{
|
||||
if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
|
||||
{
|
||||
detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
|
||||
DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
|
||||
}
|
||||
}
|
||||
|
||||
_lastLFLEncoder = PilotDefinition.Self.LFLActualPos;
|
||||
_lastLFREncoder = PilotDefinition.Self.LFRActualPos;
|
||||
_lastRFLEncoder = PilotDefinition.Self.RFLActualPos;
|
||||
_lastRFREncoder = PilotDefinition.Self.RFRActualPos;
|
||||
_lastLRLEncoder = PilotDefinition.Self.LRLActualPos;
|
||||
_lastLRREncoder = PilotDefinition.Self.LRRActualPos;
|
||||
_lastRRLEncoder = PilotDefinition.Self.RRLActualPos;
|
||||
_lastRRREncoder = PilotDefinition.Self.RRRActualPos;
|
||||
_remainDistanceList.Clear();
|
||||
_targetYList.Clear();
|
||||
continue;
|
||||
}
|
||||
}
|
||||
DLog.Log($"投影点剩余距离:{rd:F2}", "TireFollowing");
|
||||
var track = new LineTrack(path.CarPath.Src, trackDst);
|
||||
track.CarDirectionBias = CarDirection;
|
||||
controller.UpdateTracks(new List<AbstractTrack> { track });
|
||||
NoTarget = false;
|
||||
}
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
|
||||
private static float CalculateAngle2YAxis(Vector2 point1, Vector2 point2)
|
||||
{
|
||||
return -(float)(Math.Atan((point1.X - point2.X) / (point1.Y - point2.Y)) * 180 / Math.PI);
|
||||
}
|
||||
|
||||
private DriveTask _dt;
|
||||
}
|
||||
}
|
||||
@@ -1,277 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.IO;
|
||||
using System.Text;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
internal static class VehicleSyncBinaryCodec
|
||||
{
|
||||
private const byte Version = 2;
|
||||
private const byte RegisterType = 1;
|
||||
private const byte NotificationType = 2;
|
||||
private static readonly byte[] Magic = Encoding.ASCII.GetBytes("MVS1");
|
||||
|
||||
public static byte[] EncodeRegister(int carNum, VehicleSyncInfo info)
|
||||
{
|
||||
using var stream = new MemoryStream();
|
||||
using var writer = new BinaryWriter(stream, Encoding.UTF8);
|
||||
WriteHeader(writer, RegisterType);
|
||||
writer.Write(carNum);
|
||||
WriteInfo(writer, info);
|
||||
writer.Flush();
|
||||
return stream.ToArray();
|
||||
}
|
||||
|
||||
public static (int CarNum, VehicleSyncInfo Info) DecodeRegister(byte[] payload)
|
||||
{
|
||||
using var stream = new MemoryStream(payload ?? throw new ArgumentNullException(nameof(payload)));
|
||||
using var reader = new BinaryReader(stream, Encoding.UTF8);
|
||||
var version = ReadHeader(reader, RegisterType);
|
||||
var carNum = reader.ReadInt32();
|
||||
var info = ReadInfo(reader, version);
|
||||
EnsureFullyRead(stream);
|
||||
return (carNum, info);
|
||||
}
|
||||
|
||||
public static byte[] EncodeNotification(VehicleSyncNotification notification)
|
||||
{
|
||||
using var stream = new MemoryStream();
|
||||
using var writer = new BinaryWriter(stream, Encoding.UTF8);
|
||||
WriteHeader(writer, NotificationType);
|
||||
|
||||
writer.Write(notification.Seq);
|
||||
writer.Write(BuildNotificationFlags(notification));
|
||||
writer.Write(notification.Mode);
|
||||
writer.Write(notification.FleetStopSourceCar);
|
||||
writer.Write(notification.CenterX);
|
||||
writer.Write(notification.CenterY);
|
||||
writer.Write(notification.CenterTh);
|
||||
writer.Write(notification.FleetVx);
|
||||
writer.Write(notification.FleetFrontTh);
|
||||
writer.Write(notification.FleetRearTh);
|
||||
writer.Write(notification.FleetOmega);
|
||||
writer.Write(notification.RequestedFleetOmega);
|
||||
writer.Write(notification.SyncTh);
|
||||
writer.Write(notification.SyncDistance);
|
||||
writer.Write(notification.DeltaDetectCenter);
|
||||
writer.Write(notification.RotateActiveOmega);
|
||||
writer.Write(notification.RotateCompXyFac);
|
||||
writer.Write(notification.RotateCompXyIFac);
|
||||
writer.Write(notification.RotateCompXyMax);
|
||||
writer.Write(notification.RotateCompThFac);
|
||||
writer.Write(notification.RotateCompThIFac);
|
||||
writer.Write(notification.RotateCompThMax);
|
||||
writer.Write(notification.RotateCompTangentFrac);
|
||||
writer.Write(notification.RotateStartWheelAlignDeg);
|
||||
writer.Write(notification.RotateActiveWheelAlignDeg);
|
||||
writer.Write(notification.IdealX);
|
||||
writer.Write(notification.IdealY);
|
||||
writer.Write(notification.IdealTh);
|
||||
WriteString(writer, notification.FleetStopReason);
|
||||
|
||||
var fleet = notification.Fleet ?? new Dictionary<int, VehicleSyncInfo>();
|
||||
if (fleet.Count > ushort.MaxValue)
|
||||
throw new InvalidOperationException($"Fleet count {fleet.Count} exceeds binary protocol limit.");
|
||||
writer.Write((ushort)fleet.Count);
|
||||
foreach (var kv in fleet)
|
||||
{
|
||||
writer.Write(kv.Key);
|
||||
WriteInfo(writer, kv.Value);
|
||||
}
|
||||
|
||||
writer.Flush();
|
||||
return stream.ToArray();
|
||||
}
|
||||
|
||||
public static VehicleSyncNotification DecodeNotification(byte[] payload)
|
||||
{
|
||||
using var stream = new MemoryStream(payload ?? throw new ArgumentNullException(nameof(payload)));
|
||||
using var reader = new BinaryReader(stream, Encoding.UTF8);
|
||||
var version = ReadHeader(reader, NotificationType);
|
||||
|
||||
var notification = new VehicleSyncNotification
|
||||
{
|
||||
Seq = reader.ReadInt64()
|
||||
};
|
||||
|
||||
ApplyNotificationFlags(notification, reader.ReadUInt16());
|
||||
notification.Mode = reader.ReadInt32();
|
||||
notification.FleetStopSourceCar = reader.ReadInt32();
|
||||
notification.CenterX = reader.ReadSingle();
|
||||
notification.CenterY = reader.ReadSingle();
|
||||
notification.CenterTh = reader.ReadSingle();
|
||||
notification.FleetVx = reader.ReadSingle();
|
||||
notification.FleetFrontTh = reader.ReadSingle();
|
||||
notification.FleetRearTh = reader.ReadSingle();
|
||||
notification.FleetOmega = reader.ReadSingle();
|
||||
notification.RequestedFleetOmega = reader.ReadSingle();
|
||||
notification.SyncTh = reader.ReadSingle();
|
||||
notification.SyncDistance = reader.ReadSingle();
|
||||
notification.DeltaDetectCenter = reader.ReadSingle();
|
||||
notification.RotateActiveOmega = reader.ReadSingle();
|
||||
notification.RotateCompXyFac = reader.ReadSingle();
|
||||
notification.RotateCompXyIFac = reader.ReadSingle();
|
||||
notification.RotateCompXyMax = reader.ReadSingle();
|
||||
notification.RotateCompThFac = reader.ReadSingle();
|
||||
notification.RotateCompThIFac = reader.ReadSingle();
|
||||
notification.RotateCompThMax = reader.ReadSingle();
|
||||
notification.RotateCompTangentFrac = reader.ReadSingle();
|
||||
notification.RotateStartWheelAlignDeg = reader.ReadSingle();
|
||||
notification.RotateActiveWheelAlignDeg = reader.ReadSingle();
|
||||
notification.IdealX = reader.ReadSingle();
|
||||
notification.IdealY = reader.ReadSingle();
|
||||
notification.IdealTh = reader.ReadSingle();
|
||||
notification.FleetStopReason = ReadString(reader);
|
||||
|
||||
var fleetCount = reader.ReadUInt16();
|
||||
notification.Fleet = new Dictionary<int, VehicleSyncInfo>(fleetCount);
|
||||
for (var i = 0; i < fleetCount; ++i)
|
||||
{
|
||||
var carNum = reader.ReadInt32();
|
||||
notification.Fleet[carNum] = ReadInfo(reader, version);
|
||||
}
|
||||
|
||||
EnsureFullyRead(stream);
|
||||
return notification;
|
||||
}
|
||||
|
||||
private static void WriteHeader(BinaryWriter writer, byte type)
|
||||
{
|
||||
writer.Write(Magic);
|
||||
writer.Write(Version);
|
||||
writer.Write(type);
|
||||
writer.Write((ushort)0);
|
||||
}
|
||||
|
||||
private static byte ReadHeader(BinaryReader reader, byte expectedType)
|
||||
{
|
||||
for (var i = 0; i < Magic.Length; ++i)
|
||||
{
|
||||
if (reader.ReadByte() != Magic[i])
|
||||
throw new InvalidDataException("Invalid multi-vehicle sync binary magic.");
|
||||
}
|
||||
|
||||
var version = reader.ReadByte();
|
||||
if (version < 1 || version > Version)
|
||||
throw new InvalidDataException($"Unsupported multi-vehicle sync binary version {version}.");
|
||||
|
||||
var type = reader.ReadByte();
|
||||
if (type != expectedType)
|
||||
throw new InvalidDataException($"Unexpected multi-vehicle sync packet type {type}.");
|
||||
|
||||
var reserved = reader.ReadUInt16();
|
||||
if (reserved != 0)
|
||||
throw new InvalidDataException("Invalid multi-vehicle sync binary reserved field.");
|
||||
|
||||
return version;
|
||||
}
|
||||
|
||||
private static void WriteInfo(BinaryWriter writer, VehicleSyncInfo info)
|
||||
{
|
||||
writer.Write(BuildInfoFlags(info));
|
||||
WriteString(writer, info.Ip);
|
||||
writer.Write(info.Port);
|
||||
writer.Write(info.X);
|
||||
writer.Write(info.Y);
|
||||
writer.Write(info.Th);
|
||||
writer.Write(info.LayoutX);
|
||||
writer.Write(info.LayoutY);
|
||||
writer.Write(info.LayoutTh);
|
||||
WriteString(writer, info.MotionInfeasibleReason);
|
||||
WriteString(writer, info.RotateWheelAlignDetail);
|
||||
writer.Write(info.AppliedNotificationSeq);
|
||||
}
|
||||
|
||||
private static VehicleSyncInfo ReadInfo(BinaryReader reader, byte version)
|
||||
{
|
||||
var info = new VehicleSyncInfo();
|
||||
ApplyInfoFlags(info, reader.ReadUInt16());
|
||||
info.Ip = ReadString(reader);
|
||||
info.Port = reader.ReadInt32();
|
||||
info.X = reader.ReadSingle();
|
||||
info.Y = reader.ReadSingle();
|
||||
info.Th = reader.ReadSingle();
|
||||
info.LayoutX = reader.ReadSingle();
|
||||
info.LayoutY = reader.ReadSingle();
|
||||
info.LayoutTh = reader.ReadSingle();
|
||||
info.MotionInfeasibleReason = ReadString(reader);
|
||||
info.RotateWheelAlignDetail = ReadString(reader);
|
||||
info.AppliedNotificationSeq = version >= 2 ? reader.ReadInt64() : -1;
|
||||
return info;
|
||||
}
|
||||
|
||||
private static ushort BuildInfoFlags(VehicleSyncInfo info)
|
||||
{
|
||||
ushort flags = 0;
|
||||
if (info.Master) flags |= 1 << 0;
|
||||
if (info.PosAvailable) flags |= 1 << 1;
|
||||
if (info.Aligned) flags |= 1 << 2;
|
||||
if (info.DetectOk) flags |= 1 << 3;
|
||||
if (info.MotionFeasible) flags |= 1 << 4;
|
||||
if (info.RotateWheelsAligned) flags |= 1 << 5;
|
||||
return flags;
|
||||
}
|
||||
|
||||
private static void ApplyInfoFlags(VehicleSyncInfo info, ushort flags)
|
||||
{
|
||||
info.Master = (flags & (1 << 0)) != 0;
|
||||
info.PosAvailable = (flags & (1 << 1)) != 0;
|
||||
info.Aligned = (flags & (1 << 2)) != 0;
|
||||
info.DetectOk = (flags & (1 << 3)) != 0;
|
||||
info.MotionFeasible = (flags & (1 << 4)) != 0;
|
||||
info.RotateWheelsAligned = (flags & (1 << 5)) != 0;
|
||||
}
|
||||
|
||||
private static ushort BuildNotificationFlags(VehicleSyncNotification notification)
|
||||
{
|
||||
ushort flags = 0;
|
||||
if (notification.PosAvailable) flags |= 1 << 0;
|
||||
if (notification.Aligned) flags |= 1 << 1;
|
||||
if (notification.FleetMotionReleased) flags |= 1 << 2;
|
||||
if (notification.FleetStopActive) flags |= 1 << 3;
|
||||
if (notification.AutoEnabled) flags |= 1 << 4;
|
||||
if (notification.ManualEnabled) flags |= 1 << 5;
|
||||
if (notification.HasIdeal) flags |= 1 << 6;
|
||||
if (notification.RotateParamsValid) flags |= 1 << 7;
|
||||
if (notification.UseDetourCorrection) flags |= 1 << 8;
|
||||
return flags;
|
||||
}
|
||||
|
||||
private static void ApplyNotificationFlags(VehicleSyncNotification notification, ushort flags)
|
||||
{
|
||||
notification.PosAvailable = (flags & (1 << 0)) != 0;
|
||||
notification.Aligned = (flags & (1 << 1)) != 0;
|
||||
notification.FleetMotionReleased = (flags & (1 << 2)) != 0;
|
||||
notification.FleetStopActive = (flags & (1 << 3)) != 0;
|
||||
notification.AutoEnabled = (flags & (1 << 4)) != 0;
|
||||
notification.ManualEnabled = (flags & (1 << 5)) != 0;
|
||||
notification.HasIdeal = (flags & (1 << 6)) != 0;
|
||||
notification.RotateParamsValid = (flags & (1 << 7)) != 0;
|
||||
notification.UseDetourCorrection = (flags & (1 << 8)) != 0;
|
||||
}
|
||||
|
||||
private static void WriteString(BinaryWriter writer, string value)
|
||||
{
|
||||
var bytes = Encoding.UTF8.GetBytes(value ?? "");
|
||||
if (bytes.Length > ushort.MaxValue)
|
||||
throw new InvalidOperationException($"String payload length {bytes.Length} exceeds binary protocol limit.");
|
||||
writer.Write((ushort)bytes.Length);
|
||||
writer.Write(bytes);
|
||||
}
|
||||
|
||||
private static string ReadString(BinaryReader reader)
|
||||
{
|
||||
var length = reader.ReadUInt16();
|
||||
var bytes = reader.ReadBytes(length);
|
||||
if (bytes.Length != length)
|
||||
throw new EndOfStreamException("Truncated multi-vehicle sync string payload.");
|
||||
return Encoding.UTF8.GetString(bytes);
|
||||
}
|
||||
|
||||
private static void EnsureFullyRead(MemoryStream stream)
|
||||
{
|
||||
if (stream.Position != stream.Length)
|
||||
throw new InvalidDataException("Unexpected trailing bytes in multi-vehicle sync packet.");
|
||||
}
|
||||
}
|
||||
@@ -1,75 +0,0 @@
|
||||
using System.Collections.Generic;
|
||||
using ClumsyCore;
|
||||
using Newtonsoft.Json;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class VehicleSyncInfo
|
||||
{
|
||||
[JsonProperty("Master")] public bool Master { get; set; }
|
||||
[JsonProperty("Ip")] public string Ip { get; set; } = "";
|
||||
[JsonProperty("Port")] public int Port { get; set; } = 8008;
|
||||
[JsonProperty("PosAvailable")] public bool PosAvailable { get; set; }
|
||||
[JsonProperty("X")] public float X { get; set; }
|
||||
[JsonProperty("Y")] public float Y { get; set; }
|
||||
[JsonProperty("Th")] public float Th { get; set; }
|
||||
[JsonProperty("LayoutX")] public float LayoutX { get; set; }
|
||||
[JsonProperty("LayoutY")] public float LayoutY { get; set; }
|
||||
[JsonProperty("LayoutTh")] public float LayoutTh { get; set; }
|
||||
[JsonProperty("Aligned")] public bool Aligned { get; set; }
|
||||
// 本车本轮是否成功识别到邻车（关闭互识别时恒为 true）。任一车为 false 则整队停车。
|
||||
[JsonProperty("DetectOk")] public bool DetectOk { get; set; }
|
||||
[JsonProperty("MotionFeasible")] public bool MotionFeasible { get; set; } = true;
|
||||
[JsonProperty("MotionInfeasibleReason")] public string MotionInfeasibleReason { get; set; } = "";
|
||||
[JsonProperty("RotateWheelsAligned")] public bool RotateWheelsAligned { get; set; } = true;
|
||||
[JsonProperty("RotateWheelAlignDetail")] public string RotateWheelAlignDetail { get; set; } = "";
|
||||
[JsonProperty("AppliedNotificationSeq")] public long AppliedNotificationSeq { get; set; } = -1;
|
||||
}
|
||||
|
||||
public class VehicleSyncNotification
|
||||
{
|
||||
[JsonProperty("PosAvailable")] public bool PosAvailable { get; set; }
|
||||
[JsonProperty("CenterX")] public float CenterX { get; set; }
|
||||
[JsonProperty("CenterY")] public float CenterY { get; set; }
|
||||
[JsonProperty("CenterTh")] public float CenterTh { get; set; }
|
||||
[JsonProperty("Aligned")] public bool Aligned { get; set; }
|
||||
[JsonProperty("Fleet")] public Dictionary<int, VehicleSyncInfo> Fleet { get; set; } = new();
|
||||
[JsonProperty("FleetVx")] public float FleetVx { get; set; }
|
||||
[JsonProperty("FleetFrontTh")] public float FleetFrontTh { get; set; }
|
||||
[JsonProperty("FleetRearTh")] public float FleetRearTh { get; set; }
|
||||
// 联动运动模式：0=常规(前进+转向) 1=蟹行(四轮同向平移) 2=原地旋转(绕车队中心)
|
||||
[JsonProperty("Mode")] public int Mode { get; set; }
|
||||
// 原地旋转角速度(deg/s，逆时针为正)，仅 Mode==2 有效
|
||||
[JsonProperty("FleetOmega")] public float FleetOmega { get; set; }
|
||||
[JsonProperty("RequestedFleetOmega")] public float RequestedFleetOmega { get; set; }
|
||||
[JsonProperty("FleetMotionReleased")] public bool FleetMotionReleased { get; set; } = true;
|
||||
[JsonProperty("FleetStopActive")] public bool FleetStopActive { get; set; }
|
||||
[JsonProperty("FleetStopReason")] public string FleetStopReason { get; set; } = "";
|
||||
[JsonProperty("FleetStopSourceCar")] public int FleetStopSourceCar { get; set; }
|
||||
[JsonProperty("AutoEnabled")] public bool AutoEnabled { get; set; }
|
||||
[JsonProperty("ManualEnabled")] public bool ManualEnabled { get; set; }
|
||||
[JsonProperty("UseDetourCorrection")] public bool UseDetourCorrection { get; set; }
|
||||
[JsonProperty("SyncTh")] public float SyncTh { get; set; }
|
||||
[JsonProperty("SyncDistance")] public float SyncDistance { get; set; }
|
||||
[JsonProperty("DeltaDetectCenter")] public float DeltaDetectCenter { get; set; }
|
||||
// 原地旋转纠偏参数由主车广播，从车运行时使用同一套增益/限幅，避免主从补偿强度不一致。
|
||||
[JsonProperty("RotateParamsValid")] public bool RotateParamsValid { get; set; }
|
||||
[JsonProperty("RotateActiveOmega")] public float RotateActiveOmega { get; set; }
|
||||
[JsonProperty("RotateCompXyFac")] public float RotateCompXyFac { get; set; }
|
||||
[JsonProperty("RotateCompXyIFac")] public float RotateCompXyIFac { get; set; }
|
||||
[JsonProperty("RotateCompXyMax")] public float RotateCompXyMax { get; set; }
|
||||
[JsonProperty("RotateCompThFac")] public float RotateCompThFac { get; set; }
|
||||
[JsonProperty("RotateCompThIFac")] public float RotateCompThIFac { get; set; }
|
||||
[JsonProperty("RotateCompThMax")] public float RotateCompThMax { get; set; }
|
||||
[JsonProperty("RotateCompTangentFrac")] public float RotateCompTangentFrac { get; set; }
|
||||
[JsonProperty("RotateStartWheelAlignDeg")] public float RotateStartWheelAlignDeg { get; set; }
|
||||
[JsonProperty("RotateActiveWheelAlignDeg")] public float RotateActiveWheelAlignDeg { get; set; }
|
||||
// F: 单调递增序列号，从车据此丢弃乱序到达的旧 notify 包。
|
||||
[JsonProperty("Seq")] public long Seq { get; set; }
|
||||
// D: 自动模式下主车路径控制器算出的车队中心理想位姿（世界系），由 idealPos/idealAngle 透传而来。
|
||||
// HasIdeal=true 时各从车按各自 layout 推算 per-car 目标位姿做前馈+补偿，弧线路径不再只靠事后纠偏。
|
||||
[JsonProperty("HasIdeal")] public bool HasIdeal { get; set; }
|
||||
[JsonProperty("IdealX")] public float IdealX { get; set; }
|
||||
[JsonProperty("IdealY")] public float IdealY { get; set; }
|
||||
[JsonProperty("IdealTh")] public float IdealTh { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,163 @@
|
||||
{
|
||||
"runtimeTarget": {
|
||||
"name": ".NETStandard,Version=v2.0/",
|
||||
"signature": ""
|
||||
},
|
||||
"compilationOptions": {},
|
||||
"targets": {
|
||||
".NETStandard,Version=v2.0": {},
|
||||
".NETStandard,Version=v2.0/": {
|
||||
"ClumsyPilot/1.0.0": {
|
||||
"dependencies": {
|
||||
"NETStandard.Library": "2.0.3",
|
||||
"Newtonsoft.Json": "13.0.3",
|
||||
"System.Numerics.Vectors": "4.6.1",
|
||||
"CommonUsage": "1.0.0.0",
|
||||
"LessokajiWeaverUtilities": "1.0.0.0",
|
||||
"MDCSToolBox": "1.0.0.0",
|
||||
"RefClumsyCore": "0.0.0.0",
|
||||
"RefClumsyDance": "0.0.0.0",
|
||||
"RefFundamentalLib": "0.0.0.0"
|
||||
},
|
||||
"runtime": {
|
||||
"ClumsyPilot.dll": {}
|
||||
}
|
||||
},
|
||||
"Microsoft.NETCore.Platforms/1.1.0": {},
|
||||
"NETStandard.Library/2.0.3": {
|
||||
"dependencies": {
|
||||
"Microsoft.NETCore.Platforms": "1.1.0"
|
||||
}
|
||||
},
|
||||
"Newtonsoft.Json/13.0.3": {
|
||||
"runtime": {
|
||||
"lib/netstandard2.0/Newtonsoft.Json.dll": {
|
||||
"assemblyVersion": "13.0.0.0",
|
||||
"fileVersion": "13.0.3.27908"
|
||||
}
|
||||
}
|
||||
},
|
||||
"System.Numerics.Vectors/4.6.1": {
|
||||
"runtime": {
|
||||
"lib/netstandard2.0/System.Numerics.Vectors.dll": {
|
||||
"assemblyVersion": "4.1.3.0",
|
||||
"fileVersion": "4.600.125.16908"
|
||||
}
|
||||
}
|
||||
},
|
||||
"CommonUsage/1.0.0.0": {
|
||||
"runtime": {
|
||||
"CommonUsage.dll": {
|
||||
"assemblyVersion": "1.0.0.0",
|
||||
"fileVersion": "1.0.0.0"
|
||||
}
|
||||
}
|
||||
},
|
||||
"LessokajiWeaverUtilities/1.0.0.0": {
|
||||
"runtime": {
|
||||
"LessokajiWeaverUtilities.dll": {
|
||||
"assemblyVersion": "1.0.0.0",
|
||||
"fileVersion": "1.0.0.0"
|
||||
}
|
||||
}
|
||||
},
|
||||
"MDCSToolBox/1.0.0.0": {
|
||||
"runtime": {
|
||||
"MDCSToolBox.dll": {
|
||||
"assemblyVersion": "1.0.0.0",
|
||||
"fileVersion": "1.0.0.0"
|
||||
}
|
||||
}
|
||||
},
|
||||
"RefClumsyCore/0.0.0.0": {
|
||||
"runtime": {
|
||||
"RefClumsyCore.dll": {
|
||||
"assemblyVersion": "0.0.0.0",
|
||||
"fileVersion": "0.0.0.0"
|
||||
}
|
||||
}
|
||||
},
|
||||
"RefClumsyDance/0.0.0.0": {
|
||||
"runtime": {
|
||||
"RefClumsyDance.dll": {
|
||||
"assemblyVersion": "0.0.0.0",
|
||||
"fileVersion": "0.0.0.0"
|
||||
}
|
||||
}
|
||||
},
|
||||
"RefFundamentalLib/0.0.0.0": {
|
||||
"runtime": {
|
||||
"RefFundamentalLib.dll": {
|
||||
"assemblyVersion": "0.0.0.0",
|
||||
"fileVersion": "0.0.0.0"
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
"libraries": {
|
||||
"ClumsyPilot/1.0.0": {
|
||||
"type": "project",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"Microsoft.NETCore.Platforms/1.1.0": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-kz0PEW2lhqygehI/d6XsPCQzD7ff7gUJaVGPVETX611eadGsA3A877GdSlU0LRVMCTH/+P3o2iDTak+S08V2+A==",
|
||||
"path": "microsoft.netcore.platforms/1.1.0",
|
||||
"hashPath": "microsoft.netcore.platforms.1.1.0.nupkg.sha512"
|
||||
},
|
||||
"NETStandard.Library/2.0.3": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-st47PosZSHrjECdjeIzZQbzivYBJFv6P2nv4cj2ypdI204DO+vZ7l5raGMiX4eXMJ53RfOIg+/s4DHVZ54Nu2A==",
|
||||
"path": "netstandard.library/2.0.3",
|
||||
"hashPath": "netstandard.library.2.0.3.nupkg.sha512"
|
||||
},
|
||||
"Newtonsoft.Json/13.0.3": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-HrC5BXdl00IP9zeV+0Z848QWPAoCr9P3bDEZguI+gkLcBKAOxix/tLEAAHC+UvDNPv4a2d18lOReHMOagPa+zQ==",
|
||||
"path": "newtonsoft.json/13.0.3",
|
||||
"hashPath": "newtonsoft.json.13.0.3.nupkg.sha512"
|
||||
},
|
||||
"System.Numerics.Vectors/4.6.1": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-sQxefTnhagrhoq2ReR0D/6K0zJcr9Hrd6kikeXsA1I8kOCboTavcUC4r7TSfpKFeE163uMuxZcyfO1mGO3EN8Q==",
|
||||
"path": "system.numerics.vectors/4.6.1",
|
||||
"hashPath": "system.numerics.vectors.4.6.1.nupkg.sha512"
|
||||
},
|
||||
"CommonUsage/1.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"LessokajiWeaverUtilities/1.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"MDCSToolBox/1.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"RefClumsyCore/0.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"RefClumsyDance/0.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"RefFundamentalLib/0.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
}
|
||||
}
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,23 +1,23 @@
|
||||
{
|
||||
"format": 1,
|
||||
"restore": {
|
||||
"D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj": {}
|
||||
"D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj": {}
|
||||
},
|
||||
"projects": {
|
||||
"D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj": {
|
||||
"D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj": {
|
||||
"version": "1.0.0",
|
||||
"restore": {
|
||||
"projectUniqueName": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
|
||||
"projectUniqueName": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
|
||||
"projectName": "ClumsyPilot",
|
||||
"projectPath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
|
||||
"packagesPath": "C:\\Users\\Fairyland\\.nuget\\packages\\",
|
||||
"outputPath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\obj\\",
|
||||
"projectPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
|
||||
"packagesPath": "C:\\Users\\admin\\.nuget\\packages\\",
|
||||
"outputPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\obj\\",
|
||||
"projectStyle": "PackageReference",
|
||||
"fallbackFolders": [
|
||||
"D:\\VS2022\\Shared\\NuGetPackages"
|
||||
"C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages"
|
||||
],
|
||||
"configFilePaths": [
|
||||
"C:\\Users\\Fairyland\\AppData\\Roaming\\NuGet\\NuGet.Config",
|
||||
"C:\\Users\\admin\\AppData\\Roaming\\NuGet\\NuGet.Config",
|
||||
"C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.FallbackLocation.config",
|
||||
"C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.Offline.config"
|
||||
],
|
||||
@@ -58,7 +58,7 @@
|
||||
},
|
||||
"Newtonsoft.Json": {
|
||||
"target": "Package",
|
||||
"version": "[13.0.4, )"
|
||||
"version": "[13.0.3, )"
|
||||
},
|
||||
"System.Numerics.Vectors": {
|
||||
"target": "Package",
|
||||
@@ -76,7 +76,7 @@
|
||||
],
|
||||
"assetTargetFallback": true,
|
||||
"warn": true,
|
||||
"runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.308\\RuntimeIdentifierGraph.json"
|
||||
"runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.316\\RuntimeIdentifierGraph.json"
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -5,12 +5,12 @@
|
||||
<RestoreTool Condition=" '$(RestoreTool)' == '' ">NuGet</RestoreTool>
|
||||
<ProjectAssetsFile Condition=" '$(ProjectAssetsFile)' == '' ">$(MSBuildThisFileDirectory)project.assets.json</ProjectAssetsFile>
|
||||
<NuGetPackageRoot Condition=" '$(NuGetPackageRoot)' == '' ">$(UserProfile)\.nuget\packages\</NuGetPackageRoot>
|
||||
<NuGetPackageFolders Condition=" '$(NuGetPackageFolders)' == '' ">C:\Users\Fairyland\.nuget\packages\;D:\VS2022\Shared\NuGetPackages</NuGetPackageFolders>
|
||||
<NuGetPackageFolders Condition=" '$(NuGetPackageFolders)' == '' ">C:\Users\admin\.nuget\packages\;C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages</NuGetPackageFolders>
|
||||
<NuGetProjectStyle Condition=" '$(NuGetProjectStyle)' == '' ">PackageReference</NuGetProjectStyle>
|
||||
<NuGetToolVersion Condition=" '$(NuGetToolVersion)' == '' ">6.14.2</NuGetToolVersion>
|
||||
<NuGetToolVersion Condition=" '$(NuGetToolVersion)' == '' ">6.14.3</NuGetToolVersion>
|
||||
</PropertyGroup>
|
||||
<ItemGroup Condition=" '$(ExcludeRestorePackageImports)' != 'true' ">
|
||||
<SourceRoot Include="C:\Users\Fairyland\.nuget\packages\" />
|
||||
<SourceRoot Include="D:\VS2022\Shared\NuGetPackages\" />
|
||||
<SourceRoot Include="C:\Users\admin\.nuget\packages\" />
|
||||
<SourceRoot Include="C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages\" />
|
||||
</ItemGroup>
|
||||
</Project>
|
||||
@@ -1,10 +1,9 @@
|
||||
//------------------------------------------------------------------------------
|
||||
// <auto-generated>
|
||||
// 此代码由工具生成。
|
||||
// 运行时版本:4.0.30319.42000
|
||||
// This code was generated by a tool.
|
||||
//
|
||||
// 对此文件的更改可能会导致不正确的行为，并且如果
|
||||
// 重新生成代码，这些更改将会丢失。
|
||||
// Changes to this file may cause incorrect behavior and will be lost if
|
||||
// the code is regenerated.
|
||||
// </auto-generated>
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
is_global = true
|
||||
build_property.RootNamespace = ClumsyPilot
|
||||
build_property.ProjectDir = D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\
|
||||
build_property.RootNamespace = MultiWheelC
|
||||
build_property.ProjectDir = d:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\
|
||||
build_property.EnableComHosting =
|
||||
build_property.EnableGeneratedComInterfaceComImportInterop =
|
||||
build_property.CsWinRTUseWindowsUIXamlProjections = false
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -1 +1 @@
|
||||
622a214d5f25f6580d0c98fb28e188cba923d74f280b70d75d03c4009c18c55b
|
||||
cbba0ac806b0617cc6f7209f71f12565d8a56471fa82abdd118fca6c967d6a5a
|
||||
|
||||
@@ -1,11 +1,34 @@
|
||||
D:\@FariyLandTask\@FRLD-GitProject\build\Clumsy\ClumsyPilot.deps.json
|
||||
D:\@FariyLandTask\@FRLD-GitProject\build\Clumsy\ClumsyPilot.dll
|
||||
D:\@FariyLandTask\@FRLD-GitProject\build\Clumsy\ClumsyPilot.pdb
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.csproj.AssemblyReference.cache
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.AssemblyInfoInputs.cache
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.AssemblyInfo.cs
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.csproj.CoreCompileInputs.cache
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPi.5EF10E9F.Up2Date
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.dll
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.pdb
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\ClumsyPilot.deps.json
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\ClumsyPilot.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\ClumsyPilot.pdb
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\CommonUsage.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\LessokajiWeaverUtilities.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\MDCSToolBox.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\RefClumsyCore.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\RefClumsyDance.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\RefFundamentalLib.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.csproj.AssemblyReference.cache
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.AssemblyInfoInputs.cache
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.AssemblyInfo.cs
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.csproj.CoreCompileInputs.cache
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPi.5EF10E9F.Up2Date
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.pdb
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\ClumsyPilot.deps.json
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\ClumsyPilot.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\ClumsyPilot.pdb
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\CommonUsage.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\LessokajiWeaverUtilities.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\MDCSToolBox.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\RefClumsyCore.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\RefClumsyDance.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\RefFundamentalLib.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.csproj.AssemblyReference.cache
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.AssemblyInfoInputs.cache
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.AssemblyInfo.cs
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.csproj.CoreCompileInputs.cache
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPi.5EF10E9F.Up2Date
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.pdb
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -1,4 +0,0 @@
|
||||
// <autogenerated />
|
||||
using System;
|
||||
using System.Reflection;
|
||||
[assembly: global::System.Runtime.Versioning.TargetFrameworkAttribute(".NETStandard,Version=v2.0", FrameworkDisplayName = ".NET Standard 2.0")]
|
||||
@@ -1,23 +0,0 @@
|
||||
//------------------------------------------------------------------------------
|
||||
// <auto-generated>
|
||||
// 此代码由工具生成。
|
||||
// 运行时版本:4.0.30319.42000
|
||||
//
|
||||
// 对此文件的更改可能会导致不正确的行为，并且如果
|
||||
// 重新生成代码，这些更改将会丢失。
|
||||
// </auto-generated>
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
using System;
|
||||
using System.Reflection;
|
||||
|
||||
[assembly: System.Reflection.AssemblyCompanyAttribute("ClumsyPilot")]
|
||||
[assembly: System.Reflection.AssemblyConfigurationAttribute("Debug")]
|
||||
[assembly: System.Reflection.AssemblyFileVersionAttribute("1.0.0.0")]
|
||||
[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0")]
|
||||
[assembly: System.Reflection.AssemblyProductAttribute("ClumsyPilot")]
|
||||
[assembly: System.Reflection.AssemblyTitleAttribute("ClumsyPilot")]
|
||||
[assembly: System.Reflection.AssemblyVersionAttribute("1.0.0.0")]
|
||||
|
||||
// 由 MSBuild WriteCodeFragment 类生成。
|
||||
|
||||
@@ -1 +0,0 @@
|
||||
5d77794fa0720c6591db5b06ac60427413c18989a6f7b64420ccb07d122d85bc
|
||||
-8
@@ -1,8 +0,0 @@
|
||||
is_global = true
|
||||
build_property.RootNamespace = ClumsyPilot
|
||||
build_property.ProjectDir = D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\
|
||||
build_property.EnableComHosting =
|
||||
build_property.EnableGeneratedComInterfaceComImportInterop =
|
||||
build_property.CsWinRTUseWindowsUIXamlProjections = false
|
||||
build_property.EffectiveAnalysisLevelStyle =
|
||||
build_property.EnableCodeStyleSeverity =
|
||||
Binary file not shown.
Binary file not shown.
@@ -1 +0,0 @@
|
||||
66346c5ecc38869ab6b6f447e1f54c24aba69f81ee10b5736dee140cd5a474b8
|
||||
@@ -1,5 +0,0 @@
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\netstandard2.0\ClumsyPilot.csproj.AssemblyReference.cache
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\netstandard2.0\ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\netstandard2.0\ClumsyPilot.AssemblyInfoInputs.cache
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\netstandard2.0\ClumsyPilot.AssemblyInfo.cs
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\netstandard2.0\ClumsyPilot.csproj.CoreCompileInputs.cache
|
||||
@@ -26,7 +26,7 @@
|
||||
"build/netstandard2.0/NETStandard.Library.targets": {}
|
||||
}
|
||||
},
|
||||
"Newtonsoft.Json/13.0.4": {
|
||||
"Newtonsoft.Json/13.0.3": {
|
||||
"type": "package",
|
||||
"compile": {
|
||||
"lib/netstandard2.0/Newtonsoft.Json.dll": {
|
||||
@@ -199,10 +199,10 @@
|
||||
"netstandard.library.nuspec"
|
||||
]
|
||||
},
|
||||
"Newtonsoft.Json/13.0.4": {
|
||||
"sha512": "pdgNNMai3zv51W5aq268sujXUyx7SNdE2bj1wZcWjAQrKMFZV260lbqYop1d2GM67JI1huLRwxo9ZqnfF/lC6A==",
|
||||
"Newtonsoft.Json/13.0.3": {
|
||||
"sha512": "HrC5BXdl00IP9zeV+0Z848QWPAoCr9P3bDEZguI+gkLcBKAOxix/tLEAAHC+UvDNPv4a2d18lOReHMOagPa+zQ==",
|
||||
"type": "package",
|
||||
"path": "newtonsoft.json/13.0.4",
|
||||
"path": "newtonsoft.json/13.0.3",
|
||||
"files": [
|
||||
".nupkg.metadata",
|
||||
".signature.p7s",
|
||||
@@ -224,7 +224,7 @@
|
||||
"lib/netstandard1.3/Newtonsoft.Json.xml",
|
||||
"lib/netstandard2.0/Newtonsoft.Json.dll",
|
||||
"lib/netstandard2.0/Newtonsoft.Json.xml",
|
||||
"newtonsoft.json.13.0.4.nupkg.sha512",
|
||||
"newtonsoft.json.13.0.3.nupkg.sha512",
|
||||
"newtonsoft.json.nuspec",
|
||||
"packageIcon.png"
|
||||
]
|
||||
@@ -254,28 +254,28 @@
|
||||
"projectFileDependencyGroups": {
|
||||
".NETStandard,Version=v2.0": [
|
||||
"NETStandard.Library >= 2.0.3",
|
||||
"Newtonsoft.Json >= 13.0.4",
|
||||
"Newtonsoft.Json >= 13.0.3",
|
||||
"System.Numerics.Vectors >= 4.6.1"
|
||||
]
|
||||
},
|
||||
"packageFolders": {
|
||||
"C:\\Users\\Fairyland\\.nuget\\packages\\": {},
|
||||
"D:\\VS2022\\Shared\\NuGetPackages": {}
|
||||
"C:\\Users\\admin\\.nuget\\packages\\": {},
|
||||
"C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages": {}
|
||||
},
|
||||
"project": {
|
||||
"version": "1.0.0",
|
||||
"restore": {
|
||||
"projectUniqueName": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
|
||||
"projectUniqueName": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
|
||||
"projectName": "ClumsyPilot",
|
||||
"projectPath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
|
||||
"packagesPath": "C:\\Users\\Fairyland\\.nuget\\packages\\",
|
||||
"outputPath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\obj\\",
|
||||
"projectPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
|
||||
"packagesPath": "C:\\Users\\admin\\.nuget\\packages\\",
|
||||
"outputPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\obj\\",
|
||||
"projectStyle": "PackageReference",
|
||||
"fallbackFolders": [
|
||||
"D:\\VS2022\\Shared\\NuGetPackages"
|
||||
"C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages"
|
||||
],
|
||||
"configFilePaths": [
|
||||
"C:\\Users\\Fairyland\\AppData\\Roaming\\NuGet\\NuGet.Config",
|
||||
"C:\\Users\\admin\\AppData\\Roaming\\NuGet\\NuGet.Config",
|
||||
"C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.FallbackLocation.config",
|
||||
"C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.Offline.config"
|
||||
],
|
||||
@@ -316,7 +316,7 @@
|
||||
},
|
||||
"Newtonsoft.Json": {
|
||||
"target": "Package",
|
||||
"version": "[13.0.4, )"
|
||||
"version": "[13.0.3, )"
|
||||
},
|
||||
"System.Numerics.Vectors": {
|
||||
"target": "Package",
|
||||
@@ -334,7 +334,7 @@
|
||||
],
|
||||
"assetTargetFallback": true,
|
||||
"warn": true,
|
||||
"runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.308\\RuntimeIdentifierGraph.json"
|
||||
"runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.316\\RuntimeIdentifierGraph.json"
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,13 +1,13 @@
|
||||
{
|
||||
"version": 2,
|
||||
"dgSpecHash": "gHGYwM2i+So=",
|
||||
"dgSpecHash": "YBvImiCcgSo=",
|
||||
"success": true,
|
||||
"projectFilePath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
|
||||
"projectFilePath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
|
||||
"expectedPackageFiles": [
|
||||
"C:\\Users\\Fairyland\\.nuget\\packages\\microsoft.netcore.platforms\\1.1.0\\microsoft.netcore.platforms.1.1.0.nupkg.sha512",
|
||||
"C:\\Users\\Fairyland\\.nuget\\packages\\netstandard.library\\2.0.3\\netstandard.library.2.0.3.nupkg.sha512",
|
||||
"C:\\Users\\Fairyland\\.nuget\\packages\\newtonsoft.json\\13.0.4\\newtonsoft.json.13.0.4.nupkg.sha512",
|
||||
"C:\\Users\\Fairyland\\.nuget\\packages\\system.numerics.vectors\\4.6.1\\system.numerics.vectors.4.6.1.nupkg.sha512"
|
||||
"C:\\Users\\admin\\.nuget\\packages\\microsoft.netcore.platforms\\1.1.0\\microsoft.netcore.platforms.1.1.0.nupkg.sha512",
|
||||
"C:\\Users\\admin\\.nuget\\packages\\netstandard.library\\2.0.3\\netstandard.library.2.0.3.nupkg.sha512",
|
||||
"C:\\Users\\admin\\.nuget\\packages\\newtonsoft.json\\13.0.3\\newtonsoft.json.13.0.3.nupkg.sha512",
|
||||
"C:\\Users\\admin\\.nuget\\packages\\system.numerics.vectors\\4.6.1\\system.numerics.vectors.4.6.1.nupkg.sha512"
|
||||
],
|
||||
"logs": []
|
||||
}
|
||||
@@ -1,21 +1 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using CartActivator;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Medulla.Chassis;
|
||||
using MDCSToolBox.Medulla.Chassis.MultiWheel;
|
||||
|
||||
namespace MedullaAdapter
|
||||
{
|
||||
public class AlarmRoutine : MultiWheelAlarmRoutine<DiverCartDefinition>
|
||||
{
|
||||
public override void SetOtherAlarms()
|
||||
{
|
||||
AddAlarm("左夹臂驱动报警", 2, () => cart.LeftArmErrorCode != 0);
|
||||
AddAlarm("右夹臂驱动报警", 2, () => cart.RightArmErrorCode != 0);
|
||||
//AddAlarm("夹臂不同步报警", 2, () => cart.ClampOutOfSync);
|
||||
}
|
||||
}
|
||||
}
|
||||
// 驱动器、急停、夹臂等安全报警
|
||||
@@ -1,63 +1,37 @@
|
||||
using CartActivator;
|
||||
// 定义车型、上下层IO、参数和MCU初始化
|
||||
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 class DiverCartDefinition : MultiWheelCartDefinition
|
||||
{
|
||||
public DiverCommunication Embedded;
|
||||
#region 基本成员
|
||||
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;
|
||||
|
||||
internal enum ManualControlMode
|
||||
{
|
||||
Normal = 0, // 正常模式
|
||||
Spin = 1, // 自旋模式
|
||||
Crab = 2, // 螃蟹模式
|
||||
}
|
||||
internal ManualControlMode TransmitterControlMode = ManualControlMode.Normal;
|
||||
internal DateTime TransmitterLastTime; // 物理遥控器计算两次实体遥控器指令之间的时间间隔
|
||||
#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;
|
||||
|
||||
#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;
|
||||
@@ -66,99 +40,75 @@ namespace MedullaAdapter
|
||||
[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;
|
||||
[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;
|
||||
internal DateTime TransmitterLastTime;
|
||||
|
||||
#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: {0}", err.ToDescription());
|
||||
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: {0}", err.ToDescription());
|
||||
Console.WriteLine($"MCU Reset FAILED: {err.ToDescription()}");
|
||||
return;
|
||||
}
|
||||
else
|
||||
@@ -166,31 +116,28 @@ namespace MedullaAdapter
|
||||
Thread.Sleep(500);
|
||||
Console.WriteLine("MCU Reset OK");
|
||||
}
|
||||
|
||||
//Step3:GetVersion
|
||||
//Step3:获取MCU版本号
|
||||
err = Bridge.GetVersion(out var version, 100);
|
||||
if (err != MCUSerialBridgeError.OK)
|
||||
{
|
||||
Console.WriteLine("MCU GetVersion FAILED: {0}", err.ToDescription());
|
||||
Console.WriteLine($"MCU GetVersion FAILED: {err.ToDescription()}");
|
||||
return;
|
||||
}
|
||||
else
|
||||
{
|
||||
Console.WriteLine("MCU GetVersion OK: {0}", version.ToString());
|
||||
Console.WriteLine($"MCU GetVersion OK: {version}");
|
||||
}
|
||||
|
||||
//Step4:GetState
|
||||
//Step4:获取MCU状态
|
||||
err = Bridge.GetState(out var state, 100);
|
||||
if (err != MCUSerialBridgeError.OK)
|
||||
{
|
||||
Console.WriteLine("MCU GetState FAILED: {0}", err.ToDescription());
|
||||
Console.WriteLine($"MCU GetState FAILED: {err.ToDescription()}");
|
||||
return;
|
||||
}
|
||||
else
|
||||
{
|
||||
Console.WriteLine("MCU GetState OK: {0}", state.ToString());
|
||||
Console.WriteLine($"MCU GetState OK: {state}");
|
||||
}
|
||||
|
||||
//Step5:串口/CAN配置
|
||||
try
|
||||
{
|
||||
@@ -203,18 +150,15 @@ namespace MedullaAdapter
|
||||
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);
|
||||
Console.WriteLine($"Port {i}: Serial, Baud={s.Baud}, ReceiveFrameMs={s.ReceiveFrameMs}");
|
||||
else if (ports[i] is CANPortConfig c)
|
||||
Console.WriteLine("Port {0}: CAN, Baud={1}, RetryTimeMs={2}", i, c.Baud, c.RetryTimeMs);
|
||||
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: {0}", ret.ToDescription());
|
||||
Console.WriteLine($"MCU Configure FAILED: {ret.ToDescription()}");
|
||||
return;
|
||||
}
|
||||
else
|
||||
@@ -225,54 +169,11 @@ namespace MedullaAdapter
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine("Configure Exception: {0}", ex.Message);
|
||||
Console.WriteLine($"Configure Exception: {ex.Message}");
|
||||
return;
|
||||
}
|
||||
//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
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,144 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.IO.Ports;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
using FundamentalLib;
|
||||
|
||||
|
||||
namespace MedullaAdapter
|
||||
{
|
||||
public class DiverCommunication
|
||||
{
|
||||
private byte[] _receiveBytes ;
|
||||
private SerialPort _port;
|
||||
private int _state = 0;
|
||||
private List<byte> _receiveList = new List<byte>();
|
||||
private int _length = 0;
|
||||
|
||||
private MCUInterface<DiverCartDefinition> _interface;
|
||||
|
||||
public DiverCommunication(string name, int baudRate)
|
||||
{
|
||||
_interface = new MCUInterface<DiverCartDefinition>();
|
||||
_port = new SerialPort();
|
||||
_port.PortName = name; // 根据你的实际串口名称修改
|
||||
_port.BaudRate = baudRate;
|
||||
_port.Parity = Parity.None;
|
||||
_port.DataBits = 8;
|
||||
_port.StopBits = StopBits.One;
|
||||
_port.Handshake = Handshake.None;
|
||||
_port.Open();
|
||||
Console.WriteLine($"buffer:{_port.ReadBufferSize}");
|
||||
//_port.DataReceived += OnDataReceived;
|
||||
new Thread(() =>
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
Thread.Sleep(10);
|
||||
try
|
||||
{
|
||||
int bytesToRead = _port.BytesToRead;
|
||||
if (bytesToRead > 0)
|
||||
{
|
||||
var readBuffer = new byte[bytesToRead];
|
||||
var start = DateTime.Now;
|
||||
_port.Read(readBuffer, 0, bytesToRead);
|
||||
var time1 = DateTime.Now - start;
|
||||
ProcessBuffer(readBuffer);
|
||||
var time2 = DateTime.Now - start;
|
||||
Hedingben.ToastText($"total time:{time2.TotalMilliseconds},read time:{time1},read count:{bytesToRead}", "timeDebug");
|
||||
}
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
Console.WriteLine(exception);
|
||||
}
|
||||
}
|
||||
}).Start();
|
||||
}
|
||||
|
||||
private void ProcessBuffer(byte[] buffer)
|
||||
{
|
||||
for (int i = 0; i < buffer.Length; i++)
|
||||
{
|
||||
var newByte = buffer[i];
|
||||
switch (_state)
|
||||
{
|
||||
case 0:
|
||||
_receiveList.Clear();
|
||||
if (newByte == 0xBB)
|
||||
{
|
||||
_receiveList.Add(newByte);
|
||||
_state = 1;
|
||||
}
|
||||
break;
|
||||
case 1:
|
||||
if (newByte == 0xAA)
|
||||
{
|
||||
_receiveList.Add(newByte);
|
||||
_state = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
_state = 0;
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
_receiveList.Add(newByte);
|
||||
if (_receiveList.Count >= 4)
|
||||
{
|
||||
_length = BitConverter.ToUInt16(_receiveList.ToArray(), 2);
|
||||
}
|
||||
|
||||
if (_receiveList.Count == _length + 7)
|
||||
{
|
||||
_state = 0;
|
||||
if (_receiveList[_receiveList.Count - 1] == 0xEE)
|
||||
{
|
||||
_receiveBytes = _receiveList.ToArray();
|
||||
Hedingben.ToastText($"receive from mcu:{BitConverter.ToString(_receiveBytes)}","DiverReceive");
|
||||
if (_receiveBytes[5] == 0xA0)
|
||||
{
|
||||
var dataLength1 = BitConverter.ToUInt32(_receiveBytes, 6);
|
||||
var data1 = new byte[dataLength1];
|
||||
Array.Copy(_receiveBytes, 14, data1, 0, dataLength1);
|
||||
MCUInterface<DiverCartDefinition>.NotifyLowerData("default", data1);
|
||||
var logBytes = new byte[BitConverter.ToInt32(_receiveBytes, 10)];
|
||||
var memorySize = BitConverter.ToInt32(_receiveBytes, 6);
|
||||
if (logBytes.Length > 0)
|
||||
{
|
||||
Array.Copy(_receiveBytes, 14 + memorySize, logBytes, 0, logBytes.Length);
|
||||
string log = System.Text.Encoding.ASCII.GetString(logBytes);
|
||||
string[] result = log.Split(new[] { "\r\n", "\n" }, StringSplitOptions.None);
|
||||
for (int j = 0; j < result.Length; j++)
|
||||
{
|
||||
Hedingben.ToastText(result[j], $"mcuLog" + j);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
_state = 0;
|
||||
Hedingben.ToastText($"mcu date error,should end with 0xEE , actual {_receiveList[_receiveList.Count - 1]}","DiverError");
|
||||
}
|
||||
}
|
||||
break;
|
||||
default:break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void SendMessage(byte[] data)
|
||||
{
|
||||
Hedingben.ToastText($"send to mcu:{BitConverter.ToString(data)}", "DiverSend");
|
||||
_port.Write(data, 0, data.Length);
|
||||
}
|
||||
|
||||
public byte[] GetMessage()
|
||||
{
|
||||
return _receiveBytes;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,181 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.IO.Ports;
|
||||
using System.Linq;
|
||||
using System.Threading;
|
||||
using FundamentalLib;
|
||||
using Medulla;
|
||||
|
||||
namespace MedullaAdapter
|
||||
{
|
||||
public class EmbeddedCommunication
|
||||
{
|
||||
private SerialPort _port;
|
||||
|
||||
private byte[] _receiveBytes;
|
||||
|
||||
private byte[] _receiveData;
|
||||
|
||||
private const int BufferThreshold = 300; // 缓冲区大小阈值
|
||||
|
||||
private List<byte> _buffer = new List<byte>(); // 缓存接收到的数据
|
||||
|
||||
public bool CommunicationError = false;
|
||||
|
||||
private DateTime _lastTime = DateTime.Now;
|
||||
|
||||
private MCUInterface<DiverCartDefinition> _interface;
|
||||
|
||||
public EmbeddedCommunication(string name, int baudRate)
|
||||
{
|
||||
_interface = new MCUInterface<DiverCartDefinition>();
|
||||
_port = new SerialPort();
|
||||
_port.PortName = name; // 根据你的实际串口名称修改
|
||||
_port.BaudRate = baudRate;
|
||||
_port.Parity = Parity.None;
|
||||
_port.DataBits = 8;
|
||||
_port.StopBits = StopBits.One;
|
||||
_port.Handshake = Handshake.None;
|
||||
_port.Open();
|
||||
_port.DataReceived += OnDataReceived;
|
||||
}
|
||||
|
||||
public void SendMessage(byte[] data)
|
||||
{
|
||||
Hedingben.ToastText($"send to mcu:{BitConverter.ToString(data)}","DiverSend");
|
||||
_port.Write(data, 0, data.Length);
|
||||
}
|
||||
|
||||
public byte[] GetMessage()
|
||||
{
|
||||
return _receiveBytes;
|
||||
}
|
||||
|
||||
private void OnDataReceived(object sender, SerialDataReceivedEventArgs e)
|
||||
{
|
||||
try
|
||||
{
|
||||
int bytesToRead = _port.BytesToRead;
|
||||
byte[] buffer = new byte[bytesToRead];
|
||||
var startRead = DateTime.Now;
|
||||
_port.Read(buffer, 0, bytesToRead);
|
||||
var time = DateTime.Now - startRead;
|
||||
// 将新接收到的数据添加到缓冲区
|
||||
startRead = DateTime.Now;
|
||||
_buffer.AddRange(buffer);
|
||||
var time2 = DateTime.Now - startRead;
|
||||
Hedingben.ToastText($"buffer length:{_buffer.Count}","bufferDebug");
|
||||
DLog.Log($"buffer length:{_buffer.Count}");
|
||||
//if (_buffer.Count > BufferThreshold)
|
||||
//{
|
||||
// _buffer.Clear();
|
||||
// return;
|
||||
//}
|
||||
|
||||
// 尝试解析缓冲区中的报文
|
||||
var parseTime = DateTime.Now;
|
||||
ParseBuffer();
|
||||
var time3 = DateTime.Now - parseTime;
|
||||
Hedingben.ToastText(
|
||||
$"读取报文时间:{time.TotalMilliseconds},增加到缓存区:{time2.TotalMilliseconds},处理时间:{time3.TotalMilliseconds}",
|
||||
"timeDebug1");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine($"读取串口数据时发生错误: {ex.Message}"+ex.StackTrace);
|
||||
}
|
||||
}
|
||||
private void ParseBuffer()
|
||||
{
|
||||
while (_buffer.Count >= 7) // 报文的最小长度是 7
|
||||
{
|
||||
// 查找报文头部
|
||||
var findTime = DateTime.Now;
|
||||
int startIndex = _buffer.FindIndex(0, b => b == 0xBB);
|
||||
var time1 = DateTime.Now - findTime;
|
||||
if (startIndex == -1)
|
||||
{
|
||||
// 如果没有找到头部或剩余长度不足最小报文长度，结束解析
|
||||
_buffer.Clear();
|
||||
return;
|
||||
}
|
||||
|
||||
// 确保头部后还有至少 6 个字节
|
||||
if (startIndex + 1 >= _buffer.Count || _buffer[startIndex + 1] != 0xAA)
|
||||
{
|
||||
// 如果第二字节不是 0xAA，丢弃无效字节
|
||||
_buffer.RemoveAt(startIndex);
|
||||
continue;
|
||||
}
|
||||
|
||||
// 检查数据段长度
|
||||
if (startIndex + 4 >= _buffer.Count) break; // 数据不足，等待下次接收
|
||||
int dataLength = _buffer[startIndex + 2] | (_buffer[startIndex + 3] << 8);
|
||||
|
||||
// 计算报文总长度
|
||||
int totalLength = dataLength + 7;
|
||||
|
||||
// 检查总长度是否足够
|
||||
if (startIndex + totalLength > _buffer.Count) break; // 数据不足，等待下次接收
|
||||
|
||||
// 检查尾部是否是 0xCC 0xEE
|
||||
if (_buffer[startIndex + totalLength - 2] == 0xCC && _buffer[startIndex + totalLength - 1] == 0xEE)
|
||||
{
|
||||
var time2 = DateTime.Now - findTime;
|
||||
var copyTime = DateTime.Now;
|
||||
// 提取完整报文
|
||||
_receiveBytes = _buffer.Skip(startIndex).Take(totalLength).ToArray();
|
||||
var time3 = DateTime.Now - copyTime;
|
||||
if (_receiveBytes[5] == 0xA0)
|
||||
{
|
||||
var dataLength1 = BitConverter.ToUInt32(_receiveBytes, 6);
|
||||
var data1 = new byte[dataLength1];
|
||||
Array.Copy(_receiveBytes, 14, data1, 0, dataLength1);
|
||||
var notifyTime = DateTime.Now;
|
||||
MCUInterface<DiverCartDefinition>.NotifyLowerData("default", data1);
|
||||
var time4 = DateTime.Now - notifyTime;
|
||||
var logBytes = new byte[BitConverter.ToInt32(_receiveBytes, 10)];
|
||||
var memorySize = BitConverter.ToInt32(_receiveBytes, 6);
|
||||
if (logBytes.Length > 0)
|
||||
{
|
||||
Array.Copy(_receiveBytes, 14 + memorySize, logBytes, 0, logBytes.Length);
|
||||
string log = System.Text.Encoding.ASCII.GetString(logBytes);
|
||||
string[] result = log.Split(new[] { "\r\n", "\n" }, StringSplitOptions.None);
|
||||
for (int i = 0; i < result.Length; i++)
|
||||
{
|
||||
Hedingben.ToastText(result[i], $"mcuLog" + i);
|
||||
}
|
||||
}
|
||||
|
||||
Hedingben.ToastText(
|
||||
$"find head time:{time1.TotalMilliseconds},find total time:{time2.TotalMilliseconds},copy time:{time3.TotalMilliseconds},notify time:{time4.TotalMilliseconds}","timeDebug2");
|
||||
}
|
||||
|
||||
Hedingben.ToastText(
|
||||
$"receive from mcu:{BitConverter.ToString(_receiveBytes)},time:{(DateTime.Now - _lastTime).TotalMilliseconds}",
|
||||
"DiverReceive");
|
||||
if ((DateTime.Now - _lastTime).TotalMilliseconds > 200)
|
||||
{
|
||||
Hedingben.ToastText($"{(DateTime.Now - _lastTime).TotalMilliseconds}ms no message from diver","DiverTimeout");
|
||||
}
|
||||
_lastTime = DateTime.Now;
|
||||
// 从缓冲区中移除已解析的报文
|
||||
_buffer.RemoveRange(0, startIndex + totalLength);
|
||||
return; // 成功解析一条报文后退出本轮解析
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
// 如果尾部无效，丢弃头部并继续解析
|
||||
DLog.Log("DIVER报文无效");
|
||||
Console.WriteLine("DIVER报文无效");
|
||||
var errorBytes = _buffer.Skip(startIndex).Take(totalLength).ToArray();
|
||||
DLog.Log("错误报文"+string.Join(" ",errorBytes.Select(p=>$"{p:X2}")));
|
||||
_buffer.RemoveAt(startIndex);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,246 +0,0 @@
|
||||
using System;
|
||||
using CartActivator;
|
||||
using CycleGUI;
|
||||
using CycleGUI.API;
|
||||
using FundamentalLib;
|
||||
using Medulla.Types;
|
||||
|
||||
namespace MedullaAdapter
|
||||
{
|
||||
public partial class DiverCartDefinition
|
||||
{
|
||||
private bool _fleetRemoteActive;
|
||||
private DateTime _fleetDiagLastStick = DateTime.MinValue;
|
||||
|
||||
private void FleetDiag(string msg)
|
||||
{
|
||||
DLog.Log($"car{CarNum} {msg}", "FleetDiagMedulla");
|
||||
}
|
||||
|
||||
private void ZeroFleetManualFields()
|
||||
{
|
||||
MultiVehicleManualEnabled = false;
|
||||
MultiVehicleManualMode = 0;
|
||||
MultiVehicleManualVx = 0;
|
||||
MultiVehicleManualVy = 0;
|
||||
MultiVehicleManualVth = 0;
|
||||
}
|
||||
|
||||
public float ApplyFleetManualCommand(int mode, float px, float py, float speedRatio)
|
||||
{
|
||||
var ratio = Clamp(speedRatio, 0, 1);
|
||||
px = Clamp(px, -1, 1);
|
||||
py = Clamp(py, -1, 1);
|
||||
|
||||
if (mode < 0 || mode > 2) mode = 0;
|
||||
MultiVehicleManualMode = mode;
|
||||
|
||||
if (mode == 2)
|
||||
{
|
||||
MultiVehicleManualVx = 0;
|
||||
MultiVehicleManualVy = 0;
|
||||
MultiVehicleManualVth = px * MaxManualAngularSpeed * ratio;
|
||||
}
|
||||
else if (mode == 1)
|
||||
{
|
||||
MultiVehicleManualVx = py * MaxManualSpeed * ratio;
|
||||
MultiVehicleManualVy = px * MaxManualSpeed * ratio;
|
||||
MultiVehicleManualVth = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
MultiVehicleManualVx = py * MaxManualSpeed * ratio;
|
||||
MultiVehicleManualVy = 0;
|
||||
MultiVehicleManualVth = px * MaxManualAngularSpeed * ratio;
|
||||
}
|
||||
|
||||
return ratio;
|
||||
}
|
||||
|
||||
[IOObjectUtility]
|
||||
public void FleetRemote()
|
||||
{
|
||||
if (_fleetRemoteActive)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_fleetRemoteActive = true;
|
||||
var fleetOn = false;
|
||||
var crabOn = false;
|
||||
var rotateOn = false;
|
||||
var speedRatio = 1f;
|
||||
UseGesture manip = null;
|
||||
|
||||
int CurrentMode()
|
||||
{
|
||||
if (crabOn) return 1;
|
||||
if (rotateOn) return 2;
|
||||
return 0;
|
||||
}
|
||||
|
||||
void ApplyMode()
|
||||
{
|
||||
MultiVehicleManualMode = fleetOn ? CurrentMode() : 0;
|
||||
}
|
||||
|
||||
void CleanupFleetRemote()
|
||||
{
|
||||
if (manip != null)
|
||||
{
|
||||
manip.End();
|
||||
manip = null;
|
||||
}
|
||||
|
||||
fleetOn = false;
|
||||
crabOn = false;
|
||||
rotateOn = false;
|
||||
ZeroFleetManualFields();
|
||||
_fleetRemoteActive = false;
|
||||
}
|
||||
|
||||
manip = new UseGesture();
|
||||
|
||||
manip.AddWidget(new UseGesture.StickWidget
|
||||
{
|
||||
name = "fleet_stick",
|
||||
text = "速度摇杆",
|
||||
position = "37.5%+10px, 18%+10px",
|
||||
size = "37.5%-10px, 32%-10px",
|
||||
bounceBack = true,
|
||||
keyboard = "Up,Down,Left,Right",
|
||||
joystick = "Axis0,Axis1",
|
||||
OnValue = (pos, manipulating) =>
|
||||
{
|
||||
if (!fleetOn)
|
||||
{
|
||||
MultiVehicleManualVx = 0;
|
||||
MultiVehicleManualVy = 0;
|
||||
MultiVehicleManualVth = 0;
|
||||
if ((DateTime.Now - _fleetDiagLastStick).TotalMilliseconds >= 200)
|
||||
{
|
||||
_fleetDiagLastStick = DateTime.Now;
|
||||
FleetDiag($"STICK(ignored,fleetOff) pos=({pos.X:0.00},{pos.Y:0.00}) manip={manipulating}");
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
var mode = CurrentMode();
|
||||
var ratio = ApplyFleetManualCommand(mode, pos.X, pos.Y, speedRatio);
|
||||
|
||||
if ((DateTime.Now - _fleetDiagLastStick).TotalMilliseconds >= 200)
|
||||
{
|
||||
_fleetDiagLastStick = DateTime.Now;
|
||||
FleetDiag($"STICK mode={mode} pos=({pos.X:0.00},{pos.Y:0.00}) manip={manipulating} ratio={ratio:0.00} " +
|
||||
$"-> Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.0} en={MultiVehicleManualEnabled}");
|
||||
}
|
||||
}
|
||||
});
|
||||
|
||||
manip.AddWidget(new UseGesture.ToggleWidget
|
||||
{
|
||||
name = "fleet_crab",
|
||||
text = "横移模式",
|
||||
position = "12.5%+10px, 52%+10px",
|
||||
size = "37.5%-10px, 10%-10px",
|
||||
OnValue = b =>
|
||||
{
|
||||
crabOn = b;
|
||||
ApplyMode();
|
||||
FleetDiag($"TOGGLE crab={b} -> mode={MultiVehicleManualMode}");
|
||||
}
|
||||
});
|
||||
|
||||
manip.AddWidget(new UseGesture.ToggleWidget
|
||||
{
|
||||
name = "fleet_rotate",
|
||||
text = "原地旋转",
|
||||
position = "50%+10px, 52%+10px",
|
||||
size = "37.5%-10px, 10%-10px",
|
||||
OnValue = b =>
|
||||
{
|
||||
rotateOn = b;
|
||||
ApplyMode();
|
||||
FleetDiag($"TOGGLE rotate={b} -> mode={MultiVehicleManualMode}");
|
||||
}
|
||||
});
|
||||
|
||||
manip.AddWidget(new UseGesture.ToggleWidget
|
||||
{
|
||||
name = "fleet_enable",
|
||||
text = "车队联动",
|
||||
position = "12.5%+10px, 64%+10px",
|
||||
size = "37.5%-10px, 10%-10px",
|
||||
OnValue = b =>
|
||||
{
|
||||
fleetOn = b;
|
||||
MultiVehicleManualEnabled = b;
|
||||
ApplyMode();
|
||||
if (!b) ZeroFleetManualFields();
|
||||
FleetDiag($"TOGGLE fleetOn={b} -> ManualEnabled={MultiVehicleManualEnabled} mode={MultiVehicleManualMode}");
|
||||
}
|
||||
});
|
||||
|
||||
manip.AddWidget(new UseGesture.ThrottleWidget
|
||||
{
|
||||
name = "fleet_speed_ratio",
|
||||
text = "速度比例",
|
||||
position = "50%+10px, 64%+10px",
|
||||
size = "37.5%-10px, 10%-10px",
|
||||
bounceBack = false,
|
||||
OnValue = (val, _) => speedRatio = Clamp(val, 0, 1)
|
||||
});
|
||||
|
||||
manip.AddWidget(new UseGesture.ButtonWidget
|
||||
{
|
||||
name = "fleet_stop",
|
||||
text = "急停/归零",
|
||||
position = "31.25%+10px, 76%+10px",
|
||||
size = "37.5%-10px, 10%-10px",
|
||||
OnPressed = pressed =>
|
||||
{
|
||||
if (!pressed) return;
|
||||
MultiVehicleManualVx = 0;
|
||||
MultiVehicleManualVy = 0;
|
||||
MultiVehicleManualVth = 0;
|
||||
FleetDiag("BUTTON stop/zero");
|
||||
}
|
||||
});
|
||||
|
||||
manip.ChangeState(new SetAppearance { drawGuizmo = false });
|
||||
manip.Start();
|
||||
|
||||
GUI.PromptOrBringToFront(pb =>
|
||||
{
|
||||
if (pb.Closing())
|
||||
{
|
||||
CleanupFleetRemote();
|
||||
pb.Panel.Exit();
|
||||
return;
|
||||
}
|
||||
|
||||
pb.Panel.TopMost(true)
|
||||
.SetDefaultDocking(Panel.Docking.None)
|
||||
.ShowTitle("车队联动遥控")
|
||||
.InitSize(340, 190)
|
||||
.InitPos(false, -32, 32, 1, 0, 1, 0);
|
||||
|
||||
pb.SeparatorText("状态");
|
||||
var modeName = MultiVehicleManualMode == 2 ? "原地旋转" : MultiVehicleManualMode == 1 ? "横移" : "常规";
|
||||
pb.Label($"车队联动: {MultiVehicleManualEnabled} 模式: {modeName}");
|
||||
pb.Label($"速度比例: {speedRatio:0.00}");
|
||||
pb.Label($"Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} m/s, Vth={MultiVehicleManualVth:0.0}");
|
||||
pb.Label($"优先级: {CartActivator.CartDefinition.currentPriority} ({CartActivator.CartDefinition.currentPriorityDesc})");
|
||||
pb.Label("请勿同时打开手动控制面板");
|
||||
pb.Panel.Repaint();
|
||||
}, instancingObject: this);
|
||||
}
|
||||
|
||||
private static float Clamp(float value, float min, float max)
|
||||
{
|
||||
if (value < min) return min;
|
||||
if (value > max) return max;
|
||||
return value;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,3 +0,0 @@
|
||||
<Weavers xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="FodyWeavers.xsd">
|
||||
<DiverCompiler />
|
||||
</Weavers>
|
||||
@@ -1,26 +0,0 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<xs:schema xmlns:xs="http://www.w3.org/2001/XMLSchema">
|
||||
<!-- This file was generated by Fody. Manual changes to this file will be lost when your project is rebuilt. -->
|
||||
<xs:element name="Weavers">
|
||||
<xs:complexType>
|
||||
<xs:all>
|
||||
<xs:element name="DiverCompiler" minOccurs="0" maxOccurs="1" type="xs:anyType" />
|
||||
</xs:all>
|
||||
<xs:attribute name="VerifyAssembly" type="xs:boolean">
|
||||
<xs:annotation>
|
||||
<xs:documentation>'true' to run assembly verification (PEVerify) on the target assembly after all weavers have been executed.</xs:documentation>
|
||||
</xs:annotation>
|
||||
</xs:attribute>
|
||||
<xs:attribute name="VerifyIgnoreCodes" type="xs:string">
|
||||
<xs:annotation>
|
||||
<xs:documentation>A comma-separated list of error codes that can be safely ignored in assembly verification.</xs:documentation>
|
||||
</xs:annotation>
|
||||
</xs:attribute>
|
||||
<xs:attribute name="GenerateXsd" type="xs:boolean">
|
||||
<xs:annotation>
|
||||
<xs:documentation>'false' to turn off automatic generation of the XML Schema file.</xs:documentation>
|
||||
</xs:annotation>
|
||||
</xs:attribute>
|
||||
</xs:complexType>
|
||||
</xs:element>
|
||||
</xs:schema>
|
||||
@@ -1,291 +0,0 @@
|
||||
using CartActivator;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.IO;
|
||||
using System.Linq;
|
||||
using System.Reflection;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
using Newtonsoft.Json;
|
||||
using FundamentalLib;
|
||||
|
||||
|
||||
namespace MedullaAdapter
|
||||
{
|
||||
// todo: currently use this ladderlogic to act interaction with Medulla.
|
||||
// todo: directly integrate into CartActivator.
|
||||
|
||||
// MCU->Medulla is intervally exchanged information.
|
||||
public class MCUInterface<T> : LadderLogic<T> where T : DiverCartDefinition
|
||||
{
|
||||
|
||||
/// ////////////////////////////// IMPLEMENT THESE ////////////////////////////////////////////
|
||||
|
||||
public static void SetMCUProgram(string mcu_device_url, byte[] program)
|
||||
{
|
||||
List<byte[]> SplitArrayIntoChunks(byte[] array, int chunkSize)
|
||||
{
|
||||
List<byte[]> chunks = new List<byte[]>();
|
||||
|
||||
for (int i = 0; i < array.Length; i += chunkSize)
|
||||
{
|
||||
int currentChunkSize = Math.Min(chunkSize, array.Length - i);
|
||||
byte[] chunk = new byte[currentChunkSize];
|
||||
Array.Copy(array, i, chunk, 0, currentChunkSize);
|
||||
chunks.Add(chunk);
|
||||
}
|
||||
return chunks;
|
||||
}
|
||||
//todo: modify this.
|
||||
var codeList = SplitArrayIntoChunks(program, 1024);
|
||||
int i = 0;
|
||||
foreach (var codePack in codeList)
|
||||
{
|
||||
var downloadCode =
|
||||
new DownloadCodePack(program.Length, 1024 * i, codePack.Length, codePack).GetPack();
|
||||
Console.WriteLine(string.Join(" ", downloadCode.Select(p => $"{p:X2}")));
|
||||
cart.Embedded.SendMessage(downloadCode);
|
||||
Console.WriteLine($"send bytes length:{downloadCode.Length}");
|
||||
i++;
|
||||
Thread.Sleep(50);
|
||||
//while (true)
|
||||
//{
|
||||
// var receive = cart.Embedded.GetMessage();
|
||||
// if (receive == null) continue;
|
||||
// if (receive[5] == 0x90 && (receive[6] == 0x05 || receive[6]==0x06)) break;
|
||||
//}
|
||||
}
|
||||
var controlPack2 = new ControlPack(0x01).GetPack();
|
||||
cart.Embedded.SendMessage(controlPack2);
|
||||
Console.WriteLine("下发代码");
|
||||
//MCUTestRunner.DebugSetMCUProgram(program, (bs) => NotifyLowerData("default", bs));
|
||||
}
|
||||
|
||||
public static void SendUpperData(string mcu_device_url, byte[] data)
|
||||
{
|
||||
//todo: this is for VM data exchange, contains upperIO/lowerIO modifications.
|
||||
var upperPack = new UpperIOPack(data, data.Length).GetPack();
|
||||
cart.Embedded.SendMessage(upperPack);
|
||||
//MCUTestRunner.DebugSendUpper(data);
|
||||
}
|
||||
|
||||
|
||||
/// ////////////////////////////// INTERFACES ////////////////////////////////////////////
|
||||
|
||||
public static void NotifyPrint(string mcu_device_url, string message)
|
||||
{
|
||||
Console.WriteLine($"{mcu_device_url}:{message}");
|
||||
}
|
||||
|
||||
// whenever a lower io data is uploaded, call this.
|
||||
public static void NotifyLowerData(string mcu_device_url, byte[] lowerIOData)
|
||||
{
|
||||
using var ms = new MemoryStream(lowerIOData);
|
||||
using var br = new BinaryReader(ms);
|
||||
if (mcu_logics.TryGetValue(mcu_device_url, out var tup))
|
||||
{
|
||||
Hedingben.ToastText($"recv iter {br.ReadInt32()} lowerIO data from {mcu_device_url}, operation {tup.name}", $"DIVER-{tup.name}");
|
||||
while (ms.Position < lowerIOData.Length)
|
||||
{
|
||||
var cid = br.ReadInt16();
|
||||
if (cid < 0 || cid > tup.fields.Length) throw new Exception("invalid Cartfield id!");
|
||||
// if it's upperio skip, otherwise write data.
|
||||
var typeid = br.ReadByte();
|
||||
if (tup.fields[cid].typeid != typeid)
|
||||
throw new Exception($"??? typeid not match for {tup.fields[cid].field}({cid}), expected {tup.fields[cid].typeid} got {typeid}");
|
||||
|
||||
object value;
|
||||
switch (tup.fields[cid].typeid)
|
||||
{
|
||||
case 0:
|
||||
value = br.ReadBoolean();
|
||||
break;
|
||||
case 1:
|
||||
value = br.ReadByte();
|
||||
break;
|
||||
case 2:
|
||||
value = br.ReadSByte();
|
||||
break;
|
||||
case 3:
|
||||
value = br.ReadChar();
|
||||
break;
|
||||
case 4:
|
||||
value = br.ReadInt16();
|
||||
break;
|
||||
case 5:
|
||||
value = br.ReadUInt16();
|
||||
break;
|
||||
case 6:
|
||||
value = br.ReadInt32();
|
||||
break;
|
||||
case 7:
|
||||
value = br.ReadUInt32();
|
||||
break;
|
||||
case 8:
|
||||
value = br.ReadSingle();
|
||||
break;
|
||||
default:
|
||||
throw new Exception($"Unsupported type ID: {tup.fields[cid].typeid}");
|
||||
}
|
||||
|
||||
if (tup.fields[cid].isUpper) continue;
|
||||
tup.fields[cid].fi.SetValue(cart, value);
|
||||
}
|
||||
|
||||
// ok to send current data.
|
||||
using var sends = new MemoryStream();
|
||||
using var bw = new BinaryWriter(sends);
|
||||
bw.Write(tup.iterations++);
|
||||
for (var cid = 0; cid < tup.fields.Length; cid++)
|
||||
{
|
||||
bw.Write((short)cid);
|
||||
bw.Write((byte)tup.fields[cid].typeid);
|
||||
var val = tup.fields[cid].fi.GetValue(cart);
|
||||
switch (tup.fields[cid].typeid)
|
||||
{
|
||||
case 0:
|
||||
bw.Write((bool)val);
|
||||
break;
|
||||
case 1:
|
||||
bw.Write((byte)val);
|
||||
break;
|
||||
case 2:
|
||||
bw.Write((sbyte)val);
|
||||
break;
|
||||
case 3:
|
||||
bw.Write((char)val);
|
||||
break;
|
||||
case 4:
|
||||
bw.Write((short)val);
|
||||
break;
|
||||
case 5:
|
||||
bw.Write((ushort)val);
|
||||
break;
|
||||
case 6:
|
||||
bw.Write((int)val);
|
||||
break;
|
||||
case 7:
|
||||
bw.Write((uint)val);
|
||||
break;
|
||||
case 8:
|
||||
bw.Write((float)val);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
SendUpperData(mcu_device_url, sends.ToArray());
|
||||
}
|
||||
else
|
||||
{
|
||||
Hedingben.ToastText($"warning: {mcu_device_url} received lowerIOData but not registered","DIVERWarning");
|
||||
}
|
||||
}
|
||||
|
||||
public static void Start(T c)
|
||||
{
|
||||
cart = c;
|
||||
|
||||
var logics = typeof(T).Assembly.GetTypes()
|
||||
.Where(p => p.GetCustomAttribute<LogicRunOnMCUAttribute>() != null).ToArray();
|
||||
foreach (var logic in logics)
|
||||
{
|
||||
var attr = logic.GetCustomAttribute<LogicRunOnMCUAttribute>();
|
||||
Console.WriteLine($"Set logic {logic.Name} to run on MCU-VM @ device {attr.mcu_url}");
|
||||
|
||||
byte[] ReadAllBytes(Stream stream)
|
||||
{
|
||||
using (var ms = new MemoryStream())
|
||||
{
|
||||
stream.CopyTo(ms);
|
||||
return ms.ToArray();
|
||||
}
|
||||
}
|
||||
var bytes = ReadAllBytes(Assembly.GetExecutingAssembly().GetManifestResourceStream($"{logic.Name}.bin"));
|
||||
SetMCUProgram(attr.mcu_url, bytes);
|
||||
var json = UTF8Encoding.UTF8.GetString(ReadAllBytes(Assembly.GetExecutingAssembly()
|
||||
.GetManifestResourceStream($"{logic.Name}.bin.json")));
|
||||
Console.WriteLine(json);
|
||||
var fields = JsonConvert.DeserializeObject<PField[]>(json);
|
||||
if (mcu_logics.ContainsKey(attr.mcu_url))
|
||||
throw new Exception($"Already have logic for {attr.mcu_url}: LadderLogic {logic.Name}");
|
||||
mcu_logics[attr.mcu_url] = new LogicInfo() { fields = fields, name = logic.Name };
|
||||
foreach (var pField in fields)
|
||||
{
|
||||
pField.fi = typeof(T).GetField(pField.field);
|
||||
if (pField.fi == null)
|
||||
throw new Exception($"field {pField.field} doesn't exist in cart object?");
|
||||
pField.isUpper = pField.fi.IsDefined(typeof(AsUpperIO));
|
||||
// todo: check type.
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// ///////////////////////////////// DONT CARE /////////////////////////////////////
|
||||
private static T cart;
|
||||
|
||||
class PField
|
||||
{
|
||||
public string field;
|
||||
public FieldInfo fi;
|
||||
public bool isUpper;
|
||||
public int typeid, offset;
|
||||
}
|
||||
|
||||
class LogicInfo
|
||||
{
|
||||
public string name;
|
||||
public PField[] fields;
|
||||
public int iterations;
|
||||
}
|
||||
private static Dictionary<string, LogicInfo> mcu_logics = new();
|
||||
public override void Operation(int iteration)
|
||||
{
|
||||
// just send LIO.
|
||||
}
|
||||
}
|
||||
|
||||
//unsafe class MCUTestRunner
|
||||
//{
|
||||
|
||||
// public delegate void NotifyLowerDelegate(byte* changedStates, int length);
|
||||
// [DllImport("MCURuntime.dll", CallingConvention = CallingConvention.Cdecl)]
|
||||
// public static extern void set_lowerio_cb(NotifyLowerDelegate callback);
|
||||
|
||||
// private static NotifyLowerDelegate DNotifyStateChanged = StateChanged;
|
||||
|
||||
// [DllImport("MCURuntime.dll")]
|
||||
// static extern void test(byte* bin, int len);
|
||||
// [DllImport("MCURuntime.dll")]
|
||||
// static extern void put_upper(byte* bin, int len);
|
||||
|
||||
// private static Action<byte[]> lo_notifier;
|
||||
// private static void StateChanged(byte* changedstates, int length)
|
||||
// {
|
||||
// byte[] byteArray = new byte[length];
|
||||
// Marshal.Copy((IntPtr)changedstates, byteArray, 0, length);
|
||||
// lo_notifier(byteArray);
|
||||
// }
|
||||
|
||||
// public static void DebugSendUpper(byte[] data)
|
||||
// {
|
||||
// fixed (byte* ptr = data)
|
||||
// {
|
||||
// put_upper(ptr, data.Length);
|
||||
// }
|
||||
// }
|
||||
// public static void DebugSetMCUProgram(byte[] program, Action<byte[]> notifier)
|
||||
// {
|
||||
// lo_notifier = notifier;
|
||||
// set_lowerio_cb(DNotifyStateChanged);
|
||||
// new Thread(() =>
|
||||
// {
|
||||
// var allb = new byte[10240]; //10K runtime
|
||||
// Array.Copy(program, allb, program.Length);
|
||||
// fixed (byte* ptr = allb)
|
||||
// {
|
||||
// test(ptr, 10240);
|
||||
// }
|
||||
// }).Start();
|
||||
// }
|
||||
//}
|
||||
}
|
||||
@@ -1,879 +1 @@
|
||||
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];
|
||||
},
|
||||
};
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
// 实际CAN协议、反馈解析、IO、电池、急停
|
||||
@@ -1,3 +1,5 @@
|
||||
// C#调用MCU通信桥
|
||||
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
@@ -102,6 +104,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// 转换为可读字符串
|
||||
/// </summary>
|
||||
/// <returns>返回包含产品、Tag、Commit、BuildTime 的字符串</returns>
|
||||
// M层MCU适配：格式化固件版本信息便于日志显示。
|
||||
public override string ToString()
|
||||
{
|
||||
return $"Product: {ProductionName}, Tag: {GitTag}, Commit: {GitCommit}, Built: {BuildTime}";
|
||||
@@ -144,6 +147,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// 返回可读的状态字符串
|
||||
/// </summary>
|
||||
/// <returns>例如 "Bridge: Running" 或 "DIVER: Error"</returns>
|
||||
// M层MCU适配：格式化MCU运行状态便于日志显示。
|
||||
public override string ToString()
|
||||
{
|
||||
string modeStr = IsBridge ? "Bridge" : "DIVER";
|
||||
@@ -174,6 +178,7 @@ namespace MCUSerialBridgeCLR
|
||||
|
||||
/// <summary>序列化端口配置为字节数组（供 P/Invoke 使用）</summary>
|
||||
/// <returns>返回固定长度字节数组（16 bytes）</returns>
|
||||
// M层MCU适配：将端口配置序列化为原生接口字节。
|
||||
public abstract byte[] ToBytes();
|
||||
}
|
||||
|
||||
@@ -185,6 +190,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// </remarks>
|
||||
/// <param name="baud">波特率</param>
|
||||
/// <param name="receiveFrameMs">接收帧间隔</param>
|
||||
// M层MCU适配：创建串口通信参数配置。
|
||||
public class SerialPortConfig(uint baud, uint receiveFrameMs) : PortConfig
|
||||
{
|
||||
/// <summary>Serial 类型</summary>
|
||||
@@ -200,6 +206,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// 转换为字节数组
|
||||
/// </summary>
|
||||
/// <returns>16 字节数组</returns>
|
||||
// M层MCU适配：序列化串口波特率和组帧时间。
|
||||
public override byte[] ToBytes()
|
||||
{
|
||||
var c = new PortStructHelper.SerialPortConfigC
|
||||
@@ -221,6 +228,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// </remarks>
|
||||
/// <param name="baud">波特率</param>
|
||||
/// <param name="retryTimeMs">重发间隔</param>
|
||||
// M层MCU适配：创建CAN通信参数配置。
|
||||
public class CANPortConfig(uint baud, uint retryTimeMs) : PortConfig
|
||||
{
|
||||
/// <summary>CAN 类型</summary>
|
||||
@@ -236,6 +244,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// 转换为字节数组
|
||||
/// </summary>
|
||||
/// <returns>16 字节数组</returns>
|
||||
// M层MCU适配：序列化CAN波特率和重试时间。
|
||||
public override byte[] ToBytes()
|
||||
{
|
||||
var c = new PortStructHelper.CANPortConfigC
|
||||
@@ -272,6 +281,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// <returns>返回字节数组：2 bytes header + Payload</returns>
|
||||
/// <exception cref="ArgumentOutOfRangeException">如果 DLC > 8</exception>
|
||||
/// <exception cref="ArgumentException">如果 Payload 长度 != DLC</exception>
|
||||
// M层CAN适配：将标准或扩展CAN帧序列化为原生布局。
|
||||
public byte[] ToBytes()
|
||||
{
|
||||
if (DLC > 8)
|
||||
@@ -303,6 +313,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// <param name="length">实际有效长度</param>
|
||||
/// <returns>CANMessage 实例</returns>
|
||||
/// <exception cref="ArgumentException">数据长度错误</exception>
|
||||
// M层CAN适配：从原生缓冲区还原CAN消息。
|
||||
public static CANMessage FromBytes(byte[] data, uint length)
|
||||
{
|
||||
if (data == null || length > data.Length || length < 2)
|
||||
@@ -332,6 +343,7 @@ namespace MCUSerialBridgeCLR
|
||||
return msg;
|
||||
}
|
||||
|
||||
// M层CAN诊断：格式化CAN标识符和数据内容。
|
||||
public override string ToString()
|
||||
{
|
||||
string payloadStr =
|
||||
@@ -351,6 +363,7 @@ namespace MCUSerialBridgeCLR
|
||||
private const string DLL = @"mcu_serial_bridge.dll";
|
||||
|
||||
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
|
||||
// M层原生接口：打开MCU串口桥设备。
|
||||
internal static extern MCUSerialBridgeError msb_open(
|
||||
out IntPtr handle,
|
||||
[MarshalAs(UnmanagedType.LPStr)] string port,
|
||||
@@ -358,12 +371,15 @@ namespace MCUSerialBridgeCLR
|
||||
);
|
||||
|
||||
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
|
||||
// M层原生接口：关闭MCU串口桥设备。
|
||||
internal static extern MCUSerialBridgeError msb_close(IntPtr handle);
|
||||
|
||||
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
|
||||
// M层原生接口：复位MCU串口桥。
|
||||
internal static extern MCUSerialBridgeError msb_reset(IntPtr handle, uint timeout_ms);
|
||||
|
||||
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
|
||||
// M层原生接口：读取MCU固件版本。
|
||||
public static extern MCUSerialBridgeError msb_version(
|
||||
IntPtr handle,
|
||||
out VersionInfo version,
|
||||
@@ -371,6 +387,7 @@ namespace MCUSerialBridgeCLR
|
||||
);
|
||||
|
||||
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
|
||||
// M层原生接口：读取MCU当前运行状态。
|
||||
public static extern MCUSerialBridgeError mcu_state(
|
||||
IntPtr handle,
|
||||
out MCUState state,
|
||||
@@ -378,6 +395,7 @@ namespace MCUSerialBridgeCLR
|
||||
);
|
||||
|
||||
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
|
||||
// M层原生接口：下发串口桥端口配置。
|
||||
internal static extern MCUSerialBridgeError msb_configure(
|
||||
IntPtr handle,
|
||||
uint num_ports,
|
||||
@@ -386,6 +404,7 @@ namespace MCUSerialBridgeCLR
|
||||
);
|
||||
|
||||
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
|
||||
// M层原生接口：读取MCU数字输入。
|
||||
internal static extern MCUSerialBridgeError msb_read_input(
|
||||
IntPtr handle,
|
||||
[Out] byte[] inputs,
|
||||
@@ -393,6 +412,7 @@ namespace MCUSerialBridgeCLR
|
||||
);
|
||||
|
||||
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
|
||||
// M层原生接口：写入MCU数字输出。
|
||||
internal static extern MCUSerialBridgeError msb_write_output(
|
||||
IntPtr handle,
|
||||
[In] byte[] outputs,
|
||||
@@ -400,6 +420,7 @@ namespace MCUSerialBridgeCLR
|
||||
);
|
||||
|
||||
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
|
||||
// M层原生接口：从指定串口或CAN端口读取数据。
|
||||
internal static extern MCUSerialBridgeError msb_read_port(
|
||||
IntPtr handle,
|
||||
byte port_index,
|
||||
@@ -410,6 +431,7 @@ namespace MCUSerialBridgeCLR
|
||||
);
|
||||
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)]
|
||||
// M层硬件桥接：定义串口或CAN端口收到原生数据时的回调签名。
|
||||
internal delegate void msb_on_port_data_callback_function_t(
|
||||
IntPtr dst_data,
|
||||
uint dst_data_size,
|
||||
@@ -417,6 +439,7 @@ namespace MCUSerialBridgeCLR
|
||||
);
|
||||
|
||||
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
|
||||
// M层原生接口：注册端口数据到达回调。
|
||||
internal static extern MCUSerialBridgeError msb_register_port_data_callback(
|
||||
IntPtr handle,
|
||||
byte port_index,
|
||||
@@ -425,6 +448,7 @@ namespace MCUSerialBridgeCLR
|
||||
);
|
||||
|
||||
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
|
||||
// M层原生接口：向指定串口或CAN端口写入数据。
|
||||
internal static extern MCUSerialBridgeError msb_write_port(
|
||||
IntPtr handle,
|
||||
byte port_index,
|
||||
@@ -448,18 +472,21 @@ namespace MCUSerialBridgeCLR
|
||||
public bool IsOpen => nativeHandle != IntPtr.Zero;
|
||||
|
||||
/// <summary>构造函数，初始化对象</summary>
|
||||
// M层MCU适配：创建串口桥包装器并固定原生回调委托。
|
||||
public MCUSerialBridge()
|
||||
{
|
||||
nativeHandle = IntPtr.Zero;
|
||||
}
|
||||
|
||||
/// <summary>析构函数</summary>
|
||||
// M层MCU适配：对象回收时兜底释放原生串口桥句柄。
|
||||
~MCUSerialBridge()
|
||||
{
|
||||
Dispose(false);
|
||||
}
|
||||
|
||||
/// <summary>显式释放资源</summary>
|
||||
// M层MCU适配：释放串口桥句柄和非托管资源。
|
||||
public void Dispose()
|
||||
{
|
||||
Dispose(true);
|
||||
@@ -468,6 +495,7 @@ namespace MCUSerialBridgeCLR
|
||||
|
||||
/// <summary>内部释放资源方法</summary>
|
||||
/// <param name="disposing">true 表示手动释放，false 表示析构释放</param>
|
||||
// M层MCU适配：按托管或终结路径关闭原生句柄。
|
||||
private void Dispose(bool disposing)
|
||||
{
|
||||
if (nativeHandle != IntPtr.Zero)
|
||||
@@ -481,6 +509,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// <param name="portName">串口名，如 "COM3"</param>
|
||||
/// <param name="baud">波特率</param>
|
||||
/// <returns>错误码</returns>
|
||||
// M层单车通信：按端口名和波特率连接MCU串口桥。
|
||||
public MCUSerialBridgeError Open(string portName, uint baud)
|
||||
{
|
||||
return MCUSerialBridgeCoreAPI.msb_open(out nativeHandle, portName, baud);
|
||||
@@ -488,6 +517,7 @@ namespace MCUSerialBridgeCLR
|
||||
|
||||
/// <summary>关闭串口</summary>
|
||||
/// <returns>错误码</returns>
|
||||
// M层单车通信：关闭当前MCU串口桥连接。
|
||||
public MCUSerialBridgeError Close()
|
||||
{
|
||||
if (nativeHandle == IntPtr.Zero)
|
||||
@@ -500,6 +530,7 @@ namespace MCUSerialBridgeCLR
|
||||
|
||||
/// <summary>MCU 复位</summary>
|
||||
/// <returns>错误码</returns>
|
||||
// M层单车通信：请求MCU复位并等待结果。
|
||||
public MCUSerialBridgeError Reset(uint timeout = 200)
|
||||
{
|
||||
if (nativeHandle == IntPtr.Zero)
|
||||
@@ -512,6 +543,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// <param name="version">输出版本信息</param>
|
||||
/// <param name="timeout">超时时间（ms）</param>
|
||||
/// <returns>错误码</returns>
|
||||
// M层MCU诊断：读取串口桥固件版本。
|
||||
public MCUSerialBridgeError GetVersion(out VersionInfo version, uint timeout = 200)
|
||||
{
|
||||
version = new VersionInfo();
|
||||
@@ -525,6 +557,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// <param name="state">输出状态</param>
|
||||
/// <param name="timeout">超时时间（ms）</param>
|
||||
/// <returns>错误码</returns>
|
||||
// M层MCU诊断：读取串口桥运行状态。
|
||||
public MCUSerialBridgeError GetState(out MCUState state, uint timeout = 200)
|
||||
{
|
||||
state = new MCUState();
|
||||
@@ -538,6 +571,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// <param name="ports">端口集合</param>
|
||||
/// <param name="timeout">超时时间（ms）</param>
|
||||
/// <returns>错误码</returns>
|
||||
// M层MCU适配：批量配置CAN和串口通道参数。
|
||||
public MCUSerialBridgeError Configure(IEnumerable<PortConfig> ports, uint timeout = 200)
|
||||
{
|
||||
if (nativeHandle == IntPtr.Zero)
|
||||
@@ -587,6 +621,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// <param name="inputs">输出数组</param>
|
||||
/// <param name="timeout">超时（ms）</param>
|
||||
/// <returns>错误码</returns>
|
||||
// M层单车IO：读取MCU数字输入状态。
|
||||
public MCUSerialBridgeError ReadInput(out byte[] inputs, uint timeout = 100)
|
||||
{
|
||||
inputs = new byte[4];
|
||||
@@ -600,6 +635,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// <param name="outputs">数据数组</param>
|
||||
/// <param name="timeout">超时（ms）</param>
|
||||
/// <returns>错误码</returns>
|
||||
// M层单车IO：写入继电器、灯光等数字输出状态。
|
||||
public MCUSerialBridgeError WriteOutput(byte[] outputs, uint timeout = 100)
|
||||
{
|
||||
if (nativeHandle == IntPtr.Zero)
|
||||
@@ -629,6 +665,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// - NoData 当前无可读数据（仅在 timeout == 0 或等待超时）
|
||||
/// - Win_InvalidParam 参数错误
|
||||
/// </returns>
|
||||
// M层串口通信：同步读取指定MCU串口的数据。
|
||||
public MCUSerialBridgeError ReadSerial(byte portIndex, out byte[] buffer, uint timeout)
|
||||
{
|
||||
buffer = Array.Empty<byte>();
|
||||
@@ -670,6 +707,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// - OK 成功发送
|
||||
/// - 其他错误请查看 MCUSerialBridgeError
|
||||
/// </returns>
|
||||
// M层串口通信：向指定MCU串口发送数据。
|
||||
public MCUSerialBridgeError WriteSerial(byte portIndex, byte[] data, uint timeout)
|
||||
{
|
||||
if (nativeHandle == IntPtr.Zero)
|
||||
@@ -706,6 +744,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// - CAN_DataError CAN数据错误
|
||||
/// - Win_HandleNotFound 句柄无效
|
||||
/// </returns>
|
||||
// M层CAN通信：同步读取指定CAN通道的一帧消息。
|
||||
public MCUSerialBridgeError ReadCAN(byte portIndex, out CANMessage message, uint timeout)
|
||||
{
|
||||
message = null;
|
||||
@@ -753,6 +792,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// - CAN_DataError CAN 数据错误
|
||||
/// - Win_HandleNotFound 句柄无效
|
||||
/// </returns>
|
||||
// M层CAN通信：向指定CAN通道发送一帧消息。
|
||||
public MCUSerialBridgeError WriteCAN(byte portIndex, CANMessage message, uint timeout)
|
||||
{
|
||||
if (nativeHandle == IntPtr.Zero)
|
||||
@@ -796,6 +836,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// 5. 数据可能随时到来，请保证回调尽快返回，避免影响后续帧接收。
|
||||
/// 6. 不要把其他类型的端口注册到这个接口，接口不对 portIndex 做类型检查。
|
||||
/// </remarks>
|
||||
// M层串口通信：注册指定串口的异步接收回调。
|
||||
public MCUSerialBridgeError RegisterSerialPortCallback(
|
||||
byte portIndex,
|
||||
Action<byte[]> callback
|
||||
@@ -808,6 +849,7 @@ namespace MCUSerialBridgeCLR
|
||||
return MCUSerialBridgeError.Config_PortNumOver;
|
||||
|
||||
// 包装 C# 回调为 P/Invoke 委托
|
||||
// M层串口回调：复制原生缓存并转交托管回调处理。
|
||||
void del(IntPtr dst_data, uint dst_data_size, IntPtr user_ctx)
|
||||
{
|
||||
byte[] data = new byte[dst_data_size];
|
||||
@@ -842,6 +884,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// 5. 数据可能随时到来，请保证回调尽快返回，避免影响后续帧接收。
|
||||
/// 6. 不要把其他类型的端口注册到这个接口，接口不对 portIndex 做类型检查。
|
||||
/// </remarks>
|
||||
// M层CAN通信：注册指定CAN通道的异步接收回调。
|
||||
public MCUSerialBridgeError RegisterCANPortCallback(
|
||||
byte portIndex,
|
||||
Action<CANMessage> callback
|
||||
@@ -854,6 +897,7 @@ namespace MCUSerialBridgeCLR
|
||||
return MCUSerialBridgeError.Config_PortNumOver;
|
||||
|
||||
// 包装 C# 回调为 P/Invoke 委托
|
||||
// M层CAN回调：还原原生CAN帧并转交托管回调处理。
|
||||
void del(IntPtr dst_data, uint dst_data_size, IntPtr user_ctx)
|
||||
{
|
||||
try
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
using System;
|
||||
// MCU通信错误码
|
||||
|
||||
namespace MCUSerialBridgeCLR
|
||||
{
|
||||
@@ -47,6 +47,7 @@ namespace MCUSerialBridgeCLR
|
||||
|
||||
public static class MCUSerialBridgeErrorExtensions
|
||||
{
|
||||
// M层MCU适配：把串口桥错误码转换为便于诊断的说明。
|
||||
public static string ToDescription(this MCUSerialBridgeError err)
|
||||
{
|
||||
return err switch
|
||||
|
||||
@@ -1,44 +1,47 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>netstandard2.0</TargetFramework>
|
||||
<LangVersion>latest</LangVersion>
|
||||
<TargetFramework>net8.0</TargetFramework>
|
||||
<ImplicitUsings>enable</ImplicitUsings>
|
||||
<Nullable>disable</Nullable>
|
||||
<Platforms>AnyCPU</Platforms>
|
||||
|
||||
<AssemblyName>MedullaAdapter</AssemblyName>
|
||||
|
||||
<AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
|
||||
<OutputPath>build\Medulla\plugins\</OutputPath>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<Compile Remove="EmbeddedCommunication.cs" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<PackageReference Include="Fody" Version="6.9.3">
|
||||
<PrivateAssets>all</PrivateAssets>
|
||||
<IncludeAssets>runtime; build; native; contentfiles; analyzers; buildtransitive</IncludeAssets>
|
||||
</PackageReference>
|
||||
<PackageReference Include="Newtonsoft.Json" Version="13.0.4" />
|
||||
<PackageReference Include="System.IO.Ports" Version="6.0.0" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<Reference Include="CommonUsage">
|
||||
<HintPath>ref\CommonUsage.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="CycleGUI">
|
||||
<HintPath>ref\CycleGUI.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="MDCSToolBox">
|
||||
<HintPath>ref\MDCSToolBox.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="CartActivator">
|
||||
<HintPath>ref\RefCartActivator.dll</HintPath>
|
||||
<Private>false</Private>
|
||||
</Reference>
|
||||
<Reference Include="FundamentalLib">
|
||||
<HintPath>ref\RefFundamentalLib.dll</HintPath>
|
||||
</Reference>
|
||||
|
||||
<Reference Include="MedullaCore">
|
||||
<HintPath>ref\RefMedullaCore.dll</HintPath>
|
||||
<Private>false</Private>
|
||||
</Reference>
|
||||
|
||||
<Reference Include="FundamentalLib">
|
||||
<HintPath>ref\RefFundamentalLib.dll</HintPath>
|
||||
<Private>false</Private>
|
||||
</Reference>
|
||||
|
||||
<Reference Include="CommonUsage">
|
||||
<HintPath>ref\CommonUsage.dll</HintPath>
|
||||
<Private>false</Private>
|
||||
</Reference>
|
||||
|
||||
<Reference Include="MDCSToolBox">
|
||||
<HintPath>ref\MDCSToolBox.dll</HintPath>
|
||||
<Private>false</Private>
|
||||
</Reference>
|
||||
|
||||
<Reference Include="CycleGUI">
|
||||
<HintPath>ref\CycleGUI.dll</HintPath>
|
||||
<Private>false</Private>
|
||||
</Reference>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<WeaverFiles Include="deps\DiverCompiler.exe" />
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
|
||||
@@ -1,233 +1 @@
|
||||
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<DiverCartDefinition>
|
||||
{
|
||||
private DateTime _lastMoveTime = DateTime.Now;
|
||||
private DateTime _transmitterFleetDbgLastLog = DateTime.MinValue;
|
||||
private DateTime _transmitterFleetResetLastLog = DateTime.MinValue;
|
||||
|
||||
private void LogTransmitterFleet(string branch, bool force = false)
|
||||
{
|
||||
var now = DateTime.Now;
|
||||
if (!force && (now - _transmitterFleetDbgLastLog).TotalMilliseconds < 200) return;
|
||||
_transmitterFleetDbgLastLog = now;
|
||||
DLog.Log(
|
||||
$"branch={branch} connected={cart.TransmitterConnected} ctrl={cart.TransmitterControlEnable} " +
|
||||
$"SA={cart.Transmitter_SA} SC={cart.Transmitter_SC} SB={cart.Transmitter_SB} SD={cart.Transmitter_SD} car={cart.CarNum} " +
|
||||
$"joyLx={cart.TransmitterLeftJoystickValX:0.000} joyRy={cart.TransmitterRightJoystickValY:0.000} joyRx={cart.TransmitterRightJoystickValX:0.000} " +
|
||||
$"rawLx={cart.TransmitterLeftJoystickValXRaw} rawRy={cart.TransmitterRightJoystickValYRaw} " +
|
||||
$"remotePanel={cart.MultiVehicleRemoteManualEnabled} outEn={cart.MultiVehicleManualEnabled} outMode={cart.MultiVehicleManualMode} " +
|
||||
$"outVx={cart.MultiVehicleManualVx:0.000} outVy={cart.MultiVehicleManualVy:0.000} outVth={cart.MultiVehicleManualVth:0.000} hold={cart.MultiVehicleHold}",
|
||||
"TransmitterFleetDbg");
|
||||
}
|
||||
|
||||
private void LogTransmitterFleetReset(string reason)
|
||||
{
|
||||
var now = DateTime.Now;
|
||||
if ((now - _transmitterFleetResetLastLog).TotalMilliseconds < 500) return;
|
||||
_transmitterFleetResetLastLog = now;
|
||||
DLog.Log(
|
||||
$"reset reason={reason} remotePanel={cart.MultiVehicleRemoteManualEnabled} beforeEn={cart.MultiVehicleManualEnabled} " +
|
||||
$"beforeMode={cart.MultiVehicleManualMode} beforeVx={cart.MultiVehicleManualVx:0.000} beforeVy={cart.MultiVehicleManualVy:0.000} " +
|
||||
$"beforeVth={cart.MultiVehicleManualVth:0.000} hold={cart.MultiVehicleHold}",
|
||||
"TransmitterFleetDbg");
|
||||
}
|
||||
|
||||
public override void Operation(int iteration)
|
||||
{
|
||||
void ResetMultiVehicle(string reason)
|
||||
{
|
||||
if (cart.MultiVehicleRemoteManualEnabled)
|
||||
{
|
||||
LogTransmitterFleet("reset-skipped-remote-panel");
|
||||
return;
|
||||
}
|
||||
|
||||
if (cart.MultiVehicleManualEnabled || cart.MultiVehicleManualVx != 0 || cart.MultiVehicleManualVy != 0 ||
|
||||
cart.MultiVehicleManualVth != 0 || cart.MultiVehicleHold)
|
||||
LogTransmitterFleetReset(reason);
|
||||
|
||||
cart.MultiVehicleManualEnabled = false;
|
||||
cart.MultiVehicleManualVx = 0;
|
||||
cart.MultiVehicleManualVy = 0;
|
||||
cart.MultiVehicleManualVth = 0;
|
||||
cart.MultiVehicleHold = false;
|
||||
}
|
||||
|
||||
TriggerOnce(cart.TransmitterConnected && cart.Transmitter_SA, 700,
|
||||
() =>
|
||||
{
|
||||
Console.WriteLine("遥控器使能!");
|
||||
cart.TransmitterControlEnable = true;
|
||||
cart.TransmitterLastTime = DateTime.Now;
|
||||
LogTransmitterFleet("enable", true);
|
||||
});
|
||||
TriggerOnce(cart.TransmitterConnected && !cart.Transmitter_SA, 700,
|
||||
() =>
|
||||
{
|
||||
Console.WriteLine("遥控器断使能!");
|
||||
cart.TransmitterControlEnable = false;
|
||||
LogTransmitterFleet("disable", true);
|
||||
});
|
||||
|
||||
if (cart.TransmitterControlEnable && cart.Transmitter_SC == cart.CarNum)
|
||||
{
|
||||
CartDefinition.testPriority(5, "TransmitterMode");
|
||||
ResetMultiVehicle("single-car-control");
|
||||
LogTransmitterFleet("single-car-control");
|
||||
TransmitterChassisControl();
|
||||
cart.TransmitterLastTime = DateTime.Now;
|
||||
cart.CarStatu = "Transmitter控制模式使能";
|
||||
}
|
||||
else if (cart.TransmitterControlEnable && cart.Transmitter_SC == 3)
|
||||
{
|
||||
cart.CarStatu = "Transmitter双车联动模式使能";
|
||||
cart.MultiVehicleManualEnabled = true;
|
||||
var fleetMode = 0;
|
||||
if (cart.Transmitter_SB == TransmitterState.Mode0) fleetMode = 0;
|
||||
else if (cart.Transmitter_SB == TransmitterState.Mode1) fleetMode = 1;
|
||||
else if (cart.Transmitter_SB == TransmitterState.Mode2) fleetMode = 2;
|
||||
cart.ApplyFleetManualCommand(fleetMode, -cart.TransmitterLeftJoystickValX,
|
||||
cart.TransmitterRightJoystickValY, 1f);
|
||||
|
||||
// hold multi-vehicle sync, make speed 0
|
||||
cart.MultiVehicleHold = cart.Transmitter_SD == TransmitterState.Mode1;
|
||||
LogTransmitterFleet("fleet-control");
|
||||
}
|
||||
else
|
||||
{
|
||||
ResetMultiVehicle("idle-or-sc-mismatch");
|
||||
LogTransmitterFleet("idle-or-sc-mismatch");
|
||||
cart.CarStatu = "正常运行";
|
||||
}
|
||||
|
||||
cart.ClumsyControl = true;
|
||||
cart.LeftFrontPid.ChangeParameters(cart.DiffSteerKp, cart.DiffSteerKi, cart.DiffSteerKd, cart.DiffSteerMaxI,
|
||||
cart.DiffSteerDeadZone, cart.DiffSteerThresh, cart.DiffSteerSpeedAcc);
|
||||
cart.LeftRearPid.ChangeParameters(cart.DiffSteerKp, cart.DiffSteerKi, cart.DiffSteerKd, cart.DiffSteerMaxI,
|
||||
cart.DiffSteerDeadZone, cart.DiffSteerThresh, cart.DiffSteerSpeedAcc);
|
||||
cart.RightFrontPid.ChangeParameters(cart.DiffSteerKp, cart.DiffSteerKi, cart.DiffSteerKd, cart.DiffSteerMaxI,
|
||||
cart.DiffSteerDeadZone, cart.DiffSteerThresh, cart.DiffSteerSpeedAcc);
|
||||
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 diffRf = cart.RightFrontPid.GetResponse(cart.ThRightFront, false, false, "RF");
|
||||
var diffLr = cart.LeftRearPid.GetResponse(cart.ThLeftRear, false, false, "LR");
|
||||
var diffRr = cart.RightRearPid.GetResponse(cart.ThRightRear, false, false, "RR");
|
||||
// Hedingben.ToastText($"LF:{diffLf:0.00},LR:{diffLr:0.00},RF:{diffRf:0.00},RR:{diffRr:0.00}", "wheel-pid");
|
||||
DLog.Log($"[wheel pid] {cart.ActualThLeftFront:0.00}->{cart.ThLeftFront:0.00} diff:{CommonMath.ThDiff(cart.ThLeftFront, cart.ActualThLeftFront):0.00} pid:{diffLf:0.00}");
|
||||
cart.SpeedLFL = cart.SpeedLeftFrontLeft - diffLf;
|
||||
cart.SpeedLFR = cart.SpeedLeftFrontRight + diffLf;
|
||||
cart.SpeedRFL = cart.SpeedRightFrontLeft - diffRf;
|
||||
cart.SpeedRFR = cart.SpeedRightFrontRight + diffRf;
|
||||
cart.SpeedLRL = cart.SpeedLeftRearLeft - diffLr;
|
||||
cart.SpeedLRR = cart.SpeedLeftRearRight + diffLr;
|
||||
cart.SpeedRRL = cart.SpeedRightRearLeft - diffRr;
|
||||
cart.SpeedRRR = cart.SpeedRightRearRight + diffRr;
|
||||
List<float> wheelSpeed = new float[]
|
||||
{
|
||||
Math.Abs(cart.SpeedLFL), Math.Abs(cart.SpeedLFR), Math.Abs(cart.SpeedRFL),
|
||||
Math.Abs(cart.SpeedRFR), Math.Abs(cart.SpeedLRL),Math.Abs(cart.SpeedLRR), Math.Abs(cart.SpeedRRL),
|
||||
Math.Abs(cart.SpeedRRR)
|
||||
}.ToList();
|
||||
var wheelMaxSpeed = wheelSpeed.Max();
|
||||
var speedSign = Math.Sign(cart.ThresSpeed - cart.SendThresSpeed);
|
||||
var acc = Math.Abs(cart.ThresSpeed) > Math.Abs(cart.SendThresSpeed) ? cart.Chassis.AccPerSecond : cart.Chassis.DeAccPerSecond;
|
||||
cart.SendThresSpeed += speedSign * Math.Min(Math.Abs(cart.ThresSpeed - cart.SendThresSpeed),
|
||||
acc * (float)(DateTime.Now - _lastMoveTime).TotalSeconds);
|
||||
if (cart.ThresSpeed < wheelMaxSpeed && cart.SendThresSpeed > wheelMaxSpeed)
|
||||
cart.SendThresSpeed = Math.Min(cart.SendThresSpeed, wheelMaxSpeed);
|
||||
_lastMoveTime = DateTime.Now;
|
||||
|
||||
//灯光控制
|
||||
if (cart.AlarmLevel == 2)
|
||||
{
|
||||
//故障红灯常亮
|
||||
cart.LightMode = 2;
|
||||
}
|
||||
else if (cart.ThresSpeed != 1)
|
||||
{
|
||||
//避障黄灯常亮
|
||||
cart.LightMode = 3;
|
||||
}
|
||||
else if (!cart.WheelAbleState)
|
||||
{
|
||||
FlipFlop(ref cart.LightMode, 500, 0, 3);
|
||||
}
|
||||
else if (cart.Soc < cart.LowBatteryAlarmThreshold && cart.ElectricCurrent >= -10)
|
||||
{
|
||||
cart.LightMode = 3;
|
||||
}
|
||||
else if (cart.ElectricCurrent < -10)
|
||||
{
|
||||
FlipFlop(ref cart.LightMode, 500, 0, 1);
|
||||
}
|
||||
else
|
||||
{
|
||||
FlipFlop(ref cart.LightMode, 500, 0, 1);
|
||||
}
|
||||
}
|
||||
public void TransmitterChassisControl()
|
||||
{
|
||||
TriggerOnce(cart.Transmitter_SB == TransmitterState.Mode0, 100,
|
||||
() => cart.TransmitterControlMode = DiverCartDefinition.ManualControlMode.Normal);
|
||||
TriggerOnce(cart.Transmitter_SB == TransmitterState.Mode1, 100,
|
||||
() => cart.TransmitterControlMode = DiverCartDefinition.ManualControlMode.Crab);
|
||||
TriggerOnce(cart.Transmitter_SB == TransmitterState.Mode2, 100,
|
||||
() => cart.TransmitterControlMode = DiverCartDefinition.ManualControlMode.Spin);
|
||||
|
||||
Hedingben.ToastText($"X方向速度: {cart.TransmitterLeftJoystickValX} Y方向速度: {cart.TransmitterRightJoystickValY}");
|
||||
|
||||
if (cart.Transmitter_SD == TransmitterState.Mode1)
|
||||
{
|
||||
if(cart.Transmitter_T3 == 1694)
|
||||
cart.WheelReset();
|
||||
else if(cart.Transmitter_T3 == 151)
|
||||
cart.WheelDisable();
|
||||
}
|
||||
if (cart.Transmitter_SD == TransmitterState.Mode1 && cart.TransmitterLeftJoystickValXRaw == 353)
|
||||
{
|
||||
TriggerOnce(cart.TransmitterRightJoystickValYRaw == 353, 100, () => cart.TransmitterSpeed += 0.1f);
|
||||
TriggerOnce(cart.TransmitterRightJoystickValYRaw == 1694, 100, () => cart.TransmitterSpeed -= 0.1f);
|
||||
}
|
||||
cart.TransmitterSpeed = Math.Max(Math.Min(cart.TransmitterSpeed, cart.TransmitterSpeedUpperLimit), cart.TransmitterSpeedLowerLimit);
|
||||
//cart.TransmitterSpeed = Math.Max(cart.TransmitterSpeed, cart.TransmitterSpeedLowerLimit);
|
||||
if (!cart.Transmitter_SA)
|
||||
{
|
||||
cart.ManualControl(cart.TransmitterControlMode, 0,
|
||||
0, 0, cart.TransmitterSpeed,
|
||||
DateTime.Now - cart.TransmitterLastTime);
|
||||
cart.SpeedLeftArm = 0;
|
||||
cart.SpeedRightArm = 0;
|
||||
}
|
||||
//摇杆控制底盘
|
||||
else if (cart.Transmitter_SD == TransmitterState.Mode0)
|
||||
{
|
||||
cart.ManualControl(cart.TransmitterControlMode, cart.TransmitterLeftJoystickValX,
|
||||
cart.TransmitterRightJoystickValY, 0, cart.TransmitterSpeed,
|
||||
DateTime.Now - cart.TransmitterLastTime);
|
||||
cart.SpeedLeftArm = 0;
|
||||
cart.SpeedRightArm = 0;
|
||||
}
|
||||
//右摇杆同时控制左右夹臂
|
||||
else if (cart.Transmitter_SD == TransmitterState.Mode1 && cart.TransmitterLeftJoystickValX == 0)
|
||||
{
|
||||
cart.ManualControl(cart.TransmitterControlMode, 0,
|
||||
0, 0, cart.TransmitterSpeed,
|
||||
DateTime.Now - cart.TransmitterLastTime);
|
||||
cart.SpeedLeftArm = cart.TransmitterRightJoystickValX * cart.ManualArmSpeedFac;
|
||||
cart.SpeedRightArm = cart.TransmitterRightJoystickValX * cart.ManualArmSpeedFac;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
// 计算8个驱动电机的目标速度和舵角PID
|
||||
@@ -1,168 +0,0 @@
|
||||
using System;
|
||||
|
||||
namespace MedullaAdapter
|
||||
{
|
||||
public class BasePack
|
||||
{
|
||||
private static ushort CalculateCRC16(byte[] data)
|
||||
{
|
||||
const ushort polynomial = 0xA001; // CRC-16-IBM多项式
|
||||
ushort crc = 0xFFFF; // 初始值
|
||||
|
||||
foreach (byte b in data)
|
||||
{
|
||||
crc ^= b; // 将数据字节与CRC寄存器按位异或
|
||||
|
||||
for (int i = 0; i < 8; i++)
|
||||
{
|
||||
if ((crc & 0x0001) != 0)
|
||||
{
|
||||
crc >>= 1;
|
||||
crc ^= polynomial;
|
||||
}
|
||||
else
|
||||
{
|
||||
crc >>= 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
private byte[] _data;
|
||||
|
||||
public void SetData(byte[] data)
|
||||
{
|
||||
_data = data;
|
||||
}
|
||||
|
||||
public byte[] GetPack()
|
||||
{
|
||||
var pack = new byte[_data.Length+7];
|
||||
pack[0] = 0xBB;
|
||||
pack[1] = 0xAA;
|
||||
var length = BitConverter.GetBytes(_data.Length);
|
||||
pack[2] = length[0];
|
||||
pack[3] = length[1];
|
||||
for (int i = 0; i < _data.Length; i++)
|
||||
{
|
||||
pack[4+i] = _data[i];
|
||||
}
|
||||
|
||||
var crcByes = new byte[pack.Length - 5];
|
||||
Array.Copy(pack,2,crcByes,0,crcByes.Length);
|
||||
var crc = BitConverter.GetBytes(CalculateCRC16(crcByes));
|
||||
pack[pack.Length-1] = 0xEE;
|
||||
pack[pack.Length-2] = crc[1];
|
||||
pack[pack.Length-3] = crc[0];
|
||||
return pack;
|
||||
}
|
||||
}
|
||||
|
||||
public class ControlPack : BasePack
|
||||
{
|
||||
public ControlPack(byte controlCode)
|
||||
{
|
||||
byte[] controlData = new byte[3]{0x01,0x00,controlCode};
|
||||
SetData(controlData);
|
||||
}
|
||||
}
|
||||
|
||||
public class SetConfigPack : BasePack
|
||||
{
|
||||
public SetConfigPack(int can1BaudRate, int can2BaudRate,int modbus0BaudRate, int modbus1BaudRate, int modbus2BaudRate,
|
||||
int serialBaudRate,int upperSize = 1024,int lowerSize =1024 )
|
||||
{
|
||||
var upper = BitConverter.GetBytes(upperSize);
|
||||
var lower = BitConverter.GetBytes(lowerSize);
|
||||
var can = BitConverter.GetBytes(can1BaudRate);
|
||||
var can2 = BitConverter.GetBytes(can2BaudRate);
|
||||
var modbus0 = BitConverter.GetBytes(modbus0BaudRate);
|
||||
var modbus1 = BitConverter.GetBytes(modbus1BaudRate);
|
||||
var modbus2 = BitConverter.GetBytes(modbus2BaudRate);
|
||||
var serial = BitConverter.GetBytes(serialBaudRate);
|
||||
var canBuffer = BitConverter.GetBytes(128);
|
||||
var mbBuffer = BitConverter.GetBytes(512);
|
||||
var serialBuffer = BitConverter.GetBytes(1024);
|
||||
byte[] configData = new byte[]
|
||||
{
|
||||
0x01, 0x10, 0x02, upper[0], upper[1], upper[2], upper[3], lower[0], lower[1],
|
||||
lower[2], lower[3], 0x06, 0x00, 0x00, can[0], can[1], can[2], can[3], canBuffer[0], canBuffer[1], 0x00,
|
||||
can2[0], can2[1], can2[2], can2[3], canBuffer[0], canBuffer[1], 0x10, modbus0[0],
|
||||
modbus0[1], modbus0[2], modbus0[3], mbBuffer[0], mbBuffer[1], 0x10, modbus1[0],
|
||||
modbus1[1], modbus1[2], modbus1[3], mbBuffer[0], mbBuffer[1], 0x10, modbus2[0],
|
||||
modbus2[1], modbus2[2], modbus2[3], mbBuffer[0], mbBuffer[1], 0x20, serial[0], serial[1], serial[2],
|
||||
serial[3], serialBuffer[0], serialBuffer[1]
|
||||
};
|
||||
SetData(configData);
|
||||
}
|
||||
}
|
||||
|
||||
public class ReadConfigPack : BasePack
|
||||
{
|
||||
public ReadConfigPack()
|
||||
{
|
||||
var readConfigData = new byte[] { 0x01, 0x10, 0x00 };
|
||||
SetData(readConfigData);
|
||||
}
|
||||
}
|
||||
|
||||
public class DownloadCodePack : BasePack
|
||||
{
|
||||
public DownloadCodePack(int totalLength, int offset, int currentLength, byte[] codeBytes)
|
||||
{
|
||||
var codeData = new byte[12 + codeBytes.Length];
|
||||
codeData[0] = 0x01;
|
||||
codeData[1] = 0x11;
|
||||
var totalLengthBytes = BitConverter.GetBytes(totalLength);
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
codeData[2 + i] = totalLengthBytes[i];
|
||||
}
|
||||
var offsetBytes = BitConverter.GetBytes(offset);
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
codeData[6 + i] = offsetBytes[i];
|
||||
}
|
||||
var curLengthBytes = BitConverter.GetBytes(currentLength);
|
||||
for (int i = 0; i < 2; i++)
|
||||
{
|
||||
codeData[10 + i] = curLengthBytes[i];
|
||||
}
|
||||
|
||||
for (int i = 0; i < codeBytes.Length; i++)
|
||||
{
|
||||
codeData[12+i] = codeBytes[i];
|
||||
}
|
||||
SetData(codeData);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public class HeartBeatPack : BasePack
|
||||
{
|
||||
public HeartBeatPack(int state, int error)
|
||||
{
|
||||
var errorBytes = BitConverter.GetBytes(error);
|
||||
var heartBearData = new byte[] { 0x01, 0xF0, (byte)state, errorBytes[0], errorBytes[1] };
|
||||
SetData(heartBearData);
|
||||
}
|
||||
}
|
||||
|
||||
public class UpperIOPack : BasePack
|
||||
{
|
||||
public UpperIOPack(byte[] data,int size)
|
||||
{
|
||||
byte[] UpperData = new byte[6 + size];
|
||||
UpperData[0] = 0x01;
|
||||
UpperData[1] = 0x20;
|
||||
var length = BitConverter.GetBytes(size);
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
UpperData[2+i] = length[i];
|
||||
}
|
||||
Array.Copy(data,0,UpperData,6,size);
|
||||
SetData(UpperData);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,47 +1 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Text;
|
||||
using CartActivator;
|
||||
using MDCSToolBox.Medulla.Chassis.MultiWheel;
|
||||
|
||||
namespace MedullaAdapter
|
||||
{
|
||||
public class Remote:MultiWheelRemote<DiverCartDefinition>
|
||||
{
|
||||
[AsControlItem(name = "夹抱速度", LayoutRow = 0, LayoutCol = 4)]
|
||||
public Throttle ArmSpeed;
|
||||
|
||||
[AsControlItem(name = "夹抱打开", LayoutRow = 1, LayoutCol = 0)]
|
||||
public Button Open;
|
||||
|
||||
[AsControlItem(name = "夹抱关闭", LayoutRow = 1, LayoutCol = 2)]
|
||||
public Button Close;
|
||||
|
||||
public override void MultiVehicleModeChassisLogic()
|
||||
{
|
||||
// cart.MultiVehicleRemoteManualEnabled = true;
|
||||
// cart.MultiVehicleManualEnabled = true;
|
||||
// cart.MultiVehicleManualVx = SpeedPad.y * SpeedThreshold.val;
|
||||
// cart.MultiVehicleManualVth = -(float)Math.Pow(Math.Abs(SpeedPad.x), cart.ManualThetaPow) * Math.Sign(SpeedPad.x);
|
||||
}
|
||||
|
||||
public override void CustomOperation()
|
||||
{
|
||||
if (Open.pressed)
|
||||
{
|
||||
cart.SpeedLeftArm = -ArmSpeed.val * cart.ManualArmSpeedFac;
|
||||
cart.SpeedRightArm = -ArmSpeed.val * cart.ManualArmSpeedFac;
|
||||
}
|
||||
else if (Close.pressed)
|
||||
{
|
||||
cart.SpeedLeftArm = ArmSpeed.val * cart.ManualArmSpeedFac;
|
||||
cart.SpeedRightArm = ArmSpeed.val * cart.ManualArmSpeedFac;
|
||||
}
|
||||
else
|
||||
{
|
||||
cart.SpeedLeftArm = 0;
|
||||
cart.SpeedRightArm = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
// Medulla虚拟遥控器和夹臂控制
|
||||
@@ -1,47 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace CartActivator
|
||||
{
|
||||
public class LogicRunOnMCUAttribute:Attribute
|
||||
{
|
||||
public string mcu_url = "default";
|
||||
public int scanInterval = 50;
|
||||
}
|
||||
|
||||
public class RunOnMCU
|
||||
{
|
||||
// if return null: not data, or return payload data excluding CRC
|
||||
public static byte[] ReadEvent(int port, int event_id) => default;
|
||||
|
||||
public static void WriteEvent(byte[] payload, int port, int event_id) { }
|
||||
|
||||
// if return null: not data.
|
||||
public static byte[] ReadStream(int port) => default;
|
||||
|
||||
public static void WriteStream(byte[] payload, int port){}
|
||||
|
||||
// always have the same sized data.
|
||||
public static byte[] ReadSnapshot() => default;
|
||||
|
||||
public static void WriteSnapshot(byte[] payload)
|
||||
{
|
||||
}
|
||||
|
||||
public static T BytesToStruct<T>() where T: struct => default;
|
||||
|
||||
public static byte[] StructToBytes<T>(T what) where T : struct => default;
|
||||
|
||||
public static int GetMillisFromStart() => default;
|
||||
|
||||
public static T Iterate<T>(IEnumerable<T> ie) => default;
|
||||
}
|
||||
|
||||
public class MCUManager
|
||||
{
|
||||
public static void Use<T>()
|
||||
{
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,352 +0,0 @@
|
||||
{
|
||||
"runtimeTarget": {
|
||||
"name": ".NETStandard,Version=v2.0/",
|
||||
"signature": ""
|
||||
},
|
||||
"compilationOptions": {},
|
||||
"targets": {
|
||||
".NETStandard,Version=v2.0": {},
|
||||
".NETStandard,Version=v2.0/": {
|
||||
"MedullaAdapter/1.0.0": {
|
||||
"dependencies": {
|
||||
"Fody": "6.9.3",
|
||||
"NETStandard.Library": "2.0.3",
|
||||
"Newtonsoft.Json": "13.0.4",
|
||||
"System.IO.Ports": "6.0.0",
|
||||
"CommonUsage": "1.0.0.0",
|
||||
"CycleGUI": "1.0.0.0",
|
||||
"MDCSToolBox": "1.0.0.0",
|
||||
"RefCartActivator": "1.0.0.0",
|
||||
"RefFundamentalLib": "0.0.0.0",
|
||||
"RefMedullaCore": "1.0.0.0"
|
||||
},
|
||||
"runtime": {
|
||||
"MedullaAdapter.dll": {}
|
||||
}
|
||||
},
|
||||
"Fody/6.9.3": {},
|
||||
"Microsoft.NETCore.Platforms/1.1.0": {},
|
||||
"Microsoft.Win32.Registry/5.0.0": {
|
||||
"dependencies": {
|
||||
"System.Buffers": "4.5.1",
|
||||
"System.Memory": "4.5.4",
|
||||
"System.Security.AccessControl": "5.0.0",
|
||||
"System.Security.Principal.Windows": "5.0.0"
|
||||
},
|
||||
"runtime": {
|
||||
"lib/netstandard2.0/Microsoft.Win32.Registry.dll": {
|
||||
"assemblyVersion": "5.0.0.0",
|
||||
"fileVersion": "5.0.20.51904"
|
||||
}
|
||||
}
|
||||
},
|
||||
"NETStandard.Library/2.0.3": {
|
||||
"dependencies": {
|
||||
"Microsoft.NETCore.Platforms": "1.1.0"
|
||||
}
|
||||
},
|
||||
"Newtonsoft.Json/13.0.4": {
|
||||
"runtime": {
|
||||
"lib/netstandard2.0/Newtonsoft.Json.dll": {
|
||||
"assemblyVersion": "13.0.0.0",
|
||||
"fileVersion": "13.0.4.30916"
|
||||
}
|
||||
}
|
||||
},
|
||||
"runtime.linux-arm.runtime.native.System.IO.Ports/6.0.0": {},
|
||||
"runtime.linux-arm64.runtime.native.System.IO.Ports/6.0.0": {},
|
||||
"runtime.linux-x64.runtime.native.System.IO.Ports/6.0.0": {},
|
||||
"runtime.native.System.IO.Ports/6.0.0": {
|
||||
"dependencies": {
|
||||
"runtime.linux-arm.runtime.native.System.IO.Ports": "6.0.0",
|
||||
"runtime.linux-arm64.runtime.native.System.IO.Ports": "6.0.0",
|
||||
"runtime.linux-x64.runtime.native.System.IO.Ports": "6.0.0",
|
||||
"runtime.osx-arm64.runtime.native.System.IO.Ports": "6.0.0",
|
||||
"runtime.osx-x64.runtime.native.System.IO.Ports": "6.0.0"
|
||||
}
|
||||
},
|
||||
"runtime.osx-arm64.runtime.native.System.IO.Ports/6.0.0": {},
|
||||
"runtime.osx-x64.runtime.native.System.IO.Ports/6.0.0": {},
|
||||
"System.Buffers/4.5.1": {
|
||||
"runtime": {
|
||||
"lib/netstandard2.0/System.Buffers.dll": {
|
||||
"assemblyVersion": "4.0.3.0",
|
||||
"fileVersion": "4.6.28619.1"
|
||||
}
|
||||
}
|
||||
},
|
||||
"System.IO.Ports/6.0.0": {
|
||||
"dependencies": {
|
||||
"Microsoft.Win32.Registry": "5.0.0",
|
||||
"System.Memory": "4.5.4",
|
||||
"runtime.native.System.IO.Ports": "6.0.0"
|
||||
},
|
||||
"runtime": {
|
||||
"lib/netstandard2.0/System.IO.Ports.dll": {
|
||||
"assemblyVersion": "6.0.0.0",
|
||||
"fileVersion": "6.0.21.52210"
|
||||
}
|
||||
}
|
||||
},
|
||||
"System.Memory/4.5.4": {
|
||||
"dependencies": {
|
||||
"System.Buffers": "4.5.1",
|
||||
"System.Numerics.Vectors": "4.4.0",
|
||||
"System.Runtime.CompilerServices.Unsafe": "4.5.3"
|
||||
},
|
||||
"runtime": {
|
||||
"lib/netstandard2.0/System.Memory.dll": {
|
||||
"assemblyVersion": "4.0.1.1",
|
||||
"fileVersion": "4.6.28619.1"
|
||||
}
|
||||
}
|
||||
},
|
||||
"System.Numerics.Vectors/4.4.0": {
|
||||
"runtime": {
|
||||
"lib/netstandard2.0/System.Numerics.Vectors.dll": {
|
||||
"assemblyVersion": "4.1.3.0",
|
||||
"fileVersion": "4.6.25519.3"
|
||||
}
|
||||
}
|
||||
},
|
||||
"System.Runtime.CompilerServices.Unsafe/4.5.3": {
|
||||
"runtime": {
|
||||
"lib/netstandard2.0/System.Runtime.CompilerServices.Unsafe.dll": {
|
||||
"assemblyVersion": "4.0.4.1",
|
||||
"fileVersion": "4.6.28619.1"
|
||||
}
|
||||
}
|
||||
},
|
||||
"System.Security.AccessControl/5.0.0": {
|
||||
"dependencies": {
|
||||
"System.Security.Principal.Windows": "5.0.0"
|
||||
},
|
||||
"runtime": {
|
||||
"lib/netstandard2.0/System.Security.AccessControl.dll": {
|
||||
"assemblyVersion": "5.0.0.0",
|
||||
"fileVersion": "5.0.20.51904"
|
||||
}
|
||||
}
|
||||
},
|
||||
"System.Security.Principal.Windows/5.0.0": {
|
||||
"runtime": {
|
||||
"lib/netstandard2.0/System.Security.Principal.Windows.dll": {
|
||||
"assemblyVersion": "5.0.0.0",
|
||||
"fileVersion": "5.0.20.51904"
|
||||
}
|
||||
}
|
||||
},
|
||||
"CommonUsage/1.0.0.0": {
|
||||
"runtime": {
|
||||
"CommonUsage.dll": {
|
||||
"assemblyVersion": "1.0.0.0",
|
||||
"fileVersion": "1.0.0.0"
|
||||
}
|
||||
}
|
||||
},
|
||||
"CycleGUI/1.0.0.0": {
|
||||
"runtime": {
|
||||
"CycleGUI.dll": {
|
||||
"assemblyVersion": "1.0.0.0",
|
||||
"fileVersion": "1.0.0.0"
|
||||
}
|
||||
}
|
||||
},
|
||||
"MDCSToolBox/1.0.0.0": {
|
||||
"runtime": {
|
||||
"MDCSToolBox.dll": {
|
||||
"assemblyVersion": "1.0.0.0",
|
||||
"fileVersion": "1.0.0.0"
|
||||
}
|
||||
}
|
||||
},
|
||||
"RefCartActivator/1.0.0.0": {
|
||||
"runtime": {
|
||||
"RefCartActivator.dll": {
|
||||
"assemblyVersion": "1.0.0.0",
|
||||
"fileVersion": "0.0.0.0"
|
||||
}
|
||||
}
|
||||
},
|
||||
"RefFundamentalLib/0.0.0.0": {
|
||||
"runtime": {
|
||||
"RefFundamentalLib.dll": {
|
||||
"assemblyVersion": "0.0.0.0",
|
||||
"fileVersion": "0.0.0.0"
|
||||
}
|
||||
}
|
||||
},
|
||||
"RefMedullaCore/1.0.0.0": {
|
||||
"runtime": {
|
||||
"RefMedullaCore.dll": {
|
||||
"assemblyVersion": "1.0.0.0",
|
||||
"fileVersion": "0.0.0.0"
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
"libraries": {
|
||||
"MedullaAdapter/1.0.0": {
|
||||
"type": "project",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"Fody/6.9.3": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-1CUGgFdyECDKgi5HaUBhdv6k+VG9Iy4OCforGfHyar3xQXAJypZkzymgKtWj/4SPd6nSG0Qi7NH71qHrDSZLaA==",
|
||||
"path": "fody/6.9.3",
|
||||
"hashPath": "fody.6.9.3.nupkg.sha512"
|
||||
},
|
||||
"Microsoft.NETCore.Platforms/1.1.0": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-kz0PEW2lhqygehI/d6XsPCQzD7ff7gUJaVGPVETX611eadGsA3A877GdSlU0LRVMCTH/+P3o2iDTak+S08V2+A==",
|
||||
"path": "microsoft.netcore.platforms/1.1.0",
|
||||
"hashPath": "microsoft.netcore.platforms.1.1.0.nupkg.sha512"
|
||||
},
|
||||
"Microsoft.Win32.Registry/5.0.0": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-dDoKi0PnDz31yAyETfRntsLArTlVAVzUzCIvvEDsDsucrl33Dl8pIJG06ePTJTI3tGpeyHS9Cq7Foc/s4EeKcg==",
|
||||
"path": "microsoft.win32.registry/5.0.0",
|
||||
"hashPath": "microsoft.win32.registry.5.0.0.nupkg.sha512"
|
||||
},
|
||||
"NETStandard.Library/2.0.3": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-st47PosZSHrjECdjeIzZQbzivYBJFv6P2nv4cj2ypdI204DO+vZ7l5raGMiX4eXMJ53RfOIg+/s4DHVZ54Nu2A==",
|
||||
"path": "netstandard.library/2.0.3",
|
||||
"hashPath": "netstandard.library.2.0.3.nupkg.sha512"
|
||||
},
|
||||
"Newtonsoft.Json/13.0.4": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-pdgNNMai3zv51W5aq268sujXUyx7SNdE2bj1wZcWjAQrKMFZV260lbqYop1d2GM67JI1huLRwxo9ZqnfF/lC6A==",
|
||||
"path": "newtonsoft.json/13.0.4",
|
||||
"hashPath": "newtonsoft.json.13.0.4.nupkg.sha512"
|
||||
},
|
||||
"runtime.linux-arm.runtime.native.System.IO.Ports/6.0.0": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-75q52H7CSpgIoIDwXb9o833EvBZIXJ0mdPhz1E6jSisEXUBlSCPalC29cj3EXsjpuDwr0dj1LRXZepIQH/oL4Q==",
|
||||
"path": "runtime.linux-arm.runtime.native.system.io.ports/6.0.0",
|
||||
"hashPath": "runtime.linux-arm.runtime.native.system.io.ports.6.0.0.nupkg.sha512"
|
||||
},
|
||||
"runtime.linux-arm64.runtime.native.System.IO.Ports/6.0.0": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-xn2bMThmXr3CsvOYmS8ex2Yz1xo+kcnhVg2iVhS9PlmqjZPAkrEo/I40wjrBZH/tU4kvH0s1AE8opAvQ3KIS8g==",
|
||||
"path": "runtime.linux-arm64.runtime.native.system.io.ports/6.0.0",
|
||||
"hashPath": "runtime.linux-arm64.runtime.native.system.io.ports.6.0.0.nupkg.sha512"
|
||||
},
|
||||
"runtime.linux-x64.runtime.native.System.IO.Ports/6.0.0": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-16nbNXwv0sC+gLGIuecri0skjuh6R1maIJggsaNP7MQBcbVcEfWFUOkEnsnvoLEjy0XerfibuRptfQ8AmdIcWA==",
|
||||
"path": "runtime.linux-x64.runtime.native.system.io.ports/6.0.0",
|
||||
"hashPath": "runtime.linux-x64.runtime.native.system.io.ports.6.0.0.nupkg.sha512"
|
||||
},
|
||||
"runtime.native.System.IO.Ports/6.0.0": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-KaaXlpOcuZjMdmyF5wzzx3b+PRKIzt6A5Ax9dKenPDQbVJAFpev+casD0BIig1pBcbs3zx7CqWemzUJKAeHdSQ==",
|
||||
"path": "runtime.native.system.io.ports/6.0.0",
|
||||
"hashPath": "runtime.native.system.io.ports.6.0.0.nupkg.sha512"
|
||||
},
|
||||
"runtime.osx-arm64.runtime.native.System.IO.Ports/6.0.0": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-fXG12NodG1QrCdoaeSQ1gVnk/koi4WYY4jZtarMkZeQMyReBm1nZlSRoPnUjLr2ZR36TiMjpcGnQfxymieUe7w==",
|
||||
"path": "runtime.osx-arm64.runtime.native.system.io.ports/6.0.0",
|
||||
"hashPath": "runtime.osx-arm64.runtime.native.system.io.ports.6.0.0.nupkg.sha512"
|
||||
},
|
||||
"runtime.osx-x64.runtime.native.System.IO.Ports/6.0.0": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-/As+zPY49+dSUXkh+fTUbyPhqrdGN//evLxo4Vue88pfh1BHZgF7q4kMblTkxYvwR6Vi03zSYxysSFktO8/SDQ==",
|
||||
"path": "runtime.osx-x64.runtime.native.system.io.ports/6.0.0",
|
||||
"hashPath": "runtime.osx-x64.runtime.native.system.io.ports.6.0.0.nupkg.sha512"
|
||||
},
|
||||
"System.Buffers/4.5.1": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-Rw7ijyl1qqRS0YQD/WycNst8hUUMgrMH4FCn1nNm27M4VxchZ1js3fVjQaANHO5f3sN4isvP4a+Met9Y4YomAg==",
|
||||
"path": "system.buffers/4.5.1",
|
||||
"hashPath": "system.buffers.4.5.1.nupkg.sha512"
|
||||
},
|
||||
"System.IO.Ports/6.0.0": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-dRyGI7fUESar5ZLIpiBOaaNLW7YyOBGftjj5Of+xcduC/Rjl7RjhEnWDvvNBmHuF3d0tdXoqdVI/yrVA8f00XA==",
|
||||
"path": "system.io.ports/6.0.0",
|
||||
"hashPath": "system.io.ports.6.0.0.nupkg.sha512"
|
||||
},
|
||||
"System.Memory/4.5.4": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-1MbJTHS1lZ4bS4FmsJjnuGJOu88ZzTT2rLvrhW7Ygic+pC0NWA+3hgAen0HRdsocuQXCkUTdFn9yHJJhsijDXw==",
|
||||
"path": "system.memory/4.5.4",
|
||||
"hashPath": "system.memory.4.5.4.nupkg.sha512"
|
||||
},
|
||||
"System.Numerics.Vectors/4.4.0": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-UiLzLW+Lw6HLed1Hcg+8jSRttrbuXv7DANVj0DkL9g6EnnzbL75EB7EWsw5uRbhxd/4YdG8li5XizGWepmG3PQ==",
|
||||
"path": "system.numerics.vectors/4.4.0",
|
||||
"hashPath": "system.numerics.vectors.4.4.0.nupkg.sha512"
|
||||
},
|
||||
"System.Runtime.CompilerServices.Unsafe/4.5.3": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-3TIsJhD1EiiT0w2CcDMN/iSSwnNnsrnbzeVHSKkaEgV85txMprmuO+Yq2AdSbeVGcg28pdNDTPK87tJhX7VFHw==",
|
||||
"path": "system.runtime.compilerservices.unsafe/4.5.3",
|
||||
"hashPath": "system.runtime.compilerservices.unsafe.4.5.3.nupkg.sha512"
|
||||
},
|
||||
"System.Security.AccessControl/5.0.0": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-dagJ1mHZO3Ani8GH0PHpPEe/oYO+rVdbQjvjJkBRNQkX4t0r1iaeGn8+/ybkSLEan3/slM0t59SVdHzuHf2jmw==",
|
||||
"path": "system.security.accesscontrol/5.0.0",
|
||||
"hashPath": "system.security.accesscontrol.5.0.0.nupkg.sha512"
|
||||
},
|
||||
"System.Security.Principal.Windows/5.0.0": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-t0MGLukB5WAVU9bO3MGzvlGnyJPgUlcwerXn1kzBRjwLKixT96XV0Uza41W49gVd8zEMFu9vQEFlv0IOrytICA==",
|
||||
"path": "system.security.principal.windows/5.0.0",
|
||||
"hashPath": "system.security.principal.windows.5.0.0.nupkg.sha512"
|
||||
},
|
||||
"CommonUsage/1.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"CycleGUI/1.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"MDCSToolBox/1.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"RefCartActivator/1.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"RefFundamentalLib/0.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"RefMedullaCore/1.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
}
|
||||
}
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
File diff suppressed because it is too large
Load Diff
Binary file not shown.
@@ -1,13 +0,0 @@
|
||||
{
|
||||
"runtimeOptions": {
|
||||
"tfm": "net8.0",
|
||||
"framework": {
|
||||
"name": "Microsoft.NETCore.App",
|
||||
"version": "8.0.0"
|
||||
},
|
||||
"configProperties": {
|
||||
"System.Reflection.Metadata.MetadataUpdater.IsSupported": false,
|
||||
"System.Runtime.Serialization.EnableUnsafeBinaryFormatterSerialization": false
|
||||
}
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
@@ -1,13 +0,0 @@
|
||||
{
|
||||
"runtimeOptions": {
|
||||
"tfm": "net8.0",
|
||||
"framework": {
|
||||
"name": "Microsoft.NETCore.App",
|
||||
"version": "8.0.0"
|
||||
},
|
||||
"configProperties": {
|
||||
"System.Reflection.Metadata.MetadataUpdater.IsSupported": false,
|
||||
"System.Runtime.Serialization.EnableUnsafeBinaryFormatterSerialization": false
|
||||
}
|
||||
}
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,23 @@
|
||||
{
|
||||
"runtimeTarget": {
|
||||
"name": ".NETCoreApp,Version=v8.0",
|
||||
"signature": ""
|
||||
},
|
||||
"compilationOptions": {},
|
||||
"targets": {
|
||||
".NETCoreApp,Version=v8.0": {
|
||||
"MedullaAdapter/1.0.0": {
|
||||
"runtime": {
|
||||
"MedullaAdapter.dll": {}
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
"libraries": {
|
||||
"MedullaAdapter/1.0.0": {
|
||||
"type": "project",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
}
|
||||
}
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,79 +0,0 @@
|
||||
#define i1 char
|
||||
#define u1 unsigned char
|
||||
#define i2 short
|
||||
#define u2 unsigned short
|
||||
#define i4 int
|
||||
#define u4 unsigned int
|
||||
#define r4 float
|
||||
|
||||
// function begins
|
||||
|
||||
r4 cfun0(r4 arg0 ){
|
||||
//stack_vars:
|
||||
r4 stack_0_r4;;
|
||||
r4 stack_1_r4;;
|
||||
//local_vars:
|
||||
r4 var0;
|
||||
L_0001: stack_0_r4=(arg0); //IL_0001: ldarg.0: s_0, pop0, push1
|
||||
L_0002: stack_1_r4=(10.5f); //IL_0002: ldc.r4 10.5: s_1, pop0, push1
|
||||
L_0007: stack_0_r4=((stack_0_r4)/(stack_1_r4)); //IL_0007: div: s_2, pop2, push1
|
||||
L_0008: stack_1_r4=(60.0f); //IL_0008: ldc.r4 60: s_1, pop0, push1
|
||||
L_000d: stack_0_r4=((stack_0_r4)/(stack_1_r4)); //IL_000d: div: s_2, pop2, push1
|
||||
L_000e: stack_0_r4=(stack_0_r4); //IL_000e: conv.r8: s_1, pop1, push1
|
||||
L_000f: stack_1_r4=(3.1f); //IL_000f: ldc.r8 3.14159265358979: s_1, pop0, push1
|
||||
L_0018: stack_0_r4=((stack_0_r4)*(stack_1_r4)); //IL_0018: mul: s_2, pop2, push1
|
||||
L_0019: stack_1_r4=(85.0f); //IL_0019: ldc.r8 85: s_1, pop0, push1
|
||||
L_0022: stack_0_r4=((stack_0_r4)*(stack_1_r4)); //IL_0022: mul: s_2, pop2, push1
|
||||
L_0023: stack_1_r4=(1000.0f); //IL_0023: ldc.r8 1000: s_1, pop0, push1
|
||||
L_002c: stack_0_r4=((stack_0_r4)/(stack_1_r4)); //IL_002c: div: s_2, pop2, push1
|
||||
L_002d: stack_0_r4=(stack_0_r4); //IL_002d: conv.r4: s_1, pop1, push1
|
||||
L_002e: var0=stack_0_r4; //IL_002e: stloc.0: s_1, pop1, push0
|
||||
L_002f: goto L_0031; //IL_002f: br.s IL_0031: s_0, pop0, push0
|
||||
L_0031: stack_0_r4=(var0); //IL_0031: ldloc.0: s_0, pop0, push1
|
||||
L_0032: return stack_0_r4; //IL_0032: ret: s_1, pop1, push0
|
||||
}
|
||||
|
||||
r4 cfun1(void* arg0, r4 arg1 ){
|
||||
//stack_vars:
|
||||
r4 stack_0_r4;;
|
||||
r4 stack_1_r4;;
|
||||
//local_vars:
|
||||
r4 var0;
|
||||
L_0001: stack_0_r4=(arg1); //IL_0001: ldarg.1: s_0, pop0, push1
|
||||
L_0002: stack_1_r4=(10.5f); //IL_0002: ldc.r4 10.5: s_1, pop0, push1
|
||||
L_0007: stack_0_r4=((stack_0_r4)/(stack_1_r4)); //IL_0007: div: s_2, pop2, push1
|
||||
L_0008: stack_0_r4=(stack_0_r4); //IL_0008: conv.r8: s_1, pop1, push1
|
||||
L_0009: stack_1_r4=(3.1f); //IL_0009: ldc.r8 3.14159265358979: s_1, pop0, push1
|
||||
L_0012: stack_0_r4=((stack_0_r4)*(stack_1_r4)); //IL_0012: mul: s_2, pop2, push1
|
||||
L_0013: stack_1_r4=(85.0f); //IL_0013: ldc.r8 85: s_1, pop0, push1
|
||||
L_001c: stack_0_r4=((stack_0_r4)*(stack_1_r4)); //IL_001c: mul: s_2, pop2, push1
|
||||
L_001d: stack_0_r4=(stack_0_r4); //IL_001d: conv.r4: s_1, pop1, push1
|
||||
L_001e: var0=stack_0_r4; //IL_001e: stloc.0: s_1, pop1, push0
|
||||
L_001f: goto L_0021; //IL_001f: br.s IL_0021: s_0, pop0, push0
|
||||
L_0021: stack_0_r4=(var0); //IL_0021: ldloc.0: s_0, pop0, push1
|
||||
L_0022: return stack_0_r4; //IL_0022: ret: s_1, pop1, push0
|
||||
}
|
||||
|
||||
r4 cfun2(r4 arg0 ){
|
||||
//stack_vars:
|
||||
r4 stack_0_r4;;
|
||||
r4 stack_1_r4;;
|
||||
//local_vars:
|
||||
r4 var0;
|
||||
L_0001: stack_0_r4=(arg0); //IL_0001: ldarg.0: s_0, pop0, push1
|
||||
L_0002: stack_0_r4=(stack_0_r4); //IL_0002: conv.r8: s_1, pop1, push1
|
||||
L_0003: stack_1_r4=(267.0f); //IL_0003: ldc.r8 267.035375555132: s_1, pop0, push1
|
||||
L_000c: stack_0_r4=((stack_0_r4)/(stack_1_r4)); //IL_000c: div: s_2, pop2, push1
|
||||
L_000d: stack_1_r4=(10.5f); //IL_000d: ldc.r8 10.5: s_1, pop0, push1
|
||||
L_0016: stack_0_r4=((stack_0_r4)*(stack_1_r4)); //IL_0016: mul: s_2, pop2, push1
|
||||
L_0017: stack_1_r4=(60.0f); //IL_0017: ldc.r8 60: s_1, pop0, push1
|
||||
L_0020: stack_0_r4=((stack_0_r4)*(stack_1_r4)); //IL_0020: mul: s_2, pop2, push1
|
||||
L_0021: stack_1_r4=(1000.0f); //IL_0021: ldc.r8 1000: s_1, pop0, push1
|
||||
L_002a: stack_0_r4=((stack_0_r4)*(stack_1_r4)); //IL_002a: mul: s_2, pop2, push1
|
||||
L_002b: stack_0_r4=(stack_0_r4); //IL_002b: conv.r4: s_1, pop1, push1
|
||||
L_002c: var0=stack_0_r4; //IL_002c: stloc.0: s_1, pop1, push0
|
||||
L_002d: goto L_002f; //IL_002d: br.s IL_002f: s_0, pop0, push0
|
||||
L_002f: stack_0_r4=(var0); //IL_002f: ldloc.0: s_0, pop0, push1
|
||||
L_0030: return stack_0_r4; //IL_0030: ret: s_1, pop1, push0
|
||||
}
|
||||
|
||||
-4
@@ -1,4 +0,0 @@
|
||||
// <autogenerated />
|
||||
using System;
|
||||
using System.Reflection;
|
||||
[assembly: global::System.Runtime.Versioning.TargetFrameworkAttribute(".NETStandard,Version=v2.0", FrameworkDisplayName = ".NET Standard 2.0")]
|
||||
@@ -1,23 +0,0 @@
|
||||
//------------------------------------------------------------------------------
|
||||
// <auto-generated>
|
||||
// 此代码由工具生成。
|
||||
// 运行时版本:4.0.30319.42000
|
||||
//
|
||||
// 对此文件的更改可能会导致不正确的行为，并且如果
|
||||
// 重新生成代码，这些更改将会丢失。
|
||||
// </auto-generated>
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
using System;
|
||||
using System.Reflection;
|
||||
|
||||
[assembly: System.Reflection.AssemblyCompanyAttribute("MedullaAdapter")]
|
||||
[assembly: System.Reflection.AssemblyConfigurationAttribute("Debug")]
|
||||
[assembly: System.Reflection.AssemblyFileVersionAttribute("1.0.0.0")]
|
||||
[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0")]
|
||||
[assembly: System.Reflection.AssemblyProductAttribute("MedullaAdapter")]
|
||||
[assembly: System.Reflection.AssemblyTitleAttribute("MedullaAdapter")]
|
||||
[assembly: System.Reflection.AssemblyVersionAttribute("1.0.0.0")]
|
||||
|
||||
// 由 MSBuild WriteCodeFragment 类生成。
|
||||
|
||||
@@ -1 +0,0 @@
|
||||
2eac81d674c264ce890b4225c419fa343e662bbba0825694002aaadff6e734b7
|
||||
-8
@@ -1,8 +0,0 @@
|
||||
is_global = true
|
||||
build_property.RootNamespace = MedullaAdapter
|
||||
build_property.ProjectDir = D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\MedullaAdapter\
|
||||
build_property.EnableComHosting =
|
||||
build_property.EnableGeneratedComInterfaceComImportInterop =
|
||||
build_property.CsWinRTUseWindowsUIXamlProjections = false
|
||||
build_property.EffectiveAnalysisLevelStyle =
|
||||
build_property.EnableCodeStyleSeverity =
|
||||
Binary file not shown.
Binary file not shown.
@@ -1 +0,0 @@
|
||||
7311d51d953d1fc22835d320a3489ce25305e242222f67ae1dd4ce8b482c0a5a
|
||||
@@ -1,19 +0,0 @@
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\MedullaAdapter\obj\Debug\netstandard2.0\MedullaAdapter.csproj.AssemblyReference.cache
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\MedullaAdapter\obj\Debug\netstandard2.0\MedullaAdapter.GeneratedMSBuildEditorConfig.editorconfig
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\MedullaAdapter\obj\Debug\netstandard2.0\MedullaAdapter.AssemblyInfoInputs.cache
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\MedullaAdapter\obj\Debug\netstandard2.0\MedullaAdapter.AssemblyInfo.cs
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\MedullaAdapter\obj\Debug\netstandard2.0\MedullaAdapter.csproj.CoreCompileInputs.cache
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\MedullaAdapter\obj\Debug\netstandard2.0\MedullaAdapter.dll
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\MedullaAdapter\obj\Debug\netstandard2.0\MedullaAdapter.pdb
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\MedullaAdapter\bin\Debug\netstandard2.0\MedullaAdapter.deps.json
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\MedullaAdapter\bin\Debug\netstandard2.0\MedullaAdapter.dll
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\MedullaAdapter\bin\Debug\netstandard2.0\MedullaAdapter.pdb
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\MedullaAdapter\bin\Debug\netstandard2.0\CommonUsage.dll
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\MedullaAdapter\bin\Debug\netstandard2.0\CycleGUI.dll
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\MedullaAdapter\bin\Debug\netstandard2.0\MDCSToolBox.dll
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\MedullaAdapter\bin\Debug\netstandard2.0\RefCartActivator.dll
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\MedullaAdapter\bin\Debug\netstandard2.0\RefFundamentalLib.dll
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\MedullaAdapter\bin\Debug\netstandard2.0\RefMedullaCore.dll
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\MedullaAdapter\obj\Debug\netstandard2.0\MedullaAdapter.csproj.Fody.CopyLocal.cache
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\MedullaAdapter\obj\Debug\netstandard2.0\MedullaAdapter.csproj.Fody.RuntimeCopyLocal.cache
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\MedullaAdapter\obj\Debug\netstandard2.0\MedullaA.645DEFF4.Up2Date
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user