Initial commit
This commit is contained in:
@@ -0,0 +1,198 @@
|
||||
using Microsoft.EntityFrameworkCore;
|
||||
using RobotNet10.NavigationTune.Data;
|
||||
using RobotNet10.NavigationTune.Shared.Interfaces;
|
||||
using RobotNet10.NavigationTune.Shared.Models;
|
||||
|
||||
namespace RobotNet10.NavigationTune.Services;
|
||||
|
||||
/// <summary>
|
||||
/// Parameter manager implementation
|
||||
/// </summary>
|
||||
public class ParameterManager(TuningDbContext context) : IParameterManager
|
||||
{
|
||||
private readonly TuningDbContext _context = context;
|
||||
|
||||
public async Task<NavigationParameterSet?> GetByNameAsync(string name)
|
||||
{
|
||||
return await _context.ParameterSets
|
||||
.FirstOrDefaultAsync(p => p.Name == name);
|
||||
}
|
||||
|
||||
public async Task<NavigationParameterSet?> GetByIdAsync(Guid id)
|
||||
{
|
||||
return await _context.ParameterSets.FindAsync(id);
|
||||
}
|
||||
|
||||
public async Task<List<NavigationParameterSet>> GetAllAsync()
|
||||
{
|
||||
return await _context.ParameterSets
|
||||
.OrderByDescending(p => p.CreatedAt)
|
||||
.ToListAsync();
|
||||
}
|
||||
|
||||
public async Task<Guid> SaveAsync(NavigationParameterSet parameterSet)
|
||||
{
|
||||
if (parameterSet.Id == Guid.Empty)
|
||||
parameterSet.Id = Guid.NewGuid();
|
||||
|
||||
parameterSet.CreatedAt = DateTime.UtcNow;
|
||||
_context.ParameterSets.Add(parameterSet);
|
||||
await _context.SaveChangesAsync();
|
||||
return parameterSet.Id;
|
||||
}
|
||||
|
||||
public async Task UpdateAsync(NavigationParameterSet parameterSet)
|
||||
{
|
||||
parameterSet.UpdatedAt = DateTime.UtcNow;
|
||||
_context.ParameterSets.Update(parameterSet);
|
||||
await _context.SaveChangesAsync();
|
||||
}
|
||||
|
||||
public async Task DeleteAsync(Guid id)
|
||||
{
|
||||
var parameterSet = await GetByIdAsync(id);
|
||||
if (parameterSet != null)
|
||||
{
|
||||
_context.ParameterSets.Remove(parameterSet);
|
||||
await _context.SaveChangesAsync();
|
||||
}
|
||||
}
|
||||
|
||||
public ValidationResult Validate(NavigationParameterSet parameterSet)
|
||||
{
|
||||
var result = new ValidationResult { IsValid = true };
|
||||
|
||||
// Validate PID bounds
|
||||
if (parameterSet.MovePidConfig.Kp < 0.1 || parameterSet.MovePidConfig.Kp > 5.0)
|
||||
result.AddError($"Move PID Kp must be between 0.1 and 5.0, got {parameterSet.MovePidConfig.Kp}");
|
||||
|
||||
if (parameterSet.MovePidConfig.Ki < 0 || parameterSet.MovePidConfig.Ki > 2.0)
|
||||
result.AddError($"Move PID Ki must be between 0 and 2.0, got {parameterSet.MovePidConfig.Ki}");
|
||||
|
||||
if (parameterSet.MovePidConfig.Kd < 0 || parameterSet.MovePidConfig.Kd > 1.0)
|
||||
result.AddError($"Move PID Kd must be between 0 and 1.0, got {parameterSet.MovePidConfig.Kd}");
|
||||
|
||||
// Validate Pure Pursuit
|
||||
if (parameterSet.PurePursuitConfig.LookaheadMax <= parameterSet.PurePursuitConfig.LookaheadMin)
|
||||
result.AddError("LookaheadMax must be greater than LookaheadMin");
|
||||
|
||||
if (parameterSet.PurePursuitConfig.Kdd < 0.3 || parameterSet.PurePursuitConfig.Kdd > 2.0)
|
||||
result.AddError($"Kdd must be between 0.3 and 2.0, got {parameterSet.PurePursuitConfig.Kdd}");
|
||||
|
||||
// Validate velocity limits
|
||||
if (parameterSet.NavigationConfig.MaxLinearVelocity > 2.0)
|
||||
result.AddWarning("MaxLinearVelocity > 2.0 m/s may be unsafe");
|
||||
|
||||
// Validate blend ratios
|
||||
if (parameterSet.EstimatorConfig.GoodTrackingBlend < parameterSet.EstimatorConfig.PoorTrackingBlend)
|
||||
result.AddError("GoodTrackingBlend must be greater than PoorTrackingBlend (trust encoder more when tracking is good)");
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
public NavigationParameterSet GetDefaultPreset()
|
||||
{
|
||||
return new NavigationParameterSet
|
||||
{
|
||||
Name = "Default",
|
||||
Description = "Default parameter set",
|
||||
IsDefault = true,
|
||||
ControllerType = PathFollowingController.PurePursuit, // Default to Pure Pursuit
|
||||
MovePidConfig = new PIDConfig { Kp = 1.0, Ki = 0.0001, Kd = 0.6 },
|
||||
RotatePidConfig = new PIDConfig { Kp = 10.0, Ki = 0.01, Kd = 0.1 },
|
||||
PurePursuitConfig = new PurePursuitConfig
|
||||
{
|
||||
LookaheadMin = 0.3,
|
||||
Kdd = 1.0,
|
||||
LookaheadMax = 2.0,
|
||||
MaxAngularVelocity = 1.5,
|
||||
ResolutionSplit = 0.05f,
|
||||
FinalApproachThreshold = 0.2,
|
||||
HeadingTolerance = 3.0,
|
||||
GoalRegionDistance = 1.5,
|
||||
KCurvature = 2.0,
|
||||
MinLookaheadTimeRatio = 0.3,
|
||||
MaxLookaheadTimeRatio = 2.0
|
||||
},
|
||||
StanleyConfig = new StanleyConfig
|
||||
{
|
||||
K = 2.5,
|
||||
Ks = 0.1,
|
||||
WheelBase = 0.5,
|
||||
MaxSteeringAngle = 0.5,
|
||||
EnableCurvatureFeedforward = true,
|
||||
KCurvatureFF = 1.0,
|
||||
GoalTolerance = 0.05,
|
||||
HeadingTolerance = 5.0,
|
||||
ResolutionSplit = 0.05,
|
||||
GoalApproachDistance = 1.0,
|
||||
GoalGainMultiplier = 2.0,
|
||||
LowSpeedThreshold = 0.3,
|
||||
LowSpeedAngularGain = 1.5
|
||||
},
|
||||
EstimatorConfig = new VelocityEstimatorConfig(),
|
||||
SignalConfig = new VelocitySignalProcessingConfig(),
|
||||
MotorDynamicsConfig = new MotorDynamicsConfig { Tau = 0.3, Delta = 0.05f },
|
||||
NavigationConfig = new NavigationConfig
|
||||
{
|
||||
MaxLinearVelocity = 1.5,
|
||||
MaxAngularVelocity = 6.0,
|
||||
MinLinearVelocity = 0.1,
|
||||
RotateAngularVelocity = 1.0,
|
||||
ReachedRadius = 0.015,
|
||||
InitialRotationThreshold = 5.0,
|
||||
Acceleration = 0.5,
|
||||
Deceleration = 0.5
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
public NavigationParameterSet GetAggressivePreset()
|
||||
{
|
||||
var preset = GetDefaultPreset();
|
||||
preset.Name = "Aggressive";
|
||||
preset.Description = "Aggressive tuning for fast response";
|
||||
preset.MovePidConfig.Kp = 1.5;
|
||||
preset.MovePidConfig.Ki = 0.2;
|
||||
preset.MovePidConfig.Kd = 0.02;
|
||||
preset.PurePursuitConfig.Kdd = 0.8f;
|
||||
return preset;
|
||||
}
|
||||
|
||||
public NavigationParameterSet GetSmoothPreset()
|
||||
{
|
||||
var preset = GetDefaultPreset();
|
||||
preset.Name = "Smooth";
|
||||
preset.Description = "Smooth tuning for gentle motion";
|
||||
preset.MovePidConfig.Kp = 0.6;
|
||||
preset.MovePidConfig.Ki = 0.05;
|
||||
preset.MovePidConfig.Kd = 0.3;
|
||||
preset.PurePursuitConfig.Kdd = 1.5;
|
||||
preset.SignalConfig.AlphaFilter = 0.2;
|
||||
return preset;
|
||||
}
|
||||
|
||||
public NavigationParameterSet GetStanleyPreset()
|
||||
{
|
||||
var preset = GetDefaultPreset();
|
||||
preset.Name = "Stanley";
|
||||
preset.Description = "Stanley controller for high-speed path tracking";
|
||||
preset.ControllerType = PathFollowingController.Stanley;
|
||||
|
||||
// Stanley-specific tuning
|
||||
preset.StanleyConfig.K = 2.5;
|
||||
preset.StanleyConfig.Ks = 0.1;
|
||||
preset.StanleyConfig.WheelBase = 0.6;
|
||||
preset.StanleyConfig.MaxSteeringAngle = 0.5;
|
||||
preset.StanleyConfig.EnableCurvatureFeedforward = true;
|
||||
preset.StanleyConfig.KCurvatureFF = 1.0;
|
||||
preset.StanleyConfig.GoalTolerance = 0.05;
|
||||
preset.StanleyConfig.HeadingTolerance = 5.0;
|
||||
preset.StanleyConfig.GoalApproachDistance = 1.0;
|
||||
preset.StanleyConfig.GoalGainMultiplier = 2.0;
|
||||
preset.StanleyConfig.LowSpeedThreshold = 0.3;
|
||||
preset.StanleyConfig.LowSpeedAngularGain = 1.5;
|
||||
|
||||
return preset;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user