129 lines
6.5 KiB
C#
129 lines
6.5 KiB
C#
using RobotNet10.RobotApp.Interfaces;
|
|
using RobotNet10.RobotApp.Shared.Enums;
|
|
|
|
namespace RobotNet10.RobotApp.Services.Simulation.Navigation;
|
|
|
|
public class DifferentialNavigation : SimulationNavigation
|
|
{
|
|
private readonly Logger<DifferentialNavigation> Logger;
|
|
|
|
public DifferentialNavigation(IServiceProvider ServiceProvider) : base(ServiceProvider)
|
|
{
|
|
using var scope = ServiceProvider.CreateScope();
|
|
Logger = scope.ServiceProvider.GetRequiredService<Logger<DifferentialNavigation>>();
|
|
}
|
|
|
|
private bool IsBackToPath = false;
|
|
private double? BackToAngle;
|
|
|
|
protected override void NavigationHandler()
|
|
{
|
|
try
|
|
{
|
|
if (NavState == NavigationState.Rotating)
|
|
{
|
|
if (RotatePID is not null)
|
|
{
|
|
double Error = Visualization.Theta - TargetAngle;
|
|
if (Error > 180) Error -= 360;
|
|
else if (Error < -180) Error += 360;
|
|
if (Math.Abs(Error) < 1)
|
|
{
|
|
if(IsBackToPath && BackToAngle.HasValue)
|
|
{
|
|
TargetAngle = BackToAngle.Value;
|
|
BackToAngle = null;
|
|
IsBackToPath = false;
|
|
}
|
|
else if (MovePurePursuit is not null && MovePurePursuit.Waypoints_Value is not null && MovePurePursuit.Waypoints_Value.Count > 2) NavState = NavigationState.Moving;
|
|
else
|
|
{
|
|
NavState = NavigationState.Completed;
|
|
Dispose();
|
|
}
|
|
}
|
|
else
|
|
{
|
|
var SpeedCal = RotatePID.PID_step(Error * Math.PI / 180, AngularVelocity, -AngularVelocity, CycleHandlerMilliseconds / 1000.0);
|
|
VelocityController.SetSpeed(SpeedCal, SpeedCal, CycleHandlerMilliseconds / 1000.0);
|
|
}
|
|
}
|
|
}
|
|
else if (NavState == NavigationState.Moving)
|
|
{
|
|
if (MovePurePursuit is not null && MovePurePursuit.Waypoints_Value is not null && MovePurePursuit?.OrderNodes is not null && MovePurePursuit.OrderNodes.Length > 1 && GoalRotate is not null)
|
|
{
|
|
if (MovePID is not null && MoveFuzzy is not null && MovePurePursuit is not null)
|
|
{
|
|
var DistanceToGoal = Math.Sqrt(Math.Pow(Visualization.X - MovePurePursuit.OrderNodes[^1].X, 2) + Math.Pow(Visualization.Y - MovePurePursuit.OrderNodes[^1].Y, 2));
|
|
var DistanceToCheckingNode = Math.Sqrt(Math.Pow(Visualization.X - GoalRotate.X, 2) + Math.Pow(Visualization.Y - GoalRotate.Y, 2));
|
|
var deviation = GoalRotate.NodeId == MovePurePursuit.OrderNodes[^1].NodeId ? 0.02 : 0.05;
|
|
if (DistanceToCheckingNode > deviation)
|
|
{
|
|
double SpeedTarget = MovePID.PID_step(DistanceToCheckingNode, MaxVelocity, 0, CycleHandlerMilliseconds / 1000.0);
|
|
double AngularVel = MovePurePursuit.PurePursuit_step(Visualization.X, Visualization.Y, Visualization.Theta * Math.PI / 180);
|
|
AngularVel *= MovePurePursuit.Waypoints_Value[MovePurePursuit.OnNodeIndex].Direction == RobotDirection.FORWARD ? 1 : -1;
|
|
(double AngularVelocityLeft, double AngularVelocityRight) = MoveFuzzy.Fuzzy_step(SpeedTarget, AngularVel, CycleHandlerMilliseconds / 1000.0);
|
|
|
|
if (MovePurePursuit.Waypoints_Value[MovePurePursuit.OnNodeIndex].Direction == RobotDirection.FORWARD)
|
|
{
|
|
AngularVelocityLeft /= PhysicalCog.WheelRadius;
|
|
AngularVelocityRight = AngularVelocityRight / PhysicalCog.WheelRadius * -1;
|
|
}
|
|
else
|
|
{
|
|
AngularVelocityLeft = AngularVelocityLeft / PhysicalCog.WheelRadius * -1;
|
|
AngularVelocityRight /= PhysicalCog.WheelRadius;
|
|
}
|
|
VelocityController.SetSpeed(AngularVelocityLeft, AngularVelocityRight, CycleHandlerMilliseconds / 1000.0);
|
|
}
|
|
else if (DistanceToGoal < 0.02)
|
|
{
|
|
if (MovePurePursuit.OrderNodes[^1].Theta is { } theta)
|
|
{
|
|
TargetAngle = theta * 180 / Math.PI;
|
|
NavState = NavigationState.Rotating;
|
|
MovePurePursuit = null;
|
|
}
|
|
else
|
|
{
|
|
NavState = NavigationState.Completed;
|
|
Dispose();
|
|
}
|
|
}
|
|
else
|
|
{
|
|
double? targetAngle = null;
|
|
if (GoalRotate.Theta is { } theta)
|
|
{
|
|
targetAngle = theta;
|
|
ProcessedRotations.Add(GoalRotate.NodeId);
|
|
BackToAngle = GoalRotate?.ContinueTheta * 180 / Math.PI;
|
|
IsBackToPath = true;
|
|
}
|
|
|
|
var newGoalRotate = FindNextRotateGoal();
|
|
if (newGoalRotate is not null && newGoalRotate.NodeId != GoalRotate?.NodeId)
|
|
{
|
|
GoalRotate = newGoalRotate;
|
|
UpdateGoal(newGoalRotate.NodeId);
|
|
}
|
|
|
|
if(targetAngle.HasValue)
|
|
{
|
|
TargetAngle = targetAngle.Value * 180 / Math.PI;
|
|
NavState = NavigationState.Rotating;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else if (NavState == NavigationState.Paused) VelocityController.Stop();
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Logger.Error($"Error in DifferentialNavigation: {ex.Message}");
|
|
}
|
|
}
|
|
}
|