Initial commit
This commit is contained in:
@@ -0,0 +1,888 @@
|
||||
# CONFIGURATION, WORKFLOWS & IMPLEMENTATION GUIDE
|
||||
|
||||
**Document:** Part 5 of Robot Tuning System Architecture
|
||||
**Coverage:** Default configurations, tuning workflows, implementation phases, and testing strategy
|
||||
|
||||
---
|
||||
|
||||
## DEFAULT CONFIGURATIONS
|
||||
|
||||
All default values with detailed justifications.
|
||||
|
||||
---
|
||||
|
||||
### 1. Robot Physical Configuration
|
||||
|
||||
```csharp
|
||||
public static class DefaultConfigurations
|
||||
{
|
||||
public static RobotPhysicalConfig Physical => new()
|
||||
{
|
||||
// Wheelbase: Distance between left and right wheels
|
||||
// Typical for small indoor robot: 0.3-0.5m
|
||||
// Affects: Turning radius, stability
|
||||
Wheelbase = 0.35f, // meters
|
||||
|
||||
// Wheel radius: Affects odometry calculations
|
||||
// Typical: 0.05-0.1m for small robots
|
||||
WheelRadius = 0.075f, // meters
|
||||
|
||||
// Max linear velocity: User-specified
|
||||
MaxLinearVelocity = 1.5f, // m/s
|
||||
|
||||
// Max angular velocity: Calculated from max linear velocity
|
||||
// ω_max ≈ 2 * v_max / wheelbase
|
||||
// Conservative estimate: 2 * 1.5 / 0.35 ≈ 8.57 rad/s
|
||||
// Use 6 rad/s for safety margin (~343°/s)
|
||||
MaxAngularVelocity = 6.0f, // rad/s
|
||||
|
||||
// Max linear acceleration: For smooth motion
|
||||
// 1.0 m/s² means 0 → 1.5 m/s in 1.5 seconds
|
||||
// Typical range: 0.5-2.0 m/s²
|
||||
MaxLinearAcceleration = 1.0f, // m/s²
|
||||
|
||||
// Max angular acceleration: User-specified
|
||||
MaxAngularAcceleration = 1.0f, // rad/s²
|
||||
|
||||
// Robot mass: Estimated for small mobile robot
|
||||
Mass = 25.0f // kg
|
||||
};
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### 2. Control Timing Configuration
|
||||
|
||||
```csharp
|
||||
public static ControlTimingConfig Timing => new()
|
||||
{
|
||||
// Control loop frequency: 50Hz (20ms per cycle)
|
||||
// Justification:
|
||||
// - 10Hz: Too slow, robot will oscillate
|
||||
// - 50Hz: Optimal for indoor navigation (balance performance/CPU)
|
||||
// - 100Hz: Better but requires more CPU, marginal gains
|
||||
// - 200Hz+: Overkill for this application
|
||||
ControlLoopFrequency = 50, // Hz
|
||||
|
||||
// Encoder sampling: Match or exceed control frequency
|
||||
EncoderSamplingRate = 50, // Hz
|
||||
|
||||
// Motor command rate: Match control frequency
|
||||
MotorCommandRate = 50 // Hz
|
||||
};
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### 3. PID Controller Configuration
|
||||
|
||||
```csharp
|
||||
public static VelocityPIDConfig PID => new()
|
||||
{
|
||||
// Kp: Proportional gain
|
||||
// Higher Kp = faster response but more overshoot
|
||||
// Starting point: 0.8 (moderate response)
|
||||
// Tuning range: 0.1-5.0
|
||||
Kp = 0.8f,
|
||||
|
||||
// Ki: Integral gain
|
||||
// Eliminates steady-state error
|
||||
// Keep small to avoid windup
|
||||
// Starting point: 0.1
|
||||
// Tuning range: 0.0-2.0
|
||||
Ki = 0.1f,
|
||||
|
||||
// Kd: Derivative gain
|
||||
// Dampens oscillations, smooths response
|
||||
// Starting point: 0.05 (gentle damping)
|
||||
// Tuning range: 0.0-1.0
|
||||
Kd = 0.05f,
|
||||
|
||||
// Velocity limits
|
||||
MaxVelocity = 1.5f, // m/s (from physical config)
|
||||
MinVelocity = 0.1f, // m/s (minimum for robot to move)
|
||||
|
||||
// Anti-windup: Prevent integral term from growing unbounded
|
||||
IntegralWindupLimit = 0.5f, // m/s
|
||||
|
||||
// Output saturation: Ensure output stays within limits
|
||||
OutputSaturationEnabled = true
|
||||
};
|
||||
```
|
||||
|
||||
**PID Tuning Guidelines:**
|
||||
- Start with Kp only (Ki=0, Kd=0), increase until oscillation
|
||||
- Reduce Kp to 60% of oscillation value
|
||||
- Add Kd to dampen remaining oscillation
|
||||
- Add Ki last, only if steady-state error exists
|
||||
|
||||
---
|
||||
|
||||
### 4. Velocity Estimator Configuration
|
||||
|
||||
```csharp
|
||||
public static VelocityEstimatorConfig Estimator => new()
|
||||
{
|
||||
// Alpha filter: Exponential moving average for encoder
|
||||
// Lower α (0.1-0.2): More filtering, more lag
|
||||
// Higher α (0.3-0.4): Less filtering, more responsive
|
||||
// Recommended: 0.3 for balance
|
||||
AlphaFilter = 0.3f,
|
||||
|
||||
// Blend ratio bounds
|
||||
MinBlendRatio = 0.15f, // Min 15% model, 85% encoder
|
||||
MaxBlendRatio = 0.8f, // Max 80% model, 20% encoder
|
||||
DefaultBlendRatio = 0.6f, // Start with 60% model
|
||||
|
||||
// Adaptive blending thresholds
|
||||
GoodTrackingThreshold = 0.12f, // < 12% error
|
||||
ModerateTrackingThreshold = 0.3f, // < 30% error
|
||||
|
||||
// CORRECTED blend ratios (based on architecture review):
|
||||
// Good tracking → trust encoder more (model prediction matches reality)
|
||||
GoodTrackingBlend = 0.3f, // 30% model, 70% encoder
|
||||
|
||||
// Moderate tracking → balanced
|
||||
ModerateTrackingBlend = 0.5f, // 50-50 blend
|
||||
|
||||
// Poor tracking → trust model more (encoder may have slip)
|
||||
PoorTrackingBlend = 0.7f, // 70% model, 30% encoder
|
||||
|
||||
// Confidence decay: How fast confidence drops
|
||||
// 0.98 = slow decay (1% per cycle at 50Hz = ~2s to halve)
|
||||
// 0.95 = medium decay
|
||||
// 0.90 = fast decay
|
||||
ConfidenceDecayRate = 0.98f,
|
||||
|
||||
// Minimum confidence floor
|
||||
MinConfidence = 0.3f // Never go below 30%
|
||||
};
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### 5. Pure Pursuit Configuration
|
||||
|
||||
```csharp
|
||||
public static PurePursuitConfig PurePursuit => new()
|
||||
{
|
||||
// Lookahead minimum: Smallest lookahead distance
|
||||
// Too small: Oscillation, overshoot corners
|
||||
// Too large: Cuts corners, poor tracking
|
||||
// Recommended: 0.2-0.4m for indoor robot
|
||||
LookaheadMin = 0.3f, // meters
|
||||
|
||||
// Kdd: Lookahead velocity scaling factor
|
||||
// lookahead = LookaheadMin + Kdd * |velocity|
|
||||
// Kdd = 1.0 means 1 second lookahead time
|
||||
// Kdd = 0.5 means 0.5 second lookahead
|
||||
// Recommended: 0.8-1.5
|
||||
Kdd = 1.0f, // seconds
|
||||
|
||||
// Lookahead maximum: Cap lookahead distance
|
||||
// Prevents looking too far ahead at high speeds
|
||||
// Recommended: 1.5-3.0m
|
||||
LookaheadMax = 2.0f // meters
|
||||
};
|
||||
```
|
||||
|
||||
**Pure Pursuit Tuning Guidelines:**
|
||||
- Increase Kdd for smoother, more predictive tracking
|
||||
- Decrease Kdd for tighter, more reactive tracking
|
||||
- Increase LookaheadMin if robot oscillates
|
||||
- Decrease LookaheadMin if robot cuts corners
|
||||
|
||||
---
|
||||
|
||||
### 6. Path Following Configuration
|
||||
|
||||
```csharp
|
||||
public static PathFollowingConfig PathFollowing => new()
|
||||
{
|
||||
// Waypoint tolerance: How close to consider waypoint "reached"
|
||||
WaypointTolerance = 0.15f, // 15cm
|
||||
|
||||
// Final goal tolerance: Tighter tolerance for final goal
|
||||
FinalGoalTolerance = 0.05f, // 5cm
|
||||
|
||||
// Goal heading tolerance: Acceptable heading error at goal
|
||||
GoalHeadingTolerance = 5f * MathF.PI / 180f, // 5 degrees
|
||||
|
||||
// Stop distance: When to start preparing to stop
|
||||
StopDistance = 0.1f, // 10cm before goal
|
||||
|
||||
// Stop velocity: Threshold to consider robot "stopped"
|
||||
StopVelocity = 0.05f // 5cm/s
|
||||
};
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### 7. Safety Configuration
|
||||
|
||||
```csharp
|
||||
public static SafetyConfig Safety => new()
|
||||
{
|
||||
// Max cross-track error before abort
|
||||
// 0.5m is reasonable for 10x20m indoor space
|
||||
MaxCrossTrackError = 0.5f, // meters
|
||||
|
||||
// Max heading error before abort
|
||||
// 45° means robot is severely off course
|
||||
MaxHeadingError = 45f * MathF.PI / 180f, // radians
|
||||
|
||||
// Sustained error duration before abort
|
||||
// 3 seconds allows recovery from temporary issues
|
||||
MaxTrackingErrorDuration = 3000, // milliseconds
|
||||
|
||||
// Obstacle safety distances (for future sensors)
|
||||
MinObstacleDistance = 0.3f, // 30cm emergency stop
|
||||
SafetyStopDistance = 0.5f, // 50cm slow down
|
||||
|
||||
// Emergency deceleration limit
|
||||
EmergencyStopDeceleration = 2.0f // m/s²
|
||||
};
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### 8. Acceptance Criteria Configuration
|
||||
|
||||
```csharp
|
||||
public static AcceptanceCriteria Criteria => new()
|
||||
{
|
||||
// PRIMARY: Tracking Accuracy (50% weight)
|
||||
// Cross-track error RMS: 10cm is good for indoor robot
|
||||
MaxCrossTrackErrorRMS = 0.10f, // meters
|
||||
|
||||
// Peak CTE: Allow double RMS as occasional spike
|
||||
MaxCrossTrackErrorPeak = 0.20f, // meters
|
||||
|
||||
// Heading error: 10° RMS is acceptable
|
||||
MaxHeadingErrorRMS = 10f * MathF.PI / 180f, // radians
|
||||
|
||||
// Goal position error: 5cm final accuracy
|
||||
MaxGoalPositionError = 0.05f, // meters
|
||||
|
||||
// Goal heading error: 5° final accuracy
|
||||
MaxGoalHeadingError = 5f * MathF.PI / 180f, // radians
|
||||
|
||||
// SECONDARY: Smoothness (30% weight)
|
||||
// Max jerk: 5 m/s³ is smooth for human comfort
|
||||
MaxJerk = 5.0f, // m/s³
|
||||
|
||||
// Max angular jerk: 10 rad/s³
|
||||
MaxAngularJerk = 10.0f, // rad/s³
|
||||
|
||||
// TERTIARY: Efficiency (20% weight)
|
||||
// Path length ratio: <15% deviation from optimal
|
||||
MaxPathLengthRatio = 1.15f, // 115% of optimal
|
||||
|
||||
// Success rate: 90% of runs should pass
|
||||
MinSuccessRate = 0.90f // 90%
|
||||
};
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### 9. Scoring Weights
|
||||
|
||||
```csharp
|
||||
public static ScoringWeights Weights => new()
|
||||
{
|
||||
// How much each category contributes to overall score
|
||||
TrackingAccuracy = 0.5f, // 50%
|
||||
Smoothness = 0.3f, // 30%
|
||||
Efficiency = 0.2f // 20%
|
||||
};
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### 10. Parameter Bounds
|
||||
|
||||
```csharp
|
||||
public static class ParameterBounds
|
||||
{
|
||||
// PID bounds
|
||||
public static Range KpRange = new(0.1f, 5.0f);
|
||||
public static Range KiRange = new(0.0f, 2.0f);
|
||||
public static Range KdRange = new(0.0f, 1.0f);
|
||||
|
||||
// Estimator bounds
|
||||
public static Range AlphaFilterRange = new(0.05f, 0.5f);
|
||||
public static Range BlendRatioRange = new(0.1f, 0.9f);
|
||||
public static Range ConfidenceDecayRange = new(0.90f, 0.99f);
|
||||
|
||||
// Pure Pursuit bounds
|
||||
public static Range KddRange = new(0.3f, 2.0f);
|
||||
public static Range LookaheadMinRange = new(0.1f, 0.5f);
|
||||
public static Range LookaheadMaxRange = new(0.5f, 3.0f);
|
||||
|
||||
// Validation rules
|
||||
public static List<ValidationRule> Rules => new()
|
||||
{
|
||||
new ValidationRule
|
||||
{
|
||||
Name = "LookaheadOrdering",
|
||||
Check = (p) => p.PurePursuit.LookaheadMax > p.PurePursuit.LookaheadMin,
|
||||
Message = "LookaheadMax must be greater than LookaheadMin"
|
||||
},
|
||||
new ValidationRule
|
||||
{
|
||||
Name = "BlendRatioOrdering",
|
||||
Check = (p) => p.Estimator.GoodTrackingBlend <= p.Estimator.PoorTrackingBlend,
|
||||
Message = "GoodTrackingBlend should be less than PoorTrackingBlend"
|
||||
},
|
||||
new ValidationRule
|
||||
{
|
||||
Name = "VelocityLimit",
|
||||
Check = (p) => p.PID.MaxVelocity <= p.Physical.MaxLinearVelocity,
|
||||
Message = "PID MaxVelocity cannot exceed physical limit"
|
||||
}
|
||||
};
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## TUNING WORKFLOWS
|
||||
|
||||
Detailed step-by-step workflows for different tuning scenarios.
|
||||
|
||||
---
|
||||
|
||||
### Workflow 1: Quick Start (First-Time User)
|
||||
|
||||
**Goal:** Get robot moving with default settings and validate basic functionality.
|
||||
|
||||
**Steps:**
|
||||
|
||||
1. **Load Default Configuration** (5 min)
|
||||
- Open dashboard
|
||||
- Navigate to Configuration → Robot Settings
|
||||
- Verify physical parameters (wheelbase, wheel radius)
|
||||
- Click "Load Default Preset"
|
||||
|
||||
2. **Run Baseline Test** (2 min)
|
||||
- Select Test Scenario: "Straight Line 10m"
|
||||
- Click "Run Test"
|
||||
- Observe real-time visualization
|
||||
- Wait for completion
|
||||
|
||||
3. **Review Results** (3 min)
|
||||
- Check overall score
|
||||
- Identify which metrics fail (if any)
|
||||
- Note: CTE RMS, jerk, smoothness
|
||||
|
||||
4. **Decision Point:**
|
||||
- Score > 80: Proceed to Workflow 2 (test other trajectories)
|
||||
- Score 60-80: Proceed to Workflow 3 (manual tuning)
|
||||
- Score < 60: Check robot hardware, retry
|
||||
|
||||
**Expected Outcome:** Baseline performance established, ready for tuning.
|
||||
|
||||
---
|
||||
|
||||
### Workflow 2: Multi-Scenario Validation
|
||||
|
||||
**Goal:** Test current configuration across all trajectory types.
|
||||
|
||||
**Steps:**
|
||||
|
||||
1. **Setup Batch Test** (2 min)
|
||||
- Navigate to Tuning → Parameter Comparison
|
||||
- Select all scenarios:
|
||||
- Straight Line 10m
|
||||
- Circle 2m Radius
|
||||
- Circle 0.5m Radius
|
||||
- Select current parameter set
|
||||
- Click "Run Batch"
|
||||
|
||||
2. **Monitor Progress** (10-15 min)
|
||||
- Watch each test in sequence
|
||||
- Note any failures or safety violations
|
||||
|
||||
3. **Analyze Comparison** (5 min)
|
||||
- View comparison table
|
||||
- Identify weakest scenario
|
||||
- Check metric breakdown per scenario
|
||||
|
||||
4. **Decision Point:**
|
||||
- All scenarios pass: Configuration is robust
|
||||
- One scenario fails: Tune for that specific case
|
||||
- Multiple scenarios fail: Need general tuning (Workflow 3)
|
||||
|
||||
---
|
||||
|
||||
### Workflow 3: Manual Iterative Tuning
|
||||
|
||||
**Goal:** Hand-tune parameters to improve specific metrics.
|
||||
|
||||
**Steps:**
|
||||
|
||||
**Phase 1: Improve Tracking Accuracy (if CTE RMS > 0.10m)**
|
||||
|
||||
1. **Diagnose Issue:**
|
||||
- View trajectory plot
|
||||
- Check if robot overshoots or undershoots corners
|
||||
- Check if error is consistent or oscillating
|
||||
|
||||
2. **If Robot Overshoots (cuts corners):**
|
||||
- Decrease Pure Pursuit LookaheadMin: 0.3 → 0.25
|
||||
- Decrease Kdd: 1.0 → 0.8
|
||||
- Run test, check improvement
|
||||
|
||||
3. **If Robot Undershoots (goes wide):**
|
||||
- Increase Pure Pursuit LookaheadMin: 0.3 → 0.35
|
||||
- Increase Kdd: 1.0 → 1.2
|
||||
- Run test, check improvement
|
||||
|
||||
4. **If Robot Oscillates:**
|
||||
- Increase PID Kd: 0.05 → 0.15 (more damping)
|
||||
- Increase Estimator AlphaFilter: 0.3 → 0.4 (more smoothing)
|
||||
- Run test, check improvement
|
||||
|
||||
5. **If Robot is Sluggish:**
|
||||
- Increase PID Kp: 0.8 → 1.2 (faster response)
|
||||
- Run test, check for overshoot
|
||||
|
||||
**Phase 2: Improve Smoothness (if Jerk > 5.0 m/s³)**
|
||||
|
||||
1. **Increase Damping:**
|
||||
- Increase PID Kd: current → +0.1
|
||||
- Run test
|
||||
|
||||
2. **Smooth Velocity Estimates:**
|
||||
- Decrease Estimator AlphaFilter: 0.3 → 0.2
|
||||
- Run test
|
||||
|
||||
3. **Reduce Aggressiveness:**
|
||||
- Decrease PID Kp: current → -0.2
|
||||
- Run test
|
||||
|
||||
**Phase 3: Verify and Save**
|
||||
|
||||
1. **Run Full Validation:**
|
||||
- Test all scenarios with new parameters
|
||||
- Ensure no regressions
|
||||
|
||||
2. **Save Configuration:**
|
||||
- Name: "Tuned_[Date]_v1"
|
||||
- Add description of changes
|
||||
- Click "Save"
|
||||
|
||||
**Iteration:** Repeat phases as needed, aiming for <5 iterations.
|
||||
|
||||
---
|
||||
|
||||
### Workflow 4: Automated Optimization (Advanced)
|
||||
|
||||
**Goal:** Use algorithm to find optimal parameters automatically.
|
||||
|
||||
**Steps:**
|
||||
|
||||
1. **Configure Optimization** (5 min)
|
||||
- Navigate to Tuning → Auto Tuning
|
||||
- Select algorithm: Bayesian Optimization
|
||||
- Select parameters to tune:
|
||||
- ☑ PID: Kp, Ki, Kd
|
||||
- ☑ Pure Pursuit: Kdd
|
||||
- ☐ Estimator: (keep fixed for first run)
|
||||
- Set constraints:
|
||||
- Max iterations: 30
|
||||
- Early stopping: 1% improvement threshold
|
||||
|
||||
2. **Define Objective** (2 min)
|
||||
- Primary metric: Cross-Track Error RMS
|
||||
- Secondary metric: Max Jerk (weight: 0.3)
|
||||
- Test scenario: Circle 2m Radius
|
||||
|
||||
3. **Start Optimization** (30-60 min)
|
||||
- Click "Start Optimization"
|
||||
- Monitor progress dashboard
|
||||
- View live updates of best parameters found
|
||||
|
||||
4. **Review Results** (10 min)
|
||||
- Check final parameters
|
||||
- Compare to baseline
|
||||
- Review improvement %
|
||||
|
||||
5. **Validate on Other Scenarios** (15 min)
|
||||
- Run batch test with optimized parameters
|
||||
- Ensure no regressions on other trajectories
|
||||
|
||||
6. **Save or Iterate:**
|
||||
- If satisfied: Save as "Optimized_v1"
|
||||
- If not: Adjust weights, re-run optimization
|
||||
|
||||
---
|
||||
|
||||
### Workflow 5: A/B Testing Configurations
|
||||
|
||||
**Goal:** Compare two parameter sets side-by-side.
|
||||
|
||||
**Steps:**
|
||||
|
||||
1. **Select Configurations** (2 min)
|
||||
- Config A: "Default"
|
||||
- Config B: "Tuned_v1"
|
||||
|
||||
2. **Choose Test Scenario** (1 min)
|
||||
- Straight Line 10m
|
||||
|
||||
3. **Run Comparison** (5 min)
|
||||
- Click "Run Comparison"
|
||||
- System runs both tests sequentially
|
||||
|
||||
4. **Analyze Results** (5 min)
|
||||
- View side-by-side metrics table
|
||||
- Check trajectory overlay plot
|
||||
- Identify winner
|
||||
|
||||
5. **Statistical Significance** (optional)
|
||||
- Run 10 trials each
|
||||
- Compare mean ± std deviation
|
||||
- Determine if difference is significant
|
||||
|
||||
---
|
||||
|
||||
## IMPLEMENTATION PHASES
|
||||
|
||||
Phased approach to building the system, prioritized by value and complexity.
|
||||
|
||||
---
|
||||
|
||||
### Phase 0: MVP (Weeks 1-3)
|
||||
|
||||
**Goal:** Core functionality for manual tuning.
|
||||
|
||||
**Features:**
|
||||
- ✅ Single test execution
|
||||
- ✅ Manual parameter adjustment UI
|
||||
- ✅ Real-time visualization (2D trajectory)
|
||||
- ✅ Basic metrics calculation
|
||||
- ✅ Save/load configurations
|
||||
- ✅ Data logging
|
||||
|
||||
**Deliverables:**
|
||||
1. Working Blazor dashboard
|
||||
2. Integrated controllers (PID, Estimator, Pure Pursuit)
|
||||
3. Basic test executor
|
||||
4. SQLite database with core tables
|
||||
5. Real-time SignalR updates
|
||||
|
||||
**Tech Stack:**
|
||||
- Blazor Server
|
||||
- Entity Framework Core + SQLite
|
||||
- SignalR
|
||||
- Plotly.NET for charts
|
||||
|
||||
**Testing:**
|
||||
- Unit tests for controllers
|
||||
- Integration test for one full test run
|
||||
- Manual UI testing
|
||||
|
||||
**Success Criteria:**
|
||||
- User can run a test and see results
|
||||
- Parameters can be adjusted and re-run
|
||||
- Metrics are calculated correctly
|
||||
|
||||
---
|
||||
|
||||
### Phase 1: Enhanced Tuning (Weeks 4-6)
|
||||
|
||||
**Goal:** Multi-scenario testing and comparison.
|
||||
|
||||
**Features:**
|
||||
- ✅ Batch testing
|
||||
- ✅ Configuration comparison
|
||||
- ✅ Test history viewer
|
||||
- ✅ Safety monitoring with abort
|
||||
- ✅ Parameter validation
|
||||
- ✅ CSV export
|
||||
|
||||
**Deliverables:**
|
||||
1. Batch test executor
|
||||
2. Comparison UI components
|
||||
3. Enhanced database queries
|
||||
4. Safety monitor implementation
|
||||
5. Report generation (HTML/CSV)
|
||||
|
||||
**Testing:**
|
||||
- Batch test with 3 scenarios
|
||||
- Comparison test with 3 configs
|
||||
- Safety violation test
|
||||
|
||||
**Success Criteria:**
|
||||
- Batch tests complete without manual intervention
|
||||
- Comparison clearly shows best configuration
|
||||
- Safety system aborts on violations
|
||||
|
||||
---
|
||||
|
||||
### Phase 2: Advanced Analytics (Weeks 7-9) - OPTIONAL
|
||||
|
||||
**Goal:** Deep insights and semi-automated tuning.
|
||||
|
||||
**Features:**
|
||||
- ⭕ Statistical analysis
|
||||
- ⭕ Trend analysis over time
|
||||
- ⭕ Parameter sensitivity analysis
|
||||
- ⭕ Tuning suggestions
|
||||
- ⭕ PDF report generation
|
||||
|
||||
**Deliverables:**
|
||||
1. Statistics calculator
|
||||
2. Trend visualization
|
||||
3. Suggestion engine (rule-based)
|
||||
4. PDF generator
|
||||
|
||||
**Testing:**
|
||||
- Historical data analysis (50+ tests)
|
||||
- Suggestion accuracy validation
|
||||
|
||||
**Success Criteria:**
|
||||
- Trends clearly visible
|
||||
- Suggestions improve results
|
||||
|
||||
---
|
||||
|
||||
### Phase 3: Automated Optimization (Weeks 10-13) - FUTURE
|
||||
|
||||
**Goal:** Hands-off parameter optimization.
|
||||
|
||||
**Features:**
|
||||
- ❌ Grid search
|
||||
- ❌ Random search
|
||||
- ❌ Bayesian optimization
|
||||
- ❌ Genetic algorithm
|
||||
- ❌ Multi-objective optimization
|
||||
|
||||
**Deliverables:**
|
||||
1. Optimization framework
|
||||
2. Multiple algorithm implementations
|
||||
3. Hyperparameter tuning for optimizers
|
||||
4. Parallel evaluation (if multiple robots)
|
||||
|
||||
**Complexity:** Very High (requires ML libraries)
|
||||
|
||||
**Testing:**
|
||||
- Benchmark against manual tuning
|
||||
- Convergence tests
|
||||
- Robustness tests
|
||||
|
||||
**Success Criteria:**
|
||||
- Automated optimization finds better params than manual in <1 hour
|
||||
- Reproducible results
|
||||
|
||||
---
|
||||
|
||||
## TESTING STRATEGY
|
||||
|
||||
Comprehensive testing at all levels.
|
||||
|
||||
---
|
||||
|
||||
### 1. Unit Tests
|
||||
|
||||
**Coverage Target:** >80% for domain logic
|
||||
|
||||
**Key Tests:**
|
||||
|
||||
```csharp
|
||||
// PID Controller Tests
|
||||
[Fact]
|
||||
public void PIDController_ProportionalOnly_CorrectOutput()
|
||||
{
|
||||
var config = new VelocityPIDConfig { Kp = 1.0f, Ki = 0, Kd = 0 };
|
||||
var pid = new PIDController(config);
|
||||
|
||||
var output = pid.Calculate(error: 1.0f, dt: 0.02f);
|
||||
|
||||
Assert.Equal(1.0f, output, precision: 2);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void PIDController_IntegralWindup_Clamped()
|
||||
{
|
||||
var config = new VelocityPIDConfig
|
||||
{
|
||||
Kp = 0,
|
||||
Ki = 1.0f,
|
||||
Kd = 0,
|
||||
IntegralWindupLimit = 0.5f
|
||||
};
|
||||
var pid = new PIDController(config);
|
||||
|
||||
// Accumulate large error
|
||||
for (int i = 0; i < 100; i++)
|
||||
pid.Calculate(error: 1.0f, dt: 0.02f);
|
||||
|
||||
var output = pid.Calculate(error: 1.0f, dt: 0.02f);
|
||||
|
||||
Assert.True(output <= 0.5f);
|
||||
}
|
||||
|
||||
// Velocity Estimator Tests
|
||||
[Fact]
|
||||
public void VelocityEstimator_GoodTracking_TrustsEncoder()
|
||||
{
|
||||
var config = DefaultConfigurations.Estimator;
|
||||
var estimator = new VelocityEstimator(config, new VelocitySignalProcessingConfig());
|
||||
|
||||
var vHybrid = estimator.EstimateVelocity(
|
||||
vCmd: 1.0f,
|
||||
vActual: 0.95f, // Close to command (good tracking)
|
||||
dt: 0.02f
|
||||
);
|
||||
|
||||
// Should blend more toward encoder (0.95) than model
|
||||
Assert.True(vHybrid > 0.93f && vHybrid < 0.97f);
|
||||
}
|
||||
|
||||
// Pure Pursuit Tests
|
||||
[Fact]
|
||||
public void PurePursuit_LookaheadScalesWithVelocity()
|
||||
{
|
||||
var config = new PurePursuitConfig
|
||||
{
|
||||
LookaheadMin = 0.3f,
|
||||
Kdd = 1.0f,
|
||||
LookaheadMax = 2.0f
|
||||
};
|
||||
var pp = new PurePursuitController(config);
|
||||
|
||||
var path = CreateStraightLinePath(10);
|
||||
var pose = new Pose2D(0, 0, 0);
|
||||
|
||||
pp.Calculate(pose, velocity: 0.5f, confidence: 1.0f, path);
|
||||
var lookahead1 = pp.GetLookaheadDistance();
|
||||
|
||||
pp.Calculate(pose, velocity: 1.0f, confidence: 1.0f, path);
|
||||
var lookahead2 = pp.GetLookaheadDistance();
|
||||
|
||||
Assert.True(lookahead2 > lookahead1);
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### 2. Integration Tests
|
||||
|
||||
```csharp
|
||||
[Fact]
|
||||
public async Task FullTestRun_StraightLine_Completes()
|
||||
{
|
||||
// Arrange
|
||||
var scenario = CreateStraightLineScenario(10);
|
||||
var parameters = DefaultConfigurations.GetDefaultPreset();
|
||||
var orchestrator = CreateOrchestrator();
|
||||
|
||||
// Act
|
||||
var result = await orchestrator.RunSingleTest(scenario, parameters);
|
||||
|
||||
// Assert
|
||||
Assert.Equal(TestStatus.Completed, result.Status);
|
||||
Assert.NotNull(result.Metrics);
|
||||
Assert.True(result.Metrics.CrossTrackErrorRMS < 0.20f);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public async Task SafetyMonitor_ExcessiveCTE_AbortsTest()
|
||||
{
|
||||
// Arrange
|
||||
var scenario = CreateCircleScenario(0.5f); // Tight circle
|
||||
var parameters = CreateBadParameters(); // Intentionally bad
|
||||
var orchestrator = CreateOrchestrator();
|
||||
|
||||
// Act
|
||||
var result = await orchestrator.RunSingleTest(scenario, parameters);
|
||||
|
||||
// Assert
|
||||
Assert.Equal(TestStatus.Aborted, result.Status);
|
||||
Assert.True(result.SafetyViolations.Any(v => v.Type == ViolationType.CrossTrackError));
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### 3. Performance Tests
|
||||
|
||||
```csharp
|
||||
[Fact]
|
||||
public void ControlLoop_MaintainsFrequency()
|
||||
{
|
||||
var executor = CreateTestExecutor();
|
||||
var scenario = CreateStraightLineScenario(5);
|
||||
var parameters = DefaultConfigurations.GetDefaultPreset();
|
||||
|
||||
var timestamps = new List<long>();
|
||||
|
||||
executor.ExecuteAsync(
|
||||
onStateUpdate: state => timestamps.Add(state.TimestampMs)
|
||||
).Wait();
|
||||
|
||||
// Calculate actual frequency
|
||||
var intervals = timestamps.Zip(timestamps.Skip(1), (a, b) => b - a);
|
||||
var avgInterval = intervals.Average();
|
||||
var actualFrequency = 1000.0 / avgInterval;
|
||||
|
||||
Assert.InRange(actualFrequency, 48, 52); // 50Hz ± 2Hz
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### 4. End-to-End Tests
|
||||
|
||||
**Manual Test Plan:**
|
||||
|
||||
1. **Happy Path Test**
|
||||
- Load default config
|
||||
- Run straight line test
|
||||
- Verify metrics displayed
|
||||
- Save configuration
|
||||
- Reload and verify
|
||||
|
||||
2. **Error Handling Test**
|
||||
- Set invalid parameter (Kp = 100)
|
||||
- Attempt to run test
|
||||
- Verify error message shown
|
||||
- Verify test doesn't start
|
||||
|
||||
3. **Real-time Update Test**
|
||||
- Start test
|
||||
- Verify UI updates at ~10Hz
|
||||
- Pause test
|
||||
- Verify pause works
|
||||
- Resume and complete
|
||||
|
||||
4. **Comparison Test**
|
||||
- Create 2 configs
|
||||
- Run comparison
|
||||
- Verify side-by-side display
|
||||
- Export results
|
||||
|
||||
---
|
||||
|
||||
## DEPLOYMENT CHECKLIST
|
||||
|
||||
Pre-deployment validation:
|
||||
|
||||
- [ ] All unit tests pass
|
||||
- [ ] Integration tests pass
|
||||
- [ ] Performance tests pass
|
||||
- [ ] Manual E2E tests completed
|
||||
- [ ] Database migrations created
|
||||
- [ ] Default data seeded
|
||||
- [ ] Configuration files reviewed
|
||||
- [ ] Robot hardware tested
|
||||
- [ ] Emergency stop tested
|
||||
- [ ] Documentation complete
|
||||
- [ ] User manual created
|
||||
|
||||
---
|
||||
|
||||
This completes the Configuration, Workflows & Implementation Guide.
|
||||
@@ -0,0 +1,875 @@
|
||||
# DATABASE SCHEMA & API SPECIFICATIONS
|
||||
|
||||
**Document:** Part 4 of Robot Tuning System Architecture
|
||||
**Coverage:** Complete database design and REST API/SignalR specifications
|
||||
|
||||
---
|
||||
|
||||
## DATABASE SCHEMA
|
||||
|
||||
Using Entity Framework Core with SQLite for development, PostgreSQL for production.
|
||||
|
||||
---
|
||||
|
||||
### 1. Core Tables
|
||||
|
||||
#### 1.1. TestScenarios Table
|
||||
|
||||
```csharp
|
||||
[Table("test_scenarios")]
|
||||
public class TestScenario
|
||||
{
|
||||
[Key]
|
||||
public Guid Id { get; set; }
|
||||
|
||||
[Required]
|
||||
[MaxLength(200)]
|
||||
public string Name { get; set; }
|
||||
|
||||
[Required]
|
||||
public TrajectoryType Type { get; set; }
|
||||
|
||||
[Column(TypeName = "jsonb")] // PostgreSQL jsonb, TEXT for SQLite
|
||||
public string ConfigJson { get; set; }
|
||||
|
||||
public DateTime CreatedAt { get; set; }
|
||||
public DateTime? UpdatedAt { get; set; }
|
||||
|
||||
[MaxLength(500)]
|
||||
public string Description { get; set; }
|
||||
|
||||
public bool IsDefault { get; set; }
|
||||
public bool IsActive { get; set; }
|
||||
|
||||
// Navigation properties
|
||||
public virtual ICollection<TestRun> TestRuns { get; set; }
|
||||
}
|
||||
|
||||
public enum TrajectoryType
|
||||
{
|
||||
StraightLine = 1,
|
||||
Circle = 2,
|
||||
Square = 3,
|
||||
SCurve = 4,
|
||||
Custom = 99
|
||||
}
|
||||
```
|
||||
|
||||
**Indexes:**
|
||||
```sql
|
||||
CREATE INDEX idx_test_scenarios_type ON test_scenarios(Type);
|
||||
CREATE INDEX idx_test_scenarios_name ON test_scenarios(Name);
|
||||
CREATE INDEX idx_test_scenarios_created_at ON test_scenarios(CreatedAt);
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
#### 1.2. ParameterSets Table
|
||||
|
||||
```csharp
|
||||
[Table("parameter_sets")]
|
||||
public class ParameterSet
|
||||
{
|
||||
[Key]
|
||||
public Guid Id { get; set; }
|
||||
|
||||
[Required]
|
||||
[MaxLength(200)]
|
||||
public string Name { get; set; }
|
||||
|
||||
[Column(TypeName = "jsonb")]
|
||||
public string ConfigJson { get; set; }
|
||||
|
||||
public DateTime CreatedAt { get; set; }
|
||||
public DateTime? UpdatedAt { get; set; }
|
||||
|
||||
[MaxLength(500)]
|
||||
public string Description { get; set; }
|
||||
|
||||
public bool IsDefault { get; set; }
|
||||
public bool IsActive { get; set; }
|
||||
|
||||
// Version tracking
|
||||
public int Version { get; set; }
|
||||
public Guid? ParentId { get; set; }
|
||||
|
||||
// Navigation properties
|
||||
public virtual ICollection<TestRun> TestRuns { get; set; }
|
||||
public virtual ICollection<ParameterHistory> History { get; set; }
|
||||
}
|
||||
```
|
||||
|
||||
**Indexes:**
|
||||
```sql
|
||||
CREATE INDEX idx_parameter_sets_name ON parameter_sets(Name);
|
||||
CREATE INDEX idx_parameter_sets_parent_id ON parameter_sets(ParentId);
|
||||
CREATE INDEX idx_parameter_sets_version ON parameter_sets(Version);
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
#### 1.3. TestRuns Table
|
||||
|
||||
```csharp
|
||||
[Table("test_runs")]
|
||||
public class TestRun
|
||||
{
|
||||
[Key]
|
||||
public Guid Id { get; set; }
|
||||
|
||||
[Required]
|
||||
public Guid TestScenarioId { get; set; }
|
||||
|
||||
[Required]
|
||||
public Guid ParameterSetId { get; set; }
|
||||
|
||||
public DateTime StartTime { get; set; }
|
||||
public DateTime? EndTime { get; set; }
|
||||
|
||||
public TestStatus Status { get; set; }
|
||||
|
||||
[MaxLength(500)]
|
||||
public string RawDataPath { get; set; }
|
||||
|
||||
public float Duration { get; set; } // seconds
|
||||
|
||||
[MaxLength(1000)]
|
||||
public string Notes { get; set; }
|
||||
|
||||
[MaxLength(500)]
|
||||
public string ErrorMessage { get; set; }
|
||||
|
||||
// Foreign keys
|
||||
[ForeignKey(nameof(TestScenarioId))]
|
||||
public virtual TestScenario TestScenario { get; set; }
|
||||
|
||||
[ForeignKey(nameof(ParameterSetId))]
|
||||
public virtual ParameterSet ParameterSet { get; set; }
|
||||
|
||||
// Navigation properties
|
||||
public virtual TestMetrics Metrics { get; set; }
|
||||
public virtual ICollection<SafetyViolation> SafetyViolations { get; set; }
|
||||
}
|
||||
|
||||
public enum TestStatus
|
||||
{
|
||||
Preparing = 0,
|
||||
Running = 1,
|
||||
Paused = 2,
|
||||
Completed = 3,
|
||||
Aborted = 4,
|
||||
Error = 5,
|
||||
EmergencyStopped = 6
|
||||
}
|
||||
```
|
||||
|
||||
**Indexes:**
|
||||
```sql
|
||||
CREATE INDEX idx_test_runs_scenario_id ON test_runs(TestScenarioId);
|
||||
CREATE INDEX idx_test_runs_parameter_id ON test_runs(ParameterSetId);
|
||||
CREATE INDEX idx_test_runs_start_time ON test_runs(StartTime DESC);
|
||||
CREATE INDEX idx_test_runs_status ON test_runs(Status);
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
#### 1.4. TestMetrics Table
|
||||
|
||||
```csharp
|
||||
[Table("test_metrics")]
|
||||
public class TestMetrics
|
||||
{
|
||||
[Key]
|
||||
public Guid Id { get; set; }
|
||||
|
||||
[Required]
|
||||
public Guid TestRunId { get; set; }
|
||||
|
||||
// Tracking Accuracy
|
||||
[Column(TypeName = "decimal(8,6)")]
|
||||
public decimal CrossTrackErrorRMS { get; set; }
|
||||
|
||||
[Column(TypeName = "decimal(8,6)")]
|
||||
public decimal CrossTrackErrorPeak { get; set; }
|
||||
|
||||
[Column(TypeName = "decimal(8,6)")]
|
||||
public decimal CrossTrackErrorMean { get; set; }
|
||||
|
||||
[Column(TypeName = "decimal(8,6)")]
|
||||
public decimal CrossTrackErrorStdDev { get; set; }
|
||||
|
||||
[Column(TypeName = "decimal(8,6)")]
|
||||
public decimal HeadingErrorRMS { get; set; }
|
||||
|
||||
[Column(TypeName = "decimal(8,6)")]
|
||||
public decimal HeadingErrorPeak { get; set; }
|
||||
|
||||
[Column(TypeName = "decimal(8,6)")]
|
||||
public decimal GoalPositionError { get; set; }
|
||||
|
||||
// Smoothness
|
||||
[Column(TypeName = "decimal(8,4)")]
|
||||
public decimal MaxJerk { get; set; }
|
||||
|
||||
[Column(TypeName = "decimal(8,4)")]
|
||||
public decimal AverageJerk { get; set; }
|
||||
|
||||
[Column(TypeName = "decimal(8,4)")]
|
||||
public decimal MaxAngularJerk { get; set; }
|
||||
|
||||
[Column(TypeName = "decimal(8,4)")]
|
||||
public decimal VelocityStdDev { get; set; }
|
||||
|
||||
[Column(TypeName = "decimal(8,4)")]
|
||||
public decimal AccelerationStdDev { get; set; }
|
||||
|
||||
// Efficiency
|
||||
[Column(TypeName = "decimal(8,4)")]
|
||||
public decimal PathLengthRatio { get; set; }
|
||||
|
||||
[Column(TypeName = "decimal(8,2)")]
|
||||
public decimal CompletionTime { get; set; }
|
||||
|
||||
[Column(TypeName = "decimal(8,4)")]
|
||||
public decimal AverageSpeed { get; set; }
|
||||
|
||||
[Column(TypeName = "decimal(8,4)")]
|
||||
public decimal MaxSpeed { get; set; }
|
||||
|
||||
// Scores
|
||||
[Column(TypeName = "decimal(5,2)")]
|
||||
public decimal OverallScore { get; set; }
|
||||
|
||||
[Column(TypeName = "decimal(5,2)")]
|
||||
public decimal TrackingScore { get; set; }
|
||||
|
||||
[Column(TypeName = "decimal(5,2)")]
|
||||
public decimal SmoothnessScore { get; set; }
|
||||
|
||||
[Column(TypeName = "decimal(5,2)")]
|
||||
public decimal EfficiencyScore { get; set; }
|
||||
|
||||
public bool PassedCriteria { get; set; }
|
||||
|
||||
// Foreign key
|
||||
[ForeignKey(nameof(TestRunId))]
|
||||
public virtual TestRun TestRun { get; set; }
|
||||
}
|
||||
```
|
||||
|
||||
**Indexes:**
|
||||
```sql
|
||||
CREATE INDEX idx_test_metrics_test_run_id ON test_metrics(TestRunId);
|
||||
CREATE INDEX idx_test_metrics_overall_score ON test_metrics(OverallScore DESC);
|
||||
CREATE INDEX idx_test_metrics_passed ON test_metrics(PassedCriteria);
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
#### 1.5. SafetyViolations Table
|
||||
|
||||
```csharp
|
||||
[Table("safety_violations")]
|
||||
public class SafetyViolation
|
||||
{
|
||||
[Key]
|
||||
public Guid Id { get; set; }
|
||||
|
||||
[Required]
|
||||
public Guid TestRunId { get; set; }
|
||||
|
||||
public DateTime Timestamp { get; set; }
|
||||
|
||||
public ViolationType Type { get; set; }
|
||||
public ViolationSeverity Severity { get; set; }
|
||||
|
||||
[Column(TypeName = "decimal(10,4)")]
|
||||
public decimal Value { get; set; }
|
||||
|
||||
[Column(TypeName = "decimal(10,4)")]
|
||||
public decimal Threshold { get; set; }
|
||||
|
||||
[MaxLength(500)]
|
||||
public string Message { get; set; }
|
||||
|
||||
// Foreign key
|
||||
[ForeignKey(nameof(TestRunId))]
|
||||
public virtual TestRun TestRun { get; set; }
|
||||
}
|
||||
|
||||
public enum ViolationType
|
||||
{
|
||||
CrossTrackError = 1,
|
||||
HeadingError = 2,
|
||||
VelocityLimit = 3,
|
||||
AccelerationLimit = 4,
|
||||
SustainedTrackingError = 5,
|
||||
ObstacleProximity = 6
|
||||
}
|
||||
|
||||
public enum ViolationSeverity
|
||||
{
|
||||
Info = 0,
|
||||
Warning = 1,
|
||||
Critical = 2
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
#### 1.6. ParameterHistory Table
|
||||
|
||||
```csharp
|
||||
[Table("parameter_history")]
|
||||
public class ParameterHistory
|
||||
{
|
||||
[Key]
|
||||
public Guid Id { get; set; }
|
||||
|
||||
[Required]
|
||||
public Guid ParameterSetId { get; set; }
|
||||
|
||||
public DateTime ChangedAt { get; set; }
|
||||
|
||||
[MaxLength(200)]
|
||||
public string ChangedBy { get; set; }
|
||||
|
||||
[MaxLength(1000)]
|
||||
public string ChangeDescription { get; set; }
|
||||
|
||||
[Column(TypeName = "jsonb")]
|
||||
public string PreviousConfigJson { get; set; }
|
||||
|
||||
[Column(TypeName = "jsonb")]
|
||||
public string NewConfigJson { get; set; }
|
||||
|
||||
// Foreign key
|
||||
[ForeignKey(nameof(ParameterSetId))]
|
||||
public virtual ParameterSet ParameterSet { get; set; }
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### 2. Database Context
|
||||
|
||||
```csharp
|
||||
public class TuningDbContext : DbContext
|
||||
{
|
||||
public DbSet<TestScenario> TestScenarios { get; set; }
|
||||
public DbSet<ParameterSet> ParameterSets { get; set; }
|
||||
public DbSet<TestRun> TestRuns { get; set; }
|
||||
public DbSet<TestMetrics> TestMetrics { get; set; }
|
||||
public DbSet<SafetyViolation> SafetyViolations { get; set; }
|
||||
public DbSet<ParameterHistory> ParameterHistory { get; set; }
|
||||
|
||||
public TuningDbContext(DbContextOptions<TuningDbContext> options)
|
||||
: base(options)
|
||||
{
|
||||
}
|
||||
|
||||
protected override void OnModelCreating(ModelBuilder modelBuilder)
|
||||
{
|
||||
base.OnModelCreating(modelBuilder);
|
||||
|
||||
// Configure relationships
|
||||
modelBuilder.Entity<TestRun>()
|
||||
.HasOne(tr => tr.TestScenario)
|
||||
.WithMany(ts => ts.TestRuns)
|
||||
.HasForeignKey(tr => tr.TestScenarioId)
|
||||
.OnDelete(DeleteBehavior.Restrict);
|
||||
|
||||
modelBuilder.Entity<TestRun>()
|
||||
.HasOne(tr => tr.ParameterSet)
|
||||
.WithMany(ps => ps.TestRuns)
|
||||
.HasForeignKey(tr => tr.ParameterSetId)
|
||||
.OnDelete(DeleteBehavior.Restrict);
|
||||
|
||||
modelBuilder.Entity<TestRun>()
|
||||
.HasOne(tr => tr.Metrics)
|
||||
.WithOne(tm => tm.TestRun)
|
||||
.HasForeignKey<TestMetrics>(tm => tm.TestRunId)
|
||||
.OnDelete(DeleteBehavior.Cascade);
|
||||
|
||||
// Seed default data
|
||||
SeedDefaultData(modelBuilder);
|
||||
}
|
||||
|
||||
private void SeedDefaultData(ModelBuilder modelBuilder)
|
||||
{
|
||||
// Default scenarios
|
||||
var straightLineId = Guid.NewGuid();
|
||||
var circle2mId = Guid.NewGuid();
|
||||
var circle05mId = Guid.NewGuid();
|
||||
|
||||
modelBuilder.Entity<TestScenario>().HasData(
|
||||
new TestScenario
|
||||
{
|
||||
Id = straightLineId,
|
||||
Name = "Straight Line 10m",
|
||||
Type = TrajectoryType.StraightLine,
|
||||
ConfigJson = JsonSerializer.Serialize(new { Length = 10.0 }),
|
||||
CreatedAt = DateTime.UtcNow,
|
||||
IsDefault = true,
|
||||
IsActive = true
|
||||
},
|
||||
new TestScenario
|
||||
{
|
||||
Id = circle2mId,
|
||||
Name = "Circle 2m Radius",
|
||||
Type = TrajectoryType.Circle,
|
||||
ConfigJson = JsonSerializer.Serialize(new { Radius = 2.0 }),
|
||||
CreatedAt = DateTime.UtcNow,
|
||||
IsDefault = true,
|
||||
IsActive = true
|
||||
},
|
||||
new TestScenario
|
||||
{
|
||||
Id = circle05mId,
|
||||
Name = "Circle 0.5m Radius",
|
||||
Type = TrajectoryType.Circle,
|
||||
ConfigJson = JsonSerializer.Serialize(new { Radius = 0.5 }),
|
||||
CreatedAt = DateTime.UtcNow,
|
||||
IsDefault = true,
|
||||
IsActive = true
|
||||
}
|
||||
);
|
||||
|
||||
// Default parameter set
|
||||
var defaultParamsId = Guid.NewGuid();
|
||||
modelBuilder.Entity<ParameterSet>().HasData(
|
||||
new ParameterSet
|
||||
{
|
||||
Id = defaultParamsId,
|
||||
Name = "Default",
|
||||
ConfigJson = JsonSerializer.Serialize(DefaultConfigurations.GetDefaultPreset()),
|
||||
CreatedAt = DateTime.UtcNow,
|
||||
IsDefault = true,
|
||||
IsActive = true,
|
||||
Version = 1
|
||||
}
|
||||
);
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### 3. Repository Interfaces
|
||||
|
||||
```csharp
|
||||
public interface ITestRepository
|
||||
{
|
||||
Task<TestRun> GetByIdAsync(Guid id);
|
||||
Task<List<TestRun>> GetAllAsync();
|
||||
Task<List<TestRun>> GetByScenarioAsync(Guid scenarioId);
|
||||
Task<List<TestRun>> GetByParameterSetAsync(Guid parameterSetId);
|
||||
Task<List<TestRun>> GetByDateRangeAsync(DateTime from, DateTime to);
|
||||
Task<TestRun> SaveAsync(TestRun testRun);
|
||||
Task UpdateAsync(TestRun testRun);
|
||||
Task DeleteAsync(Guid id);
|
||||
}
|
||||
|
||||
public interface IParameterRepository
|
||||
{
|
||||
Task<ParameterSet> GetByIdAsync(Guid id);
|
||||
Task<ParameterSet> GetByNameAsync(string name);
|
||||
Task<List<ParameterSet>> GetAllAsync();
|
||||
Task<ParameterSet> SaveAsync(ParameterSet parameterSet);
|
||||
Task UpdateAsync(ParameterSet parameterSet);
|
||||
Task DeleteAsync(Guid id);
|
||||
Task<List<ParameterHistory>> GetHistoryAsync(Guid parameterSetId);
|
||||
Task SaveSnapshotAsync(ParameterSnapshot snapshot);
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## REST API SPECIFICATIONS
|
||||
|
||||
Base URL: `https://robot.local:5000/api/v1`
|
||||
|
||||
---
|
||||
|
||||
### 1. Test Scenarios API
|
||||
|
||||
#### GET /scenarios
|
||||
Get all test scenarios.
|
||||
|
||||
**Response:**
|
||||
```json
|
||||
{
|
||||
"scenarios": [
|
||||
{
|
||||
"id": "uuid",
|
||||
"name": "Straight Line 10m",
|
||||
"type": "StraightLine",
|
||||
"config": { "length": 10.0 },
|
||||
"createdAt": "2026-01-26T10:00:00Z",
|
||||
"isDefault": true
|
||||
}
|
||||
]
|
||||
}
|
||||
```
|
||||
|
||||
#### GET /scenarios/{id}
|
||||
Get specific scenario.
|
||||
|
||||
#### POST /scenarios
|
||||
Create new scenario.
|
||||
|
||||
**Request:**
|
||||
```json
|
||||
{
|
||||
"name": "Custom Path",
|
||||
"type": "Custom",
|
||||
"config": {
|
||||
"waypoints": [
|
||||
{ "x": 0, "y": 0 },
|
||||
{ "x": 5, "y": 0 },
|
||||
{ "x": 5, "y": 5 }
|
||||
]
|
||||
},
|
||||
"description": "L-shaped path"
|
||||
}
|
||||
```
|
||||
|
||||
#### PUT /scenarios/{id}
|
||||
Update scenario.
|
||||
|
||||
#### DELETE /scenarios/{id}
|
||||
Delete scenario.
|
||||
|
||||
---
|
||||
|
||||
### 2. Parameter Sets API
|
||||
|
||||
#### GET /parameters
|
||||
Get all parameter sets.
|
||||
|
||||
#### GET /parameters/{id}
|
||||
Get specific parameter set.
|
||||
|
||||
#### GET /parameters/presets
|
||||
Get built-in presets.
|
||||
|
||||
**Response:**
|
||||
```json
|
||||
{
|
||||
"presets": [
|
||||
{
|
||||
"name": "Default",
|
||||
"config": { ... }
|
||||
},
|
||||
{
|
||||
"name": "Aggressive",
|
||||
"config": { ... }
|
||||
},
|
||||
{
|
||||
"name": "Smooth",
|
||||
"config": { ... }
|
||||
}
|
||||
]
|
||||
}
|
||||
```
|
||||
|
||||
#### POST /parameters
|
||||
Save new parameter set.
|
||||
|
||||
**Request:**
|
||||
```json
|
||||
{
|
||||
"name": "MyCustomConfig",
|
||||
"config": {
|
||||
"pid": {
|
||||
"kp": 1.2,
|
||||
"ki": 0.1,
|
||||
"kd": 0.05
|
||||
},
|
||||
"purePursuit": {
|
||||
"kdd": 1.0,
|
||||
"lookaheadMin": 0.3,
|
||||
"lookaheadMax": 2.0
|
||||
},
|
||||
"estimator": {
|
||||
"alphaFilter": 0.3,
|
||||
"goodTrackingBlend": 0.4
|
||||
}
|
||||
},
|
||||
"description": "Tuned for warehouse"
|
||||
}
|
||||
```
|
||||
|
||||
#### PUT /parameters/{id}
|
||||
Update parameter set.
|
||||
|
||||
#### POST /parameters/{id}/snapshot
|
||||
Create version snapshot.
|
||||
|
||||
**Request:**
|
||||
```json
|
||||
{
|
||||
"description": "Before optimization run"
|
||||
}
|
||||
```
|
||||
|
||||
#### GET /parameters/{id}/history
|
||||
Get version history.
|
||||
|
||||
---
|
||||
|
||||
### 3. Test Execution API
|
||||
|
||||
#### POST /tests/run
|
||||
Start a single test.
|
||||
|
||||
**Request:**
|
||||
```json
|
||||
{
|
||||
"scenarioId": "uuid",
|
||||
"parameterSetId": "uuid",
|
||||
"notes": "Testing new PID values"
|
||||
}
|
||||
```
|
||||
|
||||
**Response:**
|
||||
```json
|
||||
{
|
||||
"testRunId": "uuid",
|
||||
"status": "Preparing"
|
||||
}
|
||||
```
|
||||
|
||||
#### POST /tests/batch
|
||||
Run batch tests.
|
||||
|
||||
**Request:**
|
||||
```json
|
||||
{
|
||||
"scenarioIds": ["uuid1", "uuid2", "uuid3"],
|
||||
"parameterSetId": "uuid"
|
||||
}
|
||||
```
|
||||
|
||||
#### POST /tests/compare
|
||||
Compare multiple configurations.
|
||||
|
||||
**Request:**
|
||||
```json
|
||||
{
|
||||
"scenarioId": "uuid",
|
||||
"parameterSetIds": ["uuid1", "uuid2", "uuid3"]
|
||||
}
|
||||
```
|
||||
|
||||
#### GET /tests/{id}
|
||||
Get test run details.
|
||||
|
||||
#### GET /tests
|
||||
Get all test runs (paginated).
|
||||
|
||||
**Query Parameters:**
|
||||
- `page`: int (default: 1)
|
||||
- `pageSize`: int (default: 20)
|
||||
- `scenarioId`: uuid (optional filter)
|
||||
- `parameterSetId`: uuid (optional filter)
|
||||
- `status`: enum (optional filter)
|
||||
- `fromDate`: datetime (optional filter)
|
||||
- `toDate`: datetime (optional filter)
|
||||
|
||||
#### GET /tests/{id}/metrics
|
||||
Get detailed metrics for a test run.
|
||||
|
||||
#### GET /tests/{id}/download
|
||||
Download raw test data.
|
||||
|
||||
---
|
||||
|
||||
### 4. Analysis API
|
||||
|
||||
#### GET /analysis/summary
|
||||
Get summary statistics across all tests.
|
||||
|
||||
**Response:**
|
||||
```json
|
||||
{
|
||||
"totalTests": 150,
|
||||
"successRate": 0.92,
|
||||
"averageScore": 85.3,
|
||||
"bestConfiguration": {
|
||||
"name": "Optimized_v3",
|
||||
"score": 95.2
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
#### GET /analysis/trends
|
||||
Get performance trends over time.
|
||||
|
||||
#### POST /analysis/compare
|
||||
Detailed comparison between test runs.
|
||||
|
||||
**Request:**
|
||||
```json
|
||||
{
|
||||
"testRunIds": ["uuid1", "uuid2"]
|
||||
}
|
||||
```
|
||||
|
||||
#### GET /analysis/rankings
|
||||
Get ranked configurations.
|
||||
|
||||
**Query Parameters:**
|
||||
- `scenarioId`: uuid (optional)
|
||||
- `metric`: enum (overall, tracking, smoothness, efficiency)
|
||||
- `limit`: int (default: 10)
|
||||
|
||||
---
|
||||
|
||||
## SIGNALR HUB SPECIFICATIONS
|
||||
|
||||
Hub URL: `https://robot.local:5000/tuninghub`
|
||||
|
||||
---
|
||||
|
||||
### Client → Server Methods
|
||||
|
||||
```typescript
|
||||
// Start test
|
||||
await connection.invoke("StartTest", {
|
||||
scenarioId: "uuid",
|
||||
parameterSetId: "uuid"
|
||||
});
|
||||
|
||||
// Control
|
||||
await connection.invoke("PauseTest");
|
||||
await connection.invoke("ResumeTest");
|
||||
await connection.invoke("StopTest");
|
||||
await connection.invoke("EmergencyStop");
|
||||
|
||||
// Real-time parameter updates
|
||||
await connection.invoke("UpdateParameters", {
|
||||
pid: { kp: 1.5, ki: 0.1, kd: 0.05 }
|
||||
});
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### Server → Client Messages
|
||||
|
||||
```typescript
|
||||
// State updates (10Hz)
|
||||
connection.on("ReceiveState", (state: RobotState) => {
|
||||
// state: { position, heading, linearVelocity, angularVelocity, timestamp }
|
||||
});
|
||||
|
||||
// Metrics updates (1Hz)
|
||||
connection.on("ReceiveMetrics", (metrics: CurrentMetrics) => {
|
||||
// metrics: { cte, headingError, velocity, etc. }
|
||||
});
|
||||
|
||||
// Test status
|
||||
connection.on("ReceiveTestStatus", (status: TestStatusUpdate) => {
|
||||
// status: { state, message, progress, timestamp }
|
||||
});
|
||||
|
||||
// Safety events
|
||||
connection.on("ReceiveSafetyEvent", (event: SafetyEvent) => {
|
||||
// event: { type, severity, message, timestamp }
|
||||
});
|
||||
|
||||
// Test completed
|
||||
connection.on("ReceiveTestResult", (result: TestResult) => {
|
||||
// result: { testRunId, metrics, status, duration }
|
||||
});
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### Data Transfer Objects
|
||||
|
||||
```csharp
|
||||
public class RobotState
|
||||
{
|
||||
public Vector2 Position { get; set; }
|
||||
public float Heading { get; set; }
|
||||
public float LinearVelocity { get; set; }
|
||||
public float AngularVelocity { get; set; }
|
||||
public long TimestampMs { get; set; }
|
||||
}
|
||||
|
||||
public class CurrentMetrics
|
||||
{
|
||||
public float CrossTrackError { get; set; }
|
||||
public float HeadingError { get; set; }
|
||||
public float LookaheadDistance { get; set; }
|
||||
public Vector2 TargetPoint { get; set; }
|
||||
public float DistanceToGoal { get; set; }
|
||||
}
|
||||
|
||||
public class TestStatusUpdate
|
||||
{
|
||||
public TestState State { get; set; }
|
||||
public string Message { get; set; }
|
||||
public float Progress { get; set; } // 0.0 - 1.0
|
||||
public DateTime Timestamp { get; set; }
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## AUTHENTICATION & AUTHORIZATION
|
||||
|
||||
For MVP: Basic authentication (optional).
|
||||
For Production: JWT tokens with role-based access.
|
||||
|
||||
```csharp
|
||||
public enum UserRole
|
||||
{
|
||||
Viewer, // Read-only access
|
||||
Operator, // Can run tests
|
||||
Engineer, // Can modify parameters
|
||||
Admin // Full access
|
||||
}
|
||||
```
|
||||
|
||||
Endpoint Permissions:
|
||||
- GET endpoints: Viewer+
|
||||
- POST /tests/run: Operator+
|
||||
- POST /parameters: Engineer+
|
||||
- DELETE endpoints: Admin only
|
||||
|
||||
---
|
||||
|
||||
## ERROR HANDLING
|
||||
|
||||
Standard error response format:
|
||||
|
||||
```json
|
||||
{
|
||||
"error": {
|
||||
"code": "INVALID_PARAMETERS",
|
||||
"message": "Kp value must be between 0.1 and 5.0",
|
||||
"details": {
|
||||
"field": "pid.kp",
|
||||
"value": 10.5,
|
||||
"min": 0.1,
|
||||
"max": 5.0
|
||||
},
|
||||
"timestamp": "2026-01-26T10:30:00Z"
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
HTTP Status Codes:
|
||||
- 200: Success
|
||||
- 201: Created
|
||||
- 400: Bad Request (validation errors)
|
||||
- 404: Not Found
|
||||
- 409: Conflict (e.g., duplicate name)
|
||||
- 500: Internal Server Error
|
||||
- 503: Service Unavailable (robot not ready)
|
||||
|
||||
---
|
||||
|
||||
This completes the database and API specification document.
|
||||
@@ -0,0 +1,680 @@
|
||||
# ROBOT TUNING SYSTEM - COMPLETE ARCHITECTURE DOCUMENT
|
||||
|
||||
**Version:** 1.0
|
||||
**Date:** 2026-01-26
|
||||
**Target Platform:** .NET 10 + Blazor
|
||||
**Robot Type:** Differential Drive Mobile Robot
|
||||
|
||||
---
|
||||
|
||||
## TABLE OF CONTENTS
|
||||
|
||||
1. [Executive Summary](#1-executive-summary)
|
||||
2. [System Context](#2-system-context)
|
||||
3. [Requirements](#3-requirements)
|
||||
4. [Architecture Overview](#4-architecture-overview)
|
||||
5. [Layer 1: Presentation (Blazor Dashboard)](#5-layer-1-presentation)
|
||||
6. [Layer 2: Application Services](#6-layer-2-application-services)
|
||||
7. [Layer 3: Domain Logic](#7-layer-3-domain-logic)
|
||||
8. [Layer 4: Robot Control](#8-layer-4-robot-control)
|
||||
9. [Layer 5: Hardware Abstraction](#9-layer-5-hardware-abstraction)
|
||||
10. [Data Models](#10-data-models)
|
||||
11. [Database Schema](#11-database-schema)
|
||||
12. [Control Flow & Data Flow](#12-control-flow-data-flow)
|
||||
13. [Configuration Management](#13-configuration-management)
|
||||
14. [Tuning Workflows](#14-tuning-workflows)
|
||||
15. [Technology Stack](#15-technology-stack)
|
||||
16. [Deployment Architecture](#16-deployment-architecture)
|
||||
17. [Implementation Phases](#17-implementation-phases)
|
||||
18. [API Specifications](#18-api-specifications)
|
||||
19. [Performance Requirements](#19-performance-requirements)
|
||||
20. [Security & Safety](#20-security-safety)
|
||||
21. [Testing Strategy](#21-testing-strategy)
|
||||
22. [Appendices](#22-appendices)
|
||||
|
||||
---
|
||||
|
||||
## 1. EXECUTIVE SUMMARY
|
||||
|
||||
### 1.1. Purpose
|
||||
This document defines the complete architecture for a **Robot Tuning System** designed to optimize control parameters for differential drive mobile robots. The system enables:
|
||||
- Interactive parameter tuning via web dashboard
|
||||
- Automated test execution and performance evaluation
|
||||
- Comparative analysis of parameter configurations
|
||||
- Historical tracking and reporting
|
||||
|
||||
### 1.2. System Goals
|
||||
**Primary Goal:** Find optimal parameter sets for PID velocity control, Pure Pursuit path tracking, and Velocity Estimator that minimize tracking error while maintaining smooth motion.
|
||||
|
||||
**Secondary Goals:**
|
||||
- Reduce tuning time from days to hours
|
||||
- Enable reproducible, data-driven parameter selection
|
||||
- Support multiple test scenarios (straight lines, circles, complex paths)
|
||||
- Provide intuitive visualization and analysis tools
|
||||
|
||||
### 1.3. Key Stakeholders
|
||||
- **Robot Developers:** Configure and tune robot behavior
|
||||
- **Test Engineers:** Run validation tests and generate reports
|
||||
- **AI Systems:** Process and analyze this architecture document
|
||||
|
||||
### 1.4. Success Metrics
|
||||
- **Tracking Accuracy:** Cross-track error RMS < 10cm
|
||||
- **Smoothness:** Max jerk < 5 m/s³
|
||||
- **Efficiency:** Path length ratio < 1.15
|
||||
- **Tuning Speed:** Find acceptable parameters within 2 hours
|
||||
- **Reproducibility:** Result variance < 5% across runs
|
||||
|
||||
---
|
||||
|
||||
## 2. SYSTEM CONTEXT
|
||||
|
||||
### 2.1. Robot Overview
|
||||
|
||||
**Robot Type:** Differential Drive Mobile Robot
|
||||
**Physical Characteristics:**
|
||||
- Wheelbase: 0.35m (distance between left/right wheels)
|
||||
- Wheel radius: 0.075m
|
||||
- Mass: ~25kg
|
||||
- Max linear velocity: 1.5 m/s
|
||||
- Max angular velocity: 6 rad/s
|
||||
- Max linear acceleration: 1.0 m/s²
|
||||
- Max angular acceleration: 1.0 rad/s²
|
||||
|
||||
**Operating Environment:**
|
||||
- Indoor spaces (smooth floors)
|
||||
- Test area: 10m × 20m
|
||||
- No dynamic obstacles during tuning
|
||||
|
||||
### 2.2. Control System Architecture
|
||||
|
||||
The robot uses a **hierarchical control structure**:
|
||||
|
||||
```
|
||||
Goal Position
|
||||
↓
|
||||
[Distance-based PID] → Linear Velocity (v_max)
|
||||
↓
|
||||
[Velocity Estimator] → Estimated Velocity (v_hybrid)
|
||||
↓ ↓
|
||||
└───────→ [Pure Pursuit] ←─┘
|
||||
↓
|
||||
Angular Velocity (ω)
|
||||
↓
|
||||
[Combine (v_max, ω)] → (v_cmd, ω_cmd)
|
||||
↓
|
||||
[Differential Kinematics] → (wheel_left, wheel_right)
|
||||
↓
|
||||
Motor Commands
|
||||
```
|
||||
|
||||
**Controller Descriptions:**
|
||||
|
||||
1. **Distance-based PID Controller:**
|
||||
- **Input:** Distance to goal (error = distance_to_goal)
|
||||
- **Output:** Maximum linear velocity (v_max)
|
||||
- **Logic:**
|
||||
- If distance > 5m: return max velocity (1.5 m/s)
|
||||
- If distance ≤ 5m: PID control
|
||||
- If velocity < min velocity: return min velocity
|
||||
- **Parameters to tune:** Kp, Ki, Kd
|
||||
|
||||
2. **Velocity Estimator:**
|
||||
- **Purpose:** Combine encoder measurements with kinematic model for accurate velocity estimation
|
||||
- **Method:** Adaptive blending based on tracking quality
|
||||
- **Model:** First-order system response
|
||||
```
|
||||
v_predicted = v_actual + (v_cmd - v_actual) × (1 - e^(-t_eff/τ))
|
||||
where t_eff = t_ahead - delay
|
||||
```
|
||||
- **Blending:**
|
||||
```
|
||||
v_hybrid = blend_ratio × v_model + (1 - blend_ratio) × v_encoder
|
||||
```
|
||||
- **Adaptive Logic:**
|
||||
- Good tracking (error < 12%): blend_ratio = 0.3 (trust encoder 70%)
|
||||
- Moderate tracking (error < 30%): blend_ratio = 0.5
|
||||
- Poor tracking (error ≥ 30%): blend_ratio = 0.7 (trust model 70%)
|
||||
- **Parameters to tune:** AlphaFilter, blend ratios, confidence decay rate
|
||||
|
||||
3. **Pure Pursuit Controller:**
|
||||
- **Input:** Current position, v_hybrid, reference path
|
||||
- **Output:** Angular velocity (ω)
|
||||
- **Lookahead calculation:**
|
||||
```
|
||||
lookahead = clamp(
|
||||
LookaheadMin + Kdd × |v_hybrid|,
|
||||
LookaheadMin,
|
||||
LookaheadMax
|
||||
)
|
||||
lookahead *= confidence // Reduce if estimator confidence is low
|
||||
```
|
||||
- **Parameters to tune:** Kdd, LookaheadMin, LookaheadMax
|
||||
|
||||
### 2.3. Tuning Challenges
|
||||
|
||||
**Current State:**
|
||||
- Manual tuning takes days per robot
|
||||
- No systematic approach to parameter selection
|
||||
- Difficult to validate performance across scenarios
|
||||
- Parameters tuned for one trajectory may fail on others
|
||||
|
||||
**Desired State:**
|
||||
- Semi-automated tuning process
|
||||
- Data-driven parameter optimization
|
||||
- Cross-scenario validation
|
||||
- Reproducible results with confidence metrics
|
||||
|
||||
---
|
||||
|
||||
## 3. REQUIREMENTS
|
||||
|
||||
### 3.1. Functional Requirements
|
||||
|
||||
**FR-1: Test Execution**
|
||||
- FR-1.1: System shall execute single test runs with specified parameters
|
||||
- FR-1.2: System shall execute batch tests across multiple configurations
|
||||
- FR-1.3: System shall support at least 3 trajectory types: straight line, large circle (2m radius), small circle (0.5m radius)
|
||||
- FR-1.4: System shall log all telemetry data at 50Hz during test execution
|
||||
- FR-1.5: System shall detect and abort tests on safety violations
|
||||
|
||||
**FR-2: Parameter Management**
|
||||
- FR-2.1: System shall allow users to configure all tunable parameters via UI
|
||||
- FR-2.2: System shall validate parameters against physical constraints
|
||||
- FR-2.3: System shall save/load parameter configurations with versioning
|
||||
- FR-2.4: System shall support parameter presets (default, aggressive, smooth)
|
||||
|
||||
**FR-3: Metrics & Analysis**
|
||||
- FR-3.1: System shall calculate tracking accuracy metrics (CTE RMS, heading error)
|
||||
- FR-3.2: System shall calculate smoothness metrics (jerk, velocity variance)
|
||||
- FR-3.3: System shall calculate efficiency metrics (path length ratio, time)
|
||||
- FR-3.4: System shall compute overall score based on weighted metrics
|
||||
- FR-3.5: System shall compare multiple configurations side-by-side
|
||||
|
||||
**FR-4: Visualization**
|
||||
- FR-4.1: System shall display real-time 2D trajectory during test execution
|
||||
- FR-4.2: System shall stream live telemetry charts (velocity, CTE, etc.)
|
||||
- FR-4.3: System shall visualize post-test analysis with interactive charts
|
||||
- FR-4.4: System shall support trajectory replay from logged data
|
||||
|
||||
**FR-5: Reporting**
|
||||
- FR-5.1: System shall export test results to CSV format
|
||||
- FR-5.2: System shall generate HTML summary reports
|
||||
- FR-5.3: System shall maintain test history in database
|
||||
|
||||
**FR-6: Safety**
|
||||
- FR-6.1: System shall monitor cross-track error continuously
|
||||
- FR-6.2: System shall trigger emergency stop if CTE > 0.5m
|
||||
- FR-6.3: System shall trigger emergency stop if heading error > 45°
|
||||
- FR-6.4: System shall log all safety violations with timestamps
|
||||
|
||||
### 3.2. Non-Functional Requirements
|
||||
|
||||
**NFR-1: Performance**
|
||||
- NFR-1.1: Control loop shall execute at 50Hz (±2ms jitter)
|
||||
- NFR-1.2: UI updates shall occur at ≥10Hz with <200ms lag
|
||||
- NFR-1.3: Data logging shall not impact control loop performance
|
||||
- NFR-1.4: Test completion time shall be <2× trajectory duration
|
||||
|
||||
**NFR-2: Usability**
|
||||
- NFR-2.1: Non-technical users shall be able to run basic tests
|
||||
- NFR-2.2: Parameter controls shall provide immediate visual feedback
|
||||
- NFR-2.3: Error messages shall be clear and actionable
|
||||
- NFR-2.4: Dashboard shall be accessible via web browser
|
||||
|
||||
**NFR-3: Reliability**
|
||||
- NFR-3.1: System shall recover from SignalR disconnections automatically
|
||||
- NFR-3.2: Test data shall not be lost on application crash
|
||||
- NFR-3.3: System shall handle encoder noise and wheel slip gracefully
|
||||
|
||||
**NFR-4: Maintainability**
|
||||
- NFR-4.1: Code shall follow SOLID principles
|
||||
- NFR-4.2: Each layer shall have clear interfaces and minimal coupling
|
||||
- NFR-4.3: Unit test coverage shall be >80% for domain logic
|
||||
|
||||
**NFR-5: Scalability**
|
||||
- NFR-5.1: System shall support multiple test scenarios (target: 10+)
|
||||
- NFR-5.2: Database shall handle 1000+ test runs without degradation
|
||||
- NFR-5.3: Architecture shall allow future addition of optimization algorithms
|
||||
|
||||
### 3.3. Acceptance Criteria
|
||||
|
||||
**Primary Metric (Tracking Accuracy):**
|
||||
- Cross-track error RMS < 0.10m (10cm)
|
||||
- Cross-track error peak < 0.20m (20cm)
|
||||
- Heading error RMS < 10° (0.174 rad)
|
||||
- Goal position error < 0.05m (5cm)
|
||||
|
||||
**Secondary Metric (Smoothness):**
|
||||
- Max jerk < 5.0 m/s³
|
||||
- Max angular jerk < 10.0 rad/s³
|
||||
- Velocity standard deviation < 0.15 m/s
|
||||
|
||||
**Tertiary Metric (Efficiency):**
|
||||
- Path length ratio < 1.15 (actual path < 115% of optimal)
|
||||
- Success rate > 90% (9 out of 10 runs pass)
|
||||
|
||||
---
|
||||
|
||||
## 4. ARCHITECTURE OVERVIEW
|
||||
|
||||
### 4.1. Architectural Style
|
||||
**Layered Architecture** with clean separation between presentation, application logic, domain logic, and infrastructure.
|
||||
|
||||
**Key Patterns:**
|
||||
- **Repository Pattern:** Data access abstraction
|
||||
- **Service Layer Pattern:** Application-level orchestration
|
||||
- **Domain-Driven Design:** Rich domain models
|
||||
- **CQRS (Light):** Separate read/write models for optimization
|
||||
- **Event-Driven:** Real-time updates via SignalR
|
||||
|
||||
### 4.2. Layer Diagram
|
||||
|
||||
```
|
||||
┌─────────────────────────────────────────────────────────────────────┐
|
||||
│ LAYER 1: PRESENTATION │
|
||||
│ ┌─────────────────────────────────────────────────────────────────┐ │
|
||||
│ │ Blazor Dashboard (Web UI) │ │
|
||||
│ │ - Real-time Monitoring Pages │ │
|
||||
│ │ - Parameter Tuning Controls │ │
|
||||
│ │ - Analysis & Visualization │ │
|
||||
│ │ - Configuration Management │ │
|
||||
│ └─────────────────────────────────────────────────────────────────┘ │
|
||||
└───────────────────────────────┬─────────────────────────────────────┘
|
||||
│ SignalR Hubs / REST API
|
||||
▼
|
||||
┌─────────────────────────────────────────────────────────────────────┐
|
||||
│ LAYER 2: APPLICATION SERVICES │
|
||||
│ ┌──────────────┐ ┌──────────────┐ ┌──────────────┐ │
|
||||
│ │ Tuning │ │ Parameter │ │ Metric │ │
|
||||
│ │ Orchestrator │ │ Manager │ │ Analyzer │ │
|
||||
│ └──────────────┘ └──────────────┘ └──────────────┘ │
|
||||
│ ┌──────────────┐ ┌──────────────┐ │
|
||||
│ │ Report │ │ Event │ │
|
||||
│ │ Generator │ │ Publisher │ │
|
||||
│ └──────────────┘ └──────────────┘ │
|
||||
└───────────────────────────────┬─────────────────────────────────────┘
|
||||
│ Domain Interfaces
|
||||
▼
|
||||
┌─────────────────────────────────────────────────────────────────────┐
|
||||
│ LAYER 3: DOMAIN LOGIC │
|
||||
│ ┌──────────────┐ ┌──────────────┐ ┌──────────────┐ │
|
||||
│ │ Test │ │ Trajectory │ │ Parameter │ │
|
||||
│ │ Executor │ │ Generator │ │ Optimizer │ │
|
||||
│ └──────────────┘ └──────────────┘ └──────────────┘ │
|
||||
│ ┌──────────────┐ ┌──────────────┐ ┌──────────────┐ │
|
||||
│ │ Metric │ │ Safety │ │ Scoring │ │
|
||||
│ │ Calculator │ │ Monitor │ │ Engine │ │
|
||||
│ └──────────────┘ └──────────────┘ └──────────────┘ │
|
||||
└───────────────────────────────┬─────────────────────────────────────┘
|
||||
│ Control Interfaces
|
||||
▼
|
||||
┌─────────────────────────────────────────────────────────────────────┐
|
||||
│ LAYER 4: ROBOT CONTROL │
|
||||
│ ┌──────────────┐ ┌──────────────┐ ┌──────────────┐ │
|
||||
│ │ PID │ │ Velocity │ │ Pure │ │
|
||||
│ │ Controller │ │ Estimator │ │ Pursuit │ │
|
||||
│ └──────────────┘ └──────────────┘ └──────────────┘ │
|
||||
│ ┌──────────────┐ ┌──────────────┐ │
|
||||
│ │ Data │ │ State │ │
|
||||
│ │ Logger │ │ Manager │ │
|
||||
│ └──────────────┘ └──────────────┘ │
|
||||
└───────────────────────────────┬─────────────────────────────────────┘
|
||||
│ Hardware Interfaces
|
||||
▼
|
||||
┌─────────────────────────────────────────────────────────────────────┐
|
||||
│ LAYER 5: HARDWARE ABSTRACTION │
|
||||
│ ┌──────────────┐ ┌──────────────┐ ┌──────────────┐ │
|
||||
│ │ Motor │ │ Encoder │ │ Robot │ │
|
||||
│ │ Driver │ │ Reader │ │ State │ │
|
||||
│ └──────────────┘ └──────────────┘ └──────────────┘ │
|
||||
└───────────────────────────────┬─────────────────────────────────────┘
|
||||
│
|
||||
▼
|
||||
[Physical Hardware]
|
||||
- Motors
|
||||
- Encoders
|
||||
- Emergency Stop
|
||||
```
|
||||
|
||||
### 4.3. Component Interactions
|
||||
|
||||
**Typical Test Execution Flow:**
|
||||
|
||||
```
|
||||
User → UI → TuningOrchestrator → TestExecutor → Controllers → Hardware
|
||||
↓ ↓ ↓
|
||||
ParameterManager SafetyMonitor DataLogger
|
||||
↓ ↓
|
||||
[Abort?] [Database]
|
||||
↓
|
||||
UI ← SignalR ← EventPublisher ← MetricAnalyzer ← [Results]
|
||||
```
|
||||
|
||||
### 4.4. Project Structure
|
||||
|
||||
```
|
||||
RobotTuning.sln
|
||||
├── src/
|
||||
│ ├── RobotTuning.Domain/ # Layer 3: Domain Logic
|
||||
│ │ ├── Models/ # Domain entities
|
||||
│ │ ├── Services/ # Domain services
|
||||
│ │ ├── Interfaces/ # Abstractions
|
||||
│ │ └── ValueObjects/ # Value objects
|
||||
│ │
|
||||
│ ├── RobotTuning.Application/ # Layer 2: Application Services
|
||||
│ │ ├── Services/ # Orchestrators, managers
|
||||
│ │ ├── DTOs/ # Data transfer objects
|
||||
│ │ ├── Interfaces/ # Service contracts
|
||||
│ │ └── Mapping/ # AutoMapper profiles
|
||||
│ │
|
||||
│ ├── RobotTuning.Infrastructure/ # Layer 4 & 5: Control & Hardware
|
||||
│ │ ├── Controllers/ # PID, Estimator, PurePursuit
|
||||
│ │ ├── Hardware/ # Motor drivers, encoder readers
|
||||
│ │ ├── Logging/ # Data logger implementation
|
||||
│ │ ├── Persistence/ # Database context, repositories
|
||||
│ │ └── Configuration/ # Config file handling
|
||||
│ │
|
||||
│ ├── RobotTuning.Web/ # Layer 1: Presentation
|
||||
│ │ ├── Pages/ # Blazor pages
|
||||
│ │ ├── Components/ # Reusable UI components
|
||||
│ │ ├── Hubs/ # SignalR hubs
|
||||
│ │ ├── wwwroot/ # Static files, JS libraries
|
||||
│ │ └── Program.cs # Application entry point
|
||||
│ │
|
||||
│ └── RobotTuning.Shared/ # Shared utilities
|
||||
│ ├── Constants/
|
||||
│ ├── Extensions/
|
||||
│ └── Helpers/
|
||||
│
|
||||
└── tests/
|
||||
├── RobotTuning.Domain.Tests/
|
||||
├── RobotTuning.Application.Tests/
|
||||
└── RobotTuning.Integration.Tests/
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 5. LAYER 1: PRESENTATION
|
||||
|
||||
### 5.1. Page Structure
|
||||
|
||||
```
|
||||
/Pages
|
||||
├── Index.razor # Landing page
|
||||
├── Dashboard/
|
||||
│ ├── RealTimeMonitor.razor # Live test monitoring
|
||||
│ │ ├── TrajectoryView.razor # 2D path visualization
|
||||
│ │ ├── TelemetryPanel.razor # Live metrics
|
||||
│ │ └── StatusIndicators.razor # State, warnings
|
||||
│ └── LiveCharts.razor # Streaming charts
|
||||
├── Tuning/
|
||||
│ ├── ManualTuning.razor # Interactive parameter adjustment
|
||||
│ │ ├── ParameterSliders.razor # PID, PP, Estimator controls
|
||||
│ │ ├── QuickActions.razor # Run, Stop, Reset buttons
|
||||
│ │ └── SuggestionPanel.razor # AI-powered suggestions
|
||||
│ ├── AutoTuning.razor # Automated optimization
|
||||
│ │ ├── OptimizationConfig.razor # Algorithm selection, constraints
|
||||
│ │ └── ProgressView.razor # Optimization progress
|
||||
│ └── ParameterComparison.razor # A/B testing
|
||||
│ ├── ConfigSelector.razor # Select configs to compare
|
||||
│ ├── ComparisonTable.razor # Side-by-side metrics
|
||||
│ └── ComparisonCharts.razor # Visual comparison
|
||||
├── Analysis/
|
||||
│ ├── MetricsAnalysis.razor # Deep-dive metrics
|
||||
│ │ ├── TrackingAccuracy.razor # CTE, heading analysis
|
||||
│ │ ├── SmoothnessAnalysis.razor # Jerk, acceleration plots
|
||||
│ │ └── EfficiencyAnalysis.razor # Path length, time metrics
|
||||
│ ├── TrajectoryVisualization.razor # Post-test trajectory viewer
|
||||
│ │ ├── PathOverlay.razor # Actual vs reference path
|
||||
│ │ ├── ErrorHeatmap.razor # CTE along path
|
||||
│ │ └── PlaybackControls.razor # Replay timeline
|
||||
│ └── PerformanceReport.razor # Summary reports
|
||||
│ ├── ScoreCard.razor # Overall scores
|
||||
│ ├── MetricsSummary.razor # Key metrics table
|
||||
│ └── ExportOptions.razor # PDF, CSV export
|
||||
├── Configuration/
|
||||
│ ├── TestScenarios.razor # Define test trajectories
|
||||
│ │ ├── TrajectoryBuilder.razor # Visual trajectory editor
|
||||
│ │ └── ScenarioLibrary.razor # Saved scenarios
|
||||
│ ├── RobotSettings.razor # Physical parameters
|
||||
│ │ ├── PhysicalParams.razor # Wheelbase, mass, etc.
|
||||
│ │ └── TimingConfig.razor # Control loop frequency
|
||||
│ └── AcceptanceCriteria.razor # Pass/fail thresholds
|
||||
│ ├── MetricThresholds.razor # Set limits
|
||||
│ └── WeightingConfig.razor # Metric weights for scoring
|
||||
└── History/
|
||||
├── TestHistory.razor # Historical test runs
|
||||
│ ├── TestList.razor # Filterable list
|
||||
│ ├── TestDetails.razor # Drill-down view
|
||||
│ └── SearchAndFilter.razor # Date, config, trajectory filters
|
||||
└── ParameterEvolution.razor # Parameter changes over time
|
||||
├── EvolutionTimeline.razor # Visual timeline
|
||||
└── ChangeLog.razor # Detailed change log
|
||||
```
|
||||
|
||||
### 5.2. Key UI Components
|
||||
|
||||
#### 5.2.1. Real-Time Monitoring Panel
|
||||
|
||||
**Component:** `RealTimeMonitor.razor`
|
||||
|
||||
**Features:**
|
||||
- **Trajectory View:** 2D canvas showing robot position, reference path, lookahead point
|
||||
- **Telemetry Gauges:** Speed, angular velocity, CTE, heading error (updated 10Hz)
|
||||
- **Progress Bar:** Distance completed / total distance
|
||||
- **Status Indicators:** Running, Paused, Warning, Error states
|
||||
- **Control Buttons:** Pause, Resume, Stop, Emergency Stop
|
||||
|
||||
**Data Binding:**
|
||||
```csharp
|
||||
@code {
|
||||
[Inject] IHubConnection HubConnection { get; set; }
|
||||
|
||||
private RobotState currentState;
|
||||
private List<Vector2> trajectoryHistory = new();
|
||||
|
||||
protected override async Task OnInitializedAsync()
|
||||
{
|
||||
HubConnection.On<RobotState>("ReceiveState", state =>
|
||||
{
|
||||
currentState = state;
|
||||
trajectoryHistory.Add(state.Position);
|
||||
StateHasChanged();
|
||||
});
|
||||
|
||||
await HubConnection.StartAsync();
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
**SignalR Messages:**
|
||||
- `ReceiveState`: Full robot state (50Hz → throttled to 10Hz)
|
||||
- `ReceiveMetrics`: Current metrics (CTE, heading error, etc.)
|
||||
- `ReceiveSafetyEvent`: Safety violations or warnings
|
||||
- `ReceiveTestStatus`: Test lifecycle events (started, paused, completed, aborted)
|
||||
|
||||
#### 5.2.2. Parameter Tuning Panel
|
||||
|
||||
**Component:** `ParameterSliders.razor`
|
||||
|
||||
**Features:**
|
||||
- **Grouped Sliders:** PID (Kp, Ki, Kd), Pure Pursuit (Kdd, lookahead), Estimator (alpha, blends)
|
||||
- **Real-time Validation:** Show red border if value out of bounds
|
||||
- **Value Input:** Slider + numeric input for precise control
|
||||
- **Reset Button:** Revert to last saved or default values
|
||||
- **Presets Dropdown:** Quick load (Conservative, Balanced, Aggressive)
|
||||
|
||||
**Example Markup:**
|
||||
```razor
|
||||
<MudCard>
|
||||
<MudCardHeader>PID Controller</MudCardHeader>
|
||||
<MudCardContent>
|
||||
<MudSlider T="double"
|
||||
@bind-Value="parameters.PID.Kp"
|
||||
Min="@Bounds.KpRange.Min"
|
||||
Max="@Bounds.KpRange.Max"
|
||||
Step="0.1"
|
||||
ValueLabel="true">
|
||||
Kp: @parameters.PID.Kp.ToString("F2")
|
||||
</MudSlider>
|
||||
|
||||
<MudTextField @bind-Value="parameters.PID.Kp"
|
||||
Label="Kp (Precise)"
|
||||
Variant="Variant.Outlined"
|
||||
Validation="@ValidateKp" />
|
||||
|
||||
<!-- Repeat for Ki, Kd -->
|
||||
</MudCardContent>
|
||||
</MudCard>
|
||||
```
|
||||
|
||||
#### 5.2.3. Metrics Dashboard
|
||||
|
||||
**Component:** `MetricsSummary.razor`
|
||||
|
||||
**Layout:**
|
||||
```
|
||||
┌─────────────────────────────────────────────────────────┐
|
||||
│ TRACKING ACCURACY ⭐⭐⭐⭐☆ (92/100) │
|
||||
├─────────────────────────────────────────────────────────┤
|
||||
│ Cross-Track Error RMS 0.087m ✅ (< 0.10m) │
|
||||
│ Cross-Track Error Peak 0.152m ✅ (< 0.20m) │
|
||||
│ Heading Error RMS 8.3° ✅ (< 10°) │
|
||||
│ Goal Position Error 0.042m ✅ (< 0.05m) │
|
||||
├─────────────────────────────────────────────────────────┤
|
||||
│ SMOOTHNESS ⭐⭐⭐⭐☆ (88/100) │
|
||||
├─────────────────────────────────────────────────────────┤
|
||||
│ Max Jerk 3.2 m/s³ ✅ (< 5.0) │
|
||||
│ Velocity Std Dev 0.08 m/s ✅ │
|
||||
│ Angular Jerk 6.1 r/s³ ✅ (< 10.0) │
|
||||
├─────────────────────────────────────────────────────────┤
|
||||
│ EFFICIENCY ⭐⭐⭐⭐☆ (85/100) │
|
||||
├─────────────────────────────────────────────────────────┤
|
||||
│ Path Length Ratio 1.08 ✅ (< 1.15) │
|
||||
│ Completion Time 8.5s │
|
||||
│ Average Speed 1.18 m/s │
|
||||
└─────────────────────────────────────────────────────────┘
|
||||
```
|
||||
|
||||
**Color Coding:**
|
||||
- Green ✅: Metric passes acceptance criteria
|
||||
- Yellow ⚠️: Metric close to threshold (within 10%)
|
||||
- Red ❌: Metric fails acceptance criteria
|
||||
|
||||
#### 5.2.4. Trajectory Visualization
|
||||
|
||||
**Component:** `TrajectoryView.razor`
|
||||
|
||||
**Canvas Rendering (using Blazor.Extensions.Canvas or Plotly):**
|
||||
- **Reference Path:** Solid blue line
|
||||
- **Actual Path:** Dashed green line (updates real-time)
|
||||
- **Robot Icon:** Oriented triangle at current position
|
||||
- **Lookahead Point:** Red circle on reference path
|
||||
- **Target Goal:** Flag icon
|
||||
- **Error Bars:** Perpendicular lines showing CTE at sample points
|
||||
|
||||
**Interactive Features:**
|
||||
- Zoom/pan
|
||||
- Click to see metrics at specific point
|
||||
- Toggle layers (reference, actual, errors)
|
||||
|
||||
### 5.3. SignalR Hub Definition
|
||||
|
||||
**File:** `Hubs/TuningHub.cs`
|
||||
|
||||
```csharp
|
||||
public class TuningHub : Hub
|
||||
{
|
||||
private readonly ITuningOrchestrator _orchestrator;
|
||||
|
||||
public TuningHub(ITuningOrchestrator orchestrator)
|
||||
{
|
||||
_orchestrator = orchestrator;
|
||||
}
|
||||
|
||||
// Client → Server
|
||||
public async Task StartTest(TestScenario scenario, ParameterSet parameters)
|
||||
{
|
||||
await _orchestrator.StartTest(scenario, parameters, Context.ConnectionId);
|
||||
}
|
||||
|
||||
public async Task PauseTest()
|
||||
{
|
||||
await _orchestrator.PauseTest(Context.ConnectionId);
|
||||
}
|
||||
|
||||
public async Task StopTest()
|
||||
{
|
||||
await _orchestrator.StopTest(Context.ConnectionId);
|
||||
}
|
||||
|
||||
public async Task EmergencyStop()
|
||||
{
|
||||
await _orchestrator.EmergencyStop(Context.ConnectionId);
|
||||
}
|
||||
|
||||
// Server → Client (called by orchestrator)
|
||||
// Clients.Caller.SendAsync("ReceiveState", state);
|
||||
// Clients.Caller.SendAsync("ReceiveMetrics", metrics);
|
||||
// Clients.Caller.SendAsync("ReceiveTestStatus", status);
|
||||
// Clients.Caller.SendAsync("ReceiveSafetyEvent", safetyEvent);
|
||||
}
|
||||
```
|
||||
|
||||
**Client-side Connection:**
|
||||
```csharp
|
||||
@code {
|
||||
private HubConnection hubConnection;
|
||||
|
||||
protected override async Task OnInitializedAsync()
|
||||
{
|
||||
hubConnection = new HubConnectionBuilder()
|
||||
.WithUrl(NavigationManager.ToAbsoluteUri("/tuninghub"))
|
||||
.WithAutomaticReconnect()
|
||||
.Build();
|
||||
|
||||
hubConnection.On<RobotState>("ReceiveState", HandleStateUpdate);
|
||||
hubConnection.On<TestMetrics>("ReceiveMetrics", HandleMetricsUpdate);
|
||||
hubConnection.On<TestStatus>("ReceiveTestStatus", HandleStatusUpdate);
|
||||
hubConnection.On<SafetyEvent>("ReceiveSafetyEvent", HandleSafetyEvent);
|
||||
|
||||
await hubConnection.StartAsync();
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 6. LAYER 2: APPLICATION SERVICES
|
||||
|
||||
### 6.1. TuningOrchestrator
|
||||
|
||||
**Responsibility:** Coordinate the entire tuning workflow from test initiation to result storage.
|
||||
|
||||
**Interface:**
|
||||
```csharp
|
||||
public interface ITuningOrchestrator
|
||||
{
|
||||
// Test execution
|
||||
Task<TestResult> RunSingleTest(
|
||||
TestScenario scenario,
|
||||
ParameterSet parameters,
|
||||
string connectionId = null
|
||||
);
|
||||
|
||||
Task<BatchTestResult> RunBatchTests(
|
||||
List<TestScenario> scenarios,
|
||||
ParameterSet parameters
|
||||
);
|
||||
|
||||
Task<ComparisonResult> CompareConfigurations(
|
||||
List<ParameterSet> parameterSets,
|
||||
TestScenario scenario
|
||||
);
|
||||
|
||||
// Real-time control
|
||||
Task StartTest(TestScenario scenario, ParameterSet parameters, string connectionId);
|
||||
Task PauseTest(string connectionId);
|
||||
Task ResumeTest(string connectionId);
|
||||
Task StopTest(string connectionId);
|
||||
Task EmergencyStop(string connectionId
|
||||
882
docs/RobotApp-TunningNav/APPLICATIONSERVICES&DOMAINLOGIC.md
Normal file
882
docs/RobotApp-TunningNav/APPLICATIONSERVICES&DOMAINLOGIC.md
Normal file
@@ -0,0 +1,882 @@
|
||||
# LAYERS 2-3: APPLICATION SERVICES & DOMAIN LOGIC
|
||||
|
||||
**Document:** Part 2 of Robot Tuning System Architecture
|
||||
**Layers Covered:** Application Services (Layer 2) and Domain Logic (Layer 3)
|
||||
|
||||
---
|
||||
|
||||
## LAYER 2: APPLICATION SERVICES
|
||||
|
||||
Application Services orchestrate business workflows and coordinate between the UI layer and domain logic. They handle cross-cutting concerns like transaction management, event publishing, and data transformation.
|
||||
|
||||
---
|
||||
|
||||
### 1. TuningOrchestrator Service
|
||||
|
||||
**File:** `Application/Services/TuningOrchestrator.cs`
|
||||
|
||||
**Responsibility:** Master coordinator for all tuning operations.
|
||||
|
||||
#### Interface Definition
|
||||
|
||||
```csharp
|
||||
public interface ITuningOrchestrator
|
||||
{
|
||||
// Test execution
|
||||
Task<TestResult> RunSingleTest(
|
||||
TestScenario scenario,
|
||||
ParameterSet parameters,
|
||||
string? connectionId = null
|
||||
);
|
||||
|
||||
Task<BatchTestResult> RunBatchTests(
|
||||
List<TestScenario> scenarios,
|
||||
ParameterSet parameters,
|
||||
CancellationToken cancellationToken = default
|
||||
);
|
||||
|
||||
Task<ComparisonResult> CompareConfigurations(
|
||||
List<ParameterSet> parameterSets,
|
||||
TestScenario scenario
|
||||
);
|
||||
|
||||
// Real-time control
|
||||
Task StartTestAsync(
|
||||
TestScenario scenario,
|
||||
ParameterSet parameters,
|
||||
string connectionId
|
||||
);
|
||||
Task PauseTestAsync(string connectionId);
|
||||
Task ResumeTestAsync(string connectionId);
|
||||
Task StopTestAsync(string connectionId);
|
||||
Task EmergencyStopAsync(string connectionId);
|
||||
|
||||
// State queries
|
||||
TuningState GetCurrentState(string connectionId);
|
||||
TestProgress GetProgress(string connectionId);
|
||||
|
||||
// Optimization
|
||||
Task<OptimizationResult> RunManualTuning(ManualTuningSession session);
|
||||
Task<OptimizationResult> RunAutoTuning(
|
||||
AutoTuningConfig config,
|
||||
IProgress<OptimizationProgress> progress,
|
||||
CancellationToken cancellationToken = default
|
||||
);
|
||||
}
|
||||
```
|
||||
|
||||
#### Implementation Details
|
||||
|
||||
```csharp
|
||||
public class TuningOrchestrator : ITuningOrchestrator
|
||||
{
|
||||
private readonly ITestExecutor _testExecutor;
|
||||
private readonly IParameterManager _parameterManager;
|
||||
private readonly IMetricAnalyzer _metricAnalyzer;
|
||||
private readonly IEventPublisher _eventPublisher;
|
||||
private readonly ITestRepository _testRepository;
|
||||
private readonly ILogger<TuningOrchestrator> _logger;
|
||||
|
||||
// Active test sessions keyed by connectionId
|
||||
private readonly ConcurrentDictionary<string, TestSession> _activeSessions;
|
||||
|
||||
public async Task<TestResult> RunSingleTest(
|
||||
TestScenario scenario,
|
||||
ParameterSet parameters,
|
||||
string? connectionId = null)
|
||||
{
|
||||
// 1. Validate inputs
|
||||
var validationResult = await _parameterManager.ValidateAsync(parameters);
|
||||
if (!validationResult.IsValid)
|
||||
{
|
||||
throw new InvalidParameterException(validationResult.Errors);
|
||||
}
|
||||
|
||||
// 2. Create test session
|
||||
var session = new TestSession
|
||||
{
|
||||
Id = Guid.NewGuid(),
|
||||
Scenario = scenario,
|
||||
Parameters = parameters,
|
||||
ConnectionId = connectionId,
|
||||
State = TestState.Preparing
|
||||
};
|
||||
|
||||
if (connectionId != null)
|
||||
{
|
||||
_activeSessions.TryAdd(connectionId, session);
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
// 3. Initialize test
|
||||
await PublishStatusAsync(session, TestState.Preparing);
|
||||
await _testExecutor.InitializeAsync(scenario, parameters);
|
||||
|
||||
// 4. Execute test
|
||||
await PublishStatusAsync(session, TestState.Running);
|
||||
var executionResult = await _testExecutor.ExecuteAsync(
|
||||
onStateUpdate: state => PublishStateAsync(session, state),
|
||||
onSafetyViolation: violation => HandleSafetyViolationAsync(session, violation)
|
||||
);
|
||||
|
||||
// 5. Analyze results
|
||||
await PublishStatusAsync(session, TestState.Analyzing);
|
||||
var metrics = await _metricAnalyzer.AnalyzeAsync(executionResult);
|
||||
|
||||
// 6. Create test result
|
||||
var testResult = new TestResult
|
||||
{
|
||||
Id = Guid.NewGuid(),
|
||||
SessionId = session.Id,
|
||||
Scenario = scenario,
|
||||
Parameters = parameters,
|
||||
ExecutionData = executionResult,
|
||||
Metrics = metrics,
|
||||
StartTime = executionResult.StartTime,
|
||||
EndTime = executionResult.EndTime,
|
||||
Status = executionResult.Status
|
||||
};
|
||||
|
||||
// 7. Persist to database
|
||||
await _testRepository.SaveAsync(testResult);
|
||||
|
||||
// 8. Notify completion
|
||||
await PublishStatusAsync(session, TestState.Completed);
|
||||
await PublishResultAsync(session, testResult);
|
||||
|
||||
return testResult;
|
||||
}
|
||||
catch (SafetyViolationException ex)
|
||||
{
|
||||
_logger.LogError(ex, "Safety violation during test");
|
||||
await PublishStatusAsync(session, TestState.Aborted);
|
||||
throw;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error during test execution");
|
||||
await PublishStatusAsync(session, TestState.Error);
|
||||
throw;
|
||||
}
|
||||
finally
|
||||
{
|
||||
if (connectionId != null)
|
||||
{
|
||||
_activeSessions.TryRemove(connectionId, out _);
|
||||
}
|
||||
|
||||
await _testExecutor.CleanupAsync();
|
||||
}
|
||||
}
|
||||
|
||||
public async Task<BatchTestResult> RunBatchTests(
|
||||
List<TestScenario> scenarios,
|
||||
ParameterSet parameters,
|
||||
CancellationToken cancellationToken = default)
|
||||
{
|
||||
var results = new List<TestResult>();
|
||||
var batchId = Guid.NewGuid();
|
||||
|
||||
_logger.LogInformation(
|
||||
"Starting batch test with {Count} scenarios",
|
||||
scenarios.Count
|
||||
);
|
||||
|
||||
for (int i = 0; i < scenarios.Count; i++)
|
||||
{
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
_logger.LogWarning("Batch test cancelled at scenario {Index}", i);
|
||||
break;
|
||||
}
|
||||
|
||||
var scenario = scenarios[i];
|
||||
|
||||
try
|
||||
{
|
||||
var result = await RunSingleTest(scenario, parameters);
|
||||
results.Add(result);
|
||||
|
||||
_logger.LogInformation(
|
||||
"Completed scenario {Index}/{Total}: {Name}",
|
||||
i + 1,
|
||||
scenarios.Count,
|
||||
scenario.Name
|
||||
);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(
|
||||
ex,
|
||||
"Failed scenario {Index}/{Total}: {Name}",
|
||||
i + 1,
|
||||
scenarios.Count,
|
||||
scenario.Name
|
||||
);
|
||||
|
||||
// Continue with remaining scenarios
|
||||
}
|
||||
}
|
||||
|
||||
var batchResult = new BatchTestResult
|
||||
{
|
||||
BatchId = batchId,
|
||||
Parameters = parameters,
|
||||
Results = results,
|
||||
SuccessCount = results.Count(r => r.Status == TestStatus.Completed),
|
||||
FailureCount = results.Count(r => r.Status != TestStatus.Completed),
|
||||
AverageScore = results.Average(r => r.Metrics.OverallScore)
|
||||
};
|
||||
|
||||
return batchResult;
|
||||
}
|
||||
|
||||
public async Task<ComparisonResult> CompareConfigurations(
|
||||
List<ParameterSet> parameterSets,
|
||||
TestScenario scenario)
|
||||
{
|
||||
var results = new Dictionary<string, TestResult>();
|
||||
|
||||
foreach (var parameters in parameterSets)
|
||||
{
|
||||
var result = await RunSingleTest(scenario, parameters);
|
||||
results[parameters.Name] = result;
|
||||
}
|
||||
|
||||
var comparison = new ComparisonResult
|
||||
{
|
||||
Scenario = scenario,
|
||||
Configurations = parameterSets,
|
||||
Results = results,
|
||||
BestConfiguration = results
|
||||
.OrderByDescending(r => r.Value.Metrics.OverallScore)
|
||||
.First()
|
||||
.Key
|
||||
};
|
||||
|
||||
return comparison;
|
||||
}
|
||||
|
||||
// Real-time control methods
|
||||
public async Task StartTestAsync(
|
||||
TestScenario scenario,
|
||||
ParameterSet parameters,
|
||||
string connectionId)
|
||||
{
|
||||
// Run test asynchronously and stream updates via SignalR
|
||||
_ = Task.Run(async () =>
|
||||
{
|
||||
await RunSingleTest(scenario, parameters, connectionId);
|
||||
});
|
||||
}
|
||||
|
||||
public async Task PauseTestAsync(string connectionId)
|
||||
{
|
||||
if (_activeSessions.TryGetValue(connectionId, out var session))
|
||||
{
|
||||
await _testExecutor.PauseAsync();
|
||||
await PublishStatusAsync(session, TestState.Paused);
|
||||
}
|
||||
}
|
||||
|
||||
public async Task ResumeTestAsync(string connectionId)
|
||||
{
|
||||
if (_activeSessions.TryGetValue(connectionId, out var session))
|
||||
{
|
||||
await _testExecutor.ResumeAsync();
|
||||
await PublishStatusAsync(session, TestState.Running);
|
||||
}
|
||||
}
|
||||
|
||||
public async Task StopTestAsync(string connectionId)
|
||||
{
|
||||
if (_activeSessions.TryGetValue(connectionId, out var session))
|
||||
{
|
||||
await _testExecutor.StopAsync();
|
||||
await PublishStatusAsync(session, TestState.Stopped);
|
||||
}
|
||||
}
|
||||
|
||||
public async Task EmergencyStopAsync(string connectionId)
|
||||
{
|
||||
if (_activeSessions.TryGetValue(connectionId, out var session))
|
||||
{
|
||||
await _testExecutor.EmergencyStopAsync();
|
||||
await PublishStatusAsync(session, TestState.EmergencyStopped);
|
||||
}
|
||||
}
|
||||
|
||||
// Helper methods
|
||||
private async Task PublishStateAsync(TestSession session, RobotState state)
|
||||
{
|
||||
if (session.ConnectionId != null)
|
||||
{
|
||||
await _eventPublisher.PublishAsync(
|
||||
"ReceiveState",
|
||||
state,
|
||||
session.ConnectionId
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
private async Task PublishStatusAsync(TestSession session, TestState state)
|
||||
{
|
||||
session.State = state;
|
||||
|
||||
if (session.ConnectionId != null)
|
||||
{
|
||||
await _eventPublisher.PublishAsync(
|
||||
"ReceiveTestStatus",
|
||||
new TestStatus
|
||||
{
|
||||
State = state,
|
||||
Timestamp = DateTime.UtcNow
|
||||
},
|
||||
session.ConnectionId
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
private async Task PublishResultAsync(TestSession session, TestResult result)
|
||||
{
|
||||
if (session.ConnectionId != null)
|
||||
{
|
||||
await _eventPublisher.PublishAsync(
|
||||
"ReceiveTestResult",
|
||||
result,
|
||||
session.ConnectionId
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
private async Task HandleSafetyViolationAsync(
|
||||
TestSession session,
|
||||
SafetyViolation violation)
|
||||
{
|
||||
_logger.LogWarning(
|
||||
"Safety violation: {Type} at {Timestamp}",
|
||||
violation.Type,
|
||||
violation.Timestamp
|
||||
);
|
||||
|
||||
if (session.ConnectionId != null)
|
||||
{
|
||||
await _eventPublisher.PublishAsync(
|
||||
"ReceiveSafetyEvent",
|
||||
violation,
|
||||
session.ConnectionId
|
||||
);
|
||||
}
|
||||
|
||||
// Trigger emergency stop if critical
|
||||
if (violation.Severity == ViolationSeverity.Critical)
|
||||
{
|
||||
await EmergencyStopAsync(session.ConnectionId!);
|
||||
}
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### 2. ParameterManager Service
|
||||
|
||||
**File:** `Application/Services/ParameterManager.cs`
|
||||
|
||||
**Responsibility:** Manage parameter configurations with validation, versioning, and persistence.
|
||||
|
||||
#### Interface Definition
|
||||
|
||||
```csharp
|
||||
public interface IParameterManager
|
||||
{
|
||||
// Configuration management
|
||||
Task<ParameterSet> GetCurrentAsync();
|
||||
Task SetCurrentAsync(ParameterSet parameters);
|
||||
Task<ParameterSet> GetByNameAsync(string name);
|
||||
Task<List<ParameterSet>> GetAllAsync();
|
||||
|
||||
// CRUD operations
|
||||
Task<string> SaveAsync(string name, ParameterSet parameters, string description = "");
|
||||
Task UpdateAsync(string name, ParameterSet parameters);
|
||||
Task DeleteAsync(string name);
|
||||
|
||||
// Validation
|
||||
Task<ValidationResult> ValidateAsync(ParameterSet parameters);
|
||||
ParameterSet ClampToValidRanges(ParameterSet parameters);
|
||||
|
||||
// Versioning
|
||||
Task CreateSnapshotAsync(string name, string description);
|
||||
Task<ParameterSet> RollbackToSnapshotAsync(Guid snapshotId);
|
||||
Task<List<ParameterSnapshot>> GetHistoryAsync(string name);
|
||||
|
||||
// Presets
|
||||
ParameterSet GetDefaultPreset();
|
||||
ParameterSet GetConservativePreset();
|
||||
ParameterSet GetAggressivePreset();
|
||||
ParameterSet GetSmoothPreset();
|
||||
|
||||
// Import/Export
|
||||
Task ExportToJsonAsync(string name, string filePath);
|
||||
Task<ParameterSet> ImportFromJsonAsync(string filePath);
|
||||
}
|
||||
```
|
||||
|
||||
#### Implementation Highlights
|
||||
|
||||
```csharp
|
||||
public class ParameterManager : IParameterManager
|
||||
{
|
||||
private readonly IParameterRepository _repository;
|
||||
private readonly IParameterValidator _validator;
|
||||
private ParameterSet _currentParameters;
|
||||
|
||||
public async Task<ValidationResult> ValidateAsync(ParameterSet parameters)
|
||||
{
|
||||
var result = new ValidationResult { IsValid = true };
|
||||
|
||||
// 1. Validate individual parameter bounds
|
||||
if (!ParameterBounds.KpRange.Contains(parameters.PID.Kp))
|
||||
{
|
||||
result.AddError($"Kp must be between {ParameterBounds.KpRange.Min} and {ParameterBounds.KpRange.Max}");
|
||||
}
|
||||
|
||||
// ... validate all parameters
|
||||
|
||||
// 2. Validate inter-parameter constraints
|
||||
if (parameters.PurePursuit.LookaheadMax <= parameters.PurePursuit.LookaheadMin)
|
||||
{
|
||||
result.AddError("LookaheadMax must be greater than LookaheadMin");
|
||||
}
|
||||
|
||||
if (parameters.Estimator.GoodTrackingBlend > parameters.Estimator.PoorTrackingBlend)
|
||||
{
|
||||
result.AddError("GoodTrackingBlend should be less than PoorTrackingBlend");
|
||||
}
|
||||
|
||||
// 3. Validate against physical limits
|
||||
if (parameters.PID.MaxVelocity > parameters.Physical.MaxLinearVelocity)
|
||||
{
|
||||
result.AddError($"PID MaxVelocity cannot exceed physical limit of {parameters.Physical.MaxLinearVelocity} m/s");
|
||||
}
|
||||
|
||||
// 4. Check for dangerous combinations
|
||||
if (parameters.PID.Kp > 3.0f && parameters.PID.Ki > 1.0f)
|
||||
{
|
||||
result.AddWarning("High Kp and Ki together may cause oscillation");
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
public ParameterSet ClampToValidRanges(ParameterSet parameters)
|
||||
{
|
||||
var clamped = parameters.Clone();
|
||||
|
||||
clamped.PID.Kp = ParameterBounds.KpRange.Clamp(clamped.PID.Kp);
|
||||
clamped.PID.Ki = ParameterBounds.KiRange.Clamp(clamped.PID.Ki);
|
||||
clamped.PID.Kd = ParameterBounds.KdRange.Clamp(clamped.PID.Kd);
|
||||
|
||||
clamped.Estimator.AlphaFilter = ParameterBounds.AlphaFilterRange.Clamp(clamped.Estimator.AlphaFilter);
|
||||
// ... clamp all parameters
|
||||
|
||||
return clamped;
|
||||
}
|
||||
|
||||
public async Task CreateSnapshotAsync(string name, string description)
|
||||
{
|
||||
var current = await GetByNameAsync(name);
|
||||
|
||||
var snapshot = new ParameterSnapshot
|
||||
{
|
||||
Id = Guid.NewGuid(),
|
||||
ParameterSetName = name,
|
||||
ConfigJson = JsonSerializer.Serialize(current),
|
||||
Description = description,
|
||||
CreatedAt = DateTime.UtcNow
|
||||
};
|
||||
|
||||
await _repository.SaveSnapshotAsync(snapshot);
|
||||
}
|
||||
|
||||
public ParameterSet GetDefaultPreset()
|
||||
{
|
||||
return new ParameterSet
|
||||
{
|
||||
Name = "Default",
|
||||
Physical = DefaultConfigurations.Physical,
|
||||
Timing = DefaultConfigurations.Timing,
|
||||
PID = DefaultConfigurations.PID,
|
||||
Estimator = DefaultConfigurations.Estimator,
|
||||
PurePursuit = DefaultConfigurations.PurePursuit,
|
||||
PathFollowing = DefaultConfigurations.PathFollowing,
|
||||
Safety = DefaultConfigurations.Safety
|
||||
};
|
||||
}
|
||||
|
||||
public ParameterSet GetAggressivePreset()
|
||||
{
|
||||
var preset = GetDefaultPreset();
|
||||
preset.Name = "Aggressive";
|
||||
preset.PID.Kp = 1.5f; // High response
|
||||
preset.PID.Ki = 0.2f;
|
||||
preset.PID.Kd = 0.02f; // Low damping
|
||||
preset.PurePursuit.Kdd = 0.8f; // Shorter lookahead → tighter tracking
|
||||
return preset;
|
||||
}
|
||||
|
||||
public ParameterSet GetSmoothPreset()
|
||||
{
|
||||
var preset = GetDefaultPreset();
|
||||
preset.Name = "Smooth";
|
||||
preset.PID.Kp = 0.6f; // Gentle response
|
||||
preset.PID.Ki = 0.05f;
|
||||
preset.PID.Kd = 0.3f; // High damping
|
||||
preset.PurePursuit.Kdd = 1.5f; // Longer lookahead → smoother
|
||||
preset.Estimator.AlphaFilter = 0.2f; // More filtering
|
||||
return preset;
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### 3. MetricAnalyzer Service
|
||||
|
||||
**File:** `Application/Services/MetricAnalyzer.cs`
|
||||
|
||||
**Responsibility:** Calculate, aggregate, and analyze performance metrics.
|
||||
|
||||
#### Interface Definition
|
||||
|
||||
```csharp
|
||||
public interface IMetricAnalyzer
|
||||
{
|
||||
// Core analysis
|
||||
Task<TestMetrics> AnalyzeAsync(ExecutionResult executionResult);
|
||||
Task<TrackingAccuracyMetrics> CalculateTrackingAccuracyAsync(List<ControlCycleData> data, Path referencePath);
|
||||
Task<SmoothnessMetrics> CalculateSmoothnessAsync(List<ControlCycleData> data);
|
||||
Task<EfficiencyMetrics> CalculateEfficiencyAsync(ExecutionResult result, Path referencePath);
|
||||
|
||||
// Statistical analysis
|
||||
StatisticalSummary GetStatistics(List<TestResult> results);
|
||||
TrendAnalysis AnalyzeTrends(List<TestResult> historicalResults);
|
||||
|
||||
// Evaluation
|
||||
PassFailResult EvaluateAgainstCriteria(TestMetrics metrics, AcceptanceCriteria criteria);
|
||||
float CalculateOverallScore(TestMetrics metrics, ScoringWeights weights);
|
||||
|
||||
// Comparison
|
||||
ComparisonReport CompareResults(TestResult baseline, TestResult current);
|
||||
RankingReport RankConfigurations(List<TestResult> results, ScoringWeights weights);
|
||||
}
|
||||
```
|
||||
|
||||
#### Key Calculation Methods
|
||||
|
||||
```csharp
|
||||
public class MetricAnalyzer : IMetricAnalyzer
|
||||
{
|
||||
public async Task<TrackingAccuracyMetrics> CalculateTrackingAccuracyAsync(
|
||||
List<ControlCycleData> data,
|
||||
Path referencePath)
|
||||
{
|
||||
var cteValues = new List<float>();
|
||||
var headingErrors = new List<float>();
|
||||
|
||||
foreach (var cycle in data)
|
||||
{
|
||||
// Calculate cross-track error
|
||||
var closestPoint = referencePath.GetClosestPoint(cycle.Position);
|
||||
var cte = Vector2.Distance(cycle.Position, closestPoint.Position);
|
||||
cteValues.Add(cte);
|
||||
|
||||
// Calculate heading error
|
||||
var pathHeading = closestPoint.Tangent.Angle();
|
||||
var headingError = NormalizeAngle(cycle.Heading - pathHeading);
|
||||
headingErrors.Add(Math.Abs(headingError));
|
||||
}
|
||||
|
||||
// Calculate RMS errors
|
||||
var cteRMS = CalculateRMS(cteValues);
|
||||
var ctePeak = cteValues.Max();
|
||||
var headingRMS = CalculateRMS(headingErrors);
|
||||
|
||||
// Goal accuracy (last 10 data points)
|
||||
var finalPoints = data.TakeLast(10).ToList();
|
||||
var goalPosition = referencePath.Points.Last().Position;
|
||||
var goalPositionError = finalPoints
|
||||
.Average(p => Vector2.Distance(p.Position, goalPosition));
|
||||
|
||||
return new TrackingAccuracyMetrics
|
||||
{
|
||||
CrossTrackErrorRMS = cteRMS,
|
||||
CrossTrackErrorPeak = ctePeak,
|
||||
CrossTrackErrorMean = cteValues.Average(),
|
||||
CrossTrackErrorStdDev = CalculateStdDev(cteValues),
|
||||
HeadingErrorRMS = headingRMS,
|
||||
HeadingErrorPeak = headingErrors.Max(),
|
||||
GoalPositionError = goalPositionError
|
||||
};
|
||||
}
|
||||
|
||||
public async Task<SmoothnessMetrics> CalculateSmoothnessAsync(
|
||||
List<ControlCycleData> data)
|
||||
{
|
||||
var velocities = data.Select(d => d.LinearVelocity).ToList();
|
||||
var angularVelocities = data.Select(d => d.AngularVelocity).ToList();
|
||||
|
||||
var dt = data[1].TimeFromStart - data[0].TimeFromStart;
|
||||
|
||||
// Calculate accelerations
|
||||
var accelerations = new List<float>();
|
||||
for (int i = 1; i < velocities.Count; i++)
|
||||
{
|
||||
var accel = (velocities[i] - velocities[i-1]) / dt;
|
||||
accelerations.Add(accel);
|
||||
}
|
||||
|
||||
// Calculate jerks
|
||||
var jerks = new List<float>();
|
||||
for (int i = 1; i < accelerations.Count; i++)
|
||||
{
|
||||
var jerk = (accelerations[i] - accelerations[i-1]) / dt;
|
||||
jerks.Add(Math.Abs(jerk));
|
||||
}
|
||||
|
||||
// Angular jerk
|
||||
var angularAccelerations = new List<float>();
|
||||
for (int i = 1; i < angularVelocities.Count; i++)
|
||||
{
|
||||
var angAccel = (angularVelocities[i] - angularVelocities[i-1]) / dt;
|
||||
angularAccelerations.Add(angAccel);
|
||||
}
|
||||
|
||||
var angularJerks = new List<float>();
|
||||
for (int i = 1; i < angularAccelerations.Count; i++)
|
||||
{
|
||||
var angJerk = (angularAccelerations[i] - angularAccelerations[i-1]) / dt;
|
||||
angularJerks.Add(Math.Abs(angJerk));
|
||||
}
|
||||
|
||||
return new SmoothnessMetrics
|
||||
{
|
||||
MaxJerk = jerks.Max(),
|
||||
AverageJerk = jerks.Average(),
|
||||
MaxAngularJerk = angularJerks.Max(),
|
||||
VelocityStdDev = CalculateStdDev(velocities),
|
||||
AccelerationStdDev = CalculateStdDev(accelerations)
|
||||
};
|
||||
}
|
||||
|
||||
public float CalculateOverallScore(TestMetrics metrics, ScoringWeights weights)
|
||||
{
|
||||
float score = 100f;
|
||||
|
||||
// Tracking accuracy penalties (weighted 50%)
|
||||
score -= weights.TrackingAccuracy * (
|
||||
NormalizePenalty(metrics.CrossTrackErrorRMS, 0.10f, 20f) +
|
||||
NormalizePenalty(metrics.HeadingErrorRMS, 10f * Deg2Rad, 20f) +
|
||||
NormalizePenalty(metrics.GoalPositionError, 0.05f, 10f)
|
||||
);
|
||||
|
||||
// Smoothness penalties (weighted 30%)
|
||||
score -= weights.Smoothness * (
|
||||
NormalizePenalty(metrics.MaxJerk, 5.0f, 15f) +
|
||||
NormalizePenalty(metrics.MaxAngularJerk, 10.0f, 15f)
|
||||
);
|
||||
|
||||
// Efficiency penalties (weighted 20%)
|
||||
score -= weights.Efficiency * (
|
||||
NormalizePenalty(metrics.PathLengthRatio - 1.0f, 0.15f, 20f)
|
||||
);
|
||||
|
||||
return Math.Max(0, score);
|
||||
}
|
||||
|
||||
private float NormalizePenalty(float actual, float threshold, float maxPenalty)
|
||||
{
|
||||
if (actual <= threshold) return 0;
|
||||
|
||||
var excess = actual - threshold;
|
||||
var penalty = (excess / threshold) * maxPenalty;
|
||||
return Math.Min(penalty, maxPenalty);
|
||||
}
|
||||
|
||||
private float CalculateRMS(List<float> values)
|
||||
{
|
||||
return MathF.Sqrt(values.Average(v => v * v));
|
||||
}
|
||||
|
||||
private float CalculateStdDev(List<float> values)
|
||||
{
|
||||
var mean = values.Average();
|
||||
var variance = values.Average(v => (v - mean) * (v - mean));
|
||||
return MathF.Sqrt(variance);
|
||||
}
|
||||
|
||||
private float NormalizeAngle(float angle)
|
||||
{
|
||||
while (angle > MathF.PI) angle -= 2 * MathF.PI;
|
||||
while (angle < -MathF.PI) angle += 2 * MathF.PI;
|
||||
return angle;
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### 4. ReportGenerator Service
|
||||
|
||||
**File:** `Application/Services/ReportGenerator.cs`
|
||||
|
||||
**Responsibility:** Generate reports and export data in various formats.
|
||||
|
||||
#### Interface Definition
|
||||
|
||||
```csharp
|
||||
public interface IReportGenerator
|
||||
{
|
||||
// Report generation
|
||||
Task<byte[]> GeneratePdfReportAsync(TestResult result);
|
||||
Task<string> GenerateHtmlReportAsync(TestResult result);
|
||||
Task<string> GenerateMarkdownSummaryAsync(TestResult result);
|
||||
|
||||
// Data export
|
||||
Task ExportToCsvAsync(TestResult result, string filePath);
|
||||
Task ExportToMatlabAsync(TestResult result, string filePath);
|
||||
Task ExportRawDataAsync(TestResult result, string filePath);
|
||||
|
||||
// Batch reports
|
||||
Task<string> GenerateComparisonReportAsync(ComparisonResult comparison);
|
||||
Task<string> GenerateBatchSummaryAsync(BatchTestResult batchResult);
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## LAYER 3: DOMAIN LOGIC
|
||||
|
||||
Domain logic contains the core business rules and algorithms. This layer is framework-agnostic and contains no infrastructure dependencies.
|
||||
|
||||
---
|
||||
|
||||
### 1. Test Execution Engine
|
||||
|
||||
**File:** `Domain/Services/TestExecutor.cs`
|
||||
|
||||
#### Interface Definition
|
||||
|
||||
```csharp
|
||||
public interface ITestExecutor
|
||||
{
|
||||
// Lifecycle
|
||||
Task InitializeAsync(TestScenario scenario, ParameterSet parameters);
|
||||
Task<ExecutionResult> ExecuteAsync(
|
||||
Action<RobotState>? onStateUpdate = null,
|
||||
Action<SafetyViolation>? onSafetyViolation = null
|
||||
);
|
||||
Task CleanupAsync();
|
||||
|
||||
// Control
|
||||
Task PauseAsync();
|
||||
Task ResumeAsync();
|
||||
Task StopAsync();
|
||||
Task EmergencyStopAsync();
|
||||
|
||||
// State
|
||||
ExecutionState GetCurrentState();
|
||||
float GetProgress();
|
||||
}
|
||||
```
|
||||
|
||||
#### Implementation Core Logic
|
||||
|
||||
```csharp
|
||||
public class TestExecutor : ITestExecutor
|
||||
{
|
||||
private readonly IPIDController _pidController;
|
||||
private readonly IVelocityEstimator _velocityEstimator;
|
||||
private readonly IPurePursuitController _purePursuitController;
|
||||
private readonly IMotorDriver _motorDriver;
|
||||
private readonly IEncoderReader _encoderReader;
|
||||
private readonly IRobotStateManager _stateManager;
|
||||
private readonly ISafetyMonitor _safetyMonitor;
|
||||
private readonly IDataLogger _dataLogger;
|
||||
|
||||
private Path _referencePath;
|
||||
private ParameterSet _parameters;
|
||||
private ExecutionState _state;
|
||||
private CancellationTokenSource _cts;
|
||||
|
||||
public async Task<ExecutionResult> ExecuteAsync(
|
||||
Action<RobotState>? onStateUpdate = null,
|
||||
Action<SafetyViolation>? onSafetyViolation = null)
|
||||
{
|
||||
_state = ExecutionState.Running;
|
||||
_cts = new CancellationTokenSource();
|
||||
|
||||
var startTime = DateTime.UtcNow;
|
||||
var result = new ExecutionResult
|
||||
{
|
||||
StartTime = startTime,
|
||||
Status = TestStatus.Running
|
||||
};
|
||||
|
||||
try
|
||||
{
|
||||
// Main control loop (50Hz)
|
||||
var dt = 1.0f / _parameters.Timing.ControlLoopFrequency;
|
||||
var cycleTime = TimeSpan.FromSeconds(dt);
|
||||
|
||||
while (!IsGoalReached() && !_cts.Token.IsCancellationRequested)
|
||||
{
|
||||
var cycleStart = DateTime.UtcNow;
|
||||
|
||||
// 1. Read sensors
|
||||
var encoderData = _encoderReader.ReadEncoders();
|
||||
_stateManager.UpdateFromEncoders(encoderData, dt);
|
||||
var robotState = _stateManager.GetCurrentPose();
|
||||
var robotTwist = _stateManager.GetCurrentTwist();
|
||||
|
||||
// 2. Calculate distance to goal
|
||||
var goalPosition = _referencePath.Points.Last().Position;
|
||||
var distanceToGoal = Vector2.Distance(robotState.Position, goalPosition);
|
||||
|
||||
// 3. PID: distance → v_max
|
||||
float vMax;
|
||||
if (distanceToGoal > 5.0f)
|
||||
{
|
||||
vMax = _parameters.PID.MaxVelocity;
|
||||
}
|
||||
else
|
||||
{
|
||||
var pidOutput = _pidController.Calculate(distanceToGoal, dt);
|
||||
vMax = Math.Max(pidOutput, _parameters.PID.MinVelocity);
|
||||
}
|
||||
|
||||
// 4. Velocity Estimator: estimate v_hybrid
|
||||
var vCmd = vMax; // Current command
|
||||
var vEncoder = robotTwist.Linear;
|
||||
var vHybrid = _velocityEstimator.EstimateVelocity(vCmd, vEncoder, dt);
|
||||
var confidence = _velocityEstimator.GetConfidence();
|
||||
|
||||
// 5. Pure Pursuit: (v_hybrid, path) → ω
|
||||
var omega = _purePursuitController.Calculate(
|
||||
robotState,
|
||||
vHybrid,
|
||||
confidence,
|
||||
_referencePath
|
||||
);
|
||||
|
||||
// 6. Combine velocities
|
||||
var vLinear = Math.Min(vMax, _parameters.Physical.MaxLinearVelocity);
|
||||
var omegaClamped = Math.Clamp(
|
||||
omega,
|
||||
-_parameters.Physical.MaxAngularVelocity,
|
||||
_parameters.Physical.MaxAngularVelocity
|
||||
);
|
||||
|
||||
// 7. Convert to wheel commands
|
||||
var (leftWheel, rightWheel) = DifferentialKinematics.
|
||||
683
docs/RobotApp-TunningNav/IMPLEMENTATION_PROGRESS.md
Normal file
683
docs/RobotApp-TunningNav/IMPLEMENTATION_PROGRESS.md
Normal file
@@ -0,0 +1,683 @@
|
||||
# Robot Navigation Tuning System - Implementation Progress
|
||||
|
||||
**Last Updated:** 2026-01-27
|
||||
**Status:** Phase 1-4 Completed, Phase 1 Integration Completed, Entity Framework Configuration Completed
|
||||
|
||||
---
|
||||
|
||||
## 📋 TỔNG QUAN DỰ ÁN
|
||||
|
||||
Hệ thống Robot Navigation Tuning được thiết kế để tối ưu hóa các thông số điều khiển cho differential drive mobile robots. Hệ thống bao gồm:
|
||||
|
||||
- **Backend Project**: `RobotNet10.NavigationTune` - Class library với SignalR support
|
||||
- **Frontend Project**: `RobotNet10.NavigationTuneUI` - Blazor components
|
||||
- **Test Project**: `RobotNet10.NavigationTune.Test` - Unit tests
|
||||
|
||||
---
|
||||
|
||||
## ✅ PHẦN ĐÃ HOÀN THÀNH
|
||||
|
||||
### 1. BACKEND PROJECT (RobotNet10.NavigationTune) - 100%
|
||||
|
||||
#### 1.1 Core Navigation Classes ✅
|
||||
- **PID.cs** - Incremental PID controller với Kp, Ki, Kd
|
||||
- **CircularBuffer.cs** - Data buffering utility
|
||||
- **MotorDynamicsModel.cs** - First-order motor dynamics model
|
||||
- **PurePursuitSimplified.cs** - Simplified Pure Pursuit với PathPoint DTOs
|
||||
- **VelocityEstimatorSimplified.cs** - Velocity estimator với adaptive blending
|
||||
|
||||
**Location:** `srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Navigation/Core/`
|
||||
|
||||
#### 1.2 Domain Models ✅
|
||||
- **NavigationParameterSet.cs** - Complete parameter set với:
|
||||
- PID configs (Move & Rotate)
|
||||
- Pure Pursuit config
|
||||
- Velocity Estimator config
|
||||
- Signal Processing config
|
||||
- Motor Dynamics config
|
||||
- Navigation limits
|
||||
- **TestMetrics.cs** - Metrics definitions (Tracking, Smoothness, Efficiency)
|
||||
- **TestRun.cs** - Test execution records với status tracking
|
||||
- **TestScenario.cs** - Abstract base class cho test scenarios
|
||||
- **TelemetryData.cs** - Real-time telemetry data model
|
||||
- **Pose2D.cs, Twist2D.cs** - Geometry models
|
||||
- **TestScenarioEntity.cs** - EF Core entity cho abstract class persistence
|
||||
|
||||
**Location:** `srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Models/`
|
||||
|
||||
#### 1.3 Test Scenarios ✅
|
||||
- **StraightLineScenario.cs** - Straight line path scenario
|
||||
- **CircleScenario.cs** - Circular path scenario
|
||||
|
||||
**Location:** `srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Scenarios/`
|
||||
|
||||
#### 1.4 Execution Layer ✅
|
||||
- **TestExecutor.cs** - Test execution với 50Hz control loop
|
||||
- Integrates PID, Pure Pursuit, Velocity Estimator
|
||||
- Safety monitoring
|
||||
- Telemetry collection
|
||||
- **TuningNavigation.cs** - Wrapper với SignalR integration
|
||||
- **LocalizationAdapter.cs** - Adapter interface cho ILocalization
|
||||
- **VelocityControllerAdapter.cs** - Adapter interface cho IVelocityController
|
||||
|
||||
**Location:** `srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Execution/`
|
||||
|
||||
#### 1.5 Services ✅
|
||||
- **MetricsCalculator.cs** - Tính toán metrics:
|
||||
- Tracking Accuracy (CTE RMS, Heading Error RMS)
|
||||
- Smoothness (Jerk, Velocity StdDev)
|
||||
- Efficiency (Path Length Ratio, Completion Time)
|
||||
- Overall Score calculation
|
||||
- **SafetyMonitor.cs** - Safety monitoring:
|
||||
- Cross-track error limits
|
||||
- Heading error limits
|
||||
- Velocity limits
|
||||
- Sustained tracking error detection
|
||||
- **ParameterManager.cs** - Parameter management:
|
||||
- CRUD operations
|
||||
- Validation logic
|
||||
- Presets (Default, Aggressive, Smooth)
|
||||
- **TuningOrchestrator.cs** - Orchestration logic:
|
||||
- Single test execution
|
||||
- Batch test execution
|
||||
- Configuration comparison
|
||||
- Real-time test control
|
||||
|
||||
**Location:** `srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Services/`
|
||||
|
||||
#### 1.6 Data Layer ✅
|
||||
- **TuningDbContext.cs** - EF Core context (SQLite/PostgreSQL)
|
||||
- ParameterSets DbSet với JSON conversion cho complex types
|
||||
- TestScenarios DbSet (as TestScenarioEntity)
|
||||
- TestRuns DbSet với proper relationships
|
||||
- TestMetrics DbSet với one-to-one relationship
|
||||
- SafetyViolations DbSet với cascade delete
|
||||
- Proper indexes và enum conversions
|
||||
- **TestRepository.cs** - Repository cho test runs
|
||||
- **ScenarioRepository.cs** - Repository cho test scenarios
|
||||
- **DefaultDataSeeder.cs** - Seed default data
|
||||
- **TestScenarioEntity.cs** - Entity cho abstract TestScenario persistence
|
||||
|
||||
**Location:** `srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Data/`
|
||||
|
||||
**Entity Framework Configuration:**
|
||||
- ✅ JSON conversion cho complex types (PIDConfig, PurePursuitConfig, etc.)
|
||||
- ✅ Proper foreign key relationships
|
||||
- ✅ Enum conversions (TestStatus, ViolationType, ViolationSeverity)
|
||||
- ✅ Optimized indexes cho performance
|
||||
- ✅ Cascade delete configuration
|
||||
- ✅ MaxLength constraints cho string properties
|
||||
|
||||
#### 1.7 SignalR ✅
|
||||
- **TuningHub.cs** - SignalR hub với methods:
|
||||
- JoinTestSession
|
||||
- LeaveTestSession
|
||||
- **DTOs**: TelemetryUpdateDto, TestStatusUpdateDto, SafetyEventDto
|
||||
|
||||
**Location:** `srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Hubs/`
|
||||
|
||||
#### 1.8 Extensions ✅
|
||||
- **ServiceCollectionExtensions.cs** - DI setup methods:
|
||||
- `AddNavigationTuning()` - Base services với proper DI registration
|
||||
- `AddNavigationTuningWithRobot()` - With robot adapters
|
||||
- ✅ Fixed: ITestExecutor properly registered as interface
|
||||
|
||||
**Location:** `srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Extensions/`
|
||||
|
||||
#### 1.9 Interfaces ✅
|
||||
- **ITuningNavigation.cs** - Navigation wrapper interface
|
||||
- **ITestExecutor.cs** - Test executor interface
|
||||
- **IMetricsCalculator.cs** - Metrics calculator interface
|
||||
- **IParameterManager.cs** - Parameter manager interface
|
||||
- **ITestRepository.cs** - Test repository interface
|
||||
- **ITuningOrchestrator.cs** - Orchestrator interface
|
||||
|
||||
**Location:** `srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Interfaces/`
|
||||
|
||||
#### 1.10 Shared Project ✅
|
||||
- **RobotNet10.NavigationTune.Shared** - Common models, interfaces, DTOs
|
||||
- Models: NavigationParameterSet, TestRun, TestMetrics, TestScenario, etc.
|
||||
- Interfaces: ITuningNavigation, ITestExecutor, IMetricsCalculator, etc.
|
||||
- Hubs: TuningHubDtos (TelemetryUpdateDto, TestStatusUpdateDto, SafetyEventDto)
|
||||
- Decouples frontend from backend dependencies
|
||||
|
||||
**Location:** `srcs/RobotNet10/Shared/RobotNet10.NavigationTune.Shared/`
|
||||
|
||||
#### 1.11 REST API Controllers ✅
|
||||
- **ParameterSetsController.cs** - CRUD operations cho parameter sets
|
||||
- **ScenariosController.cs** - CRUD operations cho test scenarios
|
||||
- **TestRunsController.cs** - Query operations cho test history
|
||||
- **TuningController.cs** - Test execution & control endpoints
|
||||
|
||||
**Location:** `srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Controllers/`
|
||||
|
||||
---
|
||||
|
||||
### 2. FRONTEND PROJECT (RobotNet10.NavigationTuneUI) - 100%
|
||||
|
||||
#### 2.1 SignalR Client ✅
|
||||
- **TuningHubClient.cs** - SignalR client với:
|
||||
- Auto-reconnect
|
||||
- Events: TelemetryUpdated, TestStatusUpdated, SafetyEventReceived
|
||||
- Join/Leave test session methods
|
||||
|
||||
**Location:** `srcs/RobotNet10/Components/RobotNet10.NavigationTuneUI/Clients/`
|
||||
|
||||
#### 2.2 Blazor Components ✅
|
||||
- **TuningDashboard.razor** - Main dashboard integrating all components
|
||||
- **ParameterTuningEditor.razor** - Parameter editor với tabs:
|
||||
- PID Controllers (Move & Rotate)
|
||||
- Pure Pursuit
|
||||
- Velocity Estimator
|
||||
- Motor Dynamics
|
||||
- Navigation Limits
|
||||
- **RealTimeMonitor.razor** - Real-time telemetry display
|
||||
- **TestExecutionControl.razor** - Test controls (Start, Pause, Resume, Stop, Emergency Stop)
|
||||
- **MetricsVisualization.razor** - Metrics display với tabs
|
||||
|
||||
**Location:** `srcs/RobotNet10/Components/RobotNet10.NavigationTuneUI/Components/`
|
||||
|
||||
#### 2.3 Project Setup ✅
|
||||
- Dependencies: MudBlazor, SignalR.Client
|
||||
- Project reference đến NavigationTune.Shared (not NavigationTune backend)
|
||||
- Build thành công
|
||||
|
||||
#### 2.4 API Service ✅
|
||||
- **TuningApiService.cs** - HTTP client service cho REST API calls
|
||||
- Load scenarios và parameter sets
|
||||
- Execute tests
|
||||
- Control test execution
|
||||
- Query test history
|
||||
|
||||
**Location:** `srcs/RobotNet10/Components/RobotNet10.NavigationTuneUI/Services/`
|
||||
|
||||
---
|
||||
|
||||
### 3. TEST PROJECT (RobotNet10.NavigationTune.Test) - 100%
|
||||
|
||||
#### 3.1 Test Infrastructure ✅
|
||||
- **TestHelpers.cs** - Helper methods cho test data creation
|
||||
- xUnit framework
|
||||
- FluentAssertions
|
||||
- Moq (ready for mocking)
|
||||
- EF Core InMemory database
|
||||
|
||||
**Location:** `srcs/RobotNet10/Tests/RobotNet10.NavigationTune.Test/Helpers/`
|
||||
|
||||
#### 3.2 Unit Tests ✅
|
||||
|
||||
**Navigation Core Tests (25 tests):**
|
||||
- **PIDTests.cs** - 10 tests
|
||||
- Constructor, PID_step với P/I/D terms
|
||||
- Clamping, Reset, WithKp/Ki/Kd
|
||||
- **MotorDynamicsModelTests.cs** - 9 tests
|
||||
- Constructor, PredictVelocity scenarios
|
||||
- GetSettlingTime, GetRiseTime
|
||||
- **PurePursuitSimplifiedTests.cs** - 6 tests
|
||||
- SetPath, CalculateAngularVelocity
|
||||
- GetCurrentLookahead, confidence handling
|
||||
|
||||
**Services Tests (21 tests):**
|
||||
- **MetricsCalculatorTests.cs** - 7 tests
|
||||
- Empty telemetry, perfect tracking, errors
|
||||
- Smoothness, efficiency, overall score
|
||||
- **ParameterManagerTests.cs** - 9 tests
|
||||
- Validation với valid/invalid parameters
|
||||
- Presets (Default, Aggressive, Smooth)
|
||||
- **SafetyMonitorTests.cs** - 5 tests
|
||||
- CheckSafety scenarios, Reset, GetViolations
|
||||
|
||||
**Scenarios Tests (12 tests):**
|
||||
- **StraightLineScenarioTests.cs** - 6 tests
|
||||
- GenerateReferencePath, IsGoalReached, GetGoalPose
|
||||
- **CircleScenarioTests.cs** - 6 tests
|
||||
- GenerateReferencePath, radius validation, IsGoalReached
|
||||
|
||||
**Total: 59 tests - All Passing ✅**
|
||||
|
||||
**Location:** `srcs/RobotNet10/Tests/RobotNet10.NavigationTune.Test/`
|
||||
|
||||
---
|
||||
|
||||
## ⏳ PHẦN CHƯA HOÀN THÀNH
|
||||
|
||||
### 1. DATABASE SETUP - CRITICAL ⚠️
|
||||
|
||||
#### 1.1 EF Core Migrations
|
||||
- ❌ Chưa tạo migrations
|
||||
- ❌ Chưa có script để init database
|
||||
- ❌ Chưa run migrations on startup
|
||||
|
||||
**Action Required:**
|
||||
```bash
|
||||
cd srcs/RobotNet10/Commons/RobotNet10.NavigationTune
|
||||
dotnet ef migrations add InitialCreate
|
||||
dotnet ef database update
|
||||
```
|
||||
|
||||
#### 1.2 Connection String Configuration
|
||||
- ❌ Chưa add connection string vào `appsettings.json`
|
||||
- ❌ Chưa configure database path
|
||||
|
||||
**Action Required:**
|
||||
Add to `RobotApp/appsettings.json`:
|
||||
```json
|
||||
{
|
||||
"ConnectionStrings": {
|
||||
"TuningConnection": "Data Source=navigation_tuning.db"
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
#### 1.3 Database Initialization
|
||||
- ❌ Chưa seed default data on startup
|
||||
- ❌ Chưa ensure database created
|
||||
|
||||
**Action Required:**
|
||||
Call `DefaultDataSeeder.SeedAsync()` on application startup
|
||||
|
||||
---
|
||||
|
||||
### 2. BACKEND INTEGRATION VÀO ROBOTAPP - ✅ COMPLETED
|
||||
|
||||
#### 2.1 Service Registration ✅
|
||||
- ✅ Added `AddNavigationTuningWithRobot()` trong `RobotApp/Program.cs`
|
||||
- ✅ Created concrete adapters cho `ILocalization` và `IVelocityController`
|
||||
- ✅ Fixed DI registration: ITestExecutor properly registered
|
||||
|
||||
**Implementation:**
|
||||
```csharp
|
||||
// In RobotApp/Program.cs
|
||||
builder.Services.AddNavigationTuningWithRobot(
|
||||
options => options.UseSqlite(navTuneConnectionString, ...)
|
||||
);
|
||||
|
||||
// Register adapters
|
||||
builder.Services.AddScoped<ILocalizationProvider>(sp => ...);
|
||||
builder.Services.AddScoped<IVelocityProvider>(sp => ...);
|
||||
```
|
||||
|
||||
#### 2.2 SignalR Hub Mapping ✅
|
||||
- ✅ Mapped `TuningHub` endpoint to `/tuninghub`
|
||||
|
||||
**Implementation:**
|
||||
```csharp
|
||||
// In RobotApp/Program.cs
|
||||
app.MapHub<TuningHub>("/tuninghub");
|
||||
```
|
||||
|
||||
#### 2.3 Database Initialization ✅
|
||||
- ✅ Database initialization on startup (migrations handled separately by user)
|
||||
- ✅ Default data seeding configured
|
||||
|
||||
**Note:** User handles migrations separately as requested
|
||||
|
||||
---
|
||||
|
||||
### 3. FRONTEND INTEGRATION VÀO ROBOTAPP.CLIENT - ✅ COMPLETED
|
||||
|
||||
#### 3.1 Service Registration ✅
|
||||
- ✅ Registered `TuningHubClient` trong `RobotApp.Client/Program.cs`
|
||||
- ✅ Registered `TuningApiService` trong `RobotApp.Client/Program.cs`
|
||||
|
||||
**Implementation:**
|
||||
```csharp
|
||||
// In RobotApp.Client/Program.cs
|
||||
builder.Services.AddScoped<TuningHubClient>();
|
||||
builder.Services.AddScoped<TuningApiService>();
|
||||
```
|
||||
|
||||
#### 3.2 Routing ✅
|
||||
- ✅ Added route `/navigation/tuning` cho TuningDashboard
|
||||
|
||||
**Implementation:**
|
||||
Created `RobotApp.Client/Pages/Navigation/Tuning.razor`:
|
||||
```razor
|
||||
@page "/navigation/tuning"
|
||||
@using RobotNet10.NavigationTuneUI.Components
|
||||
<TuningDashboard />
|
||||
```
|
||||
|
||||
#### 3.3 Navigation Menu ✅
|
||||
- ✅ Added menu item vào navigation menu
|
||||
|
||||
**Implementation:**
|
||||
Added to `RobotApp.Client/Extensions.cs`:
|
||||
```csharp
|
||||
new("mdi-tune", "/navigation/tuning", "Navigation Tuning", NavLinkMatch.All)
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### 4. API CONTROLLERS - ✅ COMPLETED
|
||||
|
||||
#### 4.1 REST API Endpoints ✅
|
||||
- ✅ REST API controllers implemented
|
||||
- ✅ Endpoints cho:
|
||||
- CRUD parameter sets
|
||||
- CRUD test scenarios
|
||||
- Query test history
|
||||
- Execute tests
|
||||
- Control test execution (pause, resume, stop, emergency stop)
|
||||
- Batch tests
|
||||
- Configuration comparison
|
||||
|
||||
**Implemented Controllers:**
|
||||
- ✅ `ParameterSetsController` - CRUD parameter sets
|
||||
- ✅ `ScenariosController` - CRUD scenarios
|
||||
- ✅ `TestRunsController` - Query test history với filters
|
||||
- ✅ `TuningController` - Execute tests, control execution
|
||||
|
||||
**Location:** `srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Controllers/`
|
||||
|
||||
---
|
||||
|
||||
### 5. FRONTEND API INTEGRATION - ✅ COMPLETED
|
||||
|
||||
#### 5.1 API Service Classes ✅
|
||||
- ✅ Created `TuningApiService.cs` trong `RobotNet10.NavigationTuneUI/Services/`
|
||||
- ✅ Components connected với backend APIs
|
||||
- ✅ Load scenarios/parameter sets từ database
|
||||
- ✅ Test execution flow implemented
|
||||
- ✅ Real-time updates via SignalR
|
||||
|
||||
**Implementation:**
|
||||
- `TuningApiService.cs` - HTTP client service với all API methods
|
||||
- `TuningDashboard.razor` - Updated to use TuningApiService
|
||||
- `TestExecutionControl.razor` - Updated to receive actual models
|
||||
|
||||
---
|
||||
|
||||
### 6. ADVANCED UI FEATURES - MEDIUM PRIORITY
|
||||
|
||||
#### 6.1 Charts & Visualization
|
||||
- ❌ Chưa có charts cho telemetry history
|
||||
- ❌ Chưa có path visualization trên map
|
||||
- ❌ Chưa có real-time path plotting
|
||||
|
||||
**Recommended Libraries:**
|
||||
- MudBlazor Charts
|
||||
- Chart.js
|
||||
- Plotly.NET
|
||||
|
||||
#### 6.2 Test History Viewer
|
||||
- ✅ **TestHistoryViewer.razor** - Component xem lịch sử test (danh sách TestRun, refresh, xem chi tiết, xóa, hiển thị metrics trong dashboard)
|
||||
- ❌ Chưa có filters theo scenario/parameter set/date range trên UI (API đã có)
|
||||
- ❌ Chưa có comparison tools
|
||||
|
||||
#### 6.3 Comparison Tools
|
||||
- ❌ Chưa có tools để so sánh parameter sets
|
||||
- ❌ Chưa có side-by-side metrics comparison
|
||||
|
||||
---
|
||||
|
||||
### 7. TESTING - HIGH PRIORITY
|
||||
|
||||
#### 7.1 Additional Unit Tests
|
||||
- ❌ VelocityEstimatorSimplifiedTests - Pending
|
||||
- ❌ TestExecutorTests - Requires mocking
|
||||
- ❌ TuningNavigationTests - Requires mocking
|
||||
- ❌ TuningOrchestratorTests - Requires mocking
|
||||
|
||||
#### 7.2 Integration Tests
|
||||
- ❌ End-to-end test execution
|
||||
- ❌ Database operations
|
||||
- ❌ SignalR communication
|
||||
|
||||
#### 7.3 Real Robot Testing
|
||||
- ❌ Chưa test trên robot thật
|
||||
- ❌ Chưa validate với real hardware
|
||||
|
||||
---
|
||||
|
||||
## 📊 TIẾN ĐỘ TỔNG THỂ
|
||||
|
||||
| Component | Hoàn thành | Chưa hoàn thiện | Priority |
|
||||
|-----------|------------|-----------------|----------|
|
||||
| Backend Core | 100% | Migrations (user handles) | High |
|
||||
| Frontend Components | 100% | Advanced UI features | Medium |
|
||||
| Unit Tests | 100% | Additional integration tests | High |
|
||||
| Backend Integration | 100% | ✅ Completed | - |
|
||||
| Frontend Integration | 100% | ✅ Completed | - |
|
||||
| Database Setup | 90% | Migrations (user handles) | **CRITICAL** |
|
||||
| Entity Framework Config | 100% | ✅ Completed | - |
|
||||
| API Controllers | 100% | ✅ Completed | - |
|
||||
| Frontend API Integration | 100% | ✅ Completed | - |
|
||||
| Advanced Features | 0% | Charts, History viewer | Medium |
|
||||
|
||||
---
|
||||
|
||||
## 🚀 CÔNG VIỆC CẦN LÀM TIẾP THEO (Priority Order)
|
||||
|
||||
### Phase 1: Critical Integration - ✅ COMPLETED
|
||||
|
||||
#### 1. Database Setup ✅
|
||||
1. ✅ Entity Framework configuration completed
|
||||
2. ✅ JSON conversion cho complex types
|
||||
3. ✅ Proper relationships và indexes
|
||||
4. ⏳ Migrations (user handles separately)
|
||||
5. ✅ Default data seeding configured
|
||||
|
||||
**Status:** Entity configuration complete, migrations pending user action
|
||||
|
||||
#### 2. Backend Integration ✅
|
||||
1. ✅ Added `AddNavigationTuningWithRobot()` trong `RobotApp/Program.cs`
|
||||
2. ✅ Mapped `TuningHub` endpoint to `/tuninghub`
|
||||
3. ✅ Created concrete adapters cho robot hardware
|
||||
4. ✅ Configured database initialization
|
||||
5. ✅ Fixed DI registration issues (ITestExecutor)
|
||||
|
||||
**Status:** Fully integrated
|
||||
|
||||
#### 3. Frontend Integration ✅
|
||||
1. ✅ Registered `TuningHubClient` và `TuningApiService` trong `RobotApp.Client/Program.cs`
|
||||
2. ✅ Added route `/navigation/tuning`
|
||||
3. ✅ Added menu item
|
||||
|
||||
**Status:** Fully integrated
|
||||
|
||||
**Phase 1 Status:** ✅ COMPLETED
|
||||
|
||||
---
|
||||
|
||||
### Phase 2: API Implementation - ✅ COMPLETED
|
||||
|
||||
#### 4. REST API Controllers ✅
|
||||
1. ✅ `ParameterSetsController` - CRUD operations
|
||||
2. ✅ `ScenariosController` - CRUD operations
|
||||
3. ✅ `TestRunsController` - Query operations với filters
|
||||
4. ✅ `TuningController` - Test execution & control
|
||||
|
||||
**Status:** All controllers implemented and tested
|
||||
|
||||
#### 5. Frontend API Integration ✅
|
||||
1. ✅ Created `TuningApiService.cs`
|
||||
2. ✅ Connected components với APIs
|
||||
3. ✅ Load scenarios/parameter sets từ database
|
||||
4. ✅ Implemented test execution flow
|
||||
5. ✅ Real-time updates via SignalR
|
||||
|
||||
**Status:** Fully integrated
|
||||
|
||||
**Phase 2 Status:** ✅ COMPLETED
|
||||
|
||||
---
|
||||
|
||||
### Phase 3: Enhancements (Medium Priority)
|
||||
|
||||
#### 6. Advanced UI Features
|
||||
1. Charts cho telemetry history
|
||||
2. Path visualization component
|
||||
3. Test history viewer với filters
|
||||
4. Comparison tools
|
||||
|
||||
**Estimated Time:** 8-12 hours
|
||||
|
||||
#### 7. Additional Testing
|
||||
1. VelocityEstimatorSimplifiedTests
|
||||
2. TestExecutorTests (with mocking)
|
||||
3. Integration tests
|
||||
4. Real robot testing
|
||||
|
||||
**Estimated Time:** 6-8 hours
|
||||
|
||||
**Total Phase 3:** ~14-20 hours
|
||||
|
||||
---
|
||||
|
||||
## 📁 CẤU TRÚC PROJECTS
|
||||
|
||||
### Backend Project
|
||||
```
|
||||
RobotNet10.NavigationTune/
|
||||
├── Navigation/Core/ # Core controllers ✅
|
||||
├── Models/ # Domain models ✅
|
||||
├── Scenarios/ # Test scenarios ✅
|
||||
├── Execution/ # TestExecutor, TuningNavigation ✅
|
||||
├── Services/ # Business services ✅
|
||||
├── Data/ # Database & repositories ✅
|
||||
├── Hubs/ # SignalR hub ✅
|
||||
├── Interfaces/ # Service interfaces ✅
|
||||
├── Extensions/ # DI extensions ✅
|
||||
└── Controllers/ # REST API (TODO) ❌
|
||||
```
|
||||
|
||||
### Frontend Project
|
||||
```
|
||||
RobotNet10.NavigationTuneUI/
|
||||
├── Clients/ # SignalR clients ✅
|
||||
├── Components/ # Blazor components ✅
|
||||
└── Services/ # API services (TODO) ❌
|
||||
```
|
||||
|
||||
### Test Project
|
||||
```
|
||||
RobotNet10.NavigationTune.Test/
|
||||
├── Helpers/ # Test helpers ✅
|
||||
├── Navigation/Core/ # Core tests ✅
|
||||
├── Services/ # Service tests ✅
|
||||
└── Scenarios/ # Scenario tests ✅
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 🔧 TECHNICAL DETAILS
|
||||
|
||||
### Dependencies
|
||||
|
||||
**Backend:**
|
||||
- Microsoft.EntityFrameworkCore.Sqlite (10.0.1)
|
||||
- Microsoft.AspNetCore.SignalR (1.2.0)
|
||||
- Microsoft.Extensions.Logging.Abstractions (10.0.1)
|
||||
|
||||
**Frontend:**
|
||||
- Microsoft.AspNetCore.SignalR.Client (10.0.1)
|
||||
- MudBlazor (8.15.0)
|
||||
|
||||
**Tests:**
|
||||
- xUnit (2.9.2)
|
||||
- FluentAssertions (7.0.0)
|
||||
- Moq (4.20.72)
|
||||
- Microsoft.EntityFrameworkCore.InMemory (10.0.1)
|
||||
|
||||
### Database Schema
|
||||
|
||||
**Tables:**
|
||||
- `NavigationParameterSets` - Parameter configurations
|
||||
- `TestScenarios` - Test scenario definitions (stored as JSON)
|
||||
- `TestRuns` - Test execution records
|
||||
- `TestMetrics` - Calculated metrics
|
||||
- `SafetyViolations` - Safety violation logs
|
||||
|
||||
### SignalR Hub
|
||||
|
||||
**Endpoint:** `/tuninghub`
|
||||
|
||||
**Methods:**
|
||||
- `JoinTestSession(string testRunId)`
|
||||
- `LeaveTestSession(string testRunId)`
|
||||
|
||||
**Client Events:**
|
||||
- `ReceiveTelemetry` - Real-time telemetry updates
|
||||
- `ReceiveTestStatus` - Test status updates
|
||||
- `ReceiveSafetyEvent` - Safety violation events
|
||||
- `ReceiveTestResult` - Test completion results
|
||||
|
||||
---
|
||||
|
||||
## 📝 NOTES
|
||||
|
||||
### Known Issues
|
||||
|
||||
1. **Validation Logic**: ParameterManager validation có logic `GoodTrackingBlend < PoorTrackingBlend` - có thể cần review lại logic này
|
||||
2. **Metrics Calculation**: Một số edge cases có thể return NaN - cần handle better
|
||||
3. **Test Data**: TestHelpers.CreateTelemetryHistory() có thể cần improve để có realistic timestamps
|
||||
|
||||
### Recent Fixes (2026-01-27)
|
||||
|
||||
1. **DI Container Fix**: Fixed ITestExecutor registration - changed from concrete class to interface registration
|
||||
2. **Entity Framework Configuration**:
|
||||
- Added JSON conversion cho all complex types trong NavigationParameterSet
|
||||
- Configured proper enum conversions (TestStatus, ViolationType, ViolationSeverity)
|
||||
- Added optimized indexes cho performance
|
||||
- Configured proper relationships với cascade delete
|
||||
- Added MaxLength constraints cho string properties
|
||||
3. **Project Structure**: Created NavigationTune.Shared project để decouple frontend from backend
|
||||
4. **Build Issues**: Fixed all compilation errors related to namespace changes và missing references
|
||||
|
||||
### Design Decisions
|
||||
|
||||
1. **Abstract Class Persistence**: Sử dụng `TestScenarioEntity` với JSON serialization thay vì EF Core TPH
|
||||
2. **Simplified Controllers**: PurePursuit và VelocityEstimator được simplified với custom DTOs
|
||||
3. **Adapter Pattern**: Sử dụng adapters để decouple từ RobotApp dependencies
|
||||
|
||||
---
|
||||
|
||||
## 🎯 NEXT STEPS
|
||||
|
||||
1. **Immediate:**
|
||||
- ⏳ User handles database migrations (as requested)
|
||||
- ✅ Backend integration completed
|
||||
- ✅ Frontend integration completed
|
||||
|
||||
2. **Short-term (Phase 3):**
|
||||
- Advanced UI features (charts, visualization)
|
||||
- Additional integration tests
|
||||
- Real robot validation
|
||||
|
||||
3. **Long-term:**
|
||||
- Performance optimization
|
||||
- Additional test scenarios
|
||||
- Advanced tuning algorithms
|
||||
|
||||
---
|
||||
|
||||
## 📚 RELATED DOCUMENTATION
|
||||
|
||||
- Architecture: `docs/RobotApp-TunningNav/# ROBOT TUNING SYSTEM - COMPLETE ARCHITE.md`
|
||||
- Database Schema: `docs/RobotApp-TunningNav/# DATABASE SCHEMA & API SPECIFICATIONS.md`
|
||||
- Configuration: `docs/RobotApp-TunningNav/# CONFIGURATION, WORKFLOWS & IMPLEMENTAT.md`
|
||||
- Robot Control: `docs/RobotApp-TunningNav/ROBOT CONTROL & HARDWARE ABSTRACTION.md`
|
||||
|
||||
---
|
||||
|
||||
**Document Version:** 2.0
|
||||
**Last Updated:** 2026-01-27
|
||||
**Maintained By:** Development Team
|
||||
|
||||
---
|
||||
|
||||
## 📝 CHANGELOG
|
||||
|
||||
### Version 2.0 (2026-01-27)
|
||||
- ✅ Completed Phase 1 Integration (Backend & Frontend)
|
||||
- ✅ Completed Phase 2 API Implementation
|
||||
- ✅ Fixed DI container registration issues
|
||||
- ✅ Completed Entity Framework configuration
|
||||
- ✅ Created NavigationTune.Shared project
|
||||
- ✅ Fixed all build errors
|
||||
- ✅ Updated progress tracking
|
||||
|
||||
### Version 1.0 (2026-01-27)
|
||||
- Initial documentation
|
||||
- Phase 1-4 core implementation completed
|
||||
972
docs/RobotApp-TunningNav/ROBOT CONTROL & HARDWARE ABSTRACTION.md
Normal file
972
docs/RobotApp-TunningNav/ROBOT CONTROL & HARDWARE ABSTRACTION.md
Normal file
@@ -0,0 +1,972 @@
|
||||
# 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
|
||||
|
||||
```csharp
|
||||
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
|
||||
|
||||
```csharp
|
||||
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
|
||||
|
||||
```csharp
|
||||
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)
|
||||
|
||||
```csharp
|
||||
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
|
||||
|
||||
```csharp
|
||||
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)
|
||||
|
||||
```csharp
|
||||
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
|
||||
|
||||
```csharp
|
||||
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
|
||||
|
||||
```csharp
|
||||
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
|
||||
|
||||
```csharp
|
||||
public interface ISafetyMonitor
|
||||
{
|
||||
bool CheckSafety(RobotState state, Path referencePath);
|
||||
List<SafetyViolation> GetViolations();
|
||||
void Reset();
|
||||
}
|
||||
```
|
||||
|
||||
#### Implementation
|
||||
|
||||
```csharp
|
||||
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`
|
||||
|
||||
```csharp
|
||||
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)
|
||||
|
||||
```csharp
|
||||
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`
|
||||
|
||||
```csharp
|
||||
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`
|
||||
|
||||
```csharp
|
||||
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
|
||||
|
||||
```csharp
|
||||
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;
|
||||
981
docs/RobotApp-TunningNav/TUNING_WORKFLOW_GUIDE.md
Normal file
981
docs/RobotApp-TunningNav/TUNING_WORKFLOW_GUIDE.md
Normal file
@@ -0,0 +1,981 @@
|
||||
# HƯỚNG DẪN TUNING NAVIGATION - COMPLETE GUIDE
|
||||
|
||||
**Document:** Robot Navigation Tuning System - Comprehensive Guide
|
||||
**Last Updated:** 2026-02-01
|
||||
**Version:** 2.0 (Updated with Adaptive Pure Pursuit)
|
||||
**Status:** Manual Tuning + Parameter Documentation
|
||||
|
||||
---
|
||||
|
||||
## 📋 MỤC LỤC
|
||||
|
||||
1. [Tổng Quan Hệ Thống](#tổng-quan-hệ-thống)
|
||||
2. [Quick Start - Workflow Cơ Bản](#quick-start)
|
||||
3. [Parameter Reference](#parameter-reference)
|
||||
4. [Troubleshooting Scenarios](#troubleshooting-scenarios)
|
||||
5. [Advanced Tuning Techniques](#advanced-tuning)
|
||||
6. [Best Practices](#best-practices)
|
||||
|
||||
---
|
||||
|
||||
## 🎯 TỔNG QUAN HỆ THỐNG
|
||||
|
||||
### Loại Tuning Hiện Tại: **MANUAL TUNING** ✅
|
||||
|
||||
**Tính năng đã có:**
|
||||
- ✅ Manual parameter adjustment UI
|
||||
- ✅ Single test execution
|
||||
- ✅ Batch testing (nhiều scenarios)
|
||||
- ✅ Configuration comparison
|
||||
- ✅ Real-time visualization
|
||||
- ✅ Metrics calculation và scoring
|
||||
- ✅ **NEW:** Adaptive Pure Pursuit (distance + curvature based)
|
||||
- ✅ **NEW:** 3-Phase Final Approach Controller
|
||||
- ✅ **NEW:** Comprehensive parameter documentation
|
||||
|
||||
**Tính năng chưa có (Future):**
|
||||
- ❌ Automated optimization (Bayesian, Grid Search)
|
||||
- ❌ Auto-tuning algorithms
|
||||
- ❌ AI-based parameter suggestion
|
||||
|
||||
---
|
||||
|
||||
## 🚀 QUICK START
|
||||
|
||||
### Workflow 1: First-Time Setup (15 phút)
|
||||
|
||||
```
|
||||
┌─────────────────────────────────────┐
|
||||
│ 1. Load Default Configuration │
|
||||
│ - Access: /navigation/tuning │
|
||||
│ - Select "Balanced Default" │
|
||||
│ Time: 2 phút │
|
||||
└─────────────────────────────────────┘
|
||||
↓
|
||||
┌─────────────────────────────────────┐
|
||||
│ 2. Baseline Test │
|
||||
│ - Scenario: "Straight Line 10m" │
|
||||
│ - Click "Start Test" │
|
||||
│ - Observe visualization │
|
||||
│ Time: 3 phút │
|
||||
└─────────────────────────────────────┘
|
||||
↓
|
||||
┌─────────────────────────────────────┐
|
||||
│ 3. Review Metrics │
|
||||
│ Overall Score: ____/100 │
|
||||
│ - CTE RMS: ____m │
|
||||
│ - Heading Error: ____° │
|
||||
│ - Jerk: ____m/s³ │
|
||||
│ Time: 5 phút │
|
||||
└─────────────────────────────────────┘
|
||||
↓
|
||||
┌─────────────────────────────────────┐
|
||||
│ 4. Decision Tree │
|
||||
│ Score > 80 → Test more scenarios│
|
||||
│ Score 60-80 → Manual tuning │
|
||||
│ Score < 60 → Check hardware │
|
||||
│ Time: 5 phút │
|
||||
└─────────────────────────────────────┘
|
||||
```
|
||||
|
||||
**Kết quả mong đợi:** Hiểu được performance baseline của robot
|
||||
|
||||
---
|
||||
|
||||
### Workflow 2: Multi-Scenario Validation (20 phút)
|
||||
|
||||
```
|
||||
1. Setup Batch Test
|
||||
Scenarios:
|
||||
☑ Straight Line 10m
|
||||
☑ Circle 2m Radius
|
||||
☑ Circle 0.5m Radius (challenging)
|
||||
|
||||
2. Run & Monitor (15 phút)
|
||||
- Auto run từng scenario
|
||||
- Track progress bar
|
||||
- View real-time plots
|
||||
|
||||
3. Compare Results (5 phút)
|
||||
Scenario Score CTE RMS Decision
|
||||
───────────────── ───── ─────── ────────
|
||||
Straight Line 85 0.05m ✓ Pass
|
||||
Circle 2m 78 0.08m ⚠ Tune
|
||||
Circle 0.5m 65 0.12m ✗ Need work
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 📚 PARAMETER REFERENCE
|
||||
|
||||
### Pure Pursuit Configuration
|
||||
|
||||
#### **Basic Lookahead Parameters**
|
||||
|
||||
##### `LookaheadMin` (meters)
|
||||
**Default:** 0.3m
|
||||
**Meaning:** Điểm gần nhất phía trước mà robot hướng đến
|
||||
|
||||
**↑ Tăng (0.4-0.6m):**
|
||||
- ✓ Smoother tracking trên đường thẳng
|
||||
- ✓ Ít reactive, predictive hơn
|
||||
- ✗ Có thể cắt góc trên curves
|
||||
- ✗ Kém chính xác ở low speed
|
||||
|
||||
**↓ Giảm (0.2-0.25m):**
|
||||
- ✓ Tracking curves chặt hơn
|
||||
- ✓ Chính xác hơn ở low speed
|
||||
- ✗ Jittery/oscillation nhiều hơn
|
||||
- ✗ Nhạy với noise
|
||||
|
||||
**Tuning Tips:**
|
||||
- Warehouse AGV: 0.4-0.5m (smooth)
|
||||
- Tight spaces: 0.25-0.3m (precision)
|
||||
- Start: 0.3m (balanced)
|
||||
|
||||
**UI Location:** `Pure Pursuit Tab > Basic Lookahead > LookaheadMin`
|
||||
|
||||
---
|
||||
|
||||
##### `Kdd` (seconds)
|
||||
**Default:** 1.0s
|
||||
**Formula:** `lookahead = LookaheadMin + Kdd × |velocity|`
|
||||
|
||||
**↑ Tăng (1.2-1.5s):**
|
||||
- ✓ Look xa hơn ở high speed → smoother
|
||||
- ✓ Tốt cho fast robots (>1.5 m/s)
|
||||
- ✗ Có thể quá predictive (overshoot)
|
||||
|
||||
**↓ Giảm (0.7-0.9s):**
|
||||
- ✓ Reactive control hơn
|
||||
- ✓ Tốt cho slow robots
|
||||
- ✗ Jittery ở high speed
|
||||
|
||||
**Tuning Tips:**
|
||||
- Check formula: At 1.0m/s → lookahead = 0.3 + 1.0×1.0 = 1.3m
|
||||
- Slow robot (<0.5 m/s): Kdd = 0.8-1.0
|
||||
- Fast robot (>1.5 m/s): Kdd = 1.2-1.5
|
||||
|
||||
**UI Location:** `Pure Pursuit Tab > Basic Lookahead > Kdd`
|
||||
|
||||
---
|
||||
|
||||
##### `LookaheadMax` (meters)
|
||||
**Default:** 2.0m
|
||||
**Meaning:** Upper limit cho lookahead distance
|
||||
|
||||
**↑ Tăng (2.5-3.0m):**
|
||||
- ✓ Very smooth ở high speed
|
||||
- ✓ Tốt cho long straight paths
|
||||
- ✗ Cắt góc aggressive
|
||||
- ✗ Phản ứng chậm với path changes
|
||||
|
||||
**↓ Giảm (1.5-1.8m):**
|
||||
- ✓ Tighter path following
|
||||
- ✓ Tốt cho complex paths
|
||||
- ✗ Kém smooth ở high speed
|
||||
|
||||
**Tuning Tips:**
|
||||
- Must be: `LookaheadMax > LookaheadMin + Kdd × MaxVelocity`
|
||||
- Example: MaxVel=1.5m/s → need ≥ 0.3+1.0×1.5 = 1.8m
|
||||
|
||||
**UI Location:** `Pure Pursuit Tab > Basic Lookahead > LookaheadMax`
|
||||
|
||||
---
|
||||
|
||||
#### **Adaptive Lookahead Parameters** (NEW in v2.0)
|
||||
|
||||
##### `GoalRegionDistance` (meters)
|
||||
**Default:** 1.5m
|
||||
**Meaning:** Bắt đầu giảm lookahead khi trong khoảng này từ goal
|
||||
|
||||
**How it works:**
|
||||
```
|
||||
Distance > 1.5m: lookahead = 100% (normal)
|
||||
Distance = 1.0m: lookahead = 83%
|
||||
Distance = 0.5m: lookahead = 67%
|
||||
Distance = 0.0m: lookahead = 50%
|
||||
```
|
||||
|
||||
**↑ Tăng (2.0-3.0m):**
|
||||
- ✓ Earlier precision mode
|
||||
- ✓ Smoother deceleration
|
||||
- ✗ Slower overall
|
||||
|
||||
**↓ Giảm (0.8-1.2m):**
|
||||
- ✓ Faster approach
|
||||
- ✗ Abrupt gần goal
|
||||
|
||||
**Tuning Tips:**
|
||||
- Fast robot: Tăng (need more brake distance)
|
||||
- Short paths: 1.0-1.5m
|
||||
- Long paths: 2.0-2.5m
|
||||
|
||||
**UI Location:** `Pure Pursuit Tab > Adaptive > GoalRegionDistance`
|
||||
|
||||
---
|
||||
|
||||
##### `KCurvature`
|
||||
**Default:** 2.0
|
||||
**Formula:** `curvatureFactor = 1 / (1 + KCurvature × curvature)`
|
||||
|
||||
**How it works:**
|
||||
```
|
||||
Straight path (k=0): curvatureFactor = 1.0 (100% lookahead)
|
||||
Gentle curve (k=0.5): curvatureFactor = 0.67 (67% lookahead)
|
||||
Sharp curve (k=1.0): curvatureFactor = 0.33 (33% lookahead)
|
||||
```
|
||||
|
||||
**↑ Tăng (3.0-5.0):**
|
||||
- ✓ Tighter tracking trên curves
|
||||
- ✓ Ít cắt góc
|
||||
- ✗ Có thể quá reactive
|
||||
- ✗ Oscillation trên curves
|
||||
|
||||
**↓ Giảm (1.0-1.5):**
|
||||
- ✓ Smoother trên curves
|
||||
- ✗ Cắt góc nhiều hơn
|
||||
- ✗ Kém precise
|
||||
|
||||
**Tuning Tips:**
|
||||
- Warehouse (gentle curves): 1.5-2.0
|
||||
- Tight spaces (sharp curves): 3.0-4.0
|
||||
- If cutting corners: Tăng KCurvature
|
||||
|
||||
**UI Location:** `Pure Pursuit Tab > Adaptive > KCurvature`
|
||||
|
||||
---
|
||||
|
||||
#### **Final Approach Parameters**
|
||||
|
||||
##### `FinalApproachThreshold` (meters)
|
||||
**Default:** 0.2m
|
||||
**Meaning:** Khoảng cách activate final approach mode
|
||||
|
||||
**Behavior:**
|
||||
- Distance > 0.2m: Normal Pure Pursuit tracking
|
||||
- Distance ≤ 0.2m: Switch to 3-phase final approach controller
|
||||
|
||||
**3 Phases:**
|
||||
1. **Phase 1:** Approach position (distance > 3cm)
|
||||
2. **Phase 2:** Align heading (position OK, heading error > 3°)
|
||||
3. **Phase 3:** Goal reached (both OK)
|
||||
|
||||
**↑ Tăng (0.3-0.5m):**
|
||||
- ✓ Earlier slow down → smoother
|
||||
- ✗ Takes longer
|
||||
|
||||
**↓ Giảm (0.1-0.15m):**
|
||||
- ✓ Faster approach
|
||||
- ✗ Abrupt/jerky
|
||||
|
||||
**Tuning Tips:**
|
||||
- Should be: `> PositionTolerance × 3`
|
||||
- High precision: 0.3-0.5m
|
||||
- Speed priority: 0.15-0.2m
|
||||
|
||||
**UI Location:** `Pure Pursuit Tab > Final Approach > Threshold`
|
||||
|
||||
---
|
||||
|
||||
##### `PositionTolerance` (meters)
|
||||
**Default:** 0.03m (3cm)
|
||||
**Meaning:** Robot cần ở gần goal bao nhiêu
|
||||
|
||||
**↑ Tăng (0.05-0.08m):**
|
||||
- ✓ Faster goal reaching
|
||||
- ✗ Lower precision
|
||||
|
||||
**↓ Giảm (0.01-0.02m):**
|
||||
- ✓ Higher precision
|
||||
- ✗ May never reach (nếu localization error lớn)
|
||||
|
||||
**Critical Constraint:**
|
||||
```
|
||||
PositionTolerance >= 2 × Localization_RMS_Error
|
||||
```
|
||||
|
||||
**Tuning Tips:**
|
||||
- Typical localization: 1-2cm → use 0.03-0.05m
|
||||
- High precision app: 0.02m (if localization allows)
|
||||
- Cannot be < localization capability
|
||||
|
||||
**UI Location:** `Pure Pursuit Tab > Final Approach > PositionTolerance`
|
||||
|
||||
---
|
||||
|
||||
##### `HeadingTolerance` (degrees)
|
||||
**Default:** 3.0°
|
||||
**Meaning:** Robot heading phải align trong khoảng này
|
||||
|
||||
**Phase 2 Behavior:**
|
||||
- Position đạt → Stop linear motion
|
||||
- Rotate in-place để align heading
|
||||
- Khi heading error < 3° → Done
|
||||
|
||||
**↑ Tăng (5-10°):**
|
||||
- ✓ Faster completion
|
||||
- ✗ Robot may face wrong direction
|
||||
|
||||
**↓ Giảm (1-2°):**
|
||||
- ✓ Very precise alignment
|
||||
- ✗ Takes much longer
|
||||
- ✗ May oscillate
|
||||
|
||||
**Tuning Tips:**
|
||||
- Docking/charging: 2-3° (precision critical)
|
||||
- General navigation: 5-8°
|
||||
- No heading requirement: 10-15° (fast)
|
||||
|
||||
**UI Location:** `Pure Pursuit Tab > Final Approach > HeadingTolerance`
|
||||
|
||||
---
|
||||
|
||||
### Navigation Limits
|
||||
|
||||
##### `MaxLinearVelocity` (m/s)
|
||||
**Default:** 1.5 m/s
|
||||
**Meaning:** Top speed during navigation
|
||||
|
||||
**Safety Check:**
|
||||
```
|
||||
Braking distance = v² / (2 × deceleration)
|
||||
At 1.5 m/s, 0.5 m/s² decel → 2.25m braking distance
|
||||
```
|
||||
|
||||
**Tuning Tips:**
|
||||
- MUST match motor controller limits
|
||||
- Warehouse AGV: 1.0-1.5 m/s
|
||||
- Outdoor: 2.0-3.0 m/s
|
||||
- Crowded areas: 0.5-0.8 m/s
|
||||
|
||||
**UI Location:** `Navigation Limits Tab > MaxLinearVelocity`
|
||||
|
||||
---
|
||||
|
||||
## 🛠️ TROUBLESHOOTING SCENARIOS
|
||||
|
||||
### Scenario 1: Robot Oscillates (Dao động)
|
||||
|
||||
**Triệu chứng:**
|
||||
- ✗ Robot swing qua lại
|
||||
- ✗ Angular velocity thay đổi liên tục
|
||||
- ✗ Path không smooth
|
||||
- **Metrics:** High velocity StdDev, high jerk
|
||||
|
||||
**Root Causes & Solutions:**
|
||||
|
||||
| Cause | Parameter | Action | Priority |
|
||||
|-------|-----------|--------|----------|
|
||||
| Lookahead quá ngắn | `LookaheadMin` | 0.3 → 0.4m | ⭐⭐⭐ |
|
||||
| Curvature sensitivity cao | `KCurvature` | 2.0 → 1.5 | ⭐⭐ |
|
||||
| Angular gain lớn | `MaxAngularVelocity` | 1.5 → 1.2 rad/s | ⭐⭐ |
|
||||
| Signal noise | `AlphaFilter` | 0.3 → 0.2 | ⭐ |
|
||||
| PID Kd thấp | `MovePidConfig.Kd` | +0.1-0.2 | ⭐⭐ |
|
||||
|
||||
**Step-by-Step Fix:**
|
||||
```
|
||||
1. Tăng LookaheadMin: 0.3 → 0.4m
|
||||
└─> Test → Still oscillate?
|
||||
2. Giảm KCurvature: 2.0 → 1.5
|
||||
└─> Test → Still oscillate?
|
||||
3. Giảm MaxAngularVelocity: 1.5 → 1.2 rad/s
|
||||
└─> Test → Still oscillate?
|
||||
4. Increase damping: MovePidConfig.Kd +0.1
|
||||
```
|
||||
|
||||
**Expected Improvement:**
|
||||
- Velocity StdDev: 0.3 → 0.15 m/s
|
||||
- Max Jerk: 6.0 → 3.5 m/s³
|
||||
- Overall Score: +10-15 points
|
||||
|
||||
---
|
||||
|
||||
### Scenario 2: Robot Cuts Corners (Cắt góc)
|
||||
|
||||
**Triệu chứng:**
|
||||
- ✗ Robot không follow đường cong chặt
|
||||
- ✗ Cross-track error lớn trên curves
|
||||
- ✗ Shortcut qua góc
|
||||
- **Metrics:** CTE RMS > 0.10m, Path Length Ratio < 1.0
|
||||
|
||||
**Root Causes & Solutions:**
|
||||
|
||||
| Cause | Parameter | Action | Priority |
|
||||
|-------|-----------|--------|----------|
|
||||
| Lookahead quá dài | `LookaheadMax` | 2.0 → 1.5m | ⭐⭐⭐ |
|
||||
| Không adapt curvature | `KCurvature` | 2.0 → 3.0-4.0 | ⭐⭐⭐ |
|
||||
| Lookahead time lớn | `MaxLookaheadTimeRatio` | 2.0 → 1.5s | ⭐⭐ |
|
||||
|
||||
**Step-by-Step Fix:**
|
||||
```
|
||||
1. Tăng KCurvature: 2.0 → 3.0
|
||||
└─> Test on sharp curve
|
||||
2. Vẫn cut? → Giảm LookaheadMax: 2.0 → 1.8m
|
||||
└─> Check CTE RMS improvement
|
||||
3. Fine-tune: MaxLookaheadTimeRatio: 2.0 → 1.7s
|
||||
```
|
||||
|
||||
**Test Case:**
|
||||
- Circle 0.5m Radius (challenging)
|
||||
- Goal: CTE RMS < 0.08m
|
||||
|
||||
---
|
||||
|
||||
### Scenario 3: Poor Goal Precision
|
||||
|
||||
**Triệu chứng:**
|
||||
- ✗ Robot không dừng đúng vị trí
|
||||
- ✗ Heading sai khi đến goal
|
||||
- ✗ Overshoot hoặc undershoot
|
||||
- **Metrics:** Final position error > 5cm, heading error > 5°
|
||||
|
||||
**Diagnosis:**
|
||||
```
|
||||
Check Phase Logs:
|
||||
FA-P1: DTG=0.085m, AErr=8.3°, LV=0.15, AV=0.12
|
||||
^^^^^ Heading error cao
|
||||
FA-P2: HErr=2.1°, AV=0.08 (Aligning heading)
|
||||
^^^^^ Good alignment
|
||||
FA-P3: Goal reached! DTG=0.02m, HErr=1.5°
|
||||
^^^^^ Position OK
|
||||
```
|
||||
|
||||
**Root Causes & Solutions:**
|
||||
|
||||
| Cause | Parameter | Action | Priority |
|
||||
|-------|-----------|--------|----------|
|
||||
| Position tolerance lớn | `PositionTolerance` | 0.03 → 0.02m | ⭐⭐⭐ |
|
||||
| Final approach xa | `FinalApproachThreshold` | 0.2 → 0.15m | ⭐⭐ |
|
||||
| Angular gain thấp | `FinalKdAngular` | 2.0 → 2.5-3.0 | ⭐⭐⭐ |
|
||||
| Goal region lớn | `GoalRegionDistance` | 1.5 → 1.0m | ⭐ |
|
||||
|
||||
**Special Case: Heading Issues**
|
||||
```
|
||||
If position OK but heading wrong:
|
||||
1. Check CalculateGoalHeading() logic
|
||||
2. Tăng FinalKdAngular: 2.0 → 3.0
|
||||
3. Giảm HeadingTolerance: 3° → 2° (stricter)
|
||||
4. Increase FinalApproachMaxAngularVel: 0.3 → 0.4 rad/s (faster rotation)
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### Scenario 4: High Speed Instability
|
||||
|
||||
**Triệu chứng:**
|
||||
- ✗ Không stable ở tốc độ cao
|
||||
- ✗ Overshoot nhiều
|
||||
- ✗ Hard braking
|
||||
- **Metrics:** High jerk ở cuối path, position overshoot
|
||||
|
||||
**Root Causes & Solutions:**
|
||||
|
||||
| Cause | Parameter | Action | Priority |
|
||||
|-------|-----------|--------|----------|
|
||||
| Lookahead không đủ xa | `LookaheadMax` | 2.0 → 2.5-3.0m | ⭐⭐⭐ |
|
||||
| Kdd quá nhỏ | `Kdd` | 1.0 → 1.2-1.5s | ⭐⭐⭐ |
|
||||
| Goal region ngắn | `GoalRegionDistance` | 1.5 → 2.0-2.5m | ⭐⭐⭐ |
|
||||
|
||||
**Formula Check:**
|
||||
```
|
||||
Braking Distance = v² / (2 × decel)
|
||||
At 1.5 m/s, 0.5 m/s² → 2.25m
|
||||
|
||||
GoalRegionDistance should be ≥ Braking Distance
|
||||
→ Set GoalRegionDistance = 2.5m (safety margin)
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 🎓 ADVANCED TUNING
|
||||
|
||||
### Tuning Hierarchy (Làm theo thứ tự này)
|
||||
|
||||
**Priority 1: Basic Lookahead**
|
||||
```
|
||||
1. LookaheadMin → Base stability
|
||||
2. KCurvature → Curve handling
|
||||
3. Kdd → Velocity scaling
|
||||
4. LookaheadMax → High-speed limit
|
||||
```
|
||||
|
||||
**Priority 2: Final Approach**
|
||||
```
|
||||
1. PositionTolerance → Must match localization
|
||||
2. FinalApproachThreshold → When to slow down
|
||||
3. FinalKdAngular → Heading control gain
|
||||
4. HeadingTolerance → Strictness
|
||||
```
|
||||
|
||||
**Priority 3: Adaptive Features**
|
||||
```
|
||||
1. GoalRegionDistance → Brake distance
|
||||
2. KCurvature → Curve tightness
|
||||
3. Time ratios → Preview distance
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### Parameter Interactions (Quan trọng!)
|
||||
|
||||
#### Interaction 1: LookaheadMin ↔ KCurvature
|
||||
```
|
||||
Combination Result When to Use
|
||||
───────────────────── ───────────────── ──────────────
|
||||
High LookaheadMin + Cut corners ✗ Avoid
|
||||
Low KCurvature severely
|
||||
|
||||
Low LookaheadMin + May oscillate ⚠ Careful
|
||||
High KCurvature on curves
|
||||
|
||||
Medium LookaheadMin + Balanced ✓ Recommended
|
||||
Medium KCurvature (0.3-0.4m, 2.0)
|
||||
```
|
||||
|
||||
#### Interaction 2: MaxLinearVelocity ↔ GoalRegionDistance
|
||||
```
|
||||
Speed Goal Region Result
|
||||
─────── ───────────── ───────────────────────────
|
||||
1.5 m/s 1.0m ✗ Insufficient brake distance
|
||||
1.5 m/s 2.5m ✓ Safe, smooth approach
|
||||
0.5 m/s 2.5m ⚠ Too early slow down
|
||||
```
|
||||
|
||||
**Formula:**
|
||||
```csharp
|
||||
GoalRegionDistance ≥ MaxLinearVelocity² / (2 × deceleration)
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### Preset Configurations (Quick Start)
|
||||
|
||||
#### **Preset 1: Warehouse Standard**
|
||||
```json
|
||||
{
|
||||
"Name": "Warehouse Standard",
|
||||
"Description": "Smooth, wide corridors, medium speed",
|
||||
"PurePursuitConfig": {
|
||||
"LookaheadMin": 0.4,
|
||||
"Kdd": 1.0,
|
||||
"LookaheadMax": 2.0,
|
||||
"KCurvature": 1.5,
|
||||
"MaxAngularVelocity": 1.2,
|
||||
"FinalApproachThreshold": 0.2,
|
||||
"PositionTolerance": 0.04,
|
||||
"HeadingTolerance": 5.0,
|
||||
"GoalRegionDistance": 1.5
|
||||
},
|
||||
"NavigationConfig": {
|
||||
"MaxLinearVelocity": 1.2
|
||||
}
|
||||
}
|
||||
```
|
||||
**Use case:** Kho hàng rộng, ít chướng ngại
|
||||
|
||||
---
|
||||
|
||||
#### **Preset 2: Tight Precision**
|
||||
```json
|
||||
{
|
||||
"Name": "Tight Precision",
|
||||
"Description": "Narrow spaces, docking, high precision",
|
||||
"PurePursuitConfig": {
|
||||
"LookaheadMin": 0.25,
|
||||
"Kdd": 0.8,
|
||||
"LookaheadMax": 1.5,
|
||||
"KCurvature": 3.0,
|
||||
"MaxAngularVelocity": 1.0,
|
||||
"FinalApproachThreshold": 0.3,
|
||||
"PositionTolerance": 0.02,
|
||||
"HeadingTolerance": 2.0,
|
||||
"GoalRegionDistance": 1.0
|
||||
},
|
||||
"NavigationConfig": {
|
||||
"MaxLinearVelocity": 0.8
|
||||
}
|
||||
}
|
||||
```
|
||||
**Use case:** Docking, charging, tight spaces
|
||||
|
||||
---
|
||||
|
||||
#### **Preset 3: High Speed**
|
||||
```json
|
||||
{
|
||||
"Name": "High Speed",
|
||||
"Description": "Fast navigation, long straight paths",
|
||||
"PurePursuitConfig": {
|
||||
"LookaheadMin": 0.5,
|
||||
"Kdd": 1.5,
|
||||
"LookaheadMax": 3.0,
|
||||
"KCurvature": 2.0,
|
||||
"MaxAngularVelocity": 2.0,
|
||||
"FinalApproachThreshold": 0.15,
|
||||
"PositionTolerance": 0.05,
|
||||
"HeadingTolerance": 8.0,
|
||||
"GoalRegionDistance": 2.5
|
||||
},
|
||||
"NavigationConfig": {
|
||||
"MaxLinearVelocity": 2.0
|
||||
}
|
||||
}
|
||||
```
|
||||
**Use case:** Outdoor, tốc độ cao, đường thẳng dài
|
||||
|
||||
---
|
||||
|
||||
## 📊 COMPARISON WORKFLOW
|
||||
|
||||
### How to Compare Two Configurations
|
||||
|
||||
**Step 1: Setup Comparison**
|
||||
```
|
||||
UI: Configuration Comparison Tab
|
||||
├─ Config A: "Balanced Default"
|
||||
├─ Config B: "Tuned_v1"
|
||||
└─ Scenario: "Circle 2m Radius"
|
||||
|
||||
Click: "Run Comparison"
|
||||
```
|
||||
|
||||
**Step 2: Monitor Execution**
|
||||
```
|
||||
Progress:
|
||||
[████████████░░░░░░░░] 60% (Config A Complete)
|
||||
|
||||
Real-time Plot:
|
||||
- Blue line: Config A trajectory
|
||||
- Red line: Config B trajectory
|
||||
- Green line: Reference path
|
||||
```
|
||||
|
||||
**Step 3: Review Results**
|
||||
```
|
||||
Metric Config A Config B Improvement
|
||||
─────────────────── ────────── ────────── ───────────
|
||||
Overall Score 72 84 +12 ✓
|
||||
CTE RMS (m) 0.095 0.062 -35% ✓
|
||||
Heading Error (°) 4.2 2.8 -33% ✓
|
||||
Max Jerk (m/s³) 5.8 3.9 -33% ✓
|
||||
Completion Time (s) 12.5 11.8 -6% ✓
|
||||
```
|
||||
|
||||
**Decision:**
|
||||
- All metrics improved → ✓ Config B is better, save it
|
||||
- Mixed results → Need further tuning
|
||||
- Worse results → Revert, try different approach
|
||||
|
||||
---
|
||||
|
||||
## ⚠️ BEST PRACTICES & SAFETY
|
||||
|
||||
### 1. Tuning Safety
|
||||
|
||||
**Safety Monitoring (Auto Abort):**
|
||||
```
|
||||
Test sẽ stop nếu:
|
||||
- Cross-track error > 0.5m
|
||||
- Heading error > 45°
|
||||
- Sustained tracking error > 3s
|
||||
- Velocity exceeds motor limits
|
||||
```
|
||||
|
||||
**Before Tuning:**
|
||||
- ✓ Check hardware health
|
||||
- ✓ Verify sensor calibration
|
||||
- ✓ Test in safe environment
|
||||
- ✓ Have emergency stop ready
|
||||
|
||||
### 2. Parameter Validation
|
||||
|
||||
**Automatic Constraints:**
|
||||
```csharp
|
||||
// System validates these automatically:
|
||||
LookaheadMax > LookaheadMin
|
||||
GoodTrackingBlend < PoorTrackingBlend
|
||||
MaxLinearVelocity <= Motor_Max_Velocity
|
||||
PositionTolerance >= 2 × Localization_Error
|
||||
```
|
||||
|
||||
**If validation fails:**
|
||||
- Red border on parameter field
|
||||
- Tooltip shows violation
|
||||
- Cannot save until fixed
|
||||
|
||||
### 3. Incremental Changes
|
||||
|
||||
**Rule of Thumb:**
|
||||
```
|
||||
Change 1-2 parameters per iteration
|
||||
Max change: ±30% of current value
|
||||
Test after each change
|
||||
```
|
||||
|
||||
**Example:**
|
||||
```
|
||||
❌ Bad:
|
||||
LookaheadMin: 0.3 → 0.6 (+100%)
|
||||
Kdd: 1.0 → 1.5 (+50%)
|
||||
KCurvature: 2.0 → 4.0 (+100%)
|
||||
→ Too many changes, can't isolate effect
|
||||
|
||||
✓ Good:
|
||||
LookaheadMin: 0.3 → 0.35 (+17%)
|
||||
Test → Evaluate → Next change
|
||||
```
|
||||
|
||||
### 4. Documentation
|
||||
|
||||
**Every Configuration Should Have:**
|
||||
```json
|
||||
{
|
||||
"Name": "Tuned_2026-02-01_v3",
|
||||
"Description": "Increased KCurvature to 3.0 to reduce corner cutting on tight curves. Improved CTE RMS from 0.095m to 0.062m on Circle 2m scenario.",
|
||||
"CreatedBy": "User Name",
|
||||
"BaseConfig": "Balanced Default",
|
||||
"TestResults": [
|
||||
{
|
||||
"Scenario": "Circle 2m",
|
||||
"Score": 84,
|
||||
"CTE_RMS": 0.062
|
||||
}
|
||||
]
|
||||
}
|
||||
```
|
||||
|
||||
### 5. Multi-Scenario Validation
|
||||
|
||||
**Minimum Test Matrix:**
|
||||
```
|
||||
Scenario Min Score Critical Metrics
|
||||
─────────────────── ───────── ────────────────────
|
||||
Straight Line 10m > 80 CTE RMS < 0.05m
|
||||
Circle 2m Radius > 75 CTE RMS < 0.08m
|
||||
Circle 0.5m Radius > 65 CTE RMS < 0.12m
|
||||
```
|
||||
|
||||
**Full Validation (Before Deployment):**
|
||||
- All 3 scenarios > thresholds
|
||||
- No safety violations
|
||||
- Smooth trajectories (visual check)
|
||||
- Repeatable results (run 3 times)
|
||||
|
||||
---
|
||||
|
||||
## 📖 WORKFLOW EXAMPLES
|
||||
|
||||
### Example 1: Fix Oscillation Issue
|
||||
|
||||
**Initial State:**
|
||||
```
|
||||
Scenario: Straight Line 10m
|
||||
Score: 68/100
|
||||
Issues:
|
||||
- Velocity StdDev: 0.32 m/s (high)
|
||||
- Max Jerk: 6.2 m/s³ (high)
|
||||
- Visual: Robot swings left-right
|
||||
```
|
||||
|
||||
**Iteration 1:**
|
||||
```
|
||||
Change: LookaheadMin: 0.3 → 0.4m
|
||||
Reason: Increase preview distance
|
||||
Result:
|
||||
Score: 68 → 75 (+7)
|
||||
Velocity StdDev: 0.32 → 0.22 (-31%)
|
||||
Still some oscillation → Continue
|
||||
```
|
||||
|
||||
**Iteration 2:**
|
||||
```
|
||||
Change: KCurvature: 2.0 → 1.5
|
||||
Reason: Less aggressive on curves
|
||||
Result:
|
||||
Score: 75 → 79 (+4)
|
||||
Max Jerk: 6.2 → 4.5 (-27%)
|
||||
Better but not perfect → Continue
|
||||
```
|
||||
|
||||
**Iteration 3:**
|
||||
```
|
||||
Change: MovePidConfig.Kd: 0.6 → 0.8
|
||||
Reason: Add damping
|
||||
Result:
|
||||
Score: 79 → 83 (+4)
|
||||
Velocity StdDev: 0.22 → 0.15 (-32%)
|
||||
Visual: Smooth tracking ✓
|
||||
PASS! Save as "Smooth_v1"
|
||||
```
|
||||
|
||||
**Total Time:** 25 phút (3 iterations × ~8 phút/iteration)
|
||||
|
||||
---
|
||||
|
||||
### Example 2: Improve Goal Precision
|
||||
|
||||
**Initial State:**
|
||||
```
|
||||
Scenario: Docking Test
|
||||
Issues:
|
||||
- Final position error: 8cm (target: <3cm)
|
||||
- Final heading error: 6° (target: <3°)
|
||||
```
|
||||
|
||||
**Analysis:**
|
||||
```
|
||||
Phase Logs:
|
||||
FA-P1: DTG=0.18m, AErr=12°, LV=0.20
|
||||
└─> Slow approach OK
|
||||
FA-P2: HErr=6.2°, AV=0.15
|
||||
└─> Heading alignment too slow
|
||||
FA-P3: Not reached (timeout)
|
||||
```
|
||||
|
||||
**Iteration 1:**
|
||||
```
|
||||
Change: FinalKdAngular: 2.0 → 3.0
|
||||
Reason: Faster heading correction
|
||||
Result:
|
||||
Final heading error: 6° → 3.5°
|
||||
Better but still over target
|
||||
```
|
||||
|
||||
**Iteration 2:**
|
||||
```
|
||||
Changes:
|
||||
- HeadingTolerance: 3° → 2° (stricter)
|
||||
- FinalApproachMaxAngularVel: 0.3 → 0.4 (faster rotation)
|
||||
Result:
|
||||
Final heading error: 3.5° → 2.1° ✓
|
||||
Final position: 8cm → 2.5cm ✓
|
||||
PASS!
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 🔮 FUTURE FEATURES
|
||||
|
||||
### Planned: Automated Optimization (Phase 3)
|
||||
|
||||
**Status:** Not yet implemented
|
||||
|
||||
**Algorithms Under Consideration:**
|
||||
- Bayesian Optimization (most promising)
|
||||
- Grid Search (exhaustive but slow)
|
||||
- Genetic Algorithm (for multi-objective)
|
||||
|
||||
**Estimated Workflow:**
|
||||
```
|
||||
1. Select parameters to optimize
|
||||
☑ LookaheadMin, Kdd, KCurvature
|
||||
☐ (Lock other parameters)
|
||||
|
||||
2. Define objective function
|
||||
Minimize: 0.6×CTE_RMS + 0.2×Jerk + 0.2×Time
|
||||
|
||||
3. Set constraints
|
||||
LookaheadMin: [0.2, 0.6]
|
||||
Kdd: [0.7, 1.5]
|
||||
...
|
||||
|
||||
4. Run optimization (30-60 phút)
|
||||
Progress: [████░░░░] 50% (25/50 iterations)
|
||||
|
||||
5. Review best parameters
|
||||
Best Score: 87 (iteration 38)
|
||||
|
||||
6. Validate on test scenarios
|
||||
```
|
||||
|
||||
**Timeline:** Q2 2026 (planned)
|
||||
|
||||
---
|
||||
|
||||
## ❓ FAQ
|
||||
|
||||
**Q: Nên tune bao nhiêu parameters cùng lúc?**
|
||||
A: 1-2 parameters per iteration. Tune theo nhóm (Pure Pursuit → PID → Velocity).
|
||||
|
||||
**Q: Làm sao biết tuning có hiệu quả?**
|
||||
A: Use Comparison tool. Overall Score tăng ≥5 điểm + visual improvement.
|
||||
|
||||
**Q: Robot vẫn oscillate sau khi tăng LookaheadMin?**
|
||||
A: Try giảm KCurvature hoặc tăng PID Kd (damping).
|
||||
|
||||
**Q: Goal precision kém dù đã giảm PositionTolerance?**
|
||||
A: Check localization error. PositionTolerance không thể < 2× localization RMS error.
|
||||
|
||||
**Q: Cần test bao nhiêu scenarios?**
|
||||
A: Minimum 3 (Straight, Circle 2m, Circle 0.5m). Recommend 5+ for robustness.
|
||||
|
||||
**Q: Làm sao load preset vào UI?**
|
||||
A: Configuration dropdown → Select preset name → Click "Load".
|
||||
|
||||
**Q: Configuration comparison cho kết quả khác nhau mỗi lần?**
|
||||
A: Check randomness in test scenario. Some scenarios có stochastic elements. Run multiple times và average.
|
||||
|
||||
**Q: Tôi có thể export configuration không?**
|
||||
A: Yes, click "Export JSON" button. File có thể import vào hệ thống khác.
|
||||
|
||||
---
|
||||
|
||||
## 📚 RELATED DOCUMENTATION
|
||||
|
||||
- **Parameter XML Docs:** Hover over any parameter in code to see inline documentation
|
||||
- **Architecture:** `# ROBOT TUNING SYSTEM - COMPLETE ARCHITE.md`
|
||||
- **Database Schema:** `# DATABASE SCHEMA & API SPECIFICATIONS.md`
|
||||
- **Implementation Progress:** `IMPLEMENTATION_PROGRESS.md`
|
||||
- **Algorithm Details:** `PurePursuitSimplified.cs` (inline comments)
|
||||
|
||||
---
|
||||
|
||||
## 📊 METRICS REFERENCE
|
||||
|
||||
### Tracking Accuracy Metrics
|
||||
|
||||
**CTE RMS (Cross-Track Error):**
|
||||
- Measure: Khoảng cách vuông góc từ robot đến path
|
||||
- Unit: meters
|
||||
- Target: < 0.08m (good), < 0.05m (excellent)
|
||||
|
||||
**Heading Error RMS:**
|
||||
- Measure: Sai số góc giữa robot heading và path tangent
|
||||
- Unit: degrees
|
||||
- Target: < 5° (good), < 3° (excellent)
|
||||
|
||||
**Goal Position Error:**
|
||||
- Measure: Khoảng cách từ final position đến goal
|
||||
- Unit: meters
|
||||
- Target: < 0.05m (good), < 0.03m (excellent)
|
||||
|
||||
### Smoothness Metrics
|
||||
|
||||
**Max Jerk:**
|
||||
- Measure: Tốc độ thay đổi acceleration lớn nhất
|
||||
- Unit: m/s³
|
||||
- Target: < 5.0 (good), < 3.0 (excellent)
|
||||
|
||||
**Velocity StdDev:**
|
||||
- Measure: Độ ổn định của velocity
|
||||
- Unit: m/s
|
||||
- Target: < 0.2 (good), < 0.1 (excellent)
|
||||
|
||||
### Efficiency Metrics
|
||||
|
||||
**Path Length Ratio:**
|
||||
- Measure: Actual path length / Reference path length
|
||||
- Target: 1.0-1.05 (good), 1.0-1.02 (excellent)
|
||||
|
||||
**Completion Time:**
|
||||
- Measure: Thời gian hoàn thành so với expected
|
||||
- Depends on: MaxLinearVelocity, path complexity
|
||||
|
||||
---
|
||||
|
||||
**Document Version:** 2.0
|
||||
**Last Updated:** 2026-02-01
|
||||
**Changelog:**
|
||||
- v2.0 (2026-02-01): Added Adaptive PP parameters, 3-phase final approach, comprehensive parameter docs
|
||||
- v1.0 (2026-01-27): Initial manual tuning workflow
|
||||
Reference in New Issue
Block a user