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/Movements.cs
+2 -2
View File
@@ -223,7 +223,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;
@@ -236,7 +236,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/TireFollowing.cs
+20 -20
View File
@@ -179,9 +179,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 +219,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 +234,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 +305,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 +323,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 +337,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,7 +347,7 @@ 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 < 1400 && Math.Abs(targetAngle) > PilotDefinition.Conf.TireFollowingAngleIgnoreThr)
@@ -358,11 +358,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,8 +373,8 @@ 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); _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 rd = (float)LessMath.PerpendicularPosition(0, 0, path.CarPath.Dst.X, path.CarPath.Dst.Y,
@@ -410,8 +410,8 @@ 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; lastDetectX = detectors[detectorIndex + 1].StartGuessingX;
lastDetectY = detectors[detectorIndex+1].StartGuessingY; lastDetectY = detectors[detectorIndex + 1].StartGuessingY;
continue; continue;
} }
} }
@@ -426,7 +426,7 @@ namespace MultiWheelC
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)
{ {