diff --git a/MultiWheel/MultiWheelC/Movements.cs b/MultiWheel/MultiWheelC/Movements.cs index f5c687a..b0abcad 100644 --- a/MultiWheel/MultiWheelC/Movements.cs +++ b/MultiWheel/MultiWheelC/Movements.cs @@ -173,7 +173,7 @@ namespace MultiWheelC { Console.WriteLine("驱动器上使能"); PilotDefinition.Self.ResetFromC = true; - Thread.Sleep(100); + Thread.Sleep(200); PilotDefinition.Self.ResetFromC = false; Console.WriteLine("驱动器上使能完成"); yield return false; @@ -186,7 +186,7 @@ namespace MultiWheelC { Console.WriteLine("驱动器下使能"); PilotDefinition.Self.DisableFromC = true; - Thread.Sleep(100); + Thread.Sleep(200); PilotDefinition.Self.DisableFromC = false; Console.WriteLine("驱动器下使能完成"); yield return false; diff --git a/MultiWheel/MultiWheelC/PilotConfig.cs b/MultiWheel/MultiWheelC/PilotConfig.cs index 7e50f7b..5945614 100644 --- a/MultiWheel/MultiWheelC/PilotConfig.cs +++ b/MultiWheel/MultiWheelC/PilotConfig.cs @@ -261,6 +261,9 @@ public class PilotConfig : MultiWheelPilotConfig [FieldMember(desc = "轮胎跟踪:离车时切换至盲走距离")] public float TireFollowingLeaveCarWalkBlindSwitchingDistance = 1200f; [FieldMember(desc = "轮胎跟踪:测试钻轮胎数量")] public int TireFollowingTireNum = 1; + [FieldMember(desc = "轮胎跟踪:过近距离")] public float TireFollowingCloseDistance = 1400; [FieldMember(desc = "轮胎跟踪:距离过近角度忽略阈值")] public float TireFollowingAngleIgnoreThr = 0.2f; + [FieldMember(desc = "轮胎跟踪:Y最大平均数")] public int TireFollowingYAverageFrameCount = 5; + #endregion } diff --git a/MultiWheel/MultiWheelC/TireFollowing.cs b/MultiWheel/MultiWheelC/TireFollowing.cs index 13950bb..4998ef7 100644 --- a/MultiWheel/MultiWheelC/TireFollowing.cs +++ b/MultiWheel/MultiWheelC/TireFollowing.cs @@ -122,6 +122,7 @@ namespace MultiWheelC private Painter _painter; private List _remainDistanceList = new List(); private List _remainAngleList = new List(); + private List _targetYList = new List(); private List SetFilters(float guessCenterX, float guessCenterY) { @@ -179,9 +180,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 +220,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 { lfDelta, lrDelta, rfDelta, rrDelta }; var xs = new List(); @@ -234,18 +235,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 +306,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 +324,7 @@ namespace MultiWheelC yield return false; } } - else if(WalkBlindStage2) + else if (WalkBlindStage2) { DLog.Log("达到第二对轮胎处，停止移动", "TireFollowing"); _dt.Stop(); @@ -337,7 +338,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,10 +348,10 @@ 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) + if (dis2target < PilotDefinition.Conf.TireFollowingCloseDistance && Math.Abs(targetAngle) > PilotDefinition.Conf.TireFollowingAngleIgnoreThr) { yield return true; continue; @@ -358,11 +359,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,11 +374,24 @@ namespace MultiWheelC } else { - lastDetectX = ((target.Src + target.Dst) / 2).X; - lastDetectY = ((target.Src + target.Dst) / 2).Y; - _painter.DrawLine(Color.GreenYellow, path.CarPath.Src, path.CarPath.Dst, endArrow: true); + lastDetectX = ((target.Src + target.Dst) / 2f).X; + lastDetectY = ((target.Src + target.Dst) / 2f).Y; - 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); _remainDistanceList.Add(rd); while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0); @@ -395,8 +409,8 @@ namespace MultiWheelC detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId); DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing"); } - WalkBlindCarPathDstX = path.CarPath.Dst.X; - WalkBlindCarPathDstY = path.CarPath.Dst.Y; + WalkBlindCarPathDstX = trackDst.X; + WalkBlindCarPathDstY = trackDst.Y; WalkBlindCarPathDstTh = angle2target + WalkBlindTh; DLog.Log($"切换至第一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " + $"{WalkBlindCarPathDstY:F2} " + @@ -410,8 +424,9 @@ namespace MultiWheelC _lastRRLEncoder = PilotDefinition.Self.RRLActualPos; _lastRRREncoder = PilotDefinition.Self.RRRActualPos; _remainDistanceList.Clear(); - lastDetectX = detectors[detectorIndex+1].StartGuessingX; - lastDetectY = detectors[detectorIndex+1].StartGuessingY; + _targetYList.Clear(); + lastDetectX = detectors[detectorIndex + 1].StartGuessingX; + lastDetectY = detectors[detectorIndex + 1].StartGuessingY; continue; } } @@ -420,13 +435,13 @@ namespace MultiWheelC if (detectors[detectorIndex].SwitchWalkBlindCondition(rd)) { WalkBlindStage2 = true; - WalkBlindCarPathDstX = path.CarPath.Dst.X; - WalkBlindCarPathDstY = path.CarPath.Dst.Y; + WalkBlindCarPathDstX = trackDst.X; + WalkBlindCarPathDstY = trackDst.Y; WalkBlindCarPathDstTh = angle2target + WalkBlindTh; 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) { @@ -444,11 +459,12 @@ namespace MultiWheelC _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, path.CarPath.Dst); + var track = new LineTrack(path.CarPath.Src, trackDst); track.CarDirectionBias = CarDirection; controller.UpdateTracks(new List { track }); NoTarget = false;