diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java index 97026e9..89e3d07 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java @@ -1,17 +1,22 @@ package org.firstinspires.ftc.teamcode.teleop; +import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodDefault; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.Pose2d; import com.arcrobotics.ftclib.gamepad.GamepadEx; import com.arcrobotics.ftclib.gamepad.GamepadKeys; import com.arcrobotics.ftclib.gamepad.ToggleButtonReader; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.subsystems.Drivetrain; import org.firstinspires.ftc.teamcode.subsystems.Intake; -import org.firstinspires.ftc.teamcode.subsystems.Rejecter; import org.firstinspires.ftc.teamcode.subsystems.Shooter; import org.firstinspires.ftc.teamcode.subsystems.Spindexer; import org.firstinspires.ftc.teamcode.subsystems.Transfer; @@ -23,7 +28,6 @@ import org.firstinspires.ftc.teamcode.utils.Robot; public class TeleopV1 extends LinearOpMode { - public static double rpos = 0.5; Robot robot; @@ -31,14 +35,10 @@ public class TeleopV1 extends LinearOpMode { Intake intake; - Rejecter rejecter; - Spindexer spindexer; Transfer transfer; - Shooter shooter; - MultipleTelemetry TELE; GamepadEx g1; @@ -49,34 +49,85 @@ public class TeleopV1 extends LinearOpMode { public static double slowMoSpeed = 0.4; - public static double shooterPower = 0.0; + public static double power = 0.0; - public static double turretPosition = 0.501; + public static double pos = hoodDefault; - public static double hoodPosition = 0.501; + public boolean all = false; + + public int ticker = 0; ToggleButtonReader g1RightBumper; - ToggleButtonReader g1LeftBumper; - ToggleButtonReader g2Circle; ToggleButtonReader g2Square; - ToggleButtonReader g2LeftBumper; ToggleButtonReader g2Triangle; + + ToggleButtonReader g2RightBumper; + + ToggleButtonReader g1LeftBumper; + + ToggleButtonReader g2LeftBumper; + + ToggleButtonReader g2DpadUp; + + ToggleButtonReader g2DpadDown; + + ToggleButtonReader g2DpadRight; + + ToggleButtonReader g2DpadLeft; + + public boolean leftBumper = false; public double g1RightBumperStamp = 0.0; + public double g1LeftBumperStamp = 0.0; + + + public double g2LeftBumperStamp = 0.0; + public static int spindexerPos = 0; - public double time = 0.0; + public boolean green = false; + + Shooter shooter; + + public boolean scoreAll = false; + + MecanumDrive drive ; + + public boolean autotrack = false; + + public int last = 0; + public int second = 0; + + public double offset = 0.0; + + public static double rIn = 0.59; + + public static double rOut = 0; + + public boolean notShooting = true; + + public boolean circle = false; + + public boolean square = false; + + public boolean tri = false; @Override public void runOpMode() throws InterruptedException { + drive = new MecanumDrive(hardwareMap, teleStart); + + + + + robot = new Robot(hardwareMap); TELE = new MultipleTelemetry( @@ -87,15 +138,15 @@ public class TeleopV1 extends LinearOpMode { g1 = new GamepadEx(gamepad1); g1RightBumper = new ToggleButtonReader( - g1, GamepadKeys.Button.RIGHT_BUMPER + g1, GamepadKeys.Button.RIGHT_BUMPER ); + g2 = new GamepadEx(gamepad2); + g1LeftBumper = new ToggleButtonReader( g1, GamepadKeys.Button.LEFT_BUMPER ); - g2 = new GamepadEx(gamepad2); - g2Circle = new ToggleButtonReader( g2, GamepadKeys.Button.B ); @@ -108,10 +159,33 @@ public class TeleopV1 extends LinearOpMode { g2, GamepadKeys.Button.X ); - g2LeftBumper = new ToggleButtonReader( + g2RightBumper = new ToggleButtonReader( + g2, GamepadKeys.Button.RIGHT_BUMPER + ); + + + g2LeftBumper = new ToggleButtonReader( g2, GamepadKeys.Button.LEFT_BUMPER ); + g2DpadUp = new ToggleButtonReader( + g2, GamepadKeys.Button.DPAD_UP + ); + + + g2DpadDown = new ToggleButtonReader( + g2, GamepadKeys.Button.DPAD_DOWN + ); + + g2DpadLeft = new ToggleButtonReader( + g2, GamepadKeys.Button.DPAD_LEFT + ); + + + g2DpadRight = new ToggleButtonReader( + g2, GamepadKeys.Button.DPAD_RIGHT + ); + @@ -127,36 +201,594 @@ public class TeleopV1 extends LinearOpMode { transfer = new Transfer(robot); - spindexer = new Spindexer(robot, TELE); - shooter = new Shooter(robot, TELE); + spindexer = new Spindexer(robot, TELE); spindexer.setTelemetryOn(true); - time = getRuntime(); + shooter = new Shooter(robot, TELE); + + shooter.setShooterMode("MANUAL"); + + robot.rejecter.setPosition(rIn); + + + waitForStart(); if (isStopRequested()) return; + drive = new MecanumDrive(hardwareMap, teleStart); + + while(opModeIsActive()){ - intake(); + drive.updatePoseEstimate(); - rejecter.rejecterPos(rpos); + TELE.addData("pose", drive.localizer.getPose()); + + TELE.addData("heading", drive.localizer.getPose().heading.toDouble()); + + + TELE.addData("off", offset); + + + robot.hood.setPosition(pos); + + g1LeftBumper.readValue(); + + if (g1LeftBumper.wasJustPressed()){ + g2LeftBumperStamp = getRuntime(); + + + + spindexer.intakeShake(getRuntime()); + + leftBumper = true; + } + + if (leftBumper){ + double time = getRuntime() - g2LeftBumperStamp; + + if (time < 1.0){ + robot.rejecter.setPosition(rOut); + } else { + robot.rejecter.setPosition(rIn); + } + + } + + + + + intake(); drivetrain.update(); TELE.update(); - transfer(); + transfer.update(); - shooter.setManualPower(shooterPower); + g2RightBumper.readValue(); - shooter.sethoodPosition(hoodPosition); + g2LeftBumper.readValue(); + + g2DpadDown.readValue(); + + g2DpadUp.readValue(); + + if (!scoreAll){ + spindexer.checkForBalls(); + } + + if(g2DpadUp.wasJustPressed()){ + pos -=0.02; + } + + if(g2DpadDown.wasJustPressed()){ + pos +=0.02; + } + + g2DpadLeft.readValue(); + + g2DpadRight.readValue(); + + if(g2DpadLeft.wasJustPressed()){ + offset -=0.02; + } + + if(g2DpadRight.wasJustPressed()){ + offset +=0.02; + } + + + + TELE.addData("hood", pos); + + + + + + + + + if (Math.abs(gamepad2.right_stick_x) < 0.1 && autotrack) { + + + + + shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset); + + } else { + + autotrack = false; + + shooter.moveTurret(0.3+offset); + + } + + if (gamepad2.right_stick_button){ + autotrack = true; + } + + + + if (g2RightBumper.wasJustPressed()){ + transfer.setTransferPower(1); + transfer.transferIn(); + shooter.setManualPower(1); + + notShooting = false; + + } + + if (g2RightBumper.wasJustReleased()){ + transfer.setTransferPower(1); + transfer.transferOut(); + } + + if (gamepad2.left_stick_y>0.5){ + + shooter.setManualPower(0); + } else if (gamepad2.left_stick_y<-0.5){ + shooter.setManualPower(1); + } + + if (g2LeftBumper.wasJustPressed()){ + g2LeftBumperStamp = getRuntime(); + notShooting = false; + scoreAll = true; + } + + if (scoreAll) { + double time = getRuntime() - g2LeftBumperStamp; + + + shooter.setManualPower(1); + + TELE.addData("greenImportant", green); + + TELE.addData("last", last); + TELE.addData("2ndLast", second); + + int numGreen = spindexer.greens(); + + if (square) { + + + if (time < 0.3) { + + ticker = 0; + + last = 0; + second = 0; + + + transfer.transferOut(); + transfer.setTransferPower(1); + } else if (time < 2) { + + if (ticker == 0) { + + if (numGreen == 2) { + last = spindexer.outtakePurple(second, last); + second = last; + } else { + last = spindexer.outtakeGreen(second, last); + second = last; + + } + } + + second = last; + + ticker++; + + + } else if (time < 2.5) { + + ticker = 0; + + second = last; + + + transfer.transferIn(); + } else if (time < 4) { + transfer.transferOut(); + + if (ticker == 0) { + + if (numGreen == 2) { + last = spindexer.outtakeGreen(second, last); + } else { + last = spindexer.outtakePurple(second, last); + + } + } + + ticker++; + } else if (time < 4.5) { + + ticker = 0; + + + transfer.transferIn(); + } else if (time < 6) { + + + transfer.transferOut(); + + if (ticker == 0) { + + if (numGreen == 2) { + last = spindexer.outtakeGreen(second, last); + } else { + last = spindexer.outtakePurple(second, last); + + } + } + + ticker++; + + } else if (time < 6.5) { + transfer.transferIn(); + } else { + + ticker = 0; + + + scoreAll = false; + transfer.transferOut(); + + shooter.setManualPower(0); + + } + } else if (tri) { + + + if (time < 0.3) { + + ticker = 0; + + last = 0; + second = 0; + + + transfer.transferOut(); + transfer.setTransferPower(1); + } else if (time < 2) { + + if (ticker == 0) { + + if (numGreen == 2) { + last = spindexer.outtakeGreen(second, last); + second = last; + } else { + last = spindexer.outtakePurple(second, last); + second = last; + + } + } + + second = last; + + ticker++; + + + } else if (time < 2.5) { + + ticker = 0; + + second = last; + + + transfer.transferIn(); + } else if (time < 4) { + transfer.transferOut(); + + if (ticker == 0) { + + if (numGreen == 2) { + last = spindexer.outtakePurple(second, last); + } else { + last = spindexer.outtakeGreen(second, last); + + } + } + + ticker++; + } else if (time < 4.5) { + + ticker = 0; + + + transfer.transferIn(); + } else if (time < 6) { + + + transfer.transferOut(); + + if (ticker == 0) { + + if (numGreen == 2) { + last = spindexer.outtakeGreen(second, last); + } else { + last = spindexer.outtakePurple(second, last); + + } + } + + ticker++; + + } else if (time < 6.5) { + transfer.transferIn(); + } else { + + ticker = 0; + + + scoreAll = false; + transfer.transferOut(); + + shooter.setManualPower(0); + + } + } else if (circle){ + + + if (time < 0.3) { + + ticker = 0; + + last = 0; + second = 0; + + + transfer.transferOut(); + transfer.setTransferPower(1); + } else if (time < 2) { + + if (ticker == 0) { + + if (numGreen == 2) { + last = spindexer.outtakeGreen(second, last); + second = last; + } else { + last = spindexer.outtakePurple(second, last); + second = last; + + } + } + + second = last; + + ticker++; + + + } else if (time < 2.5) { + + ticker = 0; + + second = last; + + + transfer.transferIn(); + } else if (time < 4) { + transfer.transferOut(); + + if (ticker == 0) { + + if (numGreen == 2) { + last = spindexer.outtakeGreen(second, last); + } else { + last = spindexer.outtakePurple(second, last); + + } + } + + ticker++; + } else if (time < 4.5) { + + ticker = 0; + + + transfer.transferIn(); + } else if (time < 6) { + + + transfer.transferOut(); + + if (ticker == 0) { + + if (numGreen == 2) { + last = spindexer.outtakePurple(second, last); + } else { + last = spindexer.outtakeGreen(second, last); + + } + } + + ticker++; + + } else if (time < 6.5) { + transfer.transferIn(); + } else { + + ticker = 0; + + + scoreAll = false; + transfer.transferOut(); + + shooter.setManualPower(0); + + } + } else{ + + + if (time < 0.3) { + + ticker = 0; + + last = 0; + second = 0; + + if (gamepad2.right_trigger > 0.5) { + green = false; + + all = gamepad2.left_trigger > 0.5; + + } else if (gamepad2.left_trigger > 0.5) { + green = true; + + all = false; + } else { + all = true; + } + + + transfer.transferOut(); + transfer.setTransferPower(1); + } else if (time < 2) { + + if (ticker == 0) { + + if (all) { + spindexer.outtake3(); + last = 3; + second = 3; + } else if (green) { + last = spindexer.outtakeGreen(second, last); + second = last; + } else { + last = spindexer.outtakePurple(second, last); + second = last; + + } + } + + second = last; + + ticker++; + + + } else if (time < 2.5) { + + ticker = 0; + + second = last; + + if (gamepad2.right_trigger > 0.5) { + green = false; + + all = gamepad2.left_trigger > 0.5; + + } else if (gamepad2.left_trigger > 0.5) { + green = true; + + all = false; + + + } + + transfer.transferIn(); + } else if (time < 4) { + transfer.transferOut(); + + if (ticker == 0) { + + if (all) { + spindexer.outtake2(); + + last = 2; + } else if (green) { + last = spindexer.outtakeGreen(second, last); + } else { + last = spindexer.outtakePurple(second, last); + + } + } + + ticker++; + } else if (time < 4.5) { + + ticker = 0; + + + if (gamepad2.right_trigger > 0.5) { + green = false; + + all = gamepad2.left_trigger > 0.5; + + } else if (gamepad2.left_trigger > 0.5) { + green = true; + + all = false; + } + + transfer.transferIn(); + } else if (time < 6) { + + + transfer.transferOut(); + + if (ticker == 0) { + + if (all) { + spindexer.outtake1(); + } else if (green) { + last = spindexer.outtakeGreen(second, last); + } else { + last = spindexer.outtakePurple(second, last); + + } + } + + ticker++; + + } else if (time < 6.5) { + transfer.transferIn(); + } else { + + ticker = 0; + + + scoreAll = false; + transfer.transferOut(); + + shooter.setManualPower(0); + + } + + + } + } + + + shooter.update(); - shooter.setTurretPosition(turretPosition); @@ -188,6 +820,10 @@ public class TeleopV1 extends LinearOpMode { if (g1RightBumper.wasJustPressed()){ + notShooting = true; + + + if (getRuntime() - g1RightBumperStamp < 0.3){ intake.reverse(); @@ -195,47 +831,65 @@ public class TeleopV1 extends LinearOpMode { intake.toggle(); } + if (intake.getIntakeState()==1){ + shooter.setManualPower(0); + } + + + + spindexer.intake(); + transfer.transferOut(); + g1RightBumperStamp = getRuntime(); } - if (intake.getIntakeState()==1) { + + if (intake.getIntakeState()==1 && notShooting) { + spindexer.intakeShake(getRuntime()); - transfer.setTransferPowerOff(); - transfer.setTransferPositionOff(); + } else { - - if (g2Circle.wasJustPressed()){ - transfer.setTransferPositionOff(); - intake.intakeMinPower(); - spindexer.outtake3(); - transfer.setTransferPowerOn(); + circle = true; + tri = false; + square = false; + + + } if (g2Triangle.wasJustPressed()){ - transfer.setTransferPositionOff(); - intake.intakeMinPower(); - spindexer.outtake2(); - transfer.setTransferPowerOn(); + circle = false; + tri = true; + square = false; } if (g2Square.wasJustPressed()){ - transfer.setTransferPositionOff(); - intake.intakeMinPower(); - spindexer.outtake1(); - transfer.setTransferPowerOn(); + circle = false; + tri = false; + square = true; } + if (gamepad2.x){ + circle = false; + tri = false; + square = false; + } + + + + } + intake.update(); - transfer.update(); + spindexer.update(); @@ -243,17 +897,5 @@ public class TeleopV1 extends LinearOpMode { - } - - public void transfer(){ - - g1LeftBumper.readValue(); - - if (g1LeftBumper.wasJustPressed()){ - transfer.setTransferPositionOn(); - } - - transfer.update(); - } }