First_Dev_Dept/Tutorial
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'master' of http://git.fairylandtech.com/ruifeng.zhou/Tutorial

Browse Source
This commit is contained in:
ruifeng.zhou
2026-06-30 16:13:04 +08:00
parent 306be14a41 d5fdf60843
commit 3411654a34
3 changed files with 49 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheel/MultiWheelC/Movements.cs
+2 -2
View File
@@ -173,7 +173,7 @@ namespace MultiWheelC
{ {
Console.WriteLine("驱动器上使能"); Console.WriteLine("驱动器上使能");
PilotDefinition.Self.ResetFromC = true; PilotDefinition.Self.ResetFromC = true;
Thread.Sleep(100); Thread.Sleep(200);
PilotDefinition.Self.ResetFromC = false; PilotDefinition.Self.ResetFromC = false;
Console.WriteLine("驱动器上使能完成"); Console.WriteLine("驱动器上使能完成");
yield return false; yield return false;
@@ -186,7 +186,7 @@ namespace MultiWheelC
{ {
Console.WriteLine("驱动器下使能"); Console.WriteLine("驱动器下使能");
PilotDefinition.Self.DisableFromC = true; PilotDefinition.Self.DisableFromC = true;
Thread.Sleep(100); Thread.Sleep(200);
PilotDefinition.Self.DisableFromC = false; PilotDefinition.Self.DisableFromC = false;
Console.WriteLine("驱动器下使能完成"); Console.WriteLine("驱动器下使能完成");
yield return false; yield return false;
MultiWheel/MultiWheelC/PilotConfig.cs
+3
View File
@@ -261,6 +261,9 @@ public class PilotConfig : MultiWheelPilotConfig
[FieldMember(desc = "轮胎跟踪:离车时切换至盲走距离")] public float TireFollowingLeaveCarWalkBlindSwitchingDistance = 1200f; [FieldMember(desc = "轮胎跟踪:离车时切换至盲走距离")] public float TireFollowingLeaveCarWalkBlindSwitchingDistance = 1200f;
[FieldMember(desc = "轮胎跟踪:测试钻轮胎数量")] public int TireFollowingTireNum = 1; [FieldMember(desc = "轮胎跟踪:测试钻轮胎数量")] public int TireFollowingTireNum = 1;
[FieldMember(desc = "轮胎跟踪:过近距离")] public float TireFollowingCloseDistance = 1400;
[FieldMember(desc = "轮胎跟踪:距离过近角度忽略阈值")] public float TireFollowingAngleIgnoreThr = 0.2f; [FieldMember(desc = "轮胎跟踪:距离过近角度忽略阈值")] public float TireFollowingAngleIgnoreThr = 0.2f;
[FieldMember(desc = "轮胎跟踪:Y最大平均数")] public int TireFollowingYAverageFrameCount = 5;
#endregion #endregion
} }
MultiWheel/MultiWheelC/TireFollowing.cs
+44 -28
View File
@@ -122,6 +122,7 @@ namespace MultiWheelC
private Painter _painter; private Painter _painter;
private List<float> _remainDistanceList = new List<float>(); private List<float> _remainDistanceList = new List<float>();
private List<float> _remainAngleList = 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) private List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
{ {
@@ -179,9 +180,9 @@ namespace MultiWheelC
} }
th = th_x == 0 ? th_y : th_x; th = th_x == 0 ? th_y : th_x;
x = (aDelta.X + bDelta.X) / 2 - (a.Y - b.Y) / 2 * th; x = (aDelta.X + bDelta.X) / 2f - (a.Y - b.Y) / 2f * th;
y = (aDelta.Y + bDelta.Y) / 2 + (a.X - b.X) / 2 * th; y = (aDelta.Y + bDelta.Y) / 2f + (a.X - b.X) / 2f * th;
return (x, y, th); return (x, y, th);
} }
@@ -219,10 +220,10 @@ namespace MultiWheelC
var curRRREncoder = PilotDefinition.Self.RRRActualPos; var curRRREncoder = PilotDefinition.Self.RRRActualPos;
// Average delta per wheel pair (LF, LR, RF, RR) // Average delta per wheel pair (LF, LR, RF, RR)
var lfDelta = (curLFLEncoder - _lastLFLEncoder + curLFREncoder - _lastLFREncoder) / 2; var lfDelta = (curLFLEncoder - _lastLFLEncoder + curLFREncoder - _lastLFREncoder) / 2f;
var lrDelta = (curLRLEncoder - _lastLRLEncoder + curLRREncoder - _lastLRREncoder) / 2; var lrDelta = (curLRLEncoder - _lastLRLEncoder + curLRREncoder - _lastLRREncoder) / 2f;
var rfDelta = (curRFLEncoder - _lastRFLEncoder + curRFREncoder - _lastRFREncoder) / 2; var rfDelta = (curRFLEncoder - _lastRFLEncoder + curRFREncoder - _lastRFREncoder) / 2f;
var rrDelta = (curRRLEncoder - _lastRRLEncoder + curRRREncoder - _lastRRREncoder) / 2; var rrDelta = (curRRLEncoder - _lastRRLEncoder + curRRREncoder - _lastRRREncoder) / 2f;
var deltaList = new List<float> { lfDelta, lrDelta, rfDelta, rrDelta }; var deltaList = new List<float> { lfDelta, lrDelta, rfDelta, rrDelta };
var xs = new List<float>(); var xs = new List<float>();
@@ -234,18 +235,18 @@ namespace MultiWheelC
{ {
var sw1 = steerWheels[i]; var sw1 = steerWheels[i];
var a = sw1.Position; var a = sw1.Position;
var tha = sw1.ReadAngle() / 180 * (float)Math.PI; var tha = sw1.ReadAngle() / 180f * (float)Math.PI;
var deltaa = deltaList[i]; var deltaa = deltaList[i];
var va = new Vector2(deltaa * (float)Math.Cos(tha), deltaa * (float)Math.Sin(tha)); var va = new Vector2(deltaa * (float)Math.Cos(tha), deltaa * (float)Math.Sin(tha));
for (var j = i + 1; j < steerWheels.Count; ++j) for (var j = i + 1; j < steerWheels.Count; ++j)
{ {
var sw2 = steerWheels[j]; var sw2 = steerWheels[j];
var b = sw2.Position; var b = sw2.Position;
var thb = sw2.ReadAngle() / 180 * (float)Math.PI; var thb = sw2.ReadAngle() / 180f * (float)Math.PI;
var deltab = deltaList[j]; var deltab = deltaList[j];
var vb = new Vector2(deltab * (float)Math.Cos(thb), deltab * (float)Math.Sin(thb)); var vb = new Vector2(deltab * (float)Math.Cos(thb), deltab * (float)Math.Sin(thb));
var (tempx, tempy, tempth) = CenterMoveFromPoints(a, va, b, vb); var (tempx, tempy, tempth) = CenterMoveFromPoints(a, va, b, vb);
Hedingben.ToastText($"{tempx:f2} {tempy:f2} {tempth / Math.PI * 180:f2} ", $"{i}_{j}"); Hedingben.ToastText($"{tempx:f2} {tempy:f2} {tempth / Math.PI * 180f:f2} ", $"{i}_{j}");
xs.Add(tempx); xs.Add(tempx);
ys.Add(tempy); ys.Add(tempy);
ths.Add(tempth); ths.Add(tempth);
@@ -305,7 +306,7 @@ namespace MultiWheelC
// 检查是否达到盲走结束条件 // 检查是否达到盲走结束条件
if (detectors[detectorIndex].FinishWalkBlindCondition(rd)) if (detectors[detectorIndex].FinishWalkBlindCondition(rd))
{ {
if(WalkBlindStage1) if (WalkBlindStage1)
{ {
DLog.Log("达到第一次盲走停止距离，停下或开始钻第二对轮胎", "TireFollowing"); DLog.Log("达到第一次盲走停止距离，停下或开始钻第二对轮胎", "TireFollowing");
//if (detectors[detectorIndex].DstId != -1 && detectors[detectorIndex].LeaveSrcFunction != null) //if (detectors[detectorIndex].DstId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
@@ -323,7 +324,7 @@ namespace MultiWheelC
yield return false; yield return false;
} }
} }
else if(WalkBlindStage2) else if (WalkBlindStage2)
{ {
DLog.Log("达到第二对轮胎处，停止移动", "TireFollowing"); DLog.Log("达到第二对轮胎处，停止移动", "TireFollowing");
_dt.Stop(); _dt.Stop();
@@ -337,7 +338,7 @@ namespace MultiWheelC
var target = detectors[detectorIndex].DetectFunction(CarDirection, lastDetectX, var target = detectors[detectorIndex].DetectFunction(CarDirection, lastDetectX,
SetFilters(lastDetectX, lastDetectY)); SetFilters(lastDetectX, lastDetectY));
if(target == null) if (target == null)
{ {
DLog.Log("无目标，等待下一帧", "TireFollowing"); DLog.Log("无目标，等待下一帧", "TireFollowing");
controller.FirstRotateMaxSpeed = 0; controller.FirstRotateMaxSpeed = 0;
@@ -347,10 +348,10 @@ namespace MultiWheelC
else controller.FirstRotateMaxSpeed = 5; else controller.FirstRotateMaxSpeed = 5;
var targetAngle = CalculateAngle2YAxis(target.Src, target.Dst); var targetAngle = CalculateAngle2YAxis(target.Src, target.Dst);
var targetPos = new Vector2((target.Src.X + target.Dst.X) / 2, (target.Src.Y + target.Dst.Y) / 2); 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)); var dis2target = (float)Math.Sqrt(Math.Pow(targetPos.X, 2) + Math.Pow(targetPos.Y, 2));
//距离较近以后角度容易跳变 //距离较近以后角度容易跳变
if (dis2target < 1400 && Math.Abs(targetAngle) > PilotDefinition.Conf.TireFollowingAngleIgnoreThr) if (dis2target < PilotDefinition.Conf.TireFollowingCloseDistance && Math.Abs(targetAngle) > PilotDefinition.Conf.TireFollowingAngleIgnoreThr)
{ {
yield return true; yield return true;
continue; continue;
@@ -358,11 +359,11 @@ namespace MultiWheelC
else _remainAngleList.Add(targetAngle); else _remainAngleList.Add(targetAngle);
while (_remainAngleList.Count > 10) _remainAngleList.RemoveAt(0); while (_remainAngleList.Count > 10) _remainAngleList.RemoveAt(0);
angle2target = _remainAngleList.Average(); angle2target = _remainAngleList.Average();
var distanceLabelPos = targetPos / 2; var distanceLabelPos = targetPos / 2f;
_painter.DrawLine(Color.Cyan, Vector2.Zero, targetPos, width: 2); _painter.DrawLine(Color.Cyan, Vector2.Zero, targetPos, width: 2);
_painter.DrawText(Color.Yellow, $"{dis2target:F3}", distanceLabelPos.X, distanceLabelPos.Y); _painter.DrawText(Color.Yellow, $"{dis2target:F3}", distanceLabelPos.X, distanceLabelPos.Y);
var path = DetectorHelper.GetApproachPath(target, CoordinateSystem.Car2D, pathLen:3000, var path = DetectorHelper.GetApproachPath(target, CoordinateSystem.Car2D, pathLen: 3000,
bias: detectors[detectorIndex].PathTransformation, bias: detectors[detectorIndex].PathTransformation,
biasAnchorDistance: detectors[detectorIndex].PathTransformationAnchorDistance); biasAnchorDistance: detectors[detectorIndex].PathTransformationAnchorDistance);
@@ -373,11 +374,24 @@ namespace MultiWheelC
} }
else else
{ {
lastDetectX = ((target.Src + target.Dst) / 2).X; lastDetectX = ((target.Src + target.Dst) / 2f).X;
lastDetectY = ((target.Src + target.Dst) / 2).Y; lastDetectY = ((target.Src + target.Dst) / 2f).Y;
_painter.DrawLine(Color.GreenYellow, path.CarPath.Src, path.CarPath.Dst, endArrow: true);
var rd = (float)LessMath.PerpendicularPosition(0, 0, path.CarPath.Dst.X, path.CarPath.Dst.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); path.CarPath.Src.X, path.CarPath.Src.Y);
_remainDistanceList.Add(rd); _remainDistanceList.Add(rd);
while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0); while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0);
@@ -395,8 +409,8 @@ namespace MultiWheelC
detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId); detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing"); DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
} }
WalkBlindCarPathDstX = path.CarPath.Dst.X; WalkBlindCarPathDstX = trackDst.X;
WalkBlindCarPathDstY = path.CarPath.Dst.Y; WalkBlindCarPathDstY = trackDst.Y;
WalkBlindCarPathDstTh = angle2target + WalkBlindTh; WalkBlindCarPathDstTh = angle2target + WalkBlindTh;
DLog.Log($"切换至第一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " + DLog.Log($"切换至第一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
$"{WalkBlindCarPathDstY:F2} " + $"{WalkBlindCarPathDstY:F2} " +
@@ -410,8 +424,9 @@ namespace MultiWheelC
_lastRRLEncoder = PilotDefinition.Self.RRLActualPos; _lastRRLEncoder = PilotDefinition.Self.RRLActualPos;
_lastRRREncoder = PilotDefinition.Self.RRRActualPos; _lastRRREncoder = PilotDefinition.Self.RRRActualPos;
_remainDistanceList.Clear(); _remainDistanceList.Clear();
lastDetectX = detectors[detectorIndex+1].StartGuessingX; _targetYList.Clear();
lastDetectY = detectors[detectorIndex+1].StartGuessingY; lastDetectX = detectors[detectorIndex + 1].StartGuessingX;
lastDetectY = detectors[detectorIndex + 1].StartGuessingY;
continue; continue;
} }
} }
@@ -420,13 +435,13 @@ namespace MultiWheelC
if (detectors[detectorIndex].SwitchWalkBlindCondition(rd)) if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
{ {
WalkBlindStage2 = true; WalkBlindStage2 = true;
WalkBlindCarPathDstX = path.CarPath.Dst.X; WalkBlindCarPathDstX = trackDst.X;
WalkBlindCarPathDstY = path.CarPath.Dst.Y; WalkBlindCarPathDstY = trackDst.Y;
WalkBlindCarPathDstTh = angle2target + WalkBlindTh; WalkBlindCarPathDstTh = angle2target + WalkBlindTh;
DLog.Log($"切换至最后一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " + DLog.Log($"切换至最后一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
$"{WalkBlindCarPathDstY:F2} " + $"{WalkBlindCarPathDstY:F2} " +
$"{WalkBlindCarPathDstTh:F2}", "TireFollowing"); $"{WalkBlindCarPathDstTh:F2}", "TireFollowing");
if(detectors.Count == 1) if (detectors.Count == 1)
{ {
if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null) if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
{ {
@@ -444,11 +459,12 @@ namespace MultiWheelC
_lastRRLEncoder = PilotDefinition.Self.RRLActualPos; _lastRRLEncoder = PilotDefinition.Self.RRLActualPos;
_lastRRREncoder = PilotDefinition.Self.RRRActualPos; _lastRRREncoder = PilotDefinition.Self.RRRActualPos;
_remainDistanceList.Clear(); _remainDistanceList.Clear();
_targetYList.Clear();
continue; continue;
} }
} }
DLog.Log($"投影点剩余距离:{rd:F2}", "TireFollowing"); DLog.Log($"投影点剩余距离:{rd:F2}", "TireFollowing");
var track = new LineTrack(path.CarPath.Src, path.CarPath.Dst); var track = new LineTrack(path.CarPath.Src, trackDst);
track.CarDirectionBias = CarDirection; track.CarDirectionBias = CarDirection;
controller.UpdateTracks(new List<AbstractTrack> { track }); controller.UpdateTracks(new List<AbstractTrack> { track });
NoTarget = false; NoTarget = false;