oops...hehe

s
ok
2025-11-30 19:34:12 -06:00 · 2025-11-30 19:22:25 -06:00 · 2025-11-30 19:20:44 -06:00 · 2025-11-30 18:16:08 -06:00 · 2025-11-30 17:31:37 -06:00 · 2025-11-30 16:59:23 -06:00
26 changed files with 1098 additions and 2428 deletions
--- a/TeamCode/build.gradle
+++ b/TeamCode/build.gradle
@@ -23,6 +23,19 @@ android {
    }
 }

+
+repositories {
+    maven {
+        url = 'https://maven.brott.dev/'
+    }
+}
+
 dependencies {
    implementation project(':FtcRobotController')
+    implementation "com.acmerobotics.roadrunner:ftc:0.1.25"
+    implementation "com.acmerobotics.roadrunner:core:1.0.1"
+    implementation "com.acmerobotics.roadrunner:actions:1.0.1"
+    implementation "com.acmerobotics.dashboard:dashboard:0.5.1"
+    implementation 'org.ftclib.ftclib:core:2.1.1' // core
+
 }
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue.java
@@ -1,478 +0,0 @@
-package org.firstinspires.ftc.teamcode.autonomous;
-
-import static org.firstinspires.ftc.teamcode.constants.Poses.h1;
-import static org.firstinspires.ftc.teamcode.constants.Poses.h2;
-import static org.firstinspires.ftc.teamcode.constants.Poses.h2_b;
-import static org.firstinspires.ftc.teamcode.constants.Poses.h3;
-import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
-import static org.firstinspires.ftc.teamcode.constants.Poses.x1;
-import static org.firstinspires.ftc.teamcode.constants.Poses.x2;
-import static org.firstinspires.ftc.teamcode.constants.Poses.x2_b;
-import static org.firstinspires.ftc.teamcode.constants.Poses.x3;
-import static org.firstinspires.ftc.teamcode.constants.Poses.y1;
-import static org.firstinspires.ftc.teamcode.constants.Poses.y2;
-import static org.firstinspires.ftc.teamcode.constants.Poses.y2_b;
-import static org.firstinspires.ftc.teamcode.constants.Poses.y3;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
-
-import androidx.annotation.NonNull;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.acmerobotics.roadrunner.Action;
-import com.acmerobotics.roadrunner.ParallelAction;
-import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
-import com.acmerobotics.roadrunner.Vector2d;
-import com.acmerobotics.roadrunner.ftc.Actions;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-
-import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
-import org.firstinspires.ftc.teamcode.subsystems.AprilTag;
-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
-@Autonomous (preselectTeleOp = "TeleopV1")
-public class Blue extends LinearOpMode {
-
-    Robot robot;
-
-    MultipleTelemetry TELE;
-
-    MecanumDrive drive;
-
-    AprilTag aprilTag;
-
-    Shooter shooter;
-
-
-
-    public static double angle = 0.1;
-
-    Spindexer spindexer;
-
-    Transfer transfer;
-
-    public class shooterOn implements Action {
-
-        int ticker = 1;
-
-        double stamp = 0.0;
-
-
-        @Override
-        public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-
-            if (ticker ==1){
-                stamp = getRuntime();
-
-            }
-
-            ticker ++;
-
-            if (getRuntime() - stamp < 0.2){
-                return true;
-            } else if (getRuntime() - stamp < 0.4) {
-
-                shooter.setManualPower(1);
-
-                shooter.update();
-
-                return true;
-            } else {
-
-                return false;
-
-            }
-        }
-    }
-
-
-
-
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-
-
-
-
-        robot = new Robot(hardwareMap);
-        TELE = new MultipleTelemetry(
-                telemetry, FtcDashboard.getInstance().getTelemetry()
-        );
-
-        drive = new MecanumDrive(hardwareMap, new Pose2d(
-                0,0,0
-        ));
-
-        aprilTag = new AprilTag(robot, TELE);
-
-        shooter = new Shooter(robot, TELE);
-
-        spindexer = new Spindexer(robot, TELE);
-
-        spindexer.outtake3();
-
-        shooter.setShooterMode("MANUAL");
-
-        shooter.sethoodPosition(0.53);
-
-        transfer = new Transfer(robot);
-
-        transfer.setTransferPosition(transferServo_out);
-
-        Intake intake = new Intake(robot);
-
-        robot.hood.setPosition(hoodDefault);
-
-
-
-
-
-
-
-
-        TrajectoryActionBuilder traj1 = drive.actionBuilder(new Pose2d(0, 0, 0))
-                .strafeToLinearHeading(new Vector2d(x1, y1), h1 );
-
-
-        TrajectoryActionBuilder traj2 = drive.actionBuilder(new Pose2d(x1, y1, h1))
-                .turnTo(Math.toRadians(-135))
-                .strafeToLinearHeading(new Vector2d(x2, -y2), -h2 );
-
-
-        TrajectoryActionBuilder traj3 = drive.actionBuilder(new Pose2d(x2, -y2, -h2))
-                .strafeToLinearHeading(new Vector2d(x1, y1), h1 );
-
-        TrajectoryActionBuilder traj4 = drive.actionBuilder(new Pose2d(x1, y1, h1))
-
-                .strafeToLinearHeading(new Vector2d(x2_b, -y2_b), -h2_b )
-
-                .strafeToLinearHeading(new Vector2d(x3, -y3), -h3 );
-
-
-        TrajectoryActionBuilder traj5 = drive.actionBuilder(new Pose2d(x3, -y3, -h3))
-                .strafeToLinearHeading(new Vector2d(x1, y1), h1 );
-
-
-        TrajectoryActionBuilder traj6 = drive.actionBuilder(new Pose2d(x1, y1, h1))
-                .strafeToLinearHeading(new Vector2d(x1, y1-30), h1 );
-
-        while(opModeInInit()) {
-
-            if (gamepad2.dpadUpWasPressed()){
-                hoodDefault -= 0.02;
-            }
-
-            if (gamepad2.dpadDownWasPressed()){
-                hoodDefault += 0.02;
-            }
-
-            robot.hood.setPosition(hoodDefault);
-
-            shooter.setTurretPosition(0.3);
-
-            aprilTag.initTelemetry();
-
-            aprilTag.update();
-            shooter.update();
-
-            TELE.addData("hood", hoodDefault);
-
-            TELE.update();
-
-
-        }
-
-
-        waitForStart();
-
-        if (isStopRequested()) return;
-
-        if (opModeIsActive()){
-
-            robot.hood.setPosition(hoodDefault);
-
-            transfer.setTransferPower(1);
-
-            transfer.update();
-
-
-
-
-            Actions.runBlocking(
-                    new ParallelAction(
-                            traj1.build(),
-                            new shooterOn()
-                    )
-            );
-
-
-            transfer.setTransferPower(1);
-
-            transfer.update();
-
-            shooter.setManualPower(1);
-
-            double stamp = getRuntime();
-
-
-             stamp = getRuntime();
-
-            while (getRuntime()-stamp<4.5) {
-
-
-
-                shooter.moveTurret(0.3);
-
-                double time = getRuntime()-stamp;
-
-
-                if (time < 0.3) {
-
-
-                    transfer.transferOut();
-                    transfer.setTransferPower(1);
-                } else if (time < 1) {
-
-                    spindexer.outtake3();
-
-
-                } else if (time < 1.4) {
-
-
-                    transfer.transferIn();
-                } else if (time < 2.6) {
-                    transfer.transferOut();
-
-                    spindexer.outtake2();
-
-
-
-                } else if (time < 3.0) {
-
-
-                    transfer.transferIn();
-                } else if (time < 4.0) {
-
-                    transfer.transferOut();
-
-                    spindexer.outtake1();
-
-                } else if (time < 4.4) {
-                    transfer.transferIn();
-                } else {
-
-                    transfer.transferOut();
-                    spindexer.outtake3();
-
-                    shooter.setManualPower(1);
-
-                }
-
-                transfer.update();
-
-                shooter.update();
-
-                spindexer.update();
-
-            }
-
-            spindexer.outtake3();
-
-            robot.intake.setPower(1);
-
-            Actions.runBlocking(
-                    traj2.build()
-            );
-
-
-            Actions.runBlocking(
-                    traj3.build()
-            );
-
-            shooter.setManualPower(1);
-
-
-
-
-            robot.intake.setPower(0);
-
-            spindexer.outtake3();
-
-
-            stamp = getRuntime();
-
-            shooter.setManualPower(1);
-            while (getRuntime()-stamp<4.5) {
-
-
-
-                shooter.moveTurret(0.3);
-
-                double time = getRuntime()-stamp;
-
-
-                if (time < 0.3) {
-
-
-                    transfer.transferOut();
-                    transfer.setTransferPower(1);
-                } else if (time < 1) {
-
-                    spindexer.outtake3();
-
-
-                } else if (time < 1.4) {
-
-
-                    transfer.transferIn();
-                } else if (time < 2.6) {
-                    transfer.transferOut();
-
-                    spindexer.outtake2();
-
-
-
-                } else if (time < 3.0) {
-
-
-                    transfer.transferIn();
-                } else if (time < 4.0) {
-
-                    transfer.transferOut();
-
-                    spindexer.outtake1();
-
-                } else if (time < 4.4) {
-                    transfer.transferIn();
-                } else {
-
-                    transfer.transferOut();
-                    spindexer.outtake3();
-
-                    shooter.setManualPower(1);
-
-                }
-
-                transfer.update();
-
-                shooter.update();
-
-                spindexer.update();
-
-            }
-
-            spindexer.outtake3();
-            robot.intake.setPower(1);
-
-            Actions.runBlocking(
-                    traj4.build()
-            );
-
-
-            Actions.runBlocking(
-                    traj5.build()
-            );
-
-            shooter.setManualPower(1);
-
-
-
-
-            robot.intake.setPower(0);
-
-
-            stamp = getRuntime();
-
-            shooter.setManualPower(1);
-
-            while (getRuntime()-stamp<4.5) {
-
-
-
-                shooter.moveTurret(0.3);
-
-                double time = getRuntime()-stamp;
-
-
-                if (time < 0.3) {
-
-
-                    transfer.transferOut();
-                    transfer.setTransferPower(1);
-                } else if (time < 1) {
-
-                    spindexer.outtake3();
-
-
-                } else if (time < 1.4) {
-
-
-                    transfer.transferIn();
-                } else if (time < 2.6) {
-                    transfer.transferOut();
-
-                    spindexer.outtake2();
-
-
-
-                } else if (time < 3.0) {
-
-
-                    transfer.transferIn();
-                } else if (time < 4.0) {
-
-                    transfer.transferOut();
-
-                    spindexer.outtake1();
-
-                } else if (time < 4.4) {
-                    transfer.transferIn();
-                } else {
-
-                    transfer.transferOut();
-                    spindexer.outtake3();
-
-                    shooter.setManualPower(1);
-
-                }
-
-                transfer.update();
-
-                shooter.update();
-
-                spindexer.update();
-
-            }
-
-            spindexer.outtake3();
-
-            Actions.runBlocking(
-                    traj6.build()
-            );
-
-            drive.updatePoseEstimate();
-
-            teleStart = drive.localizer.getPose();
-
-            TELE.addLine("finished");
-
-            TELE.update();
-
-            sleep (2000);
-
-
-        }
-
-    }
-}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java
@@ -1,431 +0,0 @@
-package org.firstinspires.ftc.teamcode.autonomous;
-
-import static org.firstinspires.ftc.teamcode.constants.Poses.*;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
-
-import androidx.annotation.NonNull;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.acmerobotics.roadrunner.Action;
-import com.acmerobotics.roadrunner.ParallelAction;
-import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.SequentialAction;
-import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
-import com.acmerobotics.roadrunner.Vector2d;
-import com.acmerobotics.roadrunner.ftc.Actions;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-
-import org.firstinspires.ftc.teamcode.constants.ServoPositions;
-import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
-import org.firstinspires.ftc.teamcode.subsystems.AprilTag;
-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;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
-
-
-
-@Config
-@Autonomous
-public class Red extends LinearOpMode {
-
-    Robot robot;
-
-    MultipleTelemetry TELE;
-
-    MecanumDrive drive;
-
-    AprilTag aprilTag;
-
-    Shooter shooter;
-
-
-
-    public static double angle = 0.1;
-
-    Spindexer spindexer;
-
-    Transfer transfer;
-
-
-
-
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-
-
-
-
-        robot = new Robot(hardwareMap);
-        TELE = new MultipleTelemetry(
-                telemetry, FtcDashboard.getInstance().getTelemetry()
-        );
-
-        drive = new MecanumDrive(hardwareMap, new Pose2d(
-                0,0,0
-        ));
-
-        aprilTag = new AprilTag(robot, TELE);
-
-        shooter = new Shooter(robot, TELE);
-
-        spindexer = new Spindexer(robot, TELE);
-
-        spindexer.outtake3();
-
-        shooter.setShooterMode("MANUAL");
-
-        shooter.sethoodPosition(hoodDefault);
-
-        transfer = new Transfer(robot);
-
-        transfer.setTransferPosition(transferServo_out);
-
-        Intake intake = new Intake(robot);
-
-        robot.hood.setPosition(hoodDefault);
-
-
-
-
-
-
-
-
-        TrajectoryActionBuilder traj1 = drive.actionBuilder(new Pose2d(0, 0, 0))
-                .strafeToLinearHeading(new Vector2d(x1, y1), h1 );
-
-
-        TrajectoryActionBuilder traj2 = drive.actionBuilder(new Pose2d(x1, y1, h1))
-                .turnTo(Math.toRadians(135))
-                .strafeToLinearHeading(new Vector2d(x2, y2), h2 );
-
-
-        TrajectoryActionBuilder traj3 = drive.actionBuilder(new Pose2d(x2, y2, h2))
-                .strafeToLinearHeading(new Vector2d(x1, y1), h1 );
-
-        TrajectoryActionBuilder traj4 = drive.actionBuilder(new Pose2d(x1, y1, h1))
-
-                .strafeToLinearHeading(new Vector2d(x2_b, y2_b), h2_b )
-
-                .strafeToLinearHeading(new Vector2d(x3, y3), h3 );
-
-
-        TrajectoryActionBuilder traj5 = drive.actionBuilder(new Pose2d(x3, y3, h3))
-                .strafeToLinearHeading(new Vector2d(x1, y1), h1 );
-
-
-        TrajectoryActionBuilder traj6 = drive.actionBuilder(new Pose2d(x1, y1, h1))
-                .strafeToLinearHeading(new Vector2d(x1, y1+30), h1 );
-
-        while(opModeInInit()) {
-
-            if (gamepad2.dpadUpWasPressed()){
-                hoodDefault -= 0.02;
-            }
-
-            if (gamepad2.dpadDownWasPressed()){
-                hoodDefault += 0.02;
-            }
-
-            robot.hood.setPosition(hoodDefault);
-
-            shooter.setTurretPosition(0.33);
-
-            aprilTag.initTelemetry();
-
-            aprilTag.update();
-            shooter.update();
-
-            TELE.update();
-        }
-
-
-        waitForStart();
-
-        if (isStopRequested()) return;
-
-        if (opModeIsActive()){
-
-            robot.hood.setPosition(hoodDefault);
-
-            transfer.setTransferPower(1);
-
-            transfer.update();
-
-
-
-
-            Actions.runBlocking(
-                    new ParallelAction(
-                            traj1.build()
-                    )
-            );
-
-
-            transfer.setTransferPower(1);
-
-            transfer.update();
-
-            shooter.setManualPower(1);
-
-            double stamp = getRuntime();
-
-
-             stamp = getRuntime();
-
-            while (getRuntime()-stamp<4.5) {
-
-
-
-                shooter.moveTurret(0.31);
-
-                double time = getRuntime()-stamp;
-
-
-                if (time < 0.3) {
-
-
-                    transfer.transferOut();
-                    transfer.setTransferPower(1);
-                } else if (time < 1) {
-
-                    spindexer.outtake3();
-
-
-                } else if (time < 1.4) {
-
-
-                    transfer.transferIn();
-                } else if (time < 2.6) {
-                    transfer.transferOut();
-
-                    spindexer.outtake2();
-
-
-
-                } else if (time < 3.0) {
-
-
-                    transfer.transferIn();
-                } else if (time < 4.0) {
-
-                    transfer.transferOut();
-
-                    spindexer.outtake1();
-
-                } else if (time < 4.4) {
-                    transfer.transferIn();
-                } else {
-
-                    transfer.transferOut();
-                    spindexer.outtake3();
-
-                    shooter.setManualPower(0);
-
-                }
-
-                transfer.update();
-
-                shooter.update();
-
-                spindexer.update();
-
-            }
-
-            spindexer.outtake3();
-
-            robot.intake.setPower(1);
-
-            Actions.runBlocking(
-                    traj2.build()
-            );
-
-
-            Actions.runBlocking(
-                    traj3.build()
-            );
-
-            shooter.setManualPower(1);
-
-
-
-
-            robot.intake.setPower(0);
-
-            spindexer.outtake3();
-
-
-            stamp = getRuntime();
-
-            shooter.setManualPower(1);
-            while (getRuntime()-stamp<4.5) {
-
-
-
-                shooter.moveTurret(0.31);
-
-                double time = getRuntime()-stamp;
-
-
-                if (time < 0.3) {
-
-
-                    transfer.transferOut();
-                    transfer.setTransferPower(1);
-                } else if (time < 1) {
-
-                    spindexer.outtake3();
-
-
-                } else if (time < 1.4) {
-
-
-                    transfer.transferIn();
-                } else if (time < 2.6) {
-                    transfer.transferOut();
-
-                    spindexer.outtake2();
-
-
-
-                } else if (time < 3.0) {
-
-
-                    transfer.transferIn();
-                } else if (time < 4.0) {
-
-                    transfer.transferOut();
-
-                    spindexer.outtake1();
-
-                } else if (time < 4.4) {
-                    transfer.transferIn();
-                } else {
-
-                    transfer.transferOut();
-                    spindexer.outtake3();
-
-                    shooter.setManualPower(0);
-
-                }
-
-                transfer.update();
-
-                shooter.update();
-
-                spindexer.update();
-
-            }
-
-            spindexer.outtake3();
-            robot.intake.setPower(1);
-
-            Actions.runBlocking(
-                    traj4.build()
-            );
-
-
-            Actions.runBlocking(
-                    traj5.build()
-            );
-
-            shooter.setManualPower(1);
-
-
-
-
-            robot.intake.setPower(0);
-
-
-            stamp = getRuntime();
-
-            shooter.setManualPower(1);
-
-            while (getRuntime()-stamp<4.5) {
-
-
-
-                shooter.moveTurret(0.31);
-
-                double time = getRuntime()-stamp;
-
-
-                if (time < 0.3) {
-
-
-                    transfer.transferOut();
-                    transfer.setTransferPower(1);
-                } else if (time < 1) {
-
-                    spindexer.outtake3();
-
-
-                } else if (time < 1.4) {
-
-
-                    transfer.transferIn();
-                } else if (time < 2.6) {
-                    transfer.transferOut();
-
-                    spindexer.outtake2();
-
-
-
-                } else if (time < 3.0) {
-
-
-                    transfer.transferIn();
-                } else if (time < 4.0) {
-
-                    transfer.transferOut();
-
-                    spindexer.outtake1();
-
-                } else if (time < 4.4) {
-                    transfer.transferIn();
-                } else {
-
-                    transfer.transferOut();
-                    spindexer.outtake3();
-
-                    shooter.setManualPower(0);
-
-                }
-
-                transfer.update();
-
-                shooter.update();
-
-                spindexer.update();
-
-            }
-
-            spindexer.outtake3();
-
-            Actions.runBlocking(
-                    traj6.build()
-            );
-
-            drive.updatePoseEstimate();
-
-            teleStart = drive.localizer.getPose();
-
-            TELE.addLine("finished");
-
-            TELE.update();
-
-            sleep (2000);
-
-
-        }
-
-    }
-}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java
@@ -0,0 +1,4 @@
+package org.firstinspires.ftc.teamcode.constants;
+
+public class Color {
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java
@@ -15,12 +15,12 @@ public class Poses {

    public static double x1 = 50, y1 = 0, h1 = 0;

-    public static double x2 = 31, y2 = 32, h2 = Math.toRadians(135);
+    public static double x2 = 31, y2 = 32, h2 = Math.toRadians(140);

-    public static double x2_b = 58, y2_b = 42, h2_b = Math.toRadians(135);
+    public static double x2_b = 58, y2_b = 42, h2_b = Math.toRadians(140);


-    public static double x3 = 34, y3 = 58, h3 = Math.toRadians(135);
+    public static double x3 = 34, y3 = 58, h3 = Math.toRadians(140);

    public static Pose2d teleStart = new Pose2d(x1,-10,0);

--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
@@ -6,18 +6,29 @@ import com.acmerobotics.dashboard.config.Config;
@Config
 public class ServoPositions {

-    public static double spindexer_intakePos = 0.665;
+    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.13;
+    public static double transferServo_out = 0.15;

-    public static double transferServo_in = 0.4;
+    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;
+

 }
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java
+++ b/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;
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md
@@ -1,147 +1,130 @@
-# Team FTC Git Workflow Guide
+## TeamCode Module

+Welcome!

-## 1. Cloning the Repository
+This module, TeamCode, is the place where you will write/paste the code for your team's
+robot controller App. This module is currently empty (a clean slate) but the
+process for adding OpModes is straightforward.

-1. Open a terminal (or the terminal inside Android Studio).
-2. Navigate to the folder where you want to keep the project.
-3. Run:
+## Creating your own OpModes

-   ```bash
-   git clone https://github.com/KeshavAnandCode/DecodeFTCMain.git
-   cd DecodeFTCMain
-   ```
+The easiest way to create your own OpMode is to copy a Sample OpMode and make it your own.

-4. Verify your remotes:
+Sample opmodes exist in the FtcRobotController module.
+To locate these samples, find the FtcRobotController module in the "Project/Android" tab.

-   ```bash
-   git remote -v
-   ```
+Expand the following tree elements:
+FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples

-   You should see:
-   ```
-   origin    https://github.com/KeshavAnandCode/DecodeFTCMain.git (fetch)
-   origin    https://github.com/KeshavAnandCode/DecodeFTCMain.git (push)
-   upstream  https://github.com/FIRST-Tech-Challenge/FtcRobotController.git (fetch)
-   upstream  https://github.com/FIRST-Tech-Challenge/FtcRobotController.git (push)
-   ```
+### Naming of Samples

---
+To gain a better understanding of how the samples are organized, and how to interpret the
+naming system, it will help to understand the conventions that were used during their creation.

-## 2. Keeping `master` Clean
+These conventions are described (in detail) in the sample_conventions.md file in this folder.

- `master` should only contain clean, tested code.
- Nobody should ever code directly on `master`.
- To stay up to date:
+To summarize: A range of different samples classes will reside in the java/external/samples.
+The class names will follow a naming convention which indicates the purpose of each class.
+The prefix of the name will be one of the following:

-   ```bash
-   git checkout master
-   git fetch upstream
-   git merge upstream/master
-   git push origin master
-   ```
+Basic:    This is a minimally functional OpMode used to illustrate the skeleton/structure
+of a particular style of OpMode. These are bare bones examples.

---
+Sensor:        This is a Sample OpMode that shows how to use a specific sensor.
+It is not intended to drive a functioning robot, it is simply showing the minimal code
+required to read and display the sensor values.

-## 3. Creating a Feature Branch
+Robot:        This is a Sample OpMode that assumes a simple two-motor (differential) drive base.
+It may be used to provide a common baseline driving OpMode, or
+to demonstrate how a particular sensor or concept can be used to navigate.

-Whenever you start a new task (feature, fix, experiment):
+Concept:    This is a sample OpMode that illustrates performing a specific function or concept.
+These may be complex, but their operation should be explained clearly in the comments,
+or the comments should reference an external doc, guide or tutorial.
+Each OpMode should try to only demonstrate a single concept so they are easy to
+locate based on their name. These OpModes may not produce a drivable robot.

-1. Update `master` (see above).
-2. Create a new branch from `master`:
+After the prefix, other conventions will apply:

-   ```bash
-   git checkout master
-   git pull origin master
-   git checkout -b feature/short-description
-   ```
+* Sensor class names are constructed as:    Sensor - Company - Type
+* Robot class names are constructed as:     Robot - Mode - Action - OpModetype
+* Concept class names are constructed as:   Concept - Topic - OpModetype

-### Branch Naming Standard
+Once you are familiar with the range of samples available, you can choose one to be the
+basis for your own robot. In all cases, the desired sample(s) needs to be copied into
+your TeamCode module to be used.

-Branches **must** follow the format:
+This is done inside Android Studio directly, using the following steps:
+
+1) Locate the desired sample class in the Project/Android tree.
+
+2) Right click on the sample class and select "Copy"
+
+3) Expand the TeamCode/java folder
+
+4) Right click on the org.firstinspires.ftc.teamcode folder and select "Paste"
+
+5) You will be prompted for a class name for the copy.
+   Choose something meaningful based on the purpose of this class.
+   Start with a capital letter, and remember that there may be more similar classes later.
+
+Once your copy has been created, you should prepare it for use on your robot.
+This is done by adjusting the OpMode's name, and enabling it to be displayed on the
+Driver Station's OpMode list.
+
+Each OpMode sample class begins with several lines of code like the ones shown below:

 ```
-<type>/<short-description>
+ @TeleOp(name="Template: Linear OpMode", group="Linear Opmode")
+ @Disabled
 ```

-Where `<type>` is one of:
- `feature/` → new functionality
- `fix/` → bug fixes
- `experiment/` → prototypes or tests
- `docs/` → documentation updates
- `chore/` → maintenance or cleanup
+The name that will appear on the driver station's "opmode list" is defined by the code:
+``name="Template: Linear OpMode"``
+You can change what appears between the quotes to better describe your opmode.
+The "group=" portion of the code can be used to help organize your list of OpModes.

-Examples:
- `feature/autonomous-path`
- `fix/motor-init`
- `experiment/vision-test`
- `docs/setup-instructions`
- `chore/gradle-update`
+As shown, the current OpMode will NOT appear on the driver station's OpMode list because of the
+``@Disabled`` annotation which has been included.
+This line can simply be deleted , or commented out, to make the OpMode visible.

-**Rules for names:**
- Use lowercase letters and hyphens (`-`) only.
- Keep it short but clear (3–5 words).
- One branch = one task. Never mix unrelated work.
+## ADVANCED Multi-Team App management:  Cloning the TeamCode Module

---
+In some situations, you have multiple teams in your club and you want them to all share
+a common code organization, with each being able to *see* the others code but each having
+their own team module with their own code that they maintain themselves.

-## 4. Working on Your Branch
+In this situation, you might wish to clone the TeamCode module, once for each of these teams.
+Each of the clones would then appear along side each other in the Android Studio module list,
+together with the FtcRobotController module (and the original TeamCode module).

- Make changes in Android Studio.
- Stage and commit your changes:
+Selective Team phones can then be programmed by selecting the desired Module from the pulldown list
+prior to clicking to the green Run arrow.

-   ```bash
-   git add .
-   git commit -m "short message about what changed"
-   ```
+Warning:  This is not for the inexperienced Software developer.
+You will need to be comfortable with File manipulations and managing Android Studio Modules.
+These changes are performed OUTSIDE of Android Studios, so close Android Studios before you do this.

- Push your branch to GitHub:
+Also.. Make a full project backup before you start this :)

-   ```bash
-   git push origin feature/short-description
-   ```
+To clone TeamCode, do the following:

---
+Note: Some names start with "Team" and others start with "team". This is intentional.

-## 5. Sharing Your Work
+1) Using your operating system file management tools, copy the whole "TeamCode"
+   folder to a sibling folder with a corresponding new name, eg: "Team0417".

- Once your branch is ready:
-    1. Open a Pull Request (PR) on GitHub to merge into `master`.
-    2. At least one teammate should review before merging.
+2) In the new Team0417 folder, delete the TeamCode.iml file.

---
+3) the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder
+   to a matching name with a lowercase 'team' eg:  "team0417".

-## 6. Branching Rules
+4) In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that
+   contains
+   package="org.firstinspires.ftc.teamcode"
+   to be
+   package="org.firstinspires.ftc.team0417"

-**Do:**
- Always branch from `master`.
- Follow the naming standard exactly.
- Keep branches small and focused.
- Delete branches after they’re merged.
+5) Add:    include ':Team0417' to the "/settings.gradle" file.

-**Don’t:**
- Don’t push commits directly to `master`.
- Don’t leave unfinished work on `master`.
- Don’t mix unrelated changes in one branch.
-
---
-
-## 7. Example Workflow
-
-```bash
-# Get latest code
-git checkout master
-git fetch upstream
-git merge upstream/master
-git push origin master
-
-# Start a new feature
-git checkout -b feature/teleop-improvements
-
-# Work on code, then commit
-git add .
-git commit -m "improved joystick scaling in TeleOp"
-
-# Push branch
-git push origin feature/teleop-improvements
-```
+6) Open up Android Studios and clean out any old files by using the menu to "Build/Clean Project""
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTag.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTag.java
@@ -1,238 +0,0 @@
-package org.firstinspires.ftc.teamcode.subsystems;
-
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-
-import org.firstinspires.ftc.teamcode.utils.Robot;
-import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
-import org.firstinspires.ftc.vision.VisionPortal;
-import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
-import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
-
-import java.util.ArrayList;
-import java.util.Collections;
-import java.util.List;
-
-public class AprilTag implements Subsystem {
-
-    private AprilTagProcessor aprilTag;
-    private VisionPortal visionPortal;
-
-    private MultipleTelemetry TELE;
-
-    private boolean teleOn = false;
-
-    private int detections = 0;
-
-    List<AprilTagDetection> currentDetections;
-
-    ArrayList<ArrayList<Double>> Data = new ArrayList<>();
-
-
-
-    public AprilTag(Robot robot, MultipleTelemetry tele) {
-
-
-
-        this.aprilTag = robot.aprilTagProcessor;
-
-
-        this.TELE = tele;
-
-
-
-
-    }
-
-    @Override
-    public void update() {
-
-        currentDetections = aprilTag.getDetections();
-
-        UpdateData();
-
-        if(teleOn){
-            tagTELE();
-            initTelemetry();
-        }
-
-
-
-
-    }
-
-    public void initTelemetry (){
-
-        TELE.addData("Camera Preview", "Check Driver Station for stream");
-        TELE.addData("Status", "Initialized - Press START");
-
-
-
-    }
-
-    public void tagTELE () {
-
-        TELE.addData("# AprilTags Detected", detections);
-
-        // Display info for each detected tag
-        for (ArrayList<Double> detection : Data) {
-            if (detection.get(0) ==1) {
-                // Known AprilTag with metadata
-                TELE.addLine(String.format("\n==== (ID %d) %s ====",
-                        detection.get(1).intValue(), ""));
-
-                TELE.addLine(String.format("XYZ: %6.1f %6.1f %6.1f  (inch)",
-                        detection.get(2),
-                        detection.get(3),
-                        detection.get(4)));
-
-                TELE.addData("Distance", getDistance(detection.get(1).intValue()));
-
-                TELE.addLine(String.format("PRY: %6.1f %6.1f %6.1f  (deg)",
-                        detection.get(5),
-                        detection.get(6),
-                        detection.get(7)));
-
-                TELE.addLine(String.format("RBE: %6.1f %6.1f %6.1f  (inch, deg, deg)",
-                        detection.get(8),
-                        detection.get(9),
-                        detection.get(10)));
-            }
-        }
-    }
-
-
-    public void turnTelemetryOn(boolean bool) {
-
-        teleOn = bool;
-
-    }
-
-
-
-    public void UpdateData () {
-
-        Data.clear();  // <--- THIS FIXES YOUR ISSUE
-
-
-        detections = currentDetections.size();
-
-
-        for (AprilTagDetection detection : currentDetections) {
-
-            ArrayList<Double> detectionData = new ArrayList<Double>();
-
-
-
-            if (detection.metadata != null) {
-
-                detectionData.add(1.0);
-                // Known AprilTag with metadata
-
-                detectionData.add( (double) detection.id);
-
-
-
-                detectionData.add(detection.ftcPose.x);
-                detectionData.add(detection.ftcPose.y);
-                detectionData.add(detection.ftcPose.z);
-
-
-
-                detectionData.add(detection.ftcPose.pitch);
-                detectionData.add(detection.ftcPose.roll);
-                detectionData.add(detection.ftcPose.yaw);
-
-                detectionData.add(detection.ftcPose.range);
-                detectionData.add(detection.ftcPose.bearing);
-                detectionData.add(detection.ftcPose.elevation);
-
-            } else {
-
-                detectionData.add(0, 0.0);
-
-            }
-
-            Data.add(detectionData);
-        }
-
-    }
-
-    public int getDetectionCount() {
-
-        return detections;
-
-    }
-
-    public boolean isDetected (int id){
-        return (!filterID(Data, (double) id ).isEmpty());
-    }
-
-
-
-    public double getDistance(int id) {
-        ArrayList<Double> d = filterID(Data, (double) id);
-        if (d.size() >= 5) {
-            double x = d.get(2);
-            double y = d.get(3);
-            double z = d.get(4);
-            return Math.sqrt(x*x + y*y + z*z);
-        }
-        return -1; // tag not found
-    }
-
-    // Returns the position as [x, y, z]
-    public List<Double> getPosition(int id) {
-        ArrayList<Double> d = filterID(Data, (double) id);
-        if (d.size() >= 5) {
-            List<Double> pos = new ArrayList<>();
-            pos.add(d.get(2));
-            pos.add(d.get(3));
-            pos.add(d.get(4));
-            return pos;
-        }
-        return Collections.emptyList();
-    }
-
-    // Returns orientation as [pitch, roll, yaw]
-    public List<Double> getOrientation(int id) {
-        ArrayList<Double> d = filterID(Data, (double) id);
-        if (d.size() >= 8) {
-            List<Double> ori = new ArrayList<>();
-            ori.add(d.get(5));
-            ori.add(d.get(6));
-            ori.add(d.get(7));
-            return ori;
-        }
-        return Collections.emptyList();
-    }
-
-    // Returns range, bearing, elevation as [range, bearing, elevation]
-    public List<Double> getRBE(int id) {
-        ArrayList<Double> d = filterID(Data, (double) id);
-        if (d.size() >= 11) {
-            List<Double> rbe = new ArrayList<>();
-            rbe.add(d.get(8));
-            rbe.add(d.get(9));
-            rbe.add(d.get(10));
-            return rbe;
-        }
-        return Collections.emptyList();
-    }
-
-    // Returns full raw data for debugging or custom processing
-    public ArrayList<Double> getRawData(int id) {
-        return filterID(Data, (double) id);
-    }
-
-    public static ArrayList<Double> filterID(ArrayList<ArrayList<Double>> data, double x) {
-        for (ArrayList<Double> innerList : data) {
-            // Ensure it has a second element
-            if (innerList.size() > 1 && Math.abs(innerList.get(1) - x) < 1e-9) {
-                return innerList; // Return the first match
-            }
-        }
-        // Return an empty ArrayList if no match found
-        return new ArrayList<>();
-    }
-
-}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java
@@ -1,99 +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 com.qualcomm.robotcore.hardware.Gamepad;
-
-import org.firstinspires.ftc.teamcode.utils.Robot;
-import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
-
-import java.util.Objects;
-
-public class Drivetrain implements Subsystem {
-
-    private GamepadEx gamepad;
-
-    public MultipleTelemetry TELE;
-
-    private String Mode = "Default";
-
-    private DcMotorEx fl;
-
-    private DcMotorEx fr;
-    private DcMotorEx bl;
-    private DcMotorEx br;
-
-    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);
-
-    }
-
-
-    @Override
-    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
-                    )
-            );
-        }
-
-    }
-}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java
@@ -1,77 +0,0 @@
-package org.firstinspires.ftc.teamcode.subsystems;
-
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.arcrobotics.ftclib.gamepad.GamepadEx;
-import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.Gamepad;
-
-
-import org.firstinspires.ftc.teamcode.utils.Robot;
-
-public class Intake implements Subsystem {
-
-    private GamepadEx gamepad;
-
-    public MultipleTelemetry TELE;
-
-
-    private DcMotorEx intake;
-
-    private double intakePower = 1.0;
-
-    private int intakeState = 0;
-
-
-    public Intake (Robot robot){
-
-
-        this.intake = robot.intake;
-
-
-    }
-
-    public int getIntakeState() {
-        return intakeState;
-    }
-
-    public void toggle(){
-        if (intakeState !=0){
-            intakeState = 0;
-        } else {
-            intakeState = 1;
-        }
-    }
-
-    public void setIntakePower(double pow){
-        intakePower = pow;
-    }
-
-    public void intake(){
-        intakeState =1;
-    }
-
-    public void reverse(){
-        intakeState =-1;
-    }
-
-
-    public void stop(){
-        intakeState =-1;
-    }
-
-
-
-
-    @Override
-    public void update() {
-
-        if (intakeState == 1){
-            intake.setPower(intakePower);
-        } else if (intakeState == -1){
-            intake.setPower(-intakePower);
-        } else {
-            intake.setPower(0);
-        }
-
-    }
-}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java
@@ -1,337 +0,0 @@
-package org.firstinspires.ftc.teamcode.subsystems;
-
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.acmerobotics.roadrunner.Pose2d;
-import com.arcrobotics.ftclib.controller.PIDController;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.PIDCoefficients;
-import com.qualcomm.robotcore.hardware.PIDFCoefficients;
-import com.qualcomm.robotcore.hardware.Servo;
-
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
-import org.firstinspires.ftc.teamcode.constants.Poses;
-import org.firstinspires.ftc.teamcode.utils.Robot;
-import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
-
-import java.util.Objects;
-
-public class Shooter implements Subsystem {
-    private final DcMotorEx fly1;
-    private final DcMotorEx fly2;
-
-    private final DcMotorEx encoder;
-    private final Servo hoodServo;
-
-    private final Servo turret1;
-
-    private final Servo turret2;
-
-
-    private final MultipleTelemetry telemetry;
-
-    private boolean telemetryOn = false;
-
-    private double manualPower = 0.0;
-    private double hoodPos = 0.0;
-
-    private double turretPos = 0.0;
-    private double velocity = 0.0;
-    private double posPower = 0.0;
-
-    private int targetPosition = 0;
-
-    private double p = 0.0003, i = 0, d = 0.00001;
-
-    private PIDController controller;
-
-
-
-
-
-    private String shooterMode = "AUTO";
-
-    private String turretMode = "AUTO";
-
-
-    public Shooter(Robot robot, MultipleTelemetry TELE) {
-        this.fly1 = robot.shooter1;
-        this.fly2 = robot.shooter2;
-        this.telemetry = TELE;
-        this.hoodServo = robot.hood;
-
-        // Reset encoders
-        fly1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        fly2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-
-        fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-
-
-        controller = new PIDController(p, i, d);
-
-
-        controller.setPID(p, i, d);
-
-        this.turret1 = robot.turr1;
-
-        this.turret2 = robot.turr2;
-
-        this.encoder = robot.shooterEncoder;
-
-
-
-
-
-
-    }
-
-    public void telemetryUpdate() {
-
-        // Telemetry
-
-        telemetry.addData("Mode", shooterMode);
-        telemetry.addData("ManualPower", manualPower);
-        telemetry.addData("Position", getPosition());
-        telemetry.addData("TargetPosition", targetPosition);
-        telemetry.addData("Velocity", getVelocity());
-        telemetry.addData("TargetVelocity", velocity);
-        telemetry.addData("hoodPos", gethoodPosition());
-        telemetry.addData("turretPos", getTurretPosition());
-        telemetry.addData("PID Coefficients", "P: %.6f, I: %.6f, D: %.6f", p, i, d);
-        telemetry.addData("Current Fly 1", fly1.getCurrent(CurrentUnit.AMPS));
-        telemetry.addData("Current Fly 2", fly2.getCurrent(CurrentUnit.AMPS));
-
-    }
-
-    public double gethoodPosition() {
-        return (hoodServo.getPosition());
-    }
-
-    public void sethoodPosition(double pos) {hoodPos = pos;}
-
-    public double getTurretPosition() {
-        return ((turret1.getPosition()+ (1-turret2.getPosition()))/2);
-    }
-
-    public void setTurretPosition(double pos) {turretPos = pos;}
-
-    public double getVelocity() {
-        return ((double) ((fly1.getVelocity(AngleUnit.DEGREES) + fly2.getVelocity(AngleUnit.DEGREES)) /2));
-    }
-
-    public void setVelocity(double vel){velocity = vel;}
-
-
-
-    public void setPosPower(double power){posPower = power;}
-
-
-    public void setTargetPosition(int pos){
-        targetPosition = pos;
-    }
-
-    public void setTolerance(int tolerance){
-        controller.setTolerance(tolerance);
-    }
-
-    public void setControllerCoefficients(double kp, double ki, double kd){
-        p = kp;
-        i = ki;
-        d = kd;
-        controller.setPID(p, i, d);
-
-    }
-
-    public PIDCoefficients  getControllerCoefficients(){
-
-        return new PIDCoefficients(p, i, d);
-
-    }
-
-
-
-
-
-
-
-    public void setManualPower(double power){manualPower = power;}
-
-    public String getShooterMode(){return shooterMode;}
-
-    public String getTurretMode(){return turretMode;}
-
-
-    public double getPosition(){
-        return ((double) ((fly1.getCurrentPosition() + fly2.getCurrentPosition()) /2));
-    }
-
-    public void setShooterMode(String mode){ shooterMode = mode;}
-
-    public void setTurretMode(String mode){ turretMode = mode;}
-
-
-    public double trackGoal(Pose2d robotPose, Pose2d goalPose, double offset){
-
-        fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-
-
-
-        Pose2d deltaPose = new Pose2d(
-                goalPose.position.x - robotPose.position.x,
-                goalPose.position.y - robotPose.position.y,
-                goalPose.heading.toDouble() - (robotPose.heading.toDouble())
-        );
-
-        double distance = Math.sqrt(
-                deltaPose.position.x * deltaPose.position.x
-                + deltaPose.position.y * deltaPose.position.y
-                + Poses.relativeGoalHeight * Poses.relativeGoalHeight
-        );
-
-        telemetry.addData("dst", distance);
-
-        double shooterPow = getPowerByDist(distance);
-
-        double hoodAngle = getAngleByDist(distance);
-
-
-//        hoodServo.setPosition(hoodAngle);
-
-        moveTurret(getTurretPosByDeltaPose(deltaPose, offset));
-
-        return distance;
-
-
-
-
-
-        //0.9974 * 355
-
-
-    }
-
-    public double getTurretPosByDeltaPose (Pose2d dPose, double offset){
-
-        double deltaAngle = Math.toDegrees(dPose.heading.toDouble());
-
-
-        double aTanAngle = Math.toDegrees(Math.atan(dPose.position.y/dPose.position.x));
-
-        telemetry.addData("deltaAngle", deltaAngle);
-
-
-
-        if (deltaAngle > 90) {
-            deltaAngle -=360;
-        }
-
-
-
-//        deltaAngle += aTanAngle;
-
-        deltaAngle /= (335);
-
-        telemetry.addData("dAngle", deltaAngle);
-
-        telemetry.addData("AtanAngle", aTanAngle);
-
-
-        return ((0.30-deltaAngle) + offset);
-
-
-
-    }
-
-    //62, 0.44
-
-    //56.5, 0.5
-
-
-    public double getPowerByDist(double dist){
-
-        //TODO: ADD LOGIC
-        return  dist;
-    }
-
-    public double getAngleByDist(double dist){
-
-
-        double newDist = dist - 56.5;
-
-        double pos = newDist*((0.44-0.5)/(62-56.5)) + 0.46;
-
-
-
-
-        return  pos;
-    }
-
-
-
-
-    public void setTelemetryOn(boolean state){telemetryOn = state;}
-
-    public void moveTurret(double pos){
-        turret1.setPosition(pos);
-        turret2.setPosition(1-pos);
-    }
-
-    @Override
-    public void update() {
-
-        if (Objects.equals(shooterMode, "MANUAL")){
-            fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-            fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-
-
-            fly1.setPower(manualPower);
-            fly2.setPower(manualPower);
-        }
-
-        else if (Objects.equals(shooterMode, "VEL")){
-
-            fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-            fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-
-            fly1.setVelocity(velocity);
-
-            fly2.setPower(fly1.getPower());
-
-
-
-
-
-
-
-        }
-
-        else if (Objects.equals(shooterMode, "POS")){
-
-
-            double powPID = controller.calculate(getPosition(), targetPosition);
-
-
-
-            fly1.setPower(powPID);
-            fly2.setPower(powPID);
-        }
-
-        if (Objects.equals(turretMode, "MANUAL")){
-//            hoodServo.setPosition(hoodPos);
-
-            moveTurret(turretPos);
-
-
-        }
-
-
-
-        if (telemetryOn) {telemetryUpdate();}
-
-
-
-    }
-}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java
@@ -1,257 +0,0 @@
-package org.firstinspires.ftc.teamcode.subsystems;
-
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
-
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.qualcomm.robotcore.hardware.AnalogInput;
-import com.qualcomm.robotcore.hardware.DigitalChannel;
-import com.qualcomm.robotcore.hardware.Servo;
-
-import org.firstinspires.ftc.teamcode.utils.Robot;
-
-import java.util.ArrayList;
-
-public class Spindexer implements Subsystem{
-
-    private Servo s1;
-    private Servo s2;
-
-    private DigitalChannel p0;
-
-    private DigitalChannel p1;
-    private DigitalChannel p2;
-    private DigitalChannel p3;
-    private DigitalChannel p4;
-
-    private DigitalChannel p5;
-
-    private AnalogInput input;
-
-    private AnalogInput input2;
-
-
-    private MultipleTelemetry TELE;
-
-    private double position = 0.501;
-
-    private boolean telemetryOn = false;
-
-    private boolean ball0 = false;
-
-    private boolean ball1 = false;
-
-    private boolean ball2 = false;
-
-    private boolean green0 = false;
-
-    private boolean green1 = false;
-
-    private boolean green2 = false;
-
-
-
-
-    public Spindexer (Robot robot, MultipleTelemetry tele){
-
-        this.s1 = robot.spin1;
-        this.s2 = robot.spin2;
-
-        this.p0 = robot.pin0;
-        this.p1 = robot.pin1;
-        this.p2 = robot.pin2;
-        this.p3 = robot.pin3;
-        this.p4 = robot.pin4;
-        this.p5 = robot.pin5;
-
-        this.input = robot.analogInput;
-
-        this.input2 = robot.analogInput2;
-
-        this.TELE = tele;
-
-    }
-
-    public void setTelemetryOn(boolean state){
-        telemetryOn = state;
-    }
-
-    public void colorSensorTelemetry() {
-
-
-        TELE.addData("ball0", ball0);
-        TELE.addData("ball1", ball1);
-        TELE.addData("ball2", ball2);
-        TELE.addData("green0", green0);
-        TELE.addData("green1", green1);
-        TELE.addData("green2", green2);
-
-
-
-    }
-
-    public void checkForBalls() {
-
-        if (p0.getState()){
-            ball0 = true;
-            green0 = p1.getState();
-        } else {
-            ball0 = false;
-        }
-
-        if (p2.getState()){
-            ball1 = true;
-            green1 = p3.getState();
-        } else {
-            ball1 = false;
-        }
-
-        if (p4.getState()){
-            ball2 = true;
-            green2 = p5.getState();
-        } else {
-            ball2 = false;
-        }
-    }
-
-    public void setPosition (double pos) {
-        position = pos;
-    }
-
-    public void intake () {
-        position = spindexer_intakePos;
-    }
-
-    public void intakeShake(double runtime) {
-        if ((runtime % 0.25) >0.125) {
-            position = spindexer_intakePos + 0.04;
-        } else {
-            position = spindexer_intakePos - 0.04;
-
-        }
-    }
-
-    public void outtake3Shake(double runtime) {
-        if ((runtime % 0.25) >0.125) {
-            position = spindexer_outtakeBall3 + 0.04;
-        } else {
-            position = spindexer_outtakeBall3 - 0.04;
-
-        }
-    }
-
-
-    public void outtake3 () {
-        position = spindexer_outtakeBall3;
-    }
-
-    public void outtake2 () {
-        position = spindexer_outtakeBall2;
-    }
-
-    public void outtake1 () {
-        position = spindexer_outtakeBall1;
-    }
-
-
-    public int outtakeGreen(int secLast, int Last) {
-        if (green2 && (secLast!=3) && (Last!=3)) {
-            outtake3();
-            return 3;
-        } else if (green1 && (secLast!=2) && (Last!=2)){
-            outtake2();
-            return 2;
-        } else if (green0 && (secLast!=1) && (Last!=1)) {
-            outtake1();
-            return 1;
-        } else {
-
-            if (secLast!=1 && Last!= 1){
-                outtake1();
-                return 1;
-            } else if (secLast!=2 && Last!=2){
-                outtake2();
-                return 2;
-            } else {
-                outtake3();
-                return 3;
-            }
-
-        }
-    }
-
-
-
-    public void outtakeGreenFs() {
-        if (green0 && ball0) {
-            outtake1();
-        } else if (green1 && ball1){
-            outtake2();
-        } else if (green2 && ball2) {
-            outtake3();
-        }
-    }
-
-    public int greens() {
-        int num = 0;
-
-        if (green0){num++;}
-
-        if (green1){num++;}
-
-
-        if (green2){num++;}
-
-        return num;
-
-
-    }
-
-
-    public int outtakePurple(int secLast, int Last) {
-        if (!green2 && (secLast!=3) && (Last!=3)) {
-            outtake3();
-            return 3;
-        } else if (!green1 && (secLast!=2) && (Last!=2)){
-            outtake2();
-            return 2;
-        } else if (!green0 && (secLast!=1) && (Last!=1)) {
-            outtake1();
-            return 1;
-        } else {
-
-            if (secLast!=1 && Last!= 1){
-                outtake1();
-                return 1;
-            } else if (secLast!=2 && Last!=2){
-                outtake2();
-                return 2;
-            } else {
-                outtake3();
-                return 3;
-            }
-
-
-        }
-    }
-
-
-
-
-    @Override
-    public void update() {
-
-        if (position !=0.501) {
-
-            s1.setPosition(position);
-            s2.setPosition(1 - position);
-        }
-
-
-        if (telemetryOn) {
-            colorSensorTelemetry();
-        }
-
-
-
-    }
-}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Subsystem.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Subsystem.java
@@ -1,6 +0,0 @@
-package org.firstinspires.ftc.teamcode.subsystems;
-
-public interface Subsystem {
-
-    public void update();
-}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java
@@ -1,58 +0,0 @@
-package org.firstinspires.ftc.teamcode.subsystems;
-
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
-
-import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.Servo;
-
-import org.firstinspires.ftc.teamcode.utils.Robot;
-
-public class Transfer implements Subsystem{
-
-    private final Servo servo;
-
-    private final DcMotorEx transfer;
-
-    private double motorPow = 0.0;
-
-    private double servoPos = 0.501;
-
-    public Transfer (Robot robot){
-
-        this.servo = robot.transferServo;
-
-        this.transfer = robot.transfer;
-
-    }
-
-    public void setTransferPosition(double pos){
-        this.servoPos  = pos;
-    }
-
-    public void setTransferPower (double pow){
-        this.motorPow = pow;
-    }
-
-    public void transferOut(){
-        this.setTransferPosition(transferServo_out);
-    }
-
-    public void transferIn(){
-        this.setTransferPosition(transferServo_in);
-    }
-
-
-
-
-
-    @Override
-    public void update() {
-
-        if (servoPos!=0.501){
-            servo.setPosition(servoPos);
-        }
-
-        transfer.setPower(motorPow);
-
-    }
-}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java
+++ b/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++;
+
+        }
+    }
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TransferTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TransferTest.java
@@ -1,35 +0,0 @@
-package org.firstinspires.ftc.teamcode.teleop;
-
-
-import com.acmerobotics.dashboard.config.Config;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-
-import org.firstinspires.ftc.teamcode.subsystems.Transfer;
-import org.firstinspires.ftc.teamcode.utils.Robot;
-
-@Config
-@TeleOp
-public class TransferTest extends LinearOpMode {
-
-    Robot robot;
-
-
-    Transfer transfer;
-
-    public static double pos = 0.40;
-    @Override
-    public void runOpMode() throws InterruptedException {
-
-        robot = new Robot(hardwareMap);
-
-        transfer = new Transfer(robot);
-
-        waitForStart();
-
-        while (opModeIsActive()){
-            transfer.setTransferPosition(pos);
-        }
-
-    }
-}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/WebcamTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/WebcamTest.java
@@ -1,59 +0,0 @@
-package org.firstinspires.ftc.teamcode.teleop;
-
-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;
-import org.firstinspires.ftc.teamcode.subsystems.AprilTag;
-
-
-@TeleOp
-@Config
-public class WebcamTest extends LinearOpMode {
-
-    AprilTag webcam;
-
-    MultipleTelemetry TELE;
-
-    Robot robot;
-
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-
-        robot = new Robot(hardwareMap);
-
-        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
-
-        webcam = new AprilTag(robot, TELE);
-
-        webcam.turnTelemetryOn(true);
-
-
-
-
-        while(opModeInInit()){
-
-            webcam.initTelemetry();
-
-            TELE.update();
-
-        };
-
-        if(isStopRequested()) return;
-
-
-        while (opModeIsActive()){
-
-            webcam.update();
-
-            TELE.update();
-
-        }
-
-
-    }
-}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java
@@ -1,7 +1,7 @@
 package org.firstinspires.ftc.teamcode.teleop;

-import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodDefault;
+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;
@@ -10,6 +10,7 @@ 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;

@@ -22,12 +23,10 @@ import org.firstinspires.ftc.teamcode.subsystems.Spindexer;
 import org.firstinspires.ftc.teamcode.subsystems.Transfer;
 import org.firstinspires.ftc.teamcode.utils.Robot;

-
@Config
@TeleOp
-
-public class TeleopV1 extends LinearOpMode {
-
+@Disabled
+public class old extends LinearOpMode {

    Robot robot;

@@ -63,7 +62,6 @@ public class TeleopV1 extends LinearOpMode {

    ToggleButtonReader g2Square;

-
    ToggleButtonReader g2Triangle;

    ToggleButtonReader g2RightBumper;
@@ -85,7 +83,6 @@ public class TeleopV1 extends LinearOpMode {

    public double g1LeftBumperStamp = 0.0;

-
    public double g2LeftBumperStamp = 0.0;

    public static int spindexerPos = 0;
@@ -96,7 +93,7 @@ public class TeleopV1 extends LinearOpMode {

    public boolean scoreAll = false;

-    MecanumDrive drive ;
+    MecanumDrive drive;

    public boolean autotrack = false;

@@ -117,17 +114,11 @@ public class TeleopV1 extends LinearOpMode {

    public boolean tri = false;

-
-
    @Override
    public void runOpMode() throws InterruptedException {

        drive = new MecanumDrive(hardwareMap, teleStart);

-
-
-
-
        robot = new Robot(hardwareMap);

        TELE = new MultipleTelemetry(
@@ -163,7 +154,6 @@ public class TeleopV1 extends LinearOpMode {
                g2, GamepadKeys.Button.RIGHT_BUMPER
        );

-
        g2LeftBumper = new ToggleButtonReader(
                g2, GamepadKeys.Button.LEFT_BUMPER
        );
@@ -172,7 +162,6 @@ public class TeleopV1 extends LinearOpMode {
                g2, GamepadKeys.Button.DPAD_UP
        );

-
        g2DpadDown = new ToggleButtonReader(
                g2, GamepadKeys.Button.DPAD_DOWN
        );
@@ -181,14 +170,10 @@ public class TeleopV1 extends LinearOpMode {
                g2, GamepadKeys.Button.DPAD_LEFT
        );

-
        g2DpadRight = new ToggleButtonReader(
                g2, GamepadKeys.Button.DPAD_RIGHT
        );

-
-
-
        drivetrain = new Drivetrain(robot, TELE, g1);

        drivetrain.setMode("Default");
@@ -201,7 +186,6 @@ public class TeleopV1 extends LinearOpMode {

        transfer = new Transfer(robot);

-
        spindexer = new Spindexer(robot, TELE);

        spindexer.setTelemetryOn(true);
@@ -212,18 +196,13 @@ public class TeleopV1 extends LinearOpMode {

        robot.rejecter.setPosition(rIn);

-
-
-
-
        waitForStart();

        if (isStopRequested()) return;

        drive = new MecanumDrive(hardwareMap, teleStart);

-
-        while(opModeIsActive()){
+        while (opModeIsActive()) {

            drive.updatePoseEstimate();

@@ -231,28 +210,24 @@ public class TeleopV1 extends LinearOpMode {

            TELE.addData("heading", drive.localizer.getPose().heading.toDouble());

-
            TELE.addData("off", offset);

-
            robot.hood.setPosition(pos);

            g1LeftBumper.readValue();

-            if (g1LeftBumper.wasJustPressed()){
+            if (g1LeftBumper.wasJustPressed()) {
                g2LeftBumperStamp = getRuntime();

-
-
                spindexer.intakeShake(getRuntime());

                leftBumper = true;
            }

-            if (leftBumper){
+            if (leftBumper) {
                double time = getRuntime() - g2LeftBumperStamp;

-                if (time < 1.0){
+                if (time < 1.0) {
                    robot.rejecter.setPosition(rOut);
                } else {
                    robot.rejecter.setPosition(rIn);
@@ -260,9 +235,6 @@ public class TeleopV1 extends LinearOpMode {

            }

-
-
-
            intake();

            drivetrain.update();
@@ -279,63 +251,49 @@ public class TeleopV1 extends LinearOpMode {

            g2DpadUp.readValue();

-            if (!scoreAll){
+            if (!scoreAll) {
                spindexer.checkForBalls();
            }

-            if(g2DpadUp.wasJustPressed()){
-                pos -=0.02;
+            if (g2DpadUp.wasJustPressed()) {
+                pos -= 0.02;
            }

-            if(g2DpadDown.wasJustPressed()){
-                pos +=0.02;
+            if (g2DpadDown.wasJustPressed()) {
+                pos += 0.02;
            }

            g2DpadLeft.readValue();

            g2DpadRight.readValue();

-            if(g2DpadLeft.wasJustPressed()){
-                offset -=0.02;
+            if (g2DpadLeft.wasJustPressed()) {
+                offset -= 0.02;
            }

-            if(g2DpadRight.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);
+                shooter.moveTurret(0.3 + offset);

            }

-            if (gamepad2.right_stick_button){
+            if (gamepad2.right_stick_button) {
                autotrack = true;
            }

-
-
-            if (g2RightBumper.wasJustPressed()){
+            if (g2RightBumper.wasJustPressed()) {
                transfer.setTransferPower(1);
                transfer.transferIn();
                shooter.setManualPower(1);
@@ -344,19 +302,19 @@ public class TeleopV1 extends LinearOpMode {

            }

-            if (g2RightBumper.wasJustReleased()){
+            if (g2RightBumper.wasJustReleased()) {
                transfer.setTransferPower(1);
                transfer.transferOut();
            }

-            if (gamepad2.left_stick_y>0.5){
+            if (gamepad2.left_stick_y > 0.5) {

                shooter.setManualPower(0);
-            } else if (gamepad2.left_stick_y<-0.5){
+            } else if (gamepad2.left_stick_y < -0.5) {
                shooter.setManualPower(1);
            }

-            if (g2LeftBumper.wasJustPressed()){
+            if (g2LeftBumper.wasJustPressed()) {
                g2LeftBumperStamp = getRuntime();
                notShooting = false;
                scoreAll = true;
@@ -365,7 +323,6 @@ public class TeleopV1 extends LinearOpMode {
            if (scoreAll) {
                double time = getRuntime() - g2LeftBumperStamp;

-
                shooter.setManualPower(1);

                TELE.addData("greenImportant", green);
@@ -377,7 +334,6 @@ public class TeleopV1 extends LinearOpMode {

                if (square) {

-
                    if (time < 0.3) {

                        ticker = 0;
@@ -385,7 +341,6 @@ public class TeleopV1 extends LinearOpMode {
                        last = 0;
                        second = 0;

-
                        transfer.transferOut();
                        transfer.setTransferPower(1);
                    } else if (time < 2) {
@@ -406,14 +361,12 @@ public class TeleopV1 extends LinearOpMode {

                        ticker++;

-
                    } else if (time < 2.5) {

                        ticker = 0;

                        second = last;

-
                        transfer.transferIn();
                    } else if (time < 4) {
                        transfer.transferOut();
@@ -433,11 +386,9 @@ public class TeleopV1 extends LinearOpMode {

                        ticker = 0;

-
                        transfer.transferIn();
                    } else if (time < 6) {

-
                        transfer.transferOut();

                        if (ticker == 0) {
@@ -458,7 +409,6 @@ public class TeleopV1 extends LinearOpMode {

                        ticker = 0;

-
                        scoreAll = false;
                        transfer.transferOut();

@@ -467,7 +417,6 @@ public class TeleopV1 extends LinearOpMode {
                    }
                } else if (tri) {

-
                    if (time < 0.3) {

                        ticker = 0;
@@ -475,7 +424,6 @@ public class TeleopV1 extends LinearOpMode {
                        last = 0;
                        second = 0;

-
                        transfer.transferOut();
                        transfer.setTransferPower(1);
                    } else if (time < 2) {
@@ -496,14 +444,12 @@ public class TeleopV1 extends LinearOpMode {

                        ticker++;

-
                    } else if (time < 2.5) {

                        ticker = 0;

                        second = last;

-
                        transfer.transferIn();
                    } else if (time < 4) {
                        transfer.transferOut();
@@ -523,11 +469,9 @@ public class TeleopV1 extends LinearOpMode {

                        ticker = 0;

-
                        transfer.transferIn();
                    } else if (time < 6) {

-
                        transfer.transferOut();

                        if (ticker == 0) {
@@ -548,15 +492,13 @@ public class TeleopV1 extends LinearOpMode {

                        ticker = 0;

-
                        scoreAll = false;
                        transfer.transferOut();

                        shooter.setManualPower(0);

                    }
-                } else if (circle){
-
+                } else if (circle) {

                    if (time < 0.3) {

@@ -565,7 +507,6 @@ public class TeleopV1 extends LinearOpMode {
                        last = 0;
                        second = 0;

-
                        transfer.transferOut();
                        transfer.setTransferPower(1);
                    } else if (time < 2) {
@@ -586,14 +527,12 @@ public class TeleopV1 extends LinearOpMode {

                        ticker++;

-
                    } else if (time < 2.5) {

                        ticker = 0;

                        second = last;

-
                        transfer.transferIn();
                    } else if (time < 4) {
                        transfer.transferOut();
@@ -613,11 +552,9 @@ public class TeleopV1 extends LinearOpMode {

                        ticker = 0;

-
                        transfer.transferIn();
                    } else if (time < 6) {

-
                        transfer.transferOut();

                        if (ticker == 0) {
@@ -638,15 +575,13 @@ public class TeleopV1 extends LinearOpMode {

                        ticker = 0;

-
                        scoreAll = false;
                        transfer.transferOut();

                        shooter.setManualPower(0);

                    }
-                } else{
-
+                } else {

                    if (time < 0.3) {

@@ -668,7 +603,6 @@ public class TeleopV1 extends LinearOpMode {
                            all = true;
                        }

-
                        transfer.transferOut();
                        transfer.setTransferPower(1);
                    } else if (time < 2) {
@@ -693,7 +627,6 @@ public class TeleopV1 extends LinearOpMode {

                        ticker++;

-
                    } else if (time < 2.5) {

                        ticker = 0;
@@ -710,7 +643,6 @@ public class TeleopV1 extends LinearOpMode {

                            all = false;

-
                        }

                        transfer.transferIn();
@@ -736,7 +668,6 @@ public class TeleopV1 extends LinearOpMode {

                        ticker = 0;

-
                        if (gamepad2.right_trigger > 0.5) {
                            green = false;

@@ -751,7 +682,6 @@ public class TeleopV1 extends LinearOpMode {
                        transfer.transferIn();
                    } else if (time < 6) {

-
                        transfer.transferOut();

                        if (ticker == 0) {
@@ -774,7 +704,6 @@ public class TeleopV1 extends LinearOpMode {

                        ticker = 0;

-
                        scoreAll = false;
                        transfer.transferOut();

@@ -782,33 +711,16 @@ public class TeleopV1 extends LinearOpMode {

                    }

-
                }
            }

-
            shooter.update();

-
-
-
-
-
-
-
-
-
-
+        }

    }

-
-
-
-    }
-
-    public void intake(){
-
+    public void intake() {

        g1RightBumper.readValue();

@@ -818,84 +730,64 @@ public class TeleopV1 extends LinearOpMode {

        g2Triangle.readValue();

-        if (g1RightBumper.wasJustPressed()){
+        if (g1RightBumper.wasJustPressed()) {

            notShooting = true;

-
-
-
-            if (getRuntime() - g1RightBumperStamp < 0.3){
+            if (getRuntime() - g1RightBumperStamp < 0.3) {
                intake.reverse();
            } else {
                intake.toggle();
            }

-            if (intake.getIntakeState()==1){
+            if (intake.getIntakeState() == 1) {
                shooter.setManualPower(0);
            }

-
-
-
            spindexer.intake();

            transfer.transferOut();

-
            g1RightBumperStamp = getRuntime();

        }

-
-        if (intake.getIntakeState()==1 && notShooting) {
+        if (intake.getIntakeState() == 1 && notShooting) {

            spindexer.intakeShake(getRuntime());

        } else {
-            if (g2Circle.wasJustPressed()){
+            if (g2Circle.wasJustPressed()) {
                circle = true;
                tri = false;
                square = false;

-
-
            }

-            if (g2Triangle.wasJustPressed()){
+            if (g2Triangle.wasJustPressed()) {
                circle = false;
                tri = true;
                square = false;
            }

-            if (g2Square.wasJustPressed()){
+            if (g2Square.wasJustPressed()) {
                circle = false;
                tri = false;
                square = true;
            }

-            if (gamepad2.x){
+            if (gamepad2.x) {
                circle = false;
                tri = false;
                square = false;
            }

-
-
-
        }

-
        intake.update();

-
-
-
        spindexer.update();

-
-
-
-
    }
 }
+
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.txt
+++ b/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();
+        }
+    }
+
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/AprilTagWebcamExample.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/AprilTagWebcamExample.java
@@ -0,0 +1,37 @@
+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.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+@Config
+@TeleOp
+public class AprilTagWebcamExample extends OpMode {
+
+    MultipleTelemetry TELE;
+    AprilTagWebcam aprilTagWebcam = new AprilTagWebcam();
+
+    @Override
+    public void init() {
+        TELE = new MultipleTelemetry(
+                telemetry, FtcDashboard.getInstance().getTelemetry()
+        );
+
+        aprilTagWebcam.init(new Robot(hardwareMap), TELE);
+
+    }
+
+    @Override
+    public void loop() {
+
+        aprilTagWebcam.update();
+        aprilTagWebcam.displayAllTelemetry();
+        TELE.update();
+
+    }
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.txt
+++ b/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();
+        }
+    }
+
+
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java
@@ -3,94 +3,108 @@ 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.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.hardware.DcMotorEx;

 import org.firstinspires.ftc.teamcode.utils.Robot;
-import org.firstinspires.ftc.teamcode.subsystems.Shooter;

-
-@TeleOp
@Config
+@TeleOp
 public class ShooterTest extends LinearOpMode {

+    public static int mode = 0;
+    public static double parameter = 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;

-    public static double pow = 0.0;
-    public static double vel = 0.0;
-
-    public static int pos = 0;
-    public static double posPower = 0.0;
-
-    public static double p = 0.000003, i = 0, d = 0.000001;
-
-
-    public static String flyMode = "MANUAL";
-
-    public static String turrMode = "MANUAL";
-
-    public static int posTolerance = 40;
-
-    public static double servoPosition = 0.501;
-
-    MultipleTelemetry TELE;
    @Override
    public void runOpMode() throws InterruptedException {

        robot = new Robot(hardwareMap);
+        DcMotorEx leftShooter = robot.shooter1;
+        DcMotorEx rightShooter = robot.shooter2;
+        DcMotorEx encoder = robot.shooter1;

-        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
-
-        Shooter shooter = new Shooter(robot, TELE);
-
-        shooter.setTelemetryOn(true);
-
-        shooter.setTurretMode(turrMode);
-
-        shooter.setShooterMode(flyMode);
-
-
-        shooter.setControllerCoefficients(p, i, d);
-
+        MultipleTelemetry TELE = new MultipleTelemetry(
+                telemetry, FtcDashboard.getInstance().getTelemetry()
+        );

        waitForStart();

-        if(isStopRequested()) return;
+        if (isStopRequested()) return;

+        while (opModeIsActive()) {

-        while(opModeIsActive()){
+            double kF = 1.0 / MAX_RPM;     // baseline feedforward

-            shooter.setControllerCoefficients(p, i, d);
+            double encoderRevolutions = (double) encoder.getCurrentPosition() / 2048;

-            shooter.setTurretMode(turrMode);
+            double velocity = -60 * (encoderRevolutions - lastEncoderRevolutions) / (getRuntime() - lastTimeStamp);

-            shooter.setShooterMode(flyMode);
+            TELE.addLine("Mode: 0 = Manual, 1 = Vel, 2 = Pos");
+            TELE.addLine("Parameter = pow, vel, or pos");
+            TELE.addData("leftShooterPower", leftShooter.getPower());
+            TELE.addData("rightShooterPower", rightShooter.getPower());
+            TELE.addData("shaftEncoderPos", encoderRevolutions);
+            TELE.addData("shaftEncoderVel", velocity);

-            shooter.setManualPower(pow);
+            double velPID;

-            shooter.setVelocity(vel);
+            if (mode == 0) {
+                rightShooter.setPower(parameter);
+                leftShooter.setPower(parameter);
+            } else if (mode == 1) {

-            shooter.setTargetPosition(pos);
+                // --- FEEDFORWARD BASE POWER ---
+                double feed = kF * parameter;        // Example: vel=2500 → feed=0.5

-            shooter.setTolerance(posTolerance);
+                // --- PROPORTIONAL CORRECTION ---
+                double error = parameter - velocity;
+                double correction = kP * error;

-            shooter.setPosPower(posPower);
+                // limit how fast power changes (prevents oscillation)
+                correction = Math.max(-maxStep, Math.min(maxStep, correction));

-            if (servoPosition!=0.501) {shooter.sethoodPosition(servoPosition);}
+                // --- FINAL MOTOR POWER ---
+                velPID = feed + correction;

+                // clamp to allowed range
+                velPID = Math.max(0, Math.min(1, velPID));

-            shooter.update();
+                rightShooter.setPower(velPID);
+                leftShooter.setPower(velPID);
+
+            }
+
+            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();

-
-
        }

-
-
    }
 }
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ed.txt
+++ b/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;
+    }
+
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/AprilTagWebcam.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/AprilTagWebcam.java
@@ -0,0 +1,104 @@
+package org.firstinspires.ftc.teamcode.utils;
+
+import android.util.Size;
+
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
+import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class AprilTagWebcam {
+
+    private AprilTagProcessor aprilTagProcessor;
+    private VisionPortal visionPortal;
+    private List<AprilTagDetection> detectedTags = new ArrayList<>();
+    private MultipleTelemetry telemetry;
+    public void init(Robot robot, MultipleTelemetry TELE){
+
+        this.telemetry = TELE;
+
+        aprilTagProcessor = new AprilTagProcessor.Builder()
+                .setDrawTagID(true)
+                .setDrawTagOutline(true)
+                .setDrawAxes(true)
+                .setDrawCubeProjection(true)
+                .setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES)
+                .build();
+
+        VisionPortal.Builder builder = new VisionPortal.Builder();
+        builder.setCamera(robot.webcam);
+        builder.setCameraResolution(new Size(640, 480));
+        builder.addProcessor(aprilTagProcessor);
+
+        visionPortal = builder.build();
+
+    }
+
+    public void update() {
+        detectedTags = aprilTagProcessor.getDetections();
+    }
+
+    public List<AprilTagDetection> getDetectedTags() {
+        return detectedTags;
+    }
+
+    public AprilTagDetection getTagById(int id){
+        for (AprilTagDetection detection : detectedTags) {
+            if (detection.id ==id){
+                return detection;
+            }
+        }
+        return null;
+    }
+
+    public void stop(){
+        if (visionPortal != null){
+            visionPortal.close();
+        }
+    }
+
+    //Helper Functions
+
+    public void displayDetectionTelemetry (AprilTagDetection detectedId){
+        if (detectedId ==null){return;}
+
+        if (detectedId.metadata != null) {
+            telemetry.addLine(String.format("\n==== (ID %d) %s", detectedId.id, detectedId.metadata.name));
+            telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f  (inch)", detectedId.ftcPose.x, detectedId.ftcPose.y, detectedId.ftcPose.z));
+            telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f  (deg)", detectedId.ftcPose.pitch, detectedId.ftcPose.roll, detectedId.ftcPose.yaw));
+            telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f  (inch, deg, deg)", detectedId.ftcPose.range, detectedId.ftcPose.bearing, detectedId.ftcPose.elevation));
+        } else {
+            telemetry.addLine(String.format("\n==== (ID %d) Unknown", detectedId.id));
+            telemetry.addLine(String.format("Center %6.0f %6.0f   (pixels)", detectedId.center.x, detectedId.center.y));
+        }
+    }
+
+    public void displayAllTelemetry (){
+        if (detectedTags ==null){return;}
+
+        telemetry.addData("# AprilTags Detected", detectedTags.size());
+
+
+        for (AprilTagDetection detection : detectedTags) {
+            if (detection.metadata != null) {
+                telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name));
+                telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f  (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z));
+                telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f  (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw));
+                telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f  (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation));
+            } else {
+                telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id));
+                telemetry.addLine(String.format("Center %6.0f %6.0f   (pixels)", detection.center.x, detection.center.y));
+            }
+        }   // end for() loop
+    }
+
+
+
+
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java
+++ b/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");
    }
 }
Author	SHA1	Message	Date
KeshavAnandCode	3440ff1783	oops...hehe	2025-11-30 19:34:12 -06:00
KeshavAnandCode	0c3fd6fc83	s	2025-11-30 19:22:25 -06:00
KeshavAnandCode	8686b79314	ok	2025-11-30 19:20:44 -06:00
KeshavAnandCode	03ae41c19b	yES	2025-11-30 18:16:08 -06:00
KeshavAnandCode	e04c5fa830	stash	2025-11-30 17:31:37 -06:00
KeshavAnandCode	f9a220bf51	daniel files added	2025-11-30 16:59:23 -06:00
KeshavAnandCode	4b96775161	Telelop drivetrain	2025-11-30 16:55:20 -06:00
KeshavAnandCode	9a884885a9	ehh	2025-11-30 16:47:30 -06:00
KeshavAnandCode	36ac31b3ec	Auto track implemented with tunable constants	2025-11-26 22:58:31 -06:00
KeshavAnandCode	a1585e605f	Shooter Test	2025-11-25 15:54:15 -06:00
KeshavAnandCode	894a8d26fb	Deleted files + Drivetrain.java	2025-11-24 17:19:18 -06:00
KeshavAnandCode	09d82c1e02	Merge branch 'master-backup' # Conflicts: # TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md # TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java	2025-11-24 17:10:21 -06:00
KeshavAnandCode	62b6d1cf81	Fix PID velocity	2025-11-24 17:01:02 -06:00
KeshavAnandCode	c7f9028011	Added April Tag Class and Working Example	2025-11-18 20:22:55 -06:00
KeshavAnandCode	6b17bd4d46	test	2025-11-18 19:16:32 -06:00
KeshavAnandCode	695361e95c	Hold code added	2025-11-11 21:30:14 -06:00