6 Commits

Author SHA1 Message Date
Eric Ratliff
9af6015aa3 Updated version to an RC1 release 2026-02-03 10:52:08 -06:00
Eric Ratliff
9c2ac97158 Changed from 2 to 3 templates in the test 2026-02-03 09:26:39 -06:00
Eric Ratliff
e6934cdb18 fix: Use full path to cmd.exe in Android Studio run configurations
Android Studio's Shell Script plugin cannot find cmd.exe when specified
as just "cmd.exe" - it doesn't search the system PATH. This caused
"Interpreter not found" errors when trying to run Build, Deploy, or Test
configurations on Windows.

Changed all Windows run configurations to use the full path:
C:\Windows\System32\cmd.exe

This fixes 5 run configurations:
- Build (Windows)
- Deploy (auto) (Windows)
- Deploy (USB) (Windows)
- Deploy (WiFi) (Windows)
- Test (Windows)

Unix configurations already used full paths (/bin/bash) so they were
unaffected.

Tested on Windows 11 with Android Studio - configurations now work
correctly without manual editing.

Fixes issue where users couldn't run any Android Studio configurations
on Windows without manually editing the interpreter path.
2026-02-03 08:40:25 -06:00
Eric Ratliff
636e1252dc 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).
2026-02-03 00:46:00 -06:00
Eric Ratliff
b07e8c7dab Updated roadmap with more 1.2.0 ideas 2026-02-03 00:07:14 -06:00
Eric Ratliff
c34e4c4dea Adding localization feature 2026-02-02 23:55:40 -06:00
27 changed files with 890 additions and 14 deletions

View File

@@ -1,6 +1,6 @@
[package] [package]
name = "weevil" name = "weevil"
version = "1.1.0-beta.2" version = "1.1.0-rc1"
edition = "2021" edition = "2021"
authors = ["Eric Ratliff <eric@nxlearn.net>"] authors = ["Eric Ratliff <eric@nxlearn.net>"]
description = "FTC robotics project generator - bores into complexity, emerges with clean code" description = "FTC robotics project generator - bores into complexity, emerges with clean code"

View File

@@ -175,12 +175,121 @@ This is the kind of code students would write if they had years of experience. N
--- ---
## Version 1.2.0 - Package Ecosystem 🔥 ## Version 1.2.0 - Package Ecosystem & Enhanced Templates 🔥
**Theme:** Transforming Weevil from project generator to ecosystem platform. Teams can extend projects with community-shared components. **Theme:** Transforming Weevil from project generator to ecosystem platform. Teams can extend projects with community-shared components and additional professional templates.
**Status:** Planning - Expected Q2 2026 **Status:** Planning - Expected Q2 2026
### Template Metadata & Improved Display ⚠️
**Feature:** Template discovery with rich metadata and formatted display
**Description:** Enhance template system with metadata files for better discoverability:
**Template Metadata (`template.toml`):**
```toml
[template]
name = "localization"
description = "Grid-based robot localization with sensor fusion"
file_count = 21
test_count = 3
perfect_for = "Advanced autonomous navigation"
[includes]
items = [
"12x12 field grid system",
"Multi-sensor fusion (encoders + IMU + vision)",
"Fault-tolerant positioning"
]
```
**Improved `--list-templates` Output:**
```
Available templates:
basic (default)
Minimal FTC project structure
Files: ~10 | Tests: 1
Perfect for: Teams starting from scratch
testing
Professional testing showcase with examples
Files: ~30 | Tests: 45
Perfect for: Learning best practices
Includes:
• 3 complete subsystems
• Hardware abstraction layer
• 45 passing tests
localization
Grid-based robot localization with sensor fusion
Files: ~21 | Tests: 3
Perfect for: Advanced autonomous navigation
Includes:
• 12x12 field grid system
• Multi-sensor fusion
• Fault-tolerant positioning
Usage:
weevil new <project-name> # Uses basic
weevil new <project-name> --template testing # Uses testing
```
**Status:** Planned for v1.2.0
**Priority:** MEDIUM-HIGH - Improves template discoverability
**Effort:** 1-2 days (metadata format + CLI formatting)
---
### Localization Template ⚠️
**Feature:** Grid-based localization template for advanced autonomous
**Description:** Professional robot localization system with sensor fusion and fault tolerance:
**What's Included:**
- **Grid System** - 12ft x 12ft field divided into 12x12 grid (12" cells)
- **Sensor Fusion** - Combine odometry (encoders), IMU (gyro), and vision (AprilTags)
- **Fault Tolerance** - Graceful degradation when sensors fail
- **Lookup Tables** - Pre-computed navigation strategies
- **21 Files** - 7 localization classes, 3 hardware interfaces, 3 mocks, 3 tests, 2 docs
- **3 Comprehensive Tests** - Grid math, sensor fusion, fault tolerance
**Core Components:**
- `GridCell.java` - Cell in 12x12 grid (0-11, 0-11)
- `Pose2D.java` - Position (x, y) + heading with angle normalization
- `FieldGrid.java` - Field coordinate system and conversions
- `RobotLocalizer.java` - Multi-sensor fusion engine
- `OdometryTracker.java` - Dead reckoning from wheel encoders
- `ImuLocalizer.java` - Heading tracking from gyroscope
- `VisionLocalizer.java` - Position from AprilTags
**Sensor Fusion Priority:**
1. Vision (AprilTags) ±2" accuracy, 100% confidence
2. IMU + Odometry ±4" accuracy, 70% confidence
3. Odometry only ±12" accuracy, 40% confidence
**Rationale:** Teaches professional robotics concepts (Kalman-filter-style sensor fusion, fault tolerance, grid-based navigation). No other FTC tool has this. Positions Nexus Workshops as teaching advanced autonomous programming.
**Usage:**
```bash
weevil new my-robot --template localization
cd my-robot
./gradlew test # 3 tests pass
# Modify for your robot's sensors and autonomous strategy
```
**Status:** Template complete, ready to integrate in v1.2.0
**Priority:** HIGH - Unique differentiator, advanced autonomous capability
**Effort:** Template already created, integration 1 day
---
### `weevil add` - Component Package Manager ⚠️ **THE NEXT BIG THING** ### `weevil add` - Component Package Manager ⚠️ **THE NEXT BIG THING**
**Feature:** Package manager for sharing and reusing FTC robot code components **Feature:** Package manager for sharing and reusing FTC robot code components

View File

@@ -513,7 +513,7 @@ class BasicTest {
<option name="INDEPENDENT_SCRIPT_WORKING_DIRECTORY" value="true" /> <option name="INDEPENDENT_SCRIPT_WORKING_DIRECTORY" value="true" />
<option name="SCRIPT_WORKING_DIRECTORY" value="$PROJECT_DIR$" /> <option name="SCRIPT_WORKING_DIRECTORY" value="$PROJECT_DIR$" />
<option name="INDEPENDENT_INTERPRETER_PATH" value="true" /> <option name="INDEPENDENT_INTERPRETER_PATH" value="true" />
<option name="INTERPRETER_PATH" value="cmd.exe" /> <option name="INTERPRETER_PATH" value="C:\Windows\System32\cmd.exe" />
<option name="INTERPRETER_OPTIONS" value="/c" /> <option name="INTERPRETER_OPTIONS" value="/c" />
<option name="EXECUTE_IN_TERMINAL" value="true" /> <option name="EXECUTE_IN_TERMINAL" value="true" />
<option name="EXECUTE_SCRIPT_FILE" value="true" /> <option name="EXECUTE_SCRIPT_FILE" value="true" />
@@ -561,7 +561,7 @@ class BasicTest {
<option name="INDEPENDENT_SCRIPT_WORKING_DIRECTORY" value="true" /> <option name="INDEPENDENT_SCRIPT_WORKING_DIRECTORY" value="true" />
<option name="SCRIPT_WORKING_DIRECTORY" value="$PROJECT_DIR$" /> <option name="SCRIPT_WORKING_DIRECTORY" value="$PROJECT_DIR$" />
<option name="INDEPENDENT_INTERPRETER_PATH" value="true" /> <option name="INDEPENDENT_INTERPRETER_PATH" value="true" />
<option name="INTERPRETER_PATH" value="cmd.exe" /> <option name="INTERPRETER_PATH" value="C:\Windows\System32\cmd.exe" />
<option name="INTERPRETER_OPTIONS" value="/c" /> <option name="INTERPRETER_OPTIONS" value="/c" />
<option name="EXECUTE_IN_TERMINAL" value="true" /> <option name="EXECUTE_IN_TERMINAL" value="true" />
<option name="EXECUTE_SCRIPT_FILE" value="true" /> <option name="EXECUTE_SCRIPT_FILE" value="true" />
@@ -609,7 +609,7 @@ class BasicTest {
<option name="INDEPENDENT_SCRIPT_WORKING_DIRECTORY" value="true" /> <option name="INDEPENDENT_SCRIPT_WORKING_DIRECTORY" value="true" />
<option name="SCRIPT_WORKING_DIRECTORY" value="$PROJECT_DIR$" /> <option name="SCRIPT_WORKING_DIRECTORY" value="$PROJECT_DIR$" />
<option name="INDEPENDENT_INTERPRETER_PATH" value="true" /> <option name="INDEPENDENT_INTERPRETER_PATH" value="true" />
<option name="INTERPRETER_PATH" value="cmd.exe" /> <option name="INTERPRETER_PATH" value="C:\Windows\System32\cmd.exe" />
<option name="INTERPRETER_OPTIONS" value="/c" /> <option name="INTERPRETER_OPTIONS" value="/c" />
<option name="EXECUTE_IN_TERMINAL" value="true" /> <option name="EXECUTE_IN_TERMINAL" value="true" />
<option name="EXECUTE_SCRIPT_FILE" value="true" /> <option name="EXECUTE_SCRIPT_FILE" value="true" />
@@ -657,7 +657,7 @@ class BasicTest {
<option name="INDEPENDENT_SCRIPT_WORKING_DIRECTORY" value="true" /> <option name="INDEPENDENT_SCRIPT_WORKING_DIRECTORY" value="true" />
<option name="SCRIPT_WORKING_DIRECTORY" value="$PROJECT_DIR$" /> <option name="SCRIPT_WORKING_DIRECTORY" value="$PROJECT_DIR$" />
<option name="INDEPENDENT_INTERPRETER_PATH" value="true" /> <option name="INDEPENDENT_INTERPRETER_PATH" value="true" />
<option name="INTERPRETER_PATH" value="cmd.exe" /> <option name="INTERPRETER_PATH" value="C:\Windows\System32\cmd.exe" />
<option name="INTERPRETER_OPTIONS" value="/c" /> <option name="INTERPRETER_OPTIONS" value="/c" />
<option name="EXECUTE_IN_TERMINAL" value="true" /> <option name="EXECUTE_IN_TERMINAL" value="true" />
<option name="EXECUTE_SCRIPT_FILE" value="true" /> <option name="EXECUTE_SCRIPT_FILE" value="true" />
@@ -705,7 +705,7 @@ class BasicTest {
<option name="INDEPENDENT_SCRIPT_WORKING_DIRECTORY" value="true" /> <option name="INDEPENDENT_SCRIPT_WORKING_DIRECTORY" value="true" />
<option name="SCRIPT_WORKING_DIRECTORY" value="$PROJECT_DIR$" /> <option name="SCRIPT_WORKING_DIRECTORY" value="$PROJECT_DIR$" />
<option name="INDEPENDENT_INTERPRETER_PATH" value="true" /> <option name="INDEPENDENT_INTERPRETER_PATH" value="true" />
<option name="INTERPRETER_PATH" value="cmd.exe" /> <option name="INTERPRETER_PATH" value="C:\Windows\System32\cmd.exe" />
<option name="INTERPRETER_OPTIONS" value="/c" /> <option name="INTERPRETER_OPTIONS" value="/c" />
<option name="EXECUTE_IN_TERMINAL" value="true" /> <option name="EXECUTE_IN_TERMINAL" value="true" />
<option name="EXECUTE_SCRIPT_FILE" value="true" /> <option name="EXECUTE_SCRIPT_FILE" value="true" />

View File

@@ -8,6 +8,7 @@ use colored::*;
// Embed template directories at compile time // Embed template directories at compile time
static BASIC_TEMPLATE: Dir = include_dir!("$CARGO_MANIFEST_DIR/templates/basic"); static BASIC_TEMPLATE: Dir = include_dir!("$CARGO_MANIFEST_DIR/templates/basic");
static TESTING_TEMPLATE: Dir = include_dir!("$CARGO_MANIFEST_DIR/templates/testing"); static TESTING_TEMPLATE: Dir = include_dir!("$CARGO_MANIFEST_DIR/templates/testing");
static LOCALIZATION_TEMPLATE: Dir = include_dir!("$CARGO_MANIFEST_DIR/templates/localization");
pub struct TemplateManager { pub struct TemplateManager {
#[allow(dead_code)] #[allow(dead_code)]
@@ -39,13 +40,14 @@ impl TemplateManager {
} }
pub fn template_exists(&self, name: &str) -> bool { pub fn template_exists(&self, name: &str) -> bool {
matches!(name, "basic" | "testing") matches!(name, "basic" | "testing" | "localization")
} }
pub fn list_templates(&self) -> Vec<String> { pub fn list_templates(&self) -> Vec<String> {
vec![ vec![
" basic - Minimal FTC project (default)".to_string(), " basic - Minimal FTC project (default)".to_string(),
" testing - Testing showcase with examples".to_string(), " testing - Testing showcase with examples".to_string(),
" localization - Grid-based positioning with sensor fusion".to_string(),
] ]
} }
@@ -68,6 +70,14 @@ impl TemplateManager {
test_count: 45, test_count: 45,
is_default: false, is_default: false,
}, },
TemplateInfo {
name: "localization".to_string(),
description: "Grid-based robot localization with sensor fusion".to_string(),
file_count: 21,
line_count: 1500,
test_count: 3,
is_default: false,
},
]) ])
} }
@@ -90,6 +100,14 @@ impl TemplateManager {
test_count: 45, test_count: 45,
is_default: false, is_default: false,
}, },
"localization" => TemplateInfo {
name: "localization".to_string(),
description: "Grid-based robot localization system with multi-sensor fusion and fault tolerance.".to_string(),
file_count: 21,
line_count: 1500,
test_count: 3,
is_default: false,
},
_ => bail!("Unknown template: {}", name), _ => bail!("Unknown template: {}", name),
}; };
@@ -109,6 +127,16 @@ impl TemplateManager {
println!(); println!();
} }
if info.name == "localization" {
println!("{}", "Features:".bright_white().bold());
println!(" • 12x12 field grid system (12-inch cells)");
println!(" • Multi-sensor fusion (encoders + IMU + vision)");
println!(" • Fault-tolerant positioning (graceful degradation)");
println!(" • Kalman-filter-style sensor fusion");
println!(" • Professional robotics patterns");
println!();
}
println!("{}", "Files included:".bright_white().bold()); println!("{}", "Files included:".bright_white().bold());
println!(" {} files", info.file_count); println!(" {} files", info.file_count);
println!(" ~{} lines of code", info.line_count); println!(" ~{} lines of code", info.line_count);
@@ -132,6 +160,7 @@ impl TemplateManager {
let template_dir = match template_name { let template_dir = match template_name {
"basic" => &BASIC_TEMPLATE, "basic" => &BASIC_TEMPLATE,
"testing" => &TESTING_TEMPLATE, "testing" => &TESTING_TEMPLATE,
"localization" => &LOCALIZATION_TEMPLATE,
_ => bail!("Unknown template: {}", template_name), _ => bail!("Unknown template: {}", template_name),
}; };
@@ -233,6 +262,7 @@ mod tests {
let mgr = TemplateManager::new().unwrap(); let mgr = TemplateManager::new().unwrap();
assert!(mgr.template_exists("basic")); assert!(mgr.template_exists("basic"));
assert!(mgr.template_exists("testing")); assert!(mgr.template_exists("testing"));
assert!(mgr.template_exists("localization"));
assert!(!mgr.template_exists("nonexistent")); assert!(!mgr.template_exists("nonexistent"));
} }
@@ -240,6 +270,19 @@ mod tests {
fn test_list_templates() { fn test_list_templates() {
let mgr = TemplateManager::new().unwrap(); let mgr = TemplateManager::new().unwrap();
let templates = mgr.list_templates(); let templates = mgr.list_templates();
assert_eq!(templates.len(), 2); assert_eq!(templates.len(), 3);
assert!(templates[0].contains("basic"));
assert!(templates[1].contains("testing"));
assert!(templates[2].contains("localization"));
}
#[test]
fn test_template_info_all() {
let mgr = TemplateManager::new().unwrap();
let infos = mgr.get_template_info_all().unwrap();
assert_eq!(infos.len(), 3);
assert_eq!(infos[0].name, "basic");
assert_eq!(infos[1].name, "testing");
assert_eq!(infos[2].name, "localization");
} }
} }

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

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

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
- **3 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.

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)
```

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.

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

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

View File

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

View File

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

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

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 + ")"; }
}

View File

@@ -0,0 +1,29 @@
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()) {
gyro.calibrate();
this.headingOffset = initialHeading - gyro.getHeading();
}
}
public Double getHeading() {
if (!gyro.isConnected() || !gyro.isCalibrated()) return null;
return Pose2D.normalizeAngle(gyro.getHeading() + headingOffset);
}
public boolean isWorking() {
return gyro.isConnected() && gyro.isCalibrated();
}
}

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

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

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

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

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

View File

@@ -0,0 +1,16 @@
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() { 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

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

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

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

View File

@@ -0,0 +1,57 @@
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();
// 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);
camera.setConnected(false);
localizer.update();
assertEquals(0.7, localizer.getConfidence(), 0.01);
gyro.setConnected(false);
localizer.update();
assertEquals(0.4, localizer.getConfidence(), 0.01);
}
}

View File

@@ -1,3 +1,3 @@
// Intentionally hardcoded. When you bump the version in Cargo.toml, // Intentionally hardcoded. When you bump the version in Cargo.toml,
// tests will fail here until you update this to match. // tests will fail here until you update this to match.
pub const EXPECTED_VERSION: &str = "1.1.0-beta.2"; pub const EXPECTED_VERSION: &str = "1.1.0-rc1";

View File

@@ -37,7 +37,7 @@ fn test_list_templates() {
let mgr = TemplateManager::new().unwrap(); let mgr = TemplateManager::new().unwrap();
let templates = mgr.list_templates(); let templates = mgr.list_templates();
assert_eq!(templates.len(), 2, "Should have exactly 2 templates"); assert_eq!(templates.len(), 3, "Should have exactly 3 templates");
assert!(templates.iter().any(|t| t.contains("basic")), "Should list basic template"); assert!(templates.iter().any(|t| t.contains("basic")), "Should list basic template");
assert!(templates.iter().any(|t| t.contains("testing")), "Should list testing template"); assert!(templates.iter().any(|t| t.contains("testing")), "Should list testing template");
} }