far auto in development part 2

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

@@ -32,7 +32,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
 @Autonomous(preselectTeleOp = "TeleopV3")
 public class Auto_LT_Close extends LinearOpMode {
     public static double shoot0Vel = 2300, shoot0Hood = 0.93;
-    public static double spindexerSpeedIncrease = 0.005;
+    public static double spindexerSpeedIncrease = 0.008;
 
     // These values are ADDED to turrDefault
     public static double redObeliskTurrPos1 = 0.12;
@@ -167,8 +167,8 @@ public class Auto_LT_Close extends LinearOpMode {
             }
 
             if (gamepad2.squareWasPressed()){
+                turret.pipelineSwitch(1);
                 robot.limelight.start();
-                robot.limelight.pipelineSwitch(1);
                 gamepad2.rumble(500);
             }
 
@@ -336,6 +336,7 @@ public class Auto_LT_Close extends LinearOpMode {
                 teleStart = drive.localizer.getPose();
 
                 flywheel.manageFlywheel(0);
+                robot.transfer.setPower(0);
 
                 TELE.addLine("finished");
                 TELE.update();

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

@@ -72,7 +72,7 @@ public class Auto_LT_Far extends LinearOpMode {
     double xStackPickupB, yStackPickupB, hStackPickupB;
     public static int pickupStackSpeed = 12;
     int prevMotif = 0;
-    public static double spindexerSpeedIncrease = 0.005;
+    public static double spindexerSpeedIncrease = 0.008;
     public static double shootAllTime = 2;
     // ---- POSITION TOLERANCES ----
     public static double posXTolerance = 5.0;
@@ -131,12 +131,6 @@ public class Auto_LT_Far extends LinearOpMode {
 
         while (opModeInInit()) {
 
-            if (gamepad2.squareWasPressed()){
-                robot.limelight.start();
-                robot.limelight.pipelineSwitch(1);
-                gamepad2.rumble(500);
-            }
-
             if (gamepad2.leftBumperWasPressed()){
                 gatePickup = !gatePickup;
             }
@@ -187,6 +181,12 @@ public class Auto_LT_Far extends LinearOpMode {
                 obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
                 obeliskTurrPos3 = turrDefault + redObeliskTurrPos3;
                 turretShootPos = turrDefault + redTurretShootPos;
+
+                if (gamepad2.squareWasPressed()){
+                    turret.pipelineSwitch(4);
+                    robot.limelight.start();
+                    gamepad2.rumble(500);
+                }
             } else {
                 robot.light.setPosition(0.6);
 
@@ -214,6 +214,12 @@ public class Auto_LT_Far extends LinearOpMode {
                 obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
                 obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3;
                 turretShootPos = turrDefault + blueTurretShootPos;
+
+                if (gamepad2.squareWasPressed()){
+                    turret.pipelineSwitch(2);
+                    robot.limelight.start();
+                    gamepad2.rumble(500);
+                }
             }
 
             leave3Ball = drive.actionBuilder(autoStart)
@@ -274,6 +280,7 @@ public class Auto_LT_Far extends LinearOpMode {
                 teleStart = drive.localizer.getPose();
 
                 flywheel.manageFlywheel(0);
+                robot.transfer.setPower(0);
 
                 TELE.addLine("finished");
                 TELE.update();

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

@@ -193,77 +193,6 @@ public class AutoActions{
         };
     }
 
-    public Action shootAll(int vel, double shootTime, double spindexSpeed) {
-        return new Action() {
-            int ticker = 1;
-            double stamp = 0.0;
-            double velo = vel;
-            int shooterTicker = 0;
-
-            @Override
-            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-
-                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();
-
-                drive.updatePoseEstimate();
-
-                teleStart = drive.localizer.getPose();
-
-                spindexer.setIntakePower(-0.1);
-
-                if (ticker == 1) {
-                    stamp = System.currentTimeMillis();
-                }
-                ticker++;
-
-                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 ((System.currentTimeMillis() - stamp < shootTime*1000 && servos.getSpinPos() < 0.85) || shooterTicker == 0) {
-
-                    if (shooterTicker == 0 && !servos.spinEqual(spinStartPos)) {
-                        servos.setSpinPos(spinStartPos);
-                    } else {
-                        servos.setTransferPos(transferServo_in);
-                        shooterTicker++;
-                        double prevSpinPos = servos.getSpinCmdPos();
-                        servos.setSpinPos(prevSpinPos + spindexSpeed);
-                    }
-
-                    return true;
-
-                } else {
-                    servos.setTransferPos(transferServo_out);
-
-                    spindexer.resetSpindexer();
-                    spindexer.processIntake();
-
-                    return false;
-
-                }
-
-            }
-        };
-    }
-
     private boolean doneShooting = false;
     public Action shootAllAuto(double shootTime, double spindexSpeed) {
         return new Action() {
@@ -385,7 +314,7 @@ public class AutoActions{
 
                 if (ticker == 0) {
                     stamp = System.currentTimeMillis();
-                    robot.limelight.pipelineSwitch(1);
+                    turret.pipelineSwitch(1);
                 }
 
                 ticker++;
@@ -403,9 +332,9 @@ public class AutoActions{
 
                 if (shouldFinish){
                     if (redAlliance){
-                        robot.limelight.pipelineSwitch(4);
+                        turret.pipelineSwitch(4);
                     } else {
-                        robot.limelight.pipelineSwitch(2);
+                        turret.pipelineSwitch(2);
                     }
                     detectingObelisk = false;
                     return false;
@@ -417,56 +346,6 @@ public class AutoActions{
         };
     }
 
-    public Action manageFlywheel(
-            double vel,
-            double hoodPos,
-            double time,
-            double posX,
-            double posY,
-            double posXTolerance,
-            double posYTolerance
-    ) {
-
-        boolean timeFallback = (time != 0.501);
-        boolean posXFallback = (posX != 0.501);
-        boolean posYFallback = (posY != 0.501);
-
-        return new Action() {
-
-            double stamp = 0.0;
-            int ticker = 0;
-
-            @Override
-            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-
-                drive.updatePoseEstimate();
-                Pose2d currentPose = drive.localizer.getPose();
-
-                if (ticker == 0) {
-                    stamp = System.currentTimeMillis();
-                }
-
-                ticker++;
-
-                double voltage = robot.voltage.getVoltage();
-                flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
-                flywheel.manageFlywheel(vel);
-                servos.setHoodPos(hoodPos);
-
-                boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
-                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;
-
-                teleStart = currentPose;
-
-                return !shouldFinish;
-
-            }
-        };
-    }
-
     public Action manageShooterAuto(
             double time,
             double posX,
@@ -557,72 +436,6 @@ public class AutoActions{
             }
         };
     }
-
-    public Action manageFlywheelAuto(
-            double time,
-            double posX,
-            double posY,
-            double posXTolerance,
-            double posYTolerance
-    ) {
-
-        boolean timeFallback = (time != 0.501);
-        boolean posXFallback = (posX != 0.501);
-        boolean posYFallback = (posY != 0.501);
-
-        return new Action() {
-
-            double stamp = 0.0;
-            int ticker = 0;
-
-            @Override
-            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-
-                drive.updatePoseEstimate();
-                Pose2d currentPose = drive.localizer.getPose();
-
-                if (ticker == 0) {
-                    stamp = System.currentTimeMillis();
-                }
-
-                ticker++;
-
-                double robotX = drive.localizer.getPose().position.x;
-                double robotY = drive.localizer.getPose().position.y;
-
-                double robotHeading = drive.localizer.getPose().heading.toDouble();
-
-                double goalX = -15;
-                double goalY = 0;
-
-                double dx = robotX - goalX;  // delta x from robot to goal
-                double dy = robotY - 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
-                        (robotX, robotY, robotHeading, 0.0, false);
-
-                servos.setHoodPos(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;
-                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;
-
-                teleStart = currentPose;
-
-                return !shouldFinish;
-
-            }
-        };
-    }
 }
 
 

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

@@ -116,10 +116,10 @@ public class TeleopV3 extends LinearOpMode {
         while (opModeInInit()) {
             robot.limelight.start();
             if (redAlliance) {
-                robot.limelight.pipelineSwitch(4);
+                turret.pipelineSwitch(4);
                 light.setManualLightColor(Color.LightRed);
             } else {
-                robot.limelight.pipelineSwitch(2);
+                turret.pipelineSwitch(2);
                 light.setManualLightColor(Color.LightBlue);
 
             }

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

@@ -14,6 +14,7 @@ import com.qualcomm.robotcore.util.ElapsedTime;
 
 import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
 import org.firstinspires.ftc.teamcode.constants.Color;
+import org.firstinspires.ftc.teamcode.constants.StateEnums;
 
 import java.util.List;
 
@@ -48,6 +49,7 @@ public class Turret {
     double limelightPosX = 0.0;
     double limelightPosY = 0.0;
     LLResult result;
+
     boolean bearingAligned = false;
     private boolean lockOffset = false;
     private int obeliskID = 0;
@@ -56,6 +58,7 @@ public class Turret {
     private double lightColor = Color.LightRed;
     private int currentTrackCount = 0;
     private double permanentOffset = 0.0;
+    private int prevPipeline = -1;
     private PIDController bearingPID;
 
     private double prevTurretPos = 0.0;
@@ -91,6 +94,12 @@ public class Turret {
         }
         prevTurrPos = pos;
     }
+    public void pipelineSwitch(int pipeline){
+        if (prevPipeline != pipeline){
+            robot.limelight.pipelineSwitch(pipeline);
+        }
+        prevPipeline = pipeline;
+    }
 
     public boolean turretEqual(double pos) {
         return Math.abs(pos - this.getTurrPos()) < turretTolerance;