test commit 3

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

@@ -59,19 +59,19 @@ public class Auto_LT_Close extends LinearOpMode {
     public static double hood0MoveTime = 2;
     public static double spindexerSpeedIncrease = 0.014;
 
-    public static double shootAllTime  = 10.0;
+    public static double shootAllTime  = 6.0;
     public static double intake1Time = 3.3;
-    public static double intake2Time = 3.8;
+    public static double intake2Time = 4.2;
 
-    public static double intake3Time = 4.2;
+    public static double intake3Time = 5.4;
 
-    public static double flywheel0Time = 2.2;
+    public static double flywheel0Time = 1.9;
-    public static double pickup1Speed = 22;
+    public static double pickup1Speed = 14;
     // ---- POSITION TOLERANCES ----
     public static double posXTolerance = 5.0;
     public static double posYTolerance = 5.0;
     // ---- OBELISK DETECTION ----
-    public static double shoot1Time = 3.0;
+    public static double shoot1Time = 2.5;
     public static double shoot2Time = 2.5;
     public static double shoot3Time = 2.5;
     public static double colorSenseTime = 1.2;
@@ -478,15 +478,15 @@ public class Auto_LT_Close extends LinearOpMode {
 
             if (gateCycle) {
                 startAutoGate();
-                shoot();
+                shoot(0.501, 0.501, 0.501);
                 cycleStackMiddleGate();
-                shoot();
+                shoot(0.501,0.501, 0.501);
 
                 while (getRuntime() - stamp < endGateTime) {
                     cycleGateIntake();
                     if (getRuntime() - stamp < lastShootTime) {
                         cycleGateShoot();
-                        shoot();
+                        shoot(0.501, 0.501, 0.501);
                     }
                 }
                 cycleStackCloseIntakeGate();
@@ -495,25 +495,28 @@ public class Auto_LT_Close extends LinearOpMode {
                     cycleStackCloseShootGate();
                 }
 
-                shoot();
+                shoot(0.501, 0.501, 0.501);
 
             } else {
+
+
+
                 startAuto();
-                shoot();
+                shoot(0.501, 0.501,0.501);
 
                 if (ballCycles > 0) {
                     cycleStackClose();
-                    shoot();
+                    shoot(xShoot, yShoot, hShoot);
                 }
 
                 if (ballCycles > 1) {
                     cycleStackMiddle();
-                    shoot();
+                    shoot(xShoot, yShoot, hShoot);
                 }
 
                 if (ballCycles > 2) {
                     cycleStackFar();
-                    shoot();
+                    shoot(xLeave, yLeave, hLeave);
                 }
             }
 
@@ -534,8 +537,8 @@ public class Auto_LT_Close extends LinearOpMode {
 
     }
 
-    void shoot() {
+    void shoot(double x, double y, double z) {
-        Actions.runBlocking(autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease));
+        Actions.runBlocking(autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease, x, y, z));
     }
 
     void startAuto() {
@@ -545,7 +548,7 @@ public class Auto_LT_Close extends LinearOpMode {
                 new ParallelAction(
                         shoot0.build(),
                         autoActions.prepareShootAll(
-                                colorSenseTime,
+                                0.8,
                                 flywheel0Time,
                                 motif,
                                 x1,

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

@@ -321,7 +321,7 @@ public class Auto_LT_Far extends LinearOpMode {
     void shoot(){
         Actions.runBlocking(
                 new ParallelAction(
-                        autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
+                        autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease, 0.501, 0.501, 0.501)
                 )
 
         );

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

@@ -21,6 +21,7 @@ import com.acmerobotics.roadrunner.Pose2d;
 import com.qualcomm.robotcore.hardware.NormalizedRGBA;
 
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.teamcode.constants.Color;
 import org.firstinspires.ftc.teamcode.constants.ServoPositions;
 import org.firstinspires.ftc.teamcode.constants.StateEnums;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
@@ -207,7 +208,8 @@ public class AutoActions {
                         } else {
                             firstSpindexShootPos = spindexer_outtakeBall3b;
                             shootForward = true;
-                            spinEndPos = 0.95;                        }
+                            spinEndPos = 0.95;
+                        }
                     } else if (motif_id == 22) {
                         if (mostGreenSlot == 1) {
                             firstSpindexShootPos = spindexer_outtakeBall3b;
@@ -253,7 +255,7 @@ public class AutoActions {
         };
     }
 
-    public Action shootAllAuto(double shootTime, double spindexSpeed) {
+    public Action shootAllAuto(double shootTime, double spindexSpeed, double posX, double posY, double posH) {
         return new Action() {
             int ticker = 1;
 
@@ -275,7 +277,7 @@ public class AutoActions {
 
                 if (ticker == 1) {
                     stamp = System.currentTimeMillis();
-                    manageShooter = manageShooterAuto(shootTime, 0.501, 0.501, 0.501, false);
+                    manageShooter = manageShooterAuto(shootTime, posX, posY, posH, false);
 
                 }
                 ticker++;
@@ -423,7 +425,8 @@ public class AutoActions {
 
                 manageShooter.run(telemetryPacket);
 
-                if ((System.currentTimeMillis() - stamp) > (time * 1000) || spindexer.isFull()) {
+                if ((System.currentTimeMillis() - stamp) > (time * 1000)) {
+                    servos.setSpinPos(spindexer_intakePos1);
                     return false;
                 } else {
                     return true;
@@ -513,14 +516,26 @@ public class AutoActions {
 
             final boolean timeFallback = (time != 0.501);
 
+
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
 
+
+
                 drive.updatePoseEstimate();
                 Pose2d currentPose = drive.localizer.getPose();
 
                 if (ticker == 0) {
                     stamp = System.currentTimeMillis();
+
+                    if (redAlliance) {
+                        turret.pipelineSwitch(4);
+                        light.setManualLightColor(Color.LightRed);
+                    } else {
+                        turret.pipelineSwitch(2);
+                        light.setManualLightColor(Color.LightBlue);
+
+                    }
                 }
 
                 ticker++;
@@ -535,14 +550,15 @@ public class AutoActions {
 
                 double dx = robotX - goalX;  // delta x from robot to goal
                 double dy = robotY - goalY;  // delta y from robot to goal
+
+
                 Pose2d deltaPose;
                 if (posX != 0.501) {
                     deltaPose = new Pose2d(posX, posY, Math.toRadians(posH));
-                    Turret.limelightUsed = false;
                 } else {
                     deltaPose = new Pose2d(dx, dy, robotHeading);
-                    Turret.limelightUsed = true;
                 }
+                Turret.limelightUsed = true;
 
 //                double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
 

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

@@ -5,18 +5,18 @@ import com.acmerobotics.dashboard.config.Config;
 @Config
 public class ServoPositions {
 
-    public static double spindexer_intakePos1 = 0.22; //0.13;
+    public static double spindexer_intakePos1 = 0.18; //0.13;
 
-    public static double spindexer_intakePos2 = 0.41; //0.33;//0.5;
+    public static double spindexer_intakePos2 = 0.37; //0.33;//0.5;
 
-    public static double spindexer_intakePos3 = 0.60; //0.53;//0.66;
+    public static double spindexer_intakePos3 = 0.56; //0.53;//0.66;
 
-    public static double spindexer_outtakeBall3 = 0.88; //0.65; //0.24;
+    public static double spindexer_outtakeBall3 = 0.84; //0.65; //0.24;
-    public static double spindexer_outtakeBall3b = 0.31; //0.65; //0.24;
+    public static double spindexer_outtakeBall3b = 0.27; //0.65; //0.24;
 
-    public static double spindexer_outtakeBall2 = 0.7; //0.46; //0.6;
+    public static double spindexer_outtakeBall2 = 0.66; //0.46; //0.6;
-    public static double spindexer_outtakeBall1 = 0.51; //0.27; //0.4;
+    public static double spindexer_outtakeBall1 = 0.47; //0.27; //0.4;
-    public static double spinStartPos = 0.14;
+    public static double spinStartPos = 0.10;
     public static double spinEndPos = 0.95;
 
     public static double shootAllSpindexerSpeedIncrease = 0.014;

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

@@ -45,9 +45,9 @@ public class TeleopV3 extends LinearOpMode {
     public double vel = 3000;
     public boolean autoVel = true;
     public boolean targetingHood = true;
-//    public boolean autoHood = true;
+    //    public boolean autoHood = true;
     public double shootStamp = 0.0;
-//    boolean fixedTurret = false;
+    //    boolean fixedTurret = false;
     Robot robot;
     MultipleTelemetry TELE;
     Light light;
@@ -66,10 +66,10 @@ public class TeleopV3 extends LinearOpMode {
     double xOffset = 0.0;
     double yOffset = 0.0;
     double hOffset = 0.0;
-//    double headingOffset = 0.0;
+    //    double headingOffset = 0.0;
     int ticker = 0;
 
-//    boolean autoSpintake = false;
+    //    boolean autoSpintake = false;
     boolean enableSpindexerManager = true;
 
 //    boolean overrideTurr = false;
@@ -142,12 +142,7 @@ public class TeleopV3 extends LinearOpMode {
 
             //DRIVETRAIN:
 
-            drivetrain.drive(
+            drivetrain.drive(-gamepad1.right_stick_y, gamepad1.right_stick_x, gamepad1.left_stick_x, gamepad1.left_trigger);
-                    -gamepad1.right_stick_y,
-                    gamepad1.right_stick_x,
-                    gamepad1.left_stick_x,
-                    gamepad1.left_trigger
-            );
 
             if (gamepad1.right_bumper) {
 
@@ -156,10 +151,10 @@ public class TeleopV3 extends LinearOpMode {
 
                 light.setState(StateEnums.LightState.BALL_COUNT);
 
-            } else if (gamepad2.triangle){
+            } else if (gamepad2.triangle) {
                 light.setState(StateEnums.LightState.BALL_COLOR);
 
-            }  else {
+            } else {
                 light.setState(StateEnums.LightState.GOAL_LOCK);
             }
 
@@ -182,17 +177,16 @@ public class TeleopV3 extends LinearOpMode {
 
 //            double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
 
-            targetingSettings = targeting.calculateSettings
+            targetingSettings = targeting.calculateSettings(robotX, robotY, robotHeading, 0.0, turretInterpolate);
-                    (robotX, robotY, robotHeading, 0.0, turretInterpolate);
 
             //RELOCALIZATION
 
-            if (gamepad2.squareWasPressed()){
+            if (gamepad2.squareWasPressed()) {
                 relocalize = !relocalize;
                 gamepad2.rumble(500);
             }
 
-            if (relocalize){
+            if (relocalize) {
                 turret.relocalize();
                 xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX;
                 yOffset = (turret.getLimelightX() * 39.3701) - robY;
@@ -231,6 +225,7 @@ public class TeleopV3 extends LinearOpMode {
 
             //HOOD:
 
+
             if (targetingHood) {
                 servo.setHoodPos(targetingSettings.hoodAngle + autoHoodOffset);
             } else {
@@ -267,7 +262,6 @@ public class TeleopV3 extends LinearOpMode {
             }
 
 
-
             if (enableSpindexerManager) {
                 //if (!shootAll) {
                 spindexer.processIntake();
@@ -299,7 +293,7 @@ public class TeleopV3 extends LinearOpMode {
                     if (shooterTicker == 0) {
                         spindexer.prepareShootAllContinous();
                         //TELE.addLine("preparing to shoot");
-//                    } else if (shooterTicker == 2) {
+//                   else if (shooterTicker == 2) {
 //                        //servo.setTransferPos(transferServo_in);
 //                        spindexer.shootAll();
 //                        TELE.addLine("starting to shoot");

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java

@@ -95,7 +95,10 @@ public class SortingTest extends LinearOpMode {
                 Actions.runBlocking(
                         autoActions.shootAllAuto(
                                 3.5,
-                                0.014
+                                0.014,
+                                0.501,
+                                0.501,
+                                0.501
                         )
                 );
                 intaking = true;

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

@@ -187,7 +187,7 @@ public class Spindexer {
         distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger);
 
         // Position 1
-        if (distanceRearCenter < 52) {
+        if (distanceRearCenter < 48) {
 
             // Mark Ball Found
             newPos1Detection = true;
@@ -200,9 +200,9 @@ public class Spindexer {
 
                 // FIXIT - Add filtering to improve accuracy.
                 if (gP >= 0.38) {
-                    ballPositions[0].ballColor = BallColor.GREEN;  // green
+                    ballPositions[commandedIntakePosition].ballColor = BallColor.GREEN;  // green
                 } else {
-                    ballPositions[0].ballColor = BallColor.PURPLE;  // purple
+                    ballPositions[commandedIntakePosition].ballColor = BallColor.PURPLE;  // purple
                 }
             }
         }
@@ -218,9 +218,9 @@ public class Spindexer {
                 double gP = color2RGBA.green / (color2RGBA.green + color2RGBA.red + color2RGBA.blue);
 
                 if (gP >= 0.4) {
-                    ballPositions[2].ballColor = BallColor.GREEN;  // green
+                    ballPositions[spindexerBallPos].ballColor = BallColor.GREEN;  // green
                 } else {
-                    ballPositions[2].ballColor = BallColor.PURPLE;  // purple
+                    ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE;  // purple
                 }
             }
         } else {
@@ -244,10 +244,10 @@ public class Spindexer {
 
                 double gP = color3RGBA.green / (color3RGBA.green + color3RGBA.red + color3RGBA.blue);
 
-                if (gP >= 0.4) {
+                if (gP >= 0.42) {
-                    ballPositions[1].ballColor = BallColor.GREEN;  // green
+                    ballPositions[spindexerBallPos].ballColor = BallColor.GREEN;  // green
                 } else {
-                    ballPositions[1].ballColor = BallColor.PURPLE;  // purple
+                    ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE;  // purple
                 }
             }
         } else {

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

@@ -59,6 +59,9 @@ public class Turret {
     private int prevPipeline = -1;
     PIDController bearingPID;
 
+    public int llCoast = 0;
+    public int LL_COAST_TICKS = 60;
+
     public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) {
         this.TELE = tele;
         this.robot = rob;
@@ -271,11 +274,18 @@ public class Turret {
 
         turretAngleDeg += permanentOffset;
 
+
         limelightRead();
         // Active correction if we see the target
         if (result.isValid() && !lockOffset && limelightUsed) {
             currentTrackOffset += bearingAlign(result);
             currentTrackCount++;
+
+            TELE.addData("LL Tracking: ", llCoast);
+
+            // Assume the last tracked value is always better than
+            // any previous value, even if its not fully aligned.
+            llCoast = LL_COAST_TICKS;
 //            double bearingError = Math.abs(tagBearingDeg);
 //
 //            if (bearingError > cameraBearingEqual) {
@@ -306,9 +316,15 @@ public class Turret {
 //            if (currentTrackCount > 20) {
 //                offset = currentTrackOffset;
 //            }
-            lightColor = Color.LightRed;
+            if (llCoast <= 0) {
-            currentTrackOffset = 0.0;
+                TELE.addData("LL No Track: ", llCoast);
-            currentTrackCount = 0;
+                lightColor = Color.LightRed;
+                currentTrackOffset = 0.0;
+                currentTrackCount = 0;
+            } else {
+                TELE.addData("LL Coasting: ", llCoast);
+                llCoast--;
+            }
         }
 
         // Apply accumulated offset

build.dependencies.gradle

@@ -6,7 +6,6 @@ repositories {
     maven { url = 'https://maven.brott.dev/' } //RR
     maven { url = "https://maven.rowanmcalpin.com/" } //Next FTC
     maven { url = "https://repo.dairy.foundation/releases" } //AS
-
 }
 
 dependencies {
@@ -25,8 +24,6 @@ dependencies {
     implementation 'com.pedropathing:ftc:2.0.6' //PedroCore
     implementation 'com.pedropathing:telemetry:1.0.0' //PedroTele
     implementation 'com.bylazar:fullpanels:1.0.2' //Panels
-
-
     implementation 'org.ftclib.ftclib:core:2.1.1' // FTC LIB
 
     implementation 'com.rowanmcalpin.nextftc:core:0.6.2' //NEXT FTC