diff --git a/MultiWheel/MultiWheelC/Movements.cs b/MultiWheel/MultiWheelC/Movements.cs index 06d84d5..37fdb21 100644 --- a/MultiWheel/MultiWheelC/Movements.cs +++ b/MultiWheel/MultiWheelC/Movements.cs @@ -223,7 +223,7 @@ namespace MultiWheelC { Console.WriteLine("驱动器上使能"); PilotDefinition.Self.ResetFromC = true; - Thread.Sleep(100); + Thread.Sleep(200); PilotDefinition.Self.ResetFromC = false; Console.WriteLine("驱动器上使能完成"); yield return false; @@ -236,7 +236,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/TireFollowing.cs b/MultiWheel/MultiWheelC/TireFollowing.cs index 13950bb..12302a1 100644 --- a/MultiWheel/MultiWheelC/TireFollowing.cs +++ b/MultiWheel/MultiWheelC/TireFollowing.cs @@ -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 { lfDelta, lrDelta, rfDelta, rrDelta }; var xs = new List(); @@ -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) {