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

45
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Simulation/SimulationVisualization.cs Normal file
View File

@@ -0,0 +1,45 @@
namespace RobotNet10.RobotApp.Services.Simulation;
public class SimulationVisualization
{
public double X { get; private set; }
public double Y { get; private set; }
public double Theta { get; private set; }
public double Vx { get; private set; }
public double Vy { get; private set; }
public double Omega { get; private set; }
private double RadiusWheel = 0;
private double RadiusRobot = 0;
public void SetPhysical(double radiusWheel, double robotWidth)
{
RadiusWheel = radiusWheel;
RadiusRobot = robotWidth / 2;
}
public (double x, double y, double angle) UpdatePosition(double wL, double wR, double time)
{
Theta = (Theta + time * (-wR - wL) * RadiusWheel / RadiusRobot * 180 / Math.PI) % 360;
X += time * (-wR + wL) * RadiusWheel * Math.Cos(Theta * Math.PI / 180) / 2;
Y += time * (-wR + wL) * RadiusWheel * Math.Sin(Theta * Math.PI / 180) / 2;
_ = UpdateVelocity(wL, wR);
if (Theta > 180) Theta -= 360;
else if (Theta < -180) Theta += 360;
return (X, Y, Theta);
}
public (double vx, double vy, double omega) UpdateVelocity(double wL, double wR)
{
Vx = (-wR + wL) * RadiusWheel / 2;
Omega = (-wR - wL) * RadiusWheel / RadiusRobot;
return (Vx, 0, Omega);
}
public void LocalizationInitialize(double x, double y, double theta)
{
X = x;
Y = y;
Theta = theta;
}
}