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,126 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.IO;
+using System.Numerics;
+using System.Security.Cryptography.X509Certificates;
+using System.Text;
+using FundamentalLib;
+using Newtonsoft.Json;
+
+namespace CommonUsage.Chassis
+{
+    public abstract class AbstractChassis
+    {
+	    protected AbstractChassis()
+	    {
+		    Valid = false;
+	    }
+
+	    public abstract void Initialize();
+
+        public abstract void Visualize();
+
+	    public abstract void AfterDirectionChanged();
+
+        /// <summary>
+        /// 当前行进方向。
+        /// </summary>
+        [Obsolete]
+        public float DirectionAngle
+        {
+	        get => _originBiasTh;
+	        set
+	        {
+                _originBiasTh = value;
+				if (_originBiasTh != _lastDirectionAngle) AfterDirectionChanged();
+                _lastDirectionAngle = _originBiasTh;
+	        }
+        }
+
+        protected float _originBiasX = 0f, _originBiasY = 0f, _originBiasTh;
+
+        public Vector3 GetOriginBias()
+        {
+            return new Vector3(_originBiasX, _originBiasY, _originBiasTh);
+        }
+
+        public class CarSpeed
+        {
+            public float Vx,Vy,Vw;
+        }
+
+        public abstract CarSpeed GetCarSpeed(bool isActual = false);
+        public List<GeometricControlPoint> GetGeometricControlPoints()
+        {
+	        return GeometricControlPoints;
+        }
+
+        public void ComputeWheelsGeometrically(float speed)
+        {
+            // 打印调用位置信息
+            var stackTrace = new StackTrace(true);
+            var callerFrame = stackTrace.GetFrame(1); // 获取调用者的帧
+            if (callerFrame != null)
+            {
+                var fileName = callerFrame.GetFileName();
+                var lineNumber = callerFrame.GetFileLineNumber();
+                DLog.Log($"s:{speed:0.000} from {fileName} ln.{lineNumber}", $"WheelComputeCaller");
+            }
+
+            DefineGeometricWheelComputation(speed);
+        }
+
+        protected abstract void DefineGeometricWheelComputation(float speed);
+
+        public void DriveStop()
+        {
+            PredefinedDriveStop();
+            CustomDriveStop?.Invoke();
+        }
+
+        public abstract void PredefinedDriveStop();
+
+        public Action CustomDriveStop;
+
+        public abstract bool ComputeRotateWheels(float rotSpeed);
+
+        public abstract float CalculateTurningSpeedDecayFac(float turn);
+
+        public enum ChassisState
+        {
+	        Standby,
+            Running,
+            AbnormalFeedback,
+            ExceedMotionAbility,
+        }
+
+        protected ChassisState State;
+
+        protected string StateDescription;
+
+        public (ChassisState State, string Description) GetChassisState()
+        {
+	        return (State, StateDescription);
+        }
+
+        public bool Debug = false;
+        public float AccPerSecond = 0.2f;
+        public float DeAccPerSecond = 0.2f;
+        public float MaxSpeed = 1; // m/s
+        public float MinTurnSpeedFac = 0.5f;
+        public float MaxTurnThreshold = 90f;
+
+        public float GcpThetaPerSecond = 10f;
+
+        public DateTime LastMoveTime = DateTime.MinValue;
+
+		protected bool Valid = false;
+		protected List<GeometricControlPoint> GeometricControlPoints = new();
+		protected bool RotatingActive = false;
+		protected bool GoingActive = false;
+		protected bool GoingWheelAligned = false;
+
+		private float _lastDirectionAngle = 0;
+    }
+}