From 61f314d71d37fc83d46a9ec0fb6d987a00dfe932 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Wed, 31 Dec 2025 16:36:06 -0600 Subject: [PATCH] stash --- .../ftc/teamcode/teleop/TeleopV2.java | 419 ++++++++++++------ 1 file changed, 273 insertions(+), 146 deletions(-) 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 index 05cc0d2..b3f3aa0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java @@ -7,6 +7,7 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.turrDefault; import static org.firstinspires.ftc.teamcode.constants.ShooterVars.waitTransfer; import static org.firstinspires.ftc.teamcode.constants.ShooterVars.waitTransferOut; import static org.firstinspires.ftc.teamcode.tests.ShooterTest.kP; @@ -14,26 +15,20 @@ import static org.firstinspires.ftc.teamcode.tests.ShooterTest.maxStep; import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.restPos; import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.scalar; - - import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; -import com.acmerobotics.roadrunner.Vector2d; -import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; -import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam; +import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; - import java.util.ArrayList; import java.util.List; @@ -46,11 +41,20 @@ public class TeleopV2 extends LinearOpMode { public static double hoodDefaultPos = 0.5; public static double desiredTurretAngle = 180; public static double velMultiplier = 20; + public static double shootStamp2 = 0.0; + public double vel = 3000; public boolean autoVel = true; public double manualOffset = 0.0; public boolean autoHood = true; - double turrCamAdjust = 0.0; + public boolean green1 = false; + public boolean green2 = false; + public boolean green3 = false; + public double shootStamp = 0.0; + public boolean circle = false; + public boolean square = false; + public boolean triangle = false; + double autoHoodOffset = 0.0; Robot robot; MultipleTelemetry TELE; boolean intake = false; @@ -58,7 +62,6 @@ public class TeleopV2 extends LinearOpMode { double xOffset = 0.0; double yOffset = 0.0; double headingOffset = 0.0; - boolean camDetected = false; int ticker = 0; List s1G = new ArrayList<>(); List s2G = new ArrayList<>(); @@ -69,12 +72,8 @@ public class TeleopV2 extends LinearOpMode { List s1 = new ArrayList<>(); List s2 = new ArrayList<>(); List s3 = new ArrayList<>(); - boolean oddBallColor = false; - AprilTagWebcam aprilTagWebcam = new AprilTagWebcam(); - - MecanumDrive drive; double hoodOffset = 0.0; boolean shoot1 = false; @@ -84,11 +83,13 @@ public class TeleopV2 extends LinearOpMode { boolean shootC = true; boolean manualTurret = false; - public boolean green1 = false; - public boolean green2 = false; - public boolean green3 = false; + boolean outtake1 = false; + boolean outtake2 = false; + boolean outtake3 = false; + boolean overrideTurr = false; List shootOrder = new ArrayList<>(); + boolean emergency = false; private double lastEncoderRevolutions = 0.0; private double lastTimeStamp = 0.0; private double velo1, velo; @@ -97,13 +98,22 @@ public class TeleopV2 extends LinearOpMode { private double transferStamp = 0.0; private int tickerA = 1; private boolean transferIn = false; - public double shootStamp = 0.0; - public boolean circle = false; - public boolean square = false; - public boolean triangle = false; - double turretPos() { - return (scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3)); + public static double velPrediction(double distance) { + + if (distance < 30) { + return 2750; + } else if (distance > 100) { + if (distance > 160) { + return 4200; + } + return 3700; + } else { + // linear interpolation between 40->2650 and 120->3600 + double slope = (3700.0 - 2750.0) / (100.0 - 30); + return (int) Math.round(2750 + slope * (distance - 30)); + } + } @Override @@ -127,10 +137,7 @@ public class TeleopV2 extends LinearOpMode { aprilTagWebcam.init(new Robot(hardwareMap), TELE); robot.turr1.setPosition(0.4); - robot.turr2.setPosition(1-0.4); - - - + robot.turr2.setPosition(1 - 0.4); waitForStart(); if (isStopRequested()) return; @@ -138,25 +145,20 @@ public class TeleopV2 extends LinearOpMode { //DRIVETRAIN: + double y = -gamepad1.right_stick_y; // Remember, Y stick value is reversed + double x = gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing + double rx = gamepad1.left_stick_x; - double y = -gamepad1.right_stick_y; // Remember, Y stick value is reversed - double x = gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing - double rx = gamepad1.left_stick_x; - - double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); - double frontLeftPower = (y + x + rx) / denominator; - double backLeftPower = (y - x + rx) / denominator; - double frontRightPower = (y - x - rx) / denominator; - double backRightPower = (y + x - rx) / denominator; - - - - robot.frontLeft.setPower(frontLeftPower); - robot.backLeft.setPower(backLeftPower); - robot.frontRight.setPower(frontRightPower); - robot.backRight.setPower(backRightPower); - + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + double frontLeftPower = (y + x + rx) / denominator; + double backLeftPower = (y - x + rx) / denominator; + double frontRightPower = (y - x - rx) / denominator; + double backRightPower = (y + x - rx) / denominator; + robot.frontLeft.setPower(frontLeftPower); + robot.backLeft.setPower(backLeftPower); + robot.frontRight.setPower(frontRightPower); + robot.backRight.setPower(backRightPower); //INTAKE: @@ -164,13 +166,14 @@ public class TeleopV2 extends LinearOpMode { intake = !intake; reject = false; shootAll = false; + emergency = false; + overrideTurr = false; } if (gamepad1.leftBumperWasPressed()) { intake = false; - reject = true; - shootAll = false; + emergency = !emergency; } @@ -264,7 +267,6 @@ public class TeleopV2 extends LinearOpMode { s3T.add(getRuntime()); } - if (!s1.isEmpty()) { green1 = checkGreen(s1, s1T); } @@ -276,7 +278,6 @@ public class TeleopV2 extends LinearOpMode { green3 = checkGreen(s3, s3T); } - //SHOOTER: double penguin = 0; @@ -325,13 +326,15 @@ public class TeleopV2 extends LinearOpMode { robot.transfer.setPower(1); - //TURRET: double offset; - double robotX = drive.localizer.getPose().position.x - xOffset; - double robotY = drive.localizer.getPose().position.y - xOffset; + double robX = drive.localizer.getPose().position.x; + double robY = drive.localizer.getPose().position.y; + + double robotX = robX - xOffset; + double robotY = robY - yOffset; double robotHeading = drive.localizer.getPose().heading.toDouble(); double goalX = -10; @@ -352,7 +355,7 @@ public class TeleopV2 extends LinearOpMode { offset -= 360; } - double pos = 0.4; + double pos = turrDefault; TELE.addData("offset", offset); @@ -364,8 +367,14 @@ public class TeleopV2 extends LinearOpMode { pos = 0.97; } - if (manualTurret){ - pos = 0.4 + (manualOffset/100); + if (manualTurret) { + pos = turrDefault + (manualOffset / 100); + } + + if (!overrideTurr) { + + robot.turr1.setPosition(pos); + robot.turr2.setPosition(1 - pos); } if (gamepad2.dpad_right) { @@ -374,8 +383,6 @@ public class TeleopV2 extends LinearOpMode { manualOffset += 2; } - - //VELOCITY AUTOMATIC if (autoVel) { @@ -392,12 +399,10 @@ public class TeleopV2 extends LinearOpMode { } else if (gamepad2.right_stick_y > 0.5) { autoVel = false; manualVel = 2700; - } - else if (gamepad2.right_stick_x > 0.5) { + } else if (gamepad2.right_stick_x > 0.5) { autoVel = false; manualVel = 3600; - } - else if (gamepad2.right_stick_x < -0.5) { + } else if (gamepad2.right_stick_x < -0.5) { autoVel = false; manualVel = 3100; } @@ -405,105 +410,133 @@ public class TeleopV2 extends LinearOpMode { //HOOD: if (autoHood) { - robot.hood.setPosition(hoodAnglePrediction(distanceToGoal)); + robot.hood.setPosition(hoodAnglePrediction(distanceToGoal) + autoHoodOffset); } else { robot.hood.setPosition(hoodDefaultPos + hoodOffset); } if (gamepad2.dpadUpWasPressed()) { hoodOffset -= 0.03; + autoHoodOffset -= 0.02; + } else if (gamepad2.dpadDownWasPressed()) { hoodOffset += 0.03; + autoHoodOffset += 0.02; + } - if (gamepad2.left_stick_x>0.5){ + if (gamepad2.left_stick_x > 0.5) { manualTurret = false; - } else if (gamepad2.left_stick_x<-0.5){ - manualTurret = true; + } else if (gamepad2.left_stick_x < -0.5) { manualOffset = 0; - if (gamepad2.left_bumper){ - xOffset = robotX; - yOffset = robotY; - headingOffset = robotHeading; + manualTurret = false; + if (gamepad2.left_bumper) { + drive = new MecanumDrive(hardwareMap, new Pose2d(2, 0, 0)); + sleep(1200); } } - if (gamepad2.left_stick_y<-0.5){ + if (gamepad2.left_stick_y < -0.5) { autoHood = true; - } else if (gamepad2.left_stick_y>0.5){ + } else if (gamepad2.left_stick_y > 0.5) { autoHood = false; hoodOffset = 0; - if (gamepad2.left_bumper){ + if (gamepad2.left_bumper) { xOffset = robotX; yOffset = robotY; headingOffset = robotHeading; } } - //SHOOT ALL: + //SHOOT ALL:] + if (emergency) { + intake = false; + reject = true; - if (shootAll) { - - TELE.addData("100% works",shootOrder); - TELE.update(); + if (getRuntime() % 3 > 1.5) { + robot.spin1.setPosition(0); + robot.spin2.setPosition(1); + } else { + robot.spin1.setPosition(1); + robot.spin2.setPosition(0); + } + robot.transferServo.setPosition(transferServo_out); + robot.transfer.setPower(1); + } else if (shootAll) { + TELE.addData("100% works", shootOrder); intake = false; reject = false; - if (!shootOrder.isEmpty() && (getRuntime()-shootStamp < 10)) { + if (!shootOrder.isEmpty() && (getRuntime() - shootStamp < 12)) { int currentSlot = shootOrder.get(0); // Peek, do NOT remove yet boolean shootingDone = false; AprilTagDetection d20 = aprilTagWebcam.getTagById(20); AprilTagDetection d24 = aprilTagWebcam.getTagById(24); + if (d20 != null) { + overrideTurr = true; + double bearing = d20.ftcPose.bearing; + + double finalPos = robot.turr1.getPosition() - (bearing / 1300); + robot.turr1.setPosition(finalPos); + robot.turr2.setPosition(1 - finalPos); + + TELE.addData("Bear", bearing); - if (d20!=null){ - turrCamAdjust = turretPos() + d20.ftcPose.bearing; - TELE.addData("Turret Pos Target by Camera", turrCamAdjust); - camDetected = true; - } else { - camDetected = false; } - if (d24!=null){ - turrCamAdjust = turretPos() + d24.ftcPose.bearing; - TELE.addData("Turret Pos Target by Camera", turrCamAdjust); - camDetected = true; - } else { - camDetected = false; + if (d24 != null) { + overrideTurr = true; + + double bearing = d24.ftcPose.bearing; + double finalPos = robot.turr1.getPosition() - (bearing / 1300); + robot.turr1.setPosition(finalPos); + robot.turr2.setPosition(1 - finalPos); + + } + + if (!outtake1) { + outtake1 = (spindexPosEqual(spindexer_outtakeBall1)); + } + if (!outtake2) { + outtake2 = (spindexPosEqual(spindexer_outtakeBall2)); + } + if (!outtake3) { + outtake3 = (spindexPosEqual(spindexer_outtakeBall3)); } switch (currentSlot) { case 1: - shootA = shootTeleop(spindexer_outtakeBall1); + shootA = shootTeleop(spindexer_outtakeBall1, outtake1, shootStamp2); TELE.addData("shootA", shootA); - if ((getRuntime()- shootStamp) < 3* (4-shootOrder.size())){ - shootingDone = !shootA; + if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) { + shootingDone = !shootA; } else { shootingDone = true; } break; case 2: - shootB = shootTeleop(spindexer_outtakeBall2); + shootB = shootTeleop(spindexer_outtakeBall2, outtake2, shootStamp2); TELE.addData("shootB", shootB); - if ((getRuntime()- shootStamp) < 3* (4-shootOrder.size())){ + if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) { shootingDone = !shootB; } else { shootingDone = true; } break; case 3: - shootC = shootTeleop(spindexer_outtakeBall3); + shootC = shootTeleop(spindexer_outtakeBall3, outtake3, shootStamp2); TELE.addData("shootC", shootC); - if ((getRuntime()- shootStamp) < 3* (4-shootOrder.size())){ + if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) { shootingDone = !shootC; } else { shootingDone = true; @@ -511,10 +544,11 @@ public class TeleopV2 extends LinearOpMode { break; } - // Remove from the list only if shooting is complete if (shootingDone) { shootOrder.remove(0); + shootStamp2 = getRuntime(); + } } else { @@ -527,31 +561,47 @@ public class TeleopV2 extends LinearOpMode { reject = false; intake = true; shootAll = false; + outtake1 = false; + outtake2 = false; + outtake3 = false; + + overrideTurr = false; + } - } - if (gamepad2.squareWasPressed()){ + if (gamepad2.squareWasPressed()) { square = true; shootStamp = getRuntime(); + shootStamp2 = getRuntime(); + outtake1 = false; + outtake2 = false; + outtake3 = false; } - - if (gamepad2.circleWasPressed()){ + if (gamepad2.circleWasPressed()) { circle = true; shootStamp = getRuntime(); + shootStamp2 = getRuntime(); + + outtake1 = false; + outtake2 = false; + outtake3 = false; } - - if (gamepad2.triangleWasPressed()){ + if (gamepad2.triangleWasPressed()) { triangle = true; shootStamp = getRuntime(); + shootStamp2 = getRuntime(); + + outtake1 = false; + outtake2 = false; + outtake3 = false; } - if (square || circle || triangle) { // Count green balls @@ -583,15 +633,6 @@ public class TeleopV2 extends LinearOpMode { square = false; triangle = false; - - } - - if (camDetected){ - robot.turr1.setPosition(turrCamAdjust); - robot.turr2.setPosition(1 - turrCamAdjust); - } else { - robot.turr1.setPosition(pos); - robot.turr2.setPosition(1 - pos); } // Right bumper shoots all balls fastest, ignoring colors @@ -599,20 +640,80 @@ public class TeleopV2 extends LinearOpMode { shootOrder.clear(); shootStamp = getRuntime(); + outtake1 = false; + outtake2 = false; + outtake3 = false; + // Fastest order (example: slot 3 → 2 → 1) - shootOrder.add(3); - shootOrder.add(2); - shootOrder.add(1); + + if (ballIn(3)) { + shootOrder.add(3); + + } + + if (ballIn(2)) { + shootOrder.add(2); + + } + + if (ballIn(1)) { + shootOrder.add(1); + + } + + if (!shootOrder.contains(3)) { + + shootOrder.add(3); + } + + if (!shootOrder.contains(2)) { + + shootOrder.add(2); + } + + if (!shootOrder.contains(1)) { + + shootOrder.add(1); + } + shootAll = true; shootPos = drive.localizer.getPose(); + } +// // Right bumper shoots all balls fastest, ignoring colors +// if (gamepad2.leftBumperWasPressed()) { +// shootOrder.clear(); +// shootStamp = getRuntime(); +// +// outtake1 = false; +// outtake2 = false; +// outtake3 = false; +// +// // Fastest order (example: slot 3 → 2 → 1) +// +// if (ballIn(3)) { +// shootOrder.add(3); +// } +// +// if (ballIn(2)) { +// shootOrder.add(2); +// } +// if (ballIn(1)) { +// shootOrder.add(1); +// } +// shootAll = true; +// shootPos = drive.localizer.getPose(); +// +// } +// + if (gamepad2.crossWasPressed()) { + emergency = true; } - if (gamepad2.x){ - shootAll = false; - + if (gamepad2.leftBumperWasPressed()) { + emergency = false; } //MISC: @@ -623,9 +724,9 @@ public class TeleopV2 extends LinearOpMode { hub.clearBulkCache(); } - TELE.addData("Spin1Green", green1); - TELE.addData("Spin2Green", green2); - TELE.addData("Spin3Green", green3); + TELE.addData("Spin1Green", green1 + ": " + ballIn(1)); + TELE.addData("Spin2Green", green2 + ": " + ballIn(2)); + TELE.addData("Spin3Green", green3 + ": " + ballIn(3)); TELE.addData("pose", drive.localizer.getPose()); TELE.addData("heading", drive.localizer.getPose().heading.toDouble()); @@ -638,7 +739,6 @@ public class TeleopV2 extends LinearOpMode { aprilTagWebcam.update(); - TELE.update(); ticker++; @@ -672,13 +772,13 @@ public class TeleopV2 extends LinearOpMode { scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) < spindexer + 0.01); } - public boolean shootTeleop(double spindexer) { + public boolean shootTeleop(double spindexer, boolean spinOk, double stamp) { // Set spin positions robot.spin1.setPosition(spindexer); robot.spin2.setPosition(1 - spindexer); // Check if spindexer has reached the target position - if (spindexPosEqual(spindexer)) { + if (spinOk || getRuntime() - stamp > 1.5) { if (tickerA == 1) { transferStamp = getRuntime(); tickerA++; @@ -714,9 +814,22 @@ public class TeleopV2 extends LinearOpMode { } } - public double hoodAnglePrediction(double distance) { + public double hoodAnglePrediction(double x) { + if (x < 34) { + double L = 1.04471; + double U = 0.711929; + double Q = 120.02263; + double B = 0.780982; + double M = 20.61191; + double v = 10.40506; - return 0.4; + double inner = 1 + Q * Math.exp(-B * (x - M)); + return L + (U - L) / Math.pow(inner, 1.0 / v); + + } else { + // x >= 34 + return 1.94372 * Math.exp(-0.0528731 * x) + 0.39; + } } void addOddThenRest(List order, boolean oddColor) { @@ -728,8 +841,6 @@ public class TeleopV2 extends LinearOpMode { TELE.addData("oddBall", oddColor); shootAll = true; - - } void addOddInMiddle(List order, boolean oddColor) { @@ -768,6 +879,7 @@ public class TeleopV2 extends LinearOpMode { shootAll = true; } + void addOddLast(List order, boolean oddColor) { // Odd ball last for (int i = 1; i <= 3; i++) if (getBallColor(i) != oddColor) order.add(i); @@ -777,35 +889,50 @@ public class TeleopV2 extends LinearOpMode { TELE.addData("oddBall", oddColor); shootAll = true; - } // Returns color of ball in slot i (1-based) boolean getBallColor(int slot) { - switch(slot) { - case 1: return green1; - case 2: return green2; - case 3: return green3; + switch (slot) { + case 1: + return green1; + case 2: + return green2; + case 3: + return green3; } return false; // default } - public static double velPrediction(double distance) { + boolean ballIn(int slot) { + switch (slot) { + case 1: - if (distance < 30) { - return 2750; - } else if (distance > 100) { - if (distance > 160) { - return 4200; - } - return 3700; - } else { - // linear interpolation between 40->2650 and 120->3600 - double slope = (3700.0 - 2750.0) / (100.0 - 30); - return (int) Math.round(2750 + slope * (distance - 30)); + if (!s1T.isEmpty()) { + + return !(s1T.get(s1T.size() - 1) < (getRuntime()) - 3); + } + + case 2: + + if (!s2T.isEmpty()) { + + return !(s2T.get(s2T.size() - 1) < (getRuntime()) - 3); + } + + case 3: + + if (!s3T.isEmpty()) { + + return !(s3T.get(s3T.size() - 1) < (getRuntime()) - 3); + + } } - + return true; // default } + public double turretPos() { + return (scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3)); + } }