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

update

Browse Source
This commit is contained in:
shuai.li
2026-06-29 17:39:28 +08:00
parent 8b19a15fbb
commit 32c380ace2
2 changed files with 22 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheel/MultiWheelC/TireFollowing.cs
+20 -20
View File
@@ -179,9 +179,9 @@ namespace MultiWheelC
}
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);
}
@@ -219,10 +219,10 @@ namespace MultiWheelC
var curRRREncoder = PilotDefinition.Self.RRRActualPos;
// Average delta per wheel pair (LF, LR, RF, RR)
var lfDelta = (curLFLEncoder - _lastLFLEncoder + curLFREncoder - _lastLFREncoder) / 2;
var lrDelta = (curLRLEncoder - _lastLRLEncoder + curLRREncoder - _lastLRREncoder) / 2;
var rfDelta = (curRFLEncoder - _lastRFLEncoder + curRFREncoder - _lastRFREncoder) / 2;
var rrDelta = (curRRLEncoder - _lastRRLEncoder + curRRREncoder - _lastRRREncoder) / 2;
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>();
@@ -234,18 +234,18 @@ namespace MultiWheelC
{
var sw1 = steerWheels[i];
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 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() / 180 * (float)Math.PI;
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 * 180:f2} ", $"{i}_{j}");
Hedingben.ToastText($"{tempx:f2} {tempy:f2} {tempth / Math.PI * 180f:f2} ", $"{i}_{j}");
xs.Add(tempx);
ys.Add(tempy);
ths.Add(tempth);
@@ -305,7 +305,7 @@ namespace MultiWheelC
// 检查是否达到盲走结束条件
if (detectors[detectorIndex].FinishWalkBlindCondition(rd))
{
if(WalkBlindStage1)
if (WalkBlindStage1)
{
DLog.Log("达到第一次盲走停止距离，停下或开始钻第二对轮胎", "TireFollowing");
//if (detectors[detectorIndex].DstId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
@@ -323,7 +323,7 @@ namespace MultiWheelC
yield return false;
}
}
else if(WalkBlindStage2)
else if (WalkBlindStage2)
{
DLog.Log("达到第二对轮胎处，停止移动", "TireFollowing");
_dt.Stop();
@@ -337,7 +337,7 @@ namespace MultiWheelC
var target = detectors[detectorIndex].DetectFunction(CarDirection, lastDetectX,
SetFilters(lastDetectX, lastDetectY));
if(target == null)
if (target == null)
{
DLog.Log("无目标，等待下一帧", "TireFollowing");
controller.FirstRotateMaxSpeed = 0;
@@ -347,7 +347,7 @@ namespace MultiWheelC
else controller.FirstRotateMaxSpeed = 5;
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));
//距离较近以后角度容易跳变
if (dis2target < 1400 && Math.Abs(targetAngle) > PilotDefinition.Conf.TireFollowingAngleIgnoreThr)
@@ -358,11 +358,11 @@ namespace MultiWheelC
else _remainAngleList.Add(targetAngle);
while (_remainAngleList.Count > 10) _remainAngleList.RemoveAt(0);
angle2target = _remainAngleList.Average();
var distanceLabelPos = targetPos / 2;
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,
var path = DetectorHelper.GetApproachPath(target, CoordinateSystem.Car2D, pathLen: 3000,
bias: detectors[detectorIndex].PathTransformation,
biasAnchorDistance: detectors[detectorIndex].PathTransformationAnchorDistance);
@@ -373,8 +373,8 @@ namespace MultiWheelC
}
else
{
lastDetectX = ((target.Src + target.Dst) / 2).X;
lastDetectY = ((target.Src + target.Dst) / 2).Y;
lastDetectX = ((target.Src + target.Dst) / 2f).X;
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,
@@ -410,8 +410,8 @@ namespace MultiWheelC
_lastRRLEncoder = PilotDefinition.Self.RRLActualPos;
_lastRRREncoder = PilotDefinition.Self.RRRActualPos;
_remainDistanceList.Clear();
lastDetectX = detectors[detectorIndex+1].StartGuessingX;
lastDetectY = detectors[detectorIndex+1].StartGuessingY;
lastDetectX = detectors[detectorIndex + 1].StartGuessingX;
lastDetectY = detectors[detectorIndex + 1].StartGuessingY;
continue;
}
}
@@ -426,7 +426,7 @@ namespace MultiWheelC
DLog.Log($"切换至最后一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
$"{WalkBlindCarPathDstY:F2} " +
$"{WalkBlindCarPathDstTh:F2}", "TireFollowing");
if(detectors.Count == 1)
if (detectors.Count == 1)
{
if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
{