feat: Add localization template with grid-based sensor fusion
Implements comprehensive robot localization system as third template option. Teams can now start with professional positioning and navigation code. Template Features: - 12x12 field grid system (12-inch cells) - Multi-sensor fusion (encoders, IMU, vision) - Kalman-filter-style sensor combination - Fault-tolerant positioning (graceful degradation) - 21 files, ~1,500 lines, 3 passing tests Core Components: - GridCell/Pose2D/FieldGrid - Coordinate system - RobotLocalizer - Sensor fusion engine - OdometryTracker - Dead reckoning from encoders - ImuLocalizer - Heading from gyroscope - VisionLocalizer - Position from AprilTags Sensor Fusion Strategy: Priority 1: Vision (AprilTags) → ±2" accuracy, 100% confidence Priority 2: IMU + Odometry → ±4" accuracy, 70% confidence Priority 3: Odometry only → ±12" accuracy, 40% confidence System gracefully degrades when sensors fail, maintaining operation even with partial sensor availability. Hardware Abstraction: - Interfaces (Encoder, GyroSensor, VisionCamera) - Mock implementations for unit testing - Teams implement FTC wrappers for their hardware Documentation: - LOCALIZATION_GUIDE.md - System architecture and usage - GRID_SYSTEM.md - Field coordinate reference - README.md - Quick start and examples Usage: weevil new my-robot --template localization cd my-robot ./gradlew test # 3 tests pass in < 1 second This teaches professional robotics concepts (sensor fusion, fault tolerance, coordinate systems) not found in other FTC tools. Positions Nexus Workshops as teaching advanced autonomous programming. Updated src/templates/mod.rs to register localization template with proper metadata and feature descriptions. All tests passing (10/10 template tests).
This commit is contained in:
@@ -11,7 +11,7 @@ Grid-based robot localization with sensor fusion and fault tolerance.
|
||||
- **Grid System** - 12x12 field grid (12" cells)
|
||||
- **Sensor Fusion** - Combine encoders, IMU, vision
|
||||
- **Fault Tolerance** - Graceful sensor failure handling
|
||||
- **20+ Tests** - All passing
|
||||
- **3 Tests** - All passing
|
||||
|
||||
## Quick Start
|
||||
|
||||
|
||||
@@ -11,7 +11,7 @@
|
||||
9 . . . . . . . . . . . .
|
||||
8 . . . . . . . . . . . .
|
||||
7 . . . . . . . . . . . .
|
||||
6 . . . . . . X . . . . .
|
||||
6 . . . . . X . . . . . .
|
||||
5 . . . . . . . . . . . .
|
||||
4 . . . . . . . . . . . .
|
||||
3 . . . . . . . . . . . .
|
||||
|
||||
@@ -1 +0,0 @@
|
||||
rootProject.name = '{{PROJECT_NAME}}'
|
||||
17
templates/localization/settings.gradle.kts.template
Normal file
17
templates/localization/settings.gradle.kts.template
Normal file
@@ -0,0 +1,17 @@
|
||||
pluginManagement {
|
||||
repositories {
|
||||
google()
|
||||
mavenCentral()
|
||||
gradlePluginPortal()
|
||||
}
|
||||
}
|
||||
|
||||
dependencyResolutionManagement {
|
||||
repositoriesMode.set(RepositoriesMode.PREFER_SETTINGS)
|
||||
repositories {
|
||||
mavenCentral()
|
||||
google()
|
||||
}
|
||||
}
|
||||
|
||||
rootProject.name = "{{PROJECT_NAME}}"
|
||||
@@ -4,4 +4,5 @@ public interface GyroSensor {
|
||||
double getHeading();
|
||||
boolean isConnected();
|
||||
void calibrate();
|
||||
boolean isCalibrated();
|
||||
}
|
||||
|
||||
@@ -13,14 +13,17 @@ public class ImuLocalizer {
|
||||
|
||||
public void calibrate(double initialHeading) {
|
||||
if (gyro.isConnected()) {
|
||||
gyro.calibrate();
|
||||
this.headingOffset = initialHeading - gyro.getHeading();
|
||||
}
|
||||
}
|
||||
|
||||
public Double getHeading() {
|
||||
if (!gyro.isConnected()) return null;
|
||||
if (!gyro.isConnected() || !gyro.isCalibrated()) return null;
|
||||
return Pose2D.normalizeAngle(gyro.getHeading() + headingOffset);
|
||||
}
|
||||
|
||||
public boolean isWorking() { return gyro.isConnected(); }
|
||||
public boolean isWorking() {
|
||||
return gyro.isConnected() && gyro.isCalibrated();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -3,11 +3,14 @@ package robot.hardware;
|
||||
public class MockGyroSensor implements GyroSensor {
|
||||
private double heading = 0;
|
||||
private boolean connected = true;
|
||||
private boolean calibrated = true;
|
||||
|
||||
public double getHeading() { return heading; }
|
||||
public boolean isConnected() { return connected; }
|
||||
public void calibrate() { }
|
||||
public void calibrate() { calibrated = true; }
|
||||
public boolean isCalibrated() { return calibrated; }
|
||||
|
||||
public void setHeading(double h) { heading = h; }
|
||||
public void setConnected(boolean c) { connected = c; }
|
||||
public void setCalibrated(boolean c) { calibrated = c; }
|
||||
}
|
||||
|
||||
@@ -34,11 +34,15 @@ class SensorFusionTest {
|
||||
MockGyroSensor gyro = new MockGyroSensor();
|
||||
MockVisionCamera camera = new MockVisionCamera();
|
||||
|
||||
// Set a pose so vision is actually "working" (not just connected)
|
||||
camera.setPose(new Pose2D(12, 12, 0));
|
||||
|
||||
OdometryTracker odometry = new OdometryTracker(left, right);
|
||||
ImuLocalizer imu = new ImuLocalizer(gyro);
|
||||
VisionLocalizer vision = new VisionLocalizer(camera);
|
||||
|
||||
RobotLocalizer localizer = new RobotLocalizer(odometry, imu, vision);
|
||||
localizer.update(); // Need to update to actually use vision
|
||||
|
||||
assertEquals(1.0, localizer.getConfidence(), 0.01);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user