here you go keshav

# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue.java

@@ -170,16 +170,16 @@ public class Blue extends LinearOpMode {
         while(opModeInInit()) {
 
             if (gamepad2.dpadUpWasPressed()){
-                hoodDefault -= 0.02;
+                hoodDefault -= 0.01;
             }
 
             if (gamepad2.dpadDownWasPressed()){
-                hoodDefault += 0.02;
+                hoodDefault += 0.01;
             }
 
             robot.hood.setPosition(hoodDefault);
 
-            shooter.setTurretPosition(0.3);
+            shooter.setTurretPosition(turret_blue);
 
             aprilTag.initTelemetry();
 
@@ -226,7 +226,7 @@ public class Blue extends LinearOpMode {
             double stamp = getRuntime();
 
 
-             stamp = getRuntime();
+            stamp = getRuntime();
 
             while (getRuntime()-stamp<4.5) {
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java

@@ -99,45 +99,45 @@ public class Red extends LinearOpMode {
 
 
 
-        TrajectoryActionBuilder traj1 = drive.actionBuilder(new Pose2d(0, 0, 0))
+        TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
                 .strafeToLinearHeading(new Vector2d(x1, y1), h1 );
 
 
-        TrajectoryActionBuilder traj2 = drive.actionBuilder(new Pose2d(x1, y1, h1))
+        TrajectoryActionBuilder pickup1 = 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))
+        TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(x2, y2, h2))
                 .strafeToLinearHeading(new Vector2d(x1, y1), h1 );
 
-        TrajectoryActionBuilder traj4 = drive.actionBuilder(new Pose2d(x1, y1, h1))
+        TrajectoryActionBuilder pickup2 = 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))
+        TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(x3, y3, h3))
                 .strafeToLinearHeading(new Vector2d(x1, y1), h1 );
 
 
-        TrajectoryActionBuilder traj6 = drive.actionBuilder(new Pose2d(x1, y1, h1))
+        TrajectoryActionBuilder park = drive.actionBuilder(new Pose2d(x1, y1, h1))
                 .strafeToLinearHeading(new Vector2d(x1, y1+30), h1 );
 
         while(opModeInInit()) {
 
             if (gamepad2.dpadUpWasPressed()){
-                hoodDefault -= 0.02;
+                hoodDefault -= 0.01;
             }
 
             if (gamepad2.dpadDownWasPressed()){
-                hoodDefault += 0.02;
+                hoodDefault += 0.01;
             }
 
             robot.hood.setPosition(hoodDefault);
 
-            shooter.setTurretPosition(0.33);
+            shooter.setTurretPosition(turret_red);
 
             aprilTag.initTelemetry();
 
@@ -165,7 +165,7 @@ public class Red extends LinearOpMode {
 
             Actions.runBlocking(
                     new ParallelAction(
-                            traj1.build()
+                            shoot0.build()
                     )
             );
 
@@ -179,7 +179,7 @@ public class Red extends LinearOpMode {
             double stamp = getRuntime();
 
 
-             stamp = getRuntime();
+            stamp = getRuntime();
 
             while (getRuntime()-stamp<4.5) {
 
@@ -245,12 +245,12 @@ public class Red extends LinearOpMode {
             robot.intake.setPower(1);
 
             Actions.runBlocking(
-                    traj2.build()
+                    pickup1.build()
             );
 
 
             Actions.runBlocking(
-                    traj3.build()
+                    shoot1.build()
             );
 
             shooter.setManualPower(1);
@@ -329,12 +329,12 @@ public class Red extends LinearOpMode {
             robot.intake.setPower(1);
 
             Actions.runBlocking(
-                    traj4.build()
+                    pickup2.build()
             );
 
 
             Actions.runBlocking(
-                    traj5.build()
+                    shoot2.build()
             );
 
             shooter.setManualPower(1);
@@ -411,7 +411,7 @@ public class Red extends LinearOpMode {
             spindexer.outtake3();
 
             Actions.runBlocking(
-                    traj6.build()
+                    park.build()
             );
 
             drive.updatePoseEstimate();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/redDaniel.java

@@ -0,0 +1,298 @@
+package org.firstinspires.ftc.teamcode.autonomous;
+
+import static org.firstinspires.ftc.teamcode.constants.Poses.*;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
+import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
+import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.*;
+
+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.ParallelAction;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
+import com.acmerobotics.roadrunner.Vector2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.acmerobotics.roadrunner.Action;
+import com.acmerobotics.roadrunner.ftc.Actions;
+
+import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystems.AprilTag;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+@Config
+@Autonomous
+public class redDaniel extends LinearOpMode {
+
+    Robot robot;
+
+    MultipleTelemetry TELE;
+
+    MecanumDrive drive;
+
+    AprilTag aprilTag;
+
+    int b1 = 0; // 0 = no ball, 1 = green, 2 = purple
+
+    int b2 = 0;// 0 = no ball, 1 = green, 2 = purple
+
+    int b3 = 0;// 0 = no ball, 1 = green, 2 = purple
+    // TODO: change this velocity PID
+    public Action initShooter(int velocity){
+        return new Action(){
+            double velo = 0.0;
+            double initPos = 0.0;
+            double stamp = 0.0;
+            double powPID = 0.0;
+            double ticker = 0.0;
+            public boolean run(@NonNull TelemetryPacket telemetryPacket){
+                velo = -60 * ((((double) robot.shooter1.getCurrentPosition() / 2048) - initPos) / (getRuntime() - stamp));
+                stamp = getRuntime();
+                initPos = (double) robot.shooter1.getCurrentPosition() / 2048;
+                if (Math.abs(velocity - velo) > initTolerance) {
+                    powPID = (double) velocity / maxVel;
+                    ticker = getRuntime();
+                } else if (velocity - velTolerance > velo) {
+                    powPID = powPID + 0.0001;
+                    ticker = getRuntime();
+                } else if (velocity + velTolerance < velo) {
+                    powPID = powPID - 0.0001;
+                    ticker  = getRuntime();
+                }
+                robot.shooter1.setPower(powPID);
+                robot.shooter2.setPower(powPID);
+                robot.transfer.setPower((powPID / 4) + 0.75);
+
+                return getRuntime() - ticker < 0.5;
+            }
+        };
+    }
+
+    public void Obelisk (){
+        // TODO: write the code to detect order
+    }
+
+    public Action Shoot(double spindexer){
+        return new Action() {
+            boolean transfer = false;
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                robot.spin1.setPosition(spindexer);
+                robot.spin2.setPosition(1-spindexer);
+                if (scalar*((robot.spin1Pos.getVoltage() - restPos) / 3.3) < spindexer + 0.01 && scalar*((robot.spin1Pos.getVoltage() - restPos) / 3.3) > spindexer - 0.01){
+                    robot.transferServo.setPosition(transferServo_in);
+                    transfer = true;
+                }
+                if (scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3) < transferServo_in + 0.01 && scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3) > transferServo_in - 0.01 && transfer){
+                    robot.transferServo.setPosition(transferServo_out);
+                    return false;
+                }
+                return true;
+            }
+        };
+    }
+
+    public Action intake (){
+        return new Action() {
+            double position = 0.0;
+            final double intakeTime = 4.0; // TODO: change this so it serves as a backup
+            final double stamp = getRuntime();
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                if ((getRuntime() % 0.3) >0.15) {
+                    position = spindexer_intakePos1 + 0.02;
+                } else {
+                    position = spindexer_intakePos1 - 0.02;
+                }
+                robot.spin1.setPosition(position);
+                robot.spin2.setPosition(1-position);
+
+                robot.intake.setPower(1);
+
+                return !(robot.pin1.getState() && robot.pin3.getState() && robot.pin5.getState()) || getRuntime() - stamp > intakeTime;
+            }
+        };
+    }
+
+    public Action ColorDetect (){
+        return new Action() {
+            int t1 = 0;
+            int t2 = 0;
+            int t3 = 0;
+            int tP1 = 0;
+            int tP2 = 0;
+            int tP3 = 0;
+            final double stamp = getRuntime();
+            final double detectTime = 3.0;
+            double position = 0.0;
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                if ((getRuntime() % 0.3) >0.15) {
+                    position = spindexer_intakePos1 + 0.02;
+                } else {
+                    position = spindexer_intakePos1 - 0.02;
+                }
+                robot.spin1.setPosition(position);
+                robot.spin2.setPosition(1-position);
+                if (robot.pin1.getState()) {
+                    t1 += 1;
+                    if (robot.pin0.getState()){
+                        tP1 += 1;
+                    }
+                }
+                if (robot.pin3.getState()) {
+                    t2 += 1;
+                    if (robot.pin0.getState()){
+                        tP2 += 1;
+                    }
+                }
+                if (robot.pin5.getState()) {
+                    t3 += 1;
+                    if (robot.pin0.getState()){
+                        tP3 += 1;
+                    }
+                }
+                if (t1 > 20){
+                    if (tP1 > 20){
+                        b1 = 2;
+                    } else {
+                        b1 = 1;
+                    }
+                }
+                if (t2 > 20){
+                    if (tP2 > 20){
+                        b2 = 2;
+                    } else {
+                        b2 = 1;
+                    }
+                }
+                if (t3 > 20){
+                    if (tP3 > 20){
+                        b3 = 2;
+                    } else {
+                        b3 = 1;
+                    }
+                }
+                return !(b1 + b2 + b3 >= 5) || (getRuntime() - stamp < detectTime);
+
+            }
+        };
+    }
+
+    @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);
+
+        TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
+                .strafeToLinearHeading(new Vector2d(x1, y1), h1);
+
+        TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1))
+                .turnTo(Math.toRadians(h2))
+                .strafeToLinearHeading(new Vector2d(x2, y2), h2);
+
+        TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(x2, y2, h2))
+                .strafeToLinearHeading(new Vector2d(x1, y1), h1);
+
+        TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(x1, y1, h1))
+
+                .strafeToLinearHeading(new Vector2d(x2_b, y2_b), h2_b)
+
+                .strafeToLinearHeading(new Vector2d(x3, y3), h3);
+
+        TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(x3, y3, h3))
+                .strafeToLinearHeading(new Vector2d(x1, y1), h1);
+
+        TrajectoryActionBuilder park = drive.actionBuilder(new Pose2d(x1, y1, h1))
+                .strafeToLinearHeading(new Vector2d(x1, y1 + 30), h1);
+
+        while (opModeInInit()) {
+
+            if (gamepad2.dpadUpWasPressed()) {
+                hoodDefault -= 0.01;
+            }
+
+            if (gamepad2.dpadDownWasPressed()) {
+                hoodDefault += 0.01;
+            }
+
+            robot.hood.setPosition(hoodDefault);
+
+            robot.turr1.setPosition(turret_red);
+            robot.turr2.setPosition(1 - turret_red);
+
+            robot.transferServo.setPosition(transferServo_out);
+
+            aprilTag.initTelemetry();
+
+            aprilTag.update();
+
+            TELE.update();
+        }
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        if (opModeIsActive()) {
+
+            robot.hood.setPosition(hoodDefault);
+
+            Actions.runBlocking(
+                    new ParallelAction(
+                            shoot0.build()
+                    )
+            );
+
+
+
+            Actions.runBlocking(
+                    pickup1.build()
+            );
+
+            Actions.runBlocking(
+                    shoot1.build()
+            );
+
+
+            Actions.runBlocking(
+                    pickup2.build()
+            );
+
+            Actions.runBlocking(
+                    shoot2.build()
+            );
+
+
+            Actions.runBlocking(
+                    park.build()
+            );
+
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
+
+            TELE.addLine("finished");
+
+            TELE.update();
+
+            sleep(2000);
+
+        }
+
+    }
+}

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

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

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

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

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

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

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

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

@@ -13,17 +13,17 @@ import java.util.Objects;
 
 public class Drivetrain implements Subsystem {
 
-    private GamepadEx gamepad;
+    private final GamepadEx gamepad;
 
     public MultipleTelemetry TELE;
 
     private String Mode = "Default";
 
-    private DcMotorEx fl;
+    private final DcMotorEx fl;
 
-    private DcMotorEx fr;
+    private final DcMotorEx fr;
-    private DcMotorEx bl;
+    private final DcMotorEx bl;
-    private DcMotorEx br;
+    private final DcMotorEx br;
 
     private double defaultSpeed = 0.7;
 

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

@@ -42,8 +42,8 @@ public class Intake implements Subsystem {
         }
     }
 
-    public void setIntakePower(double pow){
+    public void intakeMinPower(){
-        intakePower = pow;
+        intakeState = 2;
     }
 
     public void intake(){
@@ -56,7 +56,7 @@ public class Intake implements Subsystem {
 
 
     public void stop(){
-        intakeState =-1;
+        intakeState =0;
     }
 
 
@@ -69,9 +69,12 @@ public class Intake implements Subsystem {
             intake.setPower(intakePower);
         } else if (intakeState == -1){
             intake.setPower(-intakePower);
-        } else {
+        } else if (intakeState == 2){
+            intake.setPower(intakePower);
+        }else {
             intake.setPower(0);
         }
 
+
     }
 }

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

@@ -1,19 +1,19 @@
 package org.firstinspires.ftc.teamcode.subsystems;
 
+import static org.firstinspires.ftc.teamcode.tests.ShooterTest.*;
+
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.Pose2d;
 import com.arcrobotics.ftclib.controller.PIDController;
+import com.arcrobotics.ftclib.controller.PIDFController;
 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;
 
@@ -28,7 +28,6 @@ public class Shooter implements Subsystem {
 
     private final Servo turret2;
 
-
     private final MultipleTelemetry telemetry;
 
     private boolean telemetryOn = false;
@@ -40,21 +39,21 @@ public class Shooter implements Subsystem {
     private double velocity = 0.0;
     private double posPower = 0.0;
 
+    public double velo = 0.0;
+
     private int targetPosition = 0;
 
-    private double p = 0.0003, i = 0, d = 0.00001;
+    public double powPID = 1.0;
-
-    private PIDController controller;
-
-
 
+    private double p = 0.0003, i = 0, d = 0.00001, f=0;
 
+    private PIDFController controller;
+    private double pow = 0.0;
 
     private String shooterMode = "AUTO";
 
     private String turretMode = "AUTO";
 
-
     public Shooter(Robot robot, MultipleTelemetry TELE) {
         this.fly1 = robot.shooter1;
         this.fly2 = robot.shooter2;
@@ -68,11 +67,9 @@ public class Shooter implements Subsystem {
         fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
         fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
 
+        controller = new PIDFController(p, i, d, f);
 
-        controller = new PIDController(p, i, d);
+        controller.setPIDF(p, i, d, f);
-
-
-        controller.setPID(p, i, d);
 
         this.turret1 = robot.turr1;
 
@@ -80,105 +77,74 @@ public class Shooter implements Subsystem {
 
         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 void sethoodPosition(double pos) { hoodPos = pos; }
 
     public double getTurretPosition() {
-        return ((turret1.getPosition()+ (1-turret2.getPosition()))/2);
+        return ((turret1.getPosition() + (1 - turret2.getPosition())) / 2);
     }
 
-    public void setTurretPosition(double pos) {turretPos = pos;}
+    public void setTurretPosition(double pos) { turretPos = pos; }
 
-    public double getVelocity() {
+    public double getVelocity(double vel) {
-        return ((double) ((fly1.getVelocity(AngleUnit.DEGREES) + fly2.getVelocity(AngleUnit.DEGREES)) /2));
+        return vel;
     }
 
-    public void setVelocity(double vel){velocity = vel;}
+    public void setVelocity(double vel) { velocity = vel; }
 
+    public void setPosPower(double power) { posPower = power; }
 
-
+    public void setTargetPosition(int pos) {
-    public void setPosPower(double power){posPower = power;}
-
-
-    public void setTargetPosition(int pos){
         targetPosition = pos;
     }
 
-    public void setTolerance(int tolerance){
+    public void setTolerance(int tolerance) {
         controller.setTolerance(tolerance);
     }
 
-    public void setControllerCoefficients(double kp, double ki, double kd){
+    public void setControllerCoefficients(double kp, double ki, double kd, double kf) {
         p = kp;
         i = ki;
         d = kd;
-        controller.setPID(p, i, d);
+        f = kf;
+        controller.setPIDF(p, i, d, f);
 
     }
 
-    public PIDCoefficients  getControllerCoefficients(){
+    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 getECPRPosition() {
-
+        return fly1.getCurrentPosition() / (2 * ecpr);
-
-    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 double getMCPRPosition() {
+        return (double) fly1.getCurrentPosition() / 4;
+    }
 
-    public void setTurretMode(String mode){ turretMode = mode;}
+    public void setShooterMode(String mode) { shooterMode = mode; }
 
+    public void setTurretMode(String mode) { turretMode = mode; }
 
-    public double trackGoal(Pose2d robotPose, Pose2d goalPose, double offset){
+    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,
@@ -197,39 +163,28 @@ public class Shooter implements Subsystem {
 
         double hoodAngle = getAngleByDist(distance);
 
-
 //        hoodServo.setPosition(hoodAngle);
 
         moveTurret(getTurretPosByDeltaPose(deltaPose, offset));
 
         return distance;
 
-
-
-
-
         //0.9974 * 355
 
-
     }
 
-    public double getTurretPosByDeltaPose (Pose2d dPose, double offset){
+    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));
-        double aTanAngle = Math.toDegrees(Math.atan(dPose.position.y/dPose.position.x));
 
         telemetry.addData("deltaAngle", deltaAngle);
 
-
-
         if (deltaAngle > 90) {
-            deltaAngle -=360;
+            deltaAngle -= 360;
         }
 
-
-
 //        deltaAngle += aTanAngle;
 
         deltaAngle /= (335);
@@ -238,10 +193,7 @@ public class Shooter implements Subsystem {
 
         telemetry.addData("AtanAngle", aTanAngle);
 
-
+        return ((0.30 - deltaAngle) + offset);
-        return ((0.30-deltaAngle) + offset);
-
-
 
     }
 
@@ -249,89 +201,48 @@ public class Shooter implements Subsystem {
 
     //56.5, 0.5
 
-
+    public double getPowerByDist(double dist) {
-    public double getPowerByDist(double dist){
 
         //TODO: ADD LOGIC
-        return  dist;
+        return dist;
     }
 
-    public double getAngleByDist(double dist){
+    public double getAngleByDist(double dist) {
-
 
         double newDist = dist - 56.5;
 
-        double pos = newDist*((0.44-0.5)/(62-56.5)) + 0.46;
+        double pos = newDist * ((0.44 - 0.5) / (62 - 56.5)) + 0.46;
 
-
+        return pos;
-
-
-        return  pos;
     }
 
+    public void setTelemetryOn(boolean state) { telemetryOn = state; }
 
-
+    public void moveTurret(double pos) {
-
-    public void setTelemetryOn(boolean state){telemetryOn = state;}
-
-    public void moveTurret(double pos){
         turret1.setPosition(pos);
-        turret2.setPosition(1-pos);
+        turret2.setPosition(1 - pos);
+    }
+
+    public double getpowPID() {
+        return powPID;
     }
 
     @Override
     public void update() {
 
-        if (Objects.equals(shooterMode, "MANUAL")){
+        if (Objects.equals(shooterMode, "MANUAL")) {
-            fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+            fly1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-            fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+            fly2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-
 
             fly1.setPower(manualPower);
             fly2.setPower(manualPower);
-        }
+        } else if (Objects.equals(shooterMode, "VEL")) {
-
+            powPID = velocity;
-        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();}
-
-
-
     }
 }

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

@@ -9,8 +9,6 @@ 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;
@@ -118,14 +116,14 @@ public class Spindexer implements Subsystem{
     }
 
     public void intake () {
-        position = spindexer_intakePos;
+        position = spindexer_intakePos1;
     }
 
     public void intakeShake(double runtime) {
         if ((runtime % 0.25) >0.125) {
-            position = spindexer_intakePos + 0.04;
+            position = spindexer_intakePos1 + 0.04;
         } else {
-            position = spindexer_intakePos - 0.04;
+            position = spindexer_intakePos1 - 0.04;
 
         }
     }

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.Poses.*;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodDefault;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -22,13 +22,11 @@ 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 {
 
-
     Robot robot;
 
     Drivetrain drivetrain;
@@ -63,7 +61,6 @@ public class TeleopV1 extends LinearOpMode {
 
     ToggleButtonReader g2Square;
 
-
     ToggleButtonReader g2Triangle;
 
     ToggleButtonReader g2RightBumper;
@@ -85,7 +82,6 @@ public class TeleopV1 extends LinearOpMode {
 
     public double g1LeftBumperStamp = 0.0;
 
-
     public double g2LeftBumperStamp = 0.0;
 
     public static int spindexerPos = 0;
@@ -96,7 +92,7 @@ public class TeleopV1 extends LinearOpMode {
 
     public boolean scoreAll = false;
 
-    MecanumDrive drive ;
+    MecanumDrive drive;
 
     public boolean autotrack = false;
 
@@ -117,16 +113,10 @@ public class TeleopV1 extends LinearOpMode {
 
     public boolean tri = false;
 
-
-
     @Override
     public void runOpMode() throws InterruptedException {
 
-        drive  = new MecanumDrive(hardwareMap, teleStart);
+        drive = new MecanumDrive(hardwareMap, teleStart);
-
-
-
-
 
         robot = new Robot(hardwareMap);
 
@@ -137,8 +127,8 @@ public class TeleopV1 extends LinearOpMode {
 
         g1 = new GamepadEx(gamepad1);
 
-        g1RightBumper  = new ToggleButtonReader(
+        g1RightBumper = new ToggleButtonReader(
-            g1, GamepadKeys.Button.RIGHT_BUMPER
+                g1, GamepadKeys.Button.RIGHT_BUMPER
         );
 
         g2 = new GamepadEx(gamepad2);
@@ -147,48 +137,42 @@ public class TeleopV1 extends LinearOpMode {
                 g1, GamepadKeys.Button.LEFT_BUMPER
         );
 
-        g2Circle  = new ToggleButtonReader(
+        g2Circle = new ToggleButtonReader(
                 g2, GamepadKeys.Button.B
         );
 
-        g2Triangle  = new ToggleButtonReader(
+        g2Triangle = new ToggleButtonReader(
                 g2, GamepadKeys.Button.Y
         );
 
-        g2Square  = new ToggleButtonReader(
+        g2Square = new ToggleButtonReader(
                 g2, GamepadKeys.Button.X
         );
 
-        g2RightBumper  = new ToggleButtonReader(
+        g2RightBumper = new ToggleButtonReader(
                 g2, GamepadKeys.Button.RIGHT_BUMPER
         );
 
-
+        g2LeftBumper = new ToggleButtonReader(
-        g2LeftBumper  = new ToggleButtonReader(
                 g2, GamepadKeys.Button.LEFT_BUMPER
         );
 
-        g2DpadUp  = new ToggleButtonReader(
+        g2DpadUp = new ToggleButtonReader(
                 g2, GamepadKeys.Button.DPAD_UP
         );
 
-
+        g2DpadDown = new ToggleButtonReader(
-        g2DpadDown  = new ToggleButtonReader(
                 g2, GamepadKeys.Button.DPAD_DOWN
         );
 
-        g2DpadLeft  = new ToggleButtonReader(
+        g2DpadLeft = new ToggleButtonReader(
                 g2, GamepadKeys.Button.DPAD_LEFT
         );
 
-
+        g2DpadRight = new ToggleButtonReader(
-        g2DpadRight  = new ToggleButtonReader(
                 g2, GamepadKeys.Button.DPAD_RIGHT
         );
 
-
-
-
         drivetrain = new Drivetrain(robot, TELE, g1);
 
         drivetrain.setMode("Default");
@@ -201,7 +185,6 @@ public class TeleopV1 extends LinearOpMode {
 
         transfer = new Transfer(robot);
 
-
         spindexer = new Spindexer(robot, TELE);
 
         spindexer.setTelemetryOn(true);
@@ -212,18 +195,13 @@ public class TeleopV1 extends LinearOpMode {
 
         robot.rejecter.setPosition(rIn);
 
-
-
-
-
         waitForStart();
 
         if (isStopRequested()) return;
 
-        drive  = new MecanumDrive(hardwareMap, teleStart);
+        drive = new MecanumDrive(hardwareMap, teleStart);
 
-
+        while (opModeIsActive()) {
-        while(opModeIsActive()){
 
             drive.updatePoseEstimate();
 
@@ -231,28 +209,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 +234,6 @@ public class TeleopV1 extends LinearOpMode {
 
             }
 
-
-
-
             intake();
 
             drivetrain.update();
@@ -279,63 +250,49 @@ public class TeleopV1 extends LinearOpMode {
 
             g2DpadUp.readValue();
 
-            if (!scoreAll){
+            if (!scoreAll) {
                 spindexer.checkForBalls();
             }
 
-            if(g2DpadUp.wasJustPressed()){
+            if (g2DpadUp.wasJustPressed()) {
-                pos -=0.02;
+                pos -= 0.02;
             }
 
-            if(g2DpadDown.wasJustPressed()){
+            if (g2DpadDown.wasJustPressed()) {
-                pos +=0.02;
+                pos += 0.02;
             }
 
             g2DpadLeft.readValue();
 
             g2DpadRight.readValue();
 
-            if(g2DpadLeft.wasJustPressed()){
+            if (g2DpadLeft.wasJustPressed()) {
-                offset -=0.02;
+                offset -= 0.02;
             }
 
-            if(g2DpadRight.wasJustPressed()){
+            if (g2DpadRight.wasJustPressed()) {
-                offset +=0.02;
+                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 +301,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 +322,6 @@ public class TeleopV1 extends LinearOpMode {
             if (scoreAll) {
                 double time = getRuntime() - g2LeftBumperStamp;
 
-
                 shooter.setManualPower(1);
 
                 TELE.addData("greenImportant", green);
@@ -377,7 +333,6 @@ public class TeleopV1 extends LinearOpMode {
 
                 if (square) {
 
-
                     if (time < 0.3) {
 
                         ticker = 0;
@@ -385,7 +340,6 @@ public class TeleopV1 extends LinearOpMode {
                         last = 0;
                         second = 0;
 
-
                         transfer.transferOut();
                         transfer.setTransferPower(1);
                     } else if (time < 2) {
@@ -406,14 +360,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 +385,9 @@ public class TeleopV1 extends LinearOpMode {
 
                         ticker = 0;
 
-
                         transfer.transferIn();
                     } else if (time < 6) {
 
-
                         transfer.transferOut();
 
                         if (ticker == 0) {
@@ -458,7 +408,6 @@ public class TeleopV1 extends LinearOpMode {
 
                         ticker = 0;
 
-
                         scoreAll = false;
                         transfer.transferOut();
 
@@ -467,7 +416,6 @@ public class TeleopV1 extends LinearOpMode {
                     }
                 } else if (tri) {
 
-
                     if (time < 0.3) {
 
                         ticker = 0;
@@ -475,7 +423,6 @@ public class TeleopV1 extends LinearOpMode {
                         last = 0;
                         second = 0;
 
-
                         transfer.transferOut();
                         transfer.setTransferPower(1);
                     } else if (time < 2) {
@@ -496,14 +443,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 +468,9 @@ public class TeleopV1 extends LinearOpMode {
 
                         ticker = 0;
 
-
                         transfer.transferIn();
                     } else if (time < 6) {
 
-
                         transfer.transferOut();
 
                         if (ticker == 0) {
@@ -548,15 +491,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 +506,6 @@ public class TeleopV1 extends LinearOpMode {
                         last = 0;
                         second = 0;
 
-
                         transfer.transferOut();
                         transfer.setTransferPower(1);
                     } else if (time < 2) {
@@ -586,14 +526,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 +551,9 @@ public class TeleopV1 extends LinearOpMode {
 
                         ticker = 0;
 
-
                         transfer.transferIn();
                     } else if (time < 6) {
 
-
                         transfer.transferOut();
 
                         if (ticker == 0) {
@@ -638,15 +574,13 @@ public class TeleopV1 extends LinearOpMode {
 
                         ticker = 0;
 
-
                         scoreAll = false;
                         transfer.transferOut();
 
                         shooter.setManualPower(0);
 
                     }
-                } else{
+                } else {
-
 
                     if (time < 0.3) {
 
@@ -668,7 +602,6 @@ public class TeleopV1 extends LinearOpMode {
                             all = true;
                         }
 
-
                         transfer.transferOut();
                         transfer.setTransferPower(1);
                     } else if (time < 2) {
@@ -693,7 +626,6 @@ public class TeleopV1 extends LinearOpMode {
 
                         ticker++;
 
-
                     } else if (time < 2.5) {
 
                         ticker = 0;
@@ -710,7 +642,6 @@ public class TeleopV1 extends LinearOpMode {
 
                             all = false;
 
-
                         }
 
                         transfer.transferIn();
@@ -736,7 +667,6 @@ public class TeleopV1 extends LinearOpMode {
 
                         ticker = 0;
 
-
                         if (gamepad2.right_trigger > 0.5) {
                             green = false;
 
@@ -751,7 +681,6 @@ public class TeleopV1 extends LinearOpMode {
                         transfer.transferIn();
                     } else if (time < 6) {
 
-
                         transfer.transferOut();
 
                         if (ticker == 0) {
@@ -774,7 +703,6 @@ public class TeleopV1 extends LinearOpMode {
 
                         ticker = 0;
 
-
                         scoreAll = false;
                         transfer.transferOut();
 
@@ -782,33 +710,16 @@ public class TeleopV1 extends LinearOpMode {
 
                     }
 
-
                 }
             }
 
-
             shooter.update();
 
-
-
-
-
-
-
-
-
-
-
-
         }
 
-
-
-
     }
 
-    public void intake(){
+    public void intake() {
-
 
         g1RightBumper.readValue();
 
@@ -818,84 +729,63 @@ 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();
 
-
-
-
-
     }
 }

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

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

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

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

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

@@ -1,18 +1,17 @@
 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.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 org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.subsystems.Shooter;
 
-
 @TeleOp
 @Config
 public class ShooterTest extends LinearOpMode {
@@ -21,22 +20,47 @@ public class ShooterTest extends LinearOpMode {
 
     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 int pos = 0;
+    public static String flyMode = "VEL";
-    public static double posPower = 0.0;
 
-    public static double p = 0.000003, i = 0, d = 0.000001;
+    public static boolean AutoTrack = false;
 
+    double initPos = 0.0;
 
-    public static String flyMode = "MANUAL";
+    double velo = 0.0;
+    double velo1 = 0.0;
+    double velo2 = 0.0;
+    double velo3 = 0.0;
+    double velo4 = 0.0;
+    double velo5 = 0.0;
 
-    public static String turrMode = "MANUAL";
+    double stamp1 = 0.0;
 
-    public static int posTolerance = 40;
+    double initPos1 = 0.0;
 
-    public static double servoPosition = 0.501;
+    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 {
 
@@ -46,51 +70,141 @@ public class ShooterTest extends LinearOpMode {
 
         Shooter shooter = new Shooter(robot, TELE);
 
-        shooter.setTelemetryOn(true);
+        robot.shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
-        shooter.setTurretMode(turrMode);
+        shooter.setTelemetryOn(true);
 
         shooter.setShooterMode(flyMode);
 
+        initPos = shooter.getECPRPosition();
 
-        shooter.setControllerCoefficients(p, i, d);
+        int ticker = 0;
-
 
         waitForStart();
 
-        if(isStopRequested()) return;
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()) {
+
+            ticker++;
+
+            if (AutoTrack){
+                hoodPos = hoodAnglePrediction(distance);
+                vel = velPrediction(distance);
+            }
 
 
-        while(opModeIsActive()){
-
-            shooter.setControllerCoefficients(p, i, d);
-
-            shooter.setTurretMode(turrMode);
 
             shooter.setShooterMode(flyMode);
 
             shooter.setManualPower(pow);
 
-            shooter.setVelocity(vel);
+            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);
+                }
+            }
 
-            shooter.setTargetPosition(pos);
+            double penguin = 0;
+            if (ticker % 8 ==0){
+                penguin = shooter.getECPRPosition();
+                stamp = getRuntime();
+                velo1 = -60 * ((penguin - initPos1) / (stamp - stamp1));
+                initPos1 = penguin;
+                stamp1 = stamp;
+            }
 
-            shooter.setTolerance(posTolerance);
 
-            shooter.setPosPower(posPower);
+            velo = velo1;
 
-            if (servoPosition!=0.501) {shooter.sethoodPosition(servoPosition);}
+            double feed = vel / maxVel;        // Example: vel=2500 → feed=0.5
 
+            if (vel > 500){
+                feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
+            }
+
+            // --- PROPORTIONAL CORRECTION ---
+            double error = vel - velo1;
+            double correction = kP * error;
+
+            // limit how fast power changes (prevents oscillation)
+            correction = Math.max(-maxStep, Math.min(maxStep, correction));
+
+            // --- FINAL MOTOR POWER ---
+            powPID = feed + correction;
+
+            // clamp to allowed range
+            powPID = Math.max(0, Math.min(1, powPID));
+
+            if (vel - velo > 1000){
+                powPID = 1;
+            } else if (velo - vel > 1000){
+                powPID = 0;
+            }
+
+            shooter.setVelocity(powPID);
+
+            if (shoot) {
+                robot.transferServo.setPosition(transferServo_in);
+            } else {
+                robot.transferServo.setPosition(transferServo_out);
+            }
 
             shooter.update();
 
+            TELE.addData("Revolutions", shooter.getECPRPosition());
+            TELE.addData("hoodPos", shooter.gethoodPosition());
+            TELE.addData("turretPos", shooter.getTurretPosition());
+            TELE.addData("Power Fly 1", robot.shooter1.getPower());
+            TELE.addData("Power Fly 2", robot.shooter2.getPower());
+            TELE.addData("powPID", shooter.getpowPID());
+            TELE.addData("Velocity", velo);
             TELE.update();
 
 
-
         }
 
-
-
     }
+
+    public double hoodAnglePrediction(double distance) {
+        double L = 0.298317;
+        double A = 1.02124;
+        double k = 0.0157892;
+        double n = 3.39375;
+
+        double dist = Math.sqrt(distance*distance+24*24);
+
+        return L + A * Math.exp(-Math.pow(k * dist, n));
+    }
+    public static double velPrediction(double distance) {
+
+        double x = Math.sqrt(distance*distance+24*24);
+
+
+
+        double A = -211149.992;
+        double B = -1.19943;
+        double C = 3720.15909;
+
+        return A * Math.pow(x, B) + C;
+    }
+
 }

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

@@ -5,17 +5,15 @@ import com.qualcomm.hardware.rev.RevColorSensorV3;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
-
 @Config
 @TeleOp
 public class ConfigureColorRangefinder extends LinearOpMode {
 
+    public static int LED_Brightness = 50;
 
+    public static int lowerGreen = 110;
 
-    public static double lowerBound = 80;
+    public static int higherGreen = 150;
-    public static double higherBound = 120;
-
-    public static int led = 0;
 
     @Override
     public void runOpMode() throws InterruptedException {
@@ -24,11 +22,12 @@ public class ConfigureColorRangefinder extends LinearOpMode {
         /* Using this example configuration, you can detect both artifact colors based on which pin is reading true:
             pin0 --> purple
             pin1 --> green */
-        crf.setPin0Digital(ColorRangefinder.DigitalMode.DISTANCE, 3,  20);
+        crf.setPin1Digital(ColorRangefinder.DigitalMode.DISTANCE, 0, 40); // green
-        crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, lowerBound, higherBound); // green
+        crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, higherGreen / 360.0 * 255, 360 / 360.0 * 255); // purple
-        crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 25); //25 mm or closer
+        crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 0, lowerGreen/360.0 * 255);
-        crf.setLedBrightness(led);
+        crf.setPin0DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 10mm or closer requirement
 
+        crf.setLedBrightness(LED_Brightness);
     }
 }
 
@@ -143,6 +142,7 @@ class ColorRangefinder {
 
     /**
      * Read distance via I2C
+     *
      * @return distance in millimeters
      */
     public double readDistance() {

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

@@ -0,0 +1,53 @@
+package org.firstinspires.ftc.teamcode.utils;
+
+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;
+
+@TeleOp
+@Config
+public class PositionalServoProgrammer extends LinearOpMode {
+    Robot robot;
+    MultipleTelemetry TELE;
+    public static double spindexPos = 0.501;
+    public static double turretPos = 0.501;
+    public static double transferPos = 0.501;
+    public static double hoodPos = 0.501;
+
+    public static double scalar = 1.112;
+    public static double restPos = 0.158;
+    @Override
+    public void runOpMode() throws InterruptedException {
+        robot = new Robot(hardwareMap);
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        waitForStart();
+        if (isStopRequested()) return;
+        while (opModeIsActive()){
+            if (spindexPos != 0.501){
+                robot.spin1.setPosition(spindexPos);
+                robot.spin2.setPosition(1-spindexPos);
+            }
+            if (turretPos != 0.501){
+                robot.turr1.setPosition(turretPos);
+                robot.turr2.setPosition(1-turretPos);
+            }
+            if (transferPos != 0.501){
+                robot.transferServo.setPosition(transferPos);
+            }
+            if (hoodPos != 0.501){
+                robot.hood.setPosition(hoodPos);
+            }
+            TELE.addData("spindexer", scalar*((robot.spin1Pos.getVoltage() - restPos) / 3.3));
+            TELE.addData("hood", 1-scalar*((robot.hoodPos.getVoltage() - restPos) / 3.3));
+            TELE.addData("transferServo", scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3));
+            TELE.addData("turret", scalar*((robot.turr1Pos.getVoltage() - restPos) / 3.3));
+            TELE.addData("spindexerA", robot.spin1Pos.getVoltage());
+            TELE.addData("hoodA", robot.hoodPos.getVoltage());
+            TELE.addData("transferServoA", robot.transferServoPos.getVoltage());
+            TELE.addData("turretA", robot.turr1Pos.getVoltage());
+            TELE.update();
+        }
+    }
+}

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

@@ -1,5 +1,6 @@
 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;
@@ -7,6 +8,7 @@ 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.I2cDevice;
 import com.qualcomm.robotcore.hardware.Servo;
 
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
@@ -30,8 +32,6 @@ public class Robot {
 
     public DcMotorEx transfer;
 
-
-
     public DcMotorEx shooter1;
     public DcMotorEx shooter2;
     public Servo hood;
@@ -61,27 +61,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 +112,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 +142,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");
-
     }
 }