diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java index e29ad6e..102b1ea 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java @@ -29,7 +29,7 @@ import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Robot; @Config -@Autonomous +@Autonomous(preselectTeleOp = "TeleopV2") public class Blue_V2 extends LinearOpMode { Robot robot; @@ -44,9 +44,9 @@ public class Blue_V2 extends LinearOpMode { double velo = 0.0; double targetVelocity = 0.0; - public static double intake1Time = 6.5; + public static double intake1Time = 2.9; - public static double intake2Time = 6.5; + public static double intake2Time = 2.9; public static double colorDetect = 3.0; @@ -187,6 +187,39 @@ public class Blue_V2 extends LinearOpMode { }; } + public Action intakeReject() { + return new Action() { + double stamp = 0.0; + int ticker = 0; + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + + double position = 0.0; + + if ((getRuntime() % 0.3) > 0.15) { + position = spindexer_intakePos1 + 0.02; + } else { + position = spindexer_intakePos1 - 0.02; + } + + robot.spin1.setPosition(position); + robot.spin2.setPosition(1 - position); + + if (ticker == 0) { + stamp = getRuntime(); + } + ticker++; + + if (getRuntime() - stamp < 0.3){ + return true; + }else { + robot.intake.setPower(0); + return false; + } + } + }; + } + public Action spindex (double spindexer, double vel){ return new Action() { double currentPos = 0.0; @@ -359,7 +392,6 @@ public class Blue_V2 extends LinearOpMode { robot.intake.setPower(1); if ((s1D < 40.0 && s2D < 40.0 && s3D < 40.0) || getRuntime() - stamp > intakeTime) { - robot.intake.setPower(0); return false; } else { return true; @@ -567,7 +599,8 @@ public class Blue_V2 extends LinearOpMode { new ParallelAction( shoot1.build(), ColorDetect(), - steadyShooter(AUTO_CLOSE_VEL, true) + steadyShooter(AUTO_CLOSE_VEL, true), + intakeReject() ) ); @@ -599,7 +632,8 @@ public class Blue_V2 extends LinearOpMode { new ParallelAction( shoot2.build(), ColorDetect(), - steadyShooter(AUTO_CLOSE_VEL, true) + steadyShooter(AUTO_CLOSE_VEL, true), + intakeReject() ) ); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java index ad040a3..1edabf1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java @@ -28,7 +28,7 @@ import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam; import org.firstinspires.ftc.teamcode.utils.Robot; @Config -@Autonomous +@Autonomous(preselectTeleOp = "TeleopV2") public class Red_V2 extends LinearOpMode { Robot robot; @@ -43,9 +43,9 @@ public class Red_V2 extends LinearOpMode { double velo = 0.0; double targetVelocity = 0.0; - public static double intake1Time = 6.5; + public static double intake1Time = 2.9; - public static double intake2Time = 6.5; + public static double intake2Time = 2.9; public static double colorDetect = 3.0; @@ -358,7 +358,6 @@ public class Red_V2 extends LinearOpMode { robot.intake.setPower(1); if ((s1D < 40.0 && s2D < 40.0 && s3D < 40.0) || getRuntime() - stamp > intakeTime) { - robot.intake.setPower(0); return false; } else { return true; @@ -367,6 +366,27 @@ public class Red_V2 extends LinearOpMode { }; } + public Action intakeReject() { + return new Action() { + double stamp = 0.0; + int ticker = 0; + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if (ticker == 0) { + stamp = getRuntime(); + } + ticker++; + + if (getRuntime() - stamp < 0.3){ + return true; + }else { + robot.intake.setPower(0); + return false; + } + } + }; + } + public Action ColorDetect() { return new Action() { double stamp = 0.0; @@ -567,7 +587,8 @@ public class Red_V2 extends LinearOpMode { new ParallelAction( shoot1.build(), ColorDetect(), - steadyShooter(AUTO_CLOSE_VEL, true) + steadyShooter(AUTO_CLOSE_VEL, true), + intakeReject() ) ); @@ -599,7 +620,9 @@ public class Red_V2 extends LinearOpMode { new ParallelAction( shoot2.build(), ColorDetect(), - steadyShooter(AUTO_CLOSE_VEL, true) + steadyShooter(AUTO_CLOSE_VEL, true), + intakeReject() + ) ); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java index c442d1b..31fbc1d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java @@ -26,7 +26,7 @@ public class Poses { - public static double rx1 = 46, ry1 = -4, rh1 = 0; + public static double rx1 = 45, ry1 = -7, rh1 = 0; public static double rx2a = 45, ry2a = 5, rh2a = Math.toRadians(140); @@ -36,16 +36,16 @@ public class Poses { public static double rx3b = 34, ry3b = 58, rh3b = Math.toRadians(140); - public static double bx1 = 46, by1 = 4, bh1 = 0; + public static double bx1 = 45, by1 = 6, bh1 = 0; - public static double bx2a = 45, by2a = -5, bh2a = Math.toRadians(-140); + public static double bx2a = 53, by2a = -7, bh2a = Math.toRadians(-140); - public static double bx2b = 31, by2b = -32, bh2b = Math.toRadians(-140); + public static double bx2b = 23, by2b = -39, bh2b = Math.toRadians(-140); - public static double bx3a = 58, by3a = -42, bh3a = Math.toRadians(-140); + public static double bx3a = 56, by3a = -34, bh3a = Math.toRadians(-140); public static double bx3b = 34, by3b = -58, bh3b = Math.toRadians(-140); - public static Pose2d teleStart = new Pose2d(rx1, 10, 0); + public static Pose2d teleStart = new Pose2d(rx1, ry1, rh1); } 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 0d251bc..6204681 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 @@ -24,13 +24,13 @@ public class ServoPositions { public static double hoodDefault = 0.6; - public static double hoodAuto = 0.59; + public static double hoodAuto = 0.55; public static double hoodHigh = 0.21; public static double hoodLow = 1.0; - public static double turret_red = 0.4; + public static double turret_red = 0.42; public static double turret_blue = 0.38; public static double turret_detectRed = 0.2; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java index 45ad2e2..a1f82d5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java @@ -73,13 +73,13 @@ public final class MecanumDrive { public double kA = 0.000046; // path profile parameters (in inches) - public double maxWheelVel = 120; - public double minProfileAccel = -30; - public double maxProfileAccel = 120; + public double maxWheelVel = 180; + public double minProfileAccel = -40; + public double maxProfileAccel = 180; // turn profile parameters (in radians) - public double maxAngVel = 2* Math.PI; // shared with path - public double maxAngAccel = 2* Math.PI; + public double maxAngVel = 4* Math.PI; // shared with path + public double maxAngAccel = 4* Math.PI; // path controller gains public double axialGain = 4; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java index 2df5377..977aced 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java @@ -43,7 +43,6 @@ public class TeleopV2 extends LinearOpMode { public static double velMultiplier = 20; public static double shootStamp2 = 0.0; - public double vel = 3000; public boolean autoVel = true; public double manualOffset = 0.0; @@ -89,7 +88,6 @@ public class TeleopV2 extends LinearOpMode { boolean outtake3 = false; boolean overrideTurr = false; - List shootOrder = new ArrayList<>(); boolean emergency = false; private double lastEncoderRevolutions = 0.0; @@ -171,7 +169,6 @@ public class TeleopV2 extends LinearOpMode { emergency = false; overrideTurr = false; - } if (gamepad1.leftBumperWasPressed()) { @@ -180,7 +177,6 @@ public class TeleopV2 extends LinearOpMode { shootAll = false; overrideTurr = false; - } if (intake) { @@ -336,8 +332,11 @@ public class TeleopV2 extends LinearOpMode { double offset; - double robotX = drive.localizer.getPose().position.x - xOffset; - double robotY = drive.localizer.getPose().position.y - xOffset; + double robX = drive.localizer.getPose().position.x; + double robY = drive.localizer.getPose().position.y; + + double robotX = robX - xOffset; + double robotY = robY - yOffset; double robotHeading = drive.localizer.getPose().heading.toDouble(); double goalX = -10; @@ -431,12 +430,11 @@ public class TeleopV2 extends LinearOpMode { if (gamepad2.left_stick_x > 0.5) { manualTurret = false; } else if (gamepad2.left_stick_x < -0.5) { - manualTurret = true; manualOffset = 0; - autoHoodOffset = 0; + manualTurret = true; if (gamepad2.left_bumper) { - xOffset = robotX; - yOffset = robotY; + xOffset = robX; + yOffset = robY; headingOffset = robotHeading; } } @@ -456,7 +454,7 @@ public class TeleopV2 extends LinearOpMode { //SHOOT ALL:] if (emergency) { - intake = false; + intake = true; reject = false; if (getRuntime() % 3 > 1.5) { @@ -494,25 +492,22 @@ public class TeleopV2 extends LinearOpMode { if (d20 != null) { overrideTurr = true; double bearing = d20.ftcPose.bearing; - double finalPos =robot.turr1.getPosition() - (bearing/1300); + + double finalPos = robot.turr1.getPosition() - (bearing / 1300); robot.turr1.setPosition(finalPos); - robot.turr2.setPosition(1-finalPos); + robot.turr2.setPosition(1 - finalPos); TELE.addData("Bear", bearing); - - - - } if (d24 != null) { overrideTurr = true; double bearing = d24.ftcPose.bearing; - double finalPos = turretPos() + (bearing/720); + double finalPos = robot.turr1.getPosition() - (bearing / 1300); robot.turr1.setPosition(finalPos); - robot.turr2.setPosition(1-finalPos); + robot.turr2.setPosition(1 - finalPos); } @@ -577,10 +572,14 @@ public class TeleopV2 extends LinearOpMode { outtake1 = false; outtake2 = false; outtake3 = false; + + + + + + overrideTurr = false; - - } } @@ -660,17 +659,17 @@ public class TeleopV2 extends LinearOpMode { // Fastest order (example: slot 3 → 2 → 1) - if (ballIn(3)){ + if (ballIn(3)) { shootOrder.add(3); } - if (ballIn(2)){ + if (ballIn(2)) { shootOrder.add(2); } - if (ballIn(1)){ + if (ballIn(1)) { shootOrder.add(1); } @@ -690,38 +689,37 @@ public class TeleopV2 extends LinearOpMode { shootOrder.add(1); } - - shootAll = true; - shootPos = drive.localizer.getPose(); - - } - - // Right bumper shoots all balls fastest, ignoring colors - if (gamepad2.leftBumperWasPressed()) { - shootOrder.clear(); - shootStamp = getRuntime(); - - outtake1 = false; - outtake2 = false; - outtake3 = false; - - // Fastest order (example: slot 3 → 2 → 1) - - if (ballIn(3)) { - shootOrder.add(3); - } - - if (ballIn(2)) { - shootOrder.add(2); - } - if (ballIn(1)) { - shootOrder.add(1); - } shootAll = true; shootPos = drive.localizer.getPose(); } +// // Right bumper shoots all balls fastest, ignoring colors +// if (gamepad2.leftBumperWasPressed()) { +// shootOrder.clear(); +// shootStamp = getRuntime(); +// +// outtake1 = false; +// outtake2 = false; +// outtake3 = false; +// +// // Fastest order (example: slot 3 → 2 → 1) +// +// if (ballIn(3)) { +// shootOrder.add(3); +// } +// +// if (ballIn(2)) { +// shootOrder.add(2); +// } +// if (ballIn(1)) { +// shootOrder.add(1); +// } +// shootAll = true; +// shootPos = drive.localizer.getPose(); +// +// } +// if (gamepad2.crossWasPressed()) { emergency = !emergency; @@ -943,7 +941,7 @@ public class TeleopV2 extends LinearOpMode { } public double turretPos() { - return (scalar*((robot.turr1Pos.getVoltage()-restPos) /3.3 )); + return (scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3)); } }