From 2f5d4638ecf09a27a0a7ad5e8322ff278312b24a Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Fri, 5 Dec 2025 21:57:23 -0600 Subject: [PATCH] Add coloooor sensooooooer!!!! --- .../ftc/teamcode/constants/Poses.java | 4 +- .../teamcode/constants/ServoPositions.java | 5 - .../ftc/teamcode/teleop/TeleopV2.java | 91 ++++++++++++++++--- .../utils/PositionalServoProgrammer.java | 2 +- 4 files changed, 83 insertions(+), 19 deletions(-) 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 e6c93d8..c442d1b 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 @@ -26,7 +26,7 @@ public class Poses { - public static double rx1 = 50, ry1 = -4, rh1 = 0; + public static double rx1 = 46, ry1 = -4, rh1 = 0; public static double rx2a = 45, ry2a = 5, rh2a = Math.toRadians(140); @@ -36,7 +36,7 @@ public class Poses { public static double rx3b = 34, ry3b = 58, rh3b = Math.toRadians(140); - public static double bx1 = 50, by1 = 4, bh1 = 0; + public static double bx1 = 46, by1 = 4, bh1 = 0; public static double bx2a = 45, by2a = -5, bh2a = Math.toRadians(-140); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index 0719e5f..4d55956 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java @@ -1,6 +1,5 @@ package org.firstinspires.ftc.teamcode.constants; - import com.acmerobotics.dashboard.config.Config; @Config @@ -21,15 +20,12 @@ public class ServoPositions { public static double transferServo_in = 0.38; - - public static double turret_range = 0.9; public static double hoodDefault = 0.6; public static double hoodAuto = 0.59; - public static double hoodHigh = 0.21; public static double hoodLow = 1.0; @@ -41,5 +37,4 @@ public class ServoPositions { public static double turret_detectBlue = 0.6; - } 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 e535c27..3890cfb 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 @@ -40,6 +40,7 @@ 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; @@ -51,6 +52,7 @@ public class TeleopV2 extends LinearOpMode { public boolean circle = false; public boolean square = false; public boolean triangle = false; + double autoHoodOffset = 0.0; Robot robot; MultipleTelemetry TELE; boolean intake = false; @@ -399,15 +401,19 @@ 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) { @@ -415,6 +421,7 @@ public class TeleopV2 extends LinearOpMode { } else if (gamepad2.left_stick_x < -0.5) { manualTurret = true; manualOffset = 0; + autoHoodOffset = 0; if (gamepad2.left_bumper) { xOffset = robotX; yOffset = robotY; @@ -461,7 +468,6 @@ public class TeleopV2 extends LinearOpMode { } else if (shootAll) { TELE.addData("100% works", shootOrder); - TELE.update(); intake = false; reject = false; @@ -493,7 +499,7 @@ public class TeleopV2 extends LinearOpMode { switch (currentSlot) { case 1: - shootA = shootTeleop(spindexer_outtakeBall1, outtake1); + shootA = shootTeleop(spindexer_outtakeBall1, outtake1, shootStamp2); TELE.addData("shootA", shootA); if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) { @@ -503,7 +509,7 @@ public class TeleopV2 extends LinearOpMode { } break; case 2: - shootB = shootTeleop(spindexer_outtakeBall2, outtake2); + shootB = shootTeleop(spindexer_outtakeBall2, outtake2, shootStamp2); TELE.addData("shootB", shootB); if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) { shootingDone = !shootB; @@ -512,7 +518,7 @@ public class TeleopV2 extends LinearOpMode { } break; case 3: - shootC = shootTeleop(spindexer_outtakeBall3, outtake3); + shootC = shootTeleop(spindexer_outtakeBall3, outtake3, shootStamp2); TELE.addData("shootC", shootC); if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) { shootingDone = !shootC; @@ -525,6 +531,8 @@ public class TeleopV2 extends LinearOpMode { // Remove from the list only if shooting is complete if (shootingDone) { shootOrder.remove(0); + shootStamp2 = getRuntime(); + } } else { @@ -548,6 +556,7 @@ public class TeleopV2 extends LinearOpMode { if (gamepad2.squareWasPressed()) { square = true; shootStamp = getRuntime(); + shootStamp2 = getRuntime(); outtake1 = false; outtake2 = false; outtake3 = false; @@ -556,6 +565,8 @@ public class TeleopV2 extends LinearOpMode { if (gamepad2.circleWasPressed()) { circle = true; shootStamp = getRuntime(); + shootStamp2 = getRuntime(); + outtake1 = false; outtake2 = false; outtake3 = false; @@ -565,6 +576,8 @@ public class TeleopV2 extends LinearOpMode { if (gamepad2.triangleWasPressed()) { triangle = true; shootStamp = getRuntime(); + shootStamp2 = getRuntime(); + outtake1 = false; outtake2 = false; outtake3 = false; @@ -614,9 +627,39 @@ public class TeleopV2 extends LinearOpMode { outtake3 = false; // Fastest order (example: slot 3 → 2 → 1) + shootOrder.add(3); + shootOrder.add(2); + 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(); @@ -635,9 +678,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()); @@ -683,13 +726,13 @@ public class TeleopV2 extends LinearOpMode { scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) < spindexer + 0.01); } - public boolean shootTeleop(double spindexer, boolean spinOk) { + 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 (spinOk) { + if (spinOk || getRuntime() - stamp > 1.5) { if (tickerA == 1) { transferStamp = getRuntime(); tickerA++; @@ -743,7 +786,6 @@ public class TeleopV2 extends LinearOpMode { } } - void addOddThenRest(List order, boolean oddColor) { // Odd ball first for (int i = 1; i <= 3; i++) if (getBallColor(i) == oddColor) order.add(i); @@ -816,4 +858,31 @@ public class TeleopV2 extends LinearOpMode { return false; // default } + boolean ballIn(int slot) { + switch (slot) { + case 1: + + 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 + } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java index 0946902..64c001e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java @@ -17,7 +17,7 @@ public class PositionalServoProgrammer extends LinearOpMode { public static double hoodPos = 0.501; public static double scalar = 1.112; - public static double restPos = 0.158; + public static double restPos = 0.15; @Override public void runOpMode() throws InterruptedException { robot = new Robot(hardwareMap);