46 lines
1.5 KiB
C#
46 lines
1.5 KiB
C#
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;
|
|
}
|
|
}
|