oops...hehe

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

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

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

@@ -0,0 +1,29 @@
+package org.firstinspires.ftc.teamcode.constants;
+
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.Pose2d;
+
+@Config
+public class Poses {
+
+    public static double goalHeight = 42; //in inches
+
+    public static double turretHeight = 12;
+
+    public static double relativeGoalHeight = goalHeight - turretHeight;
+
+    public static double x1 = 50, y1 = 0, h1 = 0;
+
+    public static double x2 = 31, y2 = 32, h2 = Math.toRadians(140);
+
+    public static double x2_b = 58, y2_b = 42, h2_b = Math.toRadians(140);
+
+
+    public static double x3 = 34, y3 = 58, h3 = Math.toRadians(140);
+
+    public static Pose2d teleStart = new Pose2d(x1,-10,0);
+
+
+
+}

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

@@ -0,0 +1,34 @@
+package org.firstinspires.ftc.teamcode.constants;
+
+
+import com.acmerobotics.dashboard.config.Config;
+
+@Config
+public class ServoPositions {
+
+    public static double spindexer_intakePos1 = 0.665;
+
+    public static double spindexer_intakePos3 = 0.29;
+
+    public static double spindexer_intakePos2 = 0.99;
+
+    public static double spindexer_outtakeBall3 = 0.845;
+
+    public static double spindexer_outtakeBall2 = 0.48;
+    public static double spindexer_outtakeBall1 = 0.1;
+
+    public static double transferServo_out = 0.15;
+
+    public static double transferServo_in = 0.38;
+
+    public static double hoodDefault = 0.35;
+
+    public static double hoodHigh = 0.21;
+
+    public static double hoodLow = 1.0;
+
+    public static double turret_red = 0.43;
+    public static double turret_blue = 0.4;
+
+
+}

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

@@ -0,0 +1,16 @@
+package org.firstinspires.ftc.teamcode.constants;
+
+import com.acmerobotics.dashboard.config.Config;
+
+@Config
+public class ShooterVars {
+    public static double turret_GearRatio = 0.9974;
+
+    public static double turret_Range = 355;
+
+    public static int velTolerance = 300;
+
+    public static int initTolerance = 1000;
+
+    public static int maxVel = 4000;
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java

@@ -1,89 +0,0 @@
-package org.firstinspires.ftc.teamcode.subsystems;
-
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.arcrobotics.ftclib.gamepad.GamepadEx;
-import com.arcrobotics.ftclib.gamepad.GamepadKeys;
-import com.qualcomm.robotcore.hardware.DcMotorEx;
-
-import org.firstinspires.ftc.teamcode.utils.Robot;
-
-import java.util.Objects;
-
-public class Drivetrain {
-
-    private final GamepadEx gamepad;
-    private final DcMotorEx fl;
-    private final DcMotorEx fr;
-    private final DcMotorEx bl;
-    private final DcMotorEx br;
-    public MultipleTelemetry TELE;
-    private String Mode = "Default";
-    private double defaultSpeed = 0.7;
-
-    private double slowSpeed = 0.3;
-
-    public Drivetrain(Robot robot, MultipleTelemetry tele, GamepadEx gamepad1) {
-
-        this.fl = robot.frontLeft;
-        this.fr = robot.frontRight;
-        this.br = robot.backRight;
-        this.bl = robot.backLeft;
-
-        this.gamepad = gamepad1;
-
-        this.TELE = tele;
-
-    }
-
-    public void setMode(String mode) {
-        this.Mode = mode;
-    }
-
-    public void setDefaultSpeed(double speed) {
-        this.defaultSpeed = speed;
-    }
-
-    public void setSlowSpeed(double speed) {
-        this.slowSpeed = speed;
-    }
-
-    public void RobotCentric(double fwd, double strafe, double turn, double turbo) {
-
-        double y = -fwd; // Remember, Y stick value is reversed
-        double x = strafe * 1.1; // Counteract imperfect strafing
-        double rx = turn;
-
-        // Denominator is the largest motor power (absolute value) or 1
-        // This ensures all the powers maintain the same ratio,
-        // but only if at least one is out of the range [-1, 1]
-        double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
-        double frontLeftPower = (y + x + rx) / denominator;
-        double backLeftPower = (y - x + rx) / denominator;
-        double frontRightPower = (y - x - rx) / denominator;
-        double backRightPower = (y + x - rx) / denominator;
-
-        fl.setPower(frontLeftPower * turbo);
-        bl.setPower(backLeftPower * turbo);
-        fr.setPower(frontRightPower * turbo);
-        br.setPower(backRightPower * turbo);
-
-    }
-
-    public void update() {
-
-        if (Objects.equals(Mode, "Default")) {
-
-            RobotCentric(
-                    gamepad.getRightY(),
-                    gamepad.getRightX(),
-                    gamepad.getLeftX(),
-                    (gamepad.getTrigger(
-                            GamepadKeys.Trigger.RIGHT_TRIGGER) * (1 - defaultSpeed)
-                            - gamepad.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) * slowSpeed
-                            + defaultSpeed
-                    )
-            );
-        }
-
-    }
-}

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

@@ -1,25 +0,0 @@
-package org.firstinspires.ftc.teamcode.subsystems;
-
-import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-
-import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
-
-public class Robot {
-
-    public DcMotorEx shooter1;
-    public DcMotorEx shooter2;
-    public WebcamName webcamName;
-
-    public Robot(HardwareMap hardwareMap) {
-
-        webcamName = hardwareMap.get(WebcamName.class, "Webcam 1");
-        shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1");
-        shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
-
-        shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
-
-    }
-
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java

@@ -0,0 +1,246 @@
+package org.firstinspires.ftc.teamcode.teleop;
+
+import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
+import static org.firstinspires.ftc.teamcode.tests.ShooterTest.kP;
+import static org.firstinspires.ftc.teamcode.tests.ShooterTest.maxStep;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+import java.util.ArrayList;
+import java.util.List;
+
+@TeleOp
+@Config
+public class TeleopV2 extends LinearOpMode {
+
+    public static double vel = 3000;
+    public static double hood = 0.5;
+    Robot robot;
+    MultipleTelemetry TELE;
+    boolean intake = false;
+    boolean reject = false;
+    int ticker = 0;
+    List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
+    List<Double> s1G = new ArrayList<>();
+    List<Double> s2G = new ArrayList<>();
+    List<Double> s3G = new ArrayList<>();
+    List<Boolean> s1 = new ArrayList<>();
+    List<Boolean> s2 = new ArrayList<>();
+    List<Boolean> s3 = new ArrayList<>();
+    double desiredTurretAngle = 180;
+    MecanumDrive drive = new MecanumDrive(hardwareMap, teleStart);
+    private double lastEncoderRevolutions = 0.0;
+    private double lastTimeStamp = 0.0;
+    private double velo1, velo;
+    private double stamp1, stamp, initPos;
+    private boolean shootAll = false;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        for (LynxModule hub : allHubs) {
+            hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+        }
+
+        robot = new Robot(hardwareMap);
+        TELE = new MultipleTelemetry(
+                telemetry, FtcDashboard.getInstance().getTelemetry()
+        );
+
+        waitForStart();
+        if (isStopRequested()) return;
+        while (opModeIsActive()) {
+
+            //DRIVETRAIN:
+
+            double y = -gamepad1.right_stick_y; // Remember, Y stick value is reversed
+            double x = gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing
+            double rx = gamepad1.left_stick_x;
+
+            double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
+            double frontLeftPower = (y + x + rx) / denominator;
+            double backLeftPower = (y - x + rx) / denominator;
+            double frontRightPower = (y - x - rx) / denominator;
+            double backRightPower = (y + x - rx) / denominator;
+
+            robot.frontLeft.setPower(frontLeftPower);
+            robot.backLeft.setPower(backLeftPower);
+            robot.frontRight.setPower(frontRightPower);
+            robot.backRight.setPower(backRightPower);
+
+            //INTAKE:
+
+            if (gamepad1.rightBumperWasPressed()) {
+                intake = true;
+            }
+
+            if (intake) {
+                robot.intake.setPower(1);
+
+                double position;
+
+                if ((getRuntime() % 0.3) > 0.15) {
+                    position = spindexer_intakePos1 + 0.015;
+                } else {
+                    position = spindexer_intakePos1 - 0.015;
+                }
+
+                robot.spin1.setPosition(position);
+                robot.spin2.setPosition(1 - position);
+
+            } else if (reject) {
+                robot.intake.setPower(-1);
+                double position = spindexer_intakePos1;
+                robot.spin1.setPosition(position);
+                robot.spin2.setPosition(1 - position);
+            } else {
+                robot.intake.setPower(0);
+            }
+
+            //COLOR:
+
+            double s1D = robot.color1.getDistance(DistanceUnit.MM);
+            double s2D = robot.color2.getDistance(DistanceUnit.MM);
+            double s3D = robot.color3.getDistance(DistanceUnit.MM);
+
+            if (s1D < 40) {
+
+                double green = robot.color1.getNormalizedColors().green;
+                double red = robot.color1.getNormalizedColors().red;
+                double blue = robot.color1.getNormalizedColors().blue;
+
+                double gP = green / (green + red + blue);
+
+                s1G.add(gP);
+
+                if (gP >= 0.43) {
+                    s1.add(true);
+                }
+            }
+
+            if (s2D < 40) {
+
+                double green = robot.color2.getNormalizedColors().green;
+                double red = robot.color2.getNormalizedColors().red;
+                double blue = robot.color2.getNormalizedColors().blue;
+
+                double gP = green / (green + red + blue);
+
+                s2G.add(gP);
+
+                if (gP >= 0.43) {
+                    s2.add(true);
+                }
+            }
+
+            if (s3D < 30) {
+
+                double green = robot.color3.getNormalizedColors().green;
+                double red = robot.color3.getNormalizedColors().red;
+                double blue = robot.color3.getNormalizedColors().blue;
+
+                double gP = green / (green + red + blue);
+
+                s3G.add(gP);
+
+                if (gP >= 0.43) {
+                    s3.add(true);
+                }
+            }
+
+            boolean green1 = s1.get(s1.size() - 1);
+            boolean green2 = s2.get(s2.size() - 1);
+            boolean green3 = s3.get(s3.size() - 1);
+
+            //SHOOTER:
+
+            double penguin = 0;
+
+            if (ticker % 8 == 0) {
+                penguin = (double) robot.shooterEncoder.getCurrentPosition() / 2048;
+                double stamp = getRuntime();
+                velo1 = -60 * ((penguin - initPos) / (stamp - stamp1));
+                initPos = penguin;
+                stamp1 = stamp;
+            }
+
+            velo = velo1;
+
+            double feed = vel / 4500;
+
+            if (vel > 500) {
+                feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
+            }
+
+            // --- PROPORTIONAL CORRECTION ---
+            double error = vel - velo1;
+            double correction = kP * error;
+
+            // limit how fast power changes (prevents oscillation)
+            correction = Math.max(-maxStep, Math.min(maxStep, correction));
+
+            // --- FINAL MOTOR POWER ---
+            double powPID = feed + correction;
+
+            // clamp to allowed range
+            powPID = Math.max(0, Math.min(1, powPID));
+
+            if (vel - velo > 1000) {
+                powPID = 1;
+            } else if (velo - vel > 1000) {
+                powPID = 0;
+            }
+
+            robot.shooter1.setPower(powPID);
+            robot.shooter2.setPower(powPID);
+
+            robot.hood.setPosition(hood);
+
+            //TODO: ADD CODE TO CHANGE VARIABLE VEL BASED ON POSITION
+
+            //TODO: ADD CODE TO CHANGE VARIABLE HOOD ANGLE BASED ON POSITION
+
+            //SHOOT ALL:
+
+            if (gamepad2.rightBumperWasPressed()) {
+                shootAll = true;
+            }
+
+            if (shootAll) {
+                intake = false;
+                reject = false;
+
+            }
+
+            //MISC:
+
+            drive.updatePoseEstimate();
+
+            for (LynxModule hub : allHubs) {
+                hub.clearBulkCache();
+            }
+
+            TELE.addData("Spin1Green", s1.get(s1.size() - 1));
+            TELE.addData("Spin2Green", s2.get(s2.size() - 1));
+            TELE.addData("Spin3Green", s3.get(s3.size() - 1));
+
+            TELE.addData("pose", drive.localizer.getPose());
+            TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
+
+            TELE.update();
+
+            ticker++;
+
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old.txt

@@ -0,0 +1,793 @@
+package org.firstinspires.ftc.teamcode.teleop;
+
+import static org.firstinspires.ftc.teamcode.constants.Poses.*;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+import com.arcrobotics.ftclib.gamepad.ToggleButtonReader;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
+import org.firstinspires.ftc.teamcode.subsystems.Intake;
+import org.firstinspires.ftc.teamcode.subsystems.Shooter;
+import org.firstinspires.ftc.teamcode.subsystems.Spindexer;
+import org.firstinspires.ftc.teamcode.subsystems.Transfer;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+@Config
+@TeleOp
+@Disabled
+public class old extends LinearOpMode {
+
+    Robot robot;
+
+    Drivetrain drivetrain;
+
+    Intake intake;
+
+    Spindexer spindexer;
+
+    Transfer transfer;
+
+    MultipleTelemetry TELE;
+
+    GamepadEx g1;
+
+    GamepadEx g2;
+
+    public static double defaultSpeed = 1;
+
+    public static double slowMoSpeed = 0.4;
+
+    public static double power = 0.0;
+
+    public static double pos = hoodDefault;
+
+    public boolean all = false;
+
+    public int ticker = 0;
+
+    ToggleButtonReader g1RightBumper;
+
+    ToggleButtonReader g2Circle;
+
+    ToggleButtonReader g2Square;
+
+    ToggleButtonReader g2Triangle;
+
+    ToggleButtonReader g2RightBumper;
+
+    ToggleButtonReader g1LeftBumper;
+
+    ToggleButtonReader g2LeftBumper;
+
+    ToggleButtonReader g2DpadUp;
+
+    ToggleButtonReader g2DpadDown;
+
+    ToggleButtonReader g2DpadRight;
+
+    ToggleButtonReader g2DpadLeft;
+
+    public boolean leftBumper = false;
+    public double g1RightBumperStamp = 0.0;
+
+    public double g1LeftBumperStamp = 0.0;
+
+    public double g2LeftBumperStamp = 0.0;
+
+    public static int spindexerPos = 0;
+
+    public boolean green = false;
+
+    Shooter shooter;
+
+    public boolean scoreAll = false;
+
+    MecanumDrive drive;
+
+    public boolean autotrack = false;
+
+    public int last = 0;
+    public int second = 0;
+
+    public double offset = 0.0;
+
+    public static double rIn = 0.59;
+
+    public static double rOut = 0;
+
+    public boolean notShooting = true;
+
+    public boolean circle = false;
+
+    public boolean square = false;
+
+    public boolean tri = false;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        drive = new MecanumDrive(hardwareMap, teleStart);
+
+        robot = new Robot(hardwareMap);
+
+        TELE = new MultipleTelemetry(
+                FtcDashboard.getInstance().getTelemetry(),
+                telemetry
+        );
+
+        g1 = new GamepadEx(gamepad1);
+
+        g1RightBumper = new ToggleButtonReader(
+                g1, GamepadKeys.Button.RIGHT_BUMPER
+        );
+
+        g2 = new GamepadEx(gamepad2);
+
+        g1LeftBumper = new ToggleButtonReader(
+                g1, GamepadKeys.Button.LEFT_BUMPER
+        );
+
+        g2Circle = new ToggleButtonReader(
+                g2, GamepadKeys.Button.B
+        );
+
+        g2Triangle = new ToggleButtonReader(
+                g2, GamepadKeys.Button.Y
+        );
+
+        g2Square = new ToggleButtonReader(
+                g2, GamepadKeys.Button.X
+        );
+
+        g2RightBumper = new ToggleButtonReader(
+                g2, GamepadKeys.Button.RIGHT_BUMPER
+        );
+
+        g2LeftBumper = new ToggleButtonReader(
+                g2, GamepadKeys.Button.LEFT_BUMPER
+        );
+
+        g2DpadUp = new ToggleButtonReader(
+                g2, GamepadKeys.Button.DPAD_UP
+        );
+
+        g2DpadDown = new ToggleButtonReader(
+                g2, GamepadKeys.Button.DPAD_DOWN
+        );
+
+        g2DpadLeft = new ToggleButtonReader(
+                g2, GamepadKeys.Button.DPAD_LEFT
+        );
+
+        g2DpadRight = new ToggleButtonReader(
+                g2, GamepadKeys.Button.DPAD_RIGHT
+        );
+
+        drivetrain = new Drivetrain(robot, TELE, g1);
+
+        drivetrain.setMode("Default");
+
+        drivetrain.setDefaultSpeed(defaultSpeed);
+
+        drivetrain.setSlowSpeed(slowMoSpeed);
+
+        intake = new Intake(robot);
+
+        transfer = new Transfer(robot);
+
+        spindexer = new Spindexer(robot, TELE);
+
+        spindexer.setTelemetryOn(true);
+
+        shooter = new Shooter(robot, TELE);
+
+        shooter.setShooterMode("MANUAL");
+
+        robot.rejecter.setPosition(rIn);
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        drive = new MecanumDrive(hardwareMap, teleStart);
+
+        while (opModeIsActive()) {
+
+            drive.updatePoseEstimate();
+
+            TELE.addData("pose", drive.localizer.getPose());
+
+            TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
+
+            TELE.addData("off", offset);
+
+            robot.hood.setPosition(pos);
+
+            g1LeftBumper.readValue();
+
+            if (g1LeftBumper.wasJustPressed()) {
+                g2LeftBumperStamp = getRuntime();
+
+                spindexer.intakeShake(getRuntime());
+
+                leftBumper = true;
+            }
+
+            if (leftBumper) {
+                double time = getRuntime() - g2LeftBumperStamp;
+
+                if (time < 1.0) {
+                    robot.rejecter.setPosition(rOut);
+                } else {
+                    robot.rejecter.setPosition(rIn);
+                }
+
+            }
+
+            intake();
+
+            drivetrain.update();
+
+            TELE.update();
+
+            transfer.update();
+
+            g2RightBumper.readValue();
+
+            g2LeftBumper.readValue();
+
+            g2DpadDown.readValue();
+
+            g2DpadUp.readValue();
+
+            if (!scoreAll) {
+                spindexer.checkForBalls();
+            }
+
+            if (g2DpadUp.wasJustPressed()) {
+                pos -= 0.02;
+            }
+
+            if (g2DpadDown.wasJustPressed()) {
+                pos += 0.02;
+            }
+
+            g2DpadLeft.readValue();
+
+            g2DpadRight.readValue();
+
+            if (g2DpadLeft.wasJustPressed()) {
+                offset -= 0.02;
+            }
+
+            if (g2DpadRight.wasJustPressed()) {
+                offset += 0.02;
+            }
+
+            TELE.addData("hood", pos);
+
+            if (Math.abs(gamepad2.right_stick_x) < 0.1 && autotrack) {
+
+                shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset);
+
+            } else {
+
+                autotrack = false;
+
+                shooter.moveTurret(0.3 + offset);
+
+            }
+
+            if (gamepad2.right_stick_button) {
+                autotrack = true;
+            }
+
+            if (g2RightBumper.wasJustPressed()) {
+                transfer.setTransferPower(1);
+                transfer.transferIn();
+                shooter.setManualPower(1);
+
+                notShooting = false;
+
+            }
+
+            if (g2RightBumper.wasJustReleased()) {
+                transfer.setTransferPower(1);
+                transfer.transferOut();
+            }
+
+            if (gamepad2.left_stick_y > 0.5) {
+
+                shooter.setManualPower(0);
+            } else if (gamepad2.left_stick_y < -0.5) {
+                shooter.setManualPower(1);
+            }
+
+            if (g2LeftBumper.wasJustPressed()) {
+                g2LeftBumperStamp = getRuntime();
+                notShooting = false;
+                scoreAll = true;
+            }
+
+            if (scoreAll) {
+                double time = getRuntime() - g2LeftBumperStamp;
+
+                shooter.setManualPower(1);
+
+                TELE.addData("greenImportant", green);
+
+                TELE.addData("last", last);
+                TELE.addData("2ndLast", second);
+
+                int numGreen = spindexer.greens();
+
+                if (square) {
+
+                    if (time < 0.3) {
+
+                        ticker = 0;
+
+                        last = 0;
+                        second = 0;
+
+                        transfer.transferOut();
+                        transfer.setTransferPower(1);
+                    } else if (time < 2) {
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakePurple(second, last);
+                                second = last;
+                            } else {
+                                last = spindexer.outtakeGreen(second, last);
+                                second = last;
+
+                            }
+                        }
+
+                        second = last;
+
+                        ticker++;
+
+                    } else if (time < 2.5) {
+
+                        ticker = 0;
+
+                        second = last;
+
+                        transfer.transferIn();
+                    } else if (time < 4) {
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakeGreen(second, last);
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+                    } else if (time < 4.5) {
+
+                        ticker = 0;
+
+                        transfer.transferIn();
+                    } else if (time < 6) {
+
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakeGreen(second, last);
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+
+                    } else if (time < 6.5) {
+                        transfer.transferIn();
+                    } else {
+
+                        ticker = 0;
+
+                        scoreAll = false;
+                        transfer.transferOut();
+
+                        shooter.setManualPower(0);
+
+                    }
+                } else if (tri) {
+
+                    if (time < 0.3) {
+
+                        ticker = 0;
+
+                        last = 0;
+                        second = 0;
+
+                        transfer.transferOut();
+                        transfer.setTransferPower(1);
+                    } else if (time < 2) {
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakeGreen(second, last);
+                                second = last;
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+                                second = last;
+
+                            }
+                        }
+
+                        second = last;
+
+                        ticker++;
+
+                    } else if (time < 2.5) {
+
+                        ticker = 0;
+
+                        second = last;
+
+                        transfer.transferIn();
+                    } else if (time < 4) {
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakePurple(second, last);
+                            } else {
+                                last = spindexer.outtakeGreen(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+                    } else if (time < 4.5) {
+
+                        ticker = 0;
+
+                        transfer.transferIn();
+                    } else if (time < 6) {
+
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakeGreen(second, last);
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+
+                    } else if (time < 6.5) {
+                        transfer.transferIn();
+                    } else {
+
+                        ticker = 0;
+
+                        scoreAll = false;
+                        transfer.transferOut();
+
+                        shooter.setManualPower(0);
+
+                    }
+                } else if (circle) {
+
+                    if (time < 0.3) {
+
+                        ticker = 0;
+
+                        last = 0;
+                        second = 0;
+
+                        transfer.transferOut();
+                        transfer.setTransferPower(1);
+                    } else if (time < 2) {
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakeGreen(second, last);
+                                second = last;
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+                                second = last;
+
+                            }
+                        }
+
+                        second = last;
+
+                        ticker++;
+
+                    } else if (time < 2.5) {
+
+                        ticker = 0;
+
+                        second = last;
+
+                        transfer.transferIn();
+                    } else if (time < 4) {
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakeGreen(second, last);
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+                    } else if (time < 4.5) {
+
+                        ticker = 0;
+
+                        transfer.transferIn();
+                    } else if (time < 6) {
+
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakePurple(second, last);
+                            } else {
+                                last = spindexer.outtakeGreen(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+
+                    } else if (time < 6.5) {
+                        transfer.transferIn();
+                    } else {
+
+                        ticker = 0;
+
+                        scoreAll = false;
+                        transfer.transferOut();
+
+                        shooter.setManualPower(0);
+
+                    }
+                } else {
+
+                    if (time < 0.3) {
+
+                        ticker = 0;
+
+                        last = 0;
+                        second = 0;
+
+                        if (gamepad2.right_trigger > 0.5) {
+                            green = false;
+
+                            all = gamepad2.left_trigger > 0.5;
+
+                        } else if (gamepad2.left_trigger > 0.5) {
+                            green = true;
+
+                            all = false;
+                        } else {
+                            all = true;
+                        }
+
+                        transfer.transferOut();
+                        transfer.setTransferPower(1);
+                    } else if (time < 2) {
+
+                        if (ticker == 0) {
+
+                            if (all) {
+                                spindexer.outtake3();
+                                last = 3;
+                                second = 3;
+                            } else if (green) {
+                                last = spindexer.outtakeGreen(second, last);
+                                second = last;
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+                                second = last;
+
+                            }
+                        }
+
+                        second = last;
+
+                        ticker++;
+
+                    } else if (time < 2.5) {
+
+                        ticker = 0;
+
+                        second = last;
+
+                        if (gamepad2.right_trigger > 0.5) {
+                            green = false;
+
+                            all = gamepad2.left_trigger > 0.5;
+
+                        } else if (gamepad2.left_trigger > 0.5) {
+                            green = true;
+
+                            all = false;
+
+                        }
+
+                        transfer.transferIn();
+                    } else if (time < 4) {
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (all) {
+                                spindexer.outtake2();
+
+                                last = 2;
+                            } else if (green) {
+                                last = spindexer.outtakeGreen(second, last);
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+                    } else if (time < 4.5) {
+
+                        ticker = 0;
+
+                        if (gamepad2.right_trigger > 0.5) {
+                            green = false;
+
+                            all = gamepad2.left_trigger > 0.5;
+
+                        } else if (gamepad2.left_trigger > 0.5) {
+                            green = true;
+
+                            all = false;
+                        }
+
+                        transfer.transferIn();
+                    } else if (time < 6) {
+
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (all) {
+                                spindexer.outtake1();
+                            } else if (green) {
+                                last = spindexer.outtakeGreen(second, last);
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+
+                    } else if (time < 6.5) {
+                        transfer.transferIn();
+                    } else {
+
+                        ticker = 0;
+
+                        scoreAll = false;
+                        transfer.transferOut();
+
+                        shooter.setManualPower(0);
+
+                    }
+
+                }
+            }
+
+            shooter.update();
+
+        }
+
+    }
+
+    public void intake() {
+
+        g1RightBumper.readValue();
+
+        g2Circle.readValue();
+
+        g2Square.readValue();
+
+        g2Triangle.readValue();
+
+        if (g1RightBumper.wasJustPressed()) {
+
+            notShooting = true;
+
+            if (getRuntime() - g1RightBumperStamp < 0.3) {
+                intake.reverse();
+            } else {
+                intake.toggle();
+            }
+
+            if (intake.getIntakeState() == 1) {
+                shooter.setManualPower(0);
+            }
+
+            spindexer.intake();
+
+            transfer.transferOut();
+
+            g1RightBumperStamp = getRuntime();
+
+        }
+
+        if (intake.getIntakeState() == 1 && notShooting) {
+
+            spindexer.intakeShake(getRuntime());
+
+        } else {
+            if (g2Circle.wasJustPressed()) {
+                circle = true;
+                tri = false;
+                square = false;
+
+            }
+
+            if (g2Triangle.wasJustPressed()) {
+                circle = false;
+                tri = true;
+                square = false;
+            }
+
+            if (g2Square.wasJustPressed()) {
+                circle = false;
+                tri = false;
+                square = true;
+            }
+
+            if (gamepad2.x) {
+                circle = false;
+                tri = false;
+                square = false;
+            }
+
+        }
+
+        intake.update();
+
+        spindexer.update();
+
+    }
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.txt

@@ -0,0 +1,144 @@
+package org.firstinspires.ftc.teamcode.tests;
+
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
+
+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 org.firstinspires.ftc.teamcode.utils.Robot;
+
+
+@TeleOp
+@Config
+public class ActiveColorSensorTest extends LinearOpMode {
+    Robot robot;
+    MultipleTelemetry TELE;
+
+    @Override
+    public void runOpMode() throws InterruptedException{
+        robot = new Robot(hardwareMap);
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        int b1Purple = 1;
+        int b1Total = 1;
+        int b2Purple = 1;
+        int b2Total = 1;
+        int b3Purple = 1;
+        int b3Total = 1;
+
+        double totalStamp1 = 0.0;
+        double purpleStamp1 = 0.0;
+        double totalStamp2 = 0.0;
+        double purpleStamp2 = 0.0;
+        double totalStamp3 = 0.0;
+        double purpleStamp3 = 0.0;
+
+        String b1 = "none";
+        String b2 = "none";
+        String b3 = "none";
+
+        double position = 0.0;
+
+        double stamp = getRuntime();
+
+        waitForStart();
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()){
+
+            if ((getRuntime() % 0.3) >0.15) {
+                position = spindexer_intakePos1 + 0.015;
+            } else {
+                position = spindexer_intakePos1 - 0.015;
+            }
+            robot.spin1.setPosition(position);
+            robot.spin2.setPosition(1-position);
+
+            robot.intake.setPower(1);
+
+            // Reset the counters after 1 second of not reading a ball.
+            final double ColorCounterResetDelay = 1.0;
+            // Number of times the loop needs to run before deciding on a color.
+            final int ColorCounterTotalMinCount = 20;
+            // If the color sensor reads a color this percentage of time
+            // out of the total, declare the color.
+            // Usage: (Color Count)/(Total Count) > ColorCounterThreshold
+            final double ColorCounterThreshold  = 0.65;
+
+            if (robot.pin1.getState()){
+                if (robot.pin0.getState()){
+                    b1Purple ++;
+                }
+                b1Total++;
+                totalStamp1 = getRuntime();
+            }
+            if (getRuntime() - totalStamp1 > ColorCounterResetDelay) {
+                // Too Much time has passed without detecting ball
+                b1 = "none";
+                b1Total = 1;
+                b1Purple = 1;
+            }else if ((b1Total > ColorCounterTotalMinCount) && ((double) b1Purple / b1Total) >= ColorCounterThreshold){
+            // Enough Time has passed and we met the threshold
+                b1 = "Purple";
+            }else if (b1Total > ColorCounterTotalMinCount) {
+            // Enough Time passed WITHOUT meeting the threshold
+                b1 = "Green";
+            }
+
+            if (robot.pin3.getState()){
+                if (robot.pin2.getState()){
+                    b2Purple ++;
+                }
+                b2Total++;
+                totalStamp2 = getRuntime();
+            }
+            if (getRuntime() - totalStamp2 > ColorCounterResetDelay) {
+                // Too Much time has passed without detecting ball
+                b2 = "none";
+                b2Total = 1;
+                b2Purple = 1;
+            }else if ((b2Total > ColorCounterTotalMinCount) && ((double) b2Purple / b2Total) >= ColorCounterThreshold){
+                // Enough Time has passed and we met the threshold
+                b2 = "Purple";
+            }else if (b2Total > ColorCounterTotalMinCount) {
+                // Enough Time passed WITHOUT meeting the threshold
+                b2 = "Green";
+            }
+
+            if (robot.pin5.getState()){
+                if (robot.pin4.getState()){
+                    b3Purple ++;
+                }
+                b3Total++;
+                totalStamp3 = getRuntime();
+            }
+            if (getRuntime() - totalStamp3 > ColorCounterResetDelay) {
+                // Too Much time has passed without detecting ball
+                b3 = "none";
+                b3Total = 1;
+                b3Purple = 1;
+            }else if ((b3Total > ColorCounterTotalMinCount) && ((double) b3Purple / b3Total) >= ColorCounterThreshold){
+                // Enough Time has passed and we met the threshold
+                b3 = "Purple";
+            }else if (b3Total > ColorCounterTotalMinCount) {
+                // Enough Time passed WITHOUT meeting the threshold
+                b3 = "Green";
+            }
+
+            TELE.addData("Green1:", robot.pin1.getState());
+            TELE.addData("Purple1:", robot.pin0.getState());
+            TELE.addData("Green2:", robot.pin3.getState());
+            TELE.addData("Purple2:", robot.pin2.getState());
+            TELE.addData("Green3:", robot.pin5.getState());
+            TELE.addData("Purple3:", robot.pin4.getState());
+            TELE.addData("1", b1);
+            TELE.addData("2",b2);
+            TELE.addData("3",b3);
+
+            TELE.update();
+        }
+    }
+
+}

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

@@ -6,8 +6,8 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
-import org.firstinspires.ftc.teamcode.subsystems.AprilTagWebcam;
-import org.firstinspires.ftc.teamcode.subsystems.Robot;
+import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
+import org.firstinspires.ftc.teamcode.utils.Robot;
 
 @Config
 @TeleOp

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.txt

@@ -0,0 +1,63 @@
+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 org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+@TeleOp
+@Config
+public class ColorSensorTest extends LinearOpMode {
+    Robot robot;
+    MultipleTelemetry TELE;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        robot = new Robot(hardwareMap);
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        waitForStart();
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()) {
+
+// ----- COLOR 1 -----
+            double green1 = robot.color1.getNormalizedColors().green;
+            double blue1  = robot.color1.getNormalizedColors().blue;
+            double red1   = robot.color1.getNormalizedColors().red;
+
+            TELE.addData("Color1 toColor", robot.color1.getNormalizedColors().toColor());
+            TELE.addData("Color1 green", green1 / (green1 + blue1 + red1));
+            TELE.addData("Color1 distance (mm)", robot.color1.getDistance(DistanceUnit.MM));
+
+
+// ----- COLOR 2 -----
+            double green2 = robot.color2.getNormalizedColors().green;
+            double blue2  = robot.color2.getNormalizedColors().blue;
+            double red2   = robot.color2.getNormalizedColors().red;
+
+            TELE.addData("Color2 toColor", robot.color2.getNormalizedColors().toColor());
+            TELE.addData("Color2 green", green2 / (green2 + blue2 + red2));
+            TELE.addData("Color2 distance (mm)", robot.color2.getDistance(DistanceUnit.MM));
+
+
+// ----- COLOR 3 -----
+            double green3 = robot.color3.getNormalizedColors().green;
+            double blue3  = robot.color3.getNormalizedColors().blue;
+            double red3   = robot.color3.getNormalizedColors().red;
+
+            TELE.addData("Color3 toColor", robot.color3.getNormalizedColors().toColor());
+            TELE.addData("Color3 green", green3 / (green3 + blue3 + red3));
+            TELE.addData("Color3 distance (mm)", robot.color3.getDistance(DistanceUnit.MM));
+
+
+            TELE.update();
+        }
+    }
+
+
+}

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

@@ -1,16 +1,13 @@
 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.arcrobotics.ftclib.controller.PIDController;
-import com.arcrobotics.ftclib.controller.PIDFController;
 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.subsystems.Robot;
+import org.firstinspires.ftc.teamcode.utils.Robot;
 
 @Config
 @TeleOp
@@ -18,35 +15,32 @@ public class ShooterTest extends LinearOpMode {
 
     public static int mode = 0;
     public static double parameter = 0.0;
-    Robot robot;
-    private DcMotorEx leftShooter;
-    private DcMotorEx rightShooter;
-    private DcMotorEx encoder;
-    private double encoderRevolutions = 0.0;
-    private double lastEncoderRevolutions = 0.0;
-    private double timeStamp = 0.0;
-    private double lastTimeStamp = 0.0;
-
-
     // --- CONSTANTS YOU TUNE ---
     public static double MAX_RPM = 2500;      // your measured max RPM
     public static double kP = 0.01;           // small proportional gain (tune this)
     public static double maxStep = 0.2;         // prevents sudden jumps
 
+    //TODO: @Daniel FIX THE BELOW CONSTANTS A LITTLE IF NEEDED
+    public static double transferPower = 0.0;
+    public static double hoodPos = 0.501;
+
+    public static double turretPos = 0.501;
+    Robot robot;
+    private double lastEncoderRevolutions = 0.0;
+    private double lastTimeStamp = 0.0;
+
     @Override
     public void runOpMode() throws InterruptedException {
 
         robot = new Robot(hardwareMap);
-        leftShooter = robot.shooter1;
-        rightShooter = robot.shooter2;
-        encoder = robot.shooter1;
+        DcMotorEx leftShooter = robot.shooter1;
+        DcMotorEx rightShooter = robot.shooter2;
+        DcMotorEx encoder = robot.shooter1;
 
         MultipleTelemetry TELE = new MultipleTelemetry(
                 telemetry, FtcDashboard.getInstance().getTelemetry()
         );
 
-
-
         waitForStart();
 
         if (isStopRequested()) return;
@@ -55,29 +49,24 @@ public class ShooterTest extends LinearOpMode {
 
             double kF = 1.0 / MAX_RPM;     // baseline feedforward
 
+            double encoderRevolutions = (double) encoder.getCurrentPosition() / 2048;
 
-
-
-            encoderRevolutions = (double) encoder.getCurrentPosition() / 2048;
-
-            double velocity = -60*(encoderRevolutions - lastEncoderRevolutions) / (getRuntime() - lastTimeStamp);
+            double velocity = -60 * (encoderRevolutions - lastEncoderRevolutions) / (getRuntime() - lastTimeStamp);
 
             TELE.addLine("Mode: 0 = Manual, 1 = Vel, 2 = Pos");
             TELE.addLine("Parameter = pow, vel, or pos");
-            TELE.addData("leftShooterPower",  leftShooter.getPower());
+            TELE.addData("leftShooterPower", leftShooter.getPower());
             TELE.addData("rightShooterPower", rightShooter.getPower());
             TELE.addData("shaftEncoderPos", encoderRevolutions);
             TELE.addData("shaftEncoderVel", velocity);
 
-            double velPID = 0.0;
+            double velPID;
 
             if (mode == 0) {
                 rightShooter.setPower(parameter);
                 leftShooter.setPower(parameter);
             } else if (mode == 1) {
 
-
-
                 // --- FEEDFORWARD BASE POWER ---
                 double feed = kF * parameter;        // Example: vel=2500 → feed=0.5
 
@@ -102,12 +91,19 @@ public class ShooterTest extends LinearOpMode {
             lastTimeStamp = getRuntime();
             lastEncoderRevolutions = (double) encoder.getCurrentPosition() / 2048;
 
+            if (hoodPos != 0.501) {
+                robot.hood.setPosition(hoodPos);
+            }
 
+            if (turretPos!=0.501){
+                robot.turr1.setPosition(turretPos);
+                robot.turr2.setPosition(turretPos);
+            }
+
+            robot.transfer.setPower(transferPower);
 
             TELE.update();
 
-
-
         }
 
     }

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

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

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ed.txt

@@ -0,0 +1,210 @@
+package org.firstinspires.ftc.teamcode.tests;
+
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
+
+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.DcMotor;
+
+import org.firstinspires.ftc.teamcode.utils.Robot;
+import org.firstinspires.ftc.teamcode.subsystems.Shooter;
+
+@TeleOp
+@Config
+public class ShooterTest extends LinearOpMode {
+
+    Robot robot;
+
+    public static double pow = 0.0;
+    public static double vel = 0.0;
+    public static double ecpr = 1024.0; // CPR of the encoder
+    public static double hoodPos = 0.5;
+    public static double turretPos = 0.9;
+
+    public static String flyMode = "VEL";
+
+    public static boolean AutoTrack = false;
+
+    double initPos = 0.0;
+
+    double velo = 0.0;
+    double velo1 = 0.0;
+    double velo2 = 0.0;
+    double velo3 = 0.0;
+    double velo4 = 0.0;
+    double velo5 = 0.0;
+
+    double stamp1 = 0.0;
+
+    double initPos1 = 0.0;
+
+    double powPID = 0.0;
+
+    public static int maxVel = 4500;
+
+    public static boolean shoot = false;
+
+    public static int spindexPos = 1;
+
+    public static boolean intake = true;
+
+    public static int tolerance = 50;
+
+    double stamp = 0.0;
+
+    public static double kP = 0.001;           // small proportional gain (tune this)
+    public static double maxStep = 0.06;         // prevents sudden jumps
+    public static double distance = 50;
+
+    MultipleTelemetry TELE;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        robot = new Robot(hardwareMap);
+
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        Shooter shooter = new Shooter(robot, TELE);
+
+        robot.shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        shooter.setTelemetryOn(true);
+
+        shooter.setShooterMode(flyMode);
+
+        initPos = shooter.getECPRPosition();
+
+        int ticker = 0;
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()) {
+
+            ticker++;
+
+            if (AutoTrack){
+                hoodPos = hoodAnglePrediction(distance);
+                vel = velPrediction(distance);
+            }
+
+
+
+            shooter.setShooterMode(flyMode);
+
+            shooter.setManualPower(pow);
+
+            robot.hood.setPosition(hoodPos);
+            robot.turr1.setPosition(turretPos);
+            robot.turr2.setPosition(1 - turretPos);
+            if (intake) {
+                robot.transfer.setPower(0);
+                robot.intake.setPower(0.75);
+                robot.spin1.setPosition(spindexer_intakePos1);
+                robot.spin2.setPosition(1 - spindexer_intakePos1);
+            } else {
+                robot.transfer.setPower(.75 + (powPID/4));
+                robot.intake.setPower(0);
+                if (spindexPos == 1) {
+                    robot.spin1.setPosition(spindexer_outtakeBall1);
+                    robot.spin2.setPosition(1 - spindexer_outtakeBall1);
+                } else if (spindexPos == 2) {
+                    robot.spin1.setPosition(spindexer_outtakeBall2);
+                    robot.spin2.setPosition(1 - spindexer_outtakeBall2);
+                } else if (spindexPos == 3) {
+                    robot.spin1.setPosition(spindexer_outtakeBall3);
+                    robot.spin2.setPosition(1 - spindexer_outtakeBall3);
+                }
+            }
+
+            double penguin = 0;
+            if (ticker % 8 ==0){
+                penguin = shooter.getECPRPosition();
+                stamp = getRuntime();
+                velo1 = -60 * ((penguin - initPos1) / (stamp - stamp1));
+                initPos1 = penguin;
+                stamp1 = stamp;
+            }
+
+
+            velo = velo1;
+
+            double feed = vel / maxVel;        // Example: vel=2500 → feed=0.5
+
+            if (vel > 500){
+                feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
+            }
+
+            // --- PROPORTIONAL CORRECTION ---
+            double error = vel - velo1;
+            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));
+
+            if (vel - velo > 1000){
+                powPID = 1;
+            } else if (velo - vel > 1000){
+                powPID = 0;
+            }
+
+            shooter.setVelocity(powPID);
+
+            if (shoot) {
+                robot.transferServo.setPosition(transferServo_in);
+            } else {
+                robot.transferServo.setPosition(transferServo_out);
+            }
+
+            shooter.update();
+
+            TELE.addData("Revolutions", shooter.getECPRPosition());
+            TELE.addData("hoodPos", shooter.gethoodPosition());
+            TELE.addData("turretPos", shooter.getTurretPosition());
+            TELE.addData("Power Fly 1", robot.shooter1.getPower());
+            TELE.addData("Power Fly 2", robot.shooter2.getPower());
+            TELE.addData("powPID", shooter.getpowPID());
+            TELE.addData("Velocity", velo);
+            TELE.update();
+
+
+        }
+
+    }
+
+    public double hoodAnglePrediction(double distance) {
+        double L = 0.298317;
+        double A = 1.02124;
+        double k = 0.0157892;
+        double n = 3.39375;
+
+        double dist = Math.sqrt(distance*distance+24*24);
+
+        return L + A * Math.exp(-Math.pow(k * dist, n));
+    }
+    public static double velPrediction(double distance) {
+
+        double x = Math.sqrt(distance*distance+24*24);
+
+
+
+        double A = -211149.992;
+        double B = -1.19943;
+        double C = 3720.15909;
+
+        return A * Math.pow(x, B) + C;
+    }
+
+}

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

@@ -1,11 +1,9 @@
-package org.firstinspires.ftc.teamcode.subsystems;
+package org.firstinspires.ftc.teamcode.utils;
 
 import android.util.Size;
 
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.qualcomm.robotcore.hardware.HardwareMap;
 
-import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.firstinspires.ftc.vision.VisionPortal;
@@ -34,7 +32,7 @@ public class AprilTagWebcam {
                 .build();
 
         VisionPortal.Builder builder = new VisionPortal.Builder();
-        builder.setCamera(robot.webcamName);
+        builder.setCamera(robot.webcam);
         builder.setCameraResolution(new Size(640, 480));
         builder.addProcessor(aprilTagProcessor);
 

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

@@ -1,19 +1,16 @@
 package org.firstinspires.ftc.teamcode.utils;
 
+import com.qualcomm.hardware.rev.RevColorSensorV3;
 import com.qualcomm.robotcore.hardware.AnalogInput;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.DcMotorImplEx;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.DigitalChannel;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.Servo;
 
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
-import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
-import org.firstinspires.ftc.vision.VisionPortal;
 import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
-import org.openftc.easyopencv.OpenCvWebcam;
 
 public class Robot {
 
@@ -30,8 +27,6 @@ public class Robot {
 
     public DcMotorEx transfer;
 
-
-
     public DcMotorEx shooter1;
     public DcMotorEx shooter2;
     public Servo hood;
@@ -61,27 +56,31 @@ public class Robot {
 
     public AnalogInput analogInput2;
 
-    public AprilTagProcessor aprilTagProcessor;
+    public AnalogInput spin1Pos;
 
+    public AnalogInput spin2Pos;
+
+    public AnalogInput hoodPos;
+
+    public AnalogInput turr1Pos;
+
+    public AnalogInput turr2Pos;
+
+    public AnalogInput transferServoPos;
+
+    public AprilTagProcessor aprilTagProcessor;
 
     public WebcamName webcam;
 
     public DcMotorEx shooterEncoder;
 
+    public RevColorSensorV3 color1;
 
+    public RevColorSensorV3 color2;
 
+    public RevColorSensorV3 color3;
 
-
-
-
-
-
-
-
-
-
-
-    public Robot (HardwareMap hardwareMap) {
+    public Robot(HardwareMap hardwareMap) {
 
         //Define components w/ hardware map
 
@@ -108,14 +107,24 @@ public class Robot {
 
         hood = hardwareMap.get(Servo.class, "hood");
 
+        hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos");
+
         turr1 = hardwareMap.get(Servo.class, "t1");
 
+        turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos");
+
         turr2 = hardwareMap.get(Servo.class, "t2");
 
+        turr2Pos = hardwareMap.get(AnalogInput.class, "t2Pos");
+
         spin1 = hardwareMap.get(Servo.class, "spin1");
 
+        spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
+
         spin2 = hardwareMap.get(Servo.class, "spin2");
 
+        spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos");
+
         pin0 = hardwareMap.get(DigitalChannel.class, "pin0");
 
         pin1 = hardwareMap.get(DigitalChannel.class, "pin1");
@@ -128,27 +137,26 @@ public class Robot {
 
         pin5 = hardwareMap.get(DigitalChannel.class, "pin5");
 
-
-
         analogInput = hardwareMap.get(AnalogInput.class, "analog");
 
-
         analogInput2 = hardwareMap.get(AnalogInput.class, "analog2");
 
         transfer = hardwareMap.get(DcMotorEx.class, "transfer");
 
         transferServo = hardwareMap.get(Servo.class, "transferServo");
 
+        transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos");
+
         transfer.setDirection(DcMotorSimple.Direction.REVERSE);
 
         aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();
 
-
         webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
 
+        color1 = hardwareMap.get(RevColorSensorV3.class, "c1");
 
+        color2 = hardwareMap.get(RevColorSensorV3.class, "c2");
 
-
-
+        color3 = hardwareMap.get(RevColorSensorV3.class, "c3");
     }
 }