Arkm/ftc-lib
Template
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Sloth Added, LimeLight support, and Routines

Browse Source
This commit is contained in:
Arkm20
2026-03-23 14:11:04 -05:00
parent c210315e16
commit 2da0c06cc1
11 changed files with 559 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
TeamCode/build.gradle
View File

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

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

@@ -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("Current FPS", fps.getAverageFps());
unifiedTelemetry.addData("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)
}
}

110
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/EnhancedGamepad.java Normal file
View File

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

72
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/LLUtil.java Normal file
View File

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

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

@@ -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() {

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/Action.java Normal file
View File

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

130
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/Routine.java Normal file
View File

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

32
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/RoutineManager.java Normal file
View File

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

99
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/routine.md Normal file
View File

@@ -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.

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

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

19
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/teleOp.java
View File

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