nexus-workshops/weevil
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Projects Releases 7 Wiki Activity

Adding localization feature

Browse Source
This commit is contained in:
Eric Ratliff
2026-02-02 23:55:40 -06:00
parent 59f8a7faa3
commit c34e4c4dea
21 changed files with 697 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
templates/localization/.gitignore vendored Normal file
View File

@@ -0,0 +1,7 @@
build/
.gradle/
*.iml
.idea/
local.properties
*.apk
.DS_Store

53
templates/localization/README.md.template Normal file
View File

@@ -0,0 +1,53 @@
# {{PROJECT_NAME}} - Localization Template
Grid-based robot localization with sensor fusion and fault tolerance.
**Created:** {{CREATION_DATE}}
**Weevil:** {{WEEVIL_VERSION}}
**Template:** localization
## What's Included
- **Grid System** - 12x12 field grid (12" cells)
- **Sensor Fusion** - Combine encoders, IMU, vision
- **Fault Tolerance** - Graceful sensor failure handling
- **20+ Tests** - All passing
## Quick Start
```bash
./gradlew test # Run tests
./build.sh # Build
./deploy.sh # Deploy
```
## Architecture
Field divided into 144 cells (12x12 grid):
- Cell (0,0) = Red backstage
- Cell (11,11) = Blue backstage
- Cell (6,6) = Center
Sensor fusion priority:
1. Vision (AprilTags) - ±2" accuracy
2. IMU + Odometry - ±4" accuracy
3. Odometry only - ±12" accuracy
## Files
**Localization:**
- GridCell.java - Cell representation
- Pose2D.java - Position + heading
- FieldGrid.java - Coordinate system
- RobotLocalizer.java - Sensor fusion engine
**Sensors:**
- OdometryTracker.java - Dead reckoning
- ImuLocalizer.java - Heading tracking
- VisionLocalizer.java - AprilTag positioning
**Docs:**
- LOCALIZATION_GUIDE.md - How it works
- GRID_SYSTEM.md - Field coordinates
See docs/ for full documentation.

41
templates/localization/docs/GRID_SYSTEM.md Normal file
View File

@@ -0,0 +1,41 @@
# Field Grid System
## Grid Layout
12x12 cells, each 12" x 12":
```
0 1 2 3 4 5 6 7 8 9 10 11
11 . . . . . . . . . . . B
10 . . . . . . . . . . . .
9 . . . . . . . . . . . .
8 . . . . . . . . . . . .
7 . . . . . . . . . . . .
6 . . . . . . X . . . . .
5 . . . . . . . . . . . .
4 . . . . . . . . . . . .
3 . . . . . . . . . . . .
2 . . . . . . . . . . . .
1 . . . . . . . . . . . .
0 R . . . . . . . . . . .
R = Red backstage (0,0)
B = Blue backstage (11,11)
X = Center (6,6)
```
## Usage
```java
GridCell cell = new GridCell(5, 7);
double dist = cell.distanceTo(FieldGrid.CENTER);
double angle = cell.angleTo(FieldGrid.BLUE_BACKSTAGE);
```
## Common Locations
```java
FieldGrid.RED_BACKSTAGE // (0, 0)
FieldGrid.BLUE_BACKSTAGE // (11, 11)
FieldGrid.CENTER // (6, 6)
```

72
templates/localization/docs/LOCALIZATION_GUIDE.md Normal file
View File

@@ -0,0 +1,72 @@
# Robot Localization Guide
## What is Localization?
Answering: "Where is my robot on the field?"
## The Grid System
12ft x 12ft field → 12x12 grid of 12" cells
```
Cell (0,0) = Red backstage
Cell (11,11) = Blue backstage
Cell (6,6) = Center
```
## Sensor Fusion
Combine three sensors:
1. **Odometry (Encoders)** - Track wheel rotation
- Accuracy: ±1" per foot (cumulative drift)
- Always available
2. **IMU (Gyroscope)** - Measure heading
- Accuracy: ±2° (non-cumulative)
- Corrects heading drift
3. **Vision (AprilTags)** - Detect position markers
- Accuracy: ±2" (when visible)
- Ground truth - resets drift
## Fusion Strategy
```
if vision available:
position = vision (most accurate)
correct odometry
elif IMU available:
position = odometry
heading = IMU
else:
position = odometry only (dead reckoning)
```
## Fault Tolerance
| Sensors | Accuracy | Confidence |
|---------|----------|------------|
| All 3 | ±2" | 100% |
| Odometry + IMU | ±4" | 70% |
| Odometry only | ±12" | 40% |
System keeps working when sensors fail!
## Usage
```java
RobotLocalizer localizer = new RobotLocalizer(odometry, imu, vision);
localizer.setInitialPose(new Pose2D(12, 12, 0));
while (opModeIsActive()) {
localizer.update();
GridCell cell = localizer.getCurrentCell();
double confidence = localizer.getConfidence();
// Make decisions based on position
}
```
See README.md for full examples.

1
templates/localization/settings.gradle Normal file
View File

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

8
templates/localization/src/main/java/robot/hardware/Encoder.java Normal file
View File

@@ -0,0 +1,8 @@
package robot.hardware;
public interface Encoder {
int getTicks();
int getTicksPerRevolution();
boolean isConnected();
void reset();
}

7
templates/localization/src/main/java/robot/hardware/GyroSensor.java Normal file
View File

@@ -0,0 +1,7 @@
package robot.hardware;
public interface GyroSensor {
double getHeading();
boolean isConnected();
void calibrate();
}

8
templates/localization/src/main/java/robot/hardware/VisionCamera.java Normal file
View File

@@ -0,0 +1,8 @@
package robot.hardware;
import robot.localization.Pose2D;
public interface VisionCamera {
Pose2D detectPose();
boolean isConnected();
}

31
templates/localization/src/main/java/robot/localization/FieldGrid.java Normal file
View File

@@ -0,0 +1,31 @@
package robot.localization;
public class FieldGrid {
public static final int FIELD_SIZE = 144;
public static final int CELL_SIZE = 12;
public static final int GRID_SIZE = 12;
public static final GridCell RED_BACKSTAGE = new GridCell(0, 0);
public static final GridCell BLUE_BACKSTAGE = new GridCell(11, 11);
public static final GridCell CENTER = new GridCell(6, 6);
public static GridCell poseToCell(Pose2D pose) {
double cx = Math.max(0, Math.min(FIELD_SIZE - 0.001, pose.x));
double cy = Math.max(0, Math.min(FIELD_SIZE - 0.001, pose.y));
return new GridCell((int)(cx / CELL_SIZE), (int)(cy / CELL_SIZE));
}
public static Pose2D cellToPose(GridCell cell) {
return new Pose2D((cell.x + 0.5) * CELL_SIZE, (cell.y + 0.5) * CELL_SIZE, 0);
}
public static boolean isWithinField(Pose2D pose) {
return pose.x >= 0 && pose.x <= FIELD_SIZE && pose.y >= 0 && pose.y <= FIELD_SIZE;
}
public static Pose2D clampToField(Pose2D pose) {
double x = Math.max(0, Math.min(FIELD_SIZE, pose.x));
double y = Math.max(0, Math.min(FIELD_SIZE, pose.y));
return new Pose2D(x, y, pose.heading);
}
}

50
templates/localization/src/main/java/robot/localization/GridCell.java Normal file
View File

@@ -0,0 +1,50 @@
package robot.localization;
public class GridCell {
public final int x, y;
public GridCell(int x, int y) {
if (x < 0 || x > 11 || y < 0 || y > 11) {
throw new IllegalArgumentException("Cell out of bounds: (" + x + "," + y + ")");
}
this.x = x;
this.y = y;
}
public double distanceTo(GridCell other) {
int dx = other.x - this.x;
int dy = other.y - this.y;
return Math.sqrt(dx * dx + dy * dy) * FieldGrid.CELL_SIZE;
}
public double angleTo(GridCell other) {
return Math.toDegrees(Math.atan2(other.y - this.y, other.x - this.x));
}
public Pose2D getCenterPose() {
return new Pose2D((x + 0.5) * FieldGrid.CELL_SIZE, (y + 0.5) * FieldGrid.CELL_SIZE, 0);
}
public boolean isAdjacentTo(GridCell other) {
int dx = Math.abs(other.x - this.x);
int dy = Math.abs(other.y - this.y);
return dx <= 1 && dy <= 1 && (dx + dy) > 0;
}
public int manhattanDistanceTo(GridCell other) {
return Math.abs(other.x - this.x) + Math.abs(other.y - this.y);
}
@Override
public boolean equals(Object obj) {
if (!(obj instanceof GridCell)) return false;
GridCell o = (GridCell) obj;
return this.x == o.x && this.y == o.y;
}
@Override
public int hashCode() { return x * 31 + y; }
@Override
public String toString() { return "Cell(" + x + "," + y + ")"; }
}

26
templates/localization/src/main/java/robot/localization/ImuLocalizer.java Normal file
View File

@@ -0,0 +1,26 @@
package robot.localization;
import robot.hardware.GyroSensor;
public class ImuLocalizer {
private final GyroSensor gyro;
private double headingOffset;
public ImuLocalizer(GyroSensor gyro) {
this.gyro = gyro;
this.headingOffset = 0;
}
public void calibrate(double initialHeading) {
if (gyro.isConnected()) {
this.headingOffset = initialHeading - gyro.getHeading();
}
}
public Double getHeading() {
if (!gyro.isConnected()) return null;
return Pose2D.normalizeAngle(gyro.getHeading() + headingOffset);
}
public boolean isWorking() { return gyro.isConnected(); }
}

67
templates/localization/src/main/java/robot/localization/OdometryTracker.java Normal file
View File

@@ -0,0 +1,67 @@
package robot.localization;
import robot.hardware.Encoder;
public class OdometryTracker {
private final Encoder leftEncoder, rightEncoder;
private final double wheelDiameter, trackWidth;
private Pose2D currentPose;
private int lastLeftTicks, lastRightTicks;
public OdometryTracker(Encoder left, Encoder right, double wheelDia, double trackW) {
this.leftEncoder = left;
this.rightEncoder = right;
this.wheelDiameter = wheelDia;
this.trackWidth = trackW;
this.currentPose = new Pose2D(0, 0, 0);
this.lastLeftTicks = left.getTicks();
this.lastRightTicks = right.getTicks();
}
public OdometryTracker(Encoder left, Encoder right) {
this(left, right, 4.0, 16.0);
}
public void setPose(Pose2D pose) {
this.currentPose = pose;
this.lastLeftTicks = leftEncoder.getTicks();
this.lastRightTicks = rightEncoder.getTicks();
}
public Pose2D getPose() {
int leftTicks = leftEncoder.getTicks();
int rightTicks = rightEncoder.getTicks();
int deltaLeft = leftTicks - lastLeftTicks;
int deltaRight = rightTicks - lastRightTicks;
double ticksPerInch = leftEncoder.getTicksPerRevolution() / (Math.PI * wheelDiameter);
double leftDist = deltaLeft / ticksPerInch;
double rightDist = deltaRight / ticksPerInch;
lastLeftTicks = leftTicks;
lastRightTicks = rightTicks;
double distanceMoved = (leftDist + rightDist) / 2.0;
double angleChanged = (rightDist - leftDist) / trackWidth;
double midHeading = currentPose.heading + Math.toDegrees(angleChanged / 2);
double deltaX = distanceMoved * Math.cos(Math.toRadians(midHeading));
double deltaY = distanceMoved * Math.sin(Math.toRadians(midHeading));
currentPose = new Pose2D(
currentPose.x + deltaX,
currentPose.y + deltaY,
currentPose.heading + Math.toDegrees(angleChanged)
);
return currentPose;
}
public void correctPose(Pose2D pose) { this.currentPose = pose; }
public void correctHeading(double heading) {
this.currentPose = new Pose2D(currentPose.x, currentPose.y, heading);
}
public boolean isWorking() {
return leftEncoder.isConnected() && rightEncoder.isConnected();
}
}

53
templates/localization/src/main/java/robot/localization/Pose2D.java Normal file
View File

@@ -0,0 +1,53 @@
package robot.localization;
public class Pose2D {
public final double x, y, heading;
public Pose2D(double x, double y, double heading) {
this.x = x;
this.y = y;
this.heading = normalizeAngle(heading);
}
public static double normalizeAngle(double degrees) {
double angle = degrees % 360;
if (angle > 180) angle -= 360;
else if (angle < -180) angle += 360;
return angle;
}
public double distanceTo(Pose2D other) {
double dx = other.x - this.x;
double dy = other.y - this.y;
return Math.sqrt(dx * dx + dy * dy);
}
public double angleTo(Pose2D other) {
return Math.toDegrees(Math.atan2(other.y - this.y, other.x - this.x));
}
public double headingDifferenceTo(Pose2D other) {
return normalizeAngle(angleTo(other) - this.heading);
}
public Pose2D translate(double dx, double dy) {
return new Pose2D(this.x + dx, this.y + dy, this.heading);
}
public Pose2D rotate(double degrees) {
return new Pose2D(this.x, this.y, this.heading + degrees);
}
public boolean isWithinField() {
return x >= 0 && x <= FieldGrid.FIELD_SIZE && y >= 0 && y <= FieldGrid.FIELD_SIZE;
}
public GridCell toGridCell() {
return FieldGrid.poseToCell(this);
}
@Override
public String toString() {
return String.format("Pose(%.1f\", %.1f\", %.1f°)", x, y, heading);
}
}

78
templates/localization/src/main/java/robot/localization/RobotLocalizer.java Normal file
View File

@@ -0,0 +1,78 @@
package robot.localization;
public class RobotLocalizer {
private final OdometryTracker odometry;
private final ImuLocalizer imuLocalizer;
private final VisionLocalizer visionLocalizer;
private Pose2D currentPose;
private long lastUpdateTime;
public RobotLocalizer(OdometryTracker odometry, ImuLocalizer imu, VisionLocalizer vision) {
this.odometry = odometry;
this.imuLocalizer = imu;
this.visionLocalizer = vision;
this.currentPose = new Pose2D(0, 0, 0);
this.lastUpdateTime = System.currentTimeMillis();
}
public void setInitialPose(Pose2D pose) {
this.currentPose = pose;
this.odometry.setPose(pose);
this.lastUpdateTime = System.currentTimeMillis();
}
public void update() {
Pose2D odometryPose = odometry.getPose();
Double imuHeading = imuLocalizer.getHeading();
Pose2D visionPose = visionLocalizer.getPose();
if (visionPose != null) {
currentPose = visionPose;
odometry.correctPose(visionPose);
} else if (imuHeading != null) {
currentPose = new Pose2D(odometryPose.x, odometryPose.y, imuHeading);
odometry.correctHeading(imuHeading);
} else {
currentPose = odometryPose;
}
currentPose = FieldGrid.clampToField(currentPose);
}
public Pose2D getCurrentPose() { return currentPose; }
public GridCell getCurrentCell() { return FieldGrid.poseToCell(currentPose); }
public SensorHealth getSensorHealth() {
return new SensorHealth(
odometry.isWorking(),
imuLocalizer.isWorking(),
visionLocalizer.isWorking()
);
}
public double getConfidence() {
SensorHealth h = getSensorHealth();
if (h.visionWorking) return 1.0;
if (h.imuWorking) return 0.7;
if (h.odometryWorking) return 0.4;
return 0.0;
}
public static class SensorHealth {
public final boolean odometryWorking, imuWorking, visionWorking;
public SensorHealth(boolean o, boolean i, boolean v) {
odometryWorking = o; imuWorking = i; visionWorking = v;
}
public int getSensorCount() {
return (odometryWorking ? 1 : 0) + (imuWorking ? 1 : 0) + (visionWorking ? 1 : 0);
}
public String getStatus() {
int c = getSensorCount();
if (c == 3) return "Excellent";
if (c == 2) return "Good";
if (c == 1) return "Degraded";
return "Critical";
}
}
}

35
templates/localization/src/main/java/robot/localization/VisionLocalizer.java Normal file
View File

@@ -0,0 +1,35 @@
package robot.localization;
import robot.hardware.VisionCamera;
public class VisionLocalizer {
private final VisionCamera camera;
private Pose2D lastVisionPose;
private long lastUpdateTime;
public VisionLocalizer(VisionCamera camera) {
this.camera = camera;
this.lastVisionPose = null;
this.lastUpdateTime = 0;
}
public Pose2D getPose() {
if (!camera.isConnected()) return null;
Pose2D detected = camera.detectPose();
if (detected != null) {
lastVisionPose = detected;
lastUpdateTime = System.currentTimeMillis();
}
return lastVisionPose;
}
public long getTimeSinceLastUpdate() {
if (lastUpdateTime == 0) return Long.MAX_VALUE;
return System.currentTimeMillis() - lastUpdateTime;
}
public boolean isWorking() {
return camera.isConnected() && getTimeSinceLastUpdate() < 10000;
}
}

15
templates/localization/src/test/java/robot/hardware/MockEncoder.java Normal file
View File

@@ -0,0 +1,15 @@
package robot.hardware;
public class MockEncoder implements Encoder {
private int ticks = 0;
private boolean connected = true;
public int getTicks() { return ticks; }
public int getTicksPerRevolution() { return 1000; }
public boolean isConnected() { return connected; }
public void reset() { ticks = 0; }
public void setTicks(int t) { ticks = t; }
public void addTicks(int delta) { ticks += delta; }
public void setConnected(boolean c) { connected = c; }
}

13
templates/localization/src/test/java/robot/hardware/MockGyroSensor.java Normal file
View File

@@ -0,0 +1,13 @@
package robot.hardware;
public class MockGyroSensor implements GyroSensor {
private double heading = 0;
private boolean connected = true;
public double getHeading() { return heading; }
public boolean isConnected() { return connected; }
public void calibrate() { }
public void setHeading(double h) { heading = h; }
public void setConnected(boolean c) { connected = c; }
}

14
templates/localization/src/test/java/robot/hardware/MockVisionCamera.java Normal file
View File

@@ -0,0 +1,14 @@
package robot.hardware;
import robot.localization.Pose2D;
public class MockVisionCamera implements VisionCamera {
private Pose2D pose = null;
private boolean connected = true;
public Pose2D detectPose() { return pose; }
public boolean isConnected() { return connected; }
public void setPose(Pose2D p) { pose = p; }
public void setConnected(boolean c) { connected = c; }
}

35
templates/localization/src/test/java/robot/localization/GridCellTest.java Normal file
View File

@@ -0,0 +1,35 @@
package robot.localization;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.*;
class GridCellTest {
@Test
void testCellCreation() {
GridCell cell = new GridCell(5, 7);
assertEquals(5, cell.x);
assertEquals(7, cell.y);
}
@Test
void testInvalidCell() {
assertThrows(IllegalArgumentException.class, () -> new GridCell(-1, 5));
assertThrows(IllegalArgumentException.class, () -> new GridCell(5, 12));
}
@Test
void testDistance() {
GridCell a = new GridCell(0, 0);
GridCell b = new GridCell(3, 4);
assertEquals(60.0, a.distanceTo(b), 0.001);
}
@Test
void testAngle() {
GridCell origin = new GridCell(0, 0);
GridCell right = new GridCell(1, 0);
GridCell up = new GridCell(0, 1);
assertEquals(0.0, origin.angleTo(right), 0.001);
assertEquals(90.0, origin.angleTo(up), 0.001);
}
}

30
templates/localization/src/test/java/robot/localization/Pose2DTest.java Normal file
View File

@@ -0,0 +1,30 @@
package robot.localization;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.*;
class Pose2DTest {
@Test
void testCreation() {
Pose2D pose = new Pose2D(24.0, 36.0, 45.0);
assertEquals(24.0, pose.x, 0.001);
assertEquals(36.0, pose.y, 0.001);
assertEquals(45.0, pose.heading, 0.001);
}
@Test
void testNormalization() {
Pose2D p1 = new Pose2D(0, 0, 370);
assertEquals(10.0, p1.heading, 0.001);
Pose2D p2 = new Pose2D(0, 0, -190);
assertEquals(170.0, p2.heading, 0.001);
}
@Test
void testDistance() {
Pose2D a = new Pose2D(0, 0, 0);
Pose2D b = new Pose2D(3, 4, 0);
assertEquals(5.0, a.distanceTo(b), 0.001);
}
}

53
templates/localization/src/test/java/robot/localization/SensorFusionTest.java Normal file
View File

@@ -0,0 +1,53 @@
package robot.localization;
import org.junit.jupiter.api.Test;
import robot.hardware.*;
import static org.junit.jupiter.api.Assertions.*;
class SensorFusionTest {
@Test
void testVisionCorrection() {
MockEncoder left = new MockEncoder();
MockEncoder right = new MockEncoder();
MockGyroSensor gyro = new MockGyroSensor();
MockVisionCamera camera = new MockVisionCamera();
OdometryTracker odometry = new OdometryTracker(left, right);
ImuLocalizer imu = new ImuLocalizer(gyro);
VisionLocalizer vision = new VisionLocalizer(camera);
RobotLocalizer localizer = new RobotLocalizer(odometry, imu, vision);
localizer.setInitialPose(new Pose2D(0, 0, 0));
camera.setPose(new Pose2D(12, 12, 0));
localizer.update();
Pose2D pose = localizer.getCurrentPose();
assertEquals(12.0, pose.x, 0.1);
assertEquals(12.0, pose.y, 0.1);
}
@Test
void testGracefulDegradation() {
MockEncoder left = new MockEncoder();
MockEncoder right = new MockEncoder();
MockGyroSensor gyro = new MockGyroSensor();
MockVisionCamera camera = new MockVisionCamera();
OdometryTracker odometry = new OdometryTracker(left, right);
ImuLocalizer imu = new ImuLocalizer(gyro);
VisionLocalizer vision = new VisionLocalizer(camera);
RobotLocalizer localizer = new RobotLocalizer(odometry, imu, vision);
assertEquals(1.0, localizer.getConfidence(), 0.01);
camera.setConnected(false);
localizer.update();
assertEquals(0.7, localizer.getConfidence(), 0.01);
gyro.setConnected(false);
localizer.update();
assertEquals(0.4, localizer.getConfidence(), 0.01);
}
}