prototype auto

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

@@ -52,7 +52,7 @@ public class AutoClose_V3 extends LinearOpMode {
     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
-    public static double holdTurrPow = 0.1; // power to hold turret in place
+    public static double holdTurrPow = 0.01; // power to hold turret in place
 
     public Action initShooter(int vel) {
         return new Action() {
@@ -103,17 +103,17 @@ public class AutoClose_V3 extends LinearOpMode {
                 if (gpp || pgp || ppg) {
                     if (redAlliance){
                         robot.limelight.pipelineSwitch(3);
-                        double turretPID = servo.setTurrPos(turret_redClose);
+                        double turretPID = servo.setTurrPos(turret_redClose, robot.turr1Pos.getCurrentPosition());
                         robot.turr1.setPower(turretPID);
                         robot.turr2.setPower(-turretPID);
-                        return !servo.turretEqual(turret_redClose);
+                        return !servo.turretEqual(turret_redClose, robot.turr1Pos.getCurrentPosition());
 
                     } else {
                         robot.limelight.pipelineSwitch(2);
-                        double turretPID = servo.setTurrPos(turret_blueClose);
+                        double turretPID = servo.setTurrPos(turret_blueClose, robot.turr1Pos.getCurrentPosition());
                         robot.turr1.setPower(turretPID);
                         robot.turr2.setPower(-turretPID);
-                        return !servo.turretEqual(turret_blueClose);
+                        return !servo.turretEqual(turret_blueClose, robot.turr1Pos.getCurrentPosition());
                     }
                 } else {
                     return true;
@@ -132,7 +132,7 @@ public class AutoClose_V3 extends LinearOpMode {
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
 
-                spinPID = servo.setSpinPos(spindexer);
+                spinPID = servo.setSpinPos(spindexer, robot.spin1Pos.getVoltage());
                 robot.spin1.setPower(spinPID);
                 robot.spin2.setPower(-spinPID);
                 TELE.addData("Velocity", velo);
@@ -142,7 +142,7 @@ public class AutoClose_V3 extends LinearOpMode {
                 drive.updatePoseEstimate();
                 teleStart = drive.localizer.getPose();
 
-                if (servo.spinEqual(spindexer)){
+                if (servo.spinEqual(spindexer, robot.spin1Pos.getVoltage())){
                     robot.spin1.setPower(0);
                     robot.spin2.setPower(0);
                     return false;
@@ -222,27 +222,27 @@ public class AutoClose_V3 extends LinearOpMode {
                 double s2D = robot.color2.getDistance(DistanceUnit.MM);
                 double s3D = robot.color3.getDistance(DistanceUnit.MM);
 
-                if (!servo.spinEqual(position)){
+                if (!servo.spinEqual(position, robot.spin1Pos.getVoltage())){
-                    double spinPID = servo.setSpinPos(position);
+                    double spinPID = servo.setSpinPos(position, robot.spin1Pos.getVoltage());
                     robot.spin1.setPower(spinPID);
                     robot.spin2.setPower(-spinPID);
                 }
 
-                if (s1D < 43 && servo.spinEqual(position) && getRuntime() - stamp > 0.5){
+                if (s1D < 43 && servo.spinEqual(position, robot.spin1Pos.getVoltage()) && getRuntime() - stamp > 0.5){
                     if (s2D > 60){
-                        if (servo.spinEqual(spindexer_intakePos1)){
+                        if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){
                             position = spindexer_intakePos2;
-                        } else if (servo.spinEqual(spindexer_intakePos2)){
+                        } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){
                             position = spindexer_intakePos3;
-                        } else if (servo.spinEqual(spindexer_intakePos3)){
+                        } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){
                             position = spindexer_intakePos1;
                         }
                     } else if (s3D > 33){
-                        if (servo.spinEqual(spindexer_intakePos1)){
+                        if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){
                             position = spindexer_intakePos3;
-                        } else if (servo.spinEqual(spindexer_intakePos2)){
+                        } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){
                             position = spindexer_intakePos1;
-                        } else if (servo.spinEqual(spindexer_intakePos3)){
+                        } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){
                             position = spindexer_intakePos2;
                         }
                     }
@@ -411,7 +411,7 @@ public class AutoClose_V3 extends LinearOpMode {
             double turrPID;
 
             if (redAlliance){
-                turrPID = servo.setTurrPos(turret_detectRedClose);
+                turrPID = servo.setTurrPos(turret_detectRedClose, robot.turr1Pos.getCurrentPosition());
 
                 shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
                         .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
@@ -430,7 +430,7 @@ public class AutoClose_V3 extends LinearOpMode {
                 shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b))
                         .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
             } else {
-                turrPID = servo.setTurrPos(turret_detectBlueClose);
+                turrPID = servo.setTurrPos(turret_detectBlueClose, robot.turr1Pos.getCurrentPosition());
 
                 shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
                         .strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
@@ -458,8 +458,8 @@ public class AutoClose_V3 extends LinearOpMode {
             robot.transferServo.setPosition(transferServo_out);
 
             TELE.addData("Velocity", velo);
-            TELE.addData("Turret Pos", servo.getTurrPos());
+            TELE.addData("Turret Pos", servo.getTurrPos(robot.turr1Pos.getCurrentPosition()));
-            TELE.addData("Spin Pos", servo.getSpinPos());
+            TELE.addData("Spin Pos", servo.getSpinPos(robot.spin1Pos.getVoltage()));
             TELE.update();
         }
 
@@ -565,8 +565,8 @@ public class AutoClose_V3 extends LinearOpMode {
                 bearing = result.getTx();
             }
         }
-        double turretPos = servo.getTurrPos() - (bearing / 1300);
+        double turretPos = servo.getTurrPos(robot.turr1Pos.getCurrentPosition()) - (bearing / 1300);
-        double turretPID = servo.setTurrPos(turretPos);
+        double turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition());
         robot.turr1.setPower(turretPID);
         robot.turr2.setPower(-turretPID);
     }

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

@@ -52,7 +52,7 @@ public class AutoFar_V1 extends LinearOpMode {
     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
-    public static double holdTurrPow = 0.1; // power to hold turret in place
+    public static double holdTurrPow = 0.01; // power to hold turret in place
 
     public Action initShooter(int vel) {
         return new Action() {
@@ -103,17 +103,17 @@ public class AutoFar_V1 extends LinearOpMode {
                 if (gpp || pgp || ppg) {
                     if (redAlliance){
                         robot.limelight.pipelineSwitch(3);
-                        double turretPID = servo.setTurrPos(turret_redFar);
+                        double turretPID = servo.setTurrPos(turret_redFar, robot.turr1Pos.getCurrentPosition());
                         robot.turr1.setPower(turretPID);
                         robot.turr2.setPower(-turretPID);
-                        return !servo.turretEqual(turret_redFar);
+                        return !servo.turretEqual(turret_redFar, robot.turr1Pos.getCurrentPosition());
 
                     } else {
                         robot.limelight.pipelineSwitch(2);
-                        double turretPID = servo.setTurrPos(turret_blueFar);
+                        double turretPID = servo.setTurrPos(turret_blueFar, robot.turr1Pos.getCurrentPosition());
                         robot.turr1.setPower(turretPID);
                         robot.turr2.setPower(-turretPID);
-                        return !servo.turretEqual(turret_blueFar);
+                        return !servo.turretEqual(turret_blueFar, robot.turr1Pos.getCurrentPosition());
                     }
                 } else {
                     return true;
@@ -132,7 +132,7 @@ public class AutoFar_V1 extends LinearOpMode {
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
 
-                spinPID = servo.setSpinPos(spindexer);
+                spinPID = servo.setSpinPos(spindexer, robot.spin1Pos.getVoltage());
                 robot.spin1.setPower(spinPID);
                 robot.spin2.setPower(-spinPID);
                 TELE.addData("Velocity", velo);
@@ -142,7 +142,7 @@ public class AutoFar_V1 extends LinearOpMode {
                 drive.updatePoseEstimate();
                 teleStart = drive.localizer.getPose();
 
-                if (servo.spinEqual(spindexer)){
+                if (servo.spinEqual(spindexer, robot.spin1Pos.getVoltage())){
                     robot.spin1.setPower(0);
                     robot.spin2.setPower(0);
                     return false;
@@ -222,27 +222,27 @@ public class AutoFar_V1 extends LinearOpMode {
                 double s2D = robot.color2.getDistance(DistanceUnit.MM);
                 double s3D = robot.color3.getDistance(DistanceUnit.MM);
 
-                if (!servo.spinEqual(position)){
+                if (!servo.spinEqual(position, robot.spin1Pos.getVoltage())){
-                    double spinPID = servo.setSpinPos(position);
+                    double spinPID = servo.setSpinPos(position, robot.spin1Pos.getVoltage());
                     robot.spin1.setPower(spinPID);
                     robot.spin2.setPower(-spinPID);
                 }
 
-                if (s1D < 43 && servo.spinEqual(position) && getRuntime() - stamp > 0.5){
+                if (s1D < 43 && servo.spinEqual(position, robot.spin1Pos.getVoltage()) && getRuntime() - stamp > 0.5){
                     if (s2D > 60){
-                        if (servo.spinEqual(spindexer_intakePos1)){
+                        if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){
                             position = spindexer_intakePos2;
-                        } else if (servo.spinEqual(spindexer_intakePos2)){
+                        } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){
                             position = spindexer_intakePos3;
-                        } else if (servo.spinEqual(spindexer_intakePos3)){
+                        } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){
                             position = spindexer_intakePos1;
                         }
                     } else if (s3D > 33){
-                        if (servo.spinEqual(spindexer_intakePos1)){
+                        if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){
                             position = spindexer_intakePos3;
-                        } else if (servo.spinEqual(spindexer_intakePos2)){
+                        } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){
                             position = spindexer_intakePos1;
-                        } else if (servo.spinEqual(spindexer_intakePos3)){
+                        } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){
                             position = spindexer_intakePos2;
                         }
                     }
@@ -399,9 +399,9 @@ public class AutoFar_V1 extends LinearOpMode {
             double turrPID;
 
             if (redAlliance){
-                turrPID = servo.setTurrPos(turret_detectRedClose);
+                turrPID = servo.setTurrPos(turret_detectRedClose, robot.turr1Pos.getCurrentPosition());
             } else {
-                turrPID = servo.setTurrPos(turret_detectBlueClose);
+                turrPID = servo.setTurrPos(turret_detectBlueClose, robot.turr1Pos.getCurrentPosition());
             }
 
             robot.turr1.setPower(turrPID);
@@ -412,8 +412,8 @@ public class AutoFar_V1 extends LinearOpMode {
             robot.transferServo.setPosition(transferServo_out);
 
             TELE.addData("Velocity", velo);
-            TELE.addData("Turret Pos", servo.getTurrPos());
+            TELE.addData("Turret Pos", servo.getTurrPos(robot.turr1Pos.getCurrentPosition()));
-            TELE.addData("Spin Pos", servo.getSpinPos());
+            TELE.addData("Spin Pos", servo.getSpinPos(robot.spin1Pos.getVoltage()));
             TELE.update();
         }
 
@@ -466,8 +466,8 @@ public class AutoFar_V1 extends LinearOpMode {
                 bearing = result.getTx();
             }
         }
-        double turretPos = servo.getTurrPos() - (bearing / 1300);
+        double turretPos = servo.getTurrPos(robot.turr1Pos.getCurrentPosition()) - (bearing / 1300);
-        double turretPID = servo.setTurrPos(turretPos);
+        double turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition());
         robot.turr1.setPower(turretPID);
         robot.turr2.setPower(-turretPID);
     }

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

@@ -0,0 +1,680 @@
+package org.firstinspires.ftc.teamcode.autonomous;
+
+import static org.firstinspires.ftc.teamcode.constants.Color.*;
+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 androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.Action;
+import com.acmerobotics.roadrunner.ParallelAction;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.SequentialAction;
+import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
+import com.acmerobotics.roadrunner.Vector2d;
+import com.acmerobotics.roadrunner.ftc.Actions;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.LLResultTypes;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
+import org.firstinspires.ftc.teamcode.utils.FlywheelV2;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+import org.firstinspires.ftc.teamcode.utils.Servos;
+
+import java.util.List;
+
+@Config
+@Autonomous(preselectTeleOp = "TeleopV3")
+public class ProtoAutoClose_V3 extends LinearOpMode {
+    Robot robot;
+    MultipleTelemetry TELE;
+    MecanumDrive drive;
+    FlywheelV2 flywheel;
+    Servos servo;
+
+    double velo = 0.0;
+    public static double intake1Time = 2.7;
+    public static double intake2Time = 3.0;
+    public static double colorDetect = 3.0;
+    boolean gpp = false;
+    boolean pgp = false;
+    boolean ppg = false;
+    double powPID = 0.0;
+    double bearing = 0.0;
+    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
+    public static double holdTurrPow = 0.01; // power to hold turret in place
+
+    public Action initShooter(int vel) {
+        return new Action() {
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
+                velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
+                robot.shooter1.setPower(powPID);
+                robot.shooter2.setPower(powPID);
+
+                TELE.addData("Velocity", velo);
+                TELE.update();
+
+                return !flywheel.getSteady();
+            }
+        };
+    }
+
+    public Action Obelisk() {
+        return new Action() {
+            int id = 0;
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                LLResult result = robot.limelight.getLatestResult();
+                if (result != null && result.isValid()) {
+                    List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
+                    for (LLResultTypes.FiducialResult fiducial : fiducials) {
+                        id = fiducial.getFiducialId();
+                        TELE.addData("ID", id);
+                        TELE.update();
+                    }
+
+                }
+
+                if (id == 21){
+                    gpp = true;
+                } else if (id == 22){
+                    pgp = true;
+                } else if (id == 23){
+                    ppg = true;
+                }
+
+                TELE.addData("Velocity", velo);
+                TELE.addData("21", gpp);
+                TELE.addData("22", pgp);
+                TELE.addData("23", ppg);
+                TELE.update();
+
+                if (gpp || pgp || ppg) {
+                    if (redAlliance){
+                        robot.limelight.pipelineSwitch(3);
+                        double turretPID = servo.setTurrPos(turret_redClose, robot.turr1Pos.getCurrentPosition());
+                        robot.turr1.setPower(turretPID);
+                        robot.turr2.setPower(-turretPID);
+                        return !servo.turretEqual(turret_redClose, robot.turr1Pos.getCurrentPosition());
+
+                    } else {
+                        robot.limelight.pipelineSwitch(2);
+                        double turretPID = servo.setTurrPos(turret_blueClose, robot.turr1Pos.getCurrentPosition());
+                        robot.turr1.setPower(turretPID);
+                        robot.turr2.setPower(-turretPID);
+                        return !servo.turretEqual(turret_blueClose, robot.turr1Pos.getCurrentPosition());
+                    }
+                } else {
+                    return true;
+                }
+            }
+        };
+    }
+
+    public Action spindex(double spindexer, int vel) {
+        return new Action() {
+            double spinPID = 0.0;
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
+                velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
+                robot.shooter1.setPower(powPID);
+                robot.shooter2.setPower(powPID);
+
+                spinPID = servo.setSpinPos(spindexer, robot.spin1Pos.getVoltage());
+                robot.spin1.setPower(spinPID);
+                robot.spin2.setPower(-spinPID);
+                TELE.addData("Velocity", velo);
+                TELE.addLine("spindex");
+                TELE.update();
+
+                drive.updatePoseEstimate();
+                teleStart = drive.localizer.getPose();
+
+                if (servo.spinEqual(spindexer, robot.spin1Pos.getVoltage())){
+                    robot.spin1.setPower(0);
+                    robot.spin2.setPower(0);
+                    return false;
+                } else {
+                    return true;
+                }
+            }
+        };
+    }
+
+    public Action Shoot(int vel) {
+        return new Action() {
+            int ticker = 1;
+            double initPos = 0.0;
+            double finalPos = 0.0;
+            boolean zeroNeeded = false;
+            boolean zeroPassed = false;
+            double currentPos = 0.0;
+            double prevPos = 0.0;
+
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                TELE.addData("Velocity", velo);
+                TELE.addLine("shooting");
+                TELE.update();
+
+                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
+                velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
+                robot.shooter1.setPower(powPID);
+                robot.shooter2.setPower(powPID);
+
+                robot.turr1.setPower(holdTurrPow);
+                robot.turr2.setPower(holdTurrPow);
+
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
+                if (ticker == 1){
+                    robot.transferServo.setPosition(transferServo_in);
+                    initPos = servo.getSpinPos(robot.spin1Pos.getVoltage());
+
+                    finalPos = initPos + 0.6;
+
+                    if (finalPos > 1.0){
+                        finalPos = finalPos - 1;
+                        zeroNeeded = true;
+                    } else if (finalPos > 0.95){
+                        finalPos = 0.0;
+                        zeroNeeded = true;
+                    }
+                    currentPos = initPos;
+                }
+                ticker++;
+
+                if (ticker > 16){
+                    robot.spin1.setPower(0.08);
+                    robot.spin2.setPower(-0.08);
+                }
+
+                prevPos = currentPos;
+                currentPos = servo.getSpinPos(robot.spin1Pos.getVoltage());
+                if (zeroNeeded){
+                    if (currentPos - prevPos < -0.5){
+                        zeroPassed = true;
+                    }
+                    if (zeroPassed){
+                        if (currentPos > finalPos){
+                            robot.spin1.setPower(0);
+                            robot.spin2.setPower(0);
+                            return false;
+                        } else {
+                            return true;
+                        }
+                    } else {
+                        return true;
+                    }
+                } else {
+                    if (currentPos > finalPos){
+                        robot.spin1.setPower(0);
+                        robot.spin2.setPower(0);
+                        return false;
+                    } else {
+                        return true;
+                    }
+                }
+
+
+
+            }
+        };
+    }
+
+    public Action intake(double intakeTime) {
+        return new Action() {
+            double position = spindexer_intakePos1;
+            double stamp = 0.0;
+            int ticker = 0;
+            double pow = 1.0;
+
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                if (ticker == 0) {
+                    stamp = getRuntime();
+                }
+                ticker++;
+
+                robot.intake.setPower(pow);
+
+                double s1D = robot.color1.getDistance(DistanceUnit.MM);
+                double s2D = robot.color2.getDistance(DistanceUnit.MM);
+                double s3D = robot.color3.getDistance(DistanceUnit.MM);
+
+                if (!servo.spinEqual(position, robot.spin1Pos.getVoltage())){
+                    double spinPID = servo.setSpinPos(position, robot.spin1Pos.getVoltage());
+                    robot.spin1.setPower(spinPID);
+                    robot.spin2.setPower(-spinPID);
+                }
+
+                if (s1D < 43 && servo.spinEqual(position, robot.spin1Pos.getVoltage()) && getRuntime() - stamp > 0.5){
+                    if (s2D > 60){
+                        if (servo.spinEqual(spindexer_intakePos1,robot.spin1Pos.getVoltage())){
+                            position = spindexer_intakePos2;
+                        } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){
+                            position = spindexer_intakePos3;
+                        } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){
+                            position = spindexer_intakePos1;
+                        }
+                    } else if (s3D > 33){
+                        if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){
+                            position = spindexer_intakePos3;
+                        } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){
+                            position = spindexer_intakePos1;
+                        } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){
+                            position = spindexer_intakePos2;
+                        }
+                    }
+                    stamp = getRuntime();
+                }
+
+                TELE.addData("Velocity", velo);
+                TELE.addLine("Intaking");
+                TELE.update();
+
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
+                robot.intake.setPower(1);
+                if ((s1D < 43.0 && s2D < 60.0 && s3D < 33.0) || getRuntime() - stamp > intakeTime) {
+                    robot.spin1.setPower(0);
+                    robot.spin2.setPower(0);
+                    if (getRuntime() - stamp - intakeTime < 1){
+                        pow = -2*(getRuntime() - stamp - intakeTime);
+                        return true;
+                    } else {
+                        robot.intake.setPower(0);
+                        return false;
+                    }
+                } else {
+                    return true;
+                }
+            }
+        };
+    }
+
+    public Action ColorDetect(int vel) {
+        return new Action() {
+            double stamp = 0.0;
+            int ticker = 0;
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                if (ticker == 0) {
+                    stamp = getRuntime();
+                }
+                ticker++;
+
+                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
+                velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
+                robot.shooter1.setPower(powPID);
+                robot.shooter2.setPower(powPID);
+
+                double s1D = robot.color1.getDistance(DistanceUnit.MM);
+                double s2D = robot.color2.getDistance(DistanceUnit.MM);
+                double s3D = robot.color3.getDistance(DistanceUnit.MM);
+
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
+                if (s1D < 43) {
+
+                    double green = robot.color1.getNormalizedColors().green;
+                    double red = robot.color1.getNormalizedColors().red;
+                    double blue = robot.color1.getNormalizedColors().blue;
+
+                    double gP = green / (green + red + blue);
+
+                    if (gP >= 0.4) {
+                        b1 = 2;
+                    } else {
+                        b1 = 1;
+                    }
+                }
+
+                if (s2D < 60) {
+
+                    double green = robot.color2.getNormalizedColors().green;
+                    double red = robot.color2.getNormalizedColors().red;
+                    double blue = robot.color2.getNormalizedColors().blue;
+
+                    double gP = green / (green + red + blue);
+
+                    if (gP >= 0.4) {
+                        b2 = 2;
+                    } else {
+                        b2 = 1;
+                    }
+                }
+
+                if (s3D < 33) {
+
+                    double green = robot.color3.getNormalizedColors().green;
+                    double red = robot.color3.getNormalizedColors().red;
+                    double blue = robot.color3.getNormalizedColors().blue;
+
+                    double gP = green / (green + red + blue);
+
+                    if (gP >= 0.4) {
+                        b3 = 2;
+                    } else {
+                        b3 = 1;
+                    }
+                }
+
+                TELE.addData("Velocity", velo);
+                TELE.addLine("Detecting");
+                TELE.addData("Distance 1", s1D);
+                TELE.addData("Distance 2", s2D);
+                TELE.addData("Distance 3", s3D);
+                TELE.addData("B1", b1);
+                TELE.addData("B2", b2);
+                TELE.addData("B3", b3);
+                TELE.update();
+
+                return (b1 + b2 + b3 < 4) && !(getRuntime() - stamp > colorDetect);
+            }
+        };
+    }
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        robot = new Robot(hardwareMap);
+
+        flywheel = new FlywheelV2();
+
+        TELE = new MultipleTelemetry(
+                telemetry, FtcDashboard.getInstance().getTelemetry()
+        );
+
+        drive = new MecanumDrive(hardwareMap, new Pose2d(
+                0, 0, 0
+        ));
+
+        servo = new Servos();
+
+        robot.limelight.pipelineSwitch(1);
+        robot.limelight.start();
+
+        TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
+                .strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
+
+        TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(bx1, by1, bh1))
+                .strafeToLinearHeading(new Vector2d(bx2a, by2a), bh2a)
+                .strafeToLinearHeading(new Vector2d(bx2b, by2b), bh2b);
+
+        TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(bx2b, by2b, bh2b))
+                .strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
+
+        TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(bx1, by1, bh1))
+                .strafeToLinearHeading(new Vector2d(bx3a, by3a), bh3a)
+                .strafeToLinearHeading(new Vector2d(bx3b, by3b), bh3b);
+
+        TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(bx3b, by3b, bh3b))
+                .strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
+
+        while (opModeInInit()) {
+
+            if (gamepad2.dpadUpWasPressed()) {
+                hoodAuto -= 0.01;
+            }
+
+            if (gamepad2.dpadDownWasPressed()) {
+                hoodAuto += 0.01;
+            }
+
+            if (gamepad2.crossWasPressed()){
+                redAlliance = !redAlliance;
+            }
+
+            if (redAlliance){
+                shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
+                        .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
+
+                pickup1 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1))
+                        .strafeToLinearHeading(new Vector2d(rx2a, ry2a), rh2a)
+                        .strafeToLinearHeading(new Vector2d(rx2b, ry2b), rh2b);
+
+                shoot1 = drive.actionBuilder(new Pose2d(rx2b, ry2b, rh2b))
+                        .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
+
+                pickup2 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1))
+                        .strafeToLinearHeading(new Vector2d(rx3a, ry3a), rh3a)
+                        .strafeToLinearHeading(new Vector2d(rx3b, ry3b), rh3b);
+
+                shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b))
+                        .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
+            } else {
+                shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
+                        .strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
+
+                pickup1 = drive.actionBuilder(new Pose2d(bx1, by1, bh1))
+                        .strafeToLinearHeading(new Vector2d(bx2a, by2a), bh2a)
+                        .strafeToLinearHeading(new Vector2d(bx2b, by2b), bh2b);
+
+                shoot1 = drive.actionBuilder(new Pose2d(bx2b, by2b, bh2b))
+                        .strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
+
+                pickup2 = drive.actionBuilder(new Pose2d(bx1, by1, bh1))
+                        .strafeToLinearHeading(new Vector2d(bx3a, by3a), bh3a)
+                        .strafeToLinearHeading(new Vector2d(bx3b, by3b), bh3b);
+
+                shoot2 = drive.actionBuilder(new Pose2d(bx3b, by3b, bh3b))
+                        .strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
+            }
+
+            robot.hood.setPosition(hoodAuto);
+
+            robot.transferServo.setPosition(transferServo_out);
+
+            TELE.addData("Red?", redAlliance);
+            TELE.update();
+        }
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        if (opModeIsActive()) {
+
+            Actions.runBlocking(
+                    new ParallelAction(
+                            shoot0.build(),
+                            initShooter(AUTO_CLOSE_VEL)
+                    )
+            );
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
+
+            robot.transfer.setPower(1);
+
+            shootingSequence();
+
+            robot.transfer.setPower(0);
+
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
+
+            Actions.runBlocking(
+                    new ParallelAction(
+                            pickup1.build(),
+                            intake(intake1Time)
+                    )
+            );
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
+
+            Actions.runBlocking(
+                    new ParallelAction(
+                            shoot1.build()
+                    )
+            );
+
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
+
+            robot.transfer.setPower(1);
+
+            shootingSequence();
+
+            robot.transfer.setPower(0);
+
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
+
+            Actions.runBlocking(
+                    new ParallelAction(
+                            pickup2.build(),
+                            intake(intake2Time)
+                    )
+            );
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
+
+            Actions.runBlocking(
+                    new ParallelAction(
+                            shoot2.build()
+                    )
+            );
+
+            robot.transfer.setPower(1);
+
+            shootingSequence();
+
+            robot.transfer.setPower(0);
+
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
+
+            TELE.addData("Velocity", velo);
+            TELE.addLine("finished");
+            TELE.update();
+
+            sleep(2000);
+
+        }
+
+    }
+    //TODO: adjust this according to Teleop numbers
+    public void detectTag() {
+        LLResult result = robot.limelight.getLatestResult();
+        if (result != null) {
+            if (result.isValid()) {
+                bearing = result.getTx();
+            }
+        }
+        double turretPos = servo.getTurrPos(robot.turr1Pos.getCurrentPosition()) - (bearing / 1300);
+        double turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition());
+        robot.turr1.setPower(turretPID);
+        robot.turr2.setPower(-turretPID);
+    }
+
+    public void shootingSequence() {
+        TELE.addLine("Shooting");
+        TELE.update();
+        Actions.runBlocking(Shoot(AUTO_CLOSE_VEL));
+    }
+
+    public void sequence1() {
+        Actions.runBlocking(
+                new SequentialAction(
+                        spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL)
+                )
+        );
+    }
+
+    public void sequence2() {
+        Actions.runBlocking(
+                new SequentialAction(
+                        spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL)
+                )
+        );
+    }
+
+    public void sequence3() {
+        Actions.runBlocking(
+                new SequentialAction(
+                        spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
+
+                        Shoot(AUTO_CLOSE_VEL)
+                )
+        );
+    }
+
+    public void sequence4() {
+        Actions.runBlocking(
+                new SequentialAction(
+                        spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL)
+                )
+        );
+    }
+
+    public void sequence5() {
+        Actions.runBlocking(
+                new SequentialAction(
+                        spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL)
+                )
+        );
+    }
+
+    public void sequence6() {
+        Actions.runBlocking(
+                new SequentialAction(
+                        spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL)
+                )
+        );
+    }
+}

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

@@ -169,10 +169,10 @@ public class Red_V2 extends LinearOpMode {
                 TELE.update();
 
                 if (gpp || pgp || ppg){
-                    double turretPID = servo.setTurrPos(turret_redClose);
+                    double turretPID = servo.setTurrPos(turret_redClose, robot.turr1Pos.getCurrentPosition());
                     robot.turr1.setPower(turretPID);
                     robot.turr2.setPower(-turretPID);
-                    return !servo.turretEqual(turret_redClose);
+                    return !servo.turretEqual(turret_redClose, robot.turr1Pos.getCurrentPosition());
                 } else {
                     return true;
                 }
@@ -199,7 +199,7 @@ public class Red_V2 extends LinearOpMode {
 
                 teleStart = drive.localizer.getPose();
 
-                return !servo.spinEqual(spindexer);
+                return !servo.spinEqual(spindexer, robot.spin1Pos.getVoltage());
             }
         };
     }
@@ -461,7 +461,7 @@ public class Red_V2 extends LinearOpMode {
 
             aprilTag.update();
             TELE.addData("Velocity", velo);
-            TELE.addData("Turret Pos", servo.getTurrPos());
+            TELE.addData("Turret Pos", servo.getTurrPos(robot.turr1Pos.getCurrentPosition()));
             TELE.update();
         }
 
@@ -585,8 +585,8 @@ public class Red_V2 extends LinearOpMode {
             bearing = d24.ftcPose.bearing;
             TELE.addData("Bear", bearing);
         }
-        double turretPos = servo.getTurrPos() - (bearing / 1300);
+        double turretPos = servo.getTurrPos(robot.turr1Pos.getCurrentPosition()) - (bearing / 1300);
-        double turretPID = servo.setTurrPos(turretPos);
+        double turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition());
         robot.turr1.setPower(turretPID);
         robot.turr2.setPower(-turretPID);
     }

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

@@ -14,13 +14,13 @@ public class Poses {
 
     public static Pose2d goalPose = new Pose2d(-15, 0, 0);
 
-    public static double rx1 = 45, ry1 = -7, rh1 = 0;
+    public static double rx1 = 40, ry1 = -7, rh1 = 0;
     public static double rx2a = 45, ry2a = 5, rh2a = Math.toRadians(140);
     public static double rx2b = 31, ry2b = 32, rh2b = Math.toRadians(140);
     public static double rx3a = 58, ry3a = 42, rh3a = Math.toRadians(140);
     public static double rx3b = 34, ry3b = 58, rh3b = Math.toRadians(140);
 
-    public static double bx1 = 45, by1 = 6, bh1 = 0;
+    public static double bx1 = 40, by1 = 6, bh1 = 0;
     public static double bx2a = 53, by2a = -7, bh2a = Math.toRadians(-140);
     public static double bx2b = 23, by2b = -39, bh2b = Math.toRadians(-140);
     public static double bx3a = 56, by3a = -34, bh3a = Math.toRadians(-140);

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

@@ -24,7 +24,7 @@ public class ServoPositions {
 
     public static double hoodDefault = 0.6;
 
-    public static double hoodAuto = 0.55;
+    public static double hoodAuto = 0.22;
 
     public static double hoodAutoFar = 0.5; //TODO: change this;
 

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

@@ -128,7 +128,7 @@ public class TeleopV2 extends LinearOpMode {
         TELE = new MultipleTelemetry(
                 telemetry, FtcDashboard.getInstance().getTelemetry()
         );
-        servo = new Servos(hardwareMap);
+        servo = new Servos();
         flywheel = new Flywheel();
 
         drive = new MecanumDrive(hardwareMap, teleStart);
@@ -159,13 +159,13 @@ public class TeleopV2 extends LinearOpMode {
             robot.backRight.setPower(backRightPower);
 
             // PID SERVOS
-            turretPID = servo.setTurrPos(turretPos);
+            turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition());
             robot.turr1.setPower(turretPID);
             robot.turr2.setPower(-turretPID);
 
             //TODO: make sure changing position works throughout opmode
-            if (!servo.spinEqual(spindexPos)){
+            if (!servo.spinEqual(spindexPos, robot.spin1Pos.getVoltage())){
-                spindexPID = servo.setSpinPos(spindexPos);
+                spindexPID = servo.setSpinPos(spindexPos, robot.spin1Pos.getVoltage());
                 robot.spin1.setPower(spindexPID);
                 robot.spin2.setPower(-spindexPID);
             } else{
@@ -348,7 +348,7 @@ public class TeleopV2 extends LinearOpMode {
                         bearing = d24.ftcPose.bearing;
                     }
                     overrideTurr = true;
-                    turretPos = servo.getTurrPos() - (bearing/1300);
+                    turretPos = servo.getTurrPos(robot.turr1Pos.getCurrentPosition()) - (bearing/1300);
                     TELE.addData("Bear", bearing);
 
                     double bearingCorrection = bearing / 1300;
@@ -481,13 +481,13 @@ public class TeleopV2 extends LinearOpMode {
                     boolean shootingDone = false;
 
                     if (!outtake1) {
-                        outtake1 = (servo.spinEqual(spindexer_outtakeBall1));
+                        outtake1 = (servo.spinEqual(spindexer_outtakeBall1, robot.spin1Pos.getVoltage()));
                     }
                     if (!outtake2) {
-                        outtake2 = (servo.spinEqual(spindexer_outtakeBall2));
+                        outtake2 = (servo.spinEqual(spindexer_outtakeBall2, robot.spin1Pos.getVoltage()));
                     }
                     if (!outtake3) {
-                        outtake3 = (servo.spinEqual(spindexer_outtakeBall3));
+                        outtake3 = (servo.spinEqual(spindexer_outtakeBall3, robot.spin1Pos.getVoltage()));
                     }
 
                     switch (currentSlot) {

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

@@ -16,7 +16,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
-import org.firstinspires.ftc.teamcode.utils.Flywheel;
+import org.firstinspires.ftc.teamcode.utils.FlywheelV2;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.utils.Servos;
 
@@ -29,7 +29,7 @@ public class TeleopV3 extends LinearOpMode {
     Robot robot;
     MultipleTelemetry TELE;
     Servos servo;
-    Flywheel flywheel;
+    FlywheelV2 flywheel;
     MecanumDrive drive;
 
     public static double manualVel = 3000;
@@ -98,8 +98,8 @@ public class TeleopV3 extends LinearOpMode {
 
         robot = new Robot(hardwareMap);
         TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
-        servo = new Servos(hardwareMap);
+        servo = new Servos();
-        flywheel = new Flywheel();
+        flywheel = new FlywheelV2();
         drive = new MecanumDrive(hardwareMap, teleStart);
 
         if (redAlliance) {
@@ -131,12 +131,12 @@ public class TeleopV3 extends LinearOpMode {
             robot.backRight.setPower(backRightPower);
 
             // PID SERVOS
-            turretPID = servo.setTurrPos(turretPos);
+            turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition());
             robot.turr1.setPower(turretPID);
             robot.turr2.setPower(-turretPID);
 
-            if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) {
+            if (!servo.spinEqual(spindexPos, robot.spin1Pos.getVoltage()) && !gamepad1.right_bumper) {
-                spindexPID = servo.setSpinPos(spindexPos);
+                spindexPID = servo.setSpinPos(spindexPos, robot.spin1Pos.getVoltage());
                 robot.spin1.setPower(spindexPID);
                 robot.spin2.setPower(-spindexPID);
             } else {
@@ -149,7 +149,7 @@ public class TeleopV3 extends LinearOpMode {
                 robot.transferServo.setPosition(transferServo_out);
                 intakeTicker++;
                 if (intakeTicker % 16 == 0) {
-                    spinCurrentPos = servo.getSpinPos();
+                    spinCurrentPos = servo.getSpinPos(robot.spin1Pos.getVoltage());
                     if (Math.abs(spinCurrentPos - spinInitPos) == 0.0) {
                         reverse = !reverse;
                     }
@@ -256,7 +256,7 @@ public class TeleopV3 extends LinearOpMode {
 
             //SHOOTER:
 
-            double powPID = flywheel.manageFlywheel((int) vel, robot.shooter1.getCurrentPosition());
+            double powPID = flywheel.manageFlywheel((int) vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
 
             robot.shooter1.setPower(powPID);
             robot.shooter2.setPower(powPID);
@@ -311,7 +311,7 @@ public class TeleopV3 extends LinearOpMode {
                     if (result.isValid()) {
                         bearing = result.getTx();
                         overrideTurr = true;
-                        turretPos = servo.getTurrPos() - (bearing / 1300);
+                        turretPos = servo.getTurrPos(robot.turr1Pos.getCurrentPosition()) - (bearing / 1300);
 
                         double bearingCorrection = bearing / 1300;
 
@@ -427,13 +427,13 @@ public class TeleopV3 extends LinearOpMode {
                     boolean shootingDone = false;
 
                     if (!outtake1) {
-                        outtake1 = (servo.spinEqual(spindexer_outtakeBall1));
+                        outtake1 = (servo.spinEqual(spindexer_outtakeBall1, robot.spin1Pos.getVoltage()));
                     }
                     if (!outtake2) {
-                        outtake2 = (servo.spinEqual(spindexer_outtakeBall2));
+                        outtake2 = (servo.spinEqual(spindexer_outtakeBall2, robot.spin1Pos.getVoltage()));
                     }
                     if (!outtake3) {
-                        outtake3 = (servo.spinEqual(spindexer_outtakeBall3));
+                        outtake3 = (servo.spinEqual(spindexer_outtakeBall3, robot.spin1Pos.getVoltage()));
                     }
 
                     switch (currentSlot) {

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

@@ -53,7 +53,7 @@ public class IntakeTest extends LinearOpMode {
             hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
         }
         robot = new Robot(hardwareMap);
-        servo = new Servos(hardwareMap);
+        servo = new Servos();
         TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
 
         waitForStart();
@@ -65,7 +65,7 @@ public class IntakeTest extends LinearOpMode {
                 if (gamepad1.right_bumper) {
                     ticker++;
                     if (ticker % 16 == 0){
-                        currentPos = servo.getSpinPos();
+                        currentPos = servo.getSpinPos(robot.spin1Pos.getVoltage());
                         if (Math.abs(currentPos - initPos) == 0.0){
                             reverse = !reverse;
                         }
@@ -109,19 +109,19 @@ public class IntakeTest extends LinearOpMode {
 
                 if (ballIn(1) && steadySpin && intake && getRuntime() - stamp > 0.5){
                     if (!ballIn(2)){
-                        if (servo.spinEqual(spindexer_intakePos1)){
+                        if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){
                             spindexerPos = spindexer_intakePos2;
-                        } else if (servo.spinEqual(spindexer_intakePos2)){
+                        } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){
                             spindexerPos = spindexer_intakePos3;
-                        } else if (servo.spinEqual(spindexer_intakePos3)){
+                        } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){
                             spindexerPos = spindexer_intakePos1;
                         }
                     } else if (!ballIn(3)){
-                        if (servo.spinEqual(spindexer_intakePos1)){
+                        if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){
                             spindexerPos = spindexer_intakePos3;
-                        } else if (servo.spinEqual(spindexer_intakePos2)){
+                        } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){
                             spindexerPos = spindexer_intakePos1;
-                        } else if (servo.spinEqual(spindexer_intakePos3)){
+                        } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){
                             spindexerPos = spindexer_intakePos2;
                         }
                     }
@@ -158,7 +158,7 @@ public class IntakeTest extends LinearOpMode {
             TELE.addData("B1", ballIn(1));
             TELE.addData("B2", ballIn(2));
             TELE.addData("B3", ballIn(3));
-            TELE.addData("Spindex Pos", servo.getSpinPos());
+            TELE.addData("Spindex Pos", servo.getSpinPos(robot.spin1Pos.getVoltage()));
             TELE.update();
         }
     }
@@ -187,10 +187,10 @@ public class IntakeTest extends LinearOpMode {
     }
 
     public void spindexer() {
-        boolean atTarget = servo.spinEqual(spindexerPos);
+        boolean atTarget = servo.spinEqual(spindexerPos, robot.spin1Pos.getVoltage());
 
         if (!atTarget) {
-            powPID = servo.setSpinPos(spindexerPos);
+            powPID = servo.setSpinPos(spindexerPos, robot.spin1Pos.getVoltage());
             robot.spin1.setPower(powPID);
             robot.spin2.setPower(-powPID);
 

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

@@ -46,7 +46,7 @@ public class PIDServoTest extends LinearOpMode {
             controller.setPIDF(p, i, d, f);
 
             if (mode == 0) {
-                pos = scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3);
+                pos = robot.turr1Pos.getCurrentPosition();
 
                 double pid = controller.calculate(pos, target);
 
@@ -62,7 +62,7 @@ public class PIDServoTest extends LinearOpMode {
             }
 
             telemetry.addData("pos", pos);
-            telemetry.addData("Turret Voltage", robot.turr1Pos.getVoltage());
+            telemetry.addData("Turret Voltage", robot.turr1Pos.getCurrentPosition());
             telemetry.addData("Spindex Voltage", robot.spin1Pos.getVoltage());
             telemetry.addData("target", target);
             telemetry.addData("Mode", mode);

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

@@ -9,7 +9,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 
-import org.firstinspires.ftc.teamcode.utils.Flywheel;
+import org.firstinspires.ftc.teamcode.utils.FlywheelV2;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 
 @Config
@@ -26,7 +26,7 @@ public class ShooterTest extends LinearOpMode {
     public static double turretPos = 0.501;
     public static boolean shoot = false;
     Robot robot;
-    Flywheel flywheel;
+    FlywheelV2 flywheel;
 
     @Override
     public void runOpMode() throws InterruptedException {
@@ -34,7 +34,7 @@ public class ShooterTest extends LinearOpMode {
         robot = new Robot(hardwareMap);
         DcMotorEx leftShooter = robot.shooter1;
         DcMotorEx rightShooter = robot.shooter2;
-        flywheel = new Flywheel();
+        flywheel = new FlywheelV2();
 
         MultipleTelemetry TELE = new MultipleTelemetry(
                 telemetry, FtcDashboard.getInstance().getTelemetry()
@@ -50,7 +50,7 @@ public class ShooterTest extends LinearOpMode {
                 rightShooter.setPower(parameter);
                 leftShooter.setPower(parameter);
             } else if (mode == 1) {
-                double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition());
+                double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
                 rightShooter.setPower(powPID);
                 leftShooter.setPower(powPID);
                 TELE.addData("PIDPower", powPID);
@@ -71,7 +71,9 @@ public class ShooterTest extends LinearOpMode {
             } else {
                 robot.transferServo.setPosition(transferServo_out);
             }
-            TELE.addData("Velocity", flywheel.getVelo());
+            TELE.addData("Velocity", flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()));
+            TELE.addData("Velocity 1", flywheel.getVelo1());
+            TELE.addData("Velocity 2", flywheel.getVelo2());
             TELE.addData("Power", robot.shooter1.getPower());
             TELE.addData("Steady?", flywheel.getSteady());
             TELE.addData("Position", robot.shooter1.getCurrentPosition());

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

@@ -29,12 +29,12 @@ public class PositionalServoProgrammer extends LinearOpMode {
     public void runOpMode() throws InterruptedException {
         robot = new Robot(hardwareMap);
         TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
-        servo = new Servos(hardwareMap);
+        servo = new Servos();
         waitForStart();
         if (isStopRequested()) return;
         while (opModeIsActive()){
-            if (spindexPos != 0.501 && !servo.spinEqual(spindexPos) && mode == 0){
+            if (spindexPos != 0.501 && !servo.spinEqual(spindexPos, robot.spin1Pos.getVoltage()) && mode == 0){
-                double pos = servo.setSpinPos(spindexPos);
+                double pos = servo.setSpinPos(spindexPos, robot.spin1Pos.getVoltage());
                 robot.spin1.setPower(pos);
                 robot.spin2.setPower(-pos);
             } else if (mode == 0){
@@ -44,8 +44,8 @@ public class PositionalServoProgrammer extends LinearOpMode {
                 robot.spin1.setPower(spindexPow);
                 robot.spin2.setPower(-spindexPow);
             }
-            if (turretPos != 0.501 && !servo.turretEqual(turretPos)){
+            if (turretPos != 0.501 && !servo.turretEqual(turretPos, robot.turr1Pos.getCurrentPosition())){
-                double pos = servo.setTurrPos(turretPos);
+                double pos = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition());
                 robot.turr1.setPower(pos);
                 robot.turr2.setPower(-pos);
             } else if (mode == 0){
@@ -61,13 +61,25 @@ public class PositionalServoProgrammer extends LinearOpMode {
             if (hoodPos != 0.501){
                 robot.hood.setPosition(hoodPos);
             }
-            TELE.addData("spindexer", servo.getSpinPos());
+            // To check configuration of spindexer:
-            TELE.addData("turret", servo.getTurrPos());
+            // Set "mode" to 1 and spindexPow to 0.1
-            TELE.addData("spindexer voltage", robot.spin1Pos.getVoltage());
+            // If the spindexer is turning clockwise, the servos are reversed. Swap the configuration of the two servos, DO NOT TOUCH THE ACTUAL CODE
-            TELE.addData("hood voltage", robot.hoodPos.getVoltage());
+            // Do the previous test again to confirm
+            // Set "mode" to 0 but keep spindexPos at 0.501
+            // Manually turn the spindexer until "spindexer pos" is set close to 0
+            // Check each spindexer voltage:
+                // If "spindexer voltage 1" is closer to 0 than "spindexer voltage 2," then you are done!
+                // If "spindexer voltage 2" is closer to 0 than "spindexer voltage 1," swap the two spindexer analog inputs in the configuration, DO NOT TOUCH THE ACTUAL CODE
+            //TODO: @KeshavAnandCode do the above please
+
+            TELE.addData("spindexer pos", servo.getSpinPos(robot.spin1Pos.getVoltage()));
+            TELE.addData("turret pos", servo.getTurrPos(robot.turr1Pos.getCurrentPosition()));
+            TELE.addData("spindexer voltage 1", robot.spin1Pos.getVoltage());
+            TELE.addData("spindexer voltage 2", robot.spin2Pos.getVoltage());
+            TELE.addData("hood pos", robot.hood.getPosition());
             TELE.addData("transferServo voltage", robot.transferServoPos.getVoltage());
-            TELE.addData("turret voltage", robot.turr1Pos.getVoltage());
+            TELE.addData("turret voltage", robot.turr1Pos.getCurrentPosition());
-            TELE.addData("Spin Equal", servo.spinEqual(spindexPos));
+            TELE.addData("spindexer pow", robot.spin1.getPower());
             TELE.update();
         }
     }

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

@@ -7,7 +7,6 @@ import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.hardware.DigitalChannel;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.Servo;
 
@@ -28,28 +27,16 @@ public class Robot {
     public DcMotorEx shooter2;
     public Servo hood;
     public Servo transferServo;
-    public Servo rejecter;
     public CRServo turr1;
     public CRServo turr2;
     public CRServo spin1;
     public CRServo spin2;
-    public DigitalChannel pin0;
-    public DigitalChannel pin1;
-    public DigitalChannel pin2;
-    public DigitalChannel pin3;
-    public DigitalChannel pin4;
-    public DigitalChannel pin5;
-    public AnalogInput analogInput;
-    public AnalogInput analogInput2;
     public AnalogInput spin1Pos;
     public AnalogInput spin2Pos;
-    public AnalogInput hoodPos;
+    public DcMotorEx turr1Pos;
-    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;
@@ -57,10 +44,12 @@ public class Robot {
 
     public static boolean usingLimelight = true;
 
+    public static boolean usingCamera = true;
+
     public Robot(HardwareMap hardwareMap) {
 
         //Define components w/ hardware map
-
+        //TODO: fix the configuration of these - I trust you to figure it out yourself @KeshavAnandCode
         frontLeft = hardwareMap.get(DcMotorEx.class, "fl");
         frontRight = hardwareMap.get(DcMotorEx.class, "fr");
         backLeft = hardwareMap.get(DcMotorEx.class, "bl");
@@ -74,30 +63,24 @@ public class Robot {
         backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
 
         intake = hardwareMap.get(DcMotorEx.class, "intake");
-        rejecter = hardwareMap.get(Servo.class, "rejecter");
 
         shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1");
 
         shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
-
+        //TODO: figure out which shooter motor is reversed using ShooterTest and change it in code @KeshavAnandCode
         shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
         shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
-        shooterEncoder = shooter1;
-
         hood = hardwareMap.get(Servo.class, "hood");
 
-        hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos");
-
         turr1 = hardwareMap.get(CRServo.class, "t1");
 
-        turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos");
-
         turr2 = hardwareMap.get(CRServo.class, "t2");
 
-        turr2Pos = hardwareMap.get(AnalogInput.class, "t2Pos");
+        turr1Pos = intake; // Encoder of turret plugged in intake port
 
+        //TODO: check spindexer configuration (both servo and analog input) - check comments in PositionalServoProgrammer
         spin1 = hardwareMap.get(CRServo.class, "spin1");
 
         spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
@@ -109,22 +92,6 @@ public class Robot {
         spin1.setDirection(DcMotorSimple.Direction.REVERSE);
         spin2.setDirection(DcMotorSimple.Direction.REVERSE);
 
-        pin0 = hardwareMap.get(DigitalChannel.class, "pin0");
-
-        pin1 = hardwareMap.get(DigitalChannel.class, "pin1");
-
-        pin2 = hardwareMap.get(DigitalChannel.class, "pin2");
-
-        pin3 = hardwareMap.get(DigitalChannel.class, "pin3");
-
-        pin4 = hardwareMap.get(DigitalChannel.class, "pin4");
-
-        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");
@@ -132,10 +99,7 @@ public class Robot {
         transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos");
 
         transfer.setDirection(DcMotorSimple.Direction.REVERSE);
-
+        transfer.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();
-
-        webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
 
         color1 = hardwareMap.get(RevColorSensorV3.class, "c1");
 
@@ -145,6 +109,9 @@ public class Robot {
 
         if (usingLimelight){
             limelight = hardwareMap.get(Limelight3A.class, "limelight");
+        } else if (usingCamera){
+            webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
+            aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();
         }
     }
 }

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

@@ -22,39 +22,38 @@ public class Servos {
     public static double turret_scalar = 1.009;
     public static double turret_restPos = 0.0;
 
-    public Servos(HardwareMap hardwareMap) {
+    public Servos() {
-        robot = new Robot(hardwareMap);
         spinPID = new PIDFController(spinP, spinI, spinD, spinF);
         turretPID = new PIDFController(turrP, turrI, turrD, turrF);
     }
 
     // In the code below, encoder = robot.servo.getVoltage()
 
-    public double getSpinPos() {
+    public double getSpinPos(double voltage) {
-        return spin_scalar * ((robot.spin1Pos.getVoltage() - spin_restPos) / 3.3);
+        return spin_scalar * ((voltage - spin_restPos) / 3.3);
     }
     //TODO: PID warp so 0 and 1 are usable positions
-    public double setSpinPos(double pos) {
+    public double setSpinPos(double pos, double voltage) {
         spinPID.setPIDF(spinP, spinI, spinD, spinF);
 
-        return spinPID.calculate(this.getSpinPos(), pos);
+        return spinPID.calculate(this.getSpinPos(voltage), pos);
     }
 
-    public boolean spinEqual(double pos) {
+    public boolean spinEqual(double pos, double voltage) {
-        return Math.abs(pos - this.getSpinPos()) < 0.02;
+        return Math.abs(pos - this.getSpinPos(voltage)) < 0.02;
     }
 
-    public double getTurrPos() {
+    public double getTurrPos(double apos) {
-        return turret_scalar * ((robot.turr1Pos.getVoltage() - turret_restPos) / 3.3);
+        return apos;
     }
 
-    public double setTurrPos(double pos) {
+    public double setTurrPos(double pos, double apos) {
         turretPID.setPIDF(turrP, turrI, turrD, turrF);
 
-        return spinPID.calculate(this.getTurrPos(), pos);
+        return spinPID.calculate(this.getTurrPos(apos), pos);
     }
 
-    public boolean turretEqual(double pos) {
+    public boolean turretEqual(double pos, double apos) {
-        return Math.abs(pos - this.getTurrPos()) < 0.01;
+        return Math.abs(pos - this.getTurrPos(apos)) < 0.01;
     }
 }