From 70ad084ab1cf1d64d53f7a1777300f32f4f9fd5e Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sun, 11 Jan 2026 17:19:54 -0600 Subject: [PATCH 1/9] new teleop draft --- .../ftc/teamcode/constants/Color.java | 1 + .../ftc/teamcode/teleop/TeleopV3.java | 803 ++++++++++++++++++ .../ftc/teamcode/tests/IntakeTest.java | 4 +- .../ftc/teamcode/utils/Robot.java | 6 + 4 files changed, 812 insertions(+), 2 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java index 7f6f524..aabb7b5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java @@ -1,4 +1,5 @@ package org.firstinspires.ftc.teamcode.constants; public class Color { + public static boolean redAlliance = true; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java new file mode 100644 index 0000000..d69eee7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -0,0 +1,803 @@ +package org.firstinspires.ftc.teamcode.teleop; + +import static org.firstinspires.ftc.teamcode.constants.Poses.*; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; +import static org.firstinspires.ftc.teamcode.constants.Color.*; +import static org.firstinspires.ftc.teamcode.constants.ShooterVars.waitTransfer; +import static org.firstinspires.ftc.teamcode.constants.ShooterVars.waitTransferOut; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.Pose2d; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +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.Flywheel; +import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utils.Servos; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; + +import java.util.ArrayList; +import java.util.List; + +@Config +@TeleOp +public class TeleopV3 extends LinearOpMode { + Robot robot; + MultipleTelemetry TELE; + Servos servo; + Flywheel flywheel; + MecanumDrive drive; + + public static double manualVel = 3000; + public static double hoodDefaultPos = 0.5; + public static double desiredTurretAngle = 180; + public static double shootStamp2 = 0.0; + public double vel = 3000; + public boolean autoVel = true; + public double manualOffset = 0.0; + public boolean autoHood = true; + 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; + boolean intake = false; + boolean reject = false; + double xOffset = 0.0; + double yOffset = 0.0; + double headingOffset = 0.0; + int ticker = 0; + int camTicker = 0; + List s1G = new ArrayList<>(); + List s2G = new ArrayList<>(); + List s3G = new ArrayList<>(); + List s1T = new ArrayList<>(); + List s2T = new ArrayList<>(); + List s3T = new ArrayList<>(); + List s1 = new ArrayList<>(); + List s2 = new ArrayList<>(); + List s3 = new ArrayList<>(); + boolean oddBallColor = false; + double hoodOffset = 0.0; + boolean shootA = true; + boolean shootB = true; + boolean shootC = true; + boolean manualTurret = false; + List shootOrder = new ArrayList<>(); + boolean outtake1 = false; + boolean outtake2 = false; + boolean outtake3 = false; + boolean overrideTurr = false; + private boolean shootAll = false; + private double transferStamp = 0.0; + private int tickerA = 1; + private boolean transferIn = false; + double turretPID = 0.0; + double turretPos = 0.5; + double spindexPID = 0.0; + double spindexPos = spindexer_intakePos1; + double error = 0.0; + double spinCurrentPos = 0.0, spinInitPos = 0.0, intakeStamp = 0.0, spinningPow = 0.15; + boolean reverse = false; + int intakeTicker = 0; + + @Override + public void runOpMode() throws InterruptedException { + List allHubs = hardwareMap.getAll(LynxModule.class); + + for (LynxModule hub : allHubs) { + hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); + } + + robot = new Robot(hardwareMap); + TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + servo = new Servos(hardwareMap); + flywheel = new Flywheel(); + drive = new MecanumDrive(hardwareMap, teleStart); + + if (redAlliance) { + robot.limelight.pipelineSwitch(3); + } else { + robot.limelight.pipelineSwitch(2); + } + + robot.limelight.start(); + + waitForStart(); + if (isStopRequested()) return; + while (opModeIsActive()) { + //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 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); + + // PID SERVOS + turretPID = servo.setTurrPos(turretPos); + robot.turr1.setPower(turretPID); + robot.turr2.setPower(-turretPID); + + if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) { + spindexPID = servo.setSpinPos(spindexPos); + robot.spin1.setPower(spindexPID); + robot.spin2.setPower(-spindexPID); + } else { + robot.spin1.setPower(0); + robot.spin2.setPower(0); + } + + // INTAKE + if (gamepad1.right_bumper) { + robot.transferServo.setPosition(transferServo_out); + intakeTicker++; + if (intakeTicker % 16 == 0) { + spinCurrentPos = servo.getSpinPos(); + if (Math.abs(spinCurrentPos - spinInitPos) == 0.0) { + reverse = !reverse; + } + spinInitPos = spinCurrentPos; + } + if (reverse) { + robot.spin1.setPower(spinningPow); + robot.spin2.setPower(-spinningPow); + } else { + robot.spin1.setPower(-spinningPow); + robot.spin2.setPower(spinningPow); + } + robot.intake.setPower(1); + intakeStamp = getRuntime(); + TELE.addData("Reverse?", reverse); + TELE.update(); + } else if (gamepad1.left_bumper) { + robot.intake.setPower(-(getRuntime() - intakeStamp) * 2); + spindexPos = spindexer_intakePos2; + intakeTicker = 0; + } else { + if (getRuntime() - intakeStamp < 1) { + robot.intake.setPower(-(getRuntime() - intakeStamp) * 2); + } else { + robot.intake.setPower(0); + } + intakeTicker = 0; + } + + //COLOR: + + double s1D = robot.color1.getDistance(DistanceUnit.MM); + double s2D = robot.color2.getDistance(DistanceUnit.MM); + double s3D = robot.color3.getDistance(DistanceUnit.MM); + + if (s1D < 43) { + + double green = robot.color1.getNormalizedColors().green; + double red = robot.color1.getNormalizedColors().red; + double blue = robot.color1.getNormalizedColors().blue; + + double gP = green / (green + red + blue); + + s1G.add(gP); + + if (gP >= 0.43) { + s1.add(true); + } else { + s1.add(false); + } + + s1T.add(getRuntime()); + + } + + if (s2D < 60) { + + double green = robot.color2.getNormalizedColors().green; + double red = robot.color2.getNormalizedColors().red; + double blue = robot.color2.getNormalizedColors().blue; + + double gP = green / (green + red + blue); + + s2G.add(gP); + + if (gP >= 0.43) { + s2.add(true); + } else { + s2.add(false); + } + + s2T.add(getRuntime()); + } + + if (s3D < 33) { + + double green = robot.color3.getNormalizedColors().green; + double red = robot.color3.getNormalizedColors().red; + double blue = robot.color3.getNormalizedColors().blue; + + double gP = green / (green + red + blue); + + s3G.add(gP); + + if (gP >= 0.43) { + s3.add(true); + } else { + s3.add(false); + } + + s3T.add(getRuntime()); + } + + if (!s1.isEmpty()) { + green1 = checkGreen(s1, s1T); + } + if (!s2.isEmpty()) { + green2 = checkGreen(s2, s2T); + + } + if (!s3.isEmpty()) { + green3 = checkGreen(s3, s3T); + } + + //SHOOTER: + + double powPID = flywheel.manageFlywheel((int) vel, robot.shooter1.getCurrentPosition()); + + robot.shooter1.setPower(powPID); + robot.shooter2.setPower(powPID); + + robot.transfer.setPower(1); + + double offset; + + 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; + double goalY = 0; + + double dx = goalX - robotX; // delta x from robot to goal + double dy = goalY - robotY; // delta y from robot to goal + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + desiredTurretAngle = (Math.toDegrees(Math.atan2(dy, dx)) + 360) % 360; + + desiredTurretAngle += manualOffset; + + offset = desiredTurretAngle - 180 - (Math.toDegrees(robotHeading - headingOffset)); + + if (offset > 135) { + offset -= 360; + } + + //TODO: test the camera teleop code + double pos = turrDefault + (error / 8); // adds the overall error to the default + + TELE.addData("offset", offset); + + pos -= offset * (0.9 / 360); + + if (pos < 0.02) { + pos = 0.02; + } else if (pos > 0.97) { + pos = 0.97; + } + + if (y < 0.1 && y > -0.1 && x < 0.1 && x > -0.1 && rx < 0.1 && rx > -0.1) { //not moving + double bearing = 0.0; + + LLResult result = robot.limelight.getLatestResult(); + if (result != null) { + if (result.isValid()) { + bearing = result.getTx(); + overrideTurr = true; + turretPos = servo.getTurrPos() - (bearing / 1300); + + double bearingCorrection = bearing / 1300; + + // deadband: ignore tiny noise + if (Math.abs(bearing) > 0.3 && camTicker < 8) { + + // only accumulate if bearing direction is consistent + if (Math.signum(bearingCorrection) == Math.signum(error) || + error == 0) { + error += bearingCorrection; + } + } + camTicker++; + TELE.addData("tx", bearing); + TELE.addData("ty", result.getTy()); + } + } + + } else { + camTicker = 0; + overrideTurr = false; + } + + if (manualTurret) { + pos = turrDefault + (manualOffset / 100); + } + + if (!overrideTurr) { + turretPos = pos; + } + + if (gamepad2.dpad_right) { + manualOffset -= 2; + } else if (gamepad2.dpad_left) { + manualOffset += 2; + } + + //VELOCITY AUTOMATIC + + if (autoVel) { + vel = velPrediction(distanceToGoal); + } else { + vel = manualVel; + } + + if (gamepad2.right_stick_button) { + autoVel = true; + } else if (gamepad2.right_stick_y < -0.5) { + autoVel = false; + manualVel = 4100; + } else if (gamepad2.right_stick_y > 0.5) { + autoVel = false; + manualVel = 2700; + } else if (gamepad2.right_stick_x > 0.5) { + autoVel = false; + manualVel = 3600; + } else if (gamepad2.right_stick_x < -0.5) { + autoVel = false; + manualVel = 3100; + } + + //HOOD: + + if (autoHood) { + 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; + + } + + //TODO: FIX THIS GOOFY THING BECAUSE IT SUCKS @KeshavAnandCode + if (gamepad2.left_stick_x > 0.5) { + manualTurret = false; + } else if (gamepad2.left_stick_x < -0.5) { + manualOffset = 0; + manualTurret = false; + if (gamepad2.left_bumper) { + drive = new MecanumDrive(hardwareMap, new Pose2d(2, 0, 0)); + sleep(1200); + } + } + + if (gamepad2.left_stick_y < -0.5) { + autoHood = true; + } else if (gamepad2.left_stick_y > 0.5) { + autoHood = false; + hoodOffset = 0; + if (gamepad2.left_bumper) { + xOffset = robotX; + yOffset = robotY; + headingOffset = robotHeading; + } + } + + if (shootAll) { + + TELE.addData("100% works", shootOrder); + + intake = false; + reject = false; + + if (!shootOrder.isEmpty() && (getRuntime() - shootStamp < 12)) { + int currentSlot = shootOrder.get(0); // Peek, do NOT remove yet + boolean shootingDone = false; + + if (!outtake1) { + outtake1 = (servo.spinEqual(spindexer_outtakeBall1)); + } + if (!outtake2) { + outtake2 = (servo.spinEqual(spindexer_outtakeBall2)); + } + if (!outtake3) { + outtake3 = (servo.spinEqual(spindexer_outtakeBall3)); + } + + switch (currentSlot) { + case 1: + shootA = shootTeleop(spindexer_outtakeBall1, outtake1, shootStamp2); + TELE.addData("shootA", shootA); + + if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) { + shootingDone = !shootA; + } else { + shootingDone = true; + } + break; + case 2: + shootB = shootTeleop(spindexer_outtakeBall2, outtake2, shootStamp2); + TELE.addData("shootB", shootB); + if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) { + shootingDone = !shootB; + } else { + shootingDone = true; + } + break; + case 3: + shootC = shootTeleop(spindexer_outtakeBall3, outtake3, shootStamp2); + TELE.addData("shootC", shootC); + if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) { + shootingDone = !shootC; + } else { + shootingDone = true; + } + break; + } + + // Remove from the list only if shooting is complete + if (shootingDone) { + shootOrder.remove(0); + shootStamp2 = getRuntime(); + + } + + } else { + // Finished shooting all balls + spindexPos = spindexer_intakePos1; + shootA = true; + shootB = true; + shootC = true; + reject = false; + intake = true; + shootAll = false; + outtake1 = false; + outtake2 = false; + outtake3 = false; + + overrideTurr = false; + + } + + if (gamepad1.squareWasPressed()) { + square = true; + shootStamp = getRuntime(); + shootStamp2 = getRuntime(); + outtake1 = false; + outtake2 = false; + outtake3 = false; + } + + if (gamepad1.circleWasPressed()) { + circle = true; + shootStamp = getRuntime(); + shootStamp2 = getRuntime(); + + outtake1 = false; + outtake2 = false; + outtake3 = false; + + } + + if (gamepad1.triangleWasPressed()) { + triangle = true; + shootStamp = getRuntime(); + shootStamp2 = getRuntime(); + + outtake1 = false; + outtake2 = false; + outtake3 = false; + + } + + if (square || circle || triangle) { + + // Count green balls + int greenCount = 0; + if (green1) greenCount++; + if (green2) greenCount++; + if (green3) greenCount++; + + // Determine the odd ball color + oddBallColor = greenCount < 2; // true = green, false = purple + + shootOrder.clear(); + + // Determine shooting order based on button pressed + // square = odd ball first, triangle = odd ball second, circle = odd ball third + if (square) { + // Odd ball first + addOddThenRest(shootOrder, oddBallColor); + + } else if (triangle) { + // Odd ball second + addOddInMiddle(shootOrder, oddBallColor); + } else if (circle) { + // Odd ball last + addOddLast(shootOrder, oddBallColor); + } + + circle = false; + square = false; + triangle = false; + + } + + // Right bumper shoots all balls fastest, ignoring colors + if (gamepad1.crossWasPressed()) { + 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); + } + + if (!shootOrder.contains(3)) { + shootOrder.add(3); + } + + if (!shootOrder.contains(2)) { + shootOrder.add(2); + } + + if (!shootOrder.contains(1)) { + shootOrder.add(1); + } + + shootAll = true; + } + + //EXTRA STUFFINESS: + + drive.updatePoseEstimate(); + + for (LynxModule hub : allHubs) { + hub.clearBulkCache(); + } + + 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()); + TELE.addData("distanceToGoal", distanceToGoal); + TELE.addData("hood", robot.hood.getPosition()); + TELE.addData("targetVel", vel); + + TELE.addData("shootOrder", shootOrder); + TELE.addData("oddColor", oddBallColor); + + TELE.update(); + + ticker++; + } + } + } + // Helper method + private boolean checkGreen(List s, List sT) { + if (s.isEmpty()) return false; + + double lastTime = sT.get(sT.size() - 1); + int countTrue = 0; + int countWindow = 0; + + for (int i = 0; i < s.size(); i++) { + if (lastTime - sT.get(i) <= 3.0) { // element is within 2s of last + countWindow++; + if (s.get(i)) { + countTrue++; + } + } + } + + if (countWindow == 0) return false; // avoid divide by zero + return countTrue > countWindow / 2.0; // more than 50% true + } + + public boolean shootTeleop(double spindexer, boolean spinOk, double stamp) { + // Set spin positions + spindexPos = spindexer; + + // Check if spindexer has reached the target position + if (spinOk || getRuntime() - stamp > 1.5) { + if (tickerA == 1) { + transferStamp = getRuntime(); + tickerA++; + TELE.addLine("tickerSet"); + } + + if (getRuntime() - transferStamp > waitTransfer && !transferIn) { + robot.transferServo.setPosition(transferServo_in); + transferIn = true; + TELE.addLine("transferring"); + + return true; // still in progress + + } else if (getRuntime() - transferStamp > waitTransfer + waitTransferOut && transferIn) { + robot.transferServo.setPosition(transferServo_out); + transferIn = false; // reset for next shot + tickerA = 1; // reset ticker + transferStamp = 0.0; + + TELE.addLine("shotFinished"); + + return false; // finished shooting + } else { + TELE.addLine("sIP"); + return true; // still in progress + } + } else { + robot.transferServo.setPosition(transferServo_out); + tickerA = 1; + transferStamp = getRuntime(); + transferIn = false; + return true; // still moving spin + } + } + + 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; + + 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) { + // Odd ball first + for (int i = 1; i <= 3; i++) if (getBallColor(i) == oddColor) order.add(i); + TELE.addData("1", shootOrder); + for (int i = 1; i <= 3; i++) if (getBallColor(i) != oddColor) order.add(i); + TELE.addData("works", shootOrder); + TELE.addData("oddBall", oddColor); + shootAll = true; + + } + + void addOddInMiddle(List order, boolean oddColor) { + + boolean[] used = new boolean[4]; // index 1..3 + + // 1) Add a NON-odd ball first + for (int i = 1; i <= 3; i++) { + if (getBallColor(i) != oddColor) { + order.add(i); + used[i] = true; + break; + } + } + + // 2) Add the odd ball second + for (int i = 1; i <= 3; i++) { + if (!used[i] && getBallColor(i) == oddColor) { + order.add(i); + used[i] = true; + break; + } + } + + // 3) Add the remaining non-odd ball third + for (int i = 1; i <= 3; i++) { + if (!used[i] && getBallColor(i) != oddColor) { + order.add(i); + used[i] = true; + break; + } + } + + TELE.addData("works", order); + TELE.addData("oddBall", oddColor); + 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); + TELE.addData("1", shootOrder); + for (int i = 1; i <= 3; i++) if (getBallColor(i) == oddColor) order.add(i); + TELE.addData("works", shootOrder); + 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; + } + return false; // default + } + + boolean ballIn(int slot) { + List times = + slot == 1 ? s1T : + slot == 2 ? s2T : + slot == 3 ? s3T : null; + + if (times == null || times.isEmpty()) return false; + + return times.get(times.size() - 1) > getRuntime() - 2; + } + + 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)); + } + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java index 678c77a..911a6cf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java @@ -86,8 +86,8 @@ public class IntakeTest extends LinearOpMode { robot.spin1.setPower(0); robot.spin2.setPower(0); - if (getRuntime() - stamp < 0.5) { - robot.intake.setPower(-1); + if (getRuntime() - stamp < 1) { + robot.intake.setPower(-(getRuntime() - stamp)*2); } else { robot.intake.setPower(0); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index c396c16..38000fd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java @@ -55,6 +55,8 @@ public class Robot { public RevColorSensorV3 color3; public Limelight3A limelight; + public static boolean usingLimelight = true; + public Robot(HardwareMap hardwareMap) { //Define components w/ hardware map @@ -140,5 +142,9 @@ public class Robot { color2 = hardwareMap.get(RevColorSensorV3.class, "c2"); color3 = hardwareMap.get(RevColorSensorV3.class, "c3"); + + if (usingLimelight){ + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + } } } From 301b5ec7652f7dda35c90e68ef02a07751f17438 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sun, 11 Jan 2026 17:20:50 -0600 Subject: [PATCH 2/9] stash --- .../java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index d69eee7..c679437 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -11,7 +11,6 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; import com.qualcomm.hardware.limelightvision.LLResult; -import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -21,7 +20,6 @@ import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; 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; import java.util.ArrayList; import java.util.List; @@ -307,7 +305,7 @@ public class TeleopV3 extends LinearOpMode { } if (y < 0.1 && y > -0.1 && x < 0.1 && x > -0.1 && rx < 0.1 && rx > -0.1) { //not moving - double bearing = 0.0; + double bearing; LLResult result = robot.limelight.getLatestResult(); if (result != null) { From c460a4fb7a0f6a236e51eaf766a0a6b7d7fee227 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sun, 11 Jan 2026 17:23:09 -0600 Subject: [PATCH 3/9] stash --- .../java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index c679437..3381753 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -3,8 +3,7 @@ package org.firstinspires.ftc.teamcode.teleop; import static org.firstinspires.ftc.teamcode.constants.Poses.*; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; import static org.firstinspires.ftc.teamcode.constants.Color.*; -import static org.firstinspires.ftc.teamcode.constants.ShooterVars.waitTransfer; -import static org.firstinspires.ftc.teamcode.constants.ShooterVars.waitTransferOut; +import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; From 5e8727ebaae72c7b04cd3b7aa45604aae9d1c69e Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sun, 11 Jan 2026 17:46:08 -0600 Subject: [PATCH 4/9] 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 From e39fa396cb3d44261e9d43cdd5346f60e9a6c878 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sun, 11 Jan 2026 18:21:03 -0600 Subject: [PATCH 5/9] started updating the auto --- .../ftc/teamcode/autonomous/Red_V3.java | 145 ++++++++---------- .../ftc/teamcode/utils/FlywheelV2.java | 104 +++++++++++++ 2 files changed, 167 insertions(+), 82 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/FlywheelV2.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 index f0da3d2..58b7b4f 100644 --- 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 @@ -17,16 +17,18 @@ import com.acmerobotics.roadrunner.SequentialAction; import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.Actions; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; 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.FlywheelV2; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Servos; -import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; + +import java.util.List; @Config @Autonomous(preselectTeleOp = "TeleopV3") @@ -34,7 +36,7 @@ public class Red_V3 extends LinearOpMode { Robot robot; MultipleTelemetry TELE; MecanumDrive drive; - Flywheel flywheel; + FlywheelV2 flywheel; Servos servo; double velo = 0.0; @@ -49,38 +51,20 @@ public class Red_V3 extends LinearOpMode { 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(); + powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); 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; - } + + return !flywheel.getSteady(); } }; } @@ -90,8 +74,8 @@ public class Red_V3 extends LinearOpMode { double stamp = 0.0; boolean steady = false; public boolean run(@NonNull TelemetryPacket telemetryPacket) { - powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition()); - velo = flywheel.getVelo(); + powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); steady = flywheel.getSteady(); robot.shooter1.setPower(powPID); robot.shooter2.setPower(powPID); @@ -101,10 +85,9 @@ public class Red_V3 extends LinearOpMode { TELE.update(); detectTag(); - if (last && !steady){ + if (last && !steady) { stamp = getRuntime(); drive.updatePoseEstimate(); - teleStart = drive.localizer.getPose(); return false; } else if (steady) { @@ -119,24 +102,27 @@ public class Red_V3 extends LinearOpMode { public Action Obelisk() { return new Action() { - double stamp = getRuntime(); - int ticker = 0; - + int id = 0; @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp = getRuntime(); - } - ticker++; + LLResult result = robot.limelight.getLatestResult(); + if (result != null && result.isValid()) { + List fiducials = result.getFiducialResults(); + for (LLResultTypes.FiducialResult fiducial : fiducials) { + id = fiducial.getFiducialId(); + TELE.addData("ID", id); + TELE.update(); + } - if (aprilTag.getTagById(21) != null) { + } + + if (id == 21){ gpp = true; - } else if (aprilTag.getTagById(22) != null) { + } else if (id == 22){ pgp = true; - } else if (aprilTag.getTagById(23) != null) { + } else if (id == 23){ ppg = true; } - aprilTag.update(); TELE.addData("Velocity", velo); TELE.addData("21", gpp); @@ -144,10 +130,11 @@ public class Red_V3 extends LinearOpMode { TELE.addData("23", ppg); TELE.update(); - if (gpp || pgp || ppg){ + if (gpp || pgp || ppg) { double turretPID = servo.setTurrPos(turret_red); robot.turr1.setPower(turretPID); robot.turr2.setPower(-turretPID); + robot.limelight.pipelineSwitch(3); return !servo.turretEqual(turret_red); } else { return true; @@ -156,23 +143,24 @@ public class Red_V3 extends LinearOpMode { }; } - public Action spindex (double spindexer, int vel){ + public Action spindex(double spindexer, int vel) { return new Action() { + double spinPID = 0.0; @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { - powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition()); - velo = flywheel.getVelo(); + powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); robot.shooter1.setPower(powPID); robot.shooter2.setPower(powPID); - robot.spin1.setPower(spindexer); - robot.spin2.setPower(1-spindexer); + spinPID = servo.setSpinPos(spindexer); + robot.spin1.setPower(spinPID); + robot.spin2.setPower(-spinPID); TELE.addData("Velocity", velo); TELE.addLine("spindex"); TELE.update(); drive.updatePoseEstimate(); - teleStart = drive.localizer.getPose(); return !servo.spinEqual(spindexer); @@ -185,14 +173,15 @@ public class Red_V3 extends LinearOpMode { 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(); + powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); robot.shooter1.setPower(powPID); robot.shooter2.setPower(powPID); @@ -212,7 +201,8 @@ public class Red_V3 extends LinearOpMode { TELE.update(); transferIn = true; return true; - } else if (getRuntime() - transferStamp > waitTransfer+waitTransferOut && transferIn){ + } else if (getRuntime() - transferStamp > waitTransfer + waitTransferOut && + transferIn) { robot.transferServo.setPosition(transferServo_out); TELE.addData("Velocity", velo); TELE.addLine("shot once"); @@ -273,6 +263,7 @@ public class Red_V3 extends LinearOpMode { return new Action() { double stamp = 0.0; int ticker = 0; + @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { if (ticker == 0) { @@ -280,9 +271,9 @@ public class Red_V3 extends LinearOpMode { } ticker++; - if (getRuntime() - stamp < 0.3){ + if (getRuntime() - stamp < 0.3) { return true; - }else { + } else { robot.intake.setPower(0); return false; } @@ -295,6 +286,7 @@ public class Red_V3 extends LinearOpMode { double stamp = 0.0; int ticker = 0; double position = 0.0; + @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { if (ticker == 0) { @@ -326,8 +318,6 @@ public class Red_V3 extends LinearOpMode { double gP = green / (green + red + blue); - - if (gP >= 0.4) { b1 = 2; } else { @@ -385,7 +375,7 @@ public class Red_V3 extends LinearOpMode { robot = new Robot(hardwareMap); - flywheel = new Flywheel(); + flywheel = new FlywheelV2(); TELE = new MultipleTelemetry( telemetry, FtcDashboard.getInstance().getTelemetry() @@ -395,7 +385,8 @@ public class Red_V3 extends LinearOpMode { 0, 0, 0 )); - aprilTag = new AprilTagWebcam(); + robot.limelight.pipelineSwitch(1); + robot.limelight.start(); TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); @@ -416,12 +407,10 @@ public class Red_V3 extends LinearOpMode { 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; + hoodAuto -= 0.01; } if (gamepad2.dpadDownWasPressed()) { @@ -435,13 +424,11 @@ public class Red_V3 extends LinearOpMode { 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; @@ -461,8 +448,8 @@ public class Red_V3 extends LinearOpMode { teleStart = drive.localizer.getPose(); - powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition()); - velo = flywheel.getVelo(); + powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); robot.shooter1.setPower(powPID); robot.shooter2.setPower(powPID); @@ -497,8 +484,8 @@ public class Red_V3 extends LinearOpMode { teleStart = drive.localizer.getPose(); - powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition()); - velo = flywheel.getVelo(); + powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); robot.shooter1.setPower(powPID); robot.shooter2.setPower(powPID); @@ -527,8 +514,8 @@ public class Red_V3 extends LinearOpMode { ) ); - powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition()); - velo = flywheel.getVelo(); + powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); robot.shooter1.setPower(powPID); robot.shooter2.setPower(powPID); @@ -547,19 +534,13 @@ public class Red_V3 extends LinearOpMode { } } - - 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); + //TODO: adjust this + public void detectTag() { + LLResult result = robot.limelight.getLatestResult(); + if (result != null) { + if (result.isValid()) { + bearing = result.getTx(); + } } double turretPos = servo.getTurrPos() - (bearing / 1300); double turretPID = servo.setTurrPos(turretPos); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/FlywheelV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/FlywheelV2.java new file mode 100644 index 0000000..64a8c72 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/FlywheelV2.java @@ -0,0 +1,104 @@ +package org.firstinspires.ftc.teamcode.utils; + +import com.acmerobotics.dashboard.config.Config; + +@Config +public class FlywheelV2 { + public static double kP = 0.001; // small proportional gain (tune this) + public static double maxStep = 0.06; // prevents sudden jumps + double initPos1 = 0.0; + double initPos2 = 0.0; + double stamp = 0.0; + double stamp1 = 0.0; + double ticker = 0.0; + double currentPos1 = 0.0; + double currentPos2 = 0.0; + double velo = 0.0; + double velo1 = 0.0; + double velo1a = 0.0; + double velo1b = 0.0; + double velo2 = 0.0; + double velo3 = 0.0; + double velo4 = 0.0; + double velo5 = 0.0; + double targetVelocity = 0.0; + double powPID = 0.0; + boolean steady = false; + + public FlywheelV2() { + //robot = new Robot(hardwareMap); + } + + public double getVelo(double shooter1CurPos, double shooter2CurPos) { + ticker++; + if (ticker % 2 == 0) { + velo5 = velo4; + velo4 = velo3; + velo3 = velo2; + velo2 = velo1; + + currentPos1 = shooter1CurPos / 28; + currentPos2 = shooter2CurPos / 28; + stamp = getTimeSeconds(); //getRuntime(); + velo1a = 60 * ((currentPos1 - initPos1) / (stamp - stamp1)); + velo1b = 60 * ((currentPos2 - initPos2) / (stamp - stamp1)); + initPos1 = currentPos1; + initPos2 = currentPos2; + stamp1 = stamp; + + if (velo1a < 200){ + velo1 = velo1b; + } else if (velo1b < 200){ + velo1 = velo1a; + } else { + velo1 = (velo1a + velo1b) / 2; + } + } + return ((velo1 + velo2 + velo3 + velo4 + velo5) / 5); + } + + public double getVelo1() { return (velo1a + velo2 + velo3 + velo4 + velo5) / 5; } + + public double getVelo2() { return (velo1b + velo2 + velo3 + velo4 + velo5) / 5; } + + public boolean getSteady() { + return steady; + } + + private double getTimeSeconds() { + return (double) System.currentTimeMillis() / 1000.0; + } + + public double manageFlywheel(int commandedVelocity, double shooter1CurPos, double shooter2CurPos) { + targetVelocity = commandedVelocity; + velo = getVelo(shooter1CurPos, shooter2CurPos); + // Flywheel PID code here + if (targetVelocity - velo > 500) { + powPID = 1.0; + } else if (velo - targetVelocity > 500) { + powPID = 0.0; + } else { + double feed = Math.log((668.39 / (targetVelocity + 591.96)) - 0.116) / -4.18; + + // --- PROPORTIONAL CORRECTION --- + double error = targetVelocity - 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)); + } + + steady = (Math.abs(targetVelocity - velo) < 100.0); + + return powPID; + } + + public void update() { + } +} \ No newline at end of file From 46ed4f544fd48709d0edebcf6dda2f3f4ff3decb Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Mon, 12 Jan 2026 20:17:44 -0600 Subject: [PATCH 6/9] auton is updated - to be tested --- .../autonomous/{Red_V3.java => Auto_V3.java} | 245 +++++++++--------- .../ftc/teamcode/constants/Poses.java | 12 - .../ftc/teamcode/tests/IntakeTest.java | 1 - 3 files changed, 126 insertions(+), 132 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/{Red_V3.java => Auto_V3.java} (79%) 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/Auto_V3.java similarity index 79% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V3.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_V3.java index 58b7b4f..ec66770 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_V3.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.autonomous; +import static org.firstinspires.ftc.teamcode.constants.Color.*; import static org.firstinspires.ftc.teamcode.constants.Poses.*; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*; @@ -32,7 +33,7 @@ import java.util.List; @Config @Autonomous(preselectTeleOp = "TeleopV3") -public class Red_V3 extends LinearOpMode { +public class Auto_V3 extends LinearOpMode { Robot robot; MultipleTelemetry TELE; MecanumDrive drive; @@ -40,8 +41,8 @@ public class Red_V3 extends LinearOpMode { Servos servo; double velo = 0.0; - public static double intake1Time = 2.9; - public static double intake2Time = 2.9; + public static double intake1Time = 2.7; + public static double intake2Time = 3.0; public static double colorDetect = 3.0; boolean gpp = false; boolean pgp = false; @@ -51,6 +52,7 @@ public class Red_V3 extends LinearOpMode { 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 static double holdTurrPow = 0.1; // power to hold turret in place public Action initShooter(int vel) { return new Action() { @@ -59,7 +61,6 @@ public class Red_V3 extends LinearOpMode { velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); robot.shooter1.setPower(powPID); robot.shooter2.setPower(powPID); - robot.transfer.setPower(1); TELE.addData("Velocity", velo); TELE.update(); @@ -69,37 +70,6 @@ public class Red_V3 extends LinearOpMode { }; } - 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(), robot.shooter2.getCurrentPosition()); - velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - 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() { int id = 0; @@ -134,7 +104,11 @@ public class Red_V3 extends LinearOpMode { double turretPID = servo.setTurrPos(turret_red); robot.turr1.setPower(turretPID); robot.turr2.setPower(-turretPID); - robot.limelight.pipelineSwitch(3); + if (redAlliance){ + robot.limelight.pipelineSwitch(3); + } else { + robot.limelight.pipelineSwitch(2); + } return !servo.turretEqual(turret_red); } else { return true; @@ -148,11 +122,11 @@ public class Red_V3 extends LinearOpMode { double spinPID = 0.0; @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { - powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); robot.shooter1.setPower(powPID); robot.shooter2.setPower(powPID); + spinPID = servo.setSpinPos(spindexer); robot.spin1.setPower(spinPID); robot.spin2.setPower(-spinPID); @@ -163,7 +137,13 @@ public class Red_V3 extends LinearOpMode { drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); - return !servo.spinEqual(spindexer); + if (servo.spinEqual(spindexer)){ + robot.spin1.setPower(0); + robot.spin2.setPower(0); + return false; + } else { + return true; + } } }; } @@ -201,9 +181,10 @@ public class Red_V3 extends LinearOpMode { TELE.update(); transferIn = true; return true; - } else if (getRuntime() - transferStamp > waitTransfer + waitTransferOut && - transferIn) { + } else if (getRuntime() - transferStamp > waitTransfer + waitTransferOut && transferIn) { robot.transferServo.setPosition(transferServo_out); + robot.turr1.setPower(holdTurrPow); + robot.turr2.setPower(holdTurrPow); TELE.addData("Velocity", velo); TELE.addLine("shot once"); TELE.update(); @@ -218,9 +199,10 @@ public class Red_V3 extends LinearOpMode { public Action intake(double intakeTime) { return new Action() { - double position = 0.0; + double position = spindexer_intakePos1; double stamp = 0.0; int ticker = 0; + double pow = 1.0; @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { @@ -229,17 +211,38 @@ public class Red_V3 extends LinearOpMode { } ticker++; + robot.intake.setPower(pow); + 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; + if (!servo.spinEqual(position)){ + double spinPID = servo.setSpinPos(position); + robot.spin1.setPower(spinPID); + robot.spin2.setPower(-spinPID); + } + + if (s1D < 43 && servo.spinEqual(position) && getRuntime() - stamp > 0.5){ + if (s2D > 60){ + if (servo.spinEqual(spindexer_intakePos1)){ + position = spindexer_intakePos2; + } else if (servo.spinEqual(spindexer_intakePos2)){ + position = spindexer_intakePos3; + } else if (servo.spinEqual(spindexer_intakePos3)){ + position = spindexer_intakePos1; + } + } else if (s3D > 33){ + if (servo.spinEqual(spindexer_intakePos1)){ + position = spindexer_intakePos3; + } else if (servo.spinEqual(spindexer_intakePos2)){ + position = spindexer_intakePos1; + } else if (servo.spinEqual(spindexer_intakePos3)){ + position = spindexer_intakePos2; + } + } + stamp = getRuntime(); } - robot.spin1.setPower(position); - robot.spin2.setPower(1 - position); TELE.addData("Velocity", velo); TELE.addLine("Intaking"); @@ -250,7 +253,9 @@ public class Red_V3 extends LinearOpMode { teleStart = drive.localizer.getPose(); robot.intake.setPower(1); - if ((s1D < 40.0 && s2D < 40.0 && s3D < 40.0) || getRuntime() - stamp > intakeTime) { + if ((s1D < 43.0 && s2D < 60.0 && s3D < 33.0) || getRuntime() - stamp > intakeTime) { + robot.spin1.setPower(0); + robot.spin2.setPower(0); return false; } else { return true; @@ -259,11 +264,10 @@ public class Red_V3 extends LinearOpMode { }; } - public Action intakeReject() { + public Action ColorDetect(int vel) { return new Action() { double stamp = 0.0; int ticker = 0; - @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { if (ticker == 0) { @@ -271,36 +275,10 @@ public class Red_V3 extends LinearOpMode { } 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); + powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + robot.shooter1.setPower(powPID); + robot.shooter2.setPower(powPID); double s1D = robot.color1.getDistance(DistanceUnit.MM); double s2D = robot.color2.getDistance(DistanceUnit.MM); @@ -310,7 +288,7 @@ public class Red_V3 extends LinearOpMode { teleStart = drive.localizer.getPose(); - if (s1D < 40) { + if (s1D < 43) { double green = robot.color1.getNormalizedColors().green; double red = robot.color1.getNormalizedColors().red; @@ -325,7 +303,7 @@ public class Red_V3 extends LinearOpMode { } } - if (s2D < 40) { + if (s2D < 60) { double green = robot.color2.getNormalizedColors().green; double red = robot.color2.getNormalizedColors().red; @@ -340,7 +318,7 @@ public class Red_V3 extends LinearOpMode { } } - if (s3D < 30) { + if (s3D < 33) { double green = robot.color3.getNormalizedColors().green; double red = robot.color3.getNormalizedColors().red; @@ -389,23 +367,21 @@ public class Red_V3 extends LinearOpMode { robot.limelight.start(); TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) - .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); + .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); - TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1)) - .strafeToLinearHeading(new Vector2d(rx2a, ry2a), rh2a) - .strafeToLinearHeading(new Vector2d(rx2b, ry2b), rh2b); + TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(bx1, by1, bh1)) + .strafeToLinearHeading(new Vector2d(bx2a, by2a), bh2a) + .strafeToLinearHeading(new Vector2d(bx2b, by2b), bh2b); - TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(rx2b, ry2b, rh2b)) - .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); + TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(bx2b, by2b, bh2b)) + .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); - TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1)) + TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(bx1, by1, bh1)) + .strafeToLinearHeading(new Vector2d(bx3a, by3a), bh3a) + .strafeToLinearHeading(new Vector2d(bx3b, by3b), bh3b); - .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); + TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(bx3b, by3b, bh3b)) + .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); while (opModeInInit()) { @@ -417,15 +393,53 @@ public class Red_V3 extends LinearOpMode { hoodAuto += 0.01; } + if (gamepad2.crossWasPressed()){ + redAlliance = !redAlliance; + } + + if (redAlliance){ + shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) + .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); + + pickup1 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1)) + .strafeToLinearHeading(new Vector2d(rx2a, ry2a), rh2a) + .strafeToLinearHeading(new Vector2d(rx2b, ry2b), rh2b); + + shoot1 = drive.actionBuilder(new Pose2d(rx2b, ry2b, rh2b)) + .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); + + pickup2 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1)) + .strafeToLinearHeading(new Vector2d(rx3a, ry3a), rh3a) + .strafeToLinearHeading(new Vector2d(rx3b, ry3b), rh3b); + + shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b)) + .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); + } else { + shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) + .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); + + pickup1 = drive.actionBuilder(new Pose2d(bx1, by1, bh1)) + .strafeToLinearHeading(new Vector2d(bx2a, by2a), bh2a) + .strafeToLinearHeading(new Vector2d(bx2b, by2b), bh2b); + + shoot1 = drive.actionBuilder(new Pose2d(bx2b, by2b, bh2b)) + .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); + + pickup2 = drive.actionBuilder(new Pose2d(bx1, by1, bh1)) + .strafeToLinearHeading(new Vector2d(bx3a, by3a), bh3a) + .strafeToLinearHeading(new Vector2d(bx3b, by3b), bh3b); + + shoot2 = drive.actionBuilder(new Pose2d(bx3b, by3b, bh3b)) + .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); + } + robot.hood.setPosition(hoodAuto); robot.transferServo.setPosition(transferServo_out); - robot.spin1.setPower(spindexer_intakePos1); - robot.spin2.setPower(1 - spindexer_intakePos1); - TELE.addData("Velocity", velo); TELE.addData("Turret Pos", servo.getTurrPos()); + TELE.addData("Spin Pos", servo.getSpinPos()); TELE.update(); } @@ -448,13 +462,12 @@ public class Red_V3 extends LinearOpMode { teleStart = drive.localizer.getPose(); - powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); + robot.transfer.setPower(1); shootingSequence(); + robot.transfer.setPower(0); + robot.hood.setPosition(hoodAuto); drive.updatePoseEstimate(); @@ -474,9 +487,7 @@ public class Red_V3 extends LinearOpMode { Actions.runBlocking( new ParallelAction( shoot1.build(), - ColorDetect(), - steadyShooter(AUTO_CLOSE_VEL, true), - intakeReject() + ColorDetect(AUTO_CLOSE_VEL) ) ); @@ -484,12 +495,12 @@ public class Red_V3 extends LinearOpMode { teleStart = drive.localizer.getPose(); - powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); + robot.transfer.setPower(1); shootingSequence(); + + robot.transfer.setPower(0); + drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); @@ -507,20 +518,16 @@ public class Red_V3 extends LinearOpMode { Actions.runBlocking( new ParallelAction( shoot2.build(), - ColorDetect(), - steadyShooter(AUTO_CLOSE_VEL, true), - intakeReject() - + ColorDetect(AUTO_CLOSE_VEL) ) ); - powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); + robot.transfer.setPower(1); shootingSequence(); + robot.transfer.setPower(0); + drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); @@ -534,7 +541,7 @@ public class Red_V3 extends LinearOpMode { } } - //TODO: adjust this + //TODO: adjust this according to Teleop numbers public void detectTag() { LLResult result = robot.limelight.getLatestResult(); if (result != null) { 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 31fbc1d..b9dfead 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 @@ -12,20 +12,8 @@ public class Poses { public static double relativeGoalHeight = goalHeight - turretHeight; - public static double x1 = 50, y1 = 0, h1 = 0; - - public static double x2 = 31, y2 = 32, h2 = Math.toRadians(140); - - public static double x2_b = 58, y2_b = 42, h2_b = Math.toRadians(140); - - public static double x3 = 34, y3 = 58, h3 = Math.toRadians(140); - - public static double tx = 0, ty = 0, th = 0; - public static Pose2d goalPose = new Pose2d(-15, 0, 0); - - public static double rx1 = 45, ry1 = -7, rh1 = 0; public static double rx2a = 45, ry2a = 5, rh2a = Math.toRadians(140); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java index 911a6cf..b55608c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java @@ -205,7 +205,6 @@ public class IntakeTest extends LinearOpMode { } } - boolean ballIn(int slot) { List times; From 58e7289c7b690e321139ee601d846e1818f39e14 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Mon, 12 Jan 2026 20:55:09 -0600 Subject: [PATCH 7/9] new auton that is very simple --- .../{Auto_V3.java => AutoClose_V3.java} | 36 +- .../ftc/teamcode/autonomous/AutoFar_V1.java | 651 ++++++++++++++++++ .../ftc/teamcode/autonomous/Blue_V2.java | 8 +- .../ftc/teamcode/autonomous/Red_V2.java | 4 +- .../ftc/teamcode/constants/Poses.java | 10 +- .../teamcode/constants/ServoPositions.java | 16 +- .../ftc/teamcode/constants/ShooterVars.java | 1 + 7 files changed, 696 insertions(+), 30 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/{Auto_V3.java => AutoClose_V3.java} (95%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_V3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_V3.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java index ec66770..b6ab1c0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_V3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java @@ -33,7 +33,7 @@ import java.util.List; @Config @Autonomous(preselectTeleOp = "TeleopV3") -public class Auto_V3 extends LinearOpMode { +public class AutoClose_V3 extends LinearOpMode { Robot robot; MultipleTelemetry TELE; MecanumDrive drive; @@ -101,15 +101,20 @@ public class Auto_V3 extends LinearOpMode { TELE.update(); if (gpp || pgp || ppg) { - double turretPID = servo.setTurrPos(turret_red); - robot.turr1.setPower(turretPID); - robot.turr2.setPower(-turretPID); if (redAlliance){ robot.limelight.pipelineSwitch(3); + double turretPID = servo.setTurrPos(turret_redClose); + robot.turr1.setPower(turretPID); + robot.turr2.setPower(-turretPID); + return !servo.turretEqual(turret_redClose); + } else { robot.limelight.pipelineSwitch(2); + double turretPID = servo.setTurrPos(turret_blueClose); + robot.turr1.setPower(turretPID); + robot.turr2.setPower(-turretPID); + return !servo.turretEqual(turret_blueClose); } - return !servo.turretEqual(turret_red); } else { return true; } @@ -256,7 +261,13 @@ public class Auto_V3 extends LinearOpMode { if ((s1D < 43.0 && s2D < 60.0 && s3D < 33.0) || getRuntime() - stamp > intakeTime) { robot.spin1.setPower(0); robot.spin2.setPower(0); - return false; + if (getRuntime() - stamp - intakeTime < 1){ + pow = -2*(getRuntime() - stamp - intakeTime); + return true; + } else { + robot.intake.setPower(0); + return false; + } } else { return true; } @@ -397,7 +408,11 @@ public class Auto_V3 extends LinearOpMode { redAlliance = !redAlliance; } + double turrPID; + if (redAlliance){ + turrPID = servo.setTurrPos(turret_detectRedClose); + shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); @@ -415,6 +430,8 @@ public class Auto_V3 extends LinearOpMode { shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b)) .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); } else { + turrPID = servo.setTurrPos(turret_detectBlueClose); + shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); @@ -433,6 +450,9 @@ public class Auto_V3 extends LinearOpMode { .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); } + robot.turr1.setPower(turrPID); + robot.turr2.setPower(-turrPID); + robot.hood.setPosition(hoodAuto); robot.transferServo.setPosition(transferServo_out); @@ -449,8 +469,6 @@ public class Auto_V3 extends LinearOpMode { if (opModeIsActive()) { - robot.hood.setPosition(hoodAuto); - Actions.runBlocking( new ParallelAction( shoot0.build(), @@ -468,8 +486,6 @@ public class Auto_V3 extends LinearOpMode { robot.transfer.setPower(0); - robot.hood.setPosition(hoodAuto); - drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java new file mode 100644 index 0000000..88dd86e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java @@ -0,0 +1,651 @@ +package org.firstinspires.ftc.teamcode.autonomous; + +import static org.firstinspires.ftc.teamcode.constants.Color.*; +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.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +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.FlywheelV2; +import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utils.Servos; + +import java.util.List; + +@Config +@Autonomous(preselectTeleOp = "TeleopV3") +public class AutoFar_V1 extends LinearOpMode { + Robot robot; + MultipleTelemetry TELE; + MecanumDrive drive; + FlywheelV2 flywheel; + Servos servo; + + double velo = 0.0; + public static double intake1Time = 2.7; + public static double intake2Time = 3.0; + 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 static double holdTurrPow = 0.1; // power to hold turret in place + + public Action initShooter(int vel) { + return new Action() { + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + robot.shooter1.setPower(powPID); + robot.shooter2.setPower(powPID); + + TELE.addData("Velocity", velo); + TELE.update(); + + return !flywheel.getSteady(); + } + }; + } + + public Action Obelisk() { + return new Action() { + int id = 0; + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + LLResult result = robot.limelight.getLatestResult(); + if (result != null && result.isValid()) { + List fiducials = result.getFiducialResults(); + for (LLResultTypes.FiducialResult fiducial : fiducials) { + id = fiducial.getFiducialId(); + TELE.addData("ID", id); + TELE.update(); + } + + } + + if (id == 21){ + gpp = true; + } else if (id == 22){ + pgp = true; + } else if (id == 23){ + ppg = true; + } + + TELE.addData("Velocity", velo); + TELE.addData("21", gpp); + TELE.addData("22", pgp); + TELE.addData("23", ppg); + TELE.update(); + + if (gpp || pgp || ppg) { + if (redAlliance){ + robot.limelight.pipelineSwitch(3); + double turretPID = servo.setTurrPos(turret_redFar); + robot.turr1.setPower(turretPID); + robot.turr2.setPower(-turretPID); + return !servo.turretEqual(turret_redFar); + + } else { + robot.limelight.pipelineSwitch(2); + double turretPID = servo.setTurrPos(turret_blueFar); + robot.turr1.setPower(turretPID); + robot.turr2.setPower(-turretPID); + return !servo.turretEqual(turret_blueFar); + } + } else { + return true; + } + } + }; + } + + public Action spindex(double spindexer, int vel) { + return new Action() { + double spinPID = 0.0; + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + robot.shooter1.setPower(powPID); + robot.shooter2.setPower(powPID); + + spinPID = servo.setSpinPos(spindexer); + robot.spin1.setPower(spinPID); + robot.spin2.setPower(-spinPID); + TELE.addData("Velocity", velo); + TELE.addLine("spindex"); + TELE.update(); + + drive.updatePoseEstimate(); + teleStart = drive.localizer.getPose(); + + if (servo.spinEqual(spindexer)){ + robot.spin1.setPower(0); + robot.spin2.setPower(0); + return false; + } else { + return true; + } + } + }; + } + + 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(), robot.shooter2.getCurrentPosition()); + velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + 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); + robot.turr1.setPower(holdTurrPow); + robot.turr2.setPower(holdTurrPow); + 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 = spindexer_intakePos1; + double stamp = 0.0; + int ticker = 0; + double pow = 1.0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if (ticker == 0) { + stamp = getRuntime(); + } + ticker++; + + robot.intake.setPower(pow); + + double s1D = robot.color1.getDistance(DistanceUnit.MM); + double s2D = robot.color2.getDistance(DistanceUnit.MM); + double s3D = robot.color3.getDistance(DistanceUnit.MM); + + if (!servo.spinEqual(position)){ + double spinPID = servo.setSpinPos(position); + robot.spin1.setPower(spinPID); + robot.spin2.setPower(-spinPID); + } + + if (s1D < 43 && servo.spinEqual(position) && getRuntime() - stamp > 0.5){ + if (s2D > 60){ + if (servo.spinEqual(spindexer_intakePos1)){ + position = spindexer_intakePos2; + } else if (servo.spinEqual(spindexer_intakePos2)){ + position = spindexer_intakePos3; + } else if (servo.spinEqual(spindexer_intakePos3)){ + position = spindexer_intakePos1; + } + } else if (s3D > 33){ + if (servo.spinEqual(spindexer_intakePos1)){ + position = spindexer_intakePos3; + } else if (servo.spinEqual(spindexer_intakePos2)){ + position = spindexer_intakePos1; + } else if (servo.spinEqual(spindexer_intakePos3)){ + position = spindexer_intakePos2; + } + } + stamp = getRuntime(); + } + + TELE.addData("Velocity", velo); + TELE.addLine("Intaking"); + TELE.update(); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + robot.intake.setPower(1); + if ((s1D < 43.0 && s2D < 60.0 && s3D < 33.0) || getRuntime() - stamp > intakeTime) { + robot.spin1.setPower(0); + robot.spin2.setPower(0); + if (getRuntime() - stamp - intakeTime < 1){ + pow = -2*(getRuntime() - stamp - intakeTime); + return true; + } else { + robot.intake.setPower(0); + return false; + } + } else { + return true; + } + } + }; + } + + public Action ColorDetect(int vel) { + return new Action() { + double stamp = 0.0; + int ticker = 0; + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if (ticker == 0) { + stamp = getRuntime(); + } + ticker++; + + powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + robot.shooter1.setPower(powPID); + robot.shooter2.setPower(powPID); + + 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 < 43) { + + 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 < 60) { + + 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 < 33) { + + 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 FlywheelV2(); + + TELE = new MultipleTelemetry( + telemetry, FtcDashboard.getInstance().getTelemetry() + ); + + drive = new MecanumDrive(hardwareMap, new Pose2d( + 0, 0, 0 + )); + + robot.limelight.pipelineSwitch(1); + robot.limelight.start(); + + //TODO: add positions to develop auto + + TrajectoryActionBuilder park = drive.actionBuilder(new Pose2d(0,0,0)) + .strafeToLinearHeading(new Vector2d(rfx1, rfy1), rfh1); + + while (opModeInInit()) { + + if (gamepad2.dpadUpWasPressed()) { + hoodAuto -= 0.01; + } + + if (gamepad2.dpadDownWasPressed()) { + hoodAuto += 0.01; + } + + if (gamepad2.crossWasPressed()){ + redAlliance = !redAlliance; + } + + double turrPID; + + if (redAlliance){ + turrPID = servo.setTurrPos(turret_detectRedClose); + } else { + turrPID = servo.setTurrPos(turret_detectBlueClose); + } + + robot.turr1.setPower(turrPID); + robot.turr2.setPower(-turrPID); + + robot.hood.setPosition(hoodAutoFar); + + robot.transferServo.setPosition(transferServo_out); + + TELE.addData("Velocity", velo); + TELE.addData("Turret Pos", servo.getTurrPos()); + TELE.addData("Spin Pos", servo.getSpinPos()); + TELE.update(); + } + + waitForStart(); + + if (isStopRequested()) return; + + if (opModeIsActive()) { + + Actions.runBlocking( + new ParallelAction( + initShooter(AUTO_FAR_VEL), + Obelisk() + ) + ); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + robot.transfer.setPower(1); + + shootingSequence(); + + robot.transfer.setPower(0); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + Actions.runBlocking(park.build()); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + TELE.addData("Velocity", velo); + TELE.addLine("finished"); + TELE.update(); + + sleep(2000); + + } + + } + //TODO: adjust this according to Teleop numbers + public void detectTag() { + LLResult result = robot.limelight.getLatestResult(); + if (result != null) { + if (result.isValid()) { + bearing = result.getTx(); + } + } + 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_FAR_VEL), + Shoot(AUTO_FAR_VEL), + spindex(spindexer_outtakeBall2, AUTO_FAR_VEL), + Shoot(AUTO_FAR_VEL), + spindex(spindexer_outtakeBall3, AUTO_FAR_VEL), + Shoot(AUTO_FAR_VEL) + ) + ); + } + + public void sequence2() { + Actions.runBlocking( + new SequentialAction( + spindex(spindexer_outtakeBall1, AUTO_FAR_VEL), + Shoot(AUTO_FAR_VEL), + spindex(spindexer_outtakeBall3, AUTO_FAR_VEL), + Shoot(AUTO_FAR_VEL), + spindex(spindexer_outtakeBall2, AUTO_FAR_VEL), + Shoot(AUTO_FAR_VEL) + ) + ); + } + + public void sequence3() { + Actions.runBlocking( + new SequentialAction( + spindex(spindexer_outtakeBall2, AUTO_FAR_VEL), + Shoot(AUTO_FAR_VEL), + spindex(spindexer_outtakeBall1, AUTO_FAR_VEL), + Shoot(AUTO_FAR_VEL), + spindex(spindexer_outtakeBall3, AUTO_FAR_VEL), + Shoot(AUTO_FAR_VEL) + ) + ); + } + + public void sequence4() { + Actions.runBlocking( + new SequentialAction( + spindex(spindexer_outtakeBall2, AUTO_FAR_VEL), + Shoot(AUTO_FAR_VEL), + spindex(spindexer_outtakeBall3, AUTO_FAR_VEL), + Shoot(AUTO_FAR_VEL), + spindex(spindexer_outtakeBall1, AUTO_FAR_VEL), + Shoot(AUTO_FAR_VEL) + ) + ); + } + + public void sequence5() { + Actions.runBlocking( + new SequentialAction( + spindex(spindexer_outtakeBall3, AUTO_FAR_VEL), + Shoot(AUTO_FAR_VEL), + spindex(spindexer_outtakeBall1, AUTO_FAR_VEL), + Shoot(AUTO_FAR_VEL), + spindex(spindexer_outtakeBall2, AUTO_FAR_VEL), + Shoot(AUTO_FAR_VEL) + ) + ); + } + + public void sequence6() { + Actions.runBlocking( + new SequentialAction( + spindex(spindexer_outtakeBall3, AUTO_FAR_VEL), + Shoot(AUTO_FAR_VEL), + spindex(spindexer_outtakeBall2, AUTO_FAR_VEL), + Shoot(AUTO_FAR_VEL), + spindex(spindexer_outtakeBall1, AUTO_FAR_VEL), + Shoot(AUTO_FAR_VEL) + ) + ); + } +} \ No newline at end of file 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 518c249..792b6db 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 @@ -178,8 +178,8 @@ public class Blue_V2 extends LinearOpMode { TELE.update(); if (gpp || pgp || ppg){ - robot.turr1.setPower(turret_blue); - robot.turr2.setPower(1 - turret_blue); + robot.turr1.setPower(turret_blueClose); + robot.turr2.setPower(1 - turret_blueClose); return false; } else { return true; @@ -541,8 +541,8 @@ public class Blue_V2 extends LinearOpMode { robot.hood.setPosition(hoodAuto); - robot.turr1.setPower(turret_detectBlue); - robot.turr2.setPower(1 - turret_detectBlue); + robot.turr1.setPower(turret_detectBlueClose); + robot.turr2.setPower(1 - turret_detectBlueClose); robot.transferServo.setPosition(transferServo_out); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java index bcf89a0..00ce9a4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java @@ -169,10 +169,10 @@ public class Red_V2 extends LinearOpMode { TELE.update(); if (gpp || pgp || ppg){ - double turretPID = servo.setTurrPos(turret_red); + double turretPID = servo.setTurrPos(turret_redClose); robot.turr1.setPower(turretPID); robot.turr2.setPower(-turretPID); - return !servo.turretEqual(turret_red); + return !servo.turretEqual(turret_redClose); } else { return true; } 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 b9dfead..6deb4ee 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 @@ -15,25 +15,19 @@ public class Poses { public static Pose2d goalPose = new Pose2d(-15, 0, 0); public static double rx1 = 45, ry1 = -7, rh1 = 0; - public static double rx2a = 45, ry2a = 5, rh2a = Math.toRadians(140); - public static double rx2b = 31, ry2b = 32, rh2b = Math.toRadians(140); - public static double rx3a = 58, ry3a = 42, rh3a = Math.toRadians(140); - public static double rx3b = 34, ry3b = 58, rh3b = Math.toRadians(140); public static double bx1 = 45, by1 = 6, bh1 = 0; - public static double bx2a = 53, by2a = -7, bh2a = Math.toRadians(-140); - public static double bx2b = 23, by2b = -39, bh2b = Math.toRadians(-140); - public static double bx3a = 56, by3a = -34, bh3a = Math.toRadians(-140); - public static double bx3b = 34, by3b = -58, bh3b = Math.toRadians(-140); + public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this + public static Pose2d teleStart = new Pose2d(rx1, ry1, rh1); } 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 f1b49bf..fe47cfa 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 @@ -26,16 +26,20 @@ public class ServoPositions { public static double hoodAuto = 0.55; - public static double hoodHigh = 0.21; + public static double hoodAutoFar = 0.5; //TODO: change this; - public static double hoodLow = 1.0; + public static double hoodHigh = 0.21; //TODO: change this; - public static double turret_red = 0.42; - public static double turret_blue = 0.38; + public static double hoodLow = 1.0; //TODO: change this; - public static double turret_detectRed = 0.2; + public static double turret_redClose = 0.42; + public static double turret_blueClose = 0.38; + public static double turret_redFar = 0.5; //TODO: change this + public static double turret_blueFar = 0.5; // TODO: change this - public static double turret_detectBlue = 0.6; + public static double turret_detectRedClose = 0.2; + + public static double turret_detectBlueClose = 0.6; public static double turrDefault = 0.40; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java index 4494544..3216fcf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java @@ -20,4 +20,5 @@ public class ShooterVars { // VELOCITY CONSTANTS public static int AUTO_CLOSE_VEL = 3025; //3300; + public static int AUTO_FAR_VEL = 4000; //TODO: test this } \ No newline at end of file From de52f86280a0b7972df0767a19cb79617b4bbb5f Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 13 Jan 2026 19:50:24 -0600 Subject: [PATCH 8/9] fixed some flywheel stuff --- .../firstinspires/ftc/teamcode/teleop/TeleopV3.java | 8 ++++---- .../ftc/teamcode/tests/ShooterTest.java | 12 +++++++----- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index 3381753..777859d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -16,7 +16,7 @@ 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.Flywheel; +import org.firstinspires.ftc.teamcode.utils.FlywheelV2; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Servos; @@ -29,7 +29,7 @@ public class TeleopV3 extends LinearOpMode { Robot robot; MultipleTelemetry TELE; Servos servo; - Flywheel flywheel; + FlywheelV2 flywheel; MecanumDrive drive; public static double manualVel = 3000; @@ -99,7 +99,7 @@ public class TeleopV3 extends LinearOpMode { robot = new Robot(hardwareMap); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); servo = new Servos(hardwareMap); - flywheel = new Flywheel(); + flywheel = new FlywheelV2(); drive = new MecanumDrive(hardwareMap, teleStart); if (redAlliance) { @@ -256,7 +256,7 @@ public class TeleopV3 extends LinearOpMode { //SHOOTER: - double powPID = flywheel.manageFlywheel((int) vel, robot.shooter1.getCurrentPosition()); + double powPID = flywheel.manageFlywheel((int) vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); robot.shooter1.setPower(powPID); robot.shooter2.setPower(powPID); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java index 84dfb72..0a1e35b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java @@ -9,7 +9,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotorEx; -import org.firstinspires.ftc.teamcode.utils.Flywheel; +import org.firstinspires.ftc.teamcode.utils.FlywheelV2; import org.firstinspires.ftc.teamcode.utils.Robot; @Config @@ -26,7 +26,7 @@ public class ShooterTest extends LinearOpMode { public static double turretPos = 0.501; public static boolean shoot = false; Robot robot; - Flywheel flywheel; + FlywheelV2 flywheel; @Override public void runOpMode() throws InterruptedException { @@ -34,7 +34,7 @@ public class ShooterTest extends LinearOpMode { robot = new Robot(hardwareMap); DcMotorEx leftShooter = robot.shooter1; DcMotorEx rightShooter = robot.shooter2; - flywheel = new Flywheel(); + flywheel = new FlywheelV2(); MultipleTelemetry TELE = new MultipleTelemetry( telemetry, FtcDashboard.getInstance().getTelemetry() @@ -50,7 +50,7 @@ public class ShooterTest extends LinearOpMode { rightShooter.setPower(parameter); leftShooter.setPower(parameter); } else if (mode == 1) { - double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition()); + double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); rightShooter.setPower(powPID); leftShooter.setPower(powPID); TELE.addData("PIDPower", powPID); @@ -71,7 +71,9 @@ public class ShooterTest extends LinearOpMode { } else { robot.transferServo.setPosition(transferServo_out); } - TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Velocity", flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition())); + TELE.addData("Velocity 1", flywheel.getVelo1()); + TELE.addData("Velocity 2", flywheel.getVelo2()); TELE.addData("Power", robot.shooter1.getPower()); TELE.addData("Steady?", flywheel.getSteady()); TELE.addData("Position", robot.shooter1.getCurrentPosition()); From c160b3fa6b93d5f26fc578a4d507ff3bdb551d74 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 13 Jan 2026 22:10:16 -0600 Subject: [PATCH 9/9] configuration preparation --- .../ftc/teamcode/tests/PIDServoTest.java | 4 +- .../utils/PositionalServoProgrammer.java | 24 ++++++-- .../ftc/teamcode/utils/Robot.java | 55 ++++--------------- .../ftc/teamcode/utils/Servos.java | 2 +- 4 files changed, 32 insertions(+), 53 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java index ccae676..3286912 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java @@ -46,7 +46,7 @@ public class PIDServoTest extends LinearOpMode { controller.setPIDF(p, i, d, f); if (mode == 0) { - pos = scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3); + pos = robot.turr1Pos.getCurrentPosition(); double pid = controller.calculate(pos, target); @@ -62,7 +62,7 @@ public class PIDServoTest extends LinearOpMode { } telemetry.addData("pos", pos); - telemetry.addData("Turret Voltage", robot.turr1Pos.getVoltage()); + telemetry.addData("Turret Voltage", robot.turr1Pos.getCurrentPosition()); telemetry.addData("Spindex Voltage", robot.spin1Pos.getVoltage()); telemetry.addData("target", target); telemetry.addData("Mode", mode); 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 4f65624..804e535 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 @@ -61,13 +61,25 @@ public class PositionalServoProgrammer extends LinearOpMode { if (hoodPos != 0.501){ robot.hood.setPosition(hoodPos); } - TELE.addData("spindexer", servo.getSpinPos()); - TELE.addData("turret", servo.getTurrPos()); - TELE.addData("spindexer voltage", robot.spin1Pos.getVoltage()); - TELE.addData("hood voltage", robot.hoodPos.getVoltage()); + // To check configuration of spindexer: + // Set "mode" to 1 and spindexPow to 0.1 + // If the spindexer is turning clockwise, the servos are reversed. Swap the configuration of the two servos, DO NOT TOUCH THE ACTUAL CODE + // Do the previous test again to confirm + // Set "mode" to 0 but keep spindexPos at 0.501 + // Manually turn the spindexer until "spindexer pos" is set close to 0 + // Check each spindexer voltage: + // If "spindexer voltage 1" is closer to 0 than "spindexer voltage 2," then you are done! + // If "spindexer voltage 2" is closer to 0 than "spindexer voltage 1," swap the two spindexer analog inputs in the configuration, DO NOT TOUCH THE ACTUAL CODE + //TODO: @KeshavAnandCode do the above please + + TELE.addData("spindexer pos", servo.getSpinPos()); + TELE.addData("turret pos", servo.getTurrPos()); + TELE.addData("spindexer voltage 1", robot.spin1Pos.getVoltage()); + TELE.addData("spindexer voltage 2", robot.spin2Pos.getVoltage()); + TELE.addData("hood pos", robot.hood.getPosition()); TELE.addData("transferServo voltage", robot.transferServoPos.getVoltage()); - TELE.addData("turret voltage", robot.turr1Pos.getVoltage()); - TELE.addData("Spin Equal", servo.spinEqual(spindexPos)); + TELE.addData("turret voltage", robot.turr1Pos.getCurrentPosition()); + TELE.addData("spindexer pow", robot.spin1.getPower()); TELE.update(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index 38000fd..dee7dac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java @@ -7,7 +7,6 @@ import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.DigitalChannel; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; @@ -28,28 +27,16 @@ public class Robot { public DcMotorEx shooter2; public Servo hood; public Servo transferServo; - public Servo rejecter; public CRServo turr1; public CRServo turr2; public CRServo spin1; public CRServo spin2; - public DigitalChannel pin0; - public DigitalChannel pin1; - public DigitalChannel pin2; - public DigitalChannel pin3; - public DigitalChannel pin4; - public DigitalChannel pin5; - public AnalogInput analogInput; - public AnalogInput analogInput2; public AnalogInput spin1Pos; public AnalogInput spin2Pos; - public AnalogInput hoodPos; - public AnalogInput turr1Pos; - public AnalogInput turr2Pos; + public DcMotorEx turr1Pos; public AnalogInput transferServoPos; public AprilTagProcessor aprilTagProcessor; public WebcamName webcam; - public DcMotorEx shooterEncoder; public RevColorSensorV3 color1; public RevColorSensorV3 color2; public RevColorSensorV3 color3; @@ -57,10 +44,12 @@ public class Robot { public static boolean usingLimelight = true; + public static boolean usingCamera = true; + public Robot(HardwareMap hardwareMap) { //Define components w/ hardware map - + //TODO: fix the configuration of these - I trust you to figure it out yourself @KeshavAnandCode frontLeft = hardwareMap.get(DcMotorEx.class, "fl"); frontRight = hardwareMap.get(DcMotorEx.class, "fr"); backLeft = hardwareMap.get(DcMotorEx.class, "bl"); @@ -74,30 +63,24 @@ public class Robot { backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); intake = hardwareMap.get(DcMotorEx.class, "intake"); - rejecter = hardwareMap.get(Servo.class, "rejecter"); shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1"); shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2"); - + //TODO: figure out which shooter motor is reversed using ShooterTest and change it in code @KeshavAnandCode shooter1.setDirection(DcMotorSimple.Direction.REVERSE); shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - shooterEncoder = shooter1; - hood = hardwareMap.get(Servo.class, "hood"); - hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos"); - turr1 = hardwareMap.get(CRServo.class, "t1"); - turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); - turr2 = hardwareMap.get(CRServo.class, "t2"); - turr2Pos = hardwareMap.get(AnalogInput.class, "t2Pos"); + turr1Pos = intake; // Encoder of turret plugged in intake port + //TODO: check spindexer configuration (both servo and analog input) - check comments in PositionalServoProgrammer spin1 = hardwareMap.get(CRServo.class, "spin1"); spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos"); @@ -109,22 +92,6 @@ public class Robot { spin1.setDirection(DcMotorSimple.Direction.REVERSE); spin2.setDirection(DcMotorSimple.Direction.REVERSE); - pin0 = hardwareMap.get(DigitalChannel.class, "pin0"); - - pin1 = hardwareMap.get(DigitalChannel.class, "pin1"); - - pin2 = hardwareMap.get(DigitalChannel.class, "pin2"); - - pin3 = hardwareMap.get(DigitalChannel.class, "pin3"); - - pin4 = hardwareMap.get(DigitalChannel.class, "pin4"); - - pin5 = hardwareMap.get(DigitalChannel.class, "pin5"); - - analogInput = hardwareMap.get(AnalogInput.class, "analog"); - - analogInput2 = hardwareMap.get(AnalogInput.class, "analog2"); - transfer = hardwareMap.get(DcMotorEx.class, "transfer"); transferServo = hardwareMap.get(Servo.class, "transferServo"); @@ -132,10 +99,7 @@ public class Robot { transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos"); transfer.setDirection(DcMotorSimple.Direction.REVERSE); - - aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults(); - - webcam = hardwareMap.get(WebcamName.class, "Webcam 1"); + transfer.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); color1 = hardwareMap.get(RevColorSensorV3.class, "c1"); @@ -145,6 +109,9 @@ public class Robot { if (usingLimelight){ limelight = hardwareMap.get(Limelight3A.class, "limelight"); + } else if (usingCamera){ + webcam = hardwareMap.get(WebcamName.class, "Webcam 1"); + aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults(); } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java index 058565d..967fa78 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java @@ -45,7 +45,7 @@ public class Servos { } public double getTurrPos() { - return turret_scalar * ((robot.turr1Pos.getVoltage() - turret_restPos) / 3.3); + return robot.turr1Pos.getCurrentPosition(); } public double setTurrPos(double pos) {