diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java index ae72769..ef2b8f7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java @@ -99,6 +99,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { public static double shoot0SpinSpeedIncrease = 0.015; public static double spindexerSpeedIncrease = 0.03; + public static double finalSpindexerSpeedIncrease = 0.025; + public static double redObeliskTurrPos1 = 0.52; public static double redObeliskTurrPos2 = 0.53; @@ -117,6 +119,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { public static double blueTurretShootPos = 0.26; double turretShootPos = 0.0; + public static double finalShootAllTime = 3.0; public static double shootAllTime = 1.8; public static double shoot0Time = 1.6; public static double intake1Time = 3.3; @@ -125,7 +128,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { public static double intake3Time = 4.2; public static double flywheel0Time = 3.5; - public static double pickup1Speed = 19; + public static double pickup1Speed = 23; // ---- SECOND SHOT / PICKUP ---- public static double shoot1Vel = 2300; public static double shoot1Hood = 0.93; @@ -1071,13 +1074,13 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { Actions.runBlocking( new ParallelAction( manageShooterAuto( - shootAllTime, + finalShootAllTime, 0.501, 0.501, 0.501, 0.501 ), - shootAllAuto(shootAllTime, spindexerSpeedIncrease) + shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease) ) ); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java index 9ef9caf..268855c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java @@ -98,7 +98,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode { public static double autoSpinStartPos = 0.2; public static double shoot0SpinSpeedIncrease = 0.015; - public static double gateIntakeTime = 5.0; //TODO: Increase ig + public static double gateIntakeTime = 5.9; //TODO: Increase ig public static double spindexerSpeedIncrease = 0.03; @@ -116,9 +116,11 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode { double turretShootPos = 0.52; - public static double shootAllTime = 1.8; + public static double shootAllTime = 1.9; public static double shoot0Time = 1.6; - public static double intake1Time = 3.0; + public static double intake1Time = 3.3; + public static double intake2Time = 3.6; + public static double flywheel0Time = 3.5; public static double pickup1Speed = 25; // ---- SECOND SHOT / PICKUP ---- @@ -768,6 +770,8 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode { TrajectoryActionBuilder pickup3 = null; TrajectoryActionBuilder shoot3 = null; + Turret.limelightUsed = false; + robot.light.setPosition(1); while (opModeInInit()) { @@ -952,7 +956,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode { manageFlywheel( shootAllVelocity, shootAllHood, - normalIntakeTime, + intake2Time, 0.501, 0.501, 0.501, @@ -960,7 +964,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode { ), intake(intake1Time), detectObelisk( - intake1Time, + intake2Time, 0.501, 0.501, obelisk1XTolerance, @@ -1010,8 +1014,8 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode { gatePickup.build(), manageShooterAuto( gateIntakeTime, - x2b, - y2b, + 0.501, + 0.501, pickup1XTolerance, pickup1YTolerance ), @@ -1060,15 +1064,15 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode { new ParallelAction( pickup1.build(), manageShooterAuto( - normalIntakeTime, + intake1Time, 0.501, 0.501, pickup1XTolerance, pickup1YTolerance ), - intake(normalIntakeTime), + intake(intake1Time), detectObelisk( - normalIntakeTime, + intake1Time, 0.501, 0.501, obelisk1XTolerance, diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java index 687f6c5..8107157 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java @@ -14,12 +14,12 @@ public class Poses_V2 { public static double rXGateB = 56, rYGateB = 37, rHGateB = 2.7750735106709836; - public static double rXGate = 30, rYGate = 63, rHGate = Math.toRadians(179); + public static double rXGate = 28, rYGate = 65, rHGate = Math.toRadians(179); public static double bXGateA = 35.5, bYGateA = -46.5, bHGateA = -2.7; public static double bXGateB = 56, bYGateB = -37, bHGateB = -2.7750735106709836; - public static double bXGate = 30, bYGate = -63, bHGate = Math.toRadians(-179); + public static double bXGate = 28, bYGate = -65, bHGate = Math.toRadians(-179); } 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 58492c7..e3e2ba6 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,11 +5,11 @@ import com.acmerobotics.dashboard.config.Config; @Config public class ServoPositions { - public static double spindexer_intakePos1 = 0.05; //0.13; + public static double spindexer_intakePos1 = 0.07; //0.13; - public static double spindexer_intakePos2 = 0.24; //0.33;//0.5; + public static double spindexer_intakePos2 = 0.27; //0.33;//0.5; - public static double spindexer_intakePos3 = 0.43; //0.53;//0.66; + public static double spindexer_intakePos3 = 0.46; //0.53;//0.66; public static double spindexer_outtakeBall3 = 0.71; //0.65; //0.24; public static double spindexer_outtakeBall3b = 0.15; //0.65; //0.24; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index 124ad25..968f8db 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -2,7 +2,6 @@ package org.firstinspires.ftc.teamcode.teleop; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; @@ -37,16 +36,14 @@ public class TeleopV3 extends LinearOpMode { public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0; public static double spinSpeedIncrease = 0.03; public static int resetSpinTicks = 4; + public static double manualOffset = 0.0; + public static double hoodSpeedOffset = 0.01; + public static double turretSpeedOffset = 0.01; public double vel = 3000; public boolean autoVel = true; public boolean targetingHood = true; - public static double manualOffset = 0.0; public boolean autoHood = true; - public double shootStamp = 0.0; - public static double hoodSpeedOffset = 0.01; - public static double turretSpeedOffset = 0.01; - boolean fixedTurret = false; Robot robot; MultipleTelemetry TELE; @@ -113,6 +110,9 @@ public class TeleopV3 extends LinearOpMode { } } + limelightUsed= true; + + waitForStart(); if (isStopRequested()) return; @@ -210,18 +210,18 @@ public class TeleopV3 extends LinearOpMode { gamepad2.rumble(80); } - if (gamepad2.dpadLeftWasPressed()){ + if (gamepad2.dpadLeftWasPressed()) { manualOffset -= turretSpeedOffset; gamepad2.rumble(80); - } else if (gamepad2.dpadRightWasPressed()){ + } else if (gamepad2.dpadRightWasPressed()) { manualOffset += turretSpeedOffset; gamepad2.rumble(80); } - if (gamepad2.rightBumperWasPressed()){ + if (gamepad2.rightBumperWasPressed()) { limelightUsed = true; gamepad2.rumble(80); - } else if (gamepad2.leftBumperWasPressed()){ + } else if (gamepad2.leftBumperWasPressed()) { limelightUsed = false; gamepad2.rumble(80); } @@ -238,6 +238,9 @@ public class TeleopV3 extends LinearOpMode { // RIGHT_BUMPER if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) { robot.intake.setPower(1); + } else if (gamepad1.cross) { + robot.intake.setPower(-1); + } else { robot.intake.setPower(0); 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 057035a..ea5aa6e 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 @@ -201,7 +201,7 @@ public class Spindexer { // Position 2 // Find which ball position this is in the spindexer spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()]; - if (distanceFrontDriver < 50) { + if (distanceFrontDriver < 56) { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; if (detectFrontColor) {