lot of stuff to test tomorrow

# 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();
 

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()
                     )
             );
 
@@ -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/constants/ServoPositions.java

@@ -19,5 +19,8 @@ public class ServoPositions {
 
     public static double hoodDefault = 0.35;
 
+    public static double turret_red = 0.33;
+    public static double turret_blue = 0.3;
+
 
 }

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

@@ -0,0 +1,10 @@
+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;
+}

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 DcMotorEx bl;
-    private DcMotorEx br;
+    private final DcMotorEx fr;
+    private final DcMotorEx bl;
+    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){
-        intakePower = pow;
+    public void intakeMinPower(){
+        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 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;
 
+    public double powPID = 1.0;
+
     private double p = 0.0003, i = 0, d = 0.00001;
 
-    private PIDController controller;
-
-
-
-
+    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.setPID(p, i, d);
+        controller.setPIDF(p, i, d, f);
 
         this.turret1 = robot.turr1;
 
@@ -80,29 +77,6 @@ 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() {
@@ -117,17 +91,14 @@ public class Shooter implements Subsystem {
 
     public void setTurretPosition(double pos) { turretPos = pos; }
 
-    public double getVelocity() {
-        return ((double) ((fly1.getVelocity(AngleUnit.DEGREES) + fly2.getVelocity(AngleUnit.DEGREES)) /2));
+    public double getVelocity(double vel) {
+        return vel;
     }
 
     public void setVelocity(double vel) { velocity = vel; }
 
-
-
     public void setPosPower(double power) { posPower = power; }
 
-
     public void setTargetPosition(int pos) {
         targetPosition = pos;
     }
@@ -136,11 +107,12 @@ public class Shooter implements Subsystem {
         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);
 
     }
 
@@ -150,35 +122,29 @@ public class Shooter implements Subsystem {
 
     }
 
-
-
-
-
-
-
     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 double getPosition(){
-        return ((double) ((fly1.getCurrentPosition() + fly2.getCurrentPosition()) /2));
+    public double getMCPRPosition() {
+        return fly1.getCurrentPosition() / (2 * mcpr);
     }
 
     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,
@@ -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) {
 
         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);
@@ -238,18 +193,14 @@ public class Shooter implements Subsystem {
 
         telemetry.addData("AtanAngle", aTanAngle);
 
-
         return ((0.30 - deltaAngle) + offset);
 
-
-
     }
 
     //62, 0.44
 
     //56.5, 0.5
 
-
     public double getPowerByDist(double dist) {
 
         //TODO: ADD LOGIC
@@ -258,20 +209,13 @@ public class Shooter implements Subsystem {
 
     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) {
@@ -279,59 +223,26 @@ public class Shooter implements Subsystem {
         turret2.setPosition(1 - pos);
     }
 
+    public double getpowPID() {
+        return powPID;
+    }
+
     @Override
     public void update() {
 
         if (Objects.equals(shooterMode, "MANUAL")) {
-            fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-            fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-
+            fly1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+            fly2.setMode(DcMotor.RunMode.RUN_WITHOUT_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);
-
-
+        } else if (Objects.equals(shooterMode, "VEL")) {
+            powPID = velocity;
 
             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/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;
@@ -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;
@@ -117,17 +113,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 +153,6 @@ public class TeleopV1 extends LinearOpMode {
                 g2, GamepadKeys.Button.RIGHT_BUMPER
         );
 
-
         g2LeftBumper = new ToggleButtonReader(
                 g2, GamepadKeys.Button.LEFT_BUMPER
         );
@@ -172,7 +161,6 @@ public class TeleopV1 extends LinearOpMode {
                 g2, GamepadKeys.Button.DPAD_UP
         );
 
-
         g2DpadDown = new ToggleButtonReader(
                 g2, GamepadKeys.Button.DPAD_DOWN
         );
@@ -181,14 +169,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 +185,6 @@ public class TeleopV1 extends LinearOpMode {
 
         transfer = new Transfer(robot);
 
-
         spindexer = new Spindexer(robot, TELE);
 
         spindexer.setTelemetryOn(true);
@@ -212,17 +195,12 @@ public class TeleopV1 extends LinearOpMode {
 
         robot.rejecter.setPosition(rIn);
 
-
-
-
-
         waitForStart();
 
         if (isStopRequested()) return;
 
         drive = new MecanumDrive(hardwareMap, teleStart);
 
-
         while (opModeIsActive()) {
 
             drive.updatePoseEstimate();
@@ -231,10 +209,8 @@ public class TeleopV1 extends LinearOpMode {
 
             TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
 
-
             TELE.addData("off", offset);
 
-
             robot.hood.setPosition(pos);
 
             g1LeftBumper.readValue();
@@ -242,8 +218,6 @@ public class TeleopV1 extends LinearOpMode {
             if (g1LeftBumper.wasJustPressed()) {
                 g2LeftBumperStamp = getRuntime();
 
-
-
                 spindexer.intakeShake(getRuntime());
 
                 leftBumper = true;
@@ -260,9 +234,6 @@ public class TeleopV1 extends LinearOpMode {
 
             }
 
-
-
-
             intake();
 
             drivetrain.update();
@@ -303,22 +274,10 @@ public class TeleopV1 extends LinearOpMode {
                 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 {
@@ -333,8 +292,6 @@ public class TeleopV1 extends LinearOpMode {
                 autotrack = true;
             }
 
-
-
             if (g2RightBumper.wasJustPressed()) {
                 transfer.setTransferPower(1);
                 transfer.transferIn();
@@ -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,7 +491,6 @@ public class TeleopV1 extends LinearOpMode {
 
                         ticker = 0;
 
-
                         scoreAll = false;
                         transfer.transferOut();
 
@@ -557,7 +499,6 @@ public class TeleopV1 extends LinearOpMode {
                     }
                 } else if (circle) {
 
-
                     if (time < 0.3) {
 
                         ticker = 0;
@@ -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,7 +574,6 @@ public class TeleopV1 extends LinearOpMode {
 
                         ticker = 0;
 
-
                         scoreAll = false;
                         transfer.transferOut();
 
@@ -647,7 +582,6 @@ public class TeleopV1 extends LinearOpMode {
                     }
                 } else {
 
-
                     if (time < 0.3) {
 
                         ticker = 0;
@@ -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,34 +710,17 @@ public class TeleopV1 extends LinearOpMode {
 
                     }
 
-
                 }
             }
 
-
             shooter.update();
 
-
-
-
-
-
-
-
-
-
-
-
         }
 
-
-
-
     }
 
     public void intake() {
 
-
         g1RightBumper.readValue();
 
         g2Circle.readValue();
@@ -822,9 +733,6 @@ public class TeleopV1 extends LinearOpMode {
 
             notShooting = true;
 
-
-
-
             if (getRuntime() - g1RightBumperStamp < 0.3) {
                 intake.reverse();
             } else {
@@ -835,19 +743,14 @@ public class TeleopV1 extends LinearOpMode {
                 shooter.setManualPower(0);
             }
 
-
-
-
             spindexer.intake();
 
             transfer.transferOut();
 
-
             g1RightBumperStamp = getRuntime();
 
         }
 
-
         if (intake.getIntakeState() == 1 && notShooting) {
 
             spindexer.intakeShake(getRuntime());
@@ -858,8 +761,6 @@ public class TeleopV1 extends LinearOpMode {
                 tri = false;
                 square = false;
 
-
-
             }
 
             if (g2Triangle.wasJustPressed()) {
@@ -880,22 +781,11 @@ public class TeleopV1 extends LinearOpMode {
                 square = false;
             }
 
-
-
-
         }
 
-
         intake.update();
 
-
-
-
         spindexer.update();
 
-
-
-
-
     }
 }

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

@@ -0,0 +1,129 @@
+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_intakePos + 0.02;
+            } else {
+                position = spindexer_intakePos - 0.02;
+            }
+            robot.spin1.setPosition(position);
+            robot.spin2.setPosition(1-position);
+
+            robot.intake.setPower(1);
+
+            if (robot.pin1.getState()){
+                if (robot.pin0.getState()){
+                    b1Purple += 1;
+                    purpleStamp1 = getRuntime();
+                }
+                b1Total += 1;
+                totalStamp1 = getRuntime();
+            }
+            if (getRuntime() - totalStamp1 > 1){
+                b1 = "none";
+                b1Total = 1;
+                b1Purple = 1;
+            }else if (b1Total > 10 && getRuntime() - purpleStamp1 > 2){
+                b1 = "Green";
+            }else if (b1Purple > 10){
+                b1 = "Purple";
+            }
+
+            if (robot.pin3.getState()){
+                if (robot.pin2.getState()){
+                    b2Purple += 1;
+                    purpleStamp2 = getRuntime();
+                }
+                b2Total += 1;
+                totalStamp2 = getRuntime();
+            }
+            if (getRuntime() - totalStamp2 > 1){
+                b2 = "none";
+                b2Total = 1;
+                b2Purple = 1;
+            }else if (b2Total > 10 && getRuntime() - purpleStamp2 > 2){
+                b2 = "Green";
+            }else if (b2Purple > 10){
+                b2 = "Purple";
+            }
+
+            if (robot.pin5.getState()){
+                if (robot.pin4.getState()){
+                    b3Purple += 1;
+                    purpleStamp3 = getRuntime();
+                }
+                b3Total += 1;
+                totalStamp3 = getRuntime();
+            }
+            if (getRuntime() - totalStamp3 > 1){
+                b3 = "none";
+                b3Total = 1;
+                b3Purple = 1;
+            }else if (b3Total > 10 && getRuntime() - purpleStamp3 > 2){
+                b3 = "Green";
+            }else if (b3Purple > 10){
+                b3 = "Purple";
+            }
+
+            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,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.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+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()){
+            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.update();
+        }
+    }
+
+}

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

@@ -1,18 +1,16 @@
 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 +19,37 @@ public class ShooterTest extends LinearOpMode {
 
     public static double pow = 0.0;
     public static double vel = 0.0;
-
-    public static int pos = 0;
+    public static double mcpr = 28.0; // CPR of the motor
+    public static double ecpr = 1024.0; // CPR of the encoder
+    public static double hoodPos = 0.5;
     public static double posPower = 0.0;
 
-    public static double p = 0.000003, i = 0, d = 0.000001;
+    public static double turretPos = 0.9;
 
+    public static double p = 0.0003, i = 0, d = 0, f = 0;
 
     public static String flyMode = "MANUAL";
 
     public static String turrMode = "MANUAL";
 
-    public static int posTolerance = 40;
+    double initPos = 0.0;
 
-    public static double servoPosition = 0.501;
+    double velo1 = 0.0;
+
+    double stamp1 = 0.0;
+
+    double initPos1 = 0.0;
+
+    double powPID = 0.0;
+
+    public static double maxVel = 4500;
+
+    public static int tolerance = 300;
+
+    public static boolean shoot = false;
 
     MultipleTelemetry TELE;
+
     @Override
     public void runOpMode() throws InterruptedException {
 
@@ -52,45 +65,58 @@ public class ShooterTest extends LinearOpMode {
 
         shooter.setShooterMode(flyMode);
 
+        shooter.setControllerCoefficients(p, i, d, f);
 
-        shooter.setControllerCoefficients(p, i, d);
-
+        initPos = shooter.getECPRPosition();
 
         waitForStart();
 
         if (isStopRequested()) return;
 
-
         while (opModeIsActive()) {
 
-            shooter.setControllerCoefficients(p, i, d);
-
-            shooter.setTurretMode(turrMode);
+            shooter.setControllerCoefficients(p, i, d, f);
 
             shooter.setShooterMode(flyMode);
 
             shooter.setManualPower(pow);
 
-            shooter.setVelocity(vel);
+            robot.hood.setPosition(hoodPos);
+            robot.turr1.setPosition(turretPos);
+            robot.turr2.setPosition(turretPos);
 
-            shooter.setTargetPosition(pos);
-
-            shooter.setTolerance(posTolerance);
-
-            shooter.setPosPower(posPower);
-
-            if (servoPosition!=0.501) {shooter.sethoodPosition(servoPosition);}
+            velo1 = -60 * ((shooter.getECPRPosition() - initPos1) / (getRuntime() - stamp1));
+            stamp1 = getRuntime();
+            initPos1 = shooter.getECPRPosition();
+            if (Math.abs(vel - velo1) > 3 * tolerance) {
+                powPID = vel / maxVel;
+            } else if (vel - tolerance > velo1) {
+                powPID = powPID + 0.001;
+            } else if (vel + tolerance < velo1) {
+                powPID = powPID - 0.001;
+            }
+            shooter.setVelocity(powPID);
+            robot.transfer.setPower((powPID / 2) + 0.5);
 
+            if (shoot){
+                robot.transferServo.setPosition(transferServo_in);
+            } else {
+                robot.transferServo.setPosition(transferServo_out);
+            }
 
             shooter.update();
 
+            TELE.addData("ECPR Revolutions", shooter.getECPRPosition());
+            TELE.addData("MCPR Revolutions", shooter.getMCPRPosition());
+            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", velo1);
             TELE.update();
 
-
-
         }
 
-
-
     }
 }

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 = 100;
 
-    public static double lowerBound = 80;
-    public static double higherBound = 120;
-
-    public static int led = 0;
+    public static int higherGreen = 150;
 
     @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.HSV, lowerBound, higherBound); // green
-        crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 25); //25 mm or closer
-        crf.setLedBrightness(led);
+        crf.setPin1Digital(ColorRangefinder.DigitalMode.DISTANCE, 0, 40); // green
+        crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, higherGreen / 360.0 * 255, 360 / 360.0 * 255); // purple
+        crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 0, lowerGreen/360.0 * 255);
+        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,37 @@
+package org.firstinspires.ftc.teamcode.utils;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+@TeleOp
+@Config
+public class PositionalServoProgrammer extends LinearOpMode {
+    Robot robot;
+    public static double spindexPos = 0.501;
+    public static double turretPos = 0.501;
+    public static double transferPos = 0.501;
+    public static double hoodPos = 0.501;
+    @Override
+    public void runOpMode() throws InterruptedException {
+        robot = new Robot(hardwareMap);
+        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);
+            }
+        }
+    }
+}

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

@@ -30,8 +30,6 @@ public class Robot {
 
     public DcMotorEx transfer;
 
-
-
     public DcMotorEx shooter1;
     public DcMotorEx shooter2;
     public Servo hood;
@@ -63,24 +61,10 @@ public class Robot {
 
     public AprilTagProcessor aprilTagProcessor;
 
-
     public WebcamName webcam;
 
     public DcMotorEx shooterEncoder;
 
-
-
-
-
-
-
-
-
-
-
-
-
-
     public Robot(HardwareMap hardwareMap) {
 
         //Define components w/ hardware map
@@ -128,11 +112,8 @@ 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");
@@ -143,12 +124,7 @@ public class Robot {
 
         aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();
 
-
         webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
 
-
-
-
-
     }
 }