update

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;

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 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 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);
@@ -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,7 +358,7 @@ 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);
 
@@ -373,8 +373,8 @@ namespace MultiWheelC
                 }
                 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,