'Fixed' issue with GamepadChecker- the engine already has once for each, adding one in a state directly is an error, misc. refactoring, stubbed autonomous states, Move state is almost functional- in theory, added experimental custom localizer, added field and obstacle classes to manage collision avoidance of static objects.

2025-12-15 21:22:33 +00:00 · 2023-01-21 21:06:38 -06:00
parent 740a6e97e0
commit 977a90eb48
16 changed files with 534 additions and 13 deletions
--- a/TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java
+++ b/TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java
@@ -13,7 +13,7 @@ public class GamepadChecker {
  private final Gamepad gamepad;
  private final HashMap<String, Boolean> buttons = new HashMap<>();
  private final HashMap<String, Long> buttonsDebounceInterval = new HashMap<>();
-  private final long debounceTime = 50L; // ms
+  private final long debounceTime = 20L; // ms

  public GamepadChecker(CyberarmEngine engine, Gamepad gamepad) {
    this.engine = engine;
@@ -52,16 +52,12 @@ public class GamepadChecker {
          if (button) {
            if (!buttons.get(btn)) {
              engine.buttonDown(gamepad, btn);
-
-              Log.d(TAG, "Button '" + btn + "' is DOWN [" + buttons.size() + "]");
            }

            buttons.put(btn, true);
          } else {
            if (buttons.get(btn)) {
              engine.buttonUp(gamepad, btn);
-
-              Log.d(TAG, "Button '" + btn + "' is UP");
            }

            buttons.put(btn, false);
--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java
+++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java
@@ -1,5 +1,6 @@
 package org.timecrafters.minibots.cyberarm.chiron;

+import com.acmerobotics.roadrunner.geometry.Vector2d;
 import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
 import com.qualcomm.robotcore.hardware.ColorSensor;
 import com.qualcomm.robotcore.hardware.DcMotor;
@@ -16,6 +17,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
 import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
 import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
+import org.timecrafters.minibots.cyberarm.chiron.tasks.FieldLocalizer;

 public class Robot {
    public final DcMotorEx backLeftDrive, frontRightDrive, frontLeftDrive, backRightDrive, arm;
@@ -48,10 +50,20 @@ public class Robot {

    private final CyberarmEngine engine;
    private final TimeCraftersConfiguration configuration;
+    private final FieldLocalizer fieldLocalizer;
+    private final double radius, diameter;

-    public Robot(CyberarmEngine engine, TimeCraftersConfiguration configuration) {
+    public Robot(CyberarmEngine engine, TimeCraftersConfiguration configuration, FieldLocalizer fieldLocalizer) {
        this.engine = engine;
        this.configuration = configuration;
+        this.fieldLocalizer = fieldLocalizer;
+
+        this.fieldLocalizer.setRobot(this);
+        this.fieldLocalizer.standardSetup();
+
+        radius = tuningConfig("field_localizer_robot_radius").value();
+        diameter = ((double)tuningConfig("field_localizer_robot_radius").value()) * 2;
+
        imuAngleOffset = hardwareConfig("imu_angle_offset").value();

        // FIXME: Rename motors in configuration
@@ -225,11 +237,29 @@ public class Robot {
        engine.telemetry.addData("      Facing (Degrees)", facing());
        engine.telemetry.addData("      Heading (Radians)", heading());
        engine.telemetry.addData("      Turn Rate", turnRate());
+
+        engine.telemetry.addLine();
+
+        // Field Localizer
+        engine.telemetry.addLine("Field Localizer");
+
+        Vector2d pos = fieldLocalizer.position();
+        Vector2d vel = fieldLocalizer.velocity();
+        engine.telemetry.addData("      Position X", pos.getX());
+        engine.telemetry.addData("      Position Y", pos.getY());
+        engine.telemetry.addData("      Velocity X", vel.getX());
+        engine.telemetry.addData("      Velocity Y", vel.getY());
+
+        engine.telemetry.addLine();
    }

-    public TimeCraftersConfiguration getConfiguration() {
-        return configuration;
-    }
+    public double getRadius() { return radius; }
+
+    public double getDiameter() { return diameter; }
+
+    public TimeCraftersConfiguration getConfiguration() { return configuration; }
+
+    public FieldLocalizer getFieldLocalizer() { return fieldLocalizer; }

    // For: Drive Wheels
    public int unitToTicks(DistanceUnit unit, double distance) {
@@ -304,4 +334,8 @@ public class Robot {
    public double turnRate() {
        return imu.getRobotAngularVelocity(AngleUnit.DEGREES).yRotationRate; // NOTE: UNTESTED
    }
+
+    public boolean isBetween(double value, double min, double max) {
+        return value >= min && value <= max;
+    }
 }
--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousRedLeftSideEngine.java
+++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousRedLeftSideEngine.java
@@ -0,0 +1,40 @@
+package org.timecrafters.minibots.cyberarm.chiron.engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
+import org.timecrafters.minibots.cyberarm.chiron.Robot;
+import org.timecrafters.minibots.cyberarm.chiron.tasks.FieldLocalizer;
+
+@Autonomous(name = "CHIRON | RED Left Side", group = "CHIRON", preselectTeleOp = "CHIRON | TeleOp")
+public class AutonomousRedLeftSideEngine extends CyberarmEngine {
+    private Robot robot;
+    private FieldLocalizer fieldLocalizer;
+    private TimeCraftersConfiguration configuration;
+
+    @Override
+    public void setup() {
+        configuration = new TimeCraftersConfiguration("CHIRON");
+
+        fieldLocalizer = new FieldLocalizer(
+                configuration.variable("Autonomous", "Tuning_Red_LeftSide", "starting_position_x_in_inches").value(),
+                configuration.variable("Autonomous", "Tuning_Red_LeftSide", "starting_position_y_in_inches").value()
+        );
+
+        robot = new Robot(
+                this,
+                configuration,
+                fieldLocalizer
+        );
+
+        addTask(fieldLocalizer);
+
+        setupFromConfig(
+                robot.getConfiguration(),
+                "org.timecrafters.minibots.cyberarm.chiron.states.autonomous",
+                robot,
+                Robot.class,
+                "AutonomousRedLeftSide");
+    }
+}
--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/ConfigureAndTestHardwareEngine.java
+++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/ConfigureAndTestHardwareEngine.java
@@ -10,6 +10,7 @@ import org.cyberarm.engine.V2.CyberarmState;
 import org.cyberarm.engine.V2.GamepadChecker;
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
 import org.timecrafters.minibots.cyberarm.chiron.Robot;
+import org.timecrafters.minibots.cyberarm.chiron.tasks.FieldLocalizer;

@TeleOp(name = "CHIRON | Diagnostics", group = "CHIRON")
 public class ConfigureAndTestHardwareEngine extends CyberarmEngine {
@@ -24,7 +25,7 @@ public class ConfigureAndTestHardwareEngine extends CyberarmEngine {

    @Override
    public void setup() {
-        robot = new Robot(this, new TimeCraftersConfiguration("CHIRON"));
+        robot = new Robot(this, new TimeCraftersConfiguration("CHIRON"), new FieldLocalizer(0, 0));

        addState(new MotorSetupState("Back Left Drive", robot.backLeftDrive, "back_left_drive_direction_forward", robot));
        addState(new MotorSetupState("Front Right Drive", robot.frontRightDrive, "front_right_drive_direction_forward", robot));
--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/GamepadCheckerDebugEngine.java
+++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/GamepadCheckerDebugEngine.java
@@ -0,0 +1,47 @@
+package org.timecrafters.minibots.cyberarm.chiron.engines;
+
+import android.util.Log;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.android.AndroidTextToSpeech;
+
+//@TeleOp(name = "CHIRON | Gamepad Checker Debug", group = "CHIRON")
+public class GamepadCheckerDebugEngine extends CyberarmEngine {
+    @Override
+    public void setup() {
+        addState(new CyberarmState() {
+            private AndroidTextToSpeech speech;
+
+            @Override
+            public void init() {
+                Log.d(TAG, "INITIALING STATE");
+                speech = new AndroidTextToSpeech();
+                speech.initialize();
+            }
+
+            @Override
+            public void exec() {
+            }
+
+            @Override
+            public void telemetry() {
+                engine.telemetry.addData("Run Time", runTime());
+            }
+
+            @Override
+            public void buttonDown(Gamepad gamepad, String button) {
+                if (gamepad != engine.gamepad1) {
+                    return;
+                }
+
+                if (button.equals("guide")) {
+                    speech.speak("GUIDE");
+                }
+            }
+        });
+    }
+}
--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/TeleOpEngine.java
+++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/TeleOpEngine.java
@@ -5,14 +5,15 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import org.cyberarm.engine.V2.CyberarmEngine;
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
 import org.timecrafters.minibots.cyberarm.chiron.Robot;
-import org.timecrafters.minibots.cyberarm.chiron.states.DriverControlState;
+import org.timecrafters.minibots.cyberarm.chiron.states.teleop.DriverControlState;
+import org.timecrafters.minibots.cyberarm.chiron.tasks.FieldLocalizer;

@TeleOp(name = "CHIRON | TeleOp", group = "CHIRON")
 public class TeleOpEngine extends CyberarmEngine {
    private Robot robot;
    @Override
    public void setup() {
-        this.robot = new Robot(this, new TimeCraftersConfiguration("CHIRON"));
+        this.robot = new Robot(this, new TimeCraftersConfiguration("CHIRON"), new FieldLocalizer());

        addState(new DriverControlState(robot));
    }
--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Arm.java
+++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Arm.java
@@ -0,0 +1,11 @@
+package org.timecrafters.minibots.cyberarm.chiron.states.autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class Arm extends CyberarmState {
+    @Override
+    public void exec() {
+        // FIXME: NO OP
+        setHasFinished(true);
+    }
+}
--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Gripper.java
+++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Gripper.java
@@ -0,0 +1,11 @@
+package org.timecrafters.minibots.cyberarm.chiron.states.autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class Gripper extends CyberarmState {
+    @Override
+    public void exec() {
+        // FIXME: NO OP
+        setHasFinished(true);
+    }
+}
--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java
+++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java
@@ -0,0 +1,124 @@
+package org.timecrafters.minibots.cyberarm.chiron.states.autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.timecrafters.minibots.cyberarm.chiron.Robot;
+
+public class Move extends CyberarmState {
+    private final Robot robot;
+    private final String groupName, actionName;
+
+    private final double positionXInInches, positionYInInches, toleranceInInches, facing, targetVelocity, timeInMS;
+
+    private final double maxSpeed;
+    private double speed;
+
+    private int distanceAlreadyTravelled;
+    private final boolean stateDisabled;
+
+    public Move(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+
+        positionXInInches = robot.getConfiguration().variable(groupName, actionName, "positionXInInches").value();
+        positionYInInches = robot.getConfiguration().variable(groupName, actionName, "positionYInInches").value();
+        toleranceInInches = robot.getConfiguration().variable(groupName, actionName, "toleranceInInches").value();
+        facing = robot.getConfiguration().variable(groupName, actionName, "facing").value();
+        targetVelocity = robot.getConfiguration().variable(groupName, actionName, "targetVelocity").value();
+        timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value();
+        stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled;
+
+        maxSpeed = robot.tuningConfig("drivetrain_max_power").value();
+        speed = 0.0;
+    }
+
+    @Override
+    public void start() {
+        // TODO: Use a dead wheel for this
+        distanceAlreadyTravelled = robot.frontLeftDrive.getCurrentPosition();
+    }
+
+    @Override
+    public void exec() {
+        if (stateDisabled) {
+            setHasFinished(true);
+
+            return;
+        }
+
+        if (runTime() >= timeInMS) {
+            stop();
+
+            setHasFinished(true);
+
+            return;
+        }
+
+        // TODO: Double check maths
+//        int travelledDistance = (robot.frontLeftDrive.getCurrentPosition() - distanceAlreadyTravelled);
+//        int targetDistance = robot.unitToTicks(DistanceUnit.INCH, distanceInInches);
+//        int tolerance = robot.unitToTicks(DistanceUnit.INCH, toleranceInInches);
+//
+//        if (robot.isBetween(targetDistance, travelledDistance - tolerance, travelledDistance + tolerance)) {
+//            stop();
+//
+//            setHasFinished(true);
+//
+//            return;
+//        }
+
+        move();
+    }
+
+    private void move() {
+        if (Math.abs(speed) > maxSpeed) {
+            speed = speed < 0 ? -maxSpeed : maxSpeed;
+        }
+
+        double forwardSpeed = 1.0; //distanceInInches < 0 ? -1.0 : 1.0;
+        double rightSpeed   = 0.0;
+        double rotateRightSpeed = 0.0;
+
+        if (!robot.isBetween((robot.facing() + robot.turnRate()), robot.facing() - 1, robot.facing() + 1)) {
+            if (robot.facing() < facing) {
+                rotateRightSpeed = -0.5;
+            } else {
+                rotateRightSpeed = 0.5;
+            }
+        }
+
+        // FIXME: targetSpeed * speed[Limiter] will cause infinitesimal power issues
+        double y = -forwardSpeed * speed;
+        double x = (-rightSpeed * speed) * 1.1; // Counteract imperfect strafing. TODO: make tunable
+        double rx = -rotateRightSpeed * speed;
+        double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
+
+
+        double frontLeftPower = 0, frontRightPower = 0, backLeftPower = 0 , backRightPower = 0;
+
+        double heading = robot.heading();
+        double rotX = x * Math.cos(heading) - y * Math.sin(heading);
+        double rotY = x * Math.sin(heading) + y * Math.cos(heading);
+
+        frontLeftPower  = (rotY + rotX + rx) / denominator;
+        backLeftPower   = (rotY - rotX - rx) / denominator;
+        frontRightPower = (rotY - rotX + rx) / denominator;
+        backRightPower  = (rotY + rotX - rx) / denominator;
+
+        robot.frontLeftDrive.setPower(frontLeftPower);
+        robot.frontRightDrive.setPower(frontRightPower);
+
+        robot.backLeftDrive.setPower(backLeftPower);
+        robot.backRightDrive.setPower(backRightPower);
+    }
+
+    @Override
+    public void stop() {
+        robot.frontLeftDrive.setPower(0);
+        robot.frontRightDrive.setPower(0);
+
+        robot.backLeftDrive.setPower(0);
+        robot.backRightDrive.setPower(0);
+    }
+}
--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java
+++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java
@@ -0,0 +1,11 @@
+package org.timecrafters.minibots.cyberarm.chiron.states.autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class Rotate extends CyberarmState {
+    @Override
+    public void exec() {
+        // FIXME: NO OP
+        setHasFinished(true);
+    }
+}
--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SelectParkingPosition.java
+++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SelectParkingPosition.java
@@ -0,0 +1,11 @@
+package org.timecrafters.minibots.cyberarm.chiron.states.autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class SelectParkingPosition extends CyberarmState {
+    @Override
+    public void exec() {
+        // FIXME: NO OP
+        setHasFinished(true);
+    }
+}
--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SignalProcessor.java
+++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SignalProcessor.java
@@ -0,0 +1,11 @@
+package org.timecrafters.minibots.cyberarm.chiron.states.autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class SignalProcessor extends CyberarmState {
+    @Override
+    public void exec() {
+        // FIXME: NO OP
+        setHasFinished(true);
+    }
+}
--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DriverControlState.java
+++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DriverControlState.java
@@ -1,4 +1,4 @@
-package org.timecrafters.minibots.cyberarm.chiron.states;
+package org.timecrafters.minibots.cyberarm.chiron.states.teleop;

 import android.util.Log;

--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/FieldLocalizer.java
+++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/FieldLocalizer.java
@@ -0,0 +1,140 @@
+package org.timecrafters.minibots.cyberarm.chiron.tasks;
+
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.minibots.cyberarm.chiron.Robot;
+import org.timecrafters.minibots.cyberarm.chiron.tasks.field_localizer.Field;
+import org.timecrafters.minibots.cyberarm.chiron.tasks.field_localizer.Obstacle;
+
+public class FieldLocalizer extends CyberarmState {
+    private static double posX, posY;
+    private Robot robot;
+    private Field field;
+
+    private double deltaFrontLeft, deltaFrontRight, deltaBackLeft, deltaBackRight;
+    private double displacementFrontLeft, displacementFrontRight, displacementBackLeft, displacementBackRight, displacementAverage;
+    private double devFrontLeft, devFrontRight, devBackLeft, devBackRight;
+    private double lastFrontLeft, lastFrontRight, lastBackLeft, lastBackRight;
+
+    private double deltaXRotation, deltaYRotation, deltaX, deltaY;
+    private double lastX, lastY, theta;
+
+    private final double twoSqrtTwo = 2.0 / Math.sqrt(2.0);
+
+    private double wheelDisplacementPerEncoderTick = 1; // FIXME
+
+    // Use this constructor to preserve displacement between opmodes
+    public FieldLocalizer() {
+    }
+
+    public FieldLocalizer(double posX, double posY) {
+        FieldLocalizer.posX = posX;
+        FieldLocalizer.posY = posY;
+    }
+
+    public void standardSetup() {
+        field = new Field(robot);
+
+        lastFrontLeft  = 0;
+        lastFrontRight = 0;
+        lastBackLeft   = 0;
+        lastBackRight  = 0;
+
+        deltaFrontLeft  = 0;
+        deltaFrontRight = 0;
+        deltaBackLeft   = 0;
+        deltaBackRight  = 0;
+
+        displacementFrontLeft  = 0;
+        displacementFrontRight = 0;
+        displacementBackLeft   = 0;
+        displacementBackRight  = 0;
+
+        displacementAverage = 0;
+
+        devFrontLeft  = 0;
+        devFrontRight = 0;
+        devBackLeft   = 0;
+        devBackRight  = 0;
+
+        deltaX = 0;
+        deltaY = 0;
+        deltaXRotation = 0;
+        deltaYRotation = 0;
+
+        // Preserve field position, of argument-less construction is used
+        lastX = FieldLocalizer.posX;
+        lastY = FieldLocalizer.posY;
+
+        // TODO: Preserve robot IMU offset between opmodes
+        theta = robot.heading();
+    }
+
+    public void setRobot(Robot robot) {
+        this.robot = robot;
+    }
+
+    // FIXME: Return position in INCHES
+    public Vector2d position() {
+        return new Vector2d(posX, posY);
+    }
+
+    // FIXME: Return velocity in INCHES
+    public Vector2d velocity() {
+        return new Vector2d(deltaX, deltaY);
+    }
+
+    /**
+     * Calculates direction vector to avoid collision with static field elements
+     * @return Vector2d if collision projected or null of no correction needed
+     */
+    public Vector2d collisionAvoidanceDirection() {
+        for (Obstacle o : field.getObstacles()) {
+            // TODO: Calculate if robot will collide with obstacle
+        }
+
+        return null;
+    }
+
+    @Override
+    public void exec() {
+        // REF: https://www.bridgefusion.com/blog/2019/4/10/robot-localization-dead-reckoning-in-first-tech-challenge-ftc
+
+        deltaFrontLeft  = robot.frontLeftDrive.getCurrentPosition() - lastFrontLeft;
+        deltaFrontRight = robot.frontRightDrive.getCurrentPosition() - lastFrontRight;
+        deltaBackLeft   = robot.backLeftDrive.getCurrentPosition() - lastBackLeft;
+        deltaBackRight  = robot.backRightDrive.getCurrentPosition() - lastBackRight;
+
+        displacementFrontLeft  = deltaFrontLeft * wheelDisplacementPerEncoderTick;
+        displacementFrontRight = deltaFrontRight * wheelDisplacementPerEncoderTick;
+        displacementBackLeft   = deltaBackLeft * wheelDisplacementPerEncoderTick;
+        displacementBackRight  = deltaBackRight * wheelDisplacementPerEncoderTick;
+
+        displacementAverage = (displacementFrontLeft + displacementFrontRight +
+                displacementBackLeft + displacementBackRight) / 4.0; // motor count
+
+        devFrontLeft  = displacementFrontLeft - displacementAverage;
+        devFrontRight = displacementFrontRight - displacementAverage;
+        devBackLeft   = displacementBackLeft - displacementAverage;
+        devBackRight  = displacementBackRight - displacementAverage;
+
+        deltaXRotation = (devFrontLeft + devFrontRight - devBackRight - devBackLeft) / twoSqrtTwo;
+        deltaYRotation = (devFrontLeft - devFrontRight - devBackRight - devBackLeft) / twoSqrtTwo;
+
+        theta = robot.heading();
+        double sinTheta = Math.sin(theta);
+        double cosineTheta = Math.cos(theta);
+
+        deltaX = deltaXRotation * cosineTheta - deltaYRotation * sinTheta;
+        deltaY = deltaYRotation * cosineTheta + deltaXRotation * sinTheta;
+
+        posX = lastX + deltaX;
+        posY = lastY + deltaY;
+
+        lastFrontLeft  = robot.frontLeftDrive.getCurrentPosition();
+        lastFrontRight = robot.frontRightDrive.getCurrentPosition();
+        lastBackLeft   = robot.backLeftDrive.getCurrentPosition();
+        lastBackRight  = robot.backRightDrive.getCurrentPosition();
+    }
+}
--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/field_localizer/Field.java
+++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/field_localizer/Field.java
@@ -0,0 +1,65 @@
+package org.timecrafters.minibots.cyberarm.chiron.tasks.field_localizer;
+
+import org.timecrafters.minibots.cyberarm.chiron.Robot;
+
+import java.util.ArrayList;
+
+public class Field {
+    private final ArrayList<Obstacle> obstacles = new ArrayList<>();
+    private final Robot robot;
+
+    public Field(Robot robot) {
+        this.robot = robot;
+
+        populateField();
+    }
+
+    private void populateField() {
+        // ORIGIN is field CENTER point with RED alliance on the RIGHT
+
+        // Ground Junctions
+        double posX = -48;
+        double posY = -48;
+
+        for (int y = 0; y < 3; y++) {
+            posY += (y * 48); // Ground Junctions are 48 inches apart
+
+            for (int x = 0; x < 3; x++) {
+                posX += (x * 48);
+
+                addGroundJunction(posX, posY);
+            }
+        }
+
+        // Pole Junctions
+        posX = -48;
+        posY = -48;
+
+        for (int y = 0; y < 5; y++) {
+            posY += (y * 24); // Pole Junctions are 24 inches apart
+
+            for (int x = 0; x < 5; x++) {
+                posX += (x * 24);
+
+                // FIXME: Don't add phantom poles where ground junctions are placed
+                addPole(posX, posY);
+            }
+        }
+    }
+
+    private void addPole(double x, double y) {
+        obstacles.add(
+                new Obstacle(x, y, robot.getConfiguration().variable("Field", "Obstacles", "pole_radius_in_inches").value())
+        );
+    }
+
+    private void addGroundJunction(double x, double y) {
+        obstacles.add(
+            new Obstacle(x, y, robot.getConfiguration().variable("Field", "Obstacles", "ground_junction_radius_in_inches").value())
+        );
+    }
+
+    public ArrayList<Obstacle> getObstacles() {
+        return obstacles;
+    }
+}
--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/field_localizer/Obstacle.java
+++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/field_localizer/Obstacle.java
@@ -0,0 +1,18 @@
+package org.timecrafters.minibots.cyberarm.chiron.tasks.field_localizer;
+
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+
+public class Obstacle {
+    public final double x, y, radius;
+
+    public Obstacle(double x, double y, double radius) {
+        this.x = x;
+        this.y = y;
+        this.radius = radius;
+    }
+
+    // TODO: Do the maths to get a normalized vector pointing AWAY from the obstacle's origin
+    public Vector2d antiCollisionNormal(Vector2d robotVelocity) {
+        return new Vector2d(0, 0);
+    }
+}