Adding localization feature

templates/localization/src/main/java/robot/hardware/Encoder.java

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

templates/localization/src/main/java/robot/hardware/GyroSensor.java

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

templates/localization/src/main/java/robot/hardware/VisionCamera.java

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

templates/localization/src/main/java/robot/localization/FieldGrid.java

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

templates/localization/src/main/java/robot/localization/GridCell.java

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

templates/localization/src/main/java/robot/localization/ImuLocalizer.java

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

templates/localization/src/main/java/robot/localization/OdometryTracker.java

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

templates/localization/src/main/java/robot/localization/Pose2D.java

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

templates/localization/src/main/java/robot/localization/RobotLocalizer.java

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

templates/localization/src/main/java/robot/localization/VisionLocalizer.java

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

templates/localization/src/test/java/robot/hardware/MockEncoder.java

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

templates/localization/src/test/java/robot/hardware/MockGyroSensor.java

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

templates/localization/src/test/java/robot/hardware/MockVisionCamera.java

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

templates/localization/src/test/java/robot/localization/GridCellTest.java

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

templates/localization/src/test/java/robot/localization/Pose2DTest.java

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

templates/localization/src/test/java/robot/localization/SensorFusionTest.java

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