worlin

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/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;
 //            }
+            if (llCoast <= 0) {
+                TELE.addData("LL No Track: ", llCoast);
                 lightColor = Color.LightRed;
                 currentTrackOffset = 0.0;
                 currentTrackCount = 0;
+            } else {
+                TELE.addData("LL Coasting: ", llCoast);
+                llCoast--;
+            }
         }
 
         // Apply accumulated offset