From b10a723f37a795ca46e223c3713f5c6079c7a2fe Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Wed, 31 Dec 2025 16:51:27 -0600 Subject: [PATCH] stash --- .../ftc/teamcode/autonomous/Blue_V2.java | 213 +++++++++++------- 1 file changed, 129 insertions(+), 84 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java index a1007cb..52e3c23 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java @@ -1,7 +1,9 @@ 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.ServoPositions.hoodAuto; import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*; import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.*; @@ -11,23 +13,24 @@ 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 com.acmerobotics.roadrunner.Action; -import com.acmerobotics.roadrunner.ftc.Actions; 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; @Config -@Autonomous +@Autonomous(preselectTeleOp = "TeleopV2") public class Blue_V2 extends LinearOpMode { Robot robot; @@ -38,9 +41,13 @@ public class Blue_V2 extends LinearOpMode { AprilTagWebcam aprilTag; - public static double intake1Time = 4.0; + Flywheel flywheel; - public static double intake2Time = 5.5; + double velo = 0.0; + double targetVelocity = 0.0; + public static double intake1Time = 2.9; + + public static double intake2Time = 2.9; public static double colorDetect = 3.0; @@ -59,6 +66,7 @@ public class Blue_V2 extends LinearOpMode { int b3 = 0;// 0 = no ball, 1 = green, 2 = purple boolean spindexPosEqual(double spindexer) { + TELE.addData("Velocity", velo); TELE.addLine("spindex equal"); TELE.update(); return (scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) > spindexer - 0.01 && @@ -67,7 +75,6 @@ public class Blue_V2 extends LinearOpMode { public Action initShooter(int vel) { return new Action() { - double velo = 0.0; double initPos = 0.0; double stamp = 0.0; double stamp1 = 0.0; @@ -80,34 +87,15 @@ public class Blue_V2 extends LinearOpMode { stamp2 = getRuntime(); } + targetVelocity = (double) vel; ticker++; if (ticker % 16 == 0) { - currentPos = (double) robot.shooter1.getCurrentPosition() / 2048; stamp = getRuntime(); - velo = -60 * ((currentPos - initPos) / (stamp - stamp1)); - initPos = currentPos; stamp1 = stamp; } - if (vel - velo > 500) { - powPID = 1.0; - } else { - double feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18; - - // --- PROPORTIONAL CORRECTION --- - double error = vel - velo; - double correction = kP * error; - - // limit how fast power changes (prevents oscillation) - correction = Math.max(-maxStep, Math.min(maxStep, correction)); - - // --- FINAL MOTOR POWER --- - powPID = feed + correction; - - // clamp to allowed range - powPID = Math.max(0, Math.min(1, powPID)); - } - + powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition()); + velo = flywheel.getVelo(); robot.shooter1.setPower(powPID); robot.shooter2.setPower(powPID); robot.transfer.setPower(1); @@ -119,6 +107,7 @@ public class Blue_V2 extends LinearOpMode { stamp2 = getRuntime(); return true; } else if (steady && getRuntime() - stamp2 > 1.5){ + TELE.addData("Velocity", velo); TELE.addLine("finished init"); TELE.update(); return false; @@ -131,49 +120,12 @@ public class Blue_V2 extends LinearOpMode { public Action steadyShooter(int vel, boolean last) { return new Action() { - double velo = 0.0; - double initPos = 0.0; double stamp = 0.0; - double stamp1 = 0.0; - double ticker = 0.0; - double stamp2 = 0.0; - double currentPos = 0.0; boolean steady = false; public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp2 = getRuntime(); - } - - ticker++; - if (ticker % 8 == 0) { - currentPos = (double) robot.shooter1.getCurrentPosition() / 2048; - stamp = getRuntime(); - velo = -60 * ((currentPos - initPos) / (stamp - stamp1)); - initPos = currentPos; - stamp1 = stamp; - } - - if (vel - velo > 500) { - powPID = 1.0; - } else if (velo - vel > 500){ - powPID = 0.0; - } else { - double feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18; - - // --- PROPORTIONAL CORRECTION --- - double error = vel - velo; - double correction = kP * error; - - // limit how fast power changes (prevents oscillation) - correction = Math.max(-maxStep, Math.min(maxStep, correction)); - - // --- FINAL MOTOR POWER --- - powPID = feed + correction; - - // clamp to allowed range - powPID = Math.max(0, Math.min(1, powPID)); - } - + powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition()); + velo = flywheel.getVelo(); + steady = flywheel.getSteady(); robot.shooter1.setPower(powPID); robot.shooter2.setPower(powPID); robot.transfer.setPower(1); @@ -181,11 +133,14 @@ public class Blue_V2 extends LinearOpMode { TELE.addData("Velocity", velo); TELE.update(); - if (Math.abs(vel - velo) < 100 && last && !steady){ + + if (last && !steady){ stamp = getRuntime(); - steady = true; + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); return false; - } else if (steady && getRuntime() - stamp > 1.0) { + } else if (steady) { stamp = getRuntime(); return true; } else { @@ -216,6 +171,7 @@ public class Blue_V2 extends LinearOpMode { } aprilTag.update(); + TELE.addData("Velocity", velo); TELE.addData("21", gpp); TELE.addData("22", pgp); TELE.addData("23", ppg); @@ -232,11 +188,43 @@ public class Blue_V2 extends LinearOpMode { }; } + public Action intakeReject() { + return new Action() { + double stamp = 0.0; + int ticker = 0; + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + + double position = 0.0; + + if ((getRuntime() % 0.3) > 0.15) { + position = spindexer_intakePos1 + 0.02; + } else { + position = spindexer_intakePos1 - 0.02; + } + + robot.spin1.setPosition(position); + robot.spin2.setPosition(1 - position); + + if (ticker == 0) { + stamp = getRuntime(); + } + ticker++; + + if (getRuntime() - stamp < 0.3){ + return true; + }else { + robot.intake.setPower(0); + return false; + } + } + }; + } + public Action spindex (double spindexer, double vel){ return new Action() { double currentPos = 0.0; double stamp = 0.0; - double velo = 0.0; double initPos = 0.0; double stamp1 = 0.0; int ticker = 0; @@ -272,12 +260,20 @@ public class Blue_V2 extends LinearOpMode { powPID = Math.max(0, Math.min(1, powPID)); } + powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition()); + velo = flywheel.getVelo(); robot.shooter1.setPower(powPID); robot.shooter2.setPower(powPID); robot.spin1.setPosition(spindexer); robot.spin2.setPosition(1-spindexer); + TELE.addData("Velocity", velo); TELE.addLine("spindex"); TELE.update(); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + return !spindexPosEqual(spindexer); } }; @@ -290,13 +286,14 @@ public class Blue_V2 extends LinearOpMode { boolean transferIn = false; double currentPos = 0.0; double stamp = 0.0; - double velo = 0.0; double initPos = 0.0; double stamp1 = 0.0; @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { + TELE.addData("Velocity", velo); TELE.addLine("shooting"); TELE.update(); + if (ticker % 8 == 0) { currentPos = (double) robot.shooter1.getCurrentPosition() / 2048; stamp = getRuntime(); @@ -326,9 +323,15 @@ public class Blue_V2 extends LinearOpMode { powPID = Math.max(0, Math.min(1, powPID)); } + powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition()); + velo = flywheel.getVelo(); robot.shooter1.setPower(powPID); robot.shooter2.setPower(powPID); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + if (ticker == 1) { transferStamp = getRuntime(); @@ -336,12 +339,14 @@ public class Blue_V2 extends LinearOpMode { } 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; @@ -378,12 +383,16 @@ public class Blue_V2 extends LinearOpMode { robot.spin1.setPosition(position); robot.spin2.setPosition(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) { - robot.intake.setPower(0); return false; } else { return true; @@ -416,6 +425,10 @@ public class Blue_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; @@ -463,6 +476,7 @@ public class Blue_V2 extends LinearOpMode { } } + TELE.addData("Velocity", velo); TELE.addLine("Detecting"); TELE.addData("Distance 1", s1D); TELE.addData("Distance 2", s2D); @@ -482,6 +496,8 @@ public class Blue_V2 extends LinearOpMode { robot = new Robot(hardwareMap); + flywheel = new Flywheel(); + TELE = new MultipleTelemetry( telemetry, FtcDashboard.getInstance().getTelemetry() ); @@ -495,7 +511,7 @@ public class Blue_V2 extends LinearOpMode { TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); - TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(rx1, by1, bh1)) + TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(bx1, by1, bh1)) .strafeToLinearHeading(new Vector2d(bx2a, by2a), bh2a) .strafeToLinearHeading(new Vector2d(bx2b, by2b), bh2b); @@ -516,14 +532,14 @@ public class Blue_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_detectBlue); robot.turr2.setPosition(1 - turret_detectBlue); @@ -534,7 +550,7 @@ public class Blue_V2 extends LinearOpMode { robot.spin2.setPosition(1 - spindexer_intakePos1); aprilTag.update(); - + TELE.addData("Velocity", velo); TELE.update(); } @@ -544,7 +560,7 @@ public class Blue_V2 extends LinearOpMode { if (opModeIsActive()) { - robot.hood.setPosition(hoodStart); + robot.hood.setPosition(hoodAuto); Actions.runBlocking( new ParallelAction( @@ -553,13 +569,22 @@ public class Blue_V2 extends LinearOpMode { Obelisk() ) ); + 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(); - robot.hood.setPosition(hoodDefault); + robot.hood.setPosition(hoodAuto); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); Actions.runBlocking( new ParallelAction( @@ -567,19 +592,32 @@ public class Blue_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( @@ -587,15 +625,21 @@ public class Blue_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() ) ); + powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition()); + velo = flywheel.getVelo(); robot.shooter1.setPower(powPID); robot.shooter2.setPower(powPID); @@ -605,8 +649,8 @@ public class Blue_V2 extends LinearOpMode { teleStart = drive.localizer.getPose(); + TELE.addData("Velocity", velo); TELE.addLine("finished"); - TELE.update(); sleep(2000); @@ -616,6 +660,7 @@ public class Blue_V2 extends LinearOpMode { } public void shootingSequence() { + TELE.addData("Velocity", velo); if (gpp) { if (b1 + b2 + b3 == 4) { if (b1 == 2 && b2 - b3 == 0) {