Initial commit from MyParking project

2026-08-04 10:29:21 +08:00
commit d9432bd529
138 changed files with 16957 additions and 0 deletions
@@ -0,0 +1,170 @@
+using FundamentalLib;
+using System;
+using System.Collections.Generic;
+using System.Numerics;
+using System.Text;
+using System.Diagnostics;
+
+namespace CommonUsage.Chassis
+{
+	public class DifferentialChassis : AbstractChassis
+	{
+		public void SetLeftRightWheels(Wheel wheelL, Wheel wheelR)
+		{
+			_leftWheel = wheelL;
+			_rightWheel = wheelR;
+			_halfWheelTrack = Math.Abs(_leftWheel.Position.Y);
+		}
+
+        public override void Visualize()
+        {
+            
+        }
+
+        public override CarSpeed GetCarSpeed(bool isActual = false)
+        {
+            if (!isActual)
+            {
+                return new CarSpeed()
+                {
+                    Vx = (_speedL + _speedR) / 2f,
+                    Vw = (_speedR - _speedL) / Math.Abs(_leftWheel.Position.Y - _rightWheel.Position.Y) /
+                        (float)Math.PI * 180f * 1000f,
+                    Vy = 0
+                };
+            }
+            else
+            {
+                return new CarSpeed()
+                {
+                    Vx = GetLinearSpeed(),
+                    Vw = (_rightWheel.ReadSpeed() - _leftWheel.ReadSpeed()) /
+                        Math.Abs(_leftWheel.Position.Y - _rightWheel.Position.Y) /
+                        (float)Math.PI * 180f * 1000f,
+                    Vy = 0
+                };
+            }
+            
+        }
+
+        public (Wheel,Wheel) GetWheels()
+        {
+            return (_leftWheel, _rightWheel);
+        }
+
+		public float GetLinearSpeed()
+		{
+			return (_leftWheel.ReadSpeed() + _rightWheel.ReadSpeed()) / 2f;
+		}
+
+		public override void Initialize()
+		{
+			GeometricControlPoints.Add(new GeometricControlPoint(new Vector2(0, 0)));
+			Valid = true;
+		}
+
+		public override void AfterDirectionChanged()
+		{
+
+		}
+
+		public override void PredefinedDriveStop()
+		{
+			if (!Valid) return;
+			_sendSpeedL = _sendSpeedR = 0;
+            _speedL = _speedR = 0;
+			_leftWheel.WriteSpeed(_sendSpeedL);
+			_rightWheel.WriteSpeed(_sendSpeedR);
+			GoingActive = false;
+			RotatingActive = false;
+		}
+
+        protected override void DefineGeometricWheelComputation(float speed)
+        {
+            var now = DateTime.Now;
+            if (!GoingActive) LastMoveTime = now;
+
+            SendSpeed(speed, GeometricControlPoints[0].Theta, now - LastMoveTime);
+
+            GoingActive = true;
+            RotatingActive = false;
+        }
+
+		public override bool ComputeRotateWheels(float rotSpeed)
+		{
+			if (!RotatingActive) LastMoveTime = DateTime.Now;
+
+			SendSpeed(0, rotSpeed);
+
+			GoingActive = false;
+			RotatingActive = true;
+			return true;
+		}
+
+		public override float CalculateTurningSpeedDecayFac(float turn)
+		{
+			return 1 - Math.Min(turn, MaxTurnThreshold) / MaxTurnThreshold * MinTurnSpeedFac;
+		}
+
+		public void SendSpeed(float linearSpeed, float angularSpeed, TimeSpan? deltaTime = null)
+		{
+			var edgeLinearSpeed = (float)(angularSpeed / 180f * Math.PI * _halfWheelTrack / 1000);
+			var vl = linearSpeed - edgeLinearSpeed;
+			var vr = linearSpeed + edgeLinearSpeed;
+            _speedL = vl;
+			_speedR = vr;
+			AccumulateSpeed(vl, vr, deltaTime);
+            LastMoveTime = DateTime.Now;
+        }
+
+		private void AccumulateSpeed(float vl, float vr, TimeSpan? deltaTime = null)
+		{
+            // var dTime = (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds;
+            //
+            // var speedSignL = Math.Sign(vl - _sendSpeedL);
+            // var accL = Math.Abs(vl) > Math.Abs(_sendSpeedL) ? AccPerSecond : DeAccPerSecond;
+            // _sendSpeedL += speedSignL * Math.Min(Math.Abs(vl - _sendSpeedL), accL * dTime);
+            // _leftWheel.WriteSpeed(_sendSpeedL);
+            //
+            // var speedSignR = Math.Sign(vr - _sendSpeedR);
+            // var accR = Math.Abs(vr) > Math.Abs(_sendSpeedR) ? AccPerSecond : DeAccPerSecond;
+            // _sendSpeedR += speedSignR * Math.Min(Math.Abs(vr - _sendSpeedR), accR * dTime);
+            // _rightWheel.WriteSpeed(_sendSpeedR);
+
+            // if (Debug)
+            // 	Console.WriteLine($"DiffChassis, target:{v:0.00},send:{_sendSpeed:0.0}");
+            // var dTime = (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds;
+            var dTime = (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds;
+            float diffL = vl - _sendSpeedL;
+            float diffR = vr - _sendSpeedR;
+
+            float accL = Math.Abs(vl) > Math.Abs(_sendSpeedL) ? AccPerSecond : DeAccPerSecond;
+            float accR = Math.Abs(vr) > Math.Abs(_sendSpeedR) ? AccPerSecond : DeAccPerSecond;
+
+            float maxDeltaL = accL * dTime;
+            float maxDeltaR = accR * dTime;
+
+            float factorL = Math.Abs(diffL) > maxDeltaL ? maxDeltaL / Math.Abs(diffL) : 1.0f;
+            float factorR = Math.Abs(diffR) > maxDeltaR ? maxDeltaR / Math.Abs(diffR) : 1.0f;
+
+            float factor = Math.Min(factorL, factorR);
+
+            _sendSpeedL += diffL * factor;
+            _sendSpeedR += diffR * factor;
+
+            _leftWheel.WriteSpeed(_sendSpeedL);
+            _rightWheel.WriteSpeed(_sendSpeedR);
+            // Console.WriteLine($"DiffChassis, target:{vl:0.00},send:{_sendSpeedL:0.00} dTime：{dTime} diffL:{diffL} factor:{factor}" );
+        }
+
+        private Wheel _leftWheel;
+		private Wheel _rightWheel;
+		private float _halfWheelTrack; // millimeter
+
+		private float _sendSpeedL;
+		private float _sendSpeedR;
+		private int _direction = 1; // 1 forward, -1 backward
+        private float _speedL;
+        private float _speedR;
+    }
+}