76 lines
2.7 KiB
C#
76 lines
2.7 KiB
C#
namespace RobotNet.RobotManager.Services.Simulation;
|
|
|
|
public class VelocityController(VisualizationService Visualization)
|
|
{
|
|
private double Acceleration = 2;
|
|
private double Deceleration = 10;
|
|
private double AngularVelLeft;
|
|
private double AngularVelRight;
|
|
private double SampleTime = 0.05;
|
|
|
|
public VelocityController WithAcceleration(double acc)
|
|
{
|
|
Acceleration = acc;
|
|
return this;
|
|
}
|
|
|
|
public VelocityController WithDeceleration(double dec)
|
|
{
|
|
Deceleration = dec;
|
|
return this;
|
|
}
|
|
|
|
public VelocityController WithSampleTime(double time)
|
|
{
|
|
SampleTime = time;
|
|
return this;
|
|
}
|
|
|
|
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)
|
|
{
|
|
(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, SampleTime);
|
|
}
|
|
}
|