@KeshavAnandCode I need your help tomorrow to edit auto actions so they are more sensor based

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

@@ -106,10 +106,10 @@ import java.util.Objects;
 public class Auto_LT_Close extends LinearOpMode {
     public static double shoot0Vel = 2300, shoot0Hood = 0.93 + hoodOffset;
     public static double autoSpinStartPos = 0.2;
-    public static double shoot0SpinSpeedIncrease = 0.015;
+    public static double shoot0SpinSpeedIncrease = 0.02;
 
     public static double spindexerSpeedIncrease = 0.03;
-    public static double finalSpindexerSpeedIncrease = 0.025;
+    public static double finalSpindexerSpeedIncrease = 0.03;
 
     // These values are ADDED to turrDefault
     public static double redObeliskTurrPos1 = 0.12;
@@ -348,6 +348,8 @@ public class Auto_LT_Close extends LinearOpMode {
                 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);
                 flywheel.manageFlywheel(vel);
                 velo = flywheel.getVelo();
 
@@ -622,6 +624,8 @@ public class Auto_LT_Close extends LinearOpMode {
 
                 ticker++;
 
+                double voltage = robot.voltage.getVoltage();
+                flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
                 flywheel.manageFlywheel(vel);
                 robot.hood.setPosition(hoodPos);
 
@@ -693,6 +697,8 @@ public class Auto_LT_Close extends LinearOpMode {
 
                 robot.hood.setPosition(targetingSettings.hoodAngle);
 
+                double voltage = robot.voltage.getVoltage();
+                flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
                 flywheel.manageFlywheel(targetingSettings.flywheelRPM);
 
                 boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
@@ -762,6 +768,8 @@ public class Auto_LT_Close extends LinearOpMode {
 
                 robot.hood.setPosition(targetingSettings.hoodAngle);
 
+                double voltage = robot.voltage.getVoltage();
+                flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
                 flywheel.manageFlywheel(targetingSettings.flywheelRPM);
 
                 boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
@@ -992,7 +1000,7 @@ public class Auto_LT_Close extends LinearOpMode {
 
             robot.transfer.setPower(1);
 
-            startCycle();
+            startAuto();
 
             if (ballCycles > 0){
                 cycleStackClose();
@@ -1022,7 +1030,7 @@ public class Auto_LT_Close extends LinearOpMode {
 
     }
 
-    void startCycle() {
+    void startAuto() {
         assert shoot0 != null;
 
         Actions.runBlocking(

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

@@ -49,7 +49,7 @@ import java.util.Objects;
 @Config
 @Autonomous(preselectTeleOp = "TeleopV3")
 public class Auto_LT_Far extends LinearOpMode {
-    public static double shoot0Vel = 3200, shoot0Hood = 0.5 + hoodOffset;
+    public static double shoot0Vel = 3300, shoot0Hood = 0.48 + hoodOffset;
     public static double autoSpinStartPos = 0.2;
     public static double shoot0SpinSpeedIncrease = 0.015;
     public static double shoot0XTolerance = 1.0;
@@ -69,16 +69,18 @@ public class Auto_LT_Far extends LinearOpMode {
     Turret turret;
     Targeting targeting;
     Targeting.Settings targetingSettings;
-    private double firstSpindexShootPos = autoSpinStartPos;
+    double firstSpindexShootPos = autoSpinStartPos;
-    private boolean shootForward = true;
+    boolean shootForward = true;
-    private double xShoot, yShoot, hShoot;
+    double xShoot, yShoot, hShoot;
     private int driverSlotGreen = 0;
     private int passengerSlotGreen = 0;
-    private int rearSlotGreen = 0;
+    int rearSlotGreen = 0;
-    private int mostGreenSlot = 0;
+    int mostGreenSlot = 0;
-    private double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0;
+    double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0;
-    private double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0;
+    double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0;
     public static double firstShootTime = 0.3;
+    public static double flywheel0Time = 3.5;
+    public static double shoot0Time = 2;
 
     public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
         return new Action() {
@@ -97,12 +99,28 @@ public class Auto_LT_Far extends LinearOpMode {
                 ticker++;
                 robot.transferServo.setPosition(transferServo_out);
 
-                turret.manualSetTurret(turretShootPos);
-
                 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();
+
+                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);
@@ -214,6 +232,8 @@ public class Auto_LT_Far extends LinearOpMode {
                 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);
                 flywheel.manageFlywheel(vel);
                 velo = flywheel.getVelo();
 
@@ -314,6 +334,24 @@ public class Auto_LT_Far extends LinearOpMode {
 
                 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);
+
                 if (getRuntime() - stamp < shootTime) {
 
                     if (getRuntime() - stamp < firstShootTime) {
@@ -369,6 +407,9 @@ public class Auto_LT_Far extends LinearOpMode {
 
                 teleStart = drive.localizer.getPose();
 
+                TELE.addData("Full?", spindexer.isFull());
+                TELE.update();
+
                 return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull();
             }
         };
@@ -467,6 +508,8 @@ public class Auto_LT_Far extends LinearOpMode {
 
                 ticker++;
 
+                double voltage = robot.voltage.getVoltage();
+                flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
                 flywheel.manageFlywheel(vel);
                 robot.hood.setPosition(hoodPos);
 
@@ -538,6 +581,8 @@ public class Auto_LT_Far extends LinearOpMode {
 
                 robot.hood.setPosition(targetingSettings.hoodAngle);
 
+                double voltage = robot.voltage.getVoltage();
+                flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
                 flywheel.manageFlywheel(targetingSettings.flywheelRPM);
 
                 boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
@@ -607,6 +652,8 @@ public class Auto_LT_Far extends LinearOpMode {
 
                 robot.hood.setPosition(targetingSettings.hoodAngle);
 
+                double voltage = robot.voltage.getVoltage();
+                flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
                 flywheel.manageFlywheel(targetingSettings.flywheelRPM);
 
                 boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
@@ -627,6 +674,9 @@ public class Auto_LT_Far extends LinearOpMode {
         };
     }
 
+    // initialize path variables here
+    TrajectoryActionBuilder leave3Ball = null;
+
     @Override
     public void runOpMode() throws InterruptedException {
 
@@ -662,8 +712,6 @@ public class Auto_LT_Far extends LinearOpMode {
 
         robot.light.setPosition(1);
 
-        TrajectoryActionBuilder leave = null;
-
         while (opModeInInit()) {
 
             // Recalibration in initialization
@@ -673,9 +721,10 @@ public class Auto_LT_Far extends LinearOpMode {
                 gamepad2.rumble(1000);
             }
 
-            robot.hood.setPosition(shoot0Hood);
             turret.manualSetTurret(turretShootPos);
 
+            robot.hood.setPosition(shoot0Hood);
+
             if (gamepad2.crossWasPressed()) {
                 redAlliance = !redAlliance;
             }
@@ -706,10 +755,9 @@ public class Auto_LT_Far extends LinearOpMode {
                 turretShootPos = turrDefault + blueTurretShootPos;
             }
 
-            leave = drive.actionBuilder(teleEnd)
+            leave3Ball = drive.actionBuilder(teleEnd)
                     .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
 
-
             TELE.addData("Red?", redAlliance);
             TELE.addData("Turret Default", turrDefault);
             TELE.addData("Start Position", teleEnd);
@@ -726,35 +774,9 @@ public class Auto_LT_Far extends LinearOpMode {
 
             robot.transfer.setPower(1);
 
-            Actions.runBlocking(
+            startAuto();
-                    new ParallelAction(
-                            manageFlywheel(
-                                    shoot0Vel,
-                                    shoot0Hood,
-                                    5,
-                                    0.501,
-                                    0.501,
-                                    shoot0XTolerance,
-                                    0.501
-                            )
 
-                    )
+            leave3Ball();
-            );
-
-            drive.updatePoseEstimate();
-
-            teleStart = drive.localizer.getPose();
-
-            Actions.runBlocking(
-                    shootAll((int) shoot0Vel, 4, shoot0SpinSpeedIncrease)
-            );
-
-            drive.updatePoseEstimate();
-
-            teleStart = drive.localizer.getPose();
-
-            assert leave != null;
-            Actions.runBlocking(leave.build());
 
             // Actual way to end autonomous in to find final position
             while (opModeIsActive()) {
@@ -772,4 +794,30 @@ public class Auto_LT_Far extends LinearOpMode {
         }
 
     }
+
+    void startAuto(){
+        Actions.runBlocking(
+                new ParallelAction(
+                        manageFlywheel(
+                                shoot0Vel,
+                                shoot0Hood,
+                                flywheel0Time,
+                                0.501,
+                                0.501,
+                                shoot0XTolerance,
+                                0.501
+                        )
+
+                )
+        );
+
+        Actions.runBlocking(
+                shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
+        );
+    }
+
+    void leave3Ball(){
+        assert leave3Ball != null;
+        Actions.runBlocking(leave3Ball.build());
+    }
 }

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

@@ -4,7 +4,7 @@ import com.acmerobotics.dashboard.config.Config;
 
 @Config
 public class Back_Poses {
-    public static double rLeaveX = 80, rLeaveY = 70, rLeaveH = 50;
+    public static double rLeaveX = 90, rLeaveY = 80, rLeaveH = 50;
-    public static double bLeaveX = 80, bLeaveY = -70, bLeaveH = -50;
+    public static double bLeaveX = 90, bLeaveY = -80, bLeaveH = -50;
 
 }

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

@@ -35,7 +35,7 @@ public class PositionalServoProgrammer extends LinearOpMode {
         waitForStart();
         if (isStopRequested()) return;
         while (opModeIsActive()){
-            if (spindexPos != 0.501 && !servo.spinEqual(spindexPos)){
+            if (spindexPos != 0.501){
                 robot.spin1.setPosition(spindexPos);
                 robot.spin2.setPosition(1-spindexPos);
             }
@@ -52,16 +52,6 @@ public class PositionalServoProgrammer extends LinearOpMode {
             if (light !=0.501){
                 robot.light.setPosition(light);
             }
-            // To check configuration of spindexer:
-            // Set "mode" to 1 and spindexPow to 0.1
-            // If the spindexer is turning clockwise, the servos are reversed. Swap the configuration of the two servos, DO NOT TOUCH THE ACTUAL CODE
-            // Do the previous test again to confirm
-            // Set "mode" to 0 but keep spindexPos at 0.501
-            // Manually turn the spindexer until "spindexer pos" is set close to 0
-            // Check each spindexer voltage:
-                // If "spindexer voltage 1" is closer to 0 than "spindexer voltage 2," then you are done!
-                // If "spindexer voltage 2" is closer to 0 than "spindexer voltage 1," swap the two spindexer analog inputs in the configuration, DO NOT TOUCH THE ACTUAL CODE
-            //TODO: @KeshavAnandCode do the above please
 
             TELE.addData("spindexer pos", servo.getSpinPos());
             TELE.addData("turret pos", robot.turr1.getPosition());