From d88b732a11e3f35289ceb7b759654ded4f767aa0 Mon Sep 17 00:00:00 2001 From: Professor348 <141444315+professor348@users.noreply.github.com> Date: Mon, 16 Feb 2026 19:35:41 -0500 Subject: [PATCH 1/4] New gate tweaks, lights work, path tuning --- .../LOADCode/Main_/Auto_/Auto_Main_.java | 45 +++++++++++-- .../Main_/Hardware_/Actuators_/Devices.java | 26 ++++--- .../Main_/Hardware_/Actuators_/Turret.java | 25 +++++-- .../Hardware_/Drivetrain_/Pedro_Paths.java | 29 +++++--- .../Main_/Hardware_/LoadHardwareClass.java | 2 +- .../Main_/Hardware_/LoadPrismLights.java | 2 +- .../LOADCode/Main_/Teleop_/Teleop_Main_.java | 18 ++++- .../Main_/Teleop_/Teleop_Tuning_.java | 67 +++++++++---------- 8 files changed, 143 insertions(+), 71 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java index a454bdb146db..6235f109f5a6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java @@ -20,6 +20,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import dev.nextftc.core.commands.delays.Delay; +import dev.nextftc.core.commands.delays.WaitUntil; import dev.nextftc.core.commands.groups.ParallelRaceGroup; import dev.nextftc.core.commands.groups.SequentialGroup; import dev.nextftc.core.commands.utility.InstantCommand; @@ -64,7 +65,8 @@ public void onInit() { new Near_12Ball(), new Near_9Ball(), new Far_9Ball(), - new Far_6Ball() + new Far_6Ball(), + new test_auto() )); prompter.onComplete(() -> { selectedAlliance = prompter.get("alliance"); @@ -97,7 +99,7 @@ public void onWaitForStart() { @Override public void onStartButtonPressed() { - Robot.lights.setAllianceDisplay(selectedAlliance); + Robot.lights.setSolidAllianceDisplay(selectedAlliance); // Schedule the proper auto selectedAuto.runAuto(); turretOn = selectedAuto.getTurretEnabled(); @@ -113,11 +115,7 @@ public void onUpdate() { telemetry.addData("Alliance", selectedAlliance); Robot.turret.updateAimbot(turretOn, true, 0); Robot.turret.updateFlywheel(); - MecanumDrivetrainClass.robotPose = Robot.drivetrain.follower.getPose(); - - telemetry.addLine(); - telemetry.addData("Mirror -90", Math.toDegrees(paths.autoMirror(new Pose(0,0,-90)).getHeading())); telemetry.update(); } @@ -343,4 +341,39 @@ public void runAuto(){ @Override public String toString(){return "MOE 365 Far Zone Auto";} } + + private class test_auto extends Auto{ + + @Override + Pose getStartPose() { + return paths.farStart; + } + + @Override + boolean getTurretEnabled() { + return false; + } + + @Override + void runAuto() { + new SequentialGroup( + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.farStart_to_farShoot, true, 1), + new WaitUntil(() -> gamepad2.bWasPressed()), + Commands.runPath(paths.farShoot_to_rampIntake, true, 1), + new WaitUntil(() -> gamepad2.bWasPressed()), + Commands.runPath(paths.rampIntake_to_farShoot, true, 1), + new WaitUntil(() -> gamepad2.bWasPressed()), + Commands.runPath(paths.farShoot_to_hpPreload, true, 1), + new WaitUntil(() -> gamepad2.bWasPressed()), + Commands.runPath(paths.hpPreload_to_farShoot, true, 1) + ).schedule(); + } + + @NonNull + @Override + public String toString() { + return "Test Auto"; + } + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java index f0a2d149add2..b5b800a67fd0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java @@ -17,7 +17,6 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadHardwareClass; import org.firstinspires.ftc.teamcode.Prism.Color; -import org.firstinspires.ftc.teamcode.Prism.Direction; import org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver; import org.firstinspires.ftc.teamcode.Prism.PrismAnimations; @@ -372,34 +371,43 @@ public void init(@NonNull OpMode opmode, int stripLength){ } public void setStripSolidColor(Color color){ + clearStripAnimations(); prism.insertAndUpdateAnimation(GoBildaPrismDriver.LayerHeight.LAYER_0, new PrismAnimations.Solid(color, stripBrightness)); } + public void setStripBlinkingColor(Color color){ + clearStripAnimations(); + prism.insertAndUpdateAnimation(GoBildaPrismDriver.LayerHeight.LAYER_0, new PrismAnimations.Blink(color, new Color(0,0,0))); + } + public void setStripRainbow(){ - PrismAnimations.AnimationBase colorA = new PrismAnimations.Rainbow(Direction.Backward); - colorA.setIndexes(0, 12); - PrismAnimations.AnimationBase colorB = new PrismAnimations.Rainbow(Direction.Forward); - colorB.setIndexes(13, 24); - prism.insertAndUpdateAnimation(GoBildaPrismDriver.LayerHeight.LAYER_1, colorA); - prism.insertAndUpdateAnimation(GoBildaPrismDriver.LayerHeight.LAYER_2, colorB); + clearStripAnimations(); + prism.insertAndUpdateAnimation(GoBildaPrismDriver.LayerHeight.LAYER_1, new PrismAnimations.Rainbow()); } public void clearStripAnimations(){ prism.clearAllAnimations(); + prism.updateAllAnimations(); } public void setStripBrightness(int brightness){ stripBrightness = brightness; } - public void setAllianceDisplay(LoadHardwareClass.Alliance alliance){ - clearStripAnimations(); + public void setSolidAllianceDisplay(LoadHardwareClass.Alliance alliance){ if (alliance == LoadHardwareClass.Alliance.RED){ setStripSolidColor(Color.RED); }else if (alliance == LoadHardwareClass.Alliance.BLUE){ setStripSolidColor(Color.BLUE); } } + public void setBlinkingAllianceDisplay(LoadHardwareClass.Alliance alliance){ + if (alliance == LoadHardwareClass.Alliance.RED){ + setStripBlinkingColor(Color.RED); + }else if (alliance == LoadHardwareClass.Alliance.BLUE){ + setStripBlinkingColor(Color.BLUE); + } + } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java index 8a0f1ff92317..a13f0151627b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java @@ -37,13 +37,13 @@ public class Turret { //public static PIDCoefficients flywheelCoefficients = new PIDCoefficients(0.0002, 0, 0); // 4500 RPM public static PIDCoefficients flywheelCoefficients4200 = new PIDCoefficients(0.0004, 0, 0); // 4200 RPM public static PIDCoefficients flywheelCoefficients3500 = new PIDCoefficients(0.00025, 0, 0); // 3500 RPM - //public static PIDCoefficients flywheelCoefficients = new PIDCoefficients(0.00025, 0, 0); // 3000 RPM + public static PIDCoefficients flywheelCoefficients3000 = new PIDCoefficients(0.00025, 0, 0); // 3000 RPM // Flywheel FF coefficients for various speeds //public static BasicFeedforwardParameters flywheelFFCoefficients = new BasicFeedforwardParameters(0.000026,0,0); // 4500 RPM - public static BasicFeedforwardParameters flywheelFFCoefficients4200 = new BasicFeedforwardParameters(0.000032,0,0); // 4200 RPM - public static BasicFeedforwardParameters flywheelFFCoefficients3500 = new BasicFeedforwardParameters(0.000032,0,0); // 3500 RPM - //public static BasicFeedforwardParameters flywheelFFCoefficients = new BasicFeedforwardParameters(0.000031,0,0); // 3000 RPM + public static BasicFeedforwardParameters flywheelFFCoefficients4200 = new BasicFeedforwardParameters(0.0000328,0,0); // 4200 RPM + public static BasicFeedforwardParameters flywheelFFCoefficients3500 = new BasicFeedforwardParameters(0.0000323,0,0); // 3500 RPM + public static BasicFeedforwardParameters flywheelFFCoefficients3000 = new BasicFeedforwardParameters(0.000031,0,0); // 3000 RPM // Actual Flywheel Coefficients private PIDCoefficients actualFlywheelCoefficients = flywheelCoefficients3500; @@ -64,6 +64,7 @@ public enum flywheelState { public flywheelState flywheelMode = flywheelState.OFF; double targetRPM = 0; /** Controls the target speed of the flywheel when it is on.*/ + public static double flywheelReallyNearSpeed = 3000; public static double flywheelNearSpeed = 3300; public static double flywheelFarSpeed = 4200; /** Controls the upper software limit of the hood.*/ @@ -287,7 +288,7 @@ public Pose calcGoalPose(){ */ public void setGateState(gatestate state){ if (state == gatestate.CLOSED){ - gate.setAngle(0.47); + gate.setAngle(0.534); }else if (state == gatestate.OPEN){ gate.setAngle(0.5); } @@ -347,6 +348,9 @@ private void setFlywheelRPM(double rpm){ public double getFlywheelRPM(){ return flywheel.getRPM(); } + public boolean isFlywheelReady(){ + return flywheel.getRPM() > getFlywheelCurrentMaxSpeed(); + } /** * Sets the target state of the Flywheel.
@@ -377,17 +381,24 @@ public boolean zeroTurret(){ /** * Updates the flywheel PID. Must be called every loop. */ - public void updateFlywheel(){ + public void updateFlywheel() { robotZone.setPosition(Robot.drivetrain.follower.getPose().getX(), Robot.drivetrain.follower.getPose().getY()); robotZone.setRotation(Robot.drivetrain.follower.getPose().getHeading()); + Pose goalPose = new Pose(0,144,0); + if (LoadHardwareClass.selectedAlliance == LoadHardwareClass.Alliance.RED) {goalPose = new Pose(144, 144, 0);} + opMode.telemetry.addData("In Far Zone", robotZone.isInside(LoadHardwareClass.FarLaunchZone)); opMode.telemetry.addData("In Near Zone", robotZone.isInside(LoadHardwareClass.ReallyNearLaunchZoneRed)); - if (robotZone.isInside(LoadHardwareClass.FarLaunchZone)){ + if (robotZone.isInside(LoadHardwareClass.FarLaunchZone)) { targetRPM = flywheelFarSpeed; actualFlywheelCoefficients = flywheelCoefficients4200; actualFlywheelFFCoefficients = flywheelFFCoefficients4200; + }else if (Robot.drivetrain.distanceFromGoal() < 60){ + targetRPM = flywheelReallyNearSpeed; + actualFlywheelCoefficients = flywheelCoefficients3000; + actualFlywheelFFCoefficients = flywheelFFCoefficients3000; }else{ targetRPM = flywheelNearSpeed; actualFlywheelCoefficients = flywheelCoefficients3500; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java index a94d1a2ae09e..3fcf45497fee 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java @@ -23,8 +23,8 @@ public class Pedro_Paths { public Pose nearPreload = new Pose(124.000, 83.500, Math.toRadians(0)); public Pose midPreload = new Pose(130.000, 59.500, Math.toRadians(0)); public Pose farPreload = new Pose(130.000, 35.500, Math.toRadians(0)); - public Pose hpPreload = new Pose(135, 8, -90); - public Pose rampIntake = new Pose(135, 40, 80); + public Pose hpPreload = new Pose(136, 9, Math.toRadians(-90)); + public Pose rampIntake = new Pose(135, 40, Math.toRadians(80)); public Pose hpIntake = null; // Shooting Poses public Pose nearShoot = new Pose(115, 120, Math.toRadians(-35)); @@ -490,10 +490,15 @@ public void buildShootingsToHPPreload(){ farShoot_to_hpPreload = follower.pathBuilder() .addPath(new BezierCurve( farShoot, - autoMirror(new Pose(136, 30)), - hpPreload + autoMirror(new Pose(120, 60)), + autoMirror(new Pose(136, 20)) )) .setLinearHeadingInterpolation(farShoot.getHeading(), hpPreload.getHeading()) + .addPath(new BezierLine( + autoMirror(new Pose(136, 20)), + hpPreload + )) + .setConstantHeadingInterpolation(hpPreload.getHeading()) .build(); midShoot_to_hpPreload = follower.pathBuilder() .addPath(new BezierCurve( @@ -511,11 +516,11 @@ public void buildShootingsToRampIntake(){ midShoot, autoMirror(new Pose(85, 40)), autoMirror(new Pose(137, 2)), - autoMirror(new Pose(135, 30, rampIntake.getHeading())) + autoMirror(new Pose(135, 30)) )) .setLinearHeadingInterpolation(midShoot.getHeading(), rampIntake.getHeading()) .addPath(new BezierLine( - autoMirror(new Pose(135, 30, rampIntake.getHeading())), + autoMirror(new Pose(135, 30)), rampIntake )) .setConstantHeadingInterpolation(rampIntake.getHeading()) @@ -524,20 +529,21 @@ public void buildShootingsToRampIntake(){ .addPath(new BezierCurve( farShoot, autoMirror(new Pose(137, 2)), - autoMirror(new Pose(135, 30, rampIntake.getHeading())) + autoMirror(new Pose(135, 30)) )) - .setLinearHeadingInterpolation(farShoot.getHeading(), rampIntake.getHeading()) + .setLinearHeadingInterpolation(farShoot.getHeading(), rampIntake.getHeading()-Math.toRadians(10)) .addPath(new BezierLine( - autoMirror(new Pose(135, 30, rampIntake.getHeading())), + autoMirror(new Pose(135, 30)), rampIntake )) - .setConstantHeadingInterpolation(rampIntake.getHeading()) + .setLinearHeadingInterpolation(rampIntake.getHeading()-Math.toRadians(10), rampIntake.getHeading()) .build(); } public void buildHPPreloadToShootings(){ hpPreload_to_farShoot = follower.pathBuilder() - .addPath(new BezierLine( + .addPath(new BezierCurve( hpPreload, + autoMirror(new Pose(110, 40)), farShoot )) .setLinearHeadingInterpolation(hpPreload.getHeading(), farShoot.getHeading()) @@ -567,6 +573,7 @@ public void buildPaths(Follower follow){ nearPreload = autoMirror(nearPreload); midPreload = autoMirror(midPreload); farPreload = autoMirror(farPreload); + rampIntake = autoMirror(rampIntake); hpPreload = autoMirror(hpPreload); nearShoot = autoMirror(nearShoot); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java index d1c3d3453d67..3d8554a8090b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java @@ -110,7 +110,7 @@ public void init(Pose initialPose) { drivetrain.init(myOpMode, initialPose); turret.init(myOpMode, this); intake.init(myOpMode); - lights.init(myOpMode, 24); + lights.init(myOpMode, 36); // Misc telemetry myOpMode.telemetry.addData(">", "Hardware Initialized"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadPrismLights.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadPrismLights.java index 667aceca18b4..f2bf2ef1fe92 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadPrismLights.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadPrismLights.java @@ -11,7 +11,7 @@ public class LoadPrismLights { public void init(OpMode opMode, String name){ prism = opMode.hardwareMap.get(GoBildaPrismDriver.class, name); - prism.setStripLength(24); + prism.setStripLength(36); } public void setStripsRainbow(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java index 3fc8362109ba..9213a1f54c3e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java @@ -66,6 +66,7 @@ public class Teleop_Main_ extends LinearOpMode { // Declare OpMode members. private final ElapsedTime runtime = new ElapsedTime(); + private final ElapsedTime looptime = new ElapsedTime(); private final TelemetryManager panelsTelemetry = PanelsTelemetry.INSTANCE.getTelemetry(); public int shootingState = 0; @@ -79,6 +80,8 @@ public class Teleop_Main_ extends LinearOpMode { public Pose holdPoint = new Pose(72, 72, 90); public Boolean holdJustTriggered = false; + public int lightsState = 0; + // Create a new instance of our Robot class LoadHardwareClass Robot = new LoadHardwareClass(this); // Create a new Paths instance @@ -158,10 +161,11 @@ public void runOpMode() { Paths.buildPaths(Robot.drivetrain.follower); Robot.drivetrain.startTeleOpDrive(); Robot.intake.setTransfer(transferState.DOWN); - Robot.lights.setAllianceDisplay(selectedAlliance); + Robot.lights.setSolidAllianceDisplay(selectedAlliance); // run until the end of the match (driver presses STOP) while (opModeIsActive()) { + looptime.reset(); if (!Turret.zeroed){ while (!isStopRequested() && Robot.turret.zeroTurret()){ sleep(0); @@ -229,13 +233,23 @@ public void runOpMode() { // System-related Telemetry telemetry.addLine(); + telemetry.addData("Loop Time", looptime); telemetry.addData("Status", "Run Time: " + runtime); telemetry.addData("Version: ", "2/13/25"); telemetry.update(); panelsTelemetry.update(); - if (runtime.time(TimeUnit.SECONDS) > 115){ + if (runtime.time(TimeUnit.SECONDS) > 115 || lightsState == 2){ Robot.lights.setStripRainbow(); + lightsState = 2; + }else{ + if (Robot.turret.isFlywheelReady() && lightsState == 0){ + Robot.lights.setBlinkingAllianceDisplay(selectedAlliance); + lightsState = 1; + }else if (lightsState == 1){ + Robot.lights.setSolidAllianceDisplay(selectedAlliance); + lightsState = 0; + } } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Tuning_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Tuning_.java index e1cdea2eef6f..d636995067a6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Tuning_.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Tuning_.java @@ -37,8 +37,10 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Intake; +import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Turret; +import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Drivetrain_.Pedro_Paths; import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadHardwareClass; -import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadPrismLights; @Configurable @TeleOp(name="Teleop_Tuning_", group="TeleOp") @@ -49,11 +51,7 @@ public class Teleop_Tuning_ extends LinearOpMode { private ElapsedTime loopTimer = new ElapsedTime(); private TelemetryManager panelsTelemetry = PanelsTelemetry.INSTANCE.getTelemetry(); - // Panels variables - public static double hoodTargetPos = 0; - public static int turretTurnIncrement = 2; - int turretTurnMultiplier = 1; - int turretTurnAngle = 0; + double initial = 0; // Contains the start Pose of our robot. This can be changed or saved from the autonomous period. private final Pose startPose = new Pose(88.5,7.8, Math.toRadians(90)); @@ -63,60 +61,61 @@ public void runOpMode() { // Create a new instance of our Robot class LoadHardwareClass Robot = new LoadHardwareClass(this); - LoadPrismLights prism = new LoadPrismLights(); + Pedro_Paths paths = new Pedro_Paths(); // Initialize all hardware of the robot - Robot.init(startPose); + Robot.init(paths.nearStart ); telemetry.addData("Status", "Initialized"); telemetry.update(); - - while (!isStopRequested() && Robot.turret.zeroTurret()){ - sleep(0); - } - Robot.turret.setHood(0); + Robot.turret.setGateState(Turret.gatestate.CLOSED); // Wait for the game to start (driver presses START) waitForStart(); runtime.reset(); - // Begin TeleOp driving - Robot.drivetrain.startTeleOpDrive(); - - prism.init(this, "prism"); + initial = Robot.turret.rotation.getAngleAbsolute(); // run until the end of the match (driver presses STOP) while (opModeIsActive()) { - // Pass the joystick positions to our mecanum drive controller - Robot.drivetrain.pedroMecanumDrive( - gamepad1.left_stick_y, - gamepad1.left_stick_x, - gamepad1.right_stick_x, - true - ); + Robot.turret.rotation.setAngle(initial); - if (gamepad1.aWasPressed()){ - prism.setStripsRainbow(); + if (gamepad2.yWasPressed()) { + if (Robot.turret.flywheelMode == Turret.flywheelState.OFF) { + Robot.turret.setFlywheelState(Turret.flywheelState.ON); + } else { + Robot.turret.setFlywheelState(Turret.flywheelState.OFF); + } } - if (gamepad1.bWasPressed()){ - prism.setStripsRed(); - } - if (gamepad1.xWasPressed()){ - prism.setStripsBlue(); + Robot.turret.updateFlywheel(); + + if (gamepad2.dpad_down){ + Robot.intake.setMode(Intake.intakeMode.INTAKING); + }else{ + Robot.intake.setMode(Intake.intakeMode.OFF); } - if (gamepad1.yWasPressed()){ - prism.clearStrips(); + if (gamepad1.x){ + Robot.turret.setGateState(Turret.gatestate.CLOSED); + }else if (gamepad1.y){ + Robot.turret.setGateState(Turret.gatestate.OPEN); } + + telemetry.addData("gate pos", Robot.turret.getGate()); + + panelsTelemetry.addData("Flywheel Speed", Robot.turret.getFlywheelRPM()); + panelsTelemetry.addData("Flywheel Target", Robot.turret.getFlywheelCurrentMaxSpeed()); + // System-related Telemetry telemetry.addLine(); telemetry.addLine("SYSTEM DATA"); telemetry.addData("Status", "Run Time: " + runtime.toString()); telemetry.addData("Loop Time", loopTimer.toString()); - panelsTelemetry.addData("Loop Time", loopTimer.toString()); telemetry.addData("Version: ", "12/26/25"); telemetry.update(); panelsTelemetry.update(); loopTimer.reset(); + + Robot.turret.updatePIDs(); } } } From 68198566e5d5cc9cbca028e851c2bb4842847fd1 Mon Sep 17 00:00:00 2001 From: Professor348 <141444315+professor348@users.noreply.github.com> Date: Tue, 17 Feb 2026 19:45:48 -0500 Subject: [PATCH 2/4] Finalized lights for Gov. Cup using Artboards and did a bit of hood & turret tuning --- .../Main_/Hardware_/Actuators_/Devices.java | 42 ++++++++--------- .../Main_/Hardware_/Actuators_/Turret.java | 27 ++++++----- .../LOADCode/Main_/Teleop_/Teleop_Main_.java | 45 ++++++++++++++----- .../Main_/Teleop_/Teleop_Tuning_.java | 39 +++++++++------- 4 files changed, 91 insertions(+), 62 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java index b5b800a67fd0..293263251198 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java @@ -16,9 +16,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadHardwareClass; -import org.firstinspires.ftc.teamcode.Prism.Color; import org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver; -import org.firstinspires.ftc.teamcode.Prism.PrismAnimations; import dev.nextftc.control.ControlSystem; import dev.nextftc.control.KineticState; @@ -363,49 +361,45 @@ public static class GoBildaPrismBarClass { // Maximum length of 4 daisy chained strips is 36 (12 + 12 + 6 + 6) // Scrimmage length of 2 daisy chained strips is 24 (12 + 12) + // Artboards current status: + GoBildaPrismDriver.Artboard solidRED = GoBildaPrismDriver.Artboard.ARTBOARD_0; + GoBildaPrismDriver.Artboard solidBLUE = GoBildaPrismDriver.Artboard.ARTBOARD_1; + GoBildaPrismDriver.Artboard blinkingRED = GoBildaPrismDriver.Artboard.ARTBOARD_2; + GoBildaPrismDriver.Artboard blinkingBLUE = GoBildaPrismDriver.Artboard.ARTBOARD_3; + GoBildaPrismDriver.Artboard rainbow = GoBildaPrismDriver.Artboard.ARTBOARD_4; + GoBildaPrismDriver prism; - int stripBrightness = 50; + int stripBrightness = 25; public void init(@NonNull OpMode opmode, int stripLength){ prism = opmode.hardwareMap.get(GoBildaPrismDriver.class, "prism"); prism.setStripLength(stripLength); + prism.clearAllAnimations(); } - public void setStripSolidColor(Color color){ - clearStripAnimations(); - prism.insertAndUpdateAnimation(GoBildaPrismDriver.LayerHeight.LAYER_0, new PrismAnimations.Solid(color, stripBrightness)); + public void setStripBrightness(int brightness){ + stripBrightness = brightness; } - public void setStripBlinkingColor(Color color){ - clearStripAnimations(); - prism.insertAndUpdateAnimation(GoBildaPrismDriver.LayerHeight.LAYER_0, new PrismAnimations.Blink(color, new Color(0,0,0))); + public void setDisplayedArtboard(GoBildaPrismDriver.Artboard board){ + prism.loadAnimationsFromArtboard(board); } public void setStripRainbow(){ - clearStripAnimations(); - prism.insertAndUpdateAnimation(GoBildaPrismDriver.LayerHeight.LAYER_1, new PrismAnimations.Rainbow()); - } - - public void clearStripAnimations(){ - prism.clearAllAnimations(); - prism.updateAllAnimations(); - } - - public void setStripBrightness(int brightness){ - stripBrightness = brightness; + setDisplayedArtboard(rainbow); } public void setSolidAllianceDisplay(LoadHardwareClass.Alliance alliance){ if (alliance == LoadHardwareClass.Alliance.RED){ - setStripSolidColor(Color.RED); + setDisplayedArtboard(solidRED); }else if (alliance == LoadHardwareClass.Alliance.BLUE){ - setStripSolidColor(Color.BLUE); + setDisplayedArtboard(solidBLUE); } } public void setBlinkingAllianceDisplay(LoadHardwareClass.Alliance alliance){ if (alliance == LoadHardwareClass.Alliance.RED){ - setStripBlinkingColor(Color.RED); + setDisplayedArtboard(blinkingRED); }else if (alliance == LoadHardwareClass.Alliance.BLUE){ - setStripBlinkingColor(Color.BLUE); + setDisplayedArtboard(blinkingBLUE); } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java index a13f0151627b..04c25e7c4afe 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java @@ -30,7 +30,7 @@ public class Turret { public final Devices.REVHallEffectSensorClass hall = new Devices.REVHallEffectSensorClass(); // Turret PID coefficients - public static PIDCoefficients turretCoefficients = new PIDCoefficients(0.07, 0.00000000001, 0.003); // 223RPM Motor + public static PIDCoefficients turretCoefficients = new PIDCoefficients(0.018, 0.00000000005, 0.0002); // 223RPM Motor public static PIDCoefficients cameraCoefficients = new PIDCoefficients(0, 0, 0); // Flywheel PID coefficients for various speeds @@ -149,6 +149,7 @@ public void init(OpMode opmode, LoadHardwareClass robot){ hoodLUTnear.add(53.5,108); hoodLUTnear.add(71,168); hoodLUTnear.add(77, 181); + hoodLUTnear.add(84.5, 175); hoodLUTnear.add(88,185); hoodLUTnear.add(94.5,185); hoodLUTnear.add(96, 180); @@ -232,9 +233,9 @@ private void updateRotationalAimbot(){ } }else{ if (LoadHardwareClass.selectedAlliance == LoadHardwareClass.Alliance.RED){ - rotation.setAngle(Math.min(Math.max(0, rotationalAimbotLocalizer()-2), 360)); + rotation.setAngle(Math.min(Math.max(0, rotationalAimbotLocalizer()), 360)); }else{ - rotation.setAngle(Math.min(Math.max(0, rotationalAimbotLocalizer()+2), 360)); + rotation.setAngle(Math.min(Math.max(0, rotationalAimbotLocalizer()), 360)); } } } @@ -263,6 +264,9 @@ public double rotationalAimbotLocalizer (){ ) - Math.toDegrees(Robot.drivetrain.follower.getPose().getHeading()) + 90)%360; } + public static Pose rotationalNearGoalPose = new Pose(8, 136); + public static Pose rotationalFarGoalPose = new Pose(8, 136); + /** * Calculates the proper goal pose * @return a pose of the rotational aimbot's target position. @@ -271,15 +275,18 @@ public Pose calcGoalPose(){ robotZone.setPosition(Robot.drivetrain.follower.getPose().getX(), Robot.drivetrain.follower.getPose().getY()); robotZone.setRotation(Robot.drivetrain.follower.getPose().getHeading()); - Pose nearGoalPose = new Pose(8,136,0); - if (LoadHardwareClass.selectedAlliance == LoadHardwareClass.Alliance.RED) {nearGoalPose = new Pose(136, 136, 0);} - Pose farGoalPose = new Pose(16,140,0); - if (LoadHardwareClass.selectedAlliance == LoadHardwareClass.Alliance.RED) {farGoalPose = new Pose(136, 140, 0);} + Pose farPose = rotationalFarGoalPose; + Pose nearPose = rotationalNearGoalPose; + + if (LoadHardwareClass.selectedAlliance == LoadHardwareClass.Alliance.RED) { + nearPose = nearPose.mirror(); + farPose = farPose.mirror(); + } if(robotZone.isInside(LoadHardwareClass.FarLaunchZone)){ - return farGoalPose; + return farPose; }else{ - return nearGoalPose; + return nearPose; } } @@ -349,7 +356,7 @@ public double getFlywheelRPM(){ return flywheel.getRPM(); } public boolean isFlywheelReady(){ - return flywheel.getRPM() > getFlywheelCurrentMaxSpeed(); + return flywheel.getRPM() > getFlywheelCurrentMaxSpeed()-150; } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java index 9213a1f54c3e..d1f0ae36f276 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java @@ -80,8 +80,6 @@ public class Teleop_Main_ extends LinearOpMode { public Pose holdPoint = new Pose(72, 72, 90); public Boolean holdJustTriggered = false; - public int lightsState = 0; - // Create a new instance of our Robot class LoadHardwareClass Robot = new LoadHardwareClass(this); // Create a new Paths instance @@ -94,6 +92,14 @@ enum startPoses { NEAR } + enum lightsState { + SOLID, + BLINKING, + RAINBOW + } + private lightsState ledState = lightsState.SOLID; + private lightsState ledStateOld = lightsState.RAINBOW; + // Contains the start Pose of our robot. This can be changed or saved from the autonomous period. private Pose startPose = Paths.farStart; @@ -152,7 +158,7 @@ public void runOpMode() { // Wait for the game to start (driver presses START) waitForStart(); // Initialize all hardware of the robot - if (selectedAlliance == LoadHardwareClass.Alliance.BLUE){ + if (selectedAlliance == LoadHardwareClass.Alliance.BLUE && MecanumDrivetrainClass.robotPose == null){ Robot.init(startPose.mirror()); }else{ Robot.init(startPose); @@ -239,18 +245,27 @@ public void runOpMode() { telemetry.update(); panelsTelemetry.update(); - if (runtime.time(TimeUnit.SECONDS) > 115 || lightsState == 2){ - Robot.lights.setStripRainbow(); - lightsState = 2; + if (runtime.time(TimeUnit.SECONDS) > 115){ + ledState = lightsState.RAINBOW; + }else if (Robot.turret.isFlywheelReady()){ + ledState = lightsState.BLINKING; }else{ - if (Robot.turret.isFlywheelReady() && lightsState == 0){ - Robot.lights.setBlinkingAllianceDisplay(selectedAlliance); - lightsState = 1; - }else if (lightsState == 1){ - Robot.lights.setSolidAllianceDisplay(selectedAlliance); - lightsState = 0; + ledState = lightsState.SOLID; + } + if (ledState != ledStateOld){ + switch (ledState){ + case SOLID: + Robot.lights.setSolidAllianceDisplay(selectedAlliance); + break; + case BLINKING: + Robot.lights.setBlinkingAllianceDisplay(selectedAlliance); + break; + case RAINBOW: + Robot.lights.setStripRainbow(); } + ledStateOld = ledState; } + telemetry.addData("lightsState", ledState); } } @@ -346,6 +361,12 @@ public void Gamepad1() { if (gamepad1.dpadDownWasPressed()){ hoodOn = !hoodOn; } + + if (gamepad1.dpadLeftWasPressed()){ + selectedAlliance = LoadHardwareClass.Alliance.BLUE; + }else if (gamepad1.dpadRightWasPressed()){ + selectedAlliance = LoadHardwareClass.Alliance.RED; + } } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Tuning_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Tuning_.java index d636995067a6..ccbf2056edcf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Tuning_.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Tuning_.java @@ -51,6 +51,8 @@ public class Teleop_Tuning_ extends LinearOpMode { private ElapsedTime loopTimer = new ElapsedTime(); private TelemetryManager panelsTelemetry = PanelsTelemetry.INSTANCE.getTelemetry(); + public static double hoodOffset = 0; + double initial = 0; // Contains the start Pose of our robot. This can be changed or saved from the autonomous period. @@ -63,23 +65,31 @@ public void runOpMode() { LoadHardwareClass Robot = new LoadHardwareClass(this); Pedro_Paths paths = new Pedro_Paths(); // Initialize all hardware of the robot - Robot.init(paths.nearStart ); + Robot.init(startPose); telemetry.addData("Status", "Initialized"); telemetry.update(); Robot.turret.setGateState(Turret.gatestate.CLOSED); + if (!Turret.zeroed){ + while (!isStopRequested() && Robot.turret.zeroTurret()){ + sleep(0); + } + } + // Wait for the game to start (driver presses START) waitForStart(); runtime.reset(); - - initial = Robot.turret.rotation.getAngleAbsolute(); + Robot.drivetrain.startTeleOpDrive(); // run until the end of the match (driver presses STOP) while (opModeIsActive()) { - - Robot.turret.rotation.setAngle(initial); - + Robot.drivetrain.pedroMecanumDrive( + gamepad1.left_stick_y, + gamepad1.left_stick_x, + gamepad1.right_stick_x, + true + ); if (gamepad2.yWasPressed()) { if (Robot.turret.flywheelMode == Turret.flywheelState.OFF) { Robot.turret.setFlywheelState(Turret.flywheelState.ON); @@ -89,21 +99,19 @@ public void runOpMode() { } Robot.turret.updateFlywheel(); - if (gamepad2.dpad_down){ + if (Math.abs(gamepad2.left_stick_y) < 0.1){ Robot.intake.setMode(Intake.intakeMode.INTAKING); }else{ Robot.intake.setMode(Intake.intakeMode.OFF); } - if (gamepad1.x){ - Robot.turret.setGateState(Turret.gatestate.CLOSED); - }else if (gamepad1.y){ - Robot.turret.setGateState(Turret.gatestate.OPEN); - } + if (gamepad2.dpadUpWasPressed()) hoodOffset += 10; + if (gamepad2.dpadDownWasPressed()) hoodOffset -= 10; + Robot.turret.updateAimbot(false, true, hoodOffset); - telemetry.addData("gate pos", Robot.turret.getGate()); + telemetry.addData("Aimbot Hood Angle", Robot.turret.getHood()); + telemetry.addData("Hood Offset", hoodOffset); + telemetry.addData("Distance From Goal", Robot.drivetrain.distanceFromGoal()); - panelsTelemetry.addData("Flywheel Speed", Robot.turret.getFlywheelRPM()); - panelsTelemetry.addData("Flywheel Target", Robot.turret.getFlywheelCurrentMaxSpeed()); // System-related Telemetry telemetry.addLine(); @@ -114,7 +122,6 @@ public void runOpMode() { telemetry.update(); panelsTelemetry.update(); loopTimer.reset(); - Robot.turret.updatePIDs(); } } From 5d9569fa4813e9c84a0b63073510c8b2f1484d96 Mon Sep 17 00:00:00 2001 From: Professor348 <141444315+professor348@users.noreply.github.com> Date: Wed, 18 Feb 2026 16:56:15 -0500 Subject: [PATCH 3/4] Created 2 15-ball autos and finished tuning turret rotation and hood autoaim --- .../LOADCode/Main_/Auto_/Auto_Main_.java | 95 +++++++++++++++---- .../Main_/Hardware_/Actuators_/Intake.java | 8 +- .../Main_/Hardware_/Actuators_/Turret.java | 38 +++++--- .../LOADCode/Main_/Hardware_/Commands.java | 38 ++++---- .../Hardware_/Drivetrain_/Pedro_Paths.java | 9 +- .../Main_/Hardware_/LoadHardwareClass.java | 2 +- .../LOADCode/Main_/Teleop_/Teleop_Main_.java | 27 +++--- 7 files changed, 142 insertions(+), 75 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java index 6235f109f5a6..af219f554e02 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java @@ -20,7 +20,6 @@ import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import dev.nextftc.core.commands.delays.Delay; -import dev.nextftc.core.commands.delays.WaitUntil; import dev.nextftc.core.commands.groups.ParallelRaceGroup; import dev.nextftc.core.commands.groups.SequentialGroup; import dev.nextftc.core.commands.utility.InstantCommand; @@ -62,11 +61,12 @@ public void onInit() { prompter.prompt("auto", new OptionPrompt<>("Select Auto", new MOE_365_FAR(), + new Near_15Ball(), + new Near_15Ball2(), new Near_12Ball(), - new Near_9Ball(), + //new Near_9Ball(), new Far_9Ball(), - new Far_6Ball(), - new test_auto() + new Far_6Ball() )); prompter.onComplete(() -> { selectedAlliance = prompter.get("alliance"); @@ -339,41 +339,100 @@ public void runAuto(){ @NonNull @Override - public String toString(){return "MOE 365 Far Zone Auto";} + public String toString(){return "MOE 365 Far Zone";} } - private class test_auto extends Auto{ + private class Near_15Ball extends Auto{ @Override Pose getStartPose() { - return paths.farStart; + return paths.nearStart; } @Override boolean getTurretEnabled() { - return false; + return true; } @Override void runAuto() { new SequentialGroup( + Commands.setFlywheelState(Turret.flywheelState.ON), + Commands.runPath(paths.nearStart_to_midShoot, true, 1), + Commands.shootBalls(), Commands.setIntakeMode(Intake.intakeMode.INTAKING), - Commands.runPath(paths.farStart_to_farShoot, true, 1), - new WaitUntil(() -> gamepad2.bWasPressed()), - Commands.runPath(paths.farShoot_to_rampIntake, true, 1), - new WaitUntil(() -> gamepad2.bWasPressed()), - Commands.runPath(paths.rampIntake_to_farShoot, true, 1), - new WaitUntil(() -> gamepad2.bWasPressed()), - Commands.runPath(paths.farShoot_to_hpPreload, true, 1), - new WaitUntil(() -> gamepad2.bWasPressed()), - Commands.runPath(paths.hpPreload_to_farShoot, true, 1) + Commands.runPath(paths.midShoot_to_midPreload, true, 1), + Commands.runPath(paths.midPreload_to_midShoot, true, 1), + Commands.shootBalls(), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.midShoot_to_openGateIntake, true, 1), + Commands.waitForArtifacts(), + Commands.runPath(paths.openGateIntake_to_midShoot, true, 1), + Commands.shootBalls(), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.midShoot_to_farPreload, true, 1), + Commands.setIntakeMode(Intake.intakeMode.OFF), + Commands.runPath(paths.farPreload_to_midShoot, true, 1), + Commands.shootBalls(), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.midShoot_to_nearPreload, true, 1), + Commands.runPath(paths.nearPreload_to_midShoot, false, 1), + Commands.shootBalls(), + Commands.runPath(paths.midShoot_to_nearLeave, true, 1) + ).schedule(); + } + + @NonNull + @Override + public String toString() { + return "Near Zone 15 Ball (With Far Spike Mark)"; + } + } + + private class Near_15Ball2 extends Auto{ + + @Override + Pose getStartPose() { + return paths.nearStart; + } + + @Override + boolean getTurretEnabled() { + return true; + } + + @Override + void runAuto() { + new SequentialGroup( + Commands.setFlywheelState(Turret.flywheelState.ON), + Commands.runPath(paths.nearStart_to_midShoot, true, 1), + Commands.shootBalls(), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.midShoot_to_midPreload, true, 1), + Commands.runPath(paths.midPreload_to_midShoot, true, 1), + Commands.shootBalls(), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.midShoot_to_openGateIntake, true, 1), + Commands.waitForArtifacts(), + Commands.runPath(paths.openGateIntake_to_midShoot, true, 1), + Commands.shootBalls(), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.midShoot_to_openGateIntake, true, 1), + Commands.waitForArtifacts(), + Commands.runPath(paths.openGateIntake_to_midShoot, true, 1), + Commands.shootBalls(), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.midShoot_to_nearPreload, true, 1), + Commands.runPath(paths.nearPreload_to_midShoot, false, 1), + Commands.shootBalls(), + Commands.runPath(paths.midShoot_to_nearLeave, true, 1) ).schedule(); } @NonNull @Override public String toString() { - return "Test Auto"; + return "Near Zone 15 Ball (No Far Spike Mark)"; } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Intake.java index a4a9398bbbac..18e21908da50 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Intake.java @@ -23,6 +23,7 @@ public enum intakeMode { SHOOTING, NO_BELT, REVERSING, + REVERSE_NOBELT, OFF } @@ -74,7 +75,7 @@ public void setMode(intakeMode direction) { intake.setPower(1); belt.setPower(1); }else if (direction == intakeMode.SHOOTING){ - intake.setPower(0); //TODO change this to 0 if using servo powered intake belt + intake.setPower(0); belt.setPower(1); }else if (direction == intakeMode.REVERSING){ intake.setPower(-1); @@ -82,6 +83,9 @@ public void setMode(intakeMode direction) { }else if (direction == intakeMode.NO_BELT){ intake.setPower(1); belt.setPower(0); + }else if (direction == intakeMode.REVERSE_NOBELT){ + intake.setPower(-1); + belt.setPower(0); }else{ intake.setPower(0); belt.setPower(0); @@ -112,6 +116,8 @@ public intakeMode getMode(){ return intakeMode.NO_BELT; }else if (intakePower == -1 && beltPower == -1){ return intakeMode.REVERSING; + }else if (intakePower == -1 && beltPower == 0){ + return intakeMode.REVERSE_NOBELT; }else{ return intakeMode.OFF; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java index 04c25e7c4afe..1824240fc6e2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Turret.java @@ -30,7 +30,7 @@ public class Turret { public final Devices.REVHallEffectSensorClass hall = new Devices.REVHallEffectSensorClass(); // Turret PID coefficients - public static PIDCoefficients turretCoefficients = new PIDCoefficients(0.018, 0.00000000005, 0.0002); // 223RPM Motor + public static PIDCoefficients turretCoefficients = new PIDCoefficients(0.022, 0.0000000002, 0.0015); // 223RPM Motor public static PIDCoefficients cameraCoefficients = new PIDCoefficients(0, 0, 0); // Flywheel PID coefficients for various speeds @@ -64,8 +64,9 @@ public enum flywheelState { public flywheelState flywheelMode = flywheelState.OFF; double targetRPM = 0; /** Controls the target speed of the flywheel when it is on.*/ - public static double flywheelReallyNearSpeed = 3000; + public static double flywheelReallyNearSpeed = 2800; public static double flywheelNearSpeed = 3300; + public static double flywheelFarNearSpeed = 3600; public static double flywheelFarSpeed = 4200; /** Controls the upper software limit of the hood.*/ public static double upperHoodLimit = 260; @@ -141,28 +142,31 @@ public void init(OpMode opmode, LoadHardwareClass robot){ // Safety points for LUTs hoodLUTnear.add(0, 0); - hoodLUTfar.add(0, 0); + hoodLUTfar.add(0, 190); // -------------------------------------------------------- // Near zone measurements - hoodLUTnear.add(53.5,108); - hoodLUTnear.add(71,168); - hoodLUTnear.add(77, 181); + hoodLUTnear.add(60, 0); + hoodLUTnear.add(68.5, 140.5); + hoodLUTnear.add(71, 182); hoodLUTnear.add(84.5, 175); - hoodLUTnear.add(88,185); - hoodLUTnear.add(94.5,185); - hoodLUTnear.add(96, 180); + hoodLUTnear.add(88, 145); + hoodLUTnear.add(90, 145); + hoodLUTnear.add(97.5, 200); + hoodLUTnear.add(102, 187); + hoodLUTnear.add(114, 165); // Far zone measurements - hoodLUTfar.add(103, 200); - hoodLUTfar.add(204, 200); + hoodLUTfar.add(139.5, 190); + hoodLUTfar.add(150, 190); + hoodLUTfar.add(160, 160); // -------------------------------------------------------- // Safety points for LUTs - hoodLUTnear.add(300, 200); - hoodLUTfar.add(300, 200); + hoodLUTnear.add(300, 165); + hoodLUTfar.add(300, 160); // Generate Lookup Table & Initialize servo position hoodLUTnear.createLUT(); @@ -233,9 +237,9 @@ private void updateRotationalAimbot(){ } }else{ if (LoadHardwareClass.selectedAlliance == LoadHardwareClass.Alliance.RED){ - rotation.setAngle(Math.min(Math.max(0, rotationalAimbotLocalizer()), 360)); + rotation.setAngle(Math.min(Math.max(0, rotationalAimbotLocalizer()), 360), -Math.toDegrees(Robot.drivetrain.follower.getAngularVelocity())); }else{ - rotation.setAngle(Math.min(Math.max(0, rotationalAimbotLocalizer()), 360)); + rotation.setAngle(Math.min(Math.max(0, rotationalAimbotLocalizer()), 360), -Math.toDegrees(Robot.drivetrain.follower.getAngularVelocity())); } } } @@ -406,6 +410,10 @@ public void updateFlywheel() { targetRPM = flywheelReallyNearSpeed; actualFlywheelCoefficients = flywheelCoefficients3000; actualFlywheelFFCoefficients = flywheelFFCoefficients3000; + }else if (Robot.drivetrain.distanceFromGoal() > 90){ + targetRPM = flywheelFarNearSpeed; + actualFlywheelCoefficients = flywheelCoefficients3500; + actualFlywheelFFCoefficients = flywheelFFCoefficients3500; }else{ targetRPM = flywheelNearSpeed; actualFlywheelCoefficients = flywheelCoefficients3500; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java index 6f5d165ebbce..127e84edc299 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java @@ -11,6 +11,7 @@ import dev.nextftc.core.commands.Command; import dev.nextftc.core.commands.delays.Delay; import dev.nextftc.core.commands.delays.WaitUntil; +import dev.nextftc.core.commands.groups.ParallelGroup; import dev.nextftc.core.commands.groups.ParallelRaceGroup; import dev.nextftc.core.commands.groups.SequentialGroup; import dev.nextftc.core.commands.utility.InstantCommand; @@ -28,6 +29,7 @@ public Commands(@NonNull LoadHardwareClass robot){ // Delay timer for shooting sequence private static final TimerEx shootingTimerFifthSec = new TimerEx(0.2); private static final TimerEx shootingTimerHalfSec = new TimerEx(0.5); + private static final TimerEx shootingTimer1sec = new TimerEx(1); private static final TimerEx shootingTimer2sec = new TimerEx(2); private static final TimerEx shootingTimer5sec = new TimerEx(5); private static Command resetShootingTimerFifthsec() { @@ -36,6 +38,9 @@ private static Command resetShootingTimerFifthsec() { private static Command resetShootingTimerHalfsec() { return new LambdaCommand("resetShootingTimer0.5sec").setStart(shootingTimerHalfSec::restart); } + private static Command resetShootingTimer1sec() { + return new LambdaCommand("resetShootingTimer1sec").setStart(shootingTimer1sec::restart); + } private static Command resetShootingTimer2sec() { return new LambdaCommand("resetShootingTimer2sec").setStart(shootingTimer2sec::restart); } @@ -70,27 +75,20 @@ private Command setGateState(Turret.gatestate state){ public Command setIntakeMode(Intake.intakeMode state) { return new InstantCommand(new LambdaCommand("setIntakeMode()") .setStart(() -> Robot.intake.setMode(state)) - .setIsDone(() -> true) ); } public Command setTransferState(Intake.transferState state) { return new InstantCommand(new LambdaCommand("setIntakeMode()") .setStart(() -> Robot.intake.setTransfer(state)) - .setIsDone(() -> true) ); } + /** + * Waits until both proximity sensors are activated at the same time or until 2 seconds have passed + */ public Command waitForArtifacts(){ - return new SequentialGroup( - setIntakeMode(Intake.intakeMode.INTAKING), - resetShootingTimer5sec(), - new WaitUntil(() -> Robot.intake.getTopSensorState() || Robot.intake.getBottomSensorState()), - new ParallelRaceGroup( - new WaitUntil(() -> Robot.intake.getTopSensorState() && Robot.intake.getBottomSensorState()), - new WaitUntil(shootingTimer5sec::isDone) - ) - ); + return new Delay(0.7); } public Command shootBalls(){ @@ -100,22 +98,22 @@ public Command shootBalls(){ // Shoot the first two balls setGateState(Turret.gatestate.OPEN), - resetShootingTimerFifthsec(), - new WaitUntil(shootingTimerFifthSec::isDone), + new Delay(0.6), setIntakeMode(Intake.intakeMode.INTAKING), - resetShootingTimer2sec(), - new WaitUntil(() -> (Robot.intake.getTopSensorState() && !Robot.intake.getBottomSensorState() && shootingTimer2sec.isDone())), + new Delay(0.5), + new WaitUntil(() -> (Robot.intake.getTopSensorState() && !Robot.intake.getBottomSensorState())), // Shoot the last ball setIntakeMode(Intake.intakeMode.SHOOTING), setTransferState(Intake.transferState.UP), - resetShootingTimerHalfsec(), - new WaitUntil(shootingTimerHalfSec::isDone), + new Delay(0.5), // Reset the systems - setIntakeMode(Intake.intakeMode.OFF), - setGateState(Turret.gatestate.CLOSED), - setTransferState(Intake.transferState.DOWN) + new ParallelGroup( + setIntakeMode(Intake.intakeMode.OFF), + setGateState(Turret.gatestate.CLOSED), + setTransferState(Intake.transferState.DOWN) + ) ); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java index 3fcf45497fee..7f7ce194ef25 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java @@ -28,7 +28,7 @@ public class Pedro_Paths { public Pose hpIntake = null; // Shooting Poses public Pose nearShoot = new Pose(115, 120, Math.toRadians(-35)); - public Pose midShoot = new Pose(85, 85, Math.toRadians(-15)); + public Pose midShoot = new Pose(88, 87, Math.toRadians(-15)); public Pose farShoot = new Pose(85, 15, Math.toRadians(60)); public Pose noTurretMidShoot = new Pose(85, 85, Math.toRadians(45)); public Pose noTurretFarShoot = new Pose(85, 15, Math.toRadians(67.3)); @@ -39,8 +39,8 @@ public class Pedro_Paths { // Open Gate Pose public Pose openGateBasic = new Pose(127.5, 72, Math.toRadians(90)); public Pose openGateBasicReversed = new Pose(127.5, 72, Math.toRadians(-90)); - public Pose openGateIntakeGate = new Pose(131, 61.5, Math.toRadians(20)); - public Pose openGateIntakeRamp = new Pose(133, 55, Math.toRadians(40)); + public Pose openGateIntakeGate = new Pose(128.5, 62, Math.toRadians(20)); + public Pose openGateIntakeRamp = new Pose(127, 55, Math.toRadians(40)); /** *

Define all path variables.


@@ -299,8 +299,7 @@ public void buildShooting2ToPreloads(){ midShoot, autoMirror(new Pose(75, 56)), midPreload - )) - .setLinearHeadingInterpolation(midShoot.getHeading(), midPreload.getHeading()) + )).setLinearHeadingInterpolation(midShoot.getHeading(), midPreload.getHeading()) .build(); midShoot_to_farPreload = follower.pathBuilder() .addPath(new BezierCurve( diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java index 3d8554a8090b..73c15408e4b5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java @@ -126,7 +126,7 @@ public void init(Pose initialPose, Follower follower) { drivetrain.init(myOpMode, initialPose, follower); turret.init(myOpMode, this); intake.init(myOpMode); - lights.init(myOpMode, 24); + lights.init(myOpMode, 36); // Misc telemetry myOpMode.telemetry.addData(">", "Hardware Initialized"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java index d1f0ae36f276..e386307d6397 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java @@ -37,13 +37,11 @@ import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.util.ElapsedTime; import com.skeletonarmy.marrow.TimerEx; import com.skeletonarmy.marrow.prompts.OptionPrompt; import com.skeletonarmy.marrow.prompts.Prompter; -import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Intake; import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Intake.intakeMode; import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Intake.transferState; @@ -197,11 +195,8 @@ public void runOpMode() { //positional telemetry telemetry.addData("Robot Position [X, Y, H]", "[" + Robot.drivetrain.follower.getPose().getX() + ", " + Robot.drivetrain.follower.getPose().getY() + ", " + Robot.drivetrain.follower.getPose().getHeading() + "]"); telemetry.addData("Distance From Goal", Robot.drivetrain.distanceFromGoal()); - telemetry.addLine(); - panelsTelemetry.addData("FL Wheel Current", hardwareMap.get(DcMotorEx.class, "FL").getCurrent(CurrentUnit.AMPS)); - panelsTelemetry.addData("FR Wheel Current", hardwareMap.get(DcMotorEx.class, "FR").getCurrent(CurrentUnit.AMPS)); - panelsTelemetry.addData("BL Wheel Current", hardwareMap.get(DcMotorEx.class, "BL").getCurrent(CurrentUnit.AMPS)); - panelsTelemetry.addData("BR Wheel Current", hardwareMap.get(DcMotorEx.class, "BR").getCurrent(CurrentUnit.AMPS)); + telemetry.addData("Angular Velocity (Deg/sec)", Math.toDegrees(Robot.drivetrain.follower.getAngularVelocity())); + telemetry.addData("Turret Angular Velocity (Deg/sec)", Robot.turret.rotation.getDegreesPerSecond()); telemetry.addLine(); // Turret-related Telemetry @@ -267,6 +262,8 @@ public void runOpMode() { } telemetry.addData("lightsState", ledState); } + + selectedAlliance = null; } /** @@ -361,6 +358,9 @@ public void Gamepad1() { if (gamepad1.dpadDownWasPressed()){ hoodOn = !hoodOn; } + if (gamepad1.yWasPressed()){ + turretOn = !turretOn; + } if (gamepad1.dpadLeftWasPressed()){ selectedAlliance = LoadHardwareClass.Alliance.BLUE; @@ -413,9 +413,6 @@ public void Gamepad1() { * */ public void Gamepad2() { - if (gamepad1.yWasPressed()){ - turretOn = !turretOn; - } Robot.turret.updateAimbot(turretOn, hoodOn, hoodOffset); Robot.turret.rotation.setOffsetDegrees(Turret.turretOffset + turretOffset); @@ -465,7 +462,7 @@ public void Gamepad2() { } if (gamepad2.dpadLeftWasPressed()){ turretOffset += 10; - }else if (gamepad2.dpadLeftWasPressed()){ + }else if (gamepad2.dpadRightWasPressed()){ turretOffset -= 10; } @@ -496,12 +493,12 @@ public void Gamepad2() { return; case 2: if (Robot.intake.getMode() == intakeMode.OFF){ - stateTimerFullSec.restart(); - stateTimerFullSec.start(); + stateTimerHalfSec.restart(); + stateTimerHalfSec.start(); } Robot.intake.setMode(intakeMode.INTAKING); - telemetry.addData("Shooting State", "SHOOTING 2"); - if (stateTimerFullSec.isDone() && Robot.intake.getTopSensorState() && !Robot.intake.getBottomSensorState()){ + telemetry.addData("Shooting State", "SHOOTING FIRST TWO"); + if (stateTimerHalfSec.isDone() && Robot.intake.getTopSensorState() && !Robot.intake.getBottomSensorState()){ shootingState = 3; } return; From d5212165ddaff1f92b9a71730229df5ccad4751b Mon Sep 17 00:00:00 2001 From: Professor348 <141444315+professor348@users.noreply.github.com> Date: Sat, 21 Feb 2026 20:41:14 -0500 Subject: [PATCH 4/4] Final Governor's Cup Tweaks --- .../LOADCode/Main_/Auto_/Auto_Main_.java | 180 ++++++++++-------- .../LOADCode/Main_/Hardware_/Commands.java | 109 +++++------ .../Hardware_/Drivetrain_/Pedro_Paths.java | 2 +- .../Main_/Teleop_/Teleop_Tuning_.java | 1 + 4 files changed, 158 insertions(+), 134 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java index af219f554e02..c8ca6c598cbd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java @@ -8,6 +8,7 @@ import com.pedropathing.geometry.BezierLine; import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.skeletonarmy.marrow.TimerEx; import com.skeletonarmy.marrow.prompts.OptionPrompt; import com.skeletonarmy.marrow.prompts.Prompter; @@ -20,6 +21,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import dev.nextftc.core.commands.delays.Delay; +import dev.nextftc.core.commands.delays.WaitUntil; import dev.nextftc.core.commands.groups.ParallelRaceGroup; import dev.nextftc.core.commands.groups.SequentialGroup; import dev.nextftc.core.commands.utility.InstantCommand; @@ -29,6 +31,7 @@ @Autonomous(name = "Auto_Main_", group = "Main", preselectTeleOp="Teleop_Main_") public class Auto_Main_ extends NextFTCOpMode { + TimerEx timer25Sec = new TimerEx(25); // Variable to store the selected auto program Auto selectedAuto = null; // Create the prompter object for selecting Alliance and Auto @@ -64,9 +67,11 @@ public void onInit() { new Near_15Ball(), new Near_15Ball2(), new Near_12Ball(), - //new Near_9Ball(), + new Near_9Ball(), new Far_9Ball(), - new Far_6Ball() + new Far_6Ball(), + new Far_3Ball() + //new test() )); prompter.onComplete(() -> { selectedAlliance = prompter.get("alliance"); @@ -103,6 +108,7 @@ public void onStartButtonPressed() { // Schedule the proper auto selectedAuto.runAuto(); turretOn = selectedAuto.getTurretEnabled(); + timer25Sec.restart(); // Indicate that initialization is done telemetry.addLine("Initialized"); @@ -149,7 +155,7 @@ abstract static class Auto{ public abstract String toString(); } - private class Far_6Ball extends Auto{ + private class Far_9Ball extends Auto{ @Override public Pose getStartPose(){ return paths.farStart; @@ -169,16 +175,20 @@ public void runAuto(){ Commands.runPath(paths.farShoot_to_farPreload, true, 1), Commands.runPath(paths.farPreload_to_farShoot, true, 1), Commands.shootBalls(), + Commands.setFlywheelState(Turret.flywheelState.ON), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.farShoot_to_hpPreload, true, 1), + Commands.runPath(paths.hpPreload_to_farShoot, true, 1), + Commands.shootBalls(), Commands.runPath(paths.farShoot_to_farLeave, true, 1) ).schedule(); } @NonNull @Override - public String toString(){return "Far Zone 6 Ball";} + public String toString(){return "Far 9 Ball";} } - - private class Far_9Ball extends Auto{ + private class Far_6Ball extends Auto{ @Override public Pose getStartPose(){ return paths.farStart; @@ -198,20 +208,44 @@ public void runAuto(){ Commands.runPath(paths.farShoot_to_farPreload, true, 1), Commands.runPath(paths.farPreload_to_farShoot, true, 1), Commands.shootBalls(), - Commands.setFlywheelState(Turret.flywheelState.ON), - Commands.setIntakeMode(Intake.intakeMode.INTAKING), - Commands.runPath(paths.farShoot_to_midPreload, true, 1), - Commands.runPath(paths.midPreload_to_farShoot, true, 1), - Commands.shootBalls(), Commands.runPath(paths.farShoot_to_farLeave, true, 1) ).schedule(); } @NonNull @Override - public String toString(){return "Far Zone 9 Ball";} + public String toString(){return "Far 6 Ball";} } + private class Far_3Ball extends Auto{ + @Override + Pose getStartPose() { + return paths.farStart; + } + + @Override + boolean getTurretEnabled() { + return true; + } + + @Override + void runAuto() { + new SequentialGroup( + Commands.setFlywheelState(Turret.flywheelState.ON), + Commands.runPath(paths.farStart_to_farShoot, true, 1), + Commands.shootBalls(), + new WaitUntil(() -> timer25Sec.isDone()), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.farShoot_to_hpPreload, true, 1) + ).schedule(); + } + + @NonNull + @Override + public String toString() { + return "Far 3 Ball + HP Intake"; + } + } private class Near_9Ball extends Auto{ @Override public Pose getStartPose(){ @@ -245,9 +279,8 @@ public void runAuto(){ @NonNull @Override - public String toString(){return "Near Zone 9 Ball";} + public String toString(){return "Near 9 Ball";} } - private class Near_12Ball extends Auto{ @Override public Pose getStartPose(){ @@ -283,65 +316,8 @@ public void runAuto(){ @NonNull @Override - public String toString(){return "Near Zone 12 Ball";} - } - - private class MOE_365_FAR extends Auto{ - @Override - public Pose getStartPose(){ - return paths.farStart; - } - @Override - public boolean getTurretEnabled(){ - return true; - } - - @Override - public void runAuto(){ - new SequentialGroup( - new ParallelRaceGroup( - new Delay(29), - new SequentialGroup( - Commands.setFlywheelState(Turret.flywheelState.ON), - Commands.runPath(paths.farStart_to_farShoot, true, 1), - Commands.shootBalls(), - Commands.setIntakeMode(Intake.intakeMode.INTAKING), - Commands.runPath(paths.farShoot_to_farPreload, true, 1), - Commands.runPath(paths.farPreload_to_farShoot, true, 1), - Commands.shootBalls(), - Commands.setIntakeMode(Intake.intakeMode.INTAKING), - Commands.runPath(paths.farShoot_to_rampIntake, true, 1), - Commands.runPath(paths.rampIntake_to_farShoot, true, 1), - Commands.shootBalls(), - Commands.setIntakeMode(Intake.intakeMode.INTAKING), - Commands.runPath(paths.farShoot_to_hpPreload, true, 1), - Commands.runPath(paths.hpPreload_to_farShoot, true, 1), - Commands.shootBalls(), - Commands.setIntakeMode(Intake.intakeMode.INTAKING), - Commands.runPath(paths.farShoot_to_hpPreload, true, 1), - Commands.runPath(paths.hpPreload_to_farShoot, true, 1), - Commands.shootBalls(), - Commands.runPath(paths.farShoot_to_farLeave, true, 1) - ) - ), - Commands.runPath( - Robot.drivetrain.follower.pathBuilder().addPath( - new BezierLine( - Robot.drivetrain.follower.getPose(), - paths.farLeave - ) - ).setLinearHeadingInterpolation( - Robot.drivetrain.follower.getPose().getHeading(), - paths.farLeave.getHeading() - ).build(), true, 1) - ).schedule(); - } - - @NonNull - @Override - public String toString(){return "MOE 365 Far Zone";} + public String toString(){return "Near 12 Ball";} } - private class Near_15Ball extends Auto{ @Override @@ -385,10 +361,9 @@ void runAuto() { @NonNull @Override public String toString() { - return "Near Zone 15 Ball (With Far Spike Mark)"; + return "Near 15 Ball (With Far Spike Mark)"; } } - private class Near_15Ball2 extends Auto{ @Override @@ -432,7 +407,62 @@ void runAuto() { @NonNull @Override public String toString() { - return "Near Zone 15 Ball (No Far Spike Mark)"; + return "Near 15 Ball (No Far Spike Mark)"; + } + } + private class MOE_365_FAR extends Auto{ + @Override + public Pose getStartPose(){ + return paths.farStart; + } + @Override + public boolean getTurretEnabled(){ + return true; + } + + @Override + public void runAuto(){ + new SequentialGroup( + new ParallelRaceGroup( + new Delay(29), + new SequentialGroup( + Commands.setFlywheelState(Turret.flywheelState.ON), + Commands.runPath(paths.farStart_to_farShoot, true, 1), + Commands.shootBalls(), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.farShoot_to_farPreload, true, 1), + Commands.runPath(paths.farPreload_to_farShoot, true, 1), + Commands.shootBalls(), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.farShoot_to_rampIntake, true, 1), + Commands.runPath(paths.rampIntake_to_farShoot, true, 1), + Commands.shootBalls(), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.farShoot_to_hpPreload, true, 1), + Commands.runPath(paths.hpPreload_to_farShoot, true, 1), + Commands.shootBalls(), + Commands.setIntakeMode(Intake.intakeMode.INTAKING), + Commands.runPath(paths.farShoot_to_hpPreload, true, 1), + Commands.runPath(paths.hpPreload_to_farShoot, true, 1), + Commands.shootBalls(), + Commands.runPath(paths.farShoot_to_farLeave, true, 1) + ) + ), + Commands.runPath( + Robot.drivetrain.follower.pathBuilder().addPath( + new BezierLine( + Robot.drivetrain.follower.getPose(), + paths.farLeave + ) + ).setLinearHeadingInterpolation( + Robot.drivetrain.follower.getPose().getHeading(), + paths.farLeave.getHeading() + ).build(), true, 1) + ).schedule(); } + + @NonNull + @Override + public String toString(){return "MOE 365 Far";} } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java index 127e84edc299..a261d738d23a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java @@ -10,10 +10,6 @@ import dev.nextftc.core.commands.Command; import dev.nextftc.core.commands.delays.Delay; -import dev.nextftc.core.commands.delays.WaitUntil; -import dev.nextftc.core.commands.groups.ParallelGroup; -import dev.nextftc.core.commands.groups.ParallelRaceGroup; -import dev.nextftc.core.commands.groups.SequentialGroup; import dev.nextftc.core.commands.utility.InstantCommand; import dev.nextftc.core.commands.utility.LambdaCommand; import dev.nextftc.extensions.pedro.FollowPath; @@ -26,6 +22,9 @@ public Commands(@NonNull LoadHardwareClass robot){ Robot = robot; } + public static int shootingState = 0; + public static boolean isDoneShooting = false; + // Delay timer for shooting sequence private static final TimerEx shootingTimerFifthSec = new TimerEx(0.2); private static final TimerEx shootingTimerHalfSec = new TimerEx(0.5); @@ -92,60 +91,54 @@ public Command waitForArtifacts(){ } public Command shootBalls(){ - return new SequentialGroup( - // Ensure the flywheel is up to speed, if not, spin up first - setFlywheelState(Turret.flywheelState.ON), - - // Shoot the first two balls - setGateState(Turret.gatestate.OPEN), - new Delay(0.6), - setIntakeMode(Intake.intakeMode.INTAKING), - new Delay(0.5), - new WaitUntil(() -> (Robot.intake.getTopSensorState() && !Robot.intake.getBottomSensorState())), - - // Shoot the last ball - setIntakeMode(Intake.intakeMode.SHOOTING), - setTransferState(Intake.transferState.UP), - new Delay(0.5), - - // Reset the systems - new ParallelGroup( - setIntakeMode(Intake.intakeMode.OFF), - setGateState(Turret.gatestate.CLOSED), - setTransferState(Intake.transferState.DOWN) - ) - ); - } - - public Command shootTestBalls(){ - return new ParallelRaceGroup( - new SequentialGroup( - // Ensure the flywheel is up to speed, if not, spin up first - setFlywheelState(Turret.flywheelState.ON), - - // Shoot the first two balls - setGateState(Turret.gatestate.OPEN), - resetShootingTimerFifthsec(), - new WaitUntil(shootingTimerFifthSec::isDone), - setIntakeMode(Intake.intakeMode.INTAKING), - resetShootingTimer2sec(), - new WaitUntil(() -> (Robot.intake.getTopSensorState() && !Robot.intake.getBottomSensorState() && shootingTimer2sec.isDone())), - - // Shoot the last ball - setIntakeMode(Intake.intakeMode.SHOOTING), - setTransferState(Intake.transferState.UP), - resetShootingTimerHalfsec(), - new WaitUntil(shootingTimerHalfSec::isDone), - - // Reset the systems - setIntakeMode(Intake.intakeMode.OFF), - setGateState(Turret.gatestate.CLOSED), - setTransferState(Intake.transferState.DOWN) - ), - new SequentialGroup( - new Delay(8) - ) - ); + return new LambdaCommand() + .setStart(() -> { + shootingState = 1; + isDoneShooting = false; + shootingTimer5sec.restart(); + }) + .setUpdate(() -> { + switch (shootingState) { + case 1: + Robot.turret.setFlywheelState(Turret.flywheelState.ON); + if (Robot.turret.isFlywheelReady()){ + shootingState++; + shootingTimerFifthSec.restart(); + } + case 2: + Robot.turret.setGateState(Turret.gatestate.OPEN); + if (shootingTimerFifthSec.isDone()){ + shootingState++; + shootingTimerHalfSec.restart(); + } + return; + case 3: + Robot.intake.setMode(Intake.intakeMode.INTAKING); + if (shootingTimerHalfSec.isDone() && Robot.intake.getTopSensorState() && !Robot.intake.getBottomSensorState()){ + shootingState++; + shootingTimerHalfSec.restart(); + return; + } + if (shootingTimer5sec.isDone()){ + shootingState = 5; + } + return; + case 4: + Robot.intake.setMode(Intake.intakeMode.SHOOTING); + Robot.intake.setTransfer(Intake.transferState.UP); + if (shootingTimerHalfSec.isDone()) { + shootingState++; + } + return; + case 5: + Robot.turret.setGateState(Turret.gatestate.CLOSED); + Robot.intake.setMode(Intake.intakeMode.OFF); + Robot.intake.setTransfer(Intake.transferState.DOWN); + isDoneShooting = true; + } + }) + .setIsDone(() -> isDoneShooting) + .setInterruptible(true); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java index 7f7ce194ef25..a24e432fdc5c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java @@ -39,7 +39,7 @@ public class Pedro_Paths { // Open Gate Pose public Pose openGateBasic = new Pose(127.5, 72, Math.toRadians(90)); public Pose openGateBasicReversed = new Pose(127.5, 72, Math.toRadians(-90)); - public Pose openGateIntakeGate = new Pose(128.5, 62, Math.toRadians(20)); + public Pose openGateIntakeGate = new Pose(128, 62, Math.toRadians(20)); public Pose openGateIntakeRamp = new Pose(127, 55, Math.toRadians(40)); /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Tuning_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Tuning_.java index ccbf2056edcf..4248d61dd108 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Tuning_.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Tuning_.java @@ -70,6 +70,7 @@ public void runOpMode() { telemetry.addData("Status", "Initialized"); telemetry.update(); Robot.turret.setGateState(Turret.gatestate.CLOSED); + Robot.lights.setStripRainbow(); if (!Turret.zeroed){ while (!isStopRequested() && Robot.turret.zeroTurret()){