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)){
-                    double spinPID = servo.setSpinPos(position);
+                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) && 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("Spin Pos", servo.getSpinPos());
+            TELE.addData("Turret Pos", servo.getTurrPos(robot.turr1Pos.getCurrentPosition()));
+            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 turretPID = servo.setTurrPos(turretPos);
+        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);
     }

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)){
-                    double spinPID = servo.setSpinPos(position);
+                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) && 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("Spin Pos", servo.getSpinPos());
+            TELE.addData("Turret Pos", servo.getTurrPos(robot.turr1Pos.getCurrentPosition()));
+            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 turretPID = servo.setTurrPos(turretPos);
+        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);
     }

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

@@ -0,0 +1,668 @@
+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);
+                    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 turretPID = servo.setTurrPos(turretPos);
+        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);
     }

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/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 = 3025; //3300;
+    public static int AUTO_CLOSE_VEL = 3000; //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(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)){
-                spindexPID = servo.setSpinPos(spindexPos);
+            if (!servo.spinEqual(spindexPos, robot.spin1Pos.getVoltage())){
+                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

@@ -98,7 +98,7 @@ 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 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);
+            turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition());
             robot.turr1.setPower(turretPID);
             robot.turr2.setPower(-turretPID);
 
-            if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) {
-                spindexPID = servo.setSpinPos(spindexPos);
+            if (!servo.spinEqual(spindexPos, robot.spin1Pos.getVoltage()) && !gamepad1.right_bumper) {
+                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;
                     }
@@ -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/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){
-                double pos = servo.setSpinPos(spindexPos);
+            if (spindexPos != 0.501 && !servo.spinEqual(spindexPos, robot.spin1Pos.getVoltage()) && mode == 0){
+                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)){
-                double pos = servo.setTurrPos(turretPos);
+            if (turretPos != 0.501 && !servo.turretEqual(turretPos, robot.turr1Pos.getCurrentPosition())){
+                double pos = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition());
                 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());
-            TELE.addData("turret pos", servo.getTurrPos());
+            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());

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) {
-        robot = new Robot(hardwareMap);
+    public Servos() {
         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() {
-        return spin_scalar * ((robot.spin1Pos.getVoltage() - spin_restPos) / 3.3);
+    public double getSpinPos(double voltage) {
+        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) {
-        return Math.abs(pos - this.getSpinPos()) < 0.02;
+    public boolean spinEqual(double pos, double voltage) {
+        return Math.abs(pos - this.getSpinPos(voltage)) < 0.02;
     }
 
-    public double getTurrPos() {
-        return robot.turr1Pos.getCurrentPosition();
+    public double getTurrPos(double apos) {
+        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) {
-        return Math.abs(pos - this.getTurrPos()) < 0.01;
+    public boolean turretEqual(double pos, double apos) {
+        return Math.abs(pos - this.getTurrPos(apos)) < 0.01;
     }
 }