ParkingRobot/CommonUsage-MultiVehicleSync/commonusage/Chassis/SingleSteerChassis.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Numerics;
using System.Text;
using System.Diagnostics;
using CommonUsage.Mathematics;
using FundamentalLib;

namespace CommonUsage.Chassis
{
	public class SingleSteerChassis : AbstractChassis
	{
		public void SetSteerWheel(SteerWheel wheel)
		{
			_steerWheel = wheel;
		}

		public SteerWheel GetSteerWheel()
		{
			return _steerWheel;
		}

        public override void Visualize()
        {
            
        }

        public override CarSpeed GetCarSpeed(bool isActual = false)
        {
            if (!isActual)
            {
                var sendAngle = _steerWheel.GetSendAngle();
                var sendAngleRad = _steerWheel.GetSendAngle() / 180f * Math.PI;
                // VSteer* Cos = v;
                var vsteer = _sendSpeed / ((Math.Cos(Math.Abs(sendAngleRad)) + 0.000001));
                var vsteerY = vsteer * Math.Sin(sendAngleRad);
				// Console.WriteLine($"{vsteer} {vsteerY} {sendAngleRad} {_sendSpeed}");
                return new CarSpeed()
                {
                    Vx = (float)(_sendSpeed * Math.Cos(Math.Abs(sendAngle) / 180f * Math.PI)),
                    Vy = 0,
                    Vw = (float)(_sendSpeed * Math.Sin(Math.Abs(sendAngle) / 180f * Math.PI) /
                        Math.Abs(_steerWheel.Position.X / 1000f) / Math.PI * 180f)
					//阿克曼
                    // Vx = (float)(_sendSpeed),
                    // Vy = 0,
                    // Vw = (float)(vsteerY / Math.Abs(_steerWheel.Position.X / 1000f) / Math.PI * 180f)
                };
            }
            else
            {
                return new CarSpeed()
                {
                    Vx = (float)(_steerWheel.ReadSpeed() *
                                 Math.Cos(Math.Abs(_steerWheel.ReadAngle()) / 180f * Math.PI)),
                    Vy = 0,
                    Vw = (float)(_steerWheel.ReadSpeed() *
                        Math.Sin(Math.Abs(_steerWheel.ReadAngle()) / 180f * Math.PI) /
                        Math.Abs(_steerWheel.Position.X / 1000f) / Math.PI * 180f)
                };
            }
            
        }

        public override void Initialize()
		{
			GeometricControlPoints = new List<GeometricControlPoint>()
			{
				new (_steerWheel.Position),
				new (Vector2.Zero)
			};
			Valid = true;
		}

		public override void AfterDirectionChanged()
		{
			if (Math.Abs(CommonMath.ThDiff(0, _originBiasTh)) > 90)
			{
				GeometricControlPoints = new List<GeometricControlPoint>()
				{
					new (-_steerWheel.Position),
				};
				_direction = -1;
			}
			else
			{
				GeometricControlPoints = new List<GeometricControlPoint>()
				{
					new (_steerWheel.Position),
				};
				_direction = 1;
			}
		}

		public override void PredefinedDriveStop()
		{
			if (!Valid) return;
			_sendSpeed = 0;
			_steerWheel.WriteSpeed(_sendSpeed);
			GoingActive = false;
			RotatingActive = false;
		}

        protected override void DefineGeometricWheelComputation(float speed)
        {
            var now = DateTime.Now;

            if (!GoingActive)
            {
                LastMoveTime = now;
                GoingWheelAligned = false;
            }

            SendSteerMotion(speed * _direction, GeometricControlPoints[0].Theta, now - LastMoveTime);

            GoingActive = true;
            RotatingActive = false;
        }

		public override bool ComputeRotateWheels(float rotSpeed)
		{
			if (!RotatingActive)
            {
                LastMoveTime = DateTime.Now;
                GoingWheelAligned = false;
            }

			SendSteerMotion(rotSpeed, 90);

			GoingActive = false;
			RotatingActive = true;
			return true;
		}

		public void SendSteerMotion(float speed, float theta, TimeSpan? deltaTime = null)
		{
			_steerWheel.WriteAngle(theta);

            if (!GoingWheelAligned && Math.Abs(CommonMath.ThDiff(_steerWheel.ReadAngle(), theta)) < 1)
                GoingWheelAligned = true;
			if (!GoingWheelAligned) speed = 0;

			var turnThresholdSpeed = CalculateTurningSpeedDecayFac(Math.Abs(theta)) * MaxSpeed;
			AccumulateSpeed(Math.Min(turnThresholdSpeed, Math.Abs(speed)) * Math.Sign(speed), deltaTime);

            LastMoveTime = DateTime.Now;
        }

		public override float CalculateTurningSpeedDecayFac(float turn)
		{
			return 1 - Math.Min(turn, MaxTurnThreshold) / MaxTurnThreshold * MinTurnSpeedFac;
		}

		private void AccumulateSpeed(float v, TimeSpan? deltaTime = null)
		{
			// _targetSpeed = v;
			var speedSign = Math.Sign(v - _sendSpeed);
			var acc = Math.Abs(v) > Math.Abs(_sendSpeed) ? AccPerSecond : DeAccPerSecond;
			_sendSpeed += speedSign * Math.Min(Math.Abs(v - _sendSpeed),
				acc * (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds);
			_steerWheel.WriteSpeed(_sendSpeed);

			if (Debug)
				Console.WriteLine($"SingleSteer, target:{v:0.00},send:{_sendSpeed:0.0}");
		}

		private SteerWheel _steerWheel;
		private float _sendSpeed;
		private int _direction = 1; // 1 forward, -1 backward
	}
}