diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index a6fed47..23713f5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java +++ b/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.8; - public static double flywheel0Time = 2.2; + public static double flywheel0Time = 1.9; 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 = 8; + 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() { - Actions.runBlocking(autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)); + void shoot(double x, double y, double z) { + 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, diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java index f9697dc..fbb7d67 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java +++ b/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) ) ); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java index 52c4c02..3f07acd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java +++ b/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); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index 73ac55d..775b583 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/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_outtakeBall3b = 0.31; //0.65; //0.24; + public static double spindexer_outtakeBall3 = 0.84; //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_outtakeBall1 = 0.51; //0.27; //0.4; - public static double spinStartPos = 0.14; + public static double spindexer_outtakeBall2 = 0.66; //0.46; //0.6; + public static double spindexer_outtakeBall1 = 0.47; //0.27; //0.4; + public static double spinStartPos = 0.10; public static double spinEndPos = 0.95; public static double shootAllSpindexerSpeedIncrease = 0.014; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java index 6c63f72..0e30dbb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java +++ b/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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java index 089743c..43c8a7d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java +++ b/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) { - ballPositions[1].ballColor = BallColor.GREEN; // green + if (gP >= 0.42) { + ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green } else { - ballPositions[1].ballColor = BallColor.PURPLE; // purple + ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple } } } else { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java index ad71b6a..3f51931 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -271,6 +271,7 @@ public class Turret { turretAngleDeg += permanentOffset; + limelightRead(); // Active correction if we see the target if (result.isValid() && !lockOffset && limelightUsed) {