From 5e8727ebaae72c7b04cd3b7aa45604aae9d1c69e Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sun, 11 Jan 2026 17:46:08 -0600 Subject: [PATCH] stash --- .../ftc/teamcode/autonomous/Red_V3.java | 747 ++++++++++++++++++ 1 file changed, 747 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V3.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V3.java new file mode 100644 index 0000000..f0da3d2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V3.java @@ -0,0 +1,747 @@ +package org.firstinspires.ftc.teamcode.autonomous; + +import static org.firstinspires.ftc.teamcode.constants.Poses.*; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; +import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.ParallelAction; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.SequentialAction; +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.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; +import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam; +import org.firstinspires.ftc.teamcode.utils.Flywheel; +import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utils.Servos; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; + +@Config +@Autonomous(preselectTeleOp = "TeleopV3") +public class Red_V3 extends LinearOpMode { + Robot robot; + MultipleTelemetry TELE; + MecanumDrive drive; + Flywheel flywheel; + Servos servo; + + double velo = 0.0; + public static double intake1Time = 2.9; + public static double intake2Time = 2.9; + public static double colorDetect = 3.0; + boolean gpp = false; + boolean pgp = false; + boolean ppg = false; + double powPID = 0.0; + double bearing = 0.0; + int b1 = 0; // 0 = no ball, 1 = green, 2 = purple + int b2 = 0;// 0 = no ball, 1 = green, 2 = purple + int b3 = 0;// 0 = no ball, 1 = green, 2 = purple + + public Action initShooter(int vel) { + return new Action() { + double ticker = 0.0; + double stamp = 0.0; + boolean steady = false; + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if (ticker == 0) { + stamp = getRuntime(); + } + ticker++; + + powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition()); + velo = flywheel.getVelo(); + robot.shooter1.setPower(powPID); + robot.shooter2.setPower(powPID); + robot.transfer.setPower(1); + + TELE.addData("Velocity", velo); + TELE.update(); + if (vel < velo && getRuntime() - stamp > 3.0 && !steady){ + steady = true; + stamp = getRuntime(); + return true; + } else if (steady && getRuntime() - stamp > 1.5){ + TELE.addData("Velocity", velo); + TELE.addLine("finished init"); + TELE.update(); + return false; + } else { + return true; + } + } + }; + } + + public Action steadyShooter(int vel, boolean last) { + return new Action() { + double stamp = 0.0; + boolean steady = false; + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition()); + velo = flywheel.getVelo(); + steady = flywheel.getSteady(); + robot.shooter1.setPower(powPID); + robot.shooter2.setPower(powPID); + robot.transfer.setPower(1); + + TELE.addData("Velocity", velo); + TELE.update(); + detectTag(); + + if (last && !steady){ + stamp = getRuntime(); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + return false; + } else if (steady) { + stamp = getRuntime(); + return true; + } else { + return true; + } + } + }; + } + + public Action Obelisk() { + return new Action() { + double stamp = getRuntime(); + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if (ticker == 0) { + stamp = getRuntime(); + } + ticker++; + + if (aprilTag.getTagById(21) != null) { + gpp = true; + } else if (aprilTag.getTagById(22) != null) { + pgp = true; + } else if (aprilTag.getTagById(23) != null) { + ppg = true; + } + aprilTag.update(); + + TELE.addData("Velocity", velo); + TELE.addData("21", gpp); + TELE.addData("22", pgp); + TELE.addData("23", ppg); + TELE.update(); + + if (gpp || pgp || ppg){ + double turretPID = servo.setTurrPos(turret_red); + robot.turr1.setPower(turretPID); + robot.turr2.setPower(-turretPID); + return !servo.turretEqual(turret_red); + } else { + return true; + } + } + }; + } + + public Action spindex (double spindexer, int vel){ + return new Action() { + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + + powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition()); + velo = flywheel.getVelo(); + robot.shooter1.setPower(powPID); + robot.shooter2.setPower(powPID); + robot.spin1.setPower(spindexer); + robot.spin2.setPower(1-spindexer); + TELE.addData("Velocity", velo); + TELE.addLine("spindex"); + TELE.update(); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + return !servo.spinEqual(spindexer); + } + }; + } + + public Action Shoot(int vel) { + return new Action() { + double transferStamp = 0.0; + int ticker = 1; + boolean transferIn = false; + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + TELE.addData("Velocity", velo); + TELE.addLine("shooting"); + TELE.update(); + + powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition()); + velo = flywheel.getVelo(); + robot.shooter1.setPower(powPID); + robot.shooter2.setPower(powPID); + + drive.updatePoseEstimate(); + detectTag(); + + teleStart = drive.localizer.getPose(); + + if (ticker == 1) { + transferStamp = getRuntime(); + ticker++; + } + if (getRuntime() - transferStamp > waitTransfer && !transferIn) { + robot.transferServo.setPosition(transferServo_in); + TELE.addData("Velocity", velo); + TELE.addData("ticker", ticker); + TELE.update(); + transferIn = true; + return true; + } else if (getRuntime() - transferStamp > waitTransfer+waitTransferOut && transferIn){ + robot.transferServo.setPosition(transferServo_out); + TELE.addData("Velocity", velo); + TELE.addLine("shot once"); + TELE.update(); + return false; + } else { + return true; + } + + } + }; + } + + public Action intake(double intakeTime) { + return new Action() { + double position = 0.0; + double stamp = 0.0; + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if (ticker == 0) { + stamp = getRuntime(); + } + ticker++; + + double s1D = robot.color1.getDistance(DistanceUnit.MM); + double s2D = robot.color2.getDistance(DistanceUnit.MM); + double s3D = robot.color3.getDistance(DistanceUnit.MM); + + if ((getRuntime() % 0.3) > 0.15) { + position = spindexer_intakePos1 + 0.02; + } else { + position = spindexer_intakePos1 - 0.02; + } + robot.spin1.setPower(position); + robot.spin2.setPower(1 - position); + + TELE.addData("Velocity", velo); + 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) { + return false; + } else { + return true; + } + } + }; + } + + 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; + int ticker = 0; + double position = 0.0; + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if (ticker == 0) { + stamp = getRuntime(); + } + ticker++; + + if ((getRuntime() % 0.3) > 0.15) { + position = spindexer_intakePos1 + 0.02; + } else { + position = spindexer_intakePos1 - 0.02; + } + robot.spin1.setPower(position); + robot.spin2.setPower(1 - position); + + double s1D = robot.color1.getDistance(DistanceUnit.MM); + 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; + double red = robot.color1.getNormalizedColors().red; + double blue = robot.color1.getNormalizedColors().blue; + + double gP = green / (green + red + blue); + + + + if (gP >= 0.4) { + b1 = 2; + } else { + b1 = 1; + } + } + + if (s2D < 40) { + + double green = robot.color2.getNormalizedColors().green; + double red = robot.color2.getNormalizedColors().red; + double blue = robot.color2.getNormalizedColors().blue; + + double gP = green / (green + red + blue); + + if (gP >= 0.4) { + b2 = 2; + } else { + b2 = 1; + } + } + + if (s3D < 30) { + + double green = robot.color3.getNormalizedColors().green; + double red = robot.color3.getNormalizedColors().red; + double blue = robot.color3.getNormalizedColors().blue; + + double gP = green / (green + red + blue); + + if (gP >= 0.4) { + b3 = 2; + } else { + b3 = 1; + } + } + + TELE.addData("Velocity", velo); + TELE.addLine("Detecting"); + TELE.addData("Distance 1", s1D); + TELE.addData("Distance 2", s2D); + TELE.addData("Distance 3", s3D); + TELE.addData("B1", b1); + TELE.addData("B2", b2); + TELE.addData("B3", b3); + TELE.update(); + + return (b1 + b2 + b3 < 4) && !(getRuntime() - stamp > colorDetect); + } + }; + } + + @Override + public void runOpMode() throws InterruptedException { + + robot = new Robot(hardwareMap); + + flywheel = new Flywheel(); + + TELE = new MultipleTelemetry( + telemetry, FtcDashboard.getInstance().getTelemetry() + ); + + drive = new MecanumDrive(hardwareMap, new Pose2d( + 0, 0, 0 + )); + + aprilTag = new AprilTagWebcam(); + + TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) + .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); + + TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1)) + .strafeToLinearHeading(new Vector2d(rx2a, ry2a), rh2a) + .strafeToLinearHeading(new Vector2d(rx2b, ry2b), rh2b); + + TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(rx2b, ry2b, rh2b)) + .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); + + TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1)) + + .strafeToLinearHeading(new Vector2d(rx3a, ry3a), rh3a) + + .strafeToLinearHeading(new Vector2d(rx3b, ry3b), rh3b); + + TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b)) + .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); + + aprilTag.init(robot, TELE); + + while (opModeInInit()) { + + if (gamepad2.dpadUpWasPressed()) { + hoodAuto-= 0.01; + } + + if (gamepad2.dpadDownWasPressed()) { + hoodAuto += 0.01; + } + + robot.hood.setPosition(hoodAuto); + + robot.transferServo.setPosition(transferServo_out); + + robot.spin1.setPower(spindexer_intakePos1); + robot.spin2.setPower(1 - spindexer_intakePos1); + + aprilTag.update(); + TELE.addData("Velocity", velo); + TELE.addData("Turret Pos", servo.getTurrPos()); + TELE.update(); + } + + + waitForStart(); + + if (isStopRequested()) return; + + if (opModeIsActive()) { + + robot.hood.setPosition(hoodAuto); + + Actions.runBlocking( + new ParallelAction( + shoot0.build(), + initShooter(AUTO_CLOSE_VEL), + Obelisk() + ) + ); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition()); + velo = flywheel.getVelo(); + robot.shooter1.setPower(powPID); + robot.shooter2.setPower(powPID); + + shootingSequence(); + + robot.hood.setPosition(hoodAuto); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + Actions.runBlocking( + new ParallelAction( + pickup1.build(), + intake(intake1Time) + ) + ); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + Actions.runBlocking( + new ParallelAction( + shoot1.build(), + ColorDetect(), + steadyShooter(AUTO_CLOSE_VEL, true), + intakeReject() + ) + ); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, 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( + pickup2.build(), + intake(intake2Time) + ) + ); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + Actions.runBlocking( + new ParallelAction( + shoot2.build(), + ColorDetect(), + steadyShooter(AUTO_CLOSE_VEL, true), + intakeReject() + + ) + ); + + powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition()); + velo = flywheel.getVelo(); + robot.shooter1.setPower(powPID); + robot.shooter2.setPower(powPID); + + shootingSequence(); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + TELE.addData("Velocity", velo); + TELE.addLine("finished"); + TELE.update(); + + sleep(2000); + + } + + } + + public void detectTag(){ + AprilTagDetection d20 = aprilTag.getTagById(20); + AprilTagDetection d24 = aprilTag.getTagById(24); + + if (d20 != null) { + bearing = d20.ftcPose.bearing; + TELE.addData("Bear", bearing); + } + + if (d24 != null) { + bearing = d24.ftcPose.bearing; + TELE.addData("Bear", bearing); + } + double turretPos = servo.getTurrPos() - (bearing / 1300); + double turretPID = servo.setTurrPos(turretPos); + robot.turr1.setPower(turretPID); + robot.turr2.setPower(-turretPID); + } + + public void shootingSequence() { + TELE.addData("Velocity", velo); + if (gpp) { + if (b1 + b2 + b3 == 4) { + if (b1 == 2 && b2 - b3 == 0) { + sequence1(); + TELE.addLine("sequence1"); + } else if (b2 == 2 && b1 - b3 == 0) { + sequence3(); + TELE.addLine("sequence3"); + } else if (b3 == 2 && b1 - b2 == 0) { + sequence6(); + TELE.addLine("sequence6"); + } else { + sequence1(); + TELE.addLine("sequence1"); + } + } else if (b1 + b2 + b3 >= 5) { + if (b1 == 2) { + sequence1(); + TELE.addLine("sequence1"); + } else if (b2 == 2) { + sequence3(); + TELE.addLine("sequence3"); + } else if (b3 == 2) { + sequence6(); + TELE.addLine("sequence6"); + } + } else { + sequence1(); + TELE.addLine("sequence1"); + } + } else if (pgp) { + if (b1 + b2 + b3 == 4) { + if (b1 == 2 && b2 - b3 == 0) { + sequence3(); + TELE.addLine("sequence3"); + } else if (b2 == 2 && b1 - b3 == 0) { + sequence1(); + TELE.addLine("sequence1"); + } else if (b3 == 2 && b1 - b2 == 0) { + sequence4(); + TELE.addLine("sequence4"); + } else { + sequence1(); + TELE.addLine("sequence1"); + } + } else if (b1 + b2 + b3 >= 5) { + if (b1 == 2) { + sequence3(); + TELE.addLine("sequence3"); + } else if (b2 == 2) { + sequence1(); + TELE.addLine("sequence1"); + } else if (b3 == 2) { + sequence4(); + TELE.addLine("sequence4"); + } + } else { + sequence3(); + TELE.addLine("sequence3"); + } + } else if (ppg) { + if (b1 + b2 + b3 == 4) { + if (b1 == 2 && b2 - b3 == 0) { + sequence6(); + TELE.addLine("sequence6"); + } else if (b2 == 2 && b1 - b3 == 0) { + sequence5(); + TELE.addLine("sequence5"); + } else if (b3 == 2 && b1 - b2 == 0) { + sequence1(); + TELE.addLine("sequence1"); + } else { + sequence1(); + TELE.addLine("sequence1"); + } + } else if (b1 + b2 + b3 >= 5) { + if (b1 == 2) { + sequence6(); + TELE.addLine("sequence6"); + } else if (b2 == 2) { + sequence5(); + TELE.addLine("sequence5"); + } else if (b3 == 2) { + sequence1(); + TELE.addLine("sequence1"); + } + } else { + sequence6(); + TELE.addLine("sequence6"); + } + } else { + sequence1(); + TELE.addLine("sequence1"); + } + TELE.update(); + } + + public void sequence1() { + Actions.runBlocking( + new SequentialAction( + spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL) + ) + ); + } + + public void sequence2() { + Actions.runBlocking( + new SequentialAction( + spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL) + ) + ); + } + + public void sequence3() { + Actions.runBlocking( + new SequentialAction( + spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), + + Shoot(AUTO_CLOSE_VEL) + ) + ); + } + + public void sequence4() { + Actions.runBlocking( + new SequentialAction( + spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL) + ) + ); + } + + public void sequence5() { + Actions.runBlocking( + new SequentialAction( + spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL) + ) + ); + } + + public void sequence6() { + Actions.runBlocking( + new SequentialAction( + spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL) + ) + ); + } +} \ No newline at end of file