122 lines
3.6 KiB
C#
122 lines
3.6 KiB
C#
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 ModbusTcpClient client;
|
||
private CarStatusData carData;
|
||
|
||
double originalValue = 75.0;
|
||
double minValue = 0.0; // 范围的最小值
|
||
double maxValue = 850; // 范围的最大值
|
||
|
||
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(.2));
|
||
}
|
||
}
|
||
void OnDestroy()
|
||
{
|
||
client.CloseConnection();
|
||
}
|
||
async Task ModBusQueue()
|
||
{
|
||
while (true)
|
||
{
|
||
|
||
if (client.modbusQueue.Count > 0)
|
||
{
|
||
carData = client.modbusQueue.Dequeue();
|
||
Debug.Log(carData.SteeringWheelAngle);
|
||
vehicleController.steerInput = NormalizeValue();
|
||
|
||
}
|
||
|
||
await Task.Delay(TimeSpan.FromSeconds(.2));
|
||
}
|
||
}
|
||
|
||
|
||
void Update()
|
||
{
|
||
// 检测并更新挡位
|
||
CheckAndUpdateGear();
|
||
|
||
// 根据当前挡位设置车辆速度
|
||
SetVehicleSpeed(gearSpeeds[currentGear]);
|
||
|
||
//vehicleController.steerInput = 0;
|
||
//vehicleController.throttleInput = xx;
|
||
//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 double NormalizeValue(double originalValue, double minValue, double maxValue)
|
||
{
|
||
// 确保原始值在范围内
|
||
double clampedValue = Math.Max(minValue, Math.Min(originalValue, maxValue));
|
||
|
||
|
||
// 计算归一化值
|
||
double normalizedValue = (clampedValue - minValue) / (maxValue - minValue);
|
||
|
||
|
||
return normalizedValue;
|
||
}
|
||
|
||
}
|
||
|
||
} |