limelight works

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/blank.java

@@ -1,4 +0,0 @@
-package org.firstinspires.ftc.teamcode.constants;
-
-public class blank {
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java

@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.libs.RR;
 
+
 import androidx.annotation.NonNull;
 
 import com.acmerobotics.dashboard.canvas.Canvas;
@@ -211,6 +212,7 @@ public final class MecanumDrive {
                     headingDelta
             ));
 
+
             return twist.velocity().value();
         }
     }
@@ -222,7 +224,7 @@ public final class MecanumDrive {
             module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
         }
 
-        // TODO: make sure your config has motors with these names (or change them)
+        // TODO: make sure your HardwareConfig has motors with these names (or change them)
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
         leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
         leftBack = hardwareMap.get(DcMotorEx.class, "leftBack");
@@ -237,7 +239,7 @@ public final class MecanumDrive {
         // TODO: reverse motor directions if needed
         //   leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
 
-        // TODO: make sure your config has an IMU with this name (can be BNO or BHI)
+        // TODO: make sure your HardwareConfig has an IMU with this name (can be BNO or BHI)
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
         lazyImu = new LazyHardwareMapImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
                 PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/OTOSLocalizer.java

@@ -28,7 +28,7 @@ public class OTOSLocalizer implements Localizer {
     private Pose2d currentPose;
 
     public OTOSLocalizer(HardwareMap hardwareMap, Pose2d initialPose) {
-        // TODO: make sure your config has an OTOS device with this name
+        // TODO: make sure your HardwareConfig has an OTOS device with this name
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
         otos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos");
         currentPose = initialPose;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java

@@ -29,7 +29,7 @@ public final class PinpointLocalizer implements Localizer {
     private Pose2d txPinpointRobot = new Pose2d(0, 0, 0);
 
     public PinpointLocalizer(HardwareMap hardwareMap, double inPerTick, Pose2d initialPose) {
-        // TODO: make sure your config has a Pinpoint device with this name
+        // TODO: make sure your HardwareConfig has a Pinpoint device with this name
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
         driver = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint");
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/TankDrive.java

@@ -232,7 +232,7 @@ public final class TankDrive {
             module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
         }
 
-        // TODO: make sure your config has motors with these names (or change them)
+        // TODO: make sure your HardwareConfig has motors with these names (or change them)
         //   add additional motors on each side if you have them
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
         leftMotors = Arrays.asList(hardwareMap.get(DcMotorEx.class, "left"));
@@ -248,7 +248,7 @@ public final class TankDrive {
         // TODO: reverse motor directions if needed
         //   leftMotors.get(0).setDirection(DcMotorSimple.Direction.REVERSE);
 
-        // TODO: make sure your config has an IMU with this name (can be BNO or BHI)
+        // TODO: make sure your HardwareConfig has an IMU with this name (can be BNO or BHI)
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
         lazyImu = new LazyHardwareMapImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
                 PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/ThreeDeadWheelLocalizer.java

@@ -37,7 +37,7 @@ public final class ThreeDeadWheelLocalizer implements Localizer {
     private Pose2d pose;
 
     public ThreeDeadWheelLocalizer(HardwareMap hardwareMap, double inPerTick, Pose2d initialPose) {
-        // TODO: make sure your config has **motors** with these names (or change them)
+        // TODO: make sure your HardwareConfig has **motors** with these names (or change them)
         //   the encoders should be plugged into the slot matching the named motor
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
         par0 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par0")));

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/TwoDeadWheelLocalizer.java

@@ -46,7 +46,7 @@ public final class TwoDeadWheelLocalizer implements Localizer {
     private Pose2d pose;
 
     public TwoDeadWheelLocalizer(HardwareMap hardwareMap, IMU imu, double inPerTick, Pose2d initialPose) {
-        // TODO: make sure your config has **motors** with these names (or change them)
+        // TODO: make sure your HardwareConfig has **motors** with these names (or change them)
         //   the encoders should be plugged into the slot matching the named motor
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
         par = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par")));

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTester.java

@@ -0,0 +1,61 @@
+package org.firstinspires.ftc.teamcode.tests;
+
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DigitalChannel;
+
+@Config
+@TeleOp
+
+@Disabled
+public class ColorSensorTester extends LinearOpMode {
+
+
+    public static String portAName = "pin0";
+
+    public static String portBName = "pin1";
+
+
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+
+        DigitalChannel pinA = hardwareMap.digitalChannel.get(portAName);
+        DigitalChannel pinB = hardwareMap.digitalChannel.get(portBName);
+
+
+
+
+        MultipleTelemetry TELE = new MultipleTelemetry(
+                telemetry,
+                FtcDashboard.getInstance().getTelemetry()
+        );
+
+
+
+        waitForStart();
+
+        if(isStopRequested()) return;
+
+        while(opModeIsActive()){
+
+            TELE.addData("pinA", pinA.getState());
+            TELE.addData("pinB", pinB.getState());
+
+            TELE.update();
+
+
+
+
+        }
+
+
+
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java

@@ -0,0 +1,77 @@
+package org.firstinspires.ftc.teamcode.tests;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.LLResultTypes;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import java.util.List;
+
+//TODO: fix to get the apriltag that it is reading
+@Config
+@TeleOp
+public class LimelightTest extends LinearOpMode {
+    MultipleTelemetry TELE;
+    public static int pipeline = 0; //0 is for test; 1 for obelisk; 2 is for blue track; 3 is for red track
+    public static int mode = 0; //0 for bare testing, 1 for obelisk, 2 for blue track, 3 for red track
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Limelight3A limelight = hardwareMap.get(Limelight3A.class, "Limelight");
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        limelight.pipelineSwitch(pipeline);
+        waitForStart();
+        if (isStopRequested()) return;
+        limelight.start();
+        while (opModeIsActive()){
+            if (mode == 0){
+                limelight.pipelineSwitch(pipeline);
+                LLResult result = limelight.getLatestResult();
+                if (result != null) {
+                    if (result.isValid()) {
+                        TELE.addData("tx", result.getTx());
+                        TELE.addData("ty", result.getTy());
+                        TELE.update();
+                    }
+                }
+            } else if (mode == 1){
+                limelight.pipelineSwitch(1);
+                LLResult result = limelight.getLatestResult();
+                if (result != null && result.isValid()) {
+                    List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
+                    for (LLResultTypes.FiducialResult fiducial : fiducials) {
+                        int id = fiducial.getFiducialId();
+                        TELE.addData("ID", id);
+                        TELE.update();
+                    }
+
+                }
+            } else if (mode == 2){
+                limelight.pipelineSwitch(2);
+                LLResult result = limelight.getLatestResult();
+                if (result != null) {
+                    if (result.isValid()) {
+                        TELE.addData("tx", result.getTx());
+                        TELE.addData("ty", result.getTy());
+                        TELE.update();
+                    }
+                }
+            } else if (mode == 3){
+                limelight.pipelineSwitch(3);
+                LLResult result = limelight.getLatestResult();
+                if (result != null) {
+                    if (result.isValid()) {
+                        TELE.addData("tx", result.getTx());
+                        TELE.addData("ty", result.getTy());
+                        TELE.update();
+                    }
+                }
+            } else {
+                limelight.pipelineSwitch(0);
+            }
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java

@@ -0,0 +1,68 @@
+package org.firstinspires.ftc.teamcode.tests;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+
+import org.firstinspires.ftc.teamcode.utils.Flywheel;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+@Config
+@TeleOp
+public class ShooterTest extends LinearOpMode {
+    public static int mode = 0;
+    public static double parameter = 0.0;
+    // --- CONSTANTS YOU TUNE ---
+    public static double hoodPos = 0.501;
+    Robot robot;
+    Flywheel flywheel;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        robot = new Robot(hardwareMap);
+        DcMotorEx leftShooter = robot.shooter1;
+        DcMotorEx rightShooter = robot.shooter2;
+        flywheel = new Flywheel();
+
+        MultipleTelemetry TELE = new MultipleTelemetry(
+                telemetry, FtcDashboard.getInstance().getTelemetry()
+        );
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()) {
+
+            if (mode == 0) {
+                rightShooter.setPower(parameter);
+                leftShooter.setPower(parameter);
+            } else if (mode == 1) {
+                double powPID = flywheel.manageFlywheel((int) parameter, leftShooter.getCurrentPosition(), rightShooter.getCurrentPosition());
+                rightShooter.setPower(powPID);
+                leftShooter.setPower(powPID);
+                TELE.addData("PIDPower", powPID);
+            }
+
+            if (hoodPos != 0.501) {
+                robot.hood.setPosition(hoodPos);
+            }
+
+            TELE.addData("Used Velocity", flywheel.getVelo(leftShooter.getCurrentPosition(), rightShooter.getCurrentPosition()));
+            TELE.addData("Velocity1", flywheel.getVelo1());
+            TELE.addData("Velocity2", flywheel.getVelo2());
+            TELE.addData("Power", robot.shooter1.getPower());
+            TELE.addData("Steady?", flywheel.getSteady());
+            TELE.addData("Position1", robot.shooter1.getCurrentPosition()/28);
+            TELE.addData("Position2", robot.shooter2.getCurrentPosition()/28);
+
+            TELE.update();
+
+        }
+
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java

@@ -0,0 +1,224 @@
+package org.firstinspires.ftc.teamcode.utils;
+
+import com.qualcomm.hardware.rev.RevColorSensorV3;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
+
+@Disabled
+@TeleOp
+public class ConfigureColorRangefinder extends LinearOpMode {
+    @Override
+    public void runOpMode() throws InterruptedException {
+        ColorRangefinder crf = new ColorRangefinder(hardwareMap.get(RevColorSensorV3.class, "Color"));
+        waitForStart();
+        /* Using this example configuration, you can detect both artifact colors based on which pin is reading true:
+            pin0 --> purple
+            pin1 --> green */
+        crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 160 / 360.0 * 255, 190 / 360.0 * 255); // purple
+        crf.setPin0DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 25); // 10mm or closer requirement
+        crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 110 / 360.0 * 255, 140 / 360.0 * 255); // green
+        crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 25); // 10mm or closer requirement
+    }
+}
+
+/**
+ * Helper class for configuring the Brushland Labs Color Rangefinder.
+ * Online documentation: <a href="https://docs.brushlandlabs.com">...</a>
+ */
+class ColorRangefinder {
+    private final I2cDeviceSynchSimple i2c;
+
+    public ColorRangefinder(RevColorSensorV3 emulator) {
+        this.i2c = emulator.getDeviceClient();
+        this.i2c.enableWriteCoalescing(true);
+    }
+
+    /**
+     * Configure Pin 0 to be in digital mode, and add a threshold.
+     * Multiple thresholds can be added to the same pin by calling this function repeatedly.
+     * For colors, bounds should be from 0-255, and for distance, bounds should be from 0-100 (mm).
+     */
+    public void setPin0Digital(DigitalMode digitalMode, double lowerBound, double higherBound) {
+        setDigital(PinNum.PIN0, digitalMode, lowerBound, higherBound);
+    }
+
+    /**
+     * Configure Pin 1 to be in digital mode, and add a threshold.
+     * Multiple thresholds can be added to the same pin by calling this function repeatedly.
+     * For colors, bounds should be from 0-255, and for distance, bounds should be from 0-100 (mm).
+     */
+    public void setPin1Digital(DigitalMode digitalMode, double lowerBound, double higherBound) {
+        setDigital(PinNum.PIN1, digitalMode, lowerBound, higherBound);
+    }
+
+    /**
+     * Sets the maximum distance (in millimeters) within which an object must be located for Pin 0's thresholds to trigger.
+     * This is most useful when we want to know if an object is both close and the correct color.
+     */
+    public void setPin0DigitalMaxDistance(DigitalMode digitalMode, double mmRequirement) {
+        setPin0Digital(digitalMode, mmRequirement, mmRequirement);
+    }
+
+    /**
+     * Sets the maximum distance (in millimeters) within which an object must be located for Pin 1's thresholds to trigger.
+     * This is most useful when we want to know if an object is both close and the correct color.
+     */
+    public void setPin1DigitalMaxDistance(DigitalMode digitalMode, double mmRequirement) {
+        setPin1Digital(digitalMode, mmRequirement, mmRequirement);
+    }
+
+    /**
+     * Invert the hue value before thresholding it, meaning that the colors become their opposite.
+     * This is useful if we want to threshold red; instead of having two thresholds we would invert
+     * the color and look for blue.
+     */
+    public void setPin0InvertHue() {
+        setPin0DigitalMaxDistance(DigitalMode.HSV, 200);
+    }
+
+    /**
+     * Invert the hue value before thresholding it, meaning that the colors become their opposite.
+     * This is useful if we want to threshold red; instead of having two thresholds we would invert
+     * the color and look for blue.
+     */
+    public void setPin1InvertHue() {
+        setPin1DigitalMaxDistance(DigitalMode.HSV, 200);
+    }
+
+    /**
+     * The denominator is what the raw sensor readings will be divided by before being scaled to 12-bit analog.
+     * For the full range of that channel, leave the denominator as 65535 for colors or 100 for distance.
+     * Smaller values will clip off higher ranges of the data in exchange for higher resolution within a lower range.
+     */
+    public void setPin0Analog(AnalogMode analogMode, int denominator) {
+        byte denom0 = (byte) (denominator & 0xFF);
+        byte denom1 = (byte) ((denominator & 0xFF00) >> 8);
+        i2c.write(PinNum.PIN0.modeAddress, new byte[]{analogMode.value, denom0, denom1});
+    }
+
+    /**
+     * Configure Pin 0 as analog output of one of the six data channels.
+     * To read analog, make sure the physical switch on the sensor is flipped away from the
+     * connector side.
+     */
+    public void setPin0Analog(AnalogMode analogMode) {
+        setPin0Analog(analogMode, analogMode == AnalogMode.DISTANCE ? 100 : 0xFFFF);
+    }
+
+    public float[] getCalibration() {
+        java.nio.ByteBuffer bytes =
+                java.nio.ByteBuffer.wrap(i2c.read(CALIB_A_VAL_0, 16)).order(java.nio.ByteOrder.LITTLE_ENDIAN);
+        return new float[]{bytes.getFloat(), bytes.getFloat(), bytes.getFloat(), bytes.getFloat()};
+    }
+
+    /**
+     * Save a brightness value of the LED to the sensor.
+     *
+     * @param value brightness between 0-255
+     */
+    public void setLedBrightness(int value) {
+        i2c.write8(LED_BRIGHTNESS, value);
+    }
+
+    /**
+     * Change the I2C address at which the sensor will be found. The address can be reset to the
+     * default of 0x52 by holding the reset button.
+     *
+     * @param value new I2C address from 1 to 127
+     */
+    public void setI2cAddress(int value) {
+        i2c.write8(I2C_ADDRESS_REG, value << 1);
+    }
+
+    /**
+     * Read distance via I2C
+     * @return distance in millimeters
+     */
+    public double readDistance() {
+        java.nio.ByteBuffer bytes =
+                java.nio.ByteBuffer.wrap(i2c.read(PS_DISTANCE_0, 4)).order(java.nio.ByteOrder.LITTLE_ENDIAN);
+        return bytes.getFloat();
+    }
+
+    private void setDigital(
+            PinNum pinNum,
+            DigitalMode digitalMode,
+            double lowerBound,
+            double higherBound
+    ) {
+        int lo, hi;
+        if (lowerBound == higherBound) {
+            lo = (int) lowerBound;
+            hi = (int) higherBound;
+        } else if (digitalMode.value <= DigitalMode.HSV.value) { // color value 0-255
+            lo = (int) Math.round(lowerBound / 255.0 * 65535);
+            hi = (int) Math.round(higherBound / 255.0 * 65535);
+        } else { // distance in mm
+            float[] calib = getCalibration();
+            if (lowerBound < .5) hi = 2048;
+            else hi = rawFromDistance(calib[0], calib[1], calib[2], calib[3], lowerBound);
+            lo = rawFromDistance(calib[0], calib[1], calib[2], calib[3], higherBound);
+        }
+
+        byte lo0 = (byte) (lo & 0xFF);
+        byte lo1 = (byte) ((lo & 0xFF00) >> 8);
+        byte hi0 = (byte) (hi & 0xFF);
+        byte hi1 = (byte) ((hi & 0xFF00) >> 8);
+        i2c.write(pinNum.modeAddress, new byte[]{digitalMode.value, lo0, lo1, hi0, hi1});
+        try {
+            Thread.sleep(25);
+        } catch (InterruptedException e) {
+            throw new RuntimeException(e);
+        }
+    }
+
+    private double root(double n, double v) {
+        double val = Math.pow(v, 1.0 / Math.abs(n));
+        if (n < 0) val = 1.0 / val;
+        return val;
+    }
+
+    private int rawFromDistance(float a, float b, float c, float x0, double mm) {
+        return (int) (root(b, (mm - c) / a) + x0);
+    }
+
+    private enum PinNum {
+        PIN0(0x28), PIN1(0x2D);
+
+        private final byte modeAddress;
+
+        PinNum(int modeAddress) {
+            this.modeAddress = (byte) modeAddress;
+        }
+    }
+
+    // other writeable registers
+    private static final byte CALIB_A_VAL_0 = 0x32;
+    private static final byte PS_DISTANCE_0 = 0x42;
+    private static final byte LED_BRIGHTNESS = 0x46;
+    private static final byte I2C_ADDRESS_REG = 0x47;
+
+    public static int invertHue(int hue360) {
+        return ((hue360 - 180) % 360);
+    }
+
+    public enum DigitalMode {
+        RED(1), BLUE(2), GREEN(3), ALPHA(4), HSV(5), DISTANCE(6);
+        public final byte value;
+
+        DigitalMode(int value) {
+            this.value = (byte) value;
+        }
+    }
+
+    public enum AnalogMode {
+        RED(13), BLUE(14), GREEN(15), ALPHA(16), HSV(17), DISTANCE(18);
+        public final byte value;
+
+        AnalogMode(int value) {
+            this.value = (byte) value;
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java

@@ -0,0 +1,104 @@
+package org.firstinspires.ftc.teamcode.utils;
+
+import com.acmerobotics.dashboard.config.Config;
+
+@Config
+public class Flywheel {
+    public static double kP = 0.001;           // small proportional gain (tune this)
+    public static double maxStep = 0.06;         // prevents sudden jumps
+    double initPos1 = 0.0;
+    double initPos2 = 0.0;
+    double stamp = 0.0;
+    double stamp1 = 0.0;
+    double ticker = 0.0;
+    double currentPos1 = 0.0;
+    double currentPos2 = 0.0;
+    double velo = 0.0;
+    double velo1 = 0.0;
+    double velo1a = 0.0;
+    double velo1b = 0.0;
+    double velo2 = 0.0;
+    double velo3 = 0.0;
+    double velo4 = 0.0;
+    double velo5 = 0.0;
+    double targetVelocity = 0.0;
+    double powPID = 0.0;
+    boolean steady = false;
+
+    public Flywheel() {
+        //robot = new Robot(hardwareMap);
+    }
+
+    public double getVelo(double shooter1CurPos, double shooter2CurPos) {
+        ticker++;
+        if (ticker % 2 == 0) {
+            velo5 = velo4;
+            velo4 = velo3;
+            velo3 = velo2;
+            velo2 = velo1;
+
+            currentPos1 = shooter1CurPos / 28;
+            currentPos2 = shooter2CurPos / 28;
+            stamp = getTimeSeconds(); //getRuntime();
+            velo1a = 60 * ((currentPos1 - initPos1) / (stamp - stamp1));
+            velo1b = 60 * ((currentPos2 - initPos2) / (stamp - stamp1));
+            initPos1 = currentPos1;
+            initPos2 = currentPos2;
+            stamp1 = stamp;
+
+            if (velo1a < 200){
+                velo1 = velo1b;
+            } else if (velo1b < 200){
+                velo1 = velo1a;
+            } else {
+                velo1 = (velo1a + velo1b) / 2;
+            }
+        }
+        return ((velo1 + velo2 + velo3 + velo4 + velo5) / 5);
+    }
+
+    public double getVelo1() { return (velo1a + velo2 + velo3 + velo4 + velo5) / 5; }
+
+    public double getVelo2() { return (velo1b + velo2 + velo3 + velo4 + velo5) / 5; }
+
+    public boolean getSteady() {
+        return steady;
+    }
+
+    private double getTimeSeconds() {
+        return (double) System.currentTimeMillis() / 1000.0;
+    }
+
+    public double manageFlywheel(int commandedVelocity, double shooter1CurPos, double shooter2CurPos) {
+        targetVelocity = commandedVelocity;
+        velo = getVelo(shooter1CurPos, shooter2CurPos);
+        // Flywheel PID code here
+        if (targetVelocity - velo > 500) {
+            powPID = 1.0;
+        } else if (velo - targetVelocity > 500) {
+            powPID = 0.0;
+        } else {
+            double feed = Math.log((668.39 / (targetVelocity + 591.96)) - 0.116) / -4.18;
+
+            // --- PROPORTIONAL CORRECTION ---
+            double error = targetVelocity - velo;
+            double correction = kP * error;
+
+            // limit how fast power changes (prevents oscillation)
+            correction = Math.max(-maxStep, Math.min(maxStep, correction));
+
+            // --- FINAL MOTOR POWER ---
+            powPID = feed + correction;
+
+            // clamp to allowed range
+            powPID = Math.max(0, Math.min(1, powPID));
+        }
+
+        steady = (Math.abs(targetVelocity - velo) < 100.0);
+
+        return powPID;
+    }
+
+    public void update() {
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java

@@ -1,14 +1,59 @@
 package org.firstinspires.ftc.teamcode.utils;
 
+import static org.firstinspires.ftc.teamcode.variables.HardwareConfig.*;
+
+import com.qualcomm.hardware.limelightvision.Limelight3A;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.hardware.Servo;
 
 public class Robot {
 
     //Initialize Public Components
 
+    public Limelight3A limelight3A;
+
+    public IMU imu;
+
+    public DcMotorEx shooter1;
+
+    public DcMotorEx shooter2;
+
+    public Servo hood;
+
     public Robot (HardwareMap hardwareMap) {
 
         //Define components w/ hardware map
 
+        limelight3A = hardwareMap.get(Limelight3A.class, "limelight");
+
+        shooter1 = hardwareMap.get(DcMotorEx.class, "s1");
+        shooter2 = hardwareMap.get(DcMotorEx.class, "s2");
+        shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
+        shooter2.setDirection(DcMotorSimple.Direction.REVERSE);
+        shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        hood = hardwareMap.get(Servo.class, "hood");
+
+        if (USING_LL) {
+            limelight3A = hardwareMap.get(Limelight3A.class, "limelight");
+            limelight3A.start(); // This tells Limelight to start looking!
+        }
+
+        imu = hardwareMap.get(IMU.class, "imu");
+
+        RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP;
+
+        RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
+
+        RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoFacingDirection, usbFacingDirection);
+
+        imu.initialize(new IMU.Parameters(orientationOnRobot));
+
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Subsystem.java

@@ -0,0 +1,8 @@
+package org.firstinspires.ftc.teamcode.utils;
+
+public interface Subsystem {
+
+    public void update();
+
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/subsystems/Limelight.java

@@ -0,0 +1,224 @@
+package org.firstinspires.ftc.teamcode.utils.subsystems;
+
+
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.LLResultTypes;
+import com.qualcomm.hardware.limelightvision.LLStatus;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
+import com.qualcomm.robotcore.hardware.IMU;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
+import org.firstinspires.ftc.robotcore.external.navigation.Position;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+import org.firstinspires.ftc.teamcode.variables.HardwareConfig;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+import org.firstinspires.ftc.teamcode.utils.Subsystem;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Objects;
+
+public class Limelight implements Subsystem {
+
+    private final Limelight3A limelight;
+    private final MultipleTelemetry telemetry;
+
+    private LLResult result;
+    private LLStatus status;
+
+    private boolean telemetryOn = false;
+    private String mode = "AT";
+
+    // ✅ Internal cached data
+    private Pose3D botpose;
+
+    private double captureLatency = 0.0;
+    private double targetingLatency = 0.0;
+    private double parseLatency = 0.0;
+    private double[] pythonOutput = new double[0];
+
+    private double tx = 0.0;
+    private double txnc = 0.0;
+    private double ty = 0.0;
+    private double tync = 0.0;
+
+    private double ta = 0.0;
+
+    private List<LLResultTypes.BarcodeResult> barcodeResults = new ArrayList<>();
+    private List<LLResultTypes.ClassifierResult> classifierResults = new ArrayList<>();
+    private List<LLResultTypes.DetectorResult> detectorResults = new ArrayList<>();
+    private List<LLResultTypes.FiducialResult> fiducialResults = new ArrayList<>();
+    private List<LLResultTypes.ColorResult> colorResults = new ArrayList<>();
+
+    private IMU imu;
+
+    public Limelight(Robot robot, MultipleTelemetry tele) {
+
+        HardwareConfig.USING_LL= true;
+
+        this.limelight = robot.limelight3A;
+        this.telemetry = tele;
+        limelight.pipelineSwitch(1);
+
+        this.imu = robot.imu;
+
+        this.imu.resetYaw();
+
+
+    }
+
+    public void setPipeline(int pipeline) {limelight.pipelineSwitch(pipeline);}
+
+    public void setTelemetryOn(boolean state) { telemetryOn = state; }
+
+    public void setMode(String newMode) { this.mode = newMode; }
+
+
+
+    /** ✅ MAIN UPDATE LOOP */
+    @Override
+    public void update() {
+
+
+
+        result = limelight.getLatestResult();
+        status = limelight.getStatus();
+
+
+
+        if (result != null && (Objects.equals(status.getPipelineType(), "pipe_python") || result.isValid())){
+            // Refresh all cached values
+            botpose         = result.getBotpose();
+            captureLatency  = result.getCaptureLatency();
+            targetingLatency= result.getTargetingLatency();
+            parseLatency    = result.getParseLatency();
+            pythonOutput    = result.getPythonOutput();
+            tx              = result.getTx();
+            txnc            = result.getTxNC();
+            ty              = result.getTy();
+            tync            = result.getTyNC();
+            ta              = result.getTa();
+            barcodeResults  = result.getBarcodeResults();
+            classifierResults = result.getClassifierResults();
+            detectorResults   = result.getDetectorResults();
+            fiducialResults   = result.getFiducialResults();
+            colorResults      = result.getColorResults();
+        }
+
+        if (telemetryOn) telemetryUpdate();
+    }
+
+    /** ✅ Telemetry Output */
+    private void telemetryUpdate() {
+// ✅ Use getters instead of directly accessing 'status' or cached fields
+        telemetry.addData("Name", "%s", getStatus().getName());
+        telemetry.addData("LL", "Temp: %.1fC, CPU: %.1f%%, FPS: %d",
+                getStatus().getTemp(),
+                getStatus().getCpu(),
+                (int) getStatus().getFps());
+        telemetry.addData("Pipeline", "Index: %d, Type: %s",
+                getStatus().getPipelineIndex(),
+                getStatus().getPipelineType());
+        telemetry.addData("ResultNull", result == null);
+        telemetry.addData("ResultValid", result.isValid());
+
+
+
+
+        if (result != null && result.isValid()) {
+            telemetry.addData("LL Latency", getTotalLatency());
+            telemetry.addData("Capture Latency", getCaptureLatency());
+            telemetry.addData("Targeting Latency", getTargetingLatency());
+            telemetry.addData("Parse Latency", getParseLatency());
+            telemetry.addData("PythonOutput", java.util.Arrays.toString(getPythonOutput()));
+            telemetry.addData("tx", getTx());
+            telemetry.addData("txnc", getTxNC());
+            telemetry.addData("ty", getTy());
+            telemetry.addData("tync", getTyNC());
+            telemetry.addData("ta", getTa());
+
+
+
+
+            telemetry.addData("BotX", getBotX());
+
+            telemetry.addData("BotY", getBotY());
+
+
+
+
+
+            if (Objects.equals(mode, "BR"))
+                for (LLResultTypes.BarcodeResult br : getBarcodeResults())
+                    telemetry.addData("Barcode", "Data: %s", br.getData());
+
+            if (Objects.equals(mode, "CL"))
+                for (LLResultTypes.ClassifierResult cr : getClassifierResults())
+                    telemetry.addData("Classifier", "Class: %s, Confidence: %.2f",
+                            cr.getClassName(), cr.getConfidence());
+
+            if (Objects.equals(mode, "DE"))
+                for (LLResultTypes.DetectorResult dr : getDetectorResults())
+                    telemetry.addData("Detector", "Class: %s, Area: %.2f",
+                            dr.getClassName(), dr.getTargetArea());
+
+            if (Objects.equals(mode, "FI"))
+                for (LLResultTypes.FiducialResult fr : getFiducialResults())
+                    telemetry.addData("Fiducial", "ID: %d, Family: %s, X: %.2f, Y: %.2f",
+                            fr.getFiducialId(), fr.getFamily(),
+                            fr.getTargetXDegrees(), fr.getTargetYDegrees());
+
+            if (Objects.equals(mode, "CO"))
+                for (LLResultTypes.ColorResult cr : getColorResults())
+                    telemetry.addData("Color", "X: %.2f, Y: %.2f",
+                            cr.getTargetXDegrees(), cr.getTargetYDegrees());
+        } else {
+            telemetry.addData("Limelight", "No data available");
+        }
+    }
+
+    // ✅ Getter methods (for use anywhere else in your code)
+
+    public Pose3D getBotPose() {
+        if (botpose == null) {
+            botpose = new Pose3D(
+                    new Position(),
+                    new YawPitchRollAngles(AngleUnit.DEGREES, 0.0, 0.0, 0.0, 0L)
+            );
+        }
+        return botpose;
+    }
+
+    public double getCaptureLatency() { return captureLatency; }
+    public double getTargetingLatency() { return targetingLatency; }
+    public double getTotalLatency() { return captureLatency + targetingLatency; }
+    public double getParseLatency() { return parseLatency; }
+    public double[] getPythonOutput() { return pythonOutput; }
+
+    public double getTx() { return tx; }
+    public double getTxNC() { return txnc; }
+    public double getTy() { return ty; }
+    public double getTyNC() { return tync;}
+
+    public double getTa() {return ta;}
+
+    public double getBotX() {return getBotPose().getPosition().x;}
+
+    public double getBotY() {return getBotPose().getPosition().y;}
+
+
+
+
+    public List<LLResultTypes.BarcodeResult> getBarcodeResults() { return barcodeResults; }
+    public List<LLResultTypes.ClassifierResult> getClassifierResults() { return classifierResults; }
+    public List<LLResultTypes.DetectorResult> getDetectorResults() { return detectorResults; }
+    public List<LLResultTypes.FiducialResult> getFiducialResults() { return fiducialResults; }
+    public List<LLResultTypes.ColorResult> getColorResults() { return colorResults; }
+
+    public LLStatus getStatus() { return status; }
+    public LLResult getRawResult() { return result; }
+
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/variables/HardwareConfig.java

@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.variables;
+
+
+import com.acmerobotics.dashboard.config.Config;
+
+@Config
+public class HardwareConfig {
+
+    public static boolean USING_LL = false;
+
+
+}