QUYVN/I150
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-03 16:37:12 +07:00
commit 63b8c1ea8b
1931 changed files with 640587 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

128
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Simulation/Navigation/DifferentialNavigation.cs Normal file
View File

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

23
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Simulation/Navigation/ForkliftNavigation.cs Normal file
View File

@@ -0,0 +1,23 @@
namespace RobotNet10.RobotApp.Services.Simulation.Navigation;
public class ForkliftNavigation : SimulationNavigation
{
private readonly Logger<ForkliftNavigation> Logger;
public ForkliftNavigation(IServiceProvider ServiceProvider) : base(ServiceProvider)
{
using var scope = ServiceProvider.CreateScope();
Logger = scope.ServiceProvider.GetRequiredService<Logger<ForkliftNavigation>>();
}
protected override void NavigationHandler()
{
try
{
// Implement differential drive navigation logic here
}
catch (Exception ex)
{
Logger.Write($"Error in ForkliftNavigationSevice: {ex.Message}", LogLevel.Error);
}
}
}