Sloth Added, LimeLight support, and Routines

TeamCode/build.gradle

@@ -12,8 +12,20 @@
 // Custom definitions may go here
 
 // Include common definitions from above.
+
+buildscript {
+    repositories {
+        mavenCentral()
+        maven { url "https://repo.dairy.foundation/releases" }
+    }
+    dependencies {
+        classpath "dev.frozenmilk:Load:0.2.4"
+    }
+}
+
 apply from: '../build.common.gradle'
 apply from: '../build.dependencies.gradle'
+apply plugin: 'dev.frozenmilk.sinister.sloth.load'
 
 android {
     namespace = 'org.firstinspires.ftc.teamcode'
@@ -23,6 +35,16 @@ android {
     }
 }
 
+
+repositories {
+    maven { url = "https://repo.dairy.foundation/releases" }
+}
+
 dependencies {
     implementation project(':FtcRobotController')
+    // 1. Install Sloth
+    implementation("dev.frozenmilk.sinister:Sloth:0.2.4")
+
+    // 2. Install the Sloth version of Panels
+    implementation("com.bylazar.sloth:fullpanels:0.2.4+1.0.12")
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/BaseOpMode.java

@@ -6,20 +6,30 @@ import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.util.ElapsedTime;
 import org.firstinspires.ftc.robotcore.external.Telemetry;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
 
-// PANELS IMPORTS
 import com.bylazar.telemetry.JoinedTelemetry;
 
+import org.firstinspires.ftc.teamcode.lib.actions.RoutineManager;
 import org.firstinspires.ftc.teamcode.teleOp.Constants;
 import org.firstinspires.ftc.teamcode.util.FPSCounter;
 
+
 import java.util.List;
 
 public abstract class BaseOpMode extends OpMode {
     protected final SubsysManager manager = new SubsysManager();
+
+    protected final RoutineManager routines = new RoutineManager();
     protected final FPSCounter fps = new FPSCounter();
     private final ElapsedTime telemetryTimer = new ElapsedTime();
     private List<LynxModule> allHubs;
+    private final ElapsedTime voltageTimer = new ElapsedTime();
+    private VoltageSensor batterySensor;
+
+
+    protected EnhancedGamepad g1;
+    protected EnhancedGamepad g2;
 
     protected Telemetry unifiedTelemetry;
 
@@ -31,18 +41,24 @@ public abstract class BaseOpMode extends OpMode {
     public final void init() {
         unifiedTelemetry = new JoinedTelemetry(telemetry);
 
+        batterySensor = hardwareMap.voltageSensor.iterator().next();
+        PIDController.currentSystemVoltage = batterySensor.getVoltage();
+
         allHubs = hardwareMap.getAll(LynxModule.class);
         for (LynxModule hub : allHubs) {
             hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
         }
 
+
+        g1 = new EnhancedGamepad(gamepad1);
+        g2 = new EnhancedGamepad(gamepad2);
+
         setupSubsystems();
-
         manager.initAll(hardwareMap);
-
         onInit();
     }
 
+
     @Override
     public final void start() {
         fps.reset();
@@ -52,23 +68,30 @@ public abstract class BaseOpMode extends OpMode {
 
     @Override
     public final void loop() {
+        fps.startLoop();
         for (LynxModule hub : allHubs) {
             hub.clearBulkCache();
         }
 
-        fps.startLoop();
-        stateMachineUpdate();
 
+        g1.update();
+        g2.update();
+
+        if (voltageTimer.milliseconds() >= Constants.PIDController.VOLTAGE_CHECK_INTERVAL_MS) {
+            PIDController.currentSystemVoltage = batterySensor.getVoltage();
+            voltageTimer.reset();
+        }
+        stateMachineUpdate();
         manager.updateAll();
+        routines.updateAll();
 
         if (telemetryTimer.milliseconds() >= Constants.GLOBAL.TELEMETRY_DELAY_MS) {
-
             manager.publishTelemetryAll(unifiedTelemetry);
 
             if (Constants.GLOBAL.DEBUG_MODE) {
+                unifiedTelemetry.addData("System Voltage", PIDController.currentSystemVoltage);
                 unifiedTelemetry.addData("Loop Time (ms)", fps.getCurrentLoopTime());
-                unifiedTelemetry.addData("Target FPS", Constants.GLOBAL.TARGET_FPS);
+                unifiedTelemetry.addData("FPS", fps.getAverageFps());
-                unifiedTelemetry.addData("Current FPS", fps.getAverageFps());
             }
 
             unifiedTelemetry.update();
@@ -78,6 +101,6 @@ public abstract class BaseOpMode extends OpMode {
         long sleepTime = fps.getSyncTime(Constants.GLOBAL.TARGET_FPS);
         if (sleepTime > 0) {
             SystemClock.sleep(sleepTime);
-        }
+        } // CAP FPS Logic (check Constants.GLOBAL.TARGET_FPS, if 0 then uncapped)
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/EnhancedGamepad.java

@@ -0,0 +1,110 @@
+package org.firstinspires.ftc.teamcode.lib;
+
+import com.qualcomm.robotcore.hardware.Gamepad;
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
+
+public class EnhancedGamepad {
+    private final Gamepad hardwareGamepad;
+    private final Gamepad currentState;
+    private final Gamepad previousState;
+
+    public EnhancedGamepad(Gamepad gamepad) {
+        this.hardwareGamepad = gamepad;
+        this.currentState = new Gamepad();
+        this.previousState = new Gamepad();
+    }
+
+    public void update() {
+        previousState.copy(currentState);
+        currentState.copy(hardwareGamepad);
+    }
+
+    private double scale(double input) {
+        if (Math.abs(input) < Constants.Gamepad.STICK_DEADBAND) return 0;
+        return Math.pow(input, 3);
+    }
+
+    private boolean triggerAsBtn(float val) {
+        return val > Constants.Gamepad.TRIGGER_THRESHOLD;
+    }
+
+    public double left_stick_x() { return scale(currentState.left_stick_x); }
+    public double left_stick_y() { return scale(currentState.left_stick_y); }
+    public double right_stick_x() { return scale(currentState.right_stick_x); }
+    public double right_stick_y() { return scale(currentState.right_stick_y); }
+
+    public double left_trigger() { return currentState.left_trigger; }
+    public double right_trigger() { return currentState.right_trigger; }
+
+    // --- BUTTONS: CURRENT STATE ---
+    public boolean a() { return currentState.a; }
+    public boolean b() { return currentState.b; }
+    public boolean x() { return currentState.x; }
+    public boolean y() { return currentState.y; }
+
+    public boolean dpad_up() { return currentState.dpad_up; }
+    public boolean dpad_down() { return currentState.dpad_down; }
+    public boolean dpad_left() { return currentState.dpad_left; }
+    public boolean dpad_right() { return currentState.dpad_right; }
+
+    public boolean left_bumper() { return currentState.left_bumper; }
+    public boolean right_bumper() { return currentState.right_bumper; }
+    public boolean left_stick_button() { return currentState.left_stick_button; }
+    public boolean right_stick_button() { return currentState.right_stick_button; }
+
+    public boolean back() { return currentState.back; }
+    public boolean start() { return currentState.start; }
+    public boolean guide() { return currentState.guide; }
+    public boolean options() { return currentState.options; }
+
+    // PS Specific Aliases
+    public boolean ps() { return currentState.ps; }
+    public boolean share() { return currentState.share; }
+    public boolean touchpad() { return currentState.touchpad; }
+    public boolean circle() { return currentState.circle; }
+    public boolean cross() { return currentState.cross; }
+    public boolean triangle() { return currentState.triangle; }
+    public boolean square() { return currentState.square; }
+
+    // Triggers as Buttons
+    public boolean left_trigger_btn() { return triggerAsBtn(currentState.left_trigger); }
+    public boolean right_trigger_btn() { return triggerAsBtn(currentState.right_trigger); }
+
+    // --- BUTTONS: WAS PRESSED (Rising Edge) ---
+    public boolean aWasPressed() { return currentState.a && !previousState.a; }
+    public boolean bWasPressed() { return currentState.b && !previousState.b; }
+    public boolean xWasPressed() { return currentState.x && !previousState.x; }
+    public boolean yWasPressed() { return currentState.y && !previousState.y; }
+
+    public boolean dpad_upWasPressed() { return currentState.dpad_up && !previousState.dpad_up; }
+    public boolean dpad_downWasPressed() { return currentState.dpad_down && !previousState.dpad_down; }
+    public boolean dpad_leftWasPressed() { return currentState.dpad_left && !previousState.dpad_left; }
+    public boolean dpad_rightWasPressed() { return currentState.dpad_right && !previousState.dpad_right; }
+
+    public boolean left_bumperWasPressed() { return currentState.left_bumper && !previousState.left_bumper; }
+    public boolean right_bumperWasPressed() { return currentState.right_bumper && !previousState.right_bumper; }
+
+    public boolean left_stick_buttonWasPressed() { return currentState.left_stick_button && !previousState.left_stick_button; }
+    public boolean right_stick_buttonWasPressed() { return currentState.right_stick_button && !previousState.right_stick_button; }
+
+    public boolean backWasPressed() { return currentState.back && !previousState.back; }
+    public boolean startWasPressed() { return currentState.start && !previousState.start; }
+    public boolean optionsWasPressed() { return currentState.options && !previousState.options; }
+    public boolean touchpadWasPressed() { return currentState.touchpad && !previousState.touchpad; }
+
+    public boolean left_trigger_btnWasPressed() { return triggerAsBtn(currentState.left_trigger) && !triggerAsBtn(previousState.left_trigger); }
+    public boolean right_trigger_btnWasPressed() { return triggerAsBtn(currentState.right_trigger) && !triggerAsBtn(previousState.right_trigger); }
+
+    // --- BUTTONS: WAS RELEASED (Falling Edge) ---
+    public boolean aWasReleased() { return !currentState.a && previousState.a; }
+    public boolean bWasReleased() { return !currentState.b && previousState.b; }
+    public boolean xWasReleased() { return !currentState.x && previousState.x; }
+    public boolean yWasReleased() { return !currentState.y && previousState.y; }
+
+    public boolean left_bumperWasReleased() { return !currentState.left_bumper && previousState.left_bumper; }
+    public boolean right_bumperWasReleased() { return !currentState.right_bumper && previousState.right_bumper; }
+
+    // --- RUMBLE CONTROLS ---
+    public void rumble(int durationMs) { hardwareGamepad.rumble(durationMs); }
+    public void rumbleBlips(int count) { hardwareGamepad.rumbleBlips(count); }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/LLUtil.java

@@ -0,0 +1,72 @@
+package org.firstinspires.ftc.teamcode.lib;
+
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
+
+public class LLUtil {
+    private Limelight3A limelight;
+    private LLResult lastResult;
+    private final ElapsedTime timer = new ElapsedTime();
+    private double lastKnownDistance = 0;
+
+    public LLUtil(HardwareMap hwMap) {
+        limelight = hwMap.get(Limelight3A.class, Constants.LIMELIGHT.NAME);
+        limelight.setPollRateHz(Constants.LIMELIGHT.POLL_RATE);
+        limelight.start();
+    }
+
+    public void update() {
+        lastResult = limelight.getLatestResult();
+        if (hasTarget()) {
+            timer.reset();
+            lastKnownDistance = getTrigDistance();
+        }
+    }
+
+    public boolean hasTarget() {
+        return lastResult != null && lastResult.isValid();
+    }
+
+    public boolean isDataFresh() {
+        return hasTarget() || (timer.seconds() < Constants.LIMELIGHT.STALE_DATA_TIMEOUT_SEC);
+    }
+
+    public double getTx() {
+        return hasTarget() ? lastResult.getTx() : 0;
+    }
+
+    public double getTy() {
+        return hasTarget() ? lastResult.getTy() : 0;
+    }
+
+    public double getTa() {
+        return hasTarget() ? lastResult.getTa() : 0;
+    }
+
+    public double getTrigDistance() { // Trig Distance
+        if (!hasTarget()) return lastKnownDistance;
+
+        double angleToTargetRad = Math.toRadians(Constants.LIMELIGHT.MOUNT_ANGLE_DEGREES + getTy());
+        double heightDiff = Constants.LIMELIGHT.TARGET_HEIGHT_IN - Constants.LIMELIGHT.MOUNT_HEIGHT_IN;
+
+        return heightDiff / Math.tan(angleToTargetRad);
+    }
+
+    public double getAreaDistance() { // TA Regr
+        if (!hasTarget()) return lastKnownDistance;
+        return (Constants.LIMELIGHT.AREA_COEFF * Math.pow(getTa(), Constants.LIMELIGHT.AREA_EXPONENT))
+                + Constants.LIMELIGHT.AREA_OFFSET;
+    }
+
+    public void setPipeline(int index) {
+        limelight.pipelineSwitch(index);
+    }
+
+    public void stop() {
+        limelight.stop();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/PIDController.java

@@ -1,10 +1,12 @@
 package org.firstinspires.ftc.teamcode.lib;
 
 import com.qualcomm.robotcore.util.ElapsedTime;
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
 
 public class PIDController {
-    private double kP, kI, kD;
+    public static double currentSystemVoltage = 12.0;
 
+    private double kP, kI, kD;
     private double lastError = 0;
     private double integralSum = 0;
     private final ElapsedTime timer = new ElapsedTime();
@@ -39,7 +41,11 @@ public class PIDController {
         integralSum += (error * dt);
         lastError = error;
 
-        return (kP * error) + (kI * integralSum) + (kD * derivative);
+        double rawOutput = (kP * error) + (kI * integralSum) + (kD * derivative);
+
+        double compensationMultiplier = Constants.PIDController.VOLTAGE_REFERENCE / currentSystemVoltage;
+
+        return rawOutput * compensationMultiplier;
     }
 
     public void reset() {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/Action.java

@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.lib.actions;
+
+public interface Action {
+    /** Called exactly once when the action begins. */
+    void start();
+
+    /**
+     * Called every loop while the action is active.
+     * @return true if the action is finished, false if still running.
+     */
+    boolean update();
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/Routine.java

@@ -0,0 +1,130 @@
+package org.firstinspires.ftc.teamcode.lib.actions;
+
+import com.qualcomm.robotcore.util.ElapsedTime;
+import java.util.Arrays;
+import java.util.List;
+import java.util.function.BooleanSupplier;
+
+public class Routine {
+
+    /** Runs a single block of code instantly, then finishes. */
+    public static Action instant(Runnable runnable) {
+        return new InstantAction(runnable);
+    }
+
+    /** Waits for a specific amount of milliseconds without freezing the robot. */
+    public static Action wait(double milliseconds) {
+        return new WaitAction(milliseconds);
+    }
+
+    /** Waits until a specific condition becomes true (e.g. Color Sensor detects pixel) */
+    public static Action waitUntil(BooleanSupplier condition) {
+        return new ConditionAction(condition);
+    }
+
+    /** Runs multiple actions one after the other. */
+    public static Action sequence(Action... actions) {
+        return new SequentialAction(Arrays.asList(actions));
+    }
+
+    /** Runs multiple actions at the exact same time. Finishes when ALL are done. */
+    public static Action parallel(Action... actions) {
+        return new ParallelAction(Arrays.asList(actions));
+    }
+
+    private static class InstantAction implements Action {
+        private final Runnable task;
+        public InstantAction(Runnable task) { this.task = task; }
+
+        @Override
+        public void start() { task.run(); }
+
+        @Override
+        public boolean update() { return true; }
+    }
+
+    private static class WaitAction implements Action {
+        private final double duration;
+        private final ElapsedTime timer = new ElapsedTime();
+
+        public WaitAction(double duration) { this.duration = duration; }
+
+        @Override
+        public void start() { timer.reset(); }
+
+        @Override
+        public boolean update() { return timer.milliseconds() >= duration; }
+    }
+
+    private static class ConditionAction implements Action {
+        private final BooleanSupplier condition;
+        public ConditionAction(BooleanSupplier condition) { this.condition = condition; }
+
+        @Override
+        public void start() {}
+
+        @Override
+        public boolean update() { return condition.getAsBoolean(); }
+    }
+
+    private static class SequentialAction implements Action {
+        private final List<Action> actions;
+        private int currentIndex = 0;
+        private boolean isCurrentStarted = false;
+
+        public SequentialAction(List<Action> actions) { this.actions = actions; }
+
+        @Override
+        public void start() {
+            currentIndex = 0;
+            isCurrentStarted = false;
+        }
+
+        @Override
+        public boolean update() {
+            if (currentIndex >= actions.size()) return true;
+
+            if (!isCurrentStarted) {
+                actions.get(currentIndex).start();
+                isCurrentStarted = true;
+            }
+
+            if (actions.get(currentIndex).update()) {
+                currentIndex++;
+                isCurrentStarted = false;
+            }
+
+            return currentIndex >= actions.size();
+        }
+    }
+
+    private static class ParallelAction implements Action {
+        private final List<Action> actions;
+        private final boolean[] finished;
+
+        public ParallelAction(List<Action> actions) {
+            this.actions = actions;
+            this.finished = new boolean[actions.size()];
+        }
+
+        @Override
+        public void start() {
+            for (int i = 0; i < actions.size(); i++) {
+                finished[i] = false;
+                actions.get(i).start();
+            }
+        }
+
+        @Override
+        public boolean update() {
+            boolean allDone = true;
+            for (int i = 0; i < actions.size(); i++) {
+                if (!finished[i]) {
+                    finished[i] = actions.get(i).update();
+                    if (!finished[i]) allDone = false;
+                }
+            }
+            return allDone;
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/RoutineManager.java

@@ -0,0 +1,32 @@
+package org.firstinspires.ftc.teamcode.lib.actions;
+
+import java.util.ArrayList;
+import java.util.Iterator;
+import java.util.List;
+
+public class RoutineManager {
+    private final List<Action> activeRoutines = new ArrayList<>();
+
+    public void run(Action action) {
+        action.start();
+        activeRoutines.add(action);
+    }
+
+    public void updateAll() {
+        Iterator<Action> iterator = activeRoutines.iterator();
+        while (iterator.hasNext()) {
+            Action action = iterator.next();
+            if (action.update()) {
+                iterator.remove();
+            }
+        }
+    }
+
+    public void cancelAll() {
+        activeRoutines.clear();
+    }
+
+    public boolean isBusy() {
+        return !activeRoutines.isEmpty();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/routine.md

@@ -0,0 +1,99 @@
+
+
+
+# Routines & Action Sequencer 🎬
+
+The **Routines** system is one of the most powerful features in `ftc-lib`. It allows you to write complex, multi-step robot scripts (like an Auto-Score macro) **without freezing the robot**. 
+
+In standard linear OpModes, calling `sleep(1000)` completely stops the robot from driving. The `ftc-lib` Routine Manager runs your scripts asynchronously in the background. Your driver retains full control of the drivetrain while the robot automatically orchestrates the lift, intake, and servos.
+
+---
+
+## 🧩 The Building Blocks (Actions)
+
+Everything in a routine is an `Action`. You do not need to create these manually; use the `Routine` factory class to build them instantly.
+
+| Method | Description | Example |
+| :--- | :--- | :--- |
+| `Routine.instant()` | Runs a single block of code immediately. | `Routine.instant(() -> lift.setTarget(1000))` |
+| `Routine.wait()` | Pauses **only this sequence** for `x` milliseconds. | `Routine.wait(500)` |
+| `Routine.waitUntil()` | Pauses the sequence until a condition is `true`. | `Routine.waitUntil(() -> lift.isAtTarget())` |
+| `Routine.sequence()` | Runs multiple Actions **one after the other**. | `Routine.sequence(action1, action2)` |
+| `Routine.parallel()` | Runs multiple Actions **at the exact same time**. | `Routine.parallel(action1, action2)` |
+
+---
+
+## 🚀 Running a Routine in TeleOp
+
+Because `BaseOpMode` automatically manages the `RoutineManager` under the hood, all you have to do is call `routines.run()` inside your `stateMachineUpdate()` loop. 
+
+Here is a real-world example of an **Auto-Score Macro** mapped to the `Y` button:
+
+```java
+@Override
+protected void stateMachineUpdate() {
+    
+    // Drive code continues running perfectly while routines run!
+    drive.setDriveVectors(-g1.left_stick_y(), g1.left_stick_x(), g1.right_stick_x());
+
+    // 1. Trigger the Routine
+    if (g1.yWasPressed() && !routines.isBusy()) {
+        
+        routines.run(
+            Routine.sequence(
+                // Step 1: Tell the lift to go up
+                Routine.instant(() -> lift.setTargetState(LiftState.SCORING_HIGH)),
+                
+                // Step 2: Wait until the lift physically reaches the top
+                Routine.waitUntil(() -> lift.isAtTarget()),
+                
+                // Step 3: Open the claw
+                Routine.instant(() -> claw.open()),
+                
+                // Step 4: Wait 250ms for the pixel/sample to fall out
+                Routine.wait(250),
+                
+                // Step 5: Retract the lift AND close the claw simultaneously
+                Routine.parallel(
+                    Routine.instant(() -> lift.setTargetState(LiftState.RETRACTED)),
+                    Routine.instant(() -> claw.close())
+                ),
+
+                // Step 6: Vibrate the controller to tell the driver it's done
+                Routine.instant(() -> g1.rumble(500))
+            )
+        );
+    }
+}
+```
+
+---
+
+## 🛑 Driver Overrides & Canceling (Crucial for Safety)
+
+Because routines run automatically, **you must give your driver a way to cancel them** in case something goes wrong (e.g., the lift gets stuck on another robot).
+
+Use `routines.cancelAll()` mapped to a panic button (like `B`) to instantly kill all running scripts and return manual control to the driver.
+
+```java
+@Override
+protected void stateMachineUpdate() {
+
+    // PANIC BUTTON: Instantly kill the macro
+    if (g1.bWasPressed()) {
+        routines.cancelAll();
+        lift.setTargetState(LiftState.RETRACTED); // Force hardware to a safe state
+        g1.rumbleBlips(2); // Audio/Haptic feedback that the macro died
+    }
+
+    // ... routine triggering logic ...
+}
+```
+
+---
+
+## 💡 Best Practices
+
+1. **Check `!routines.isBusy()`**: Always ensure a routine isn't currently running before starting a new one, otherwise you might trigger the "Auto-Score" sequence twice at the same time and cause conflicting hardware commands.
+2. **Use Subsystem States, Not Hardware!**: Do **not** put raw hardware commands (like `motor.setPower(1)`) inside a `Routine.instant()`. Always use your Subsystem's `setTargetState()` methods. The Subsystem should still be in charge of calculating its own PID and limits.
+3. **Use `waitUntil` instead of `wait`**: Whenever possible, wait for a sensor or an encoder position rather than a fixed amount of time. If your battery is low, a `wait(500)` might not be enough time for the lift to go up. A `waitUntil(() -> lift.isAtTarget())` works perfectly on *every* battery voltage.

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java

@@ -12,6 +12,33 @@ public class Constants {
         public static int TARGET_FPS = 0; // No target FPS (UNCAPPED)
     }
 
+    public static class Gamepad {
+        public static double STICK_DEADBAND = 0.05;
+        public static double TRIGGER_THRESHOLD = 0.3;
+    }
+    public static class PIDController {
+        public static double VOLTAGE_REFERENCE = 12.0; // Voltage PID Controller was tuned at
+        public static double VOLTAGE_CHECK_INTERVAL_MS = 500.0; // Every half second
+    }
+
+    @Configurable
+    public static class LIMELIGHT {
+        public static String NAME = "limelight";
+        public static int POLL_RATE = 100;
+
+
+        // Trig Distance
+        public static double MOUNT_HEIGHT_IN = 0.0;
+        public static double TARGET_HEIGHT_IN = 29.5; // DECODE SEAON GOAL HEIGHT
+        public static double MOUNT_ANGLE_DEGREES = 0.0;
+
+        public static double AREA_COEFF = 74.34095;
+        public static double AREA_EXPONENT = -0.518935;
+        public static double AREA_OFFSET = 4.0;
+
+        public static double STALE_DATA_TIMEOUT_SEC = 0.5;
+    }
+
     @Configurable
     public static class DRIVE {
         // Hardware Names

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/teleOp.java

@@ -5,7 +5,8 @@ import org.firstinspires.ftc.teamcode.lib.BaseOpMode;
 import org.firstinspires.ftc.teamcode.subsys.Drivetrain;
 import org.firstinspires.ftc.teamcode.util.AutoTransfer;
 
-@TeleOp(name = "Competition TeleOp", group = "Main")
+
+@TeleOp(name = "Basic TeleOp", group = "Main")
 public class teleOp extends BaseOpMode {
 
     public enum GlobalState {
@@ -32,14 +33,24 @@ public class teleOp extends BaseOpMode {
 
     @Override
     protected void stateMachineUpdate() {
-        drive.setDriveVectors(-gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x);
+        if (g1.backWasPressed()) {
+
+            routines.cancelAll();
+
+            robotState = GlobalState.IDLE;
+
+            drive.setTargetState(Drivetrain.DriveState.NORMAL);
+
+            g1.rumbleBlips(2);
+        }
+        drive.setDriveVectors(-g1.left_stick_y(), g1.left_stick_x(), g1.right_stick_y());
 
         switch (robotState) {
 
             case IDLE:
                 drive.setTargetState(Drivetrain.DriveState.NORMAL);
 
-                if (gamepad1.aWasPressed()) {
+                if (g1.aWasPressed()) {
                     robotState = GlobalState.SCORING;
                 }
                 break;
@@ -47,7 +58,7 @@ public class teleOp extends BaseOpMode {
             case SCORING:
                 drive.setTargetState(Drivetrain.DriveState.PRECISION);
 
-                if (gamepad1.bWasPressed()) {
+                if (g2.bWasPressed()) {
                     robotState = GlobalState.IDLE;
                 }
                 break;