remodeled close auto and it works except for poses (@KeshavAnandCode that is your job

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

@@ -2,7 +2,7 @@ package org.firstinspires.ftc.teamcode.autonomous;
 
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
 import static org.firstinspires.ftc.teamcode.constants.Front_Poses.*;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
 import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
 import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
@@ -31,12 +31,8 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
 @Config
 @Autonomous(preselectTeleOp = "TeleopV3")
 public class Auto_LT_Close extends LinearOpMode {
-    public static double shoot0Vel = 2300, shoot0Hood = 0.93 + hoodOffset;
+    public static double shoot0Vel = 2300, shoot0Hood = 0.93;
-    public static double autoSpinStartPos = 0.2;
+    public static double spindexerSpeedIncrease = 0.012;
-    public static double shoot0SpinSpeedIncrease = 0.005;
-
-    public static double spindexerSpeedIncrease = 0.005;
-    public static double finalSpindexerSpeedIncrease = 0.005;
 
     // These values are ADDED to turrDefault
     public static double redObeliskTurrPos1 = 0.12;
@@ -51,40 +47,25 @@ public class Auto_LT_Close extends LinearOpMode {
     double obeliskTurrPos1 = 0.0;
     double obeliskTurrPos2 = 0.0;
     double obeliskTurrPos3 = 0.0;
-    public static double normalIntakeTime = 3.3;
+
-    public static double shoot1Turr = 0.57;
-    public static double shoot0XTolerance = 1.0;
     double turretShootPos = 0.0;
 
-    public static double finalShootAllTime = 3.0;
+    public static double shootAllTime = 2;
-    public static double shootAllTime = 1.8;
-    public static double shoot0Time = 1.6;
     public static double intake1Time = 3.3;
     public static double intake2Time = 3.8;
 
     public static double intake3Time = 4.2;
 
     public static double flywheel0Time = 3.5;
-    public static double pickup1Speed = 15;
+    public static double pickup1Speed = 12;
-    // ---- SECOND SHOT / PICKUP ----
+    // ---- POSITION TOLERANCES ----
-    public static double shoot1Vel = 2300;
+    public static double posXTolerance = 2.0;
-    public static double shootAllVelocity = 2500;
+    public static double posYTolerance = 2.0;
-    public static double shootAllHood = 0.78 + hoodOffset;
-    // ---- PICKUP POSITION TOLERANCES ----
-    public static double pickup1XTolerance = 2.0;
-    public static double pickup1YTolerance = 2.0;
     // ---- OBELISK DETECTION ----
-    public static double obelisk1Time = 1.5;
-    public static double obelisk1XTolerance = 2.0;
-    public static double obelisk1YTolerance = 2.0;
-    public static double shoot1ToleranceX = 2.0;
-    public static double shoot1ToleranceY = 2.0;
     public static double shoot1Time = 2;
     public static double shoot2Time = 2;
     public static double shoot3Time = 2;
     public static double colorSenseTime = 1.2;
-
-    public static double firstShootTime = 0.3;
     public int motif = 0;
 
     Robot robot;
@@ -153,7 +134,7 @@ public class Auto_LT_Close extends LinearOpMode {
 
         autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret);
 
-        servos.setSpinPos(autoSpinStartPos);
+        servos.setSpinPos(spinStartPos);
 
         servos.setTransferPos(transferServo_out);
 
@@ -331,17 +312,21 @@ public class Auto_LT_Close extends LinearOpMode {
             robot.transfer.setPower(1);
 
             startAuto();
+            shoot();
 
             if (ballCycles > 0){
                 cycleStackClose();
+                shoot();
             }
 
             if (ballCycles > 1){
                 cycleStackMiddle();
+                shoot();
             }
 
             if (ballCycles > 2){
                 cycleStackFar();
+                shoot();
             }
 
             while (opModeIsActive()) {
@@ -360,50 +345,61 @@ public class Auto_LT_Close extends LinearOpMode {
 
     }
 
+    void shoot(){
+        Actions.runBlocking(
+                new ParallelAction(
+                        autoActions.manageShooterAuto(
+                                shootAllTime,
+                                0.501,
+                                0.501,
+                                0.501,
+                                0.501,
+                                false
+                        ),
+                        autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
+                )
+
+        );
+    }
+
     void startAuto() {
         assert shoot0 != null;
 
         Actions.runBlocking(
                 new ParallelAction(
                         shoot0.build(),
-                        autoActions.manageFlywheel(
+                        autoActions.manageShooterAuto(
-                                shoot0Vel,
-                                shoot0Hood,
                                 flywheel0Time,
                                 x1,
-                                0.501,
+                                y1,
-                                shoot0XTolerance,
+                                posXTolerance,
-                                0.501
+                                posYTolerance,
+                                false
                         )
 
                 )
         );
-
-        Actions.runBlocking(
-                autoActions.shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
-        );
     }
 
     void cycleStackClose(){
         Actions.runBlocking(
                 new ParallelAction(
                         pickup1.build(),
-                        autoActions.manageFlywheel(
+                        autoActions.manageShooterAuto(
-                                shootAllVelocity,
-                                shootAllHood,
                                 intake1Time,
-                                0.501,
+                                x2b,
-                                0.501,
+                                y2b,
-                                pickup1XTolerance,
+                                posXTolerance,
-                                pickup1YTolerance
+                                posYTolerance,
+                                true
                         ),
                         autoActions.intake(intake1Time),
                         autoActions.detectObelisk(
                                 intake1Time,
-                                0.501,
+                                x2b,
-                                0.501,
+                                y2b,
-                                obelisk1XTolerance,
+                                posXTolerance,
-                                obelisk1YTolerance,
+                                posYTolerance,
                                 obeliskTurrPos1
                         )
 
@@ -415,35 +411,29 @@ public class Auto_LT_Close extends LinearOpMode {
         if (motif == 0) motif = 22;
         prevMotif = motif;
 
-        Actions.runBlocking(
+        double posX;
-                new ParallelAction(
+        double posY;
-                        autoActions.manageFlywheel(
+        if (ballCycles > 1){
-                                shootAllVelocity,
+            posX = xShoot;
-                                shootAllHood,
+            posY = yShoot;
-                                shoot1Time,
+        } else {
-                                0.501,
+            posX = xLeave;
-                                0.501,
+            posY = yLeave;
-                                0.501,
+        }
-                                0.501
-                        ),
-                        shoot1.build(),
-                        autoActions.prepareShootAll(colorSenseTime, shoot1Time, motif)
-                )
-        );
-
 
         Actions.runBlocking(
                 new ParallelAction(
                         autoActions.manageShooterAuto(
-                                shootAllTime,
+                                shoot1Time,
-                                0.501,
+                                posX,
-                                0.501,
+                                posY,
-                                0.501,
+                                posXTolerance,
-                                0.501
+                                posYTolerance,
+                                false
                         ),
-                        autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
+                        shoot1.build(),
+                        autoActions.prepareShootAll(colorSenseTime, shoot1Time, motif)
                 )
-
         );
     }
 
@@ -453,18 +443,19 @@ public class Auto_LT_Close extends LinearOpMode {
                         pickup2.build(),
                         autoActions.manageShooterAuto(
                                 intake2Time,
-                                0.501,
+                                x3b,
-                                0.501,
+                                y3b,
-                                pickup1XTolerance,
+                                posXTolerance,
-                                pickup1YTolerance
+                                posYTolerance,
+                                true
                         ),
                         autoActions.intake(intake2Time),
                         autoActions.detectObelisk(
                                 intake2Time,
-                                0.501,
+                                x3b,
-                                0.501,
+                                y3b,
-                                obelisk1XTolerance,
+                                posXTolerance,
-                                obelisk1YTolerance,
+                                posYTolerance,
                                 obeliskTurrPos2
                         )
 
@@ -476,32 +467,29 @@ public class Auto_LT_Close extends LinearOpMode {
         if (motif == 0) motif = prevMotif;
         prevMotif = motif;
 
-        Actions.runBlocking(
+        double posX;
-                new ParallelAction(
+        double posY;
-                        autoActions.manageFlywheelAuto(
+        if (ballCycles > 2){
-                                shoot2Time,
+            posX = xShoot;
-                                0.501,
+            posY = yShoot;
-                                0.501,
+        } else {
-                                0.501,
+            posX = xLeave;
-                                0.501
+            posY = yLeave;
-                        ),
+        }
-                        shoot2.build(),
-                        autoActions.prepareShootAll(colorSenseTime, shoot2Time, motif)
-                )
-        );
 
         Actions.runBlocking(
                 new ParallelAction(
                         autoActions.manageShooterAuto(
-                                shootAllTime,
+                                shoot2Time,
-                                0.501,
+                                posX,
-                                0.501,
+                                posY,
-                                0.501,
+                                posXTolerance,
-                                0.501
+                                posYTolerance,
+                                false
                         ),
-                        autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
+                        shoot2.build(),
+                        autoActions.prepareShootAll(colorSenseTime, shoot2Time, motif)
                 )
-
         );
     }
 
@@ -511,18 +499,19 @@ public class Auto_LT_Close extends LinearOpMode {
                         pickup3.build(),
                         autoActions.manageShooterAuto(
                                 intake3Time,
-                                0.501,
+                                x4b,
-                                0.501,
+                                y4b,
-                                pickup1XTolerance,
+                                posXTolerance,
-                                pickup1YTolerance
+                                posYTolerance,
+                                true
                         ),
                         autoActions.intake(intake3Time),
                         autoActions.detectObelisk(
                                 intake3Time,
-                                0.501,
+                                x4b,
-                                0.501,
+                                y4b,
-                                obelisk1XTolerance,
+                                posXTolerance,
-                                obelisk1YTolerance,
+                                posYTolerance,
                                 obeliskTurrPos3
                         )
 
@@ -536,30 +525,17 @@ public class Auto_LT_Close extends LinearOpMode {
 
         Actions.runBlocking(
                 new ParallelAction(
-                        autoActions.manageFlywheelAuto(
+                        autoActions.manageShooterAuto(
                                 shoot3Time,
-                                0.501,
+                                xLeave,
-                                0.501,
+                                yLeave,
-                                0.501,
+                                posXTolerance,
-                                0.501
+                                posYTolerance,
+                                false
                         ),
                         shoot3.build(),
                         autoActions.prepareShootAll(colorSenseTime, shoot3Time, motif)
                 )
         );
-
-        Actions.runBlocking(
-                new ParallelAction(
-                        autoActions.manageShooterAuto(
-                                finalShootAllTime,
-                                0.501,
-                                0.501,
-                                0.501,
-                                0.501
-                        ),
-                        autoActions.shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease)
-                )
-
-        );
     }
 }

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

@@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.autonomous;
 import static org.firstinspires.ftc.teamcode.constants.Back_Poses.*;
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
 import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
@@ -45,7 +44,7 @@ import java.util.Objects;
 @Config
 @Autonomous(preselectTeleOp = "TeleopV3")
 public class Auto_LT_Far extends LinearOpMode {
-    public static double shoot0Vel = 3300, shoot0Hood = 0.48 + hoodOffset;
+    public static double shoot0Vel = 3300, shoot0Hood = 0.48;
     public static double autoSpinStartPos = 0.2;
     public static double shoot0SpinSpeedIncrease = 0.015;
     public static double shoot0XTolerance = 1.0;
@@ -270,25 +269,37 @@ public class Auto_LT_Far extends LinearOpMode {
 
     }
 
+//    void shoot(){
+//        Actions.runBlocking(
+//                new ParallelAction(
+//                        autoActions.manageShooterAuto(
+//                                shootAllTime,
+//                                0.501,
+//                                0.501,
+//                                0.501,
+//                                0.501,
+//                                false
+//                        ),
+//                        autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
+//                )
+//
+//        );
+//    }
+
     void startAuto(){
         Actions.runBlocking(
                 new ParallelAction(
-                        autoActions.manageFlywheel(
+                        autoActions.manageShooterAuto(
-                                shoot0Vel,
-                                shoot0Hood,
                                 flywheel0Time,
                                 0.501,
                                 0.501,
-                                shoot0XTolerance,
+                                0.501,
-                                0.501
+                                0.501,
+                                false
                         )
 
                 )
         );
-
-        Actions.runBlocking(
-                autoActions.shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
-        );
     }
 
     void leave3Ball(){

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java

@@ -62,6 +62,7 @@ public class AutoActions{
         this.targetingSettings = tS;
         this.turret = tur;
     }
+
     public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
         return new Action() {
             double stamp = 0.0;
@@ -82,29 +83,6 @@ public class AutoActions{
 
                 teleStart = drive.localizer.getPose();
 
-                double robX = drive.localizer.getPose().position.x;
-                double robY = drive.localizer.getPose().position.y;
-                double robotHeading = drive.localizer.getPose().heading.toDouble();
-
-                double goalX = -15;
-                double goalY = 0;
-
-                double dx = robX - goalX;  // delta x from robot to goal
-                double dy = robY - goalY;  // delta y from robot to goal
-                Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
-
-                double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
-
-                targetingSettings = targeting.calculateSettings
-                        (robX, robY, robotHeading, 0.0, turretInterpolate);
-
-                turret.trackGoal(deltaPose);
-
-                TELE.addData("Velocity", flywheel.getVelo());
-                TELE.addData("Hood", robot.hood.getPosition());
-                TELE.addData("motif", motif_id);
-                TELE.update();
-
                 if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) {
 
                     spindexerWiggle *= -1.0;
@@ -181,8 +159,6 @@ public class AutoActions{
 
                     return true;
                 } else if ((System.currentTimeMillis() - stamp) < (time * 1000)) {
-//                    TELE.addData("MostGreenSlot", mostGreenSlot);
-//                    TELE.update();
                     spindexer.setIntakePower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000);
 
                     servos.setSpinPos(firstSpindexShootPos);
@@ -205,9 +181,6 @@ public class AutoActions{
 
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                TELE.addData("Velocity", flywheel.getVelo());
-                TELE.addData("Hood", robot.hood.getPosition());
-                TELE.update();
 
                 double voltage = robot.voltage.getVoltage();
                 flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
@@ -218,18 +191,13 @@ public class AutoActions{
 
                 teleStart = drive.localizer.getPose();
 
-                spindexer.setIntakePower(-0.3);
+                spindexer.setIntakePower(-0.1);
 
                 if (ticker == 1) {
                     stamp = System.currentTimeMillis();
                 }
                 ticker++;
 
-                spindexer.setIntakePower(0);
-                drive.updatePoseEstimate();
-
-                teleStart = drive.localizer.getPose();
-
                 double robX = drive.localizer.getPose().position.x;
                 double robY = drive.localizer.getPose().position.y;
                 double robotHeading = drive.localizer.getPose().heading.toDouble();
@@ -287,9 +255,6 @@ public class AutoActions{
 
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                TELE.addData("Velocity", flywheel.getVelo());
-                TELE.addData("Hood", robot.hood.getPosition());
-                TELE.update();
 
                 velo = flywheel.getVelo();
 
@@ -297,51 +262,30 @@ public class AutoActions{
 
                 teleStart = drive.localizer.getPose();
 
-                spindexer.setIntakePower(-0.3);
+                spindexer.setIntakePower(-0.1);
 
                 if (ticker == 1) {
                     stamp = System.currentTimeMillis();
                 }
                 ticker++;
 
-                spindexer.setIntakePower(0);
+                double prevSpinPos = servos.getSpinCmdPos();
-                drive.updatePoseEstimate();
 
-                teleStart = drive.localizer.getPose();
+                if (System.currentTimeMillis() - stamp < shootTime*1000 || (prevSpinPos < spinEndPos && shooterTicker < 2)) {
 
-                double robX = drive.localizer.getPose().position.x;
+                    if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker == 0) {
-                double robY = drive.localizer.getPose().position.y;
-                double robotHeading = drive.localizer.getPose().heading.toDouble();
-
-                double goalX = -15;
-                double goalY = 0;
-
-                double dx = robX - goalX;  // delta x from robot to goal
-                double dy = robY - goalY;  // delta y from robot to goal
-                Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
-
-                double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
-
-                targetingSettings = targeting.calculateSettings
-                        (robX, robY, robotHeading, 0.0, turretInterpolate);
-
-                turret.trackGoal(deltaPose);
-
-                if (System.currentTimeMillis() - stamp < shootTime*1000) {
-
-                    if (System.currentTimeMillis() - stamp < firstShootTime*1000) {
                         servos.setTransferPos(transferServo_out);
                         servos.setSpinPos(firstSpindexShootPos);
                     } else {
                         servos.setTransferPos(transferServo_in);
                         shooterTicker++;
-                        double prevSpinPos = servos.getSpinCmdPos();
 
                         if (shootForward) {
                             servos.setSpinPos(prevSpinPos + spindexSpeed);
                         } else {
                             servos.setSpinPos(prevSpinPos - spindexSpeed);
                         }
+
                     }
 
                     return true;
@@ -379,14 +323,12 @@ public class AutoActions{
 
                 teleStart = drive.localizer.getPose();
 
-                TELE.addData("Full?", spindexer.isFull());
-                TELE.update();
-
                 return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull();
             }
         };
     }
 
+    private boolean detectingObelisk = false;
     public Action detectObelisk(
             double time,
             double posX,
@@ -407,7 +349,7 @@ public class AutoActions{
 
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-
+                detectingObelisk = true;
                 drive.updatePoseEstimate();
                 Pose2d currentPose = drive.localizer.getPose();
 
@@ -425,13 +367,9 @@ public class AutoActions{
                 boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
                 boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
 
-                boolean shouldFinish = timeDone || xDone || yDone;
+                boolean shouldFinish = timeDone || (xDone && yDone) || spindexer.isFull();
-                drive.updatePoseEstimate();
 
-                teleStart = drive.localizer.getPose();
+                teleStart = currentPose;
-                TELE.addData("Velocity", flywheel.getVelo());
-                TELE.addData("Hood", robot.hood.getPosition());
-                TELE.update();
 
                 if (shouldFinish){
                     if (redAlliance){
@@ -439,6 +377,7 @@ public class AutoActions{
                     } else {
                         robot.limelight.pipelineSwitch(2);
                     }
+                    detectingObelisk = false;
                     return false;
                 } else {
                     return true;
@@ -489,12 +428,8 @@ public class AutoActions{
                 boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
 
                 boolean shouldFinish = timeDone || xDone || yDone;
-                drive.updatePoseEstimate();
 
-                teleStart = drive.localizer.getPose();
+                teleStart = currentPose;
-                TELE.addData("Velocity", flywheel.getVelo());
-                TELE.addData("Hood", robot.hood.getPosition());
-                TELE.update();
 
                 return !shouldFinish;
 
@@ -507,7 +442,8 @@ public class AutoActions{
             double posX,
             double posY,
             double posXTolerance,
-            double posYTolerance
+            double posYTolerance,
+            boolean whileIntaking
     ) {
 
         boolean timeFallback = (time != 0.501);
@@ -531,10 +467,10 @@ public class AutoActions{
 
                 ticker++;
 
-                double robotX = drive.localizer.getPose().position.x;
+                double robotX = currentPose.position.x;
-                double robotY = drive.localizer.getPose().position.y;
+                double robotY = currentPose.position.y;
 
-                double robotHeading = drive.localizer.getPose().heading.toDouble();
+                double robotHeading = currentPose.heading.toDouble();
 
                 double goalX = -15;
                 double goalY = 0;
@@ -548,7 +484,7 @@ public class AutoActions{
                 targetingSettings = targeting.calculateSettings
                         (robotX, robotY, robotHeading, 0.0, false);
 
-                turret.trackGoal(deltaPose);
+                if (!detectingObelisk){turret.trackGoal(deltaPose);}
 
                 servos.setHoodPos(targetingSettings.hoodAngle);
 
@@ -557,17 +493,16 @@ public class AutoActions{
                 flywheel.manageFlywheel(targetingSettings.flywheelRPM);
 
                 boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
-                boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
+                boolean xDone = posXFallback && Math.abs(robotX - posX) < posXTolerance;
-                boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
+                boolean yDone = posYFallback && Math.abs(robotY - posY) < posYTolerance;
+                boolean shouldFinish;
+                if (whileIntaking){
+                    shouldFinish = timeDone || (xDone && yDone) || spindexer.isFull();
+                } else {
+                    shouldFinish = timeDone || (xDone && yDone);
+                }
 
-                boolean shouldFinish = timeDone || xDone || yDone;
+                teleStart = currentPose;
-                drive.updatePoseEstimate();
-
-                teleStart = drive.localizer.getPose();
-
-                TELE.addData("Velocity", flywheel.getVelo());
-                TELE.addData("Hood", robot.hood.getPosition());
-                TELE.update();
 
                 return !shouldFinish;
 
@@ -632,12 +567,8 @@ public class AutoActions{
                 boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
 
                 boolean shouldFinish = timeDone || xDone || yDone;
-                drive.updatePoseEstimate();
 
-                teleStart = drive.localizer.getPose();
+                teleStart = currentPose;
-                TELE.addData("Velocity", flywheel.getVelo());
-                TELE.addData("Hood", robot.hood.getPosition());
-                TELE.update();
 
                 return !shouldFinish;
 

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

@@ -38,8 +38,8 @@ public class Flywheel {
     }
 
     // Set the robot PIDF for the next cycle.
-    private double prevF = 0.501;
+    private double prevF = 0;
-    public static int voltagePIDFDifference = 5;
+    public static double voltagePIDFDifference = 0.8;
     public void setPIDF(double p, double i, double d, double f) {
         shooterPIDF1.p = p;
         shooterPIDF1.i = i;

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

@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.utils;
 
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
+
 import com.acmerobotics.dashboard.config.Config;
 import com.arcrobotics.ftclib.controller.PIDFController;
 import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -72,7 +74,7 @@ public class Servos {
 
     public double setHoodPos(double pos){
         if (firstHoodPos || !servoPosEqual(pos, prevHoodPos)) {
-            robot.hood.setPosition(pos);
+            robot.hood.setPosition(pos + hoodOffset);
             firstHoodPos = false;
         }
 

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

@@ -186,7 +186,7 @@ public class Targeting {
         if (true) { //!interpolate) {
             if ((robotGridY < 6) && (robotGridX < 6)) {
                 recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM;
-                recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle + ServoPositions.hoodOffset;
+                recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle;
             }
             return recommendedSettings;
         } else {

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

@@ -24,7 +24,7 @@ public class Turret {
     public static double turretTolerance = 0.02;
     public static double turrPosScalar = 0.00011264432;
     public static double turret180Range = 0.4;
-    public static double turrDefault = 0.39;
+    public static double turrDefault = 0.37;
     public static double turrMin = 0.15;
     public static double turrMax = 0.85;
     public static boolean limelightUsed = true;