DriveConstants.java

package org.firstinspires.ftc.team16911.drive;

import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;

/*
 * Constants shared between multiple drive types.
 *
 * TODO: Tune or adjust the following constants to fit your robot. Note that the non-final
 * fields may also be edited through the dashboard (connect to the robot's WiFi network and
 * navigate to https://192.168.49.1:8080/dash). Make sure to save the values here after you
 * adjust them in the dashboard; **config variable changes don't persist between app restarts**.
 *
 * These are not the only parameters; some are located in the localizer classes, drive base classes,
 * and op modes themselves.
 */
@Config
public class DriveConstants {

    /*
     * These are motor constants that should be listed online for your motors.
     */
    public static final double TICKS_PER_REV = 537.6;
    public static final double MAX_RPM = 312.5;

    /*
     * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders.
     * Set this flag to false if drive encoders are not present and an alternative localization
     * method is in use (e.g., tracking wheels).
     *
     * If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients
     * from DriveVelocityPIDTuner.
     */
    public static final boolean RUN_USING_ENCODER = false;
    public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0,
            getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));

    /*
     * These are physical constants that can be determined from your robot (including the track
     * width; it will be tune empirically later although a rough estimate is important). Users are
     * free to chose whichever linear distance unit they would like so long as it is consistently
     * used. The default values were selected with inches in mind. Road runner uses radians for
     * angular distances although most angular parameters are wrapped in Math.toRadians() for
     * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
     */
    public static double WHEEL_RADIUS = 1.47638; // in
    public static double GEAR_RATIO = 26.0 / 20.0; // output (wheel) speed / input (motor) speed
    public static double TRACK_WIDTH = 11.2; // in

    /*
     * These are the feedforward parameters used to model the drive motor behavior. If you are using
     * the built-in velocity PID, *these values are fine as is*. However, if you do not have drive
     * motor encoders or have elected not to use them for velocity control, these values should be
     * empirically tuned.
     */
    public static double kV = 0.0130578218;
//    public static double kV = 1.0 / rpmToVelocity(MAX_RPM);
    public static double kA = 0.00198;
    public static double kStatic = 0.0759;

    /*
     * These values are used to generate the trajectories for you robot. To ensure proper operation,
     * the constraints should never exceed ~80% of the robot's actual capabilities. While Road
     * Runner is designed to enable faster autonomous motion, it is a good idea for testing to start
     * small and gradually increase them later after everything is working. All distance units are
     * inches.
     */
    public static double MAX_VEL = 21;
    public static double MAX_ACCEL = 28;
    public static double MAX_ANG_VEL = Math.toRadians(120);
    public static double MAX_ANG_ACCEL = Math.toRadians(90);


    public static double encoderTicksToInches(double ticks) {
        return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
    }

    public static double rpmToVelocity(double rpm) {
        return rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS / 60.0;
    }

    public static double getMotorVelocityF(double ticksPerSecond) {
        // see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx
        return 32767 / ticksPerSecond;
    }
}

Lockup.java

package org.firstinspires.ftc.teamcode.gamepad;

import com.qualcomm.robotcore.util.ElapsedTime;

/**
 * The Lockup strategy is used specifically for ensuring gamepad switching does not accidentally
 * activate buttons. This is done by simply activating the 'lockup' state for a defined period
 * after the Options (PS4 controller) button is pressed.
 */
public class Lockup implements RawBooleanStrategy {
    private ElapsedTime time; // Amount of time passed since init or initial button press.
    private double lockupTime; // Amount of time that state should be active for after button press.

    private boolean previous; // The previous button state.
    private boolean state; // Whether lockup is active or not.

    public Lockup() {
        this(1.5);
    }

    public Lockup(double lockupTime) {
        this.lockupTime = lockupTime;
        time = new ElapsedTime();
    }

    @Override
    public void update(boolean next) {
        if (state) {
            // If state is active, we ignore if the button changed or not.
            if (time.seconds() >= lockupTime) {
                // Lockup time has passed, 'unlock'.
                state = false;
            }
        } else if (next && !previous) {
            // If lockup is not active but the button has been just pressed, lockup and start timing.
            time.reset();
            state = true;
        }

        // Remember current state
        previous = next;
    }

    @Override
    public boolean read() {
        return state;
    }
}

Macaroni.java

package org.firstinspires.ftc.team16911.telop;

import com.acmerobotics.dashboard.FtcDashboard;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.util.ElapsedTime;

import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.team16911.hardware.MacaroniHardware;
import org.firstinspires.ftc.teamcode.Util;

@TeleOp(name = "Macaroni", group = "Linear OpMode")
public class Macaroni extends LinearOpMode {
    final MacaroniHardware robot = new MacaroniHardware();
    final ElapsedTime runTime = new ElapsedTime();
    double lastControlsUpdate;

    double wheelPower = 1;
    private static final double powerGranularity = 0.05;
    double launcherPower = 1f;

    // Wobble Goal Servo states
    boolean armActive = false; // Current state of the Lower Wobble Goal (Arm)
    boolean clawActive = false; // Current state of the Upper Wobble Goal (Claw)
    boolean prevArmState = false;
    boolean prevClawState = false;

    // Ring Stopper Servo states
    boolean ringStopperActive = true;
    boolean prevRingStopper = true;

    // Other states
    boolean toggleLauncher = false;
    boolean intakeDirection = true;

    FtcDashboard dashboard;
    Telemetry dash_telemetry;

    @Override
    public void runOpMode() {
        lastControlsUpdate = runTime.seconds();
        dashboard = FtcDashboard.getInstance();
        dash_telemetry = dashboard.getTelemetry();

        waitForStart();

        robot.init(hardwareMap);

        // TODO: Implement Road Runner drivetrain controls
        // TODO: Motor speed shooting block

        if (isStopRequested()) return;

        while (opModeIsActive()) {
            driverControls();
            operatorControls();
            dash_telemetry.update();
        }
    }

    private void operatorControls() {
        // Lower Wobble (Arm) controls
        if (gamepad2.x && !prevArmState) {
            robot.armServo.setPosition(armActive ? 0 : 1);
            armActive = !armActive;
        }
        prevArmState = gamepad2.x;

        // Upper Wobble (Claw) controls
        if (gamepad2.circle && !prevClawState) {
//            robot.leftClawServo.setPosition(clawActive ? 1 : 0);
//            robot.rightClawServo.setPosition(clawActive ? 0 : 1);
            clawActive = !clawActive;
        }
        prevClawState = gamepad2.circle;

        // Ring Stopper controls
        if (gamepad2.triangle && !prevRingStopper) {
//            robot.ringStopperServo.setPosition(ringStopperActive ? 0 : 1);
            ringStopperActive = !ringStopperActive;
        }
        prevRingStopper = gamepad2.triangle;

        boolean controllerTouched = true;
        if (controllerLoop(0.15)) {
            if (gamepad2.dpad_left)
                launcherPower = 0.65;
            else if (gamepad2.dpad_right)
                launcherPower = 0.75;
            else if (gamepad2.dpad_up || gamepad2.dpad_down) {
                // Change launcher motor power with D-Pad controls
                double powerChange = gamepad2.dpad_up ? powerGranularity : -powerGranularity;
                launcherPower = Util.clamp((float) (launcherPower + powerChange), 0, 1);
            } else
                controllerTouched = false;

            if (controllerTouched)
                lastControlsUpdate = runTime.seconds();

            // Toggle launcher
            // if (gamepad2.share) toggleLauncher = !toggleLauncher;
        }
        dash_telemetry.addData("launcherPower", launcherPower);
        telemetry.addData("Launcher Power", ((int) (launcherPower * 100)) + "%");
        telemetry.update();

        // Intake if Right Trigger
        if (gamepad1.right_trigger > 0) {
            double rt = intakeDirection ? Util.cubicEasing(gamepad2.right_trigger) : -Util.cubicEasing(gamepad2.right_trigger);
//            robot.intakeMotor.setPower(rt);
        }

        // Right trigger to run all intake motors/servos at desired speed
        if (gamepad1.left_bumper) {
//            robot.beltMotor.setPower(-1);
//            robot.intakeMotor.setPower(-1);
        } else if (gamepad1.right_bumper) {
//            robot.beltMotor.setPower(1);
//            robot.intakeMotor.setPower(1);
        } else {
//            robot.intakeMotor.setPower(0);
//            robot.beltMotor.setPower(gamepad2.left_stick_y > 0.05 ? 1 : (gamepad2.left_stick_y < -0.05 ? -1 : 0));
        }

//        dash_telemetry.addData("beltMotor", robot.beltMotor.getPower());
//        dash_telemetry.addData("intakeMotor", robot.intakeMotor.getPower());
        dash_telemetry.addData("beltMotor", gamepad2.left_stick_y);
//        dash_telemetry.addData("launcherMotor", robot.launcherMotor.getPower());
//        robot.launcherMotor.setPower(gamepad2.left_trigger > 0 ? launcherPower : 0);
    }

    /**
     * Processes the driver gamepad's controls.
     * <p>
     * Left and Right Stick: Move robot
     * Left and Right Bumpers: Rotate robot slowly
     */
    private void driverControls() {
        // Apply easing function to all stick inputs
        double left_stick_y = Util.cubicEasing(gamepad1.left_stick_y);
        double left_stick_x = Util.cubicEasing(gamepad1.left_stick_x);
        double right_stick_x = Util.cubicEasing(gamepad1.right_stick_x);
        double right_stick_y = Util.cubicEasing(gamepad1.right_stick_y);

        // Update wheel power multipier
        if (gamepad1.square)
            wheelPower = 1;
        else if (gamepad1.triangle)
            wheelPower = 0.25;
        else if (gamepad1.circle)
            wheelPower = 0.5;
        else if (gamepad1.cross)
            wheelPower = 0.75;

        if (gamepad1.dpad_up)
            left_stick_y = -1;
        else if (gamepad1.dpad_down)
            left_stick_y = 1;

        if (gamepad1.dpad_right)
            left_stick_x = 1;
        else if (gamepad1.dpad_left)
            left_stick_x = -1;


        // Left and Right bumper controls (slow rotate)
        right_stick_x = gamepad1.right_bumper ? 0.2 : (gamepad1.left_bumper ? -0.2 : right_stick_x);

        // Mechanum trig math
        double radius = Math.hypot(left_stick_x, -left_stick_y);
        double ang = Math.atan2(-left_stick_y, left_stick_x) - Math.PI / 4;
        double turnCon = right_stick_x * .75;

        // Final motor powers, with multiplier applied
        double v1 = (radius * Math.cos(ang) + turnCon) * wheelPower;
        double v2 = (radius * Math.sin(ang) - turnCon) * wheelPower;
        double v3 = (radius * Math.sin(ang) + turnCon) * wheelPower;
        double v4 = (radius * Math.cos(ang) - turnCon) * wheelPower;

        dash_telemetry.addData("leftFront", v1);
        dash_telemetry.addData("rightFront", v2);
        dash_telemetry.addData("leftRear", v3);
        dash_telemetry.addData("rightRear", v4);

        dash_telemetry.addData("leftEncoder", robot.encoderLeft.getCorrectedVelocity());
        dash_telemetry.addData("rightEncoder", robot.encoderRight.getCorrectedVelocity());
        dash_telemetry.addData("forwardEncoder", robot.encoderFront.getCorrectedVelocity());

        // Sets power of motor, spins wheels
        robot.motorLeftFront.setPower(v1);
        robot.motorRightFront.setPower(v2);
        robot.motorLeftRear.setPower(v3);
        robot.motorRightRear.setPower(v4);
    }

    /**
     * A simple helper function that assists with spacing out controller input polls.
     * <p>
     * Without a proper guard in place, controller input code for button presses will be ran hundreds of
     * times per second, resulting in strange behavior. By adding a guard that only lets the button input
     * code run every other fraction of a second, button input is much more reliable for humans.
     *
     * @param duration The minimum duration that must be elapsed before the controller loop is ran.
     * @return True if the controller should be polled now, false if the duration has not yet elapsed.
     */
    public boolean controllerLoop(double duration) {
        double waited = runTime.seconds() - lastControlsUpdate;
        dash_telemetry.addData("controllerLoop", Util.clamp((float) (waited / duration), 0f, 1f));
        return waited >= duration;
    }
}

MacaroniBasic.java

package org.firstinspires.ftc.team16911.auton;


import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;

import org.firstinspires.ftc.team16911.drive.SampleMecanumDrive;
import org.firstinspires.ftc.team16911.hardware.MacaroniHardware;

/*
 * This is a simple routine to test translational drive capabilities.
 */
@Config
@Autonomous(group = "drive")
public class MacaroniBasic extends LinearOpMode {
    public static final Pose2d START_POSITION = new Pose2d(-(72 - 10.5), 24);

    private static final double LAUNCHER_POWER = 0.875;
    private static final int PRESPIN_TIME = 3500;
    private static final int RESPIN_TIME = 1500;
    private static final int RAMP_TIME = 200;

    @Override
    public void runOpMode() throws InterruptedException {
        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
        MacaroniHardware robot = new MacaroniHardware();
        robot.init(hardwareMap);

        drive.setPoseEstimate(START_POSITION);

        Trajectory traj1 = drive.trajectoryBuilder(START_POSITION)
                .splineToLinearHeading(new Pose2d(36, 18), 0)
                .build();

        Trajectory traj2 = drive.trajectoryBuilder(traj1.end())
                .splineToLinearHeading(new Pose2d(0, 24), 0)
                .build();

        Trajectory traj3 = drive.trajectoryBuilder(traj2.end())
                .splineToLinearHeading(new Pose2d(12, 24), 0)
                .build();

        waitForStart();

        if (isStopRequested()) return;

        // Drive forward
        drive.followTrajectory(traj1);

        // Lower arms and drop wobble goal
        sleep(400);
        robot.armServo.setPosition(0.2);
        sleep(700);
        robot.setClaws(1);
        sleep(500);
        robot.armServo.setPosition(1);
        sleep(400);
        robot.setClaws(0.2);
        sleep(250);

        // Drive back to launching line
        drive.followTrajectory(traj2);

        robot.spinLauncher(LAUNCHER_POWER);
        sleep(PRESPIN_TIME);

        robot.setIntake(0.9, 0);
        sleep(3500);
        robot.spinLauncher(0);
        robot.setIntake(0, 0);
        sleep(500);

        drive.followTrajectory(traj3);

        while (!isStopRequested() && opModeIsActive()) {
            idle();
        }

        Pose2d poseEstimate = drive.getPoseEstimate();
        telemetry.addData("finalX", poseEstimate.getX());
        telemetry.addData("finalY", poseEstimate.getY());
        telemetry.addData("finalHeading", poseEstimate.getHeading());
        telemetry.update();
    }
}

MacaroniOld.java

package org.firstinspires.ftc.team16911.auton;


import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;

import org.firstinspires.ftc.team16911.drive.MacaroniDrive;

//import org.firstinspires.ftc.team16911.drive.MacaroniDrive;

/*
 * This is a simple routine to test translational drive capabilities.
 */
@Config
@Autonomous(group = "drive", preselectTeleOp = "Macaroni v2")
public class MacaroniOld extends LinearOpMode {

    private static final double LAUNCHER_POWER = 0.80;
    private static final int PRESPIN_TIME = 2500;
    private static final int RESPIN_TIME = 1500;
    private static final int RAMP_TIME = 200;

    @Override
    public void runOpMode() throws InterruptedException {
        MacaroniDrive drive = new MacaroniDrive(hardwareMap);

        Pose2d startPose = new Pose2d(-60, 24, 0);
        drive.setPoseEstimate(startPose);
//
        Trajectory traj1 = drive.trajectoryBuilder(startPose)
                .forward(90)
//                .lineTo(new Vector2d(36, 24))
                .build();

        Trajectory traj2 = drive.trajectoryBuilder(traj1.end())
                .back(34)
                .build();

        Trajectory traj3 = drive.trajectoryBuilder(traj2.end())
                .strafeLeft(18)
                .build();

        Trajectory traj4 = drive.trajectoryBuilder(traj3.end())
                .forward(7)
                .build();

        waitForStart();

        if (isStopRequested()) return;

        // Drive forward
        drive.followTrajectory(traj1);

        // Lower arms and drop wobble goal
        sleep(400);
        drive.armServo.setPosition(0.6);
        sleep(700);
        drive.setClaws(1);
        sleep(1100);

        // Drive back to launching line
        drive.followTrajectory(traj2);
        drive.followTrajectory(traj3);

        drive.launcherMotor.setPower(Math.max(1, LAUNCHER_POWER + 0.15));
        sleep(PRESPIN_TIME - 500);
        drive.launcherMotor.setPower(LAUNCHER_POWER);
        sleep(500);

        runRamp(drive, RAMP_TIME);
        sleep(RESPIN_TIME);

        runRamp(drive, RAMP_TIME);
        sleep(RESPIN_TIME);

        runRamp(drive, RAMP_TIME);
        sleep(250);

        drive.launcherMotor.setPower(0);
        sleep(500);

        drive.followTrajectory(traj4);

        while (!isStopRequested() && opModeIsActive()) {
            idle();
        };

        Pose2d poseEstimate = drive.getPoseEstimate();
        telemetry.addData("finalX", poseEstimate.getX());
        telemetry.addData("finalY", poseEstimate.getY());
        telemetry.addData("finalHeading", poseEstimate.getHeading());
        telemetry.update();
    }

    public void runRamp(MacaroniDrive drive, float duration) {
        drive.beltMotor.setPower(1);
        sleep((long) duration);
        drive.beltMotor.setPower(0);

    }
}

MacaroniTest.java

package org.firstinspires.ftc.team16911.auton;

import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;

import org.firstinspires.ftc.team16911.drive.SampleMecanumDrive;
import org.firstinspires.ftc.team16911.hardware.MacaroniHardware;

@Config
@Autonomous(group = "drive", preselectTeleOp = "Macaroni v2")
public class MacaroniTest extends LinearOpMode {
    @Override
    public void runOpMode() throws InterruptedException {
        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);

        Pose2d startPose = new Pose2d(-60, 24, 0);
        drive.setPoseEstimate(startPose);

        Trajectory traj1 = drive.trajectoryBuilder(startPose)
                .forward(90)
//                .lineTo(new Vector2d(36, 24))
                .build();

        Trajectory traj2 = drive.trajectoryBuilder(traj1.end())
                .back(31)
                .build();

        Trajectory traj3 = drive.trajectoryBuilder(traj2.end())
                .strafeLeft(18)
                .build();

        Trajectory traj4 = drive.trajectoryBuilder(traj3.end())
                .forward(7)
                .build();

        waitForStart();

        if (isStopRequested()) return;

        drive.followTrajectory(traj1);
        sleep(1000);
        drive.followTrajectory(traj2);
        sleep(1000);
        drive.followTrajectory(traj3);
        sleep(1000);
        drive.followTrajectory(traj4);
        sleep(1000);

        while (!isStopRequested() && opModeIsActive()) {
            idle();
        }

        Pose2d poseEstimate = drive.getPoseEstimate();
        telemetry.addData("finalX", poseEstimate.getX());
        telemetry.addData("finalY", poseEstimate.getY());
        telemetry.addData("finalHeading", poseEstimate.getHeading());
        telemetry.update();
    }
}

MacaroniV2.java

package org.firstinspires.ftc.team16911.telop;

import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.team16911.auton.MacaroniBasic;
import org.firstinspires.ftc.team16911.drive.SampleMecanumDrive;
import org.firstinspires.ftc.team16911.hardware.MacaroniHardware;
import org.firstinspires.ftc.teamcode.gamepad.Lockup;
import org.firstinspires.ftc.teamcode.gamepad.SingleDown;
import org.firstinspires.ftc.teamcode.gamepad.TimedGamepad;
import org.firstinspires.ftc.teamcode.gamepad.Toggle;

@TeleOp(name = "Macaroni v2", group = "Linear OpMode")
public class MacaroniV2 extends OpMode {
    // Drivetrain, Robot Hardware and Telemetry controllers
    private SampleMecanumDrive drive;
    private final MacaroniHardware robot = new MacaroniHardware();
    private Telemetry dash_telemetry;

    // Gamepad State Control
    private TimedGamepad timedGamepad1;
    private TimedGamepad timedGamepad2;
    private Lockup gamepad1_lockup;
    private Lockup gamepad2_lockup;
    private Toggle armToggle;
    private Toggle clawToggle;
    private Toggle stopperToggle;
    private SingleDown dpad_up;
    private SingleDown dpad_down;

    // Tele-operated Configuration Constants
    public static double NUDGE_AMOUNT = 0.5;
    public static double X_SCALE = 0.7;
    public static double Y_SCALE = 0.7;
    public static double TURN_SCALE = 0.85;

    public double LAUNCHER_POWER = 0.85;


    @Override
    public void init() {
        FtcDashboard dashboard = FtcDashboard.getInstance();
        dash_telemetry = dashboard.getTelemetry();

        drive = new SampleMecanumDrive(hardwareMap);
        drive.setPoseEstimate(MacaroniBasic.START_POSITION);
        robot.init(hardwareMap);

        timedGamepad1 = new TimedGamepad(this.gamepad1);
        timedGamepad2 = new TimedGamepad(this.gamepad2);

        armToggle = new Toggle();
        clawToggle = new Toggle();
        stopperToggle = new Toggle();

        dpad_up = new SingleDown();
        dpad_down = new SingleDown();

        gamepad1_lockup = new Lockup();
        gamepad2_lockup = new Lockup();

        timedGamepad1.options.installStrategy(gamepad1_lockup);
        timedGamepad2.options.installStrategy(gamepad2_lockup);

        timedGamepad2.square.installStrategy(armToggle);
        timedGamepad2.circle.installStrategy(clawToggle);
        timedGamepad2.triangle.installStrategy(stopperToggle);

        timedGamepad2.dpad_up.installStrategy(dpad_up);
        timedGamepad2.dpad_down.installStrategy(dpad_down);
    }

    @Override
    public void loop() {
        timed_gamepads();
        driver();
        operator();

        telemetry.update();
        dash_telemetry.update();
    }

    /**
     * Update the TimedGamepad instances.
     */
    private void timed_gamepads() {
        timedGamepad1.tick();
        timedGamepad2.tick();
    }

    /**
     * Processes operator (Gamepad 2) controls.
     */
    private void operator() {
        // Don't process this when the gamepad is locked up by pressing Gamepad2's Options button.
        if (!gamepad2_lockup.read()) {
            // Servo toggles
            robot.armServo.setPosition(armToggle.read() ? 0 : 1);
            robot.setClaws(clawToggle.read() ? 0 : 1);
            robot.setRingStopper(stopperToggle.read());
        }

        // Spin the launcher with left trigger to the specified launcher power
        robot.spinLauncher(gamepad2.left_trigger > 0.5 ? LAUNCHER_POWER : 0.0);

        // Intake with right trigger, outtake with right bumper
        if (gamepad2.right_trigger > 0.5)
            robot.setIntake(1, 1);
        else if (gamepad2.right_bumper)
            robot.setIntake(-1, -1);
        else
            robot.setIntake(0, 0);

        // Power change with DPAD up/down
        if (dpad_up.poll())
            LAUNCHER_POWER += 0.05;
        else if (dpad_down.poll())
            LAUNCHER_POWER -= 0.05;
        LAUNCHER_POWER = Math.max(1, Math.min(0, LAUNCHER_POWER));

        telemetry.addData("Launcher Power", ((int) (LAUNCHER_POWER * 100)) + "%");
    }

    private void driver() {
        // Nudge controls, dpad + left/righter trigger
        double x_nudge = gamepad1.dpad_right ? NUDGE_AMOUNT : (gamepad1.dpad_left ? -NUDGE_AMOUNT : 0);
        double y_nudge = gamepad1.dpad_up ? -NUDGE_AMOUNT : (gamepad1.dpad_down ? NUDGE_AMOUNT : 0);
        double ang_nudge = gamepad1.left_trigger > 0
                ? gamepad1.left_trigger * -NUDGE_AMOUNT
                : gamepad1.right_trigger * NUDGE_AMOUNT;

        // Road runner based controls with left/right stick XY
        if (!drive.isBusy()) {
            drive.setWeightedDrivePower(
                    new Pose2d(
                            -(gamepad1.left_stick_y * Y_SCALE + y_nudge),
                            -(gamepad1.left_stick_x * X_SCALE + x_nudge),
                            -(gamepad1.right_stick_x * TURN_SCALE + ang_nudge)
                    )
            );
        }

        drive.update();

        if (gamepad1.left_bumper) {
            drive.followTrajectoryAsync(drive.trajectoryBuilder(drive.getPoseEstimate())
                    .lineToSplineHeading(MacaroniBasic.START_POSITION)
                    .build());
        } else if (gamepad1.right_bumper) {
            drive.followTrajectoryAsync(drive.trajectoryBuilder(drive.getPoseEstimate())
                    .lineToSplineHeading(new Pose2d(0, 0, 0))
                    .build()
            );
        }

        // Drive speed change
        if (!gamepad1_lockup.read()) {
            if (gamepad1.triangle)
                X_SCALE = Y_SCALE = 0.25;
            else if (gamepad1.circle)
                X_SCALE = Y_SCALE = 0.5;
            else if (gamepad1.cross)
                X_SCALE = Y_SCALE = 0.75;
            else if (gamepad1.square)
                X_SCALE = Y_SCALE = 1.0;
        }

        telemetry.addData("X Scale", ((int) (X_SCALE * 100)) + "%");
        telemetry.addData("Y Scale", ((int) (Y_SCALE * 100)) + "%");
    }
}

SingleDown.java

package org.firstinspires.ftc.teamcode.gamepad;

/**
 * The SingleDown strategy fires on the tick that the button is pressed down.
 */
public class SingleDown implements PolledStrategy {
    private boolean previous = false; // The last state of the button.
    private boolean state = false; // The current state of the strategy.
    private boolean polled = false; // Whether or not the current variable state has been read.

    public SingleDown() {
    }

    @Override
    public void update(boolean next) {
        // If the last state was off and it's now on, the key was pressed.
        if (!previous && next) {
            state = true;
        } else {
            // Clear states.
            state = false;
        }
        polled = false;

        previous = next;
    }

    /**
     * Returns gamepad button state. Polling can only return once in a row per call to update.
     *
     * @return Returns the state of the gamepad button.
     * @see #update(boolean)
     */
    public boolean poll() {
        if (polled)
            return false;
        else {
            polled = true;
            return state;
        }
    }

    /**
     * @return Returns the true state of the gamepad button state.
     */
    public boolean peek() {
        return state;
    }
}

StandardTrackingWheelLocalizer.java

package org.firstinspires.ftc.team16911.drive;

import androidx.annotation.NonNull;

import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;

import org.firstinspires.ftc.teamcode.hardware.ComponentIds;
import org.firstinspires.ftc.teamcode.util.Encoder;

import java.util.Arrays;
import java.util.List;

/*
 * Sample tracking wheel localizer implementation assuming the standard configuration:
 *
 *    /--------------\
 *    |     ____     |
 *    |     ----     |
 *    | ||        || |
 *    | ||        || |
 *    |              |
 *    |              |
 *    \--------------/
 *
 */
@Config
public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer {
    public static double TICKS_PER_REV = 8192;
    public static double WHEEL_RADIUS = 1.1811; // in
    public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed

    public static double LATERAL_DISTANCE = 11.264766; // in; distance between the left and right wheels
    public static double FORWARD_OFFSET = 4.5; // in; offset of the lateral wheel

    public static double X_MULTIPLIER = 1.005562007; // Multiplier in the X direction
    public static double Y_MULTIPLIER = 1.007993909; // Multiplier in the Y direction

    private Encoder leftEncoder, rightEncoder, frontEncoder;

    public StandardTrackingWheelLocalizer(HardwareMap hardwareMap) {
        super(Arrays.asList(
                new Pose2d(0, LATERAL_DISTANCE / 2, 0), // left
                new Pose2d(0, -LATERAL_DISTANCE / 2, 0), // right
                new Pose2d(FORWARD_OFFSET, 0, Math.toRadians(90)) // front
        ));

        leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, ComponentIds.LEFT_ENCODER));
        rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, ComponentIds.RIGHT_ENCODER));
        frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, ComponentIds.FRONT_ENCODER));

        // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
//        frontEncoder.setDirection(Encoder.Direction.REVERSE);
        leftEncoder.setDirection(Encoder.Direction.REVERSE);
        rightEncoder.setDirection(Encoder.Direction.REVERSE);
    }

    public static double encoderTicksToInches(double ticks) {
        return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
    }

    @NonNull
    @Override
    public List<Double> getWheelPositions() {
        return Arrays.asList(
                encoderTicksToInches(leftEncoder.getCurrentPosition()) * X_MULTIPLIER,
                encoderTicksToInches(rightEncoder.getCurrentPosition()) * X_MULTIPLIER,
                encoderTicksToInches(frontEncoder.getCurrentPosition()) * Y_MULTIPLIER
        );
    }

    @NonNull
    @Override
    public List<Double> getWheelVelocities() {
        // TODO: If your encoder velocity can exceed 32767 counts / second (such as the REV Through Bore and other
        //  competing magnetic encoders), change Encoder.getRawVelocity() to Encoder.getCorrectedVelocity() to enable a
        //  compensation method

        return Arrays.asList(
                encoderTicksToInches(leftEncoder.getCorrectedVelocity()) * X_MULTIPLIER,
                encoderTicksToInches(rightEncoder.getCorrectedVelocity()) * X_MULTIPLIER,
                encoderTicksToInches(frontEncoder.getCorrectedVelocity()) * Y_MULTIPLIER
        );
    }
}

Toggle.java

package org.firstinspires.ftc.teamcode.gamepad;

/**
 * The Toggle strategy changes state between true and false each time the button is pressed.
 * Can be configured to respond during either the button up or down tick.
 */
public class Toggle implements RawBooleanStrategy {
    private boolean state = false; // Current strategy toggle state
    private boolean previous = false; // Last button state
    private boolean onDown = true; // Change state on the button up or down tick


    /**
     * @param onDown Whether or not the Toggle class should change state on the button up or down tick.
     */
    Toggle(boolean onDown) {
        this.onDown = onDown;
    }

    /**
     * Defaults to responding to state change during the button down tick.
     */
    public Toggle() {
    }

    @Override
    public void update(boolean next) {
        // Change state every time the button is depressed/lifted depending on configuration
        if ((next != previous) && (onDown ? next : previous))
            state = !state;

        // Remember previous tick's button state
        previous = next;
    }

    @Override
    public boolean read() {
        return state;
    }
}