H_SafeExperienceDrivingSystem/U3D_DrivingSystem/Assets/Script/CarGearControl.cs

using EVP;
using UnityEngine;

namespace Script
{
    using ModbusManager;
    using System;
    using System.Threading.Tasks;
    using Unity.VisualScripting;
    using UnityEditor.PackageManager;
    using UnityEngine;
    using static UnityEditor.LightingExplorerTableColumn;

    public class CarGearControl : MonoBehaviour
    {
        private VehicleController vehicleController;

        private VehicleStandardInput vehicleStandardInput;
        public int currentGear = 0; // 当前挡位
        public float[] gearSpeeds = { 0f, 20f, 40f, 60f }; // 每个挡位的最大速度

        public float xx;
        float steerInput, throttleInput, brakeInput, handbrakeInput;

        private CarStatusData carData;

        float minValue = -300f; // 范围的最小值
        float maxValue = 300; // 范围的最大值

        public CarInfoManager carInfoManager;

        void Start()
        {
            vehicleController = GetComponent<VehicleController>();
            vehicleStandardInput = GetComponent<VehicleStandardInput>();

            // StartModbus();
            ModBusQueue();
        }

        // async Task StartModbus()
        // {
        //     client = new ModbusTcpClient();
        //     await client.ConnectToServer();
        //     while (true)
        //     {
        //         await client.SendModbusRequest();
        //
        //         await Task.Delay(TimeSpan.FromSeconds(.1));
        //     }
        // }

        // void OnDestroy()
        // {
        //     client.CloseConnection();
        // }

        async Task ModBusQueue()
        {
            while (true)
            {
                // Debug.Log(client.modbusQueue.Count);
                if (ModbusTcpClient.modbusTcpClient.modbusQueue.Count > 0)
                {
                    carData = ModbusTcpClient.modbusTcpClient.modbusQueue.Dequeue();

                    //方向盘
                    vehicleController.steerInput = NormalizeValue(carData.SteeringWheelAngle, minValue, maxValue);
                    //手刹
                    // vehicleController.throttleInput = carData.HandbrakeStatus == 0 ? 1 : 0;
                    //挡位 00空挡，1前进档，2倒挡，3为P档
                    //当钥匙为点火状态，才能启动车
                    if (carData.IgnitionSwitch == 2)
                    {
                        switch (carData.GearPosition)
                        {
                            case 0:
                                break;
                            case 1: //1前进档
                                if (carData.HandbrakeStatus == 0)
                                {
                                    if (carData.AcceleratorPedalPosition > 30)
                                    {
                                        vehicleController.throttleInput = 1;
                                    }
                                    else
                                    {
                                        vehicleController.throttleInput = 0;
                                    }
                                }
                                else
                                {
                                    vehicleController.throttleInput = 0;
                                }

                                break;
                            case 2: //2倒挡
                                if (carData.HandbrakeStatus == 0)
                                {
                                    if (carData.AcceleratorPedalPosition > 30)
                                    {
                                        vehicleController.throttleInput = -0.5f;
                                    }
                                    else
                                    {
                                        vehicleController.throttleInput = 0;
                                    }
                                }
                                else
                                {
                                    vehicleController.throttleInput = 0;
                                }

                                break;
                            case 3:
                                break;
                        }
                    }

                    //转向灯 00是未开转向灯，1是左转向灯，2是右转向灯
                    //当钥匙转为通电才能启动转向灯 0熄火，1通电，2启动
                    if (carData.IgnitionSwitch == 1)
                    {
                        switch (carData.TurnSignalStatus)
                        {
                            case 0:
                                carInfoManager.StopBlinking();
                                break;
                            case 1:
                                carInfoManager.LeftStartBlinking();
                                break;
                            case 2:
                                carInfoManager.RightStartBlinking();
                                break;
                        }
                    }
                }

                await Task.Delay(TimeSpan.FromSeconds(.1));
            }
        }


        void Update()
        {
            // 检测并更新挡位
            CheckAndUpdateGear();

            // 根据当前挡位设置车辆速度
            SetVehicleSpeed(gearSpeeds[currentGear]);

            //vehicleController.steerInput = 0;

            // vehicleController.brakeInput = 0;
            // vehicleController.handbrakeInput = 0;
            //Debug.Log($"{steerInput}--{throttleInput}--{brakeInput}--{handbrakeInput}");

            // vehicleStandardInput.SetThrottleValue(xx);

            // SetSpeedBasedOnValue(1);
        }

        void CheckAndUpdateGear()
        {
            if (Input.GetKeyDown(KeyCode.Alpha1)) currentGear = 1;
            if (Input.GetKeyDown(KeyCode.Alpha2)) currentGear = 2;
            if (Input.GetKeyDown(KeyCode.Alpha3)) currentGear = 3;
            if (Input.GetKeyDown(KeyCode.Alpha0)) currentGear = 0; // 假设0为空挡
        }

        void SetVehicleSpeed(float maxSpeed)
        {
            // 在这里，你需要根据你的车辆控制系统来调整速度。
            // 以下是一个假设的方法，你可能需要根据EVP5的API来修改它。
            vehicleController.maxSpeedForward = maxSpeed;
            //vehicleController.throttleInput = 1;
        }

        public float NormalizeValue(float originalValue, float minValue, float maxValue)
        {
            // 确保原始值在范围内
            float clampedValue = Math.Max(minValue, Math.Min(originalValue, maxValue));

            // 计算归一化值
            float normalizedValue = (2 * (clampedValue - minValue) / (maxValue - minValue)) - 1;

            return normalizedValue;
        }
    }
}