添加单车底盘仿真平台并完善运动控制与夹臂功能
This commit is contained in:
@@ -0,0 +1,38 @@
|
||||
using System.Diagnostics;
|
||||
|
||||
namespace MyParking.Simulation.Core;
|
||||
|
||||
/// <summary>
|
||||
/// 以固定周期推进离线仿真世界。
|
||||
/// </summary>
|
||||
public sealed class SimulationClock(
|
||||
SimulationWorld world,
|
||||
ILogger<SimulationClock> logger) : BackgroundService
|
||||
{
|
||||
private static readonly TimeSpan TickInterval =
|
||||
TimeSpan.FromMilliseconds(20);
|
||||
|
||||
protected override async Task ExecuteAsync(
|
||||
CancellationToken stoppingToken)
|
||||
{
|
||||
logger.LogInformation(
|
||||
"停车机器人离线仿真时钟已启动，周期{Period}ms。",
|
||||
TickInterval.TotalMilliseconds);
|
||||
|
||||
using var timer = new PeriodicTimer(TickInterval);
|
||||
var stopwatch = Stopwatch.StartNew();
|
||||
var previousSeconds = stopwatch.Elapsed.TotalSeconds;
|
||||
|
||||
while (await timer.WaitForNextTickAsync(stoppingToken))
|
||||
{
|
||||
var currentSeconds = stopwatch.Elapsed.TotalSeconds;
|
||||
var deltaTimeSeconds = Math.Clamp(
|
||||
currentSeconds - previousSeconds,
|
||||
0.001,
|
||||
0.1);
|
||||
|
||||
previousSeconds = currentSeconds;
|
||||
world.Step(deltaTimeSeconds);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,382 @@
|
||||
using MyParking.Shared;
|
||||
using MyParking.Simulation.Models;
|
||||
|
||||
namespace MyParking.Simulation.Core;
|
||||
|
||||
/// <summary>
|
||||
/// 保存单辆四舵轮停车机器人的离线仿真状态。
|
||||
/// </summary>
|
||||
public sealed class SimulationVehicle
|
||||
{
|
||||
public const double BodyLengthMeters = 1.472;
|
||||
public const double BodyWidthMeters = 0.948;
|
||||
|
||||
// 当前MDCSToolBox.dll的MultiWheelChassisInitializer运行时轮位：
|
||||
// X=±750mm、Y=±500mm，舵角机械限位为±120°。
|
||||
private const double WheelX = 0.75;
|
||||
private const double WheelY = 0.5;
|
||||
private const double MaximumBodyAcceleration = 0.6;
|
||||
private const double MaximumAngularAcceleration = 0.8;
|
||||
|
||||
private readonly List<VirtualSteerWheel> _wheels;
|
||||
private readonly double _initialX;
|
||||
private readonly double _initialY;
|
||||
private readonly double _initialYaw;
|
||||
|
||||
private Twist2D _targetBodyTwist = Twist2D.Zero;
|
||||
private double _actualVx;
|
||||
private double _actualVy;
|
||||
private double _actualOmega;
|
||||
|
||||
public SimulationVehicle(
|
||||
int vehicleId,
|
||||
double initialX,
|
||||
double initialY,
|
||||
double initialYaw)
|
||||
{
|
||||
VehicleId = vehicleId;
|
||||
_initialX = initialX;
|
||||
_initialY = initialY;
|
||||
_initialYaw = initialYaw;
|
||||
|
||||
_wheels =
|
||||
[
|
||||
new VirtualSteerWheel("左前", WheelX, WheelY),
|
||||
new VirtualSteerWheel("右前", WheelX, -WheelY),
|
||||
new VirtualSteerWheel("左后", -WheelX, WheelY),
|
||||
new VirtualSteerWheel("右后", -WheelX, -WheelY)
|
||||
];
|
||||
|
||||
Reset();
|
||||
}
|
||||
|
||||
public int VehicleId { get; }
|
||||
|
||||
public string Mode { get; private set; } = "Normal";
|
||||
|
||||
public double XMeters { get; private set; }
|
||||
|
||||
public double YMeters { get; private set; }
|
||||
|
||||
public double YawRadians { get; private set; }
|
||||
|
||||
public bool ModeReady => _wheels.All(wheel => wheel.IsAligned);
|
||||
|
||||
/// <summary>
|
||||
/// 停车并切换舵轮准备模式。
|
||||
/// </summary>
|
||||
public bool SetMode(string mode)
|
||||
{
|
||||
Stop();
|
||||
|
||||
var success = mode switch
|
||||
{
|
||||
"Normal" => PrepareParallelDirection(0.0),
|
||||
"CrabLeft" => PrepareParallelDirection(90.0),
|
||||
"CrabRight" => PrepareParallelDirection(-90.0),
|
||||
"Spin" => PrepareSpinDirection(),
|
||||
_ => false
|
||||
};
|
||||
|
||||
if (success)
|
||||
Mode = mode;
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按当前模式发送前进或后退命令。
|
||||
/// </summary>
|
||||
public bool Move(double directionSign)
|
||||
{
|
||||
if (!ModeReady)
|
||||
return false;
|
||||
|
||||
const double linearSpeed = 0.35;
|
||||
const double angularSpeed = 0.45;
|
||||
|
||||
var command = Mode switch
|
||||
{
|
||||
"Normal" => new Twist2D(
|
||||
directionSign * linearSpeed, 0.0, 0.0),
|
||||
"CrabLeft" => new Twist2D(
|
||||
0.0, directionSign * linearSpeed, 0.0),
|
||||
"CrabRight" => new Twist2D(
|
||||
0.0, -directionSign * linearSpeed, 0.0),
|
||||
"Spin" => new Twist2D(
|
||||
0.0, 0.0, directionSign * angularSpeed),
|
||||
_ => Twist2D.Zero
|
||||
};
|
||||
|
||||
return ApplyCommand(
|
||||
new ChassisCommand(VehicleId, command));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在当前运动模式下增加逆时针或顺时针转动。
|
||||
/// </summary>
|
||||
public bool Turn(double directionSign)
|
||||
{
|
||||
if (!ModeReady || Mode == "Spin")
|
||||
return false;
|
||||
|
||||
var command = new Twist2D(
|
||||
_targetBodyTwist.VxMetersPerSecond,
|
||||
_targetBodyTwist.VyMetersPerSecond,
|
||||
directionSign * 0.28);
|
||||
|
||||
return ApplyCommand(
|
||||
new ChassisCommand(VehicleId, command));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将网页虚拟遥控器的油门和转向组合为连续车体速度命令。
|
||||
/// </summary>
|
||||
public bool ManualDrive(
|
||||
double throttle,
|
||||
double steering,
|
||||
double speedScale,
|
||||
double steeringScale)
|
||||
{
|
||||
if (!AreFinite(
|
||||
throttle,
|
||||
steering,
|
||||
speedScale,
|
||||
steeringScale))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
throttle = Math.Clamp(throttle, -1.0, 1.0);
|
||||
steering = Math.Clamp(steering, -1.0, 1.0);
|
||||
speedScale = Math.Clamp(speedScale, 0.0, 1.0);
|
||||
steeringScale = Math.Clamp(steeringScale, 0.0, 1.0);
|
||||
|
||||
if (Math.Abs(throttle) < 0.001 &&
|
||||
Math.Abs(steering) < 0.001)
|
||||
{
|
||||
Stop();
|
||||
return true;
|
||||
}
|
||||
|
||||
if (!ModeReady)
|
||||
return false;
|
||||
|
||||
const double maximumLinearSpeed = 0.6;
|
||||
const double maximumAngularSpeed = 0.7;
|
||||
|
||||
var linearSpeed =
|
||||
throttle * maximumLinearSpeed * speedScale;
|
||||
var angularSpeed =
|
||||
steering * maximumAngularSpeed * steeringScale;
|
||||
|
||||
var twist = Mode switch
|
||||
{
|
||||
"Normal" => new Twist2D(
|
||||
linearSpeed,
|
||||
0.0,
|
||||
angularSpeed),
|
||||
"CrabLeft" => new Twist2D(
|
||||
0.0,
|
||||
linearSpeed,
|
||||
angularSpeed),
|
||||
"CrabRight" => new Twist2D(
|
||||
0.0,
|
||||
-linearSpeed,
|
||||
angularSpeed),
|
||||
"Spin" => new Twist2D(
|
||||
0.0,
|
||||
0.0,
|
||||
angularSpeed),
|
||||
_ => Twist2D.Zero
|
||||
};
|
||||
|
||||
return ApplyCommand(
|
||||
new ChassisCommand(VehicleId, twist));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 应用统一车体速度命令并分解为四个舵轮速度向量。
|
||||
/// </summary>
|
||||
public bool ApplyCommand(ChassisCommand command)
|
||||
{
|
||||
if (command.VehicleId != VehicleId)
|
||||
return false;
|
||||
|
||||
var twist = command.BodyTwist;
|
||||
var wheelCommands = _wheels.Select(wheel =>
|
||||
{
|
||||
var wheelVx =
|
||||
twist.VxMetersPerSecond -
|
||||
twist.OmegaRadiansPerSecond * wheel.YMeters;
|
||||
|
||||
var wheelVy =
|
||||
twist.VyMetersPerSecond +
|
||||
twist.OmegaRadiansPerSecond * wheel.XMeters;
|
||||
|
||||
return (Wheel: wheel, Vx: wheelVx, Vy: wheelVy);
|
||||
}).ToArray();
|
||||
|
||||
foreach (var item in wheelCommands)
|
||||
{
|
||||
if (!item.Wheel.SetVelocityVector(
|
||||
item.Vx,
|
||||
item.Vy))
|
||||
{
|
||||
Stop();
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
_targetBodyTwist = twist;
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将车辆目标速度设置为零并保持当前舵轮角度。
|
||||
/// </summary>
|
||||
public void Stop()
|
||||
{
|
||||
_targetBodyTwist = Twist2D.Zero;
|
||||
foreach (var wheel in _wheels)
|
||||
wheel.Stop();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 更新舵轮反馈和车辆世界位姿。
|
||||
/// </summary>
|
||||
public void Step(double deltaTimeSeconds)
|
||||
{
|
||||
foreach (var wheel in _wheels)
|
||||
wheel.Step(deltaTimeSeconds);
|
||||
|
||||
var canMove = _wheels.All(wheel => wheel.IsAligned);
|
||||
var targetVx = canMove
|
||||
? _targetBodyTwist.VxMetersPerSecond
|
||||
: 0.0;
|
||||
var targetVy = canMove
|
||||
? _targetBodyTwist.VyMetersPerSecond
|
||||
: 0.0;
|
||||
var targetOmega = canMove
|
||||
? _targetBodyTwist.OmegaRadiansPerSecond
|
||||
: 0.0;
|
||||
|
||||
_actualVx = MoveTowards(
|
||||
_actualVx,
|
||||
targetVx,
|
||||
MaximumBodyAcceleration * deltaTimeSeconds);
|
||||
|
||||
_actualVy = MoveTowards(
|
||||
_actualVy,
|
||||
targetVy,
|
||||
MaximumBodyAcceleration * deltaTimeSeconds);
|
||||
|
||||
_actualOmega = MoveTowards(
|
||||
_actualOmega,
|
||||
targetOmega,
|
||||
MaximumAngularAcceleration * deltaTimeSeconds);
|
||||
|
||||
var cos = Math.Cos(YawRadians);
|
||||
var sin = Math.Sin(YawRadians);
|
||||
|
||||
var worldVx = cos * _actualVx - sin * _actualVy;
|
||||
var worldVy = sin * _actualVx + cos * _actualVy;
|
||||
|
||||
XMeters += worldVx * deltaTimeSeconds;
|
||||
YMeters += worldVy * deltaTimeSeconds;
|
||||
YawRadians = FrameTransform2D.NormalizeAngle(
|
||||
YawRadians + _actualOmega * deltaTimeSeconds);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 恢复车辆初始位置和舵轮状态。
|
||||
/// </summary>
|
||||
public void Reset()
|
||||
{
|
||||
XMeters = _initialX;
|
||||
YMeters = _initialY;
|
||||
YawRadians = _initialYaw;
|
||||
Mode = "Normal";
|
||||
_targetBodyTwist = Twist2D.Zero;
|
||||
_actualVx = 0.0;
|
||||
_actualVy = 0.0;
|
||||
_actualOmega = 0.0;
|
||||
|
||||
foreach (var wheel in _wheels)
|
||||
wheel.Reset();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建供网页读取的不可变状态快照。
|
||||
/// </summary>
|
||||
public VehicleStateDto GetSnapshot()
|
||||
{
|
||||
return new VehicleStateDto(
|
||||
VehicleId,
|
||||
XMeters,
|
||||
YMeters,
|
||||
YawRadians,
|
||||
BodyLengthMeters,
|
||||
BodyWidthMeters,
|
||||
Mode,
|
||||
ModeReady,
|
||||
new TwistStateDto(
|
||||
_targetBodyTwist.VxMetersPerSecond,
|
||||
_targetBodyTwist.VyMetersPerSecond,
|
||||
_targetBodyTwist.OmegaRadiansPerSecond),
|
||||
new TwistStateDto(
|
||||
_actualVx,
|
||||
_actualVy,
|
||||
_actualOmega),
|
||||
_wheels.Select(wheel =>
|
||||
new WheelStateDto(
|
||||
wheel.Name,
|
||||
wheel.XMeters,
|
||||
wheel.YMeters,
|
||||
wheel.TargetAngleDegrees,
|
||||
wheel.ActualAngleDegrees,
|
||||
wheel.TargetSpeedMetersPerSecond,
|
||||
wheel.ActualSpeedMetersPerSecond,
|
||||
wheel.IsAligned)).ToArray());
|
||||
}
|
||||
|
||||
private bool PrepareParallelDirection(double targetAngleDegrees)
|
||||
{
|
||||
return _wheels.All(wheel =>
|
||||
wheel.PrepareDirection(targetAngleDegrees));
|
||||
}
|
||||
|
||||
private bool PrepareSpinDirection()
|
||||
{
|
||||
var success = true;
|
||||
|
||||
foreach (var wheel in _wheels)
|
||||
{
|
||||
var vx = -wheel.YMeters;
|
||||
var vy = wheel.XMeters;
|
||||
success &= wheel.SetVelocityVector(vx, vy);
|
||||
wheel.Stop();
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
private static double MoveTowards(
|
||||
double current,
|
||||
double target,
|
||||
double maximumChange)
|
||||
{
|
||||
var difference = target - current;
|
||||
if (Math.Abs(difference) <= maximumChange)
|
||||
return target;
|
||||
|
||||
return current + Math.Sign(difference) * maximumChange;
|
||||
}
|
||||
|
||||
private static bool AreFinite(params double[] values)
|
||||
{
|
||||
return values.All(value =>
|
||||
!double.IsNaN(value) &&
|
||||
!double.IsInfinity(value));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,205 @@
|
||||
using MyParking.Shared;
|
||||
using MyParking.Simulation.Models;
|
||||
|
||||
namespace MyParking.Simulation.Core;
|
||||
|
||||
/// <summary>
|
||||
/// 管理离线仿真车辆、车队中心和成员布局。
|
||||
/// </summary>
|
||||
public sealed class SimulationWorld
|
||||
{
|
||||
private readonly object _syncRoot = new();
|
||||
private Dictionary<int, SimulationVehicle> _vehicles = new();
|
||||
private SimulationConfigurationDto _configuration;
|
||||
|
||||
public SimulationWorld()
|
||||
{
|
||||
_configuration = CreateDefaultConfiguration();
|
||||
ApplyConfigurationCore(_configuration);
|
||||
}
|
||||
|
||||
public T WithVehicle<T>(
|
||||
int vehicleId,
|
||||
Func<SimulationVehicle, T> action)
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (!_vehicles.TryGetValue(
|
||||
vehicleId,
|
||||
out var vehicle))
|
||||
{
|
||||
throw new KeyNotFoundException(
|
||||
$"不存在车辆{vehicleId}。");
|
||||
}
|
||||
|
||||
return action(vehicle);
|
||||
}
|
||||
}
|
||||
|
||||
public void Step(double deltaTimeSeconds)
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
foreach (var vehicle in _vehicles.Values)
|
||||
vehicle.Step(deltaTimeSeconds);
|
||||
}
|
||||
}
|
||||
|
||||
public IReadOnlyList<VehicleStateDto> GetSnapshot()
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
return _vehicles.Values
|
||||
.OrderBy(vehicle => vehicle.VehicleId)
|
||||
.Select(vehicle => vehicle.GetSnapshot())
|
||||
.ToArray();
|
||||
}
|
||||
}
|
||||
|
||||
public SimulationConfigurationDto GetConfiguration()
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
return CloneConfiguration(_configuration);
|
||||
}
|
||||
}
|
||||
|
||||
public void ApplyConfiguration(
|
||||
SimulationConfigurationDto configuration)
|
||||
{
|
||||
ValidateConfiguration(configuration);
|
||||
|
||||
lock (_syncRoot)
|
||||
{
|
||||
_configuration = CloneConfiguration(configuration);
|
||||
ApplyConfigurationCore(_configuration);
|
||||
}
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
ApplyConfigurationCore(_configuration);
|
||||
}
|
||||
}
|
||||
|
||||
private void ApplyConfigurationCore(
|
||||
SimulationConfigurationDto configuration)
|
||||
{
|
||||
var fleetPoseInWorld = new Pose2D(
|
||||
configuration.FleetCenter.XMeters,
|
||||
configuration.FleetCenter.YMeters,
|
||||
configuration.FleetCenter.YawRadians);
|
||||
|
||||
_vehicles = configuration.Vehicles
|
||||
.Take(configuration.VehicleCount)
|
||||
.Select(layout =>
|
||||
{
|
||||
var bodyPoseInFleet = new Pose2D(
|
||||
layout.XMeters,
|
||||
layout.YMeters,
|
||||
layout.YawRadians);
|
||||
|
||||
var bodyPoseInWorld =
|
||||
FrameTransform2D.Compose(
|
||||
fleetPoseInWorld,
|
||||
bodyPoseInFleet);
|
||||
|
||||
return new SimulationVehicle(
|
||||
layout.VehicleId,
|
||||
bodyPoseInWorld.XMeters,
|
||||
bodyPoseInWorld.YMeters,
|
||||
bodyPoseInWorld.YawRadians);
|
||||
})
|
||||
.ToDictionary(vehicle => vehicle.VehicleId);
|
||||
}
|
||||
|
||||
private static void ValidateConfiguration(
|
||||
SimulationConfigurationDto configuration)
|
||||
{
|
||||
if (configuration.VehicleCount is < 1 or > 8)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(configuration.VehicleCount),
|
||||
"仿真车辆数量必须在1到8之间。");
|
||||
}
|
||||
|
||||
if (configuration.FleetCenter == null)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
"必须提供车队中心位姿。",
|
||||
nameof(configuration));
|
||||
}
|
||||
|
||||
if (configuration.Vehicles == null ||
|
||||
configuration.Vehicles.Count <
|
||||
configuration.VehicleCount)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
"成员布局数量不能少于车辆数量。",
|
||||
nameof(configuration));
|
||||
}
|
||||
|
||||
var selectedLayouts = configuration.Vehicles
|
||||
.Take(configuration.VehicleCount)
|
||||
.ToArray();
|
||||
|
||||
if (selectedLayouts.Any(layout =>
|
||||
layout.VehicleId <= 0) ||
|
||||
selectedLayouts
|
||||
.Select(layout => layout.VehicleId)
|
||||
.Distinct()
|
||||
.Count() != selectedLayouts.Length)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
"车辆编号必须大于零且不能重复。",
|
||||
nameof(configuration));
|
||||
}
|
||||
|
||||
var values = new[]
|
||||
{
|
||||
configuration.FleetCenter.XMeters,
|
||||
configuration.FleetCenter.YMeters,
|
||||
configuration.FleetCenter.YawRadians
|
||||
}.Concat(selectedLayouts.SelectMany(layout => new[]
|
||||
{
|
||||
layout.XMeters,
|
||||
layout.YMeters,
|
||||
layout.YawRadians
|
||||
}));
|
||||
|
||||
if (values.Any(value =>
|
||||
double.IsNaN(value) ||
|
||||
double.IsInfinity(value)))
|
||||
{
|
||||
throw new ArgumentException(
|
||||
"车队和车辆布局不能包含NaN或无穷大。",
|
||||
nameof(configuration));
|
||||
}
|
||||
}
|
||||
|
||||
private static SimulationConfigurationDto
|
||||
CreateDefaultConfiguration()
|
||||
{
|
||||
return new SimulationConfigurationDto(
|
||||
1,
|
||||
new FleetCenterDto(0.0, 0.0, 0.0),
|
||||
new[]
|
||||
{
|
||||
new VehicleLayoutDto(1, 0.0, 0.0, 0.0)
|
||||
});
|
||||
}
|
||||
|
||||
private static SimulationConfigurationDto
|
||||
CloneConfiguration(
|
||||
SimulationConfigurationDto configuration)
|
||||
{
|
||||
return new SimulationConfigurationDto(
|
||||
configuration.VehicleCount,
|
||||
configuration.FleetCenter with { },
|
||||
configuration.Vehicles
|
||||
.Select(layout => layout with { })
|
||||
.ToArray());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,174 @@
|
||||
namespace MyParking.Simulation.Core;
|
||||
|
||||
/// <summary>
|
||||
/// 模拟单个舵轮的转向和驱动响应，不包含真实电机物理模型。
|
||||
/// </summary>
|
||||
public sealed class VirtualSteerWheel
|
||||
{
|
||||
private const double AngleLowerLimitDegrees = -120.0;
|
||||
private const double AngleUpperLimitDegrees = 120.0;
|
||||
private const double MaximumSteeringRateDegreesPerSecond = 90.0;
|
||||
private const double MaximumDriveAccelerationMetersPerSecondSquared = 0.8;
|
||||
private const double AlignmentToleranceDegrees = 1.5;
|
||||
|
||||
public VirtualSteerWheel(
|
||||
string name,
|
||||
double xMeters,
|
||||
double yMeters)
|
||||
{
|
||||
Name = name;
|
||||
XMeters = xMeters;
|
||||
YMeters = yMeters;
|
||||
}
|
||||
|
||||
public string Name { get; }
|
||||
|
||||
public double XMeters { get; }
|
||||
|
||||
public double YMeters { get; }
|
||||
|
||||
public double TargetAngleDegrees { get; private set; }
|
||||
|
||||
public double ActualAngleDegrees { get; private set; }
|
||||
|
||||
public double TargetSpeedMetersPerSecond { get; private set; }
|
||||
|
||||
public double ActualSpeedMetersPerSecond { get; private set; }
|
||||
|
||||
public bool IsAligned =>
|
||||
Math.Abs(TargetAngleDegrees - ActualAngleDegrees) <=
|
||||
AlignmentToleranceDegrees;
|
||||
|
||||
/// <summary>
|
||||
/// 设置期望轮胎速度向量，并在机械限位内选择等价舵角。
|
||||
/// </summary>
|
||||
public bool SetVelocityVector(
|
||||
double vxMetersPerSecond,
|
||||
double vyMetersPerSecond)
|
||||
{
|
||||
var speed = Math.Sqrt(
|
||||
vxMetersPerSecond * vxMetersPerSecond +
|
||||
vyMetersPerSecond * vyMetersPerSecond);
|
||||
|
||||
if (speed < 1e-6)
|
||||
{
|
||||
TargetSpeedMetersPerSecond = 0.0;
|
||||
return true;
|
||||
}
|
||||
|
||||
var desiredAngleDegrees =
|
||||
Math.Atan2(vyMetersPerSecond, vxMetersPerSecond) *
|
||||
180.0 / Math.PI;
|
||||
|
||||
return SetDirectionAndSpeed(
|
||||
desiredAngleDegrees,
|
||||
speed);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 停车时设置舵轮预对齐方向。
|
||||
/// </summary>
|
||||
public bool PrepareDirection(double targetAngleDegrees)
|
||||
{
|
||||
TargetSpeedMetersPerSecond = 0.0;
|
||||
return SetDirectionAndSpeed(
|
||||
targetAngleDegrees,
|
||||
0.0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将驱动目标设置为零，并保持当前舵轮方向。
|
||||
/// </summary>
|
||||
public void Stop()
|
||||
{
|
||||
TargetSpeedMetersPerSecond = 0.0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按固定转向速度和驱动加速度更新虚拟反馈。
|
||||
/// </summary>
|
||||
public void Step(double deltaTimeSeconds)
|
||||
{
|
||||
ActualAngleDegrees = MoveTowards(
|
||||
ActualAngleDegrees,
|
||||
TargetAngleDegrees,
|
||||
MaximumSteeringRateDegreesPerSecond *
|
||||
deltaTimeSeconds);
|
||||
|
||||
var allowedTargetSpeed =
|
||||
IsAligned ? TargetSpeedMetersPerSecond : 0.0;
|
||||
|
||||
ActualSpeedMetersPerSecond = MoveTowards(
|
||||
ActualSpeedMetersPerSecond,
|
||||
allowedTargetSpeed,
|
||||
MaximumDriveAccelerationMetersPerSecondSquared *
|
||||
deltaTimeSeconds);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 恢复舵轮初始状态。
|
||||
/// </summary>
|
||||
public void Reset()
|
||||
{
|
||||
TargetAngleDegrees = 0.0;
|
||||
ActualAngleDegrees = 0.0;
|
||||
TargetSpeedMetersPerSecond = 0.0;
|
||||
ActualSpeedMetersPerSecond = 0.0;
|
||||
}
|
||||
|
||||
private bool SetDirectionAndSpeed(
|
||||
double desiredAngleDegrees,
|
||||
double desiredSpeedMetersPerSecond)
|
||||
{
|
||||
var candidates = new[]
|
||||
{
|
||||
(Angle: NormalizeDegrees(desiredAngleDegrees),
|
||||
Speed: desiredSpeedMetersPerSecond),
|
||||
(Angle: NormalizeDegrees(desiredAngleDegrees + 180.0),
|
||||
Speed: -desiredSpeedMetersPerSecond),
|
||||
(Angle: NormalizeDegrees(desiredAngleDegrees - 180.0),
|
||||
Speed: -desiredSpeedMetersPerSecond)
|
||||
};
|
||||
|
||||
var validCandidates = candidates
|
||||
.Where(candidate =>
|
||||
candidate.Angle >= AngleLowerLimitDegrees &&
|
||||
candidate.Angle <= AngleUpperLimitDegrees)
|
||||
.OrderBy(candidate =>
|
||||
Math.Abs(candidate.Angle - ActualAngleDegrees))
|
||||
.ToArray();
|
||||
|
||||
if (validCandidates.Length == 0)
|
||||
{
|
||||
TargetSpeedMetersPerSecond = 0.0;
|
||||
return false;
|
||||
}
|
||||
|
||||
var selected = validCandidates[0];
|
||||
TargetAngleDegrees = selected.Angle;
|
||||
TargetSpeedMetersPerSecond = selected.Speed;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static double MoveTowards(
|
||||
double current,
|
||||
double target,
|
||||
double maximumChange)
|
||||
{
|
||||
var difference = target - current;
|
||||
if (Math.Abs(difference) <= maximumChange)
|
||||
return target;
|
||||
|
||||
return current + Math.Sign(difference) * maximumChange;
|
||||
}
|
||||
|
||||
private static double NormalizeDegrees(double angleDegrees)
|
||||
{
|
||||
angleDegrees %= 360.0;
|
||||
if (angleDegrees >= 180.0)
|
||||
angleDegrees -= 360.0;
|
||||
if (angleDegrees < -180.0)
|
||||
angleDegrees += 360.0;
|
||||
return angleDegrees;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user