57 lines
2.5 KiB
C#
57 lines
2.5 KiB
C#
namespace RobotNet10.RobotApp.Services.Simulation;
|
|
|
|
public class SimulationVelocity(SimulationVisualization Visualization, SimulationConfig Model)
|
|
{
|
|
private readonly double Acceleration = Model.Acceleration;
|
|
private readonly double Deceleration = Model.Deceleration;
|
|
private double AngularVelLeft;
|
|
private double AngularVelRight;
|
|
|
|
private (double angularVelLeft, double angularVelRight) AccelerationCalculator(double wL, double wR, double wL_Current, double wR_Current)
|
|
{
|
|
var angularVelLeft = wL_Current;
|
|
var angularVelRight = wR_Current;
|
|
if (wL_Current == 0 || wL / wL_Current < 0)
|
|
{
|
|
if (wL != 0) angularVelLeft += wL / Math.Abs(wL) * Acceleration;
|
|
else angularVelLeft = wL;
|
|
}
|
|
else
|
|
{
|
|
if (Math.Abs(wL) - Math.Abs(wL_Current) > Acceleration) angularVelLeft += Acceleration * wL_Current / Math.Abs(wL_Current);
|
|
else if (Math.Abs(wL_Current) - Math.Abs(wL) > Deceleration) angularVelLeft -= Deceleration * wL_Current / Math.Abs(wL_Current);
|
|
else angularVelLeft = wL;
|
|
}
|
|
|
|
if (wR_Current == 0 || wR / wR_Current < 0)
|
|
{
|
|
if (wR != 0) angularVelRight += wR / Math.Abs(wR) * Acceleration;
|
|
else angularVelRight = wR;
|
|
}
|
|
else
|
|
{
|
|
if (Math.Abs(wR) - Math.Abs(wR_Current) > Acceleration) angularVelRight += Acceleration * wR_Current / Math.Abs(wR_Current);
|
|
else if (Math.Abs(wR_Current) - Math.Abs(wR) > Deceleration) angularVelRight -= Deceleration * wR_Current / Math.Abs(wR_Current);
|
|
else angularVelRight = wR;
|
|
}
|
|
|
|
if (Math.Abs(angularVelLeft) > Math.Abs(wL)) angularVelLeft = wL;
|
|
if (Math.Abs(angularVelRight) > Math.Abs(wR)) angularVelRight = wR;
|
|
return (angularVelLeft, angularVelRight);
|
|
}
|
|
|
|
public bool SetSpeed(double angularVelLeft, double angularVelRight, double sampleTime)
|
|
{
|
|
(AngularVelLeft, AngularVelRight) = AccelerationCalculator(angularVelLeft, angularVelRight, AngularVelLeft, AngularVelRight);
|
|
//Console.WriteLine($"AngularVelLeft = {AngularVelLeft:0.####}, AngularVelRight = {AngularVelRight:0.####}");
|
|
_ = Visualization.UpdatePosition(AngularVelLeft, AngularVelRight, sampleTime);
|
|
return true;
|
|
}
|
|
|
|
public void Stop()
|
|
{
|
|
(AngularVelLeft, AngularVelRight) = (0, 0);
|
|
_ = Visualization.UpdatePosition(AngularVelLeft, AngularVelRight, 0.05);
|
|
}
|
|
}
|