Compare commits
6 Commits
59f8a7faa3
...
master
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
9af6015aa3 | ||
|
|
9c2ac97158 | ||
|
|
e6934cdb18 | ||
|
|
636e1252dc | ||
|
|
b07e8c7dab | ||
|
|
c34e4c4dea |
@@ -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"
|
||||||
|
|||||||
113
docs/ROADMAP.md
113
docs/ROADMAP.md
@@ -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
|
||||||
|
|||||||
@@ -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" />
|
||||||
|
|||||||
@@ -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
7
templates/localization/.gitignore
vendored
Normal file
@@ -0,0 +1,7 @@
|
|||||||
|
build/
|
||||||
|
.gradle/
|
||||||
|
*.iml
|
||||||
|
.idea/
|
||||||
|
local.properties
|
||||||
|
*.apk
|
||||||
|
.DS_Store
|
||||||
53
templates/localization/README.md.template
Normal file
53
templates/localization/README.md.template
Normal 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.
|
||||||
41
templates/localization/docs/GRID_SYSTEM.md
Normal file
41
templates/localization/docs/GRID_SYSTEM.md
Normal 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
72
templates/localization/docs/LOCALIZATION_GUIDE.md
Normal 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.
|
||||||
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}}"
|
||||||
@@ -0,0 +1,8 @@
|
|||||||
|
package robot.hardware;
|
||||||
|
|
||||||
|
public interface Encoder {
|
||||||
|
int getTicks();
|
||||||
|
int getTicksPerRevolution();
|
||||||
|
boolean isConnected();
|
||||||
|
void reset();
|
||||||
|
}
|
||||||
@@ -0,0 +1,8 @@
|
|||||||
|
package robot.hardware;
|
||||||
|
|
||||||
|
public interface GyroSensor {
|
||||||
|
double getHeading();
|
||||||
|
boolean isConnected();
|
||||||
|
void calibrate();
|
||||||
|
boolean isCalibrated();
|
||||||
|
}
|
||||||
@@ -0,0 +1,8 @@
|
|||||||
|
package robot.hardware;
|
||||||
|
|
||||||
|
import robot.localization.Pose2D;
|
||||||
|
|
||||||
|
public interface VisionCamera {
|
||||||
|
Pose2D detectPose();
|
||||||
|
boolean isConnected();
|
||||||
|
}
|
||||||
@@ -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);
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -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 + ")"; }
|
||||||
|
}
|
||||||
@@ -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();
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -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();
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -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);
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -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";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -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;
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -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; }
|
||||||
|
}
|
||||||
@@ -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; }
|
||||||
|
}
|
||||||
@@ -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; }
|
||||||
|
}
|
||||||
@@ -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);
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -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);
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -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);
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -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";
|
||||||
@@ -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");
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user