From 66e76285b283a625bf626cb145295d10cdd67040 Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Wed, 14 Jan 2026 22:57:32 -0600 Subject: [PATCH] update --- .../ftc/teamcode/teleop/TeleopV3.java | 1192 +++++++++-------- 1 file changed, 637 insertions(+), 555 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 777859d..d404815 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 @@ -1,18 +1,22 @@ 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.*; +import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; +import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; 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.acmerobotics.roadrunner.ProfileAccelConstraint; +import com.acmerobotics.roadrunner.TrajectoryActionBuilder; +import com.acmerobotics.roadrunner.TranslationalVelConstraint; +import com.acmerobotics.roadrunner.Vector2d; +import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; @@ -26,16 +30,12 @@ import java.util.List; @Config @TeleOp public class TeleopV3 extends LinearOpMode { - Robot robot; - MultipleTelemetry TELE; - Servos servo; - FlywheelV2 flywheel; - MecanumDrive drive; - public static double manualVel = 3000; + public static int intakeJamSwap = 12; public static double hoodDefaultPos = 0.5; public static double desiredTurretAngle = 180; public static double shootStamp2 = 0.0; + public static double spinningPow = 0.15; public double vel = 3000; public boolean autoVel = true; public double manualOffset = 0.0; @@ -47,6 +47,13 @@ public class TeleopV3 extends LinearOpMode { public boolean circle = false; public boolean square = false; public boolean triangle = false; + public TranslationalVelConstraint VEL_CONSTRAINT = new TranslationalVelConstraint(200); + public ProfileAccelConstraint ACCEL_CONSTRAINT = new ProfileAccelConstraint(-Math.abs(60), 200); + Robot robot; + MultipleTelemetry TELE; + Servos servo; + FlywheelV2 flywheel; + MecanumDrive drive; double autoHoodOffset = 0.0; boolean intake = false; boolean reject = false; @@ -75,18 +82,20 @@ public class TeleopV3 extends LinearOpMode { 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; + double spinCurrentPos = 0.0, spinInitPos = 0.0, intakeStamp = 0.0; boolean reverse = false; int intakeTicker = 0; + Pose2d brakePos = new Pose2d(0, 0, 0); + boolean autoDrive = false; + private boolean shootAll = false; + private double transferStamp = 0.0; + private int tickerA = 1; + private boolean transferIn = false; @Override public void runOpMode() throws InterruptedException { @@ -115,49 +124,87 @@ public class TeleopV3 extends LinearOpMode { 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; + if (!autoDrive) { - 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; + 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; - robot.frontLeft.setPower(frontLeftPower); - robot.backLeft.setPower(backLeftPower); - robot.frontRight.setPower(frontRightPower); - robot.backRight.setPower(backRightPower); + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + double frontLeftPower = (y + x + rx) / denominator; + double backLeftPower = (y - x + rx) / denominator; + double frontRightPower = (y - x - rx) / denominator; + double backRightPower = (y + x - rx) / denominator; - // 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); + robot.frontLeft.setPower(frontLeftPower); + robot.backLeft.setPower(backLeftPower); + robot.frontRight.setPower(frontRightPower); + robot.backRight.setPower(backRightPower); } - // INTAKE + if (gamepad1.left_trigger > 0.4 && robot.frontLeft.getZeroPowerBehavior() != DcMotor.ZeroPowerBehavior.BRAKE && !autoDrive) { + robot.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + drive.updatePoseEstimate(); + + brakePos = drive.localizer.getPose(); + autoDrive = true; + + } else if (gamepad1.left_trigger > 0.4) { + drive.updatePoseEstimate(); + + Pose2d currentPos = drive.localizer.getPose(); + + TrajectoryActionBuilder traj2 = drive.actionBuilder(currentPos) + .strafeToLinearHeading(new Vector2d(brakePos.position.x, brakePos.position.y), brakePos.heading.toDouble(), VEL_CONSTRAINT, ACCEL_CONSTRAINT); + + if (Math.abs(currentPos.position.x - brakePos.position.x) > 1 || Math.abs(currentPos.position.y - brakePos.position.y) > 1) { + Actions.runBlocking( + traj2.build() + ); + } + } else { + autoDrive = false; + robot.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + robot.frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + robot.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + robot.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + + } + +// // 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); +// } + + //TODO: Use color sensors to switch between positions...switch after ball detected in + if (gamepad1.right_bumper) { - robot.transferServo.setPosition(transferServo_out); intakeTicker++; - if (intakeTicker % 16 == 0) { + if (intakeTicker % 3 == 0) { spinCurrentPos = servo.getSpinPos(); - if (Math.abs(spinCurrentPos - spinInitPos) == 0.0) { - reverse = !reverse; + if (Math.abs(spinCurrentPos - spinInitPos) < 0.02) { + reverse = true; + } else { + reverse = false; } spinInitPos = spinCurrentPos; } - if (reverse) { - robot.spin1.setPower(spinningPow); - robot.spin2.setPower(-spinningPow); + if (reverse && intakeTicker % intakeJamSwap < (intakeJamSwap/2)) { + robot.spin1.setPower(1); + robot.spin2.setPower(-1); + } else if (reverse) { + robot.spin1.setPower(-1); + robot.spin2.setPower(1); } else { robot.spin1.setPower(-spinningPow); robot.spin2.setPower(spinningPow); @@ -166,19 +213,54 @@ public class TeleopV3 extends LinearOpMode { 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 { + robot.spin1.setPower(0); + robot.spin2.setPower(0); + if (getRuntime() - intakeStamp < 1) { robot.intake.setPower(-(getRuntime() - intakeStamp) * 2); } else { robot.intake.setPower(0); } + intakeTicker = 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); +// +// } 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); @@ -254,372 +336,372 @@ public class TeleopV3 extends LinearOpMode { green3 = checkGreen(s3, s3T); } - //SHOOTER: +// //SHOOTER: +// +// double powPID = flywheel.manageFlywheel((int) vel, robot.shooter1.getCurrentPosition(), robot.shooter2.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; +// +// 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; +// } - double powPID = flywheel.manageFlywheel((int) vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + //EXTRA STUFFINESS: - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); + drive.updatePoseEstimate(); - 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; + for (LynxModule hub : allHubs) { + hub.clearBulkCache(); } +// +// TELE.addData("Spin1Green", green1 + ": " + ballIn(1)); +// TELE.addData("Spin2Green", green2 + ": " + ballIn(2)); +// TELE.addData("Spin3Green", green3 + ": " + ballIn(3)); - //TODO: test the camera teleop code - double pos = turrDefault + (error / 8); // adds the overall error to the default + 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("offset", offset); + TELE.addData("shootOrder", shootOrder); + TELE.addData("oddColor", oddBallColor); - pos -= offset * (0.9 / 360); + TELE.update(); - 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; - - 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++; - } + ticker++; } } - // Helper method + + // Helper methods private boolean checkGreen(List s, List sT) { if (s.isEmpty()) return false; @@ -640,161 +722,161 @@ public class TeleopV3 extends LinearOpMode { 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 - } - + // +// 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; + 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)); - } - } +// 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)); +// } }