27 KiB
27 KiB
LAYERS 4-5: ROBOT CONTROL & HARDWARE ABSTRACTION
Document: Part 3 of Robot Tuning System Architecture
Layers Covered: Robot Control (Layer 4) and Hardware Abstraction (Layer 5)
LAYER 4: ROBOT CONTROL
This layer contains the actual control algorithms that drive the robot. All controllers implement tunable interfaces to support dynamic parameter updates.
1. PID Controller
File: Infrastructure/Controllers/PIDController.cs
Interface Definition
public interface IPIDController : ITunableController
{
float Calculate(float error, float dt);
void Reset();
PIDState GetState();
}
public class PIDState
{
public float ProportionalTerm { get; set; }
public float IntegralTerm { get; set; }
public float DerivativeTerm { get; set; }
public float Output { get; set; }
public float Error { get; set; }
public float PreviousError { get; set; }
}
Implementation
public class PIDController : IPIDController
{
private VelocityPIDConfig _config;
private float _integral;
private float _previousError;
private bool _firstRun = true;
public PIDController(VelocityPIDConfig config)
{
_config = config;
}
public float Calculate(float error, float dt)
{
// Proportional term
var pTerm = _config.Kp * error;
// Integral term with anti-windup
_integral += error * dt;
_integral = Math.Clamp(
_integral,
-_config.IntegralWindupLimit,
_config.IntegralWindupLimit
);
var iTerm = _config.Ki * _integral;
// Derivative term with filtering
float dTerm = 0;
if (!_firstRun)
{
var derivative = (error - _previousError) / dt;
dTerm = _config.Kd * derivative;
}
_firstRun = false;
_previousError = error;
// Combined output
var output = pTerm + iTerm + dTerm;
// Apply saturation if enabled
if (_config.OutputSaturationEnabled)
{
output = Math.Clamp(
output,
_config.MinVelocity,
_config.MaxVelocity
);
}
return output;
}
public void Reset()
{
_integral = 0;
_previousError = 0;
_firstRun = true;
}
public void UpdateParameters(ParameterSet parameters)
{
_config = parameters.PID;
// Optionally reset integral term when parameters change
_integral = 0;
}
public ParameterSet GetCurrentParameters()
{
return new ParameterSet { PID = _config };
}
public PIDState GetState()
{
return new PIDState
{
ProportionalTerm = _config.Kp * _previousError,
IntegralTerm = _config.Ki * _integral,
DerivativeTerm = 0, // Would need to track
Output = Calculate(_previousError, 0),
Error = _previousError,
PreviousError = _previousError
};
}
public TelemetryData GetTelemetry()
{
var state = GetState();
return new TelemetryData
{
ControllerType = "PID",
Data = new Dictionary<string, float>
{
["Error"] = state.Error,
["P_Term"] = state.ProportionalTerm,
["I_Term"] = state.IntegralTerm,
["D_Term"] = state.DerivativeTerm,
["Output"] = state.Output,
["Integral_Accumulator"] = _integral
}
};
}
}
2. Velocity Estimator
File: Infrastructure/Controllers/VelocityEstimator.cs
Interface Definition
public interface IVelocityEstimator : ITunableController
{
float EstimateVelocity(float vCmd, float vActual, float dt);
float GetConfidence();
void Reset();
EstimatorState GetState();
}
public class EstimatorState
{
public float ModelVelocity { get; set; }
public float EncoderVelocity { get; set; }
public float FilteredEncoderVelocity { get; set; }
public float HybridVelocity { get; set; }
public float BlendRatio { get; set; }
public float Confidence { get; set; }
public float TrackingError { get; set; }
}
Implementation (Based on User's Formula)
public class VelocityEstimator : IVelocityEstimator
{
private VelocityEstimatorConfig _config;
private VelocitySignalProcessingConfig _signalConfig;
// State variables
private float _filteredEncoderVel;
private float _confidence = 1.0f;
private float _blendRatio;
// Model parameters (from user's code)
private const float Tau = 0.3f; // Time constant for first-order model
private const float Delta = 0.05f; // System delay
public VelocityEstimator(
VelocityEstimatorConfig config,
VelocitySignalProcessingConfig signalConfig)
{
_config = config;
_signalConfig = signalConfig;
_blendRatio = config.DefaultBlendRatio;
}
public float EstimateVelocity(float vCmd, float vActual, float dt)
{
// 1. Filter encoder velocity (exponential moving average)
_filteredEncoderVel = _signalConfig.AlphaFilter * vActual +
(1 - _signalConfig.AlphaFilter) * _filteredEncoderVel;
// 2. Predict velocity using model (user's formula)
var timeAhead = dt;
float vModel;
if (timeAhead < Delta)
{
vModel = vActual;
}
else
{
var effectiveTime = timeAhead - Delta;
var response = 1.0f - MathF.Exp(-effectiveTime / Tau);
vModel = vActual + (vCmd - vActual) * response;
}
// 3. Calculate tracking error
var trackingError = Math.Abs(vModel - vActual) /
(Math.Abs(vActual) + 0.01f); // Avoid division by zero
// 4. Adaptive blending based on tracking quality
_blendRatio = CalculateBlendRatio(trackingError);
// 5. Update confidence
UpdateConfidence(trackingError);
// 6. Hybrid estimation
var vHybrid = _blendRatio * vModel + (1 - _blendRatio) * _filteredEncoderVel;
return vHybrid;
}
private float CalculateBlendRatio(float trackingError)
{
if (trackingError < _config.GoodTrackingThreshold)
{
// Good tracking → trust encoder more
return _config.GoodTrackingBlend;
}
else if (trackingError < _config.ModerateTrackingThreshold)
{
// Moderate tracking → balanced
return _config.ModerateTrackingBlend;
}
else
{
// Poor tracking (possible wheel slip) → trust model more
return _config.PoorTrackingBlend;
}
}
private void UpdateConfidence(float trackingError)
{
if (trackingError < _config.GoodTrackingThreshold)
{
// Increase confidence (but cap at 1.0)
_confidence = Math.Min(1.0f, _confidence + 0.01f);
}
else
{
// Decay confidence
_confidence *= _config.ConfidenceDecayRate;
_confidence = Math.Max(_config.MinConfidence, _confidence);
}
}
public float GetConfidence()
{
return _confidence;
}
public void Reset()
{
_filteredEncoderVel = 0;
_confidence = 1.0f;
_blendRatio = _config.DefaultBlendRatio;
}
public void UpdateParameters(ParameterSet parameters)
{
_config = parameters.Estimator;
_signalConfig = parameters.SignalProcessing;
}
public EstimatorState GetState()
{
return new EstimatorState
{
EncoderVelocity = _filteredEncoderVel,
FilteredEncoderVelocity = _filteredEncoderVel,
BlendRatio = _blendRatio,
Confidence = _confidence
};
}
public TelemetryData GetTelemetry()
{
var state = GetState();
return new TelemetryData
{
ControllerType = "VelocityEstimator",
Data = new Dictionary<string, float>
{
["Encoder_Velocity"] = state.EncoderVelocity,
["Filtered_Encoder"] = state.FilteredEncoderVelocity,
["Hybrid_Velocity"] = state.HybridVelocity,
["Blend_Ratio"] = state.BlendRatio,
["Confidence"] = state.Confidence
}
};
}
}
3. Pure Pursuit Controller
File: Infrastructure/Controllers/PurePursuitController.cs
Interface Definition
public interface IPurePursuitController : ITunableController
{
float Calculate(
Pose2D robotPose,
float velocity,
float confidence,
Path referencePath
);
Vector2 GetTargetPoint();
float GetLookaheadDistance();
PurePursuitState GetState();
}
public class PurePursuitState
{
public Vector2 TargetPoint { get; set; }
public float LookaheadDistance { get; set; }
public float Curvature { get; set; }
public float AngularVelocity { get; set; }
}
Implementation (Based on User's Formula)
public class PurePursuitController : IPurePursuitController
{
private PurePursuitConfig _config;
private Vector2 _targetPoint;
private float _lookaheadDistance;
public PurePursuitController(PurePursuitConfig config)
{
_config = config;
}
public float Calculate(
Pose2D robotPose,
float velocity,
float confidence,
Path referencePath)
{
// 1. Calculate lookahead distance (user's formula)
_lookaheadDistance = _config.LookaheadMin + _config.Kdd * Math.Abs(velocity);
_lookaheadDistance = Math.Clamp(
_lookaheadDistance,
_config.LookaheadMin,
_config.LookaheadMax
);
// 2. Adjust lookahead based on confidence
_lookaheadDistance *= confidence;
_lookaheadDistance = Math.Clamp(
_lookaheadDistance,
_config.LookaheadMin * 0.5f,
_config.LookaheadMax
);
// 3. Find target point on path
_targetPoint = FindTargetPoint(robotPose.Position, referencePath);
// 4. Transform target to robot frame
var targetLocal = TransformToRobotFrame(robotPose, _targetPoint);
// 5. Calculate curvature (Pure Pursuit formula)
// κ = 2 * x / L² where x is lateral offset, L is lookahead
var curvature = 2.0f * targetLocal.Y /
(_lookaheadDistance * _lookaheadDistance);
// 6. Calculate angular velocity: ω = v * κ
var omega = velocity * curvature;
return omega;
}
private Vector2 FindTargetPoint(Vector2 robotPos, Path referencePath)
{
// Find point on path that is approximately lookahead distance ahead
var closestPoint = referencePath.GetClosestPoint(robotPos);
var distanceAlongPath = closestPoint.DistanceFromStart;
// Look ahead
var targetDistance = distanceAlongPath + _lookaheadDistance;
// Handle end of path
if (targetDistance >= referencePath.TotalLength)
{
return referencePath.Points.Last().Position;
}
var targetPathPoint = referencePath.GetPointAtDistance(targetDistance);
return targetPathPoint.Position;
}
private Vector2 TransformToRobotFrame(Pose2D robotPose, Vector2 worldPoint)
{
// Translate to robot origin
var translated = worldPoint - robotPose.Position;
// Rotate by -heading to align with robot frame
var cos = MathF.Cos(-robotPose.Heading);
var sin = MathF.Sin(-robotPose.Heading);
return new Vector2(
translated.X * cos - translated.Y * sin,
translated.X * sin + translated.Y * cos
);
}
public Vector2 GetTargetPoint() => _targetPoint;
public float GetLookaheadDistance() => _lookaheadDistance;
public void UpdateParameters(ParameterSet parameters)
{
_config = parameters.PurePursuit;
}
public PurePursuitState GetState()
{
return new PurePursuitState
{
TargetPoint = _targetPoint,
LookaheadDistance = _lookaheadDistance
};
}
public TelemetryData GetTelemetry()
{
var state = GetState();
return new TelemetryData
{
ControllerType = "PurePursuit",
Data = new Dictionary<string, float>
{
["Lookahead_Distance"] = state.LookaheadDistance,
["Target_X"] = state.TargetPoint.X,
["Target_Y"] = state.TargetPoint.Y,
["Curvature"] = state.Curvature,
["Angular_Velocity"] = state.AngularVelocity
}
};
}
}
4. Data Logger
File: Infrastructure/Logging/DataLogger.cs
Interface Definition
public interface IDataLogger
{
Task StartLoggingAsync(string testId);
void LogCycle(ControlCycleData data);
Task<string> StopLoggingAsync();
Task<List<ControlCycleData>> LoadLogAsync(string filePath);
}
Implementation with High-Frequency Logging
public class DataLogger : IDataLogger
{
private readonly string _logDirectory;
private BlockingCollection<ControlCycleData> _buffer;
private Task _writerTask;
private CancellationTokenSource _cts;
private string _currentLogFile;
public DataLogger(string logDirectory)
{
_logDirectory = logDirectory;
Directory.CreateDirectory(logDirectory);
}
public Task StartLoggingAsync(string testId)
{
_currentLogFile = Path.Combine(
_logDirectory,
$"test_{testId}_{DateTime.UtcNow:yyyyMMdd_HHmmss}.msgpack"
);
_buffer = new BlockingCollection<ControlCycleData>(
boundedCapacity: 10000 // Buffer up to 10k samples (200 seconds at 50Hz)
);
_cts = new CancellationTokenSource();
// Start async writer task
_writerTask = Task.Run(async () => await WriteLoopAsync(_cts.Token));
return Task.CompletedTask;
}
public void LogCycle(ControlCycleData data)
{
// Non-blocking add to buffer
if (!_buffer.TryAdd(data, millisecondsTimeout: 10))
{
// Buffer full - drop oldest data (or implement overflow strategy)
Console.WriteLine("WARNING: Data logger buffer overflow");
}
}
public async Task<string> StopLoggingAsync()
{
// Signal completion
_buffer.CompleteAdding();
// Wait for writer to flush all data
await _writerTask;
_cts.Dispose();
return _currentLogFile;
}
private async Task WriteLoopAsync(CancellationToken cancellationToken)
{
using var fileStream = File.OpenWrite(_currentLogFile);
// Write header
var header = new LogFileHeader
{
Version = 1,
Frequency = 50,
StartTime = DateTime.UtcNow
};
await MessagePackSerializer.SerializeAsync(fileStream, header);
// Write data as it arrives
foreach (var data in _buffer.GetConsumingEnumerable(cancellationToken))
{
await MessagePackSerializer.SerializeAsync(fileStream, data);
}
await fileStream.FlushAsync();
}
public async Task<List<ControlCycleData>> LoadLogAsync(string filePath)
{
var data = new List<ControlCycleData>();
using var fileStream = File.OpenRead(filePath);
// Read header
var header = await MessagePackSerializer.DeserializeAsync<LogFileHeader>(fileStream);
// Read all data
while (fileStream.Position < fileStream.Length)
{
var cycle = await MessagePackSerializer.DeserializeAsync<ControlCycleData>(fileStream);
data.Add(cycle);
}
return data;
}
}
5. Safety Monitor
File: Infrastructure/Safety/SafetyMonitor.cs
Interface Definition
public interface ISafetyMonitor
{
bool CheckSafety(RobotState state, Path referencePath);
List<SafetyViolation> GetViolations();
void Reset();
}
Implementation
public class SafetyMonitor : ISafetyMonitor
{
private readonly SafetyConfig _config;
private readonly List<SafetyViolation> _violations = new();
private DateTime? _trackingErrorStart;
public SafetyMonitor(SafetyConfig config)
{
_config = config;
}
public bool CheckSafety(RobotState state, Path referencePath)
{
var isSafe = true;
// 1. Check cross-track error
var closestPoint = referencePath.GetClosestPoint(state.Position);
var cte = Vector2.Distance(state.Position, closestPoint.Position);
if (cte > _config.MaxCrossTrackError)
{
LogViolation(new SafetyViolation
{
Type = ViolationType.CrossTrackError,
Severity = ViolationSeverity.Critical,
Value = cte,
Threshold = _config.MaxCrossTrackError,
Message = $"CTE {cte:F3}m exceeds limit {_config.MaxCrossTrackError:F3}m"
});
isSafe = false;
}
// 2. Check heading error
var pathHeading = closestPoint.Tangent.Angle();
var headingError = Math.Abs(NormalizeAngle(state.Heading - pathHeading));
if (headingError > _config.MaxHeadingError)
{
LogViolation(new SafetyViolation
{
Type = ViolationType.HeadingError,
Severity = ViolationSeverity.Critical,
Value = headingError,
Threshold = _config.MaxHeadingError,
Message = $"Heading error {headingError * 180 / MathF.PI:F1}° exceeds limit"
});
isSafe = false;
}
// 3. Check velocity limits
if (Math.Abs(state.LinearVelocity) > _config.MaxLinearVelocity * 1.1f)
{
LogViolation(new SafetyViolation
{
Type = ViolationType.VelocityLimit,
Severity = ViolationSeverity.Warning,
Value = state.LinearVelocity,
Threshold = _config.MaxLinearVelocity,
Message = $"Linear velocity {state.LinearVelocity:F2} m/s exceeds limit"
});
}
// 4. Check sustained tracking error
if (cte > _config.MaxCrossTrackError * 0.5f)
{
_trackingErrorStart ??= DateTime.UtcNow;
var duration = (DateTime.UtcNow - _trackingErrorStart.Value).TotalMilliseconds;
if (duration > _config.MaxTrackingErrorDuration)
{
LogViolation(new SafetyViolation
{
Type = ViolationType.SustainedTrackingError,
Severity = ViolationSeverity.Critical,
Value = (float)duration,
Threshold = _config.MaxTrackingErrorDuration,
Message = $"Tracking error sustained for {duration:F0}ms"
});
isSafe = false;
}
}
else
{
_trackingErrorStart = null;
}
return isSafe;
}
private void LogViolation(SafetyViolation violation)
{
violation.Timestamp = DateTime.UtcNow;
_violations.Add(violation);
}
public List<SafetyViolation> GetViolations() => _violations;
public void Reset()
{
_violations.Clear();
_trackingErrorStart = null;
}
private float NormalizeAngle(float angle)
{
while (angle > MathF.PI) angle -= 2 * MathF.PI;
while (angle < -MathF.PI) angle += 2 * MathF.PI;
return angle;
}
}
LAYER 5: HARDWARE ABSTRACTION
This layer provides interfaces to physical hardware. Implementations will vary based on actual robot hardware.
1. Motor Driver Interface
File: Infrastructure/Hardware/IMotorDriver.cs
public interface IMotorDriver
{
// Initialization
Task InitializeAsync();
Task ShutdownAsync();
// Commands
void SetVelocity(float leftWheelVelocity, float rightWheelVelocity);
void SetVelocityRampRate(float maxAcceleration);
void Stop();
void EmergencyStop();
// Status
MotorStatus GetStatus();
bool IsReady();
bool IsError();
string GetErrorMessage();
// Configuration
void SetMaxVelocity(float maxVel);
void SetAccelerationLimit(float maxAccel);
void EnableSoftStart(bool enable);
}
public class MotorStatus
{
public bool IsReady { get; set; }
public bool IsMoving { get; set; }
public bool IsError { get; set; }
public float LeftWheelActualVelocity { get; set; }
public float RightWheelActualVelocity { get; set; }
public float LeftWheelCurrent { get; set; }
public float RightWheelCurrent { get; set; }
public float BatteryVoltage { get; set; }
}
Example Implementation (Mock for Testing)
public class MockMotorDriver : IMotorDriver
{
private float _leftCmd, _rightCmd;
private float _leftActual, _rightActual;
private bool _isReady = true;
private float _maxAccel = 1.0f;
public Task InitializeAsync()
{
Console.WriteLine("MockMotorDriver: Initialized");
return Task.CompletedTask;
}
public void SetVelocity(float leftWheelVelocity, float rightWheelVelocity)
{
_leftCmd = leftWheelVelocity;
_rightCmd = rightWheelVelocity;
// Simulate first-order lag
_leftActual += (_leftCmd - _leftActual) * 0.3f;
_rightActual += (_rightCmd - _rightActual) * 0.3f;
}
public void Stop()
{
SetVelocity(0, 0);
}
public void EmergencyStop()
{
_leftCmd = _rightCmd = 0;
_leftActual = _rightActual = 0;
Console.WriteLine("MockMotorDriver: EMERGENCY STOP");
}
public MotorStatus GetStatus()
{
return new MotorStatus
{
IsReady = _isReady,
IsMoving = Math.Abs(_leftActual) > 0.01f || Math.Abs(_rightActual) > 0.01f,
LeftWheelActualVelocity = _leftActual,
RightWheelActualVelocity = _rightActual,
BatteryVoltage = 24.0f
};
}
public bool IsReady() => _isReady;
public bool IsError() => false;
public string GetErrorMessage() => "";
public void SetMaxVelocity(float maxVel) { }
public void SetAccelerationLimit(float maxAccel) => _maxAccel = maxAccel;
public void SetVelocityRampRate(float maxAcceleration) => _maxAccel = maxAcceleration;
public void EnableSoftStart(bool enable) { }
public Task ShutdownAsync() => Task.CompletedTask;
}
2. Encoder Reader Interface
File: Infrastructure/Hardware/IEncoderReader.cs
public interface IEncoderReader
{
// Initialization
Task InitializeAsync();
Task ShutdownAsync();
// Reading
EncoderData ReadEncoders();
(float left, float right) GetWheelVelocities();
(int left, int right) GetCounts();
// Configuration
void SetResolution(int pulsesPerRevolution);
void SetWheelRadius(float radius);
void ResetCounters();
// Calibration
Task CalibrateAsync();
EncoderCalibration GetCalibration();
}
[MessagePackObject]
public class EncoderData
{
[Key(0)]
public long TimestampMs { get; set; }
[Key(1)]
public int LeftCount { get; set; }
[Key(2)]
public int RightCount { get; set; }
[Key(3)]
public float LeftVelocity { get; set; } // m/s
[Key(4)]
public float RightVelocity { get; set; } // m/s
[Key(5)]
public float DeltaTime { get; set; } // seconds since last read
}
public class EncoderCalibration
{
public float LeftScale { get; set; } = 1.0f;
public float RightScale { get; set; } = 1.0f;
public float LeftOffset { get; set; } = 0.0f;
public float RightOffset { get; set; } = 0.0f;
}
3. Robot State Manager
File: Infrastructure/State/RobotStateManager.cs
public interface IRobotStateManager
{
// Odometry
Pose2D GetCurrentPose();
Twist2D GetCurrentTwist();
void ResetPose(Pose2D initialPose);
// Updates
void UpdateFromEncoders(EncoderData encoderData, float dt);
void UpdateFromIMU(IMUData imuData); // Optional
// Transforms
Vector2 RobotToWorld(Vector2 localPoint);
Vector2 WorldToRobot(Vector2 worldPoint);
float GetTotalDistance();
}
[MessagePackObject]
public struct Pose2D
{
[Key(0)]
public Vector2 Position { get; set; }
[Key(1)]
public float Heading { get; set; } // radians
public Pose2D(float x, float y, float heading)
{
Position = new Vector2(x, y);
Heading = heading;
}
}
[MessagePackObject]
public struct Twist2D
{
[Key(0)]
public float Linear { get; set; } // m/s
[Key(1)]
public float Angular { get; set; } // rad/s
}
Implementation
public class RobotStateManager : IRobotStateManager
{
private readonly RobotPhysicalConfig _config;
private Pose2D _pose;
private Twist2D _twist;
private float _totalDistance;
public RobotStateManager(RobotPhysicalConfig config)
{
_config = config;
_pose = new Pose2D(0, 0, 0);
}
public void UpdateFromEncoders(EncoderData encoderData, float dt)
{
// Differential drive kinematics
// v = (v_left + v_right) / 2
// ω = (v_right - v_left) / wheelbase
var vLeft = encoderData.LeftVelocity;