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:
Eric Ratliff
2026-02-03 00:46:00 -06:00
parent b07e8c7dab
commit 636e1252dc
9 changed files with 80 additions and 10 deletions

View File

@@ -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

View File

@@ -11,7 +11,7 @@
9 . . . . . . . . . . . .
8 . . . . . . . . . . . .
7 . . . . . . . . . . . .
6 . . . . . . X . . . . .
6 . . . . . X . . . . . .
5 . . . . . . . . . . . .
4 . . . . . . . . . . . .
3 . . . . . . . . . . . .

View File

@@ -1 +0,0 @@
rootProject.name = '{{PROJECT_NAME}}'

View File

@@ -0,0 +1,17 @@
pluginManagement {
repositories {
google()
mavenCentral()
gradlePluginPortal()
}
}
dependencyResolutionManagement {
repositoriesMode.set(RepositoriesMode.PREFER_SETTINGS)
repositories {
mavenCentral()
google()
}
}
rootProject.name = "{{PROJECT_NAME}}"

View File

@@ -4,4 +4,5 @@ public interface GyroSensor {
double getHeading();
boolean isConnected();
void calibrate();
boolean isCalibrated();
}

View File

@@ -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();
}
}

View File

@@ -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; }
}

View File

@@ -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);