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 new file mode 100644 index 0000000..17fd9b8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.teleop; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +public class TeleopV2 extends LinearOpMode { + + Rob + + + @Override + public void runOpMode() throws InterruptedException { + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old.java new file mode 100644 index 0000000..1ce3e14 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old.java @@ -0,0 +1,793 @@ +package org.firstinspires.ftc.teamcode.teleop; + +import static org.firstinspires.ftc.teamcode.constants.Poses.*; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; + +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.Disabled; +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.Shooter; +import org.firstinspires.ftc.teamcode.subsystems.Spindexer; +import org.firstinspires.ftc.teamcode.subsystems.Transfer; +import org.firstinspires.ftc.teamcode.utils.Robot; + +@Config +@TeleOp +@Disabled +public class old extends LinearOpMode { + + Robot robot; + + Drivetrain drivetrain; + + Intake intake; + + Spindexer spindexer; + + Transfer transfer; + + MultipleTelemetry TELE; + + GamepadEx g1; + + GamepadEx g2; + + public static double defaultSpeed = 1; + + public static double slowMoSpeed = 0.4; + + public static double power = 0.0; + + public static double pos = hoodDefault; + + public boolean all = false; + + public int ticker = 0; + + ToggleButtonReader g1RightBumper; + + ToggleButtonReader g2Circle; + + ToggleButtonReader g2Square; + + 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 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( + FtcDashboard.getInstance().getTelemetry(), + telemetry + ); + + g1 = new GamepadEx(gamepad1); + + g1RightBumper = new ToggleButtonReader( + g1, GamepadKeys.Button.RIGHT_BUMPER + ); + + g2 = new GamepadEx(gamepad2); + + g1LeftBumper = new ToggleButtonReader( + g1, GamepadKeys.Button.LEFT_BUMPER + ); + + g2Circle = new ToggleButtonReader( + g2, GamepadKeys.Button.B + ); + + g2Triangle = new ToggleButtonReader( + g2, GamepadKeys.Button.Y + ); + + g2Square = new ToggleButtonReader( + g2, GamepadKeys.Button.X + ); + + 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 + ); + + drivetrain = new Drivetrain(robot, TELE, g1); + + drivetrain.setMode("Default"); + + drivetrain.setDefaultSpeed(defaultSpeed); + + drivetrain.setSlowSpeed(slowMoSpeed); + + intake = new Intake(robot); + + transfer = new Transfer(robot); + + spindexer = new Spindexer(robot, TELE); + + spindexer.setTelemetryOn(true); + + shooter = new Shooter(robot, TELE); + + shooter.setShooterMode("MANUAL"); + + robot.rejecter.setPosition(rIn); + + waitForStart(); + + if (isStopRequested()) return; + + drive = new MecanumDrive(hardwareMap, teleStart); + + while (opModeIsActive()) { + + drive.updatePoseEstimate(); + + 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.update(); + + g2RightBumper.readValue(); + + 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(); + + } + + } + + public void intake() { + + g1RightBumper.readValue(); + + g2Circle.readValue(); + + g2Square.readValue(); + + g2Triangle.readValue(); + + if (g1RightBumper.wasJustPressed()) { + + notShooting = true; + + if (getRuntime() - g1RightBumperStamp < 0.3) { + intake.reverse(); + } else { + intake.toggle(); + } + + if (intake.getIntakeState() == 1) { + shooter.setManualPower(0); + } + + spindexer.intake(); + + transfer.transferOut(); + + g1RightBumperStamp = getRuntime(); + + } + + if (intake.getIntakeState() == 1 && notShooting) { + + spindexer.intakeShake(getRuntime()); + + } else { + if (g2Circle.wasJustPressed()) { + circle = true; + tri = false; + square = false; + + } + + if (g2Triangle.wasJustPressed()) { + circle = false; + tri = true; + square = false; + } + + if (g2Square.wasJustPressed()) { + circle = false; + tri = false; + square = true; + } + + if (gamepad2.x) { + circle = false; + tri = false; + square = false; + } + + } + + intake.update(); + + spindexer.update(); + + } +} +