From dc432f7686051828c81ccf8931f5a446f7094995 Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Sat, 8 Nov 2025 07:46:04 -0600 Subject: [PATCH] Before LM1 --- .../ftc/teamcode/autonomous/Blue.java | 425 ++++++++++++++++++ .../ftc/teamcode/autonomous/Red.java | 8 +- .../ftc/teamcode/constants/Poses.java | 2 +- .../ftc/teamcode/teleop/TeleopV1.java | 5 - 4 files changed, 433 insertions(+), 7 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue.java new file mode 100644 index 0000000..f557a1d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue.java @@ -0,0 +1,425 @@ +package org.firstinspires.ftc.teamcode.autonomous; + +import static org.firstinspires.ftc.teamcode.constants.Poses.h1; +import static org.firstinspires.ftc.teamcode.constants.Poses.h2; +import static org.firstinspires.ftc.teamcode.constants.Poses.h2_b; +import static org.firstinspires.ftc.teamcode.constants.Poses.h3; +import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Poses.x1; +import static org.firstinspires.ftc.teamcode.constants.Poses.x2; +import static org.firstinspires.ftc.teamcode.constants.Poses.x2_b; +import static org.firstinspires.ftc.teamcode.constants.Poses.x3; +import static org.firstinspires.ftc.teamcode.constants.Poses.y1; +import static org.firstinspires.ftc.teamcode.constants.Poses.y2; +import static org.firstinspires.ftc.teamcode.constants.Poses.y2_b; +import static org.firstinspires.ftc.teamcode.constants.Poses.y3; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.ParallelAction; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.TrajectoryActionBuilder; +import com.acmerobotics.roadrunner.Vector2d; +import com.acmerobotics.roadrunner.ftc.Actions; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; +import org.firstinspires.ftc.teamcode.subsystems.AprilTag; +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 +@Autonomous +public class Blue extends LinearOpMode { + + Robot robot; + + MultipleTelemetry TELE; + + MecanumDrive drive; + + AprilTag aprilTag; + + Shooter shooter; + + + + public static double angle = 0.1; + + Spindexer spindexer; + + Transfer transfer; + + + + + + @Override + public void runOpMode() throws InterruptedException { + + + + + robot = new Robot(hardwareMap); + TELE = new MultipleTelemetry( + telemetry, FtcDashboard.getInstance().getTelemetry() + ); + + drive = new MecanumDrive(hardwareMap, new Pose2d( + 0,0,0 + )); + + aprilTag = new AprilTag(robot, TELE); + + shooter = new Shooter(robot, TELE); + + spindexer = new Spindexer(robot, TELE); + + spindexer.outtake3(); + + shooter.setShooterMode("MANUAL"); + + shooter.sethoodPosition(0.53); + + transfer = new Transfer(robot); + + transfer.setTransferPosition(transferServo_out); + + Intake intake = new Intake(robot); + + robot.hood.setPosition(0.54); + + + + + + + + + TrajectoryActionBuilder traj1 = drive.actionBuilder(new Pose2d(0, 0, 0)) + .strafeToLinearHeading(new Vector2d(x1, y1), h1 ); + + + TrajectoryActionBuilder traj2 = drive.actionBuilder(new Pose2d(x1, y1, h1)) + .turnTo(Math.toRadians(-135)) + .strafeToLinearHeading(new Vector2d(x2, -y2), -h2 ); + + + TrajectoryActionBuilder traj3 = drive.actionBuilder(new Pose2d(x2, -y2, -h2)) + .strafeToLinearHeading(new Vector2d(x1, y1), h1 ); + + TrajectoryActionBuilder traj4 = drive.actionBuilder(new Pose2d(x1, y1, h1)) + + .strafeToLinearHeading(new Vector2d(x2_b, -y2_b), -h2_b ) + + .strafeToLinearHeading(new Vector2d(x3, -y3), -h3 ); + + + TrajectoryActionBuilder traj5 = drive.actionBuilder(new Pose2d(x3, -y3, -h3)) + .strafeToLinearHeading(new Vector2d(x1, y1), h1 ); + + + TrajectoryActionBuilder traj6 = drive.actionBuilder(new Pose2d(x1, y1, h1)) + .strafeToLinearHeading(new Vector2d(x1, y1-30), h1 ); + + while(opModeInInit()) { + + shooter.setTurretPosition(0.33); + + aprilTag.initTelemetry(); + + aprilTag.update(); + shooter.update(); + + TELE.update(); + } + + + waitForStart(); + + if (isStopRequested()) return; + + if (opModeIsActive()){ + + robot.hood.setPosition(0.54); + + transfer.setTransferPower(1); + + transfer.update(); + + + + + Actions.runBlocking( + new ParallelAction( + traj1.build() + ) + ); + + + transfer.setTransferPower(1); + + transfer.update(); + + shooter.setManualPower(1); + + double stamp = getRuntime(); + + + stamp = getRuntime(); + + while (getRuntime()-stamp<4.5) { + + + + shooter.moveTurret(0.31); + + double time = getRuntime()-stamp; + + + if (time < 0.3) { + + + transfer.transferOut(); + transfer.setTransferPower(1); + } else if (time < 1) { + + spindexer.outtake3(); + + + } else if (time < 1.4) { + + + transfer.transferIn(); + } else if (time < 2.6) { + transfer.transferOut(); + + spindexer.outtake2(); + + + + } else if (time < 3.0) { + + + transfer.transferIn(); + } else if (time < 4.0) { + + transfer.transferOut(); + + spindexer.outtake1(); + + } else if (time < 4.4) { + transfer.transferIn(); + } else { + + transfer.transferOut(); + spindexer.outtake3(); + + shooter.setManualPower(0); + + } + + transfer.update(); + + shooter.update(); + + spindexer.update(); + + } + + spindexer.outtake3(); + + robot.intake.setPower(1); + + Actions.runBlocking( + traj2.build() + ); + + + Actions.runBlocking( + traj3.build() + ); + + shooter.setManualPower(1); + + + + + robot.intake.setPower(0); + + spindexer.outtake3(); + + + stamp = getRuntime(); + + shooter.setManualPower(1); + while (getRuntime()-stamp<4.5) { + + + + shooter.moveTurret(0.31); + + double time = getRuntime()-stamp; + + + if (time < 0.3) { + + + transfer.transferOut(); + transfer.setTransferPower(1); + } else if (time < 1) { + + spindexer.outtake3(); + + + } else if (time < 1.4) { + + + transfer.transferIn(); + } else if (time < 2.6) { + transfer.transferOut(); + + spindexer.outtake2(); + + + + } else if (time < 3.0) { + + + transfer.transferIn(); + } else if (time < 4.0) { + + transfer.transferOut(); + + spindexer.outtake1(); + + } else if (time < 4.4) { + transfer.transferIn(); + } else { + + transfer.transferOut(); + spindexer.outtake3(); + + shooter.setManualPower(0); + + } + + transfer.update(); + + shooter.update(); + + spindexer.update(); + + } + + spindexer.outtake3(); + robot.intake.setPower(1); + + Actions.runBlocking( + traj4.build() + ); + + + Actions.runBlocking( + traj5.build() + ); + + shooter.setManualPower(1); + + + + + robot.intake.setPower(0); + + + stamp = getRuntime(); + + shooter.setManualPower(1); + + while (getRuntime()-stamp<4.5) { + + + + shooter.moveTurret(0.31); + + double time = getRuntime()-stamp; + + + if (time < 0.3) { + + + transfer.transferOut(); + transfer.setTransferPower(1); + } else if (time < 1) { + + spindexer.outtake3(); + + + } else if (time < 1.4) { + + + transfer.transferIn(); + } else if (time < 2.6) { + transfer.transferOut(); + + spindexer.outtake2(); + + + + } else if (time < 3.0) { + + + transfer.transferIn(); + } else if (time < 4.0) { + + transfer.transferOut(); + + spindexer.outtake1(); + + } else if (time < 4.4) { + transfer.transferIn(); + } else { + + transfer.transferOut(); + spindexer.outtake3(); + + shooter.setManualPower(0); + + } + + transfer.update(); + + shooter.update(); + + spindexer.update(); + + } + + spindexer.outtake3(); + + Actions.runBlocking( + traj6.build() + ); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + TELE.addLine("finished"); + + TELE.update(); + + sleep (2000); + + + } + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java index a8d1fb9..09db0d2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java @@ -59,6 +59,8 @@ public class Red extends LinearOpMode { public void runOpMode() throws InterruptedException { + + robot = new Robot(hardwareMap); TELE = new MultipleTelemetry( telemetry, FtcDashboard.getInstance().getTelemetry() @@ -86,6 +88,8 @@ public class Red extends LinearOpMode { Intake intake = new Intake(robot); + robot.hood.setPosition(0.54); + @@ -117,7 +121,7 @@ public class Red extends LinearOpMode { TrajectoryActionBuilder traj6 = drive.actionBuilder(new Pose2d(x1, y1, h1)) - .strafeToLinearHeading(new Vector2d(x1, y1-10), h1 ); + .strafeToLinearHeading(new Vector2d(x1, y1+30), h1 ); while(opModeInInit()) { @@ -138,6 +142,8 @@ public class Red extends LinearOpMode { if (opModeIsActive()){ + robot.hood.setPosition(0.54); + transfer.setTransferPower(1); transfer.update(); 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 d3f1208..83e6530 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 @@ -17,7 +17,7 @@ public class Poses { public static double x2 = 31, y2 = 32, h2 = Math.toRadians(135); - public static double x2_b = 75, y2_b = 25, h2_b = Math.toRadians(135); + public static double x2_b = 65, y2_b = 35, h2_b = Math.toRadians(135); public static double x3 = 45, y3 = 53, h3 = Math.toRadians(135); 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 2f43a59..a992074 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 @@ -295,12 +295,7 @@ public class TeleopV1 extends LinearOpMode { offset +=0.02; } - if (gamepad2.right_stick_y < 0.7){ - pos = shooter.getAngleByDist( - shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset) - ); - } TELE.addData("hood", pos);