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 6cea6cf..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; @@ -131,8 +131,12 @@ public class Red_V2 extends LinearOpMode { TELE.addData("Velocity", velo); TELE.update(); + if (last && !steady){ stamp = getRuntime(); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); return false; } else if (steady) { stamp = getRuntime(); @@ -230,6 +234,11 @@ public class Red_V2 extends LinearOpMode { TELE.addData("Velocity", velo); TELE.addLine("spindex"); TELE.update(); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + return !spindexPosEqual(spindexer); } }; @@ -284,6 +293,10 @@ public class Red_V2 extends LinearOpMode { robot.shooter1.setPower(powPID); robot.shooter2.setPower(powPID); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + if (ticker == 1) { transferStamp = getRuntime(); @@ -339,9 +352,12 @@ public class Red_V2 extends LinearOpMode { TELE.addLine("Intaking"); TELE.update(); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + 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; @@ -350,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; @@ -374,6 +411,10 @@ public class Red_V2 extends LinearOpMode { double s2D = robot.color2.getDistance(DistanceUnit.MM); double s3D = robot.color3.getDistance(DistanceUnit.MM); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + if (s1D < 40) { double green = robot.color1.getNormalizedColors().green; @@ -477,14 +518,14 @@ public class Red_V2 extends LinearOpMode { while (opModeInInit()) { if (gamepad2.dpadUpWasPressed()) { - hoodDefault -= 0.01; + hoodAuto-= 0.01; } if (gamepad2.dpadDownWasPressed()) { - hoodDefault += 0.01; + hoodAuto += 0.01; } - robot.hood.setPosition(hoodDefault); + robot.hood.setPosition(hoodAuto); robot.turr1.setPosition(turret_detectRed); robot.turr2.setPosition(1 - turret_detectRed); @@ -499,13 +540,14 @@ public class Red_V2 extends LinearOpMode { TELE.update(); } + waitForStart(); if (isStopRequested()) return; if (opModeIsActive()) { - robot.hood.setPosition(hoodStart); + robot.hood.setPosition(hoodAuto); Actions.runBlocking( new ParallelAction( @@ -514,6 +556,9 @@ public class Red_V2 extends LinearOpMode { Obelisk() ) ); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition()); velo = flywheel.getVelo(); @@ -522,7 +567,11 @@ public class Red_V2 extends LinearOpMode { shootingSequence(); - robot.hood.setPosition(hoodDefault); + robot.hood.setPosition(hoodAuto); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); Actions.runBlocking( new ParallelAction( @@ -530,21 +579,32 @@ public class Red_V2 extends LinearOpMode { intake(intake1Time) ) ); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); Actions.runBlocking( new ParallelAction( shoot1.build(), ColorDetect(), - steadyShooter(AUTO_CLOSE_VEL, true) + steadyShooter(AUTO_CLOSE_VEL, true), + intakeReject() ) ); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition()); velo = flywheel.getVelo(); robot.shooter1.setPower(powPID); robot.shooter2.setPower(powPID); shootingSequence(); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); Actions.runBlocking( new ParallelAction( @@ -552,12 +612,17 @@ public class Red_V2 extends LinearOpMode { intake(intake2Time) ) ); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); Actions.runBlocking( new ParallelAction( shoot2.build(), ColorDetect(), - steadyShooter(AUTO_CLOSE_VEL, true) + steadyShooter(AUTO_CLOSE_VEL, true), + intakeReject() + ) );