From 08ba099d5ba0e1ae30e2a619cf5e0ce3138d6ea5 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 7 Apr 2026 19:12:34 -0500 Subject: [PATCH] area code --- .../teamcode/autonomous/Auto_LT_Close.java | 2 +- .../ftc/teamcode/autonomous/Auto_LT_Far.java | 87 ++++++++----- .../ftc/teamcode/constants/Back_Poses.java | 12 +- .../ftc/teamcode/teleop/TeleopV3.java | 118 +++++++++--------- .../ftc/teamcode/utils/Drivetrain.java | 8 +- .../ftc/teamcode/utils/Turret.java | 5 +- 6 files changed, 129 insertions(+), 103 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index 4aef555..8b8b5a2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java @@ -503,7 +503,7 @@ public class Auto_LT_Close extends LinearOpMode { if (gateCycle) { - startAutoGate(); + startAuto(); shoot(0.501, 0.501, 0.501); cycleStackClose(); shoot(0.501,0.501, 0.501); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java index 1ee944f..763d938 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java @@ -79,8 +79,8 @@ public class Auto_LT_Far extends LinearOpMode { public static double shootStackTime = 2; public static double shootGateTime = 2.5; public static double colorSenseTime = 1; - public static double intakeStackTime = 2.5; - public static double intakeGateTime = 2; + public static double intakeStackTime = 5; + public static double intakeGateTime = 3.75; double obeliskTurrPos1 = 0.0; double obeliskTurrPos2 = 0.0; double obeliskTurrPos3 = 0.0; @@ -122,6 +122,8 @@ public class Auto_LT_Far extends LinearOpMode { robot.limelight.pipelineSwitch(1); turret = new Turret(robot, TELE, robot.limelight); + limelightUsed = false; + // Spindexer.teleop = false; @@ -190,6 +192,7 @@ public class Auto_LT_Far extends LinearOpMode { turret.setTurret(turrDefault); servos.setSpinPos(spinStartPos); + limelightUsed = true; servos.setTransferPos(transferServo_out); } @@ -231,6 +234,7 @@ public class Auto_LT_Far extends LinearOpMode { gamepad2.rumble(500); sleep(1000); turret.setTurret(turrDefault); + limelightUsed = true; servos.setSpinPos(spinStartPos); @@ -259,10 +263,10 @@ public class Auto_LT_Far extends LinearOpMode { shootGate = drive.actionBuilder(new Pose2d(pickupGateX, pickupGateY, Math.toRadians(pickupGateH))) .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); - limelightUsed = true; TELE.addData("Red?", redAlliance); TELE.addData("Turret Default", turrDefault); + TELE.addData("Limelight On?",limelightUsed); TELE.addData("Gate Cycle?", gatePickup); TELE.addData("Pickup Stack?", stack3); TELE.addData("Start Position", autoStart); @@ -281,11 +285,17 @@ public class Auto_LT_Far extends LinearOpMode { robot.transfer.setPower(1); startAuto(); - shoot(); + if (redAlliance){ + shoot(autoStartRX, autoStartRY, autoStartRH); + + } else { + shoot(autoStartBX, autoStartBY, autoStartBH); + + } if (stack3){ cycleStackFar(); - shoot(); + shoot(xShoot, yShoot, hShoot); } while (gatePickup && getRuntime() - stamp < endAutoTime){ @@ -294,10 +304,7 @@ public class Auto_LT_Far extends LinearOpMode { break; } cycleGatePrepareShoot(); - if (getRuntime() - stamp > endAutoTime + shootAllTime + 1){ - break; - } - shoot(); + shoot(xShoot, yShoot, hShoot); } if (gatePickup || stack3){ @@ -324,28 +331,46 @@ public class Auto_LT_Far extends LinearOpMode { } - void shoot(){ + void shoot(double x, double y, double z){ Actions.runBlocking( new ParallelAction( - autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease, 0.501, 0.501, 0.501) + autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease, x, y, z) ) ); } void startAuto(){ - Actions.runBlocking( - new ParallelAction( - autoActions.manageShooterAuto( - flywheel0Time, - 0.501, - 0.501, - 0.501, - true - ) - ) - ); + if (redAlliance){ + Actions.runBlocking( + new ParallelAction( + autoActions.manageShooterAuto( + flywheel0Time, + autoStartRX, + autoStartRY, + autoStartRH, + true + ) + + ) + ); + + } else { + Actions.runBlocking( + new ParallelAction( + autoActions.manageShooterAuto( + flywheel0Time, + autoStartBX, + autoStartBY, + autoStartBH, + true + ) + + ) + ); + } + } void leave3Ball(){ @@ -371,9 +396,13 @@ public class Auto_LT_Far extends LinearOpMode { ) ); servos.setSpinPos(spinStartPos); + spindexer.setIntakePower(-0.1); Actions.runBlocking( new ParallelAction( - shoot3.build() + shoot3.build(), + autoActions.manageShooterAuto( + shootStackTime,xShoot, yShoot, hShoot, false + ) ) ); } @@ -383,7 +412,7 @@ public class Auto_LT_Far extends LinearOpMode { new ParallelAction( pickupGate.build(), autoActions.intake( - intakeStackTime, + intakeGateTime, pickupGateX, pickupGateY, pickupGateH @@ -393,16 +422,16 @@ public class Auto_LT_Far extends LinearOpMode { } void cycleGatePrepareShoot(){ + spindexer.setIntakePower(-0.1); Actions.runBlocking( new ParallelAction( shootGate.build(), - autoActions.prepareShootAll( - colorSenseTime, - shootGateTime, - motif, + autoActions.manageShooterAuto( + shootGateTime, xShoot, yShoot, - hShoot + hShoot, + false ) ) ); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java index 1f6a8a4..472fd95 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java @@ -10,11 +10,11 @@ public class Back_Poses { public static double rShootX = 100, rShootY = 60, rShootH = 125.2; public static double bShootX = 100, bShootY = -60, bShootH = -125.2; - public static double rStackPickupFarAX = 73, rStackPickupFarAY = 51, rStackPickupFarAH = 145; - public static double bStackPickupFarAX = 73, bStackPickupFarAY = -51, bStackPickupFarAH = -145; + public static double rStackPickupFarAX = 75, rStackPickupFarAY = 45, rStackPickupFarAH = 150; + public static double bStackPickupFarAX = 75, bStackPickupFarAY = -45, bStackPickupFarAH = -150; - public static double rStackPickupFarBX = 53, rStackPickupFarBY = 71, rStackPickupFarBH = 145.1; - public static double bStackPickupFarBX = 55, bStackPickupFarBY = -71, bStackPickupFarBH = -145.1; + public static double rStackPickupFarBX = 45, rStackPickupFarBY = 80, rStackPickupFarBH = 145.1; + public static double bStackPickupFarBX = 45, bStackPickupFarBY = -80, bStackPickupFarBH = -145.1; public static double rStackPickupMiddleAX = 55, rStackPickupMiddleAY = 39, rStackPickupMiddleAH = 145; public static double bStackPickupMiddleAX = 55, bStackPickupMiddleAY = -39, bStackPickupMiddleAH = -145; @@ -22,8 +22,8 @@ public class Back_Poses { public static double rStackPickupMiddleBX = 45, rStackPickupMiddleBY = 49, rStackPickupMiddleBH = 145.1; public static double bStackPickupMiddleBX = 45, bStackPickupMiddleBY = -49, bStackPickupMiddleBH = -145.1; - public static double rPickupGateXA = 60, rPickupGateYA = 83, rPickupGateHA = 145; - public static double bPickupGateXA = 60, bPickupGateYA = -83, bPickupGateHA = -145; + public static double rPickupGateXA = 60, rPickupGateYA = 90, rPickupGateHA = 140; + public static double bPickupGateXA = 60, bPickupGateYA = -90, bPickupGateHA = -140; public static double rPickupGateXB = 84, rPickupGateYB = 76, rPickupGateHB = 145; public static double bPickupGateXB = 84, bPickupGateYB = -76, bPickupGateHB = -145; public static double rPickupGateXC = 50, rPickupGateYC = 83, rPickupGateHC = 190; 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 a92bc8b..4fd769b 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 @@ -142,6 +142,62 @@ public class TeleopV3 extends LinearOpMode { while (opModeIsActive()) { //TELE.addData("Is limelight on?", robot.limelight.getStatus()); + drive.updatePoseEstimate(); + Pose2d currentPose = drive.localizer.getPose(); + + if (enableSpindexerManager) { + //if (!shootAll) { + spindexer.processIntake(); + //} + + // RIGHT_BUMPER + if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) { + spindexer.setIntakePower(1); + } else if (gamepad1.cross) { + spindexer.setIntakePower(-1); + } else { + spindexer.setIntakePower(0); + } + + // LEFT_BUMPER + if (!shootAll && gamepad1.leftBumperWasReleased()) { + shootStamp = getRuntime(); + shootAll = true; + + shooterTicker = 0; + + } + intakeTicker++; + if (shootAll) { + intakeTicker = 0; + intake = false; + reject = false; + + if (shooterTicker == 0) { + spindexer.prepareShootAllContinous(); + //TELE.addLine("preparing to shoot"); +// } else if (shooterTicker == 2) { +// //servo.setTransferPos(transferServo_in); +// spindexer.shootAll(); +// TELE.addLine("starting to shoot"); + } else if (spindexer.shootAllComplete()) { + //spindexPos = spindexer_intakePos1; + shootAll = false; + spindexer.resetSpindexer(); + //spindexer.processIntake(); + //TELE.addLine("stop shooting"); + } + shooterTicker++; + //spindexer.processIntake(); + } + + if (gamepad1.left_stick_button) { + servo.setTransferPos(transferServo_out); + //spindexPos = spindexer_intakePos1; + shootAll = false; + spindexer.resetSpindexer(); + } + } //DRIVETRAIN: @@ -169,9 +225,9 @@ public class TeleopV3 extends LinearOpMode { //TURRET TRACKING - double robX = drive.localizer.getPose().position.x; - double robY = drive.localizer.getPose().position.y; - double robH = drive.localizer.getPose().heading.toDouble(); + double robX = currentPose.position.x; + double robY = currentPose.position.y; + double robH = currentPose.heading.toDouble(); double robotX = robX + xOffset; double robotY = robY + yOffset; @@ -278,63 +334,7 @@ public class TeleopV3 extends LinearOpMode { shootAllSpindexerSpeedIncrease = shootAllSpindexerSpeedIncrease+0.01; } - - if (enableSpindexerManager) { - //if (!shootAll) { - spindexer.processIntake(); - //} - - // RIGHT_BUMPER - if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) { - spindexer.setIntakePower(1); - } else if (gamepad1.cross) { - spindexer.setIntakePower(-1); - } else { - spindexer.setIntakePower(0); - } - - // LEFT_BUMPER - if (!shootAll && gamepad1.leftBumperWasReleased()) { - shootStamp = getRuntime(); - shootAll = true; - - shooterTicker = 0; - - } - intakeTicker++; - if (shootAll) { - intakeTicker = 0; - intake = false; - reject = false; - - if (shooterTicker == 0) { - spindexer.prepareShootAllContinous(); - //TELE.addLine("preparing to shoot"); -// } else if (shooterTicker == 2) { -// //servo.setTransferPos(transferServo_in); -// spindexer.shootAll(); -// TELE.addLine("starting to shoot"); - } else if (spindexer.shootAllComplete()) { - //spindexPos = spindexer_intakePos1; - shootAll = false; - spindexer.resetSpindexer(); - //spindexer.processIntake(); - //TELE.addLine("stop shooting"); - } - shooterTicker++; - //spindexer.processIntake(); - } - - if (gamepad1.left_stick_button) { - servo.setTransferPos(transferServo_out); - //spindexPos = spindexer_intakePos1; - shootAll = false; - spindexer.resetSpindexer(); - } - } - //EXTRA STUFFINESS: - drive.updatePoseEstimate(); for (LynxModule hub : allHubs) { hub.clearBulkCache(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java index f843d89..edd598a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java @@ -71,6 +71,8 @@ public class Drivetrain { robot.frontRight.setPower(frontRightPower); robot.backRight.setPower(backRightPower); } + Pose2d currentPos = drive.localizer.getPose(); + brakePos = currentPos; if (brake > 0.4 && robot.frontLeft.getZeroPowerBehavior() != DcMotor.ZeroPowerBehavior.BRAKE && !autoDrive) { robot.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -78,15 +80,9 @@ public class Drivetrain { robot.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); robot.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - drive.updatePoseEstimate(); - - brakePos = drive.localizer.getPose(); autoDrive = true; } else if (brake > 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); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java index b2cdfff..96ddb29 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -242,6 +242,7 @@ public class Turret { return bearingOffset; } + double targetTurretPos; public void trackGoal(Pose2d deltaPos) { /* ---------------- FIELD → TURRET GEOMETRY ---------------- */ @@ -273,7 +274,7 @@ public class Turret { limelightRead(); // Active correction if we see the target - if (result.isValid() && !lockOffset && limelightUsed) { + if (result.isValid() && !lockOffset && limelightUsed && targetTurretPos > turrMin && targetTurretPos < turrMax) { currentTrackOffset += bearingAlign(result); currentTrackCount++; @@ -329,7 +330,7 @@ public class Turret { /* ---------------- ANGLE → SERVO POSITION ---------------- */ - double targetTurretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360); + targetTurretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360); // Clamp to physical servo limits targetTurretPos = Math.max(turrMin, Math.min(targetTurretPos, turrMax));