Merge branch 'danielv2'

# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java

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.01; // power to hold turret in place
+    public static double holdTurrPow = 0.1; // 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, robot.turr1Pos.getCurrentPosition());
+                        double turretPID = servo.setTurrPos(turret_redClose);
                         robot.turr1.setPower(turretPID);
                         robot.turr2.setPower(-turretPID);
-                        return !servo.turretEqual(turret_redClose, robot.turr1Pos.getCurrentPosition());
+                        return !servo.turretEqual(turret_redClose);
 
                     } else {
                         robot.limelight.pipelineSwitch(2);
-                        double turretPID = servo.setTurrPos(turret_blueClose, robot.turr1Pos.getCurrentPosition());
+                        double turretPID = servo.setTurrPos(turret_blueClose);
                         robot.turr1.setPower(turretPID);
                         robot.turr2.setPower(-turretPID);
-                        return !servo.turretEqual(turret_blueClose, robot.turr1Pos.getCurrentPosition());
+                        return !servo.turretEqual(turret_blueClose);
                     }
                 } 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, robot.spin1Pos.getVoltage());
+                spinPID = servo.setSpinPos(spindexer);
                 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, robot.spin1Pos.getVoltage())){
+                if (servo.spinEqual(spindexer)){
                     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, robot.spin1Pos.getVoltage())){
+                if (!servo.spinEqual(position)){
-                    double spinPID = servo.setSpinPos(position, robot.spin1Pos.getVoltage());
+                    double spinPID = servo.setSpinPos(position);
                     robot.spin1.setPower(spinPID);
                     robot.spin2.setPower(-spinPID);
                 }
 
-                if (s1D < 43 && servo.spinEqual(position, robot.spin1Pos.getVoltage()) && getRuntime() - stamp > 0.5){
+                if (s1D < 43 && servo.spinEqual(position) && getRuntime() - stamp > 0.5){
                     if (s2D > 60){
-                        if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){
+                        if (servo.spinEqual(spindexer_intakePos1)){
                             position = spindexer_intakePos2;
-                        } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){
+                        } else if (servo.spinEqual(spindexer_intakePos2)){
                             position = spindexer_intakePos3;
-                        } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){
+                        } else if (servo.spinEqual(spindexer_intakePos3)){
                             position = spindexer_intakePos1;
                         }
                     } else if (s3D > 33){
-                        if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){
+                        if (servo.spinEqual(spindexer_intakePos1)){
                             position = spindexer_intakePos3;
-                        } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){
+                        } else if (servo.spinEqual(spindexer_intakePos2)){
                             position = spindexer_intakePos1;
-                        } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){
+                        } else if (servo.spinEqual(spindexer_intakePos3)){
                             position = spindexer_intakePos2;
                         }
                     }
@@ -411,7 +411,7 @@ public class AutoClose_V3 extends LinearOpMode {
             double turrPID;
 
             if (redAlliance){
-                turrPID = servo.setTurrPos(turret_detectRedClose, robot.turr1Pos.getCurrentPosition());
+                turrPID = servo.setTurrPos(turret_detectRedClose);
 
                 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, robot.turr1Pos.getCurrentPosition());
+                turrPID = servo.setTurrPos(turret_detectBlueClose);
 
                 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(robot.turr1Pos.getCurrentPosition()));
+            TELE.addData("Turret Pos", servo.getTurrPos());
-            TELE.addData("Spin Pos", servo.getSpinPos(robot.spin1Pos.getVoltage()));
+            TELE.addData("Spin Pos", servo.getSpinPos());
             TELE.update();
         }
 
@@ -565,8 +565,8 @@ public class AutoClose_V3 extends LinearOpMode {
                 bearing = result.getTx();
             }
         }
-        double turretPos = servo.getTurrPos(robot.turr1Pos.getCurrentPosition()) - (bearing / 1300);
+        double turretPos = servo.getTurrPos() - (bearing / 1300);
-        double turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition());
+        double turretPID = servo.setTurrPos(turretPos);
         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.01; // power to hold turret in place
+    public static double holdTurrPow = 0.1; // 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, robot.turr1Pos.getCurrentPosition());
+                        double turretPID = servo.setTurrPos(turret_redFar);
                         robot.turr1.setPower(turretPID);
                         robot.turr2.setPower(-turretPID);
-                        return !servo.turretEqual(turret_redFar, robot.turr1Pos.getCurrentPosition());
+                        return !servo.turretEqual(turret_redFar);
 
                     } else {
                         robot.limelight.pipelineSwitch(2);
-                        double turretPID = servo.setTurrPos(turret_blueFar, robot.turr1Pos.getCurrentPosition());
+                        double turretPID = servo.setTurrPos(turret_blueFar);
                         robot.turr1.setPower(turretPID);
                         robot.turr2.setPower(-turretPID);
-                        return !servo.turretEqual(turret_blueFar, robot.turr1Pos.getCurrentPosition());
+                        return !servo.turretEqual(turret_blueFar);
                     }
                 } 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, robot.spin1Pos.getVoltage());
+                spinPID = servo.setSpinPos(spindexer);
                 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, robot.spin1Pos.getVoltage())){
+                if (servo.spinEqual(spindexer)){
                     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, robot.spin1Pos.getVoltage())){
+                if (!servo.spinEqual(position)){
-                    double spinPID = servo.setSpinPos(position, robot.spin1Pos.getVoltage());
+                    double spinPID = servo.setSpinPos(position);
                     robot.spin1.setPower(spinPID);
                     robot.spin2.setPower(-spinPID);
                 }
 
-                if (s1D < 43 && servo.spinEqual(position, robot.spin1Pos.getVoltage()) && getRuntime() - stamp > 0.5){
+                if (s1D < 43 && servo.spinEqual(position) && getRuntime() - stamp > 0.5){
                     if (s2D > 60){
-                        if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){
+                        if (servo.spinEqual(spindexer_intakePos1)){
                             position = spindexer_intakePos2;
-                        } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){
+                        } else if (servo.spinEqual(spindexer_intakePos2)){
                             position = spindexer_intakePos3;
-                        } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){
+                        } else if (servo.spinEqual(spindexer_intakePos3)){
                             position = spindexer_intakePos1;
                         }
                     } else if (s3D > 33){
-                        if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){
+                        if (servo.spinEqual(spindexer_intakePos1)){
                             position = spindexer_intakePos3;
-                        } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){
+                        } else if (servo.spinEqual(spindexer_intakePos2)){
                             position = spindexer_intakePos1;
-                        } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){
+                        } else if (servo.spinEqual(spindexer_intakePos3)){
                             position = spindexer_intakePos2;
                         }
                     }
@@ -399,9 +399,9 @@ public class AutoFar_V1 extends LinearOpMode {
             double turrPID;
 
             if (redAlliance){
-                turrPID = servo.setTurrPos(turret_detectRedClose, robot.turr1Pos.getCurrentPosition());
+                turrPID = servo.setTurrPos(turret_detectRedClose);
             } else {
-                turrPID = servo.setTurrPos(turret_detectBlueClose, robot.turr1Pos.getCurrentPosition());
+                turrPID = servo.setTurrPos(turret_detectBlueClose);
             }
 
             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(robot.turr1Pos.getCurrentPosition()));
+            TELE.addData("Turret Pos", servo.getTurrPos());
-            TELE.addData("Spin Pos", servo.getSpinPos(robot.spin1Pos.getVoltage()));
+            TELE.addData("Spin Pos", servo.getSpinPos());
             TELE.update();
         }
 
@@ -466,8 +466,8 @@ public class AutoFar_V1 extends LinearOpMode {
                 bearing = result.getTx();
             }
         }
-        double turretPos = servo.getTurrPos(robot.turr1Pos.getCurrentPosition()) - (bearing / 1300);
+        double turretPos = servo.getTurrPos() - (bearing / 1300);
-        double turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition());
+        double turretPID = servo.setTurrPos(turretPos);
         robot.turr1.setPower(turretPID);
         robot.turr2.setPower(-turretPID);
     }

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

@@ -1,668 +0,0 @@
-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 stamp = 0.0;
-            int ticker = 0;
-            double spinCurrentPos = 0.0;
-            double spinInitPos = 0.0;
-            boolean reverse = false;
-
-            @Override
-            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                if (ticker == 0) {
-                    stamp = getRuntime();
-                }
-                ticker++;
-
-                if (ticker % 12 < 3) {
-
-                    robot.spin1.setPower(-1);
-                    robot.spin2.setPower(1);
-
-                } else if (reverse) {
-                    robot.spin1.setPower(1);
-                    robot.spin2.setPower(-1);
-                } else {
-                    robot.spin1.setPower(-0.15);
-                    robot.spin2.setPower(0.15);
-                }
-                robot.intake.setPower(1);
-                TELE.addData("Reverse?", reverse);
-                TELE.update();
-
-                if (getRuntime() - stamp > intakeTime){
-                    if (reverse){
-                        robot.spin1.setPower(-1);
-                        robot.spin2.setPower(1);
-                    } else {
-                        robot.spin1.setPower(1);
-                        robot.spin2.setPower(-1);
-                    }
-                    return false;
-                } else {
-                    if (ticker % 4 == 0) {
-                        spinCurrentPos = servo.getSpinPos(robot.spin1Pos.getVoltage());
-                        reverse = Math.abs(spinCurrentPos - spinInitPos) < 0.02;
-                        spinInitPos = spinCurrentPos;
-                    }
-                    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;
-            }
-
-
-            double turretPID;
-            if (redAlliance){
-                turretPID = servo.setTurrPos(turret_redClose, robot.turr1Pos.getCurrentPosition());
-
-                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 {
-                turretPID = servo.setTurrPos(turret_blueClose, robot.turr1Pos.getCurrentPosition());
-
-                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.turr1.setPower(turretPID);
-            robot.turr2.setPower(-turretPID);
-
-            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, robot.turr1Pos.getCurrentPosition());
+                    double turretPID = servo.setTurrPos(turret_redClose);
                     robot.turr1.setPower(turretPID);
                     robot.turr2.setPower(-turretPID);
-                    return !servo.turretEqual(turret_redClose, robot.turr1Pos.getCurrentPosition());
+                    return !servo.turretEqual(turret_redClose);
                 } else {
                     return true;
                 }
@@ -199,7 +199,7 @@ public class Red_V2 extends LinearOpMode {
 
                 teleStart = drive.localizer.getPose();
 
-                return !servo.spinEqual(spindexer, robot.spin1Pos.getVoltage());
+                return !servo.spinEqual(spindexer);
             }
         };
     }
@@ -461,7 +461,7 @@ public class Red_V2 extends LinearOpMode {
 
             aprilTag.update();
             TELE.addData("Velocity", velo);
-            TELE.addData("Turret Pos", servo.getTurrPos(robot.turr1Pos.getCurrentPosition()));
+            TELE.addData("Turret Pos", servo.getTurrPos());
             TELE.update();
         }
 
@@ -585,8 +585,8 @@ public class Red_V2 extends LinearOpMode {
             bearing = d24.ftcPose.bearing;
             TELE.addData("Bear", bearing);
         }
-        double turretPos = servo.getTurrPos(robot.turr1Pos.getCurrentPosition()) - (bearing / 1300);
+        double turretPos = servo.getTurrPos() - (bearing / 1300);
-        double turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition());
+        double turretPID = servo.setTurrPos(turretPos);
         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 = 40, ry1 = -7, rh1 = 0;
+    public static double rx1 = 45, 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 = 40, by1 = 6, bh1 = 0;
+    public static double bx1 = 45, 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.22;
+    public static double hoodAuto = 0.55;
 
     public static double hoodAutoFar = 0.5; //TODO: change this;
 

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

@@ -19,6 +19,6 @@ public class ShooterVars {
     public static double maxStep = 0.06;         // prevents sudden jumps
 
     // VELOCITY CONSTANTS
-    public static int AUTO_CLOSE_VEL = 3000; //3300;
+    public static int AUTO_CLOSE_VEL = 3025; //3300;
     public static int AUTO_FAR_VEL = 4000; //TODO: test 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();
+        servo = new Servos(hardwareMap);
         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, robot.turr1Pos.getCurrentPosition());
+            turretPID = servo.setTurrPos(turretPos);
             robot.turr1.setPower(turretPID);
             robot.turr2.setPower(-turretPID);
 
             //TODO: make sure changing position works throughout opmode
-            if (!servo.spinEqual(spindexPos, robot.spin1Pos.getVoltage())){
+            if (!servo.spinEqual(spindexPos)){
-                spindexPID = servo.setSpinPos(spindexPos, robot.spin1Pos.getVoltage());
+                spindexPID = servo.setSpinPos(spindexPos);
                 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(robot.turr1Pos.getCurrentPosition()) - (bearing/1300);
+                    turretPos = servo.getTurrPos() - (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, robot.spin1Pos.getVoltage()));
+                        outtake1 = (servo.spinEqual(spindexer_outtakeBall1));
                     }
                     if (!outtake2) {
-                        outtake2 = (servo.spinEqual(spindexer_outtakeBall2, robot.spin1Pos.getVoltage()));
+                        outtake2 = (servo.spinEqual(spindexer_outtakeBall2));
                     }
                     if (!outtake3) {
-                        outtake3 = (servo.spinEqual(spindexer_outtakeBall3, robot.spin1Pos.getVoltage()));
+                        outtake3 = (servo.spinEqual(spindexer_outtakeBall3));
                     }
 
                     switch (currentSlot) {

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

@@ -98,7 +98,7 @@ public class TeleopV3 extends LinearOpMode {
 
         robot = new Robot(hardwareMap);
         TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
-        servo = new Servos();
+        servo = new Servos(hardwareMap);
         flywheel = new FlywheelV2();
         drive = new MecanumDrive(hardwareMap, teleStart);
 
@@ -131,12 +131,12 @@ public class TeleopV3 extends LinearOpMode {
             robot.backRight.setPower(backRightPower);
 
             // PID SERVOS
-            turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition());
+            turretPID = servo.setTurrPos(turretPos);
             robot.turr1.setPower(turretPID);
             robot.turr2.setPower(-turretPID);
 
-            if (!servo.spinEqual(spindexPos, robot.spin1Pos.getVoltage()) && !gamepad1.right_bumper) {
+            if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) {
-                spindexPID = servo.setSpinPos(spindexPos, robot.spin1Pos.getVoltage());
+                spindexPID = servo.setSpinPos(spindexPos);
                 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(robot.spin1Pos.getVoltage());
+                    spinCurrentPos = servo.getSpinPos();
                     if (Math.abs(spinCurrentPos - spinInitPos) == 0.0) {
                         reverse = !reverse;
                     }
@@ -311,7 +311,7 @@ public class TeleopV3 extends LinearOpMode {
                     if (result.isValid()) {
                         bearing = result.getTx();
                         overrideTurr = true;
-                        turretPos = servo.getTurrPos(robot.turr1Pos.getCurrentPosition()) - (bearing / 1300);
+                        turretPos = servo.getTurrPos() - (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, robot.spin1Pos.getVoltage()));
+                        outtake1 = (servo.spinEqual(spindexer_outtakeBall1));
                     }
                     if (!outtake2) {
-                        outtake2 = (servo.spinEqual(spindexer_outtakeBall2, robot.spin1Pos.getVoltage()));
+                        outtake2 = (servo.spinEqual(spindexer_outtakeBall2));
                     }
                     if (!outtake3) {
-                        outtake3 = (servo.spinEqual(spindexer_outtakeBall3, robot.spin1Pos.getVoltage()));
+                        outtake3 = (servo.spinEqual(spindexer_outtakeBall3));
                     }
 
                     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();
+        servo = new Servos(hardwareMap);
         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(robot.spin1Pos.getVoltage());
+                        currentPos = servo.getSpinPos();
                         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, robot.spin1Pos.getVoltage())){
+                        if (servo.spinEqual(spindexer_intakePos1)){
                             spindexerPos = spindexer_intakePos2;
-                        } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){
+                        } else if (servo.spinEqual(spindexer_intakePos2)){
                             spindexerPos = spindexer_intakePos3;
-                        } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){
+                        } else if (servo.spinEqual(spindexer_intakePos3)){
                             spindexerPos = spindexer_intakePos1;
                         }
                     } else if (!ballIn(3)){
-                        if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){
+                        if (servo.spinEqual(spindexer_intakePos1)){
                             spindexerPos = spindexer_intakePos3;
-                        } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){
+                        } else if (servo.spinEqual(spindexer_intakePos2)){
                             spindexerPos = spindexer_intakePos1;
-                        } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){
+                        } else if (servo.spinEqual(spindexer_intakePos3)){
                             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(robot.spin1Pos.getVoltage()));
+            TELE.addData("Spindex Pos", servo.getSpinPos());
             TELE.update();
         }
     }
@@ -187,10 +187,10 @@ public class IntakeTest extends LinearOpMode {
     }
 
     public void spindexer() {
-        boolean atTarget = servo.spinEqual(spindexerPos, robot.spin1Pos.getVoltage());
+        boolean atTarget = servo.spinEqual(spindexerPos);
 
         if (!atTarget) {
-            powPID = servo.setSpinPos(spindexerPos, robot.spin1Pos.getVoltage());
+            powPID = servo.setSpinPos(spindexerPos);
             robot.spin1.setPower(powPID);
             robot.spin2.setPower(-powPID);
 

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();
+        servo = new Servos(hardwareMap);
         waitForStart();
         if (isStopRequested()) return;
         while (opModeIsActive()){
-            if (spindexPos != 0.501 && !servo.spinEqual(spindexPos, robot.spin1Pos.getVoltage()) && mode == 0){
+            if (spindexPos != 0.501 && !servo.spinEqual(spindexPos) && mode == 0){
-                double pos = servo.setSpinPos(spindexPos, robot.spin1Pos.getVoltage());
+                double pos = servo.setSpinPos(spindexPos);
                 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, robot.turr1Pos.getCurrentPosition())){
+            if (turretPos != 0.501 && !servo.turretEqual(turretPos)){
-                double pos = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition());
+                double pos = servo.setTurrPos(turretPos);
                 robot.turr1.setPower(pos);
                 robot.turr2.setPower(-pos);
             } else if (mode == 0){
@@ -72,8 +72,8 @@ public class PositionalServoProgrammer extends LinearOpMode {
                 // 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("spindexer pos", servo.getSpinPos());
-            TELE.addData("turret pos", servo.getTurrPos(robot.turr1Pos.getCurrentPosition()));
+            TELE.addData("turret pos", servo.getTurrPos());
             TELE.addData("spindexer voltage 1", robot.spin1Pos.getVoltage());
             TELE.addData("spindexer voltage 2", robot.spin2Pos.getVoltage());
             TELE.addData("hood pos", robot.hood.getPosition());

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

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