55 lines
1.7 KiB
C#
55 lines
1.7 KiB
C#
using RobotNet.RobotManager.Services.Simulation.Algorithm;
|
|
using RobotNet.RobotManager.Services.Simulation.Models;
|
|
|
|
namespace RobotNet.RobotManager.Services.Simulation;
|
|
|
|
public class VisualizationService
|
|
{
|
|
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;
|
|
private double RadiusRobot;
|
|
|
|
public VisualizationService WithRadiusWheel(double radiusWheel)
|
|
{
|
|
RadiusWheel = radiusWheel;
|
|
return this;
|
|
}
|
|
|
|
public VisualizationService WithRadiusRobot(double radiusRobot)
|
|
{
|
|
RadiusRobot = radiusRobot;
|
|
return this;
|
|
}
|
|
|
|
public (double x, double y, double angle) UpdatePosition(double wL, double wR, double time)
|
|
{
|
|
Theta = (Theta + time * (-wR - wL) * RadiusWheel / RadiusRobot * MathExtension.ToDegree) % 360;
|
|
X += time * (-wR + wL) * RadiusWheel * Math.Cos(Theta * MathExtension.ToRad) / 2;
|
|
Y += time * (-wR + wL) * RadiusWheel * Math.Sin(Theta * MathExtension.ToRad) / 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;
|
|
}
|
|
}
|