QUYVN/BQP
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-13 09:25:40 +07:00
parent c08ff54676
commit bccfb156d7
1938 changed files with 641646 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Simulation/SimulationVelocity.cs Normal file
View File

@@ -0,0 +1,56 @@
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);
}
}