From d669cdc038bdae26ada816eec240ad1e38f16a13 Mon Sep 17 00:00:00 2001 From: ultimatehecker <86735991+ultimatehecker@users.noreply.github.com> Date: Wed, 4 Feb 2026 23:24:30 -0500 Subject: [PATCH 01/10] made the turret a default command --- .../ftc/teamcode/TeleopBindings.java | 17 +++++-- .../autonomous/SelectableAutonomous.java | 9 +++- .../commands/AimTowardShootingRegion.java | 42 +++++++++++++++++ .../teamcode/constants/TurretConstants.java | 5 +- .../ftc/teamcode/subsystems/Turret.java | 47 +++++++++++-------- 5 files changed, 94 insertions(+), 26 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/AimTowardShootingRegion.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopBindings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopBindings.java index 6c141d1..119b152 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopBindings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopBindings.java @@ -6,7 +6,9 @@ import org.firstinspires.ftc.teamcode.command_factories.IntakeFactory; import org.firstinspires.ftc.teamcode.command_factories.ShooterFactory; import org.firstinspires.ftc.teamcode.command_factories.TransferFactory; +import org.firstinspires.ftc.teamcode.commands.AimTowardShootingRegion; import org.firstinspires.ftc.teamcode.commands.TeleopMecanum; +import org.firstinspires.ftc.teamcode.constants.GlobalConstants; import org.firstinspires.ftc.teamcode.constants.LEDConstants; import org.firstinspires.ftc.teamcode.constants.ShooterConstants; import org.firstinspires.ftc.teamcode.constants.TransferConstants; @@ -32,8 +34,6 @@ public static void configureBindings(GamepadEx driver, GamepadEx operator, Drive .whenActive(ShooterFactory.velocitySetpointCommand(shooter, () -> 3500)) .whenInactive(ShooterFactory.openLoopSetpointCommand(shooter, () -> 0)); - new Trigger(() -> transfer.firstCSDistance() < 2.5).whenActive(() -> led.setSolid(LEDConstants.ColorValue.YELLOW)); - driver.getGamepadButton(GamepadKeys.Button.SQUARE).toggleWhenActive( () -> shooter.setHoodPosition(-2), () -> shooter.setHoodPosition(2) @@ -83,9 +83,16 @@ public static void configureDefaultCommands(GamepadEx driver, GamepadEx operator drivetrain.setDefaultCommand(new TeleopMecanum( drivetrain, driver::getLeftY, - () -> -driver.getLeftX(), - () -> -driver.getRightX(), - () -> false + driver::getLeftX, + driver::getRightX, + drivetrain::isRobotCentric + )); + + turret.setDefaultCommand(new AimTowardShootingRegion( + turret, + drivetrain::getPose, + GlobalConstants::getCurrentAllianceColor, + turret::isTurretAutoTrackingEnabled )); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/SelectableAutonomous.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/SelectableAutonomous.java index 75b6776..3ddecdd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/SelectableAutonomous.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/SelectableAutonomous.java @@ -17,6 +17,7 @@ import org.firstinspires.ftc.library.command.WaitUntilCommand; import org.firstinspires.ftc.library.math.Pair; import org.firstinspires.ftc.library.utilities.Timing; +import org.firstinspires.ftc.teamcode.commands.AimTowardShootingRegion; import org.firstinspires.ftc.teamcode.constants.DrivetrainConstants; import org.firstinspires.ftc.teamcode.constants.GlobalConstants; import org.firstinspires.ftc.teamcode.constants.LEDConstants; @@ -175,13 +176,19 @@ private void scheduleRoutine() { schedule( new RunCommand(drivetrain::update), new RunCommand(led::update), - new RunCommand(() -> turret.setPosition(turret.computeAngle(drivetrain.getPose(), turret.getTargetPose(selectedAlliance), 0, 0))), new SequentialCommandGroup( new WaitUntilCommand(this::opModeIsActive), routine.getSecond().getSecond() ) ); + turret.setDefaultCommand(new AimTowardShootingRegion( + turret, + drivetrain::getPose, + GlobalConstants::getCurrentAllianceColor, + turret::isTurretAutoTrackingEnabled + )); + drivetrain.setStartingPose(routine.getFirst()); transfer.onInitialization(true, true); drivetrain.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/AimTowardShootingRegion.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/AimTowardShootingRegion.java new file mode 100644 index 0000000..c0876d1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/AimTowardShootingRegion.java @@ -0,0 +1,42 @@ +package org.firstinspires.ftc.teamcode.commands; + +import com.pedropathing.geometry.Pose; + +import org.firstinspires.ftc.library.command.CommandBase; +import org.firstinspires.ftc.library.math.geometry.Pose2d; +import org.firstinspires.ftc.teamcode.constants.GlobalConstants; +import org.firstinspires.ftc.teamcode.constants.TurretConstants; +import org.firstinspires.ftc.teamcode.subsystems.Drivetrain; +import org.firstinspires.ftc.teamcode.subsystems.Turret; + +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; + +public class AimTowardShootingRegion extends CommandBase { + private final Turret turret; + private final Supplier currentRobotPose; + private final Supplier allianceColorSupplier; + private final BooleanSupplier autoTrackingEnabled; + + public AimTowardShootingRegion(final Turret turret, final Supplier currentRobotPose, final Supplier allianceColorSupplier, final BooleanSupplier autoTrackingEnabled) { + this.turret = turret; + this.currentRobotPose = currentRobotPose; + this.allianceColorSupplier = allianceColorSupplier; + this.autoTrackingEnabled = autoTrackingEnabled; + + addRequirements(turret); + } + + @Override + public void execute() { + Pose targetPose = turret.getTargetPose(allianceColorSupplier.get()); + + double desiredAngle = turret.computeAngle(currentRobotPose.get(), targetPose, TurretConstants.kTurretOffsetFromCenterOfRotationX, TurretConstants.kTurretOffsetFromCenterOfRotationY); + if(autoTrackingEnabled.getAsBoolean()) turret.setPosition(desiredAngle); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TurretConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TurretConstants.java index 515d400..6182bac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TurretConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TurretConstants.java @@ -18,6 +18,9 @@ public class TurretConstants { public static double pD = 0.05; public static double pF = 0.0; - public static double pidfSwitch = Math.PI / 18; public static double tuningSetpoint = 0; + + public static final double kTurretOffsetFromCenterOfRotationX = -3; + public static final double kTurretOffsetFromCenterOfRotationY = 0.00; + public static final double kTurretOffsetFromFloorZ = 14.00; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java index 782a63c..cc7f1c6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java @@ -22,12 +22,17 @@ import org.firstinspires.ftc.teamcode.constants.GlobalConstants; import org.firstinspires.ftc.teamcode.constants.TurretConstants; +import lombok.Getter; +import lombok.Setter; + public class Turret extends SubsystemBase { private DcMotorEx turretMotor; private RevTouchSensor homingSwitch; private PIDFController primaryPositionController; + private boolean enableTurretAutoTracking = true; + @IgnoreConfigurable static TelemetryManager telemetryM; @@ -70,6 +75,25 @@ public void setPosition(double radians) { turretMotor.setPower(MathUtility.clamp(primaryPositionController.calculate(getCurrentPosition(), radians), -0.45, 0.45)); } + public double computeAngle(Pose2d robotPose, Pose targetPose, double turretOffsetX, double turretOffsetY) { + double robotX = robotPose.getX(); + double robotY = robotPose.getY(); + double robotHeading = robotPose.getRotation().getRadians(); + + double turretX = robotX + turretOffsetX * Math.cos(robotHeading) - turretOffsetY * Math.sin(robotHeading); + double turretY = robotY + turretOffsetX * Math.sin(robotHeading) + turretOffsetY * Math.cos(robotHeading); + + double dx = targetPose.getX() - turretX; + double dy = targetPose.getY() - turretY; + + double targetAngleGlobal = Math.atan2(dy, dx); + double desiredTurretAngle = targetAngleGlobal - robotHeading; + double normalizedAngle = AngleUnit.normalizeRadians(desiredTurretAngle); + + telemetryM.addData(TurretConstants.kSubsystemName + "Computed Desired Angle to Goal", normalizedAngle); + return MathUtility.clamp(normalizedAngle, -Math.PI / 2, Math.PI / 2); + } + public void setManualPower(double speed) { telemetryM.addData(TurretConstants.kSubsystemName + "Setpoint Open Loop", speed); turretMotor.setPower(speed); @@ -96,26 +120,11 @@ public boolean isHomingTriggered() { return homingSwitch.isPressed(); } - public double computeAngle(Pose2d robotPose, Pose targetPose, double turretOffsetX, double turretOffsetY) { - double robotX = robotPose.getX(); - double robotY = robotPose.getY(); - double robotHeading = robotPose.getRotation().getRadians(); - - double turretX = robotX + turretOffsetX * Math.cos(robotHeading) - turretOffsetY * Math.sin(robotHeading); - double turretY = robotY + turretOffsetX * Math.sin(robotHeading) + turretOffsetY * Math.cos(robotHeading); - - double dx = targetPose.getX() - turretX; - double dy = targetPose.getY() - turretY; - - double targetAngleGlobal = Math.atan2(dy, dx); - double desiredTurretAngle = targetAngleGlobal - robotHeading; - double normalizedAngle = AngleUnit.normalizeRadians(desiredTurretAngle); - - telemetryM.addData(TurretConstants.kSubsystemName + "Computed Desired Angle to Goal", normalizedAngle); - return MathUtility.clamp(normalizedAngle, -Math.PI / 2, Math.PI / 2); - } - public Pose getTargetPose(GlobalConstants.AllianceColor allianceColor) { return allianceColor == GlobalConstants.AllianceColor.BLUE ? GlobalConstants.kBlueGoalPose : GlobalConstants.kRedGoalPose; } + + public boolean isTurretAutoTrackingEnabled() { + return enableTurretAutoTracking; + } } From 05e639744206f248d60434707549b533ea3c88a8 Mon Sep 17 00:00:00 2001 From: ultimatehecker <86735991+ultimatehecker@users.noreply.github.com> Date: Wed, 4 Feb 2026 23:26:12 -0500 Subject: [PATCH 02/10] fixed small bugs and add ability for togglable robotCentric --- .../ftc/teamcode/RobotContoller.java | 15 ++++++++++----- .../ftc/teamcode/commands/TeleopMecanum.java | 4 ++-- .../constants/DrivetrainConstants.java | 2 -- .../teamcode/constants/GlobalConstants.java | 18 ++++++++++++++++++ .../ftc/teamcode/subsystems/Drivetrain.java | 9 +++++++-- 5 files changed, 37 insertions(+), 11 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotContoller.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotContoller.java index 5fff143..a6df488 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotContoller.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotContoller.java @@ -104,8 +104,7 @@ public void initialize() { schedule( new RunCommand(() -> telemetryManager.update(telemetry)), - new RunCommand(led::update), - new RunCommand(() -> turret.setPosition(turret.computeAngle(drivetrain.getPose(), turret.getTargetPose(savedAllianceColor), 0, -3))) + new RunCommand(led::update) ); } @@ -155,19 +154,20 @@ public void initialize_loop() { drawUnlockedUI(elapsed); } else { drawLockedUI(); + setGlobalSettings(); } telemetryManager.update(telemetry); led.update(); } - public void initializeOpMode() { + private void initializeOpMode() { if (teleopInitialized) return; teleopInitialized = true; } @SuppressLint("DefaultLocale") - public void drawUnlockedUI(double elaspedTimeSinceStarted) { + private void drawUnlockedUI(double elaspedTimeSinceStarted) { telemetryManager.addData("Saved Location", savedLocation); telemetryManager.addData("Saved Autonomous Routine", savedAutonomousRoutine); telemetryManager.addData("Saved Alliance Selection", savedAllianceColor); @@ -181,13 +181,18 @@ public void drawUnlockedUI(double elaspedTimeSinceStarted) { telemetryManager.addLine("Locks automatically at 4s"); } - public void drawLockedUI() { + private void drawLockedUI() { telemetry.addLine("=== Configuration Locked ==="); telemetryManager.addData("Location", savedLocation); telemetryManager.addData("Auto", savedAutonomousRoutine); telemetryManager.addData("Alliance", savedAllianceColor); } + private void setGlobalSettings() { + GlobalConstants.allianceColor = savedAllianceColor; + GlobalConstants.opModeType = GlobalConstants.OpModeType.TELEOP; + } + private T cycleRight(T current, T[] values) { for (int i = 0; i < values.length; i++) { if (values[i] == current) return values[(i + 1) % values.length]; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/TeleopMecanum.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/TeleopMecanum.java index 144ec2b..10ac1b4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/TeleopMecanum.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/TeleopMecanum.java @@ -33,8 +33,8 @@ public void initialize() { public void execute() { drivetrain.setMovementVectors( forwardSupplier.getAsDouble(), - strafeSupplier.getAsDouble(), - rotationSupplier.getAsDouble(), + -strafeSupplier.getAsDouble(), + -rotationSupplier.getAsDouble(), robotCentric.getAsBoolean() ); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/DrivetrainConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/DrivetrainConstants.java index 892401d..d018efc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/DrivetrainConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/DrivetrainConstants.java @@ -17,8 +17,6 @@ public class DrivetrainConstants { public static final double kMaximumLinearVelocityInchesPerSecond = 5.27 * 12; public static final double kMaximumRotationRadiansPerSecond = 2 * Math.PI; - public static final double kShooterHeightFromFloor = 12; - public static final Pose kCloseGoalStartingPoseBlue = new Pose(24.250, 130.250, Math.toRadians(144)); public static final Pose kFarStartingPoseBlue = new Pose(56.000, 8.75, Math.toRadians(90)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/GlobalConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/GlobalConstants.java index 04485bb..8d4f569 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/GlobalConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/GlobalConstants.java @@ -14,10 +14,28 @@ public enum AllianceColor { BLUE } + public enum DriverType { + HANISH, + HECKER + } + public static OpModeType opModeType; public static AllianceColor allianceColor = AllianceColor.BLUE; + public static DriverType driverType = DriverType.HANISH; public static boolean kTuningMode = true; public static final Pose kBlueGoalPose = new Pose(9, 138); public static final Pose kRedGoalPose = kBlueGoalPose.mirror(); + + public static AllianceColor getCurrentAllianceColor() { + return allianceColor; + } + + public static OpModeType getCurrentOpModeType() { + return opModeType; + } + + public static DriverType getCurrentDriverType() { + return driverType; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java index a520667..ef169e0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java @@ -21,6 +21,7 @@ import org.firstinspires.ftc.teamcode.pedropathing.Constants; import lombok.Getter; +import lombok.Setter; public class Drivetrain extends SubsystemBase { private IMU imu; @@ -31,6 +32,9 @@ public class Drivetrain extends SubsystemBase { private KalmanFilter yFilter; private KalmanFilter headingFilter; + @Getter @Setter + private boolean isRobotCentric = false; + KalmanFilterParameters filterParameters = new KalmanFilterParameters( 0.01, // modelCovariance: how much you trust your model (lower = trust model more) 0.1 // dataCovariance: how noisy your vision data is (lower = trust vision more) @@ -100,12 +104,12 @@ public void resetVisionFilters(double x, double y, double heading) { headingFilter.reset(heading, 0.1, 1.0); } - public double getDistanceToPose3D(Pose3D targetPose, double robotZ) { + public double getDistanceToPose3D(Pose3D targetPose, double turretZ) { Pose2d robotPose = getPose(); double dx = targetPose.getPosition().x - robotPose.getX(); double dy = targetPose.getPosition().y - robotPose.getY(); - double dz = targetPose.getPosition().z - DrivetrainConstants.kShooterHeightFromFloor; + double dz = targetPose.getPosition().z - turretZ; return Math.sqrt(dx * dx + dy * dy + dz * dz); } @@ -161,4 +165,5 @@ public void update() { public boolean isFollowingTrajectory() { return follower.isBusy(); } + } From 74af75a7c61bb5b27ef0174cc90e54d87f94f501 Mon Sep 17 00:00:00 2001 From: ultimatehecker <86735991+ultimatehecker@users.noreply.github.com> Date: Wed, 4 Feb 2026 23:26:32 -0500 Subject: [PATCH 03/10] ball tracking --- .../teamcode/constants/TransferConstants.java | 1 + .../ftc/teamcode/subsystems/Transfer.java | 80 ++++++++++++++++++- 2 files changed, 78 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TransferConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TransferConstants.java index dd8e4de..5f980dd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TransferConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TransferConstants.java @@ -21,4 +21,5 @@ public class TransferConstants { public static final double kickerFeedPosition = 0.1; public static final double kFirstColorSensorDistanceThreshold = 2.00; // In inches + public static final double kSensorDebounceTime = 0.35; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java index f7a09ba..42c7902 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.subsystems; +import android.annotation.SuppressLint; + import com.bylazar.configurables.annotations.IgnoreConfigurable; import com.bylazar.telemetry.TelemetryManager; import com.qualcomm.hardware.rev.RevColorSensorV3; @@ -8,6 +10,7 @@ import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.library.command.SubsystemBase; +import org.firstinspires.ftc.library.utilities.Debouncer; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.constants.TransferConstants; @@ -20,10 +23,17 @@ public class Transfer extends SubsystemBase { private final Servo blockerServo; private final RevColorSensorV3 firstColorSensor; - private final DigitalChannel secondBeamBreak; private final DigitalChannel thirdBeamBreak; + private final Debouncer firstSensorDebouncer; + private final Debouncer secondSensorDebouncer; + private final Debouncer thirdSensorDebouncer; + + private boolean prevFirstDebounced = false; + private boolean prevSecondDebounced = false; + private boolean prevThirdDebounced = false; + @IgnoreConfigurable static TelemetryManager telemetryM; @@ -55,11 +65,30 @@ public Transfer(HardwareMap hMap, TelemetryManager telemetryM) { secondBeamBreak.setMode(DigitalChannel.Mode.INPUT); thirdBeamBreak.setMode(DigitalChannel.Mode.INPUT); + firstSensorDebouncer = new Debouncer(TransferConstants.kSensorDebounceTime, Debouncer.DebounceType.Both); + secondSensorDebouncer = new Debouncer(TransferConstants.kSensorDebounceTime, Debouncer.DebounceType.Both); + thirdSensorDebouncer = new Debouncer(TransferConstants.kSensorDebounceTime, Debouncer.DebounceType.Both); + this.telemetryM = telemetryM; } @Override public void periodic() { + boolean firstRaw = isFirstBeamBreakBroken(); + boolean secondRaw = isSecondBeamBroken(); + boolean thirdRaw = isThirdBeamBroken(); + + boolean firstDebounced = firstSensorDebouncer.calculate(firstRaw); + boolean secondDebounced = secondSensorDebouncer.calculate(secondRaw); + boolean thirdDebounced = thirdSensorDebouncer.calculate(thirdRaw); + + trackBallMovement(firstDebounced, secondDebounced, thirdDebounced); + + prevFirstDebounced = firstDebounced; + prevSecondDebounced = secondDebounced; + prevThirdDebounced = thirdDebounced; + + telemetryM.addData(TransferConstants.kSubsystemName + "Ball Count", currentNumberOfBalls); telemetryM.addData(TransferConstants.kSubsystemName + "fCS Distance Reading", firstCSDistance()); telemetryM.addData(TransferConstants.kSubsystemName + "sBB Status", isSecondBeamBroken()); telemetryM.addData(TransferConstants.kSubsystemName + "tBB Status", isThirdBeamBroken()); @@ -76,7 +105,52 @@ public void onInitialization(boolean initKicker, boolean initBlocker) { isBlockerEngaged = true; } - setCurrentNumberOfBalls((isFirstBeamBreakBroken() ? 1 : 0) + (isSecondBeamBroken() ? 1 : 0) + (isThirdBeamBroken() ? 1 : 0)); + boolean firstBroken = isFirstBeamBreakBroken(); + boolean secondBroken = isSecondBeamBroken(); + boolean thirdBroken = isThirdBeamBroken(); + + setCurrentNumberOfBalls((firstBroken ? 1 : 0) + (secondBroken ? 1 : 0) + (thirdBroken ? 1 : 0)); + + firstSensorDebouncer.reset(firstBroken); + secondSensorDebouncer.reset(secondBroken); + thirdSensorDebouncer.reset(thirdBroken); + + // Initialize previous states + prevFirstDebounced = firstBroken; + prevSecondDebounced = secondBroken; + prevThirdDebounced = thirdBroken; + } + + private void trackBallMovement(boolean first, boolean second, boolean third) { + if (first && !prevFirstDebounced) { + onBallEntered(); + } + + if (!third && prevThirdDebounced) { + onBallExited(); + } + + validateBallCount(first, second, third); + } + + private void onBallEntered() { + currentNumberOfBalls = Math.min(currentNumberOfBalls + 1, 3); + telemetryM.addData(TransferConstants.kSubsystemName + "Event", "Ball Entered"); + } + + private void onBallExited() { + currentNumberOfBalls = Math.max(currentNumberOfBalls - 1, 0); + telemetryM.addData(TransferConstants.kSubsystemName + "Event", "Ball Exited"); + } + + @SuppressLint("DefaultLocale") + private void validateBallCount(boolean first, boolean second, boolean third) { + int sensorCount = (first ? 1 : 0) + (second ? 1 : 0) + (third ? 1 : 0); + + // If there's a large discrepancy, log a warning + if (Math.abs(sensorCount - currentNumberOfBalls) > 1) { + telemetryM.addData(TransferConstants.kSubsystemName + "Warning", String.format("Count mismatch! Tracked: %d, Sensors: %d", currentNumberOfBalls, sensorCount)); + } } public void setKickerPosition(double position) { @@ -93,7 +167,7 @@ public void setBlockerPosition(double position) { blockerServo.setPosition(position); } - public double firstCSDistance() { + private double firstCSDistance() { return firstColorSensor.getDistance(DistanceUnit.INCH); } From 27b8019212db45a1fa2bd109b9b15973a64cdb64 Mon Sep 17 00:00:00 2001 From: ultimatehecker <86735991+ultimatehecker@users.noreply.github.com> Date: Thu, 5 Feb 2026 00:42:54 -0500 Subject: [PATCH 04/10] changed to prevent turret from resuming same speed (in case manual override) --- .../teamcode/commands/AimTowardShootingRegion.java | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/AimTowardShootingRegion.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/AimTowardShootingRegion.java index c0876d1..6294d74 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/AimTowardShootingRegion.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/AimTowardShootingRegion.java @@ -18,6 +18,9 @@ public class AimTowardShootingRegion extends CommandBase { private final Supplier allianceColorSupplier; private final BooleanSupplier autoTrackingEnabled; + public boolean wasTrackingEnabled = true; + + public AimTowardShootingRegion(final Turret turret, final Supplier currentRobotPose, final Supplier allianceColorSupplier, final BooleanSupplier autoTrackingEnabled) { this.turret = turret; this.currentRobotPose = currentRobotPose; @@ -30,9 +33,16 @@ public AimTowardShootingRegion(final Turret turret, final Supplier curre @Override public void execute() { Pose targetPose = turret.getTargetPose(allianceColorSupplier.get()); + boolean isEnabled = autoTrackingEnabled.getAsBoolean(); + + if (isEnabled) { + double desiredAngle = turret.computeAngle(currentRobotPose.get(), targetPose, TurretConstants.kTurretOffsetFromCenterOfRotationX, TurretConstants.kTurretOffsetFromCenterOfRotationY); + turret.setPosition(desiredAngle); + } else if(wasTrackingEnabled) { + turret.setManualPower(0.0); + } - double desiredAngle = turret.computeAngle(currentRobotPose.get(), targetPose, TurretConstants.kTurretOffsetFromCenterOfRotationX, TurretConstants.kTurretOffsetFromCenterOfRotationY); - if(autoTrackingEnabled.getAsBoolean()) turret.setPosition(desiredAngle); + wasTrackingEnabled = isEnabled; } @Override From 794d7e04a7f936aafd24b653c25a7a109791c3dc Mon Sep 17 00:00:00 2001 From: ultimatehecker <86735991+ultimatehecker@users.noreply.github.com> Date: Thu, 5 Feb 2026 22:46:40 -0500 Subject: [PATCH 05/10] shooter completely tuned --- .../command_factories/ShooterFactory.java | 2 +- .../commands/MaintainShooterNumericals.java | 40 +++++++++++++++++++ .../teamcode/constants/ShooterConstants.java | 18 +++++---- .../ftc/teamcode/subsystems/Shooter.java | 39 ++++++++++-------- 4 files changed, 74 insertions(+), 25 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/MaintainShooterNumericals.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/ShooterFactory.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/ShooterFactory.java index 95ecccd..be8d8a3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/ShooterFactory.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/ShooterFactory.java @@ -12,7 +12,7 @@ public static Command velocitySetpointCommand(Shooter shooter, DoubleSupplier se return Commands.runOnce(() -> { double velocity = setpoint.getAsDouble(); shooter.setVelocitySetpoint(velocity); - }).andThen(new WaitCommand(1500)).withName("Shooter Velocity"); + }).withName("Shooter Velocity"); } public static Command openLoopSetpointCommand(Shooter shooter, DoubleSupplier setpoint) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/MaintainShooterNumericals.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/MaintainShooterNumericals.java new file mode 100644 index 0000000..1f99a1a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/MaintainShooterNumericals.java @@ -0,0 +1,40 @@ +package org.firstinspires.ftc.teamcode.commands; + +import org.firstinspires.ftc.library.command.CommandBase; +import org.firstinspires.ftc.teamcode.constants.ShooterConstants; +import org.firstinspires.ftc.teamcode.subsystems.Shooter; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +public class MaintainShooterNumericals extends CommandBase { + private final Shooter shooter; + private final DoubleSupplier flywheelVelocitySetpoint; + private final DoubleSupplier hoodPositionSetpoint; + private final BooleanSupplier isFlywheelCommanded; + + public MaintainShooterNumericals(final Shooter shooter, final DoubleSupplier flywheelVelocitySetpoint, final DoubleSupplier hoodPositionSetpoint, final BooleanSupplier isFlywheelCommanded) { + this.shooter = shooter; + this.flywheelVelocitySetpoint = flywheelVelocitySetpoint; + this.hoodPositionSetpoint = hoodPositionSetpoint; + this.isFlywheelCommanded = isFlywheelCommanded; + + addRequirements(shooter); + } + + @Override + public void execute() { + if(isFlywheelCommanded.getAsBoolean()) { + shooter.setVelocitySetpoint(flywheelVelocitySetpoint.getAsDouble()); + } else { + shooter.setVelocitySetpoint(ShooterConstants.kShooterIdleRPM); + } + + shooter.setHoodPosition(hoodPositionSetpoint.getAsDouble()); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterConstants.java index 64a85b5..ea8f546 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterConstants.java @@ -9,21 +9,23 @@ public class ShooterConstants { public static final String hoodServoID = "hoodServo"; public static final String blockerServoID = "blockServo"; - public static final double shooterReadyRPM = 600; - public static final double hoodIdlePosition = 0.0; + public static final double kShooterIdleRPM = 600; + public static final double kShooterMaximumAchievableRPM = 5200; - public static double shooterRPM = 0; - public static double hoodPosition = 1; + public static final double kHoodMinimumPosition = 1.0; + public static final double kHoodMaximumPosition = 0.0; + + public static double kTuningFlywheelVelocitySetpoint = 0; + public static double kTuningHoodPositionSetpoint = kHoodMinimumPosition; - public static final double shooterMotorMaximumRPM = 6000; public static final double shooterMotorCPR = 28; - public static double kP = 0.000007; + public static double kP = 0.001; public static double kI = 0.0; public static double kD = 0.0; public static double kF = 0.0; - public static double kS = 0.043; - public static double kV = 0.0002; + public static double kS = 0.031; + public static double kV = 0.000165; public static double kA = 0.000001 * 12; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java index a3d4dfd..b52476d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java @@ -22,8 +22,8 @@ public class Shooter extends SubsystemBase { private final PIDFController velocityPIDFController; private final SimpleMotorFeedforward velocityFeedforward; - private final InterpLUT shooterInteroperableMap = new InterpLUT(); - private final InterpLUT hoodInteroperableMap = new InterpLUT(); + private final InterpLUT shooterInteroperableMap; + private final InterpLUT hoodInteroperableMap; @IgnoreConfigurable static TelemetryManager telemetryM; @@ -34,19 +34,26 @@ public Shooter(HardwareMap hardwareMap, TelemetryManager telemetryM) { hoodServo = hardwareMap.get(Servo.class, ShooterConstants.hoodServoID); - shooterInteroperableMap.add(460.0, 3500); - shooterInteroperableMap.add(578.0, 3600); - shooterInteroperableMap.add(675.2, 3800); - shooterInteroperableMap.add(710.5, 4100); - shooterInteroperableMap.add(782.3, 4150); - shooterInteroperableMap.createLUT(); - - hoodInteroperableMap.add(460.0, 0.4); - hoodInteroperableMap.add(578.0, 0.41); - hoodInteroperableMap.add(675.2, 0.43); - hoodInteroperableMap.add(710.5, 0.44); - hoodInteroperableMap.add(782.3, 0.445); - hoodInteroperableMap.createLUT(); + shooterInteroperableMap = new InterpLUT() + .add(20, 3200) + .add(47.259, 3600) + .add(63.864, 3800) + .add(80.426, 4100) + .add(96.217, 4300) + .add(132.282, 4700) + .add(148.92, 5000) + .add(200, 5600) + .createLUT(); + + hoodInteroperableMap = new InterpLUT() + .add(20, 1) + .add(47.259, 0.685) + .add(63.864, 0.45) + .add(80.426, 0.3) + .add(96.217, 0.2) + .add(132.282, 0.0) + .add(200, 0.0) + .createLUT(); velocityPIDFController = new PIDFController(ShooterConstants.kP, ShooterConstants.kI, ShooterConstants.kD, ShooterConstants.kF); velocityFeedforward = new SimpleMotorFeedforward(ShooterConstants.kS, ShooterConstants.kV, ShooterConstants.kA); @@ -58,7 +65,7 @@ public Shooter(HardwareMap hardwareMap, TelemetryManager telemetryM) { public void onInitialization() { shooterMotor.setPower(0.0); - hoodServo.setPosition(ShooterConstants.hoodIdlePosition); + hoodServo.setPosition(ShooterConstants.kHoodMinimumPosition); } @Override From d0eb0a27b0877f6f547a94c6d96b07af991cdd64 Mon Sep 17 00:00:00 2001 From: ultimatehecker <86735991+ultimatehecker@users.noreply.github.com> Date: Thu, 5 Feb 2026 22:47:37 -0500 Subject: [PATCH 06/10] tuned turret --- .../ftc/teamcode/constants/TurretConstants.java | 6 +++--- .../org/firstinspires/ftc/teamcode/subsystems/Turret.java | 6 +++++- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TurretConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TurretConstants.java index 6182bac..c4e7e1b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TurretConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TurretConstants.java @@ -13,10 +13,10 @@ public class TurretConstants { public static final String homingSwitchID = "tHS"; public static final double turretGearRatio = (24.0 / 48.0) * (135.0 / 26.0); - public static double pP = 2; + public static double pP = 2.5; public static double pI = 0.0; - public static double pD = 0.05; - public static double pF = 0.0; + public static double pD = 0.1; + public static double pF = 0.093; public static double tuningSetpoint = 0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java index cc7f1c6..a9363cf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java @@ -72,7 +72,7 @@ public void setPosition(double radians) { } primaryPositionController.setSetPoint(radians); - turretMotor.setPower(MathUtility.clamp(primaryPositionController.calculate(getCurrentPosition(), radians), -0.45, 0.45)); + turretMotor.setPower(MathUtility.clamp(primaryPositionController.calculate(getCurrentPosition(), radians), -0.55, 0.55)); } public double computeAngle(Pose2d robotPose, Pose targetPose, double turretOffsetX, double turretOffsetY) { @@ -127,4 +127,8 @@ public Pose getTargetPose(GlobalConstants.AllianceColor allianceColor) { public boolean isTurretAutoTrackingEnabled() { return enableTurretAutoTracking; } + + public boolean disableTurretAutoTracking() { + return enableTurretAutoTracking = false; + } } From e57821ce8fdc9afa06e348865588d47cb0884ecf Mon Sep 17 00:00:00 2001 From: ultimatehecker <86735991+ultimatehecker@users.noreply.github.com> Date: Thu, 5 Feb 2026 23:15:01 -0500 Subject: [PATCH 07/10] bulk reading and follow tracjectory for engame park --- .../autonomous/SelectableAutonomous.java | 18 ++++ .../commands/FollowTrajectoryCommand.java | 2 +- .../constants/DrivetrainConstants.java | 10 +++ .../ftc/teamcode/subsystems/Drivetrain.java | 87 +++++++++++++++---- .../utilities/SavedConfiguration.java | 8 +- 5 files changed, 101 insertions(+), 24 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/SelectableAutonomous.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/SelectableAutonomous.java index 3ddecdd..bc6bd58 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/SelectableAutonomous.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/SelectableAutonomous.java @@ -17,13 +17,20 @@ import org.firstinspires.ftc.library.command.WaitUntilCommand; import org.firstinspires.ftc.library.math.Pair; import org.firstinspires.ftc.library.utilities.Timing; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; import org.firstinspires.ftc.teamcode.commands.AimTowardShootingRegion; +import org.firstinspires.ftc.teamcode.commands.MaintainShooterNumericals; import org.firstinspires.ftc.teamcode.constants.DrivetrainConstants; import org.firstinspires.ftc.teamcode.constants.GlobalConstants; import org.firstinspires.ftc.teamcode.constants.LEDConstants; import org.firstinspires.ftc.teamcode.subsystems.Drivetrain; import org.firstinspires.ftc.teamcode.subsystems.Intake; import org.firstinspires.ftc.teamcode.subsystems.LED; +import org.firstinspires.ftc.teamcode.subsystems.Lift; import org.firstinspires.ftc.teamcode.subsystems.Shooter; import org.firstinspires.ftc.teamcode.subsystems.Transfer; import org.firstinspires.ftc.teamcode.subsystems.Turret; @@ -58,6 +65,7 @@ public enum AutoSelectState { private Turret turret; private Shooter shooter; private Vision vision; + private Lift lift; private LED led; private AutoChooser autoChooser; @@ -86,6 +94,7 @@ public void initialize() { shooter = new Shooter(hardwareMap, telemetryManager); vision = new Vision(hardwareMap, telemetryManager); led = new LED(hardwareMap, telemetryManager, lightsManager); + lift = new Lift(hardwareMap, telemetryManager); autoChooser = new AutoChooser(drivetrain, intake, transfer, turret, shooter, vision, led); @@ -122,6 +131,8 @@ public void initialize_loop() { telemetryManager.update(telemetry); lastTriangle = triangle; + + lift.onInitialization(); led.update(); } @@ -189,6 +200,13 @@ private void scheduleRoutine() { turret::isTurretAutoTrackingEnabled )); + shooter.setDefaultCommand(new MaintainShooterNumericals( + shooter, + () -> shooter.calculateFlywheelSpeeds(drivetrain.getDistanceToPose3D(GlobalConstants.getCurrentAllianceColor() == GlobalConstants.AllianceColor.BLUE ? GlobalConstants.kBlueGoalPose : GlobalConstants.kRedGoalPose, 38, 12)), + () -> shooter.calculateHoodPosition(drivetrain.getDistanceToPose3D(GlobalConstants.getCurrentAllianceColor() == GlobalConstants.AllianceColor.BLUE ? GlobalConstants.kBlueGoalPose : GlobalConstants.kRedGoalPose, 38, 12)), + () -> true + )); + drivetrain.setStartingPose(routine.getFirst()); transfer.onInitialization(true, true); drivetrain.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/FollowTrajectoryCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/FollowTrajectoryCommand.java index 0bfe789..42c9560 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/FollowTrajectoryCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/FollowTrajectoryCommand.java @@ -13,7 +13,7 @@ public class FollowTrajectoryCommand extends CommandBase { private final PathChain path; private boolean holdEnd; private double maxPower = 1.0; - private double completionThreshold = 0.985; + private double completionThreshold = 0.995; public FollowTrajectoryCommand(Drivetrain drivetrain, PathChain path) { this(drivetrain, path, true); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/DrivetrainConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/DrivetrainConstants.java index d018efc..1b167ac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/DrivetrainConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/DrivetrainConstants.java @@ -20,6 +20,16 @@ public class DrivetrainConstants { public static final Pose kCloseGoalStartingPoseBlue = new Pose(24.250, 130.250, Math.toRadians(144)); public static final Pose kFarStartingPoseBlue = new Pose(56.000, 8.75, Math.toRadians(90)); + public static final Pose kTopLeftParkingPoseBlue = new Pose(98, 26.5, Math.toRadians(315)); + public static final Pose kTopRightParkingPoseBlue = new Pose(98, 40, Math.toRadians(225)); + public static final Pose kBottomLeftParkingPoseBlue = new Pose(112.1, 26.5, Math.toRadians(45)); + public static final Pose kBottomRightParkingPoseBlue = new Pose(112.1, 40, Math.toRadians(135)); + + public static final Pose kTopLeftParkingPoseRed = kTopRightParkingPoseBlue.mirror(); + public static final Pose kTopRightParkingPoseRed = kTopLeftParkingPoseBlue.mirror(); + public static final Pose kBottomLeftParkingPoseRed = kBottomRightParkingPoseBlue.mirror(); + public static final Pose kBottomRightParkingPoseRed = kBottomLeftParkingPoseBlue.mirror(); + public static final Pose kAutoCloseShootingPositionBlue = new Pose(56, 84, Math.toRadians(180)); public static final Pose kAutoClosePickupOnePositionBlue = new Pose(15, 86, Math.toRadians(180)); public static final Pose kAutoClosePickupTwoControlPositionBlue = new Pose(59, 58, Math.toRadians(180)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java index ef169e0..9836b35 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java @@ -7,6 +7,8 @@ import com.pedropathing.paths.PathBuilder; import com.pedropathing.paths.PathChain; +import com.pedropathing.util.PoseHistory; +import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; @@ -18,15 +20,19 @@ import org.firstinspires.ftc.library.math.geometry.Rotation2d; import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; import org.firstinspires.ftc.teamcode.constants.DrivetrainConstants; +import org.firstinspires.ftc.teamcode.constants.GlobalConstants; import org.firstinspires.ftc.teamcode.pedropathing.Constants; +import org.firstinspires.ftc.teamcode.pedropathing.Drawing; + +import java.util.List; import lombok.Getter; import lombok.Setter; public class Drivetrain extends SubsystemBase { private IMU imu; - - private Follower follower; + private static Follower follower; + private List allHubs; private KalmanFilter xFilter; private KalmanFilter yFilter; @@ -35,6 +41,9 @@ public class Drivetrain extends SubsystemBase { @Getter @Setter private boolean isRobotCentric = false; + @Setter @Getter + private Pose drivetrainParkingPose = DrivetrainConstants.kTopLeftParkingPoseBlue; + KalmanFilterParameters filterParameters = new KalmanFilterParameters( 0.01, // modelCovariance: how much you trust your model (lower = trust model more) 0.1 // dataCovariance: how noisy your vision data is (lower = trust vision more) @@ -43,6 +52,9 @@ public class Drivetrain extends SubsystemBase { @IgnoreConfigurable static TelemetryManager telemetryM; + @IgnoreConfigurable + static PoseHistory poseHistory; + public Drivetrain(HardwareMap hMap, TelemetryManager telemetryM) { follower = Constants.createFollower(hMap); @@ -50,8 +62,16 @@ public Drivetrain(HardwareMap hMap, TelemetryManager telemetryM) { yFilter = new KalmanFilter(filterParameters); headingFilter = new KalmanFilter(filterParameters); - initializeImu(hMap); + allHubs = hMap.getAll(LynxModule.class); + for (LynxModule hub : allHubs) { + hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); + } + this.telemetryM = telemetryM; + poseHistory = follower.getPoseHistory(); + + initializeImu(hMap); + drawCurrent(); } public void initializeImu(HardwareMap hardwareMap) { @@ -69,6 +89,12 @@ public void periodic() { telemetryM.addData(DrivetrainConstants.kSubsystemName + "Pose X", getPose().getX()); telemetryM.addData(DrivetrainConstants.kSubsystemName + "Pose Y", getPose().getY()); telemetryM.addData(DrivetrainConstants.kSubsystemName + "Pose θ", getPose().getRotation().getDegrees()); + + for (LynxModule hub : allHubs) { + hub.clearBulkCache(); + } + + drawCurrentAndHistory(); } public void updateWithVision(Pose2d estimatedVisionPose) { @@ -104,22 +130,45 @@ public void resetVisionFilters(double x, double y, double heading) { headingFilter.reset(heading, 0.1, 1.0); } - public double getDistanceToPose3D(Pose3D targetPose, double turretZ) { + /** NOTE THAT THE POSE3D IS IN PEDRO COORDINATES */ + public double getDistanceToPose3D(Pose targetPose, double targetHeight, double turretZ) { Pose2d robotPose = getPose(); - double dx = targetPose.getPosition().x - robotPose.getX(); - double dy = targetPose.getPosition().y - robotPose.getY(); - double dz = targetPose.getPosition().z - turretZ; + double dx = targetPose.getX() - robotPose.getX(); + double dy = targetPose.getY() - robotPose.getY(); + double dz = targetHeight - turretZ; return Math.sqrt(dx * dx + dy * dy + dz * dz); } + public static void drawCurrent() { + try { + Drawing.drawRobot(follower.getPose()); + Drawing.sendPacket(); + } catch (Exception e) { + throw new RuntimeException("Drawing failed " + e); + } + } + + public static void drawCurrentAndHistory() { + Drawing.drawPoseHistory(poseHistory); + drawCurrent(); + } + public PathBuilder getPathBuilder() { return new PathBuilder(follower); } - public void startTeleopDriving() { - follower.startTeleopDrive(true); + public double getVelocity() { + return follower.getVelocity().getMagnitude(); + } + + public Pose2d getPose() { + return new Pose2d(follower.getPose().getX(), follower.getPose().getY(), Rotation2d.fromRadians(follower.getPose().getHeading())); + } + + public void setStartingPose(Pose pose) { + follower.setStartingPose(pose); } public void setMaxPower(final double maxPower) { @@ -146,24 +195,28 @@ public void resetHeading() { imu.resetYaw(); } - public double getVelocity() { - return follower.getVelocity().getMagnitude(); + public void startTeleopDriving() { + follower.startTeleopDrive(true); } - public Pose2d getPose() { - return new Pose2d(follower.getPose().getX(), follower.getPose().getY(), Rotation2d.fromRadians(follower.getPose().getHeading())); + public void update() { + follower.update(); } - public void setStartingPose(Pose pose) { - follower.setStartingPose(pose); + public void toggleRobotFieldCentric() { + isRobotCentric = !isRobotCentric; } - public void update() { - follower.update(); + public void manualResetPoseForAlliance() { + resetPose(GlobalConstants.getCurrentAllianceColor() == GlobalConstants.AllianceColor.BLUE ? new Pose(8.75, 7.5, 180).mirror() : new Pose(8.75, 7.5, 180)); } public boolean isFollowingTrajectory() { return follower.isBusy(); } + public boolean isDrivetrainAtSetpoint() { + return !follower.isBusy(); + } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilities/SavedConfiguration.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilities/SavedConfiguration.java index d52de17..5818ea7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilities/SavedConfiguration.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilities/SavedConfiguration.java @@ -12,15 +12,11 @@ private SavedConfiguration() {} public static Location selectedLocation = Location.CLOSE; public static Auto selectedAuto = Auto.IDLE; public static GlobalConstants.AllianceColor selectedAlliance = GlobalConstants.AllianceColor.BLUE; - public static Pose pathEndPose = new Pose(8, 8, 0); + public static Pose pathEndPose = new Pose(8.75, 7.5, 0); - public static Pose finalDrivetrainPose = new Pose(8.75, 7.5, 0); + public static Pose finalDrivetrainPose = GlobalConstants.getCurrentAllianceColor() == GlobalConstants.AllianceColor.BLUE ? new Pose(8.75, 7.5, 0).mirror() : new Pose(8.75, 7.5, 0); public static double finalDrivetrainVelocity = 0.0; public static double savedTurretPosition = 0.0; public static boolean autoLocked = false; - - public static void clear() { - autoLocked = false; - } } From 971d965caae7d39d149d0ac0a80821f12b2893f0 Mon Sep 17 00:00:00 2001 From: ultimatehecker <86735991+ultimatehecker@users.noreply.github.com> Date: Thu, 5 Feb 2026 23:17:00 -0500 Subject: [PATCH 08/10] added hecker bindings --- .../ftc/teamcode/TeleopBindings.java | 126 +++++++++++++----- .../ftc/teamcode/TestTeleop.java | 117 ---------------- .../ftc/teamcode/TestingOpMode.java | 60 ++++++--- .../teamcode/constants/GlobalConstants.java | 4 +- .../ftc/teamcode/pedropathing/Tuning.java | 2 +- 5 files changed, 133 insertions(+), 176 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestTeleop.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopBindings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopBindings.java index 119b152..4984e26 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopBindings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopBindings.java @@ -1,13 +1,17 @@ package org.firstinspires.ftc.teamcode; +import org.firstinspires.ftc.library.command.InstantCommand; import org.firstinspires.ftc.library.command.button.Trigger; import org.firstinspires.ftc.library.gamepad.GamepadEx; import org.firstinspires.ftc.library.gamepad.GamepadKeys; import org.firstinspires.ftc.teamcode.command_factories.IntakeFactory; import org.firstinspires.ftc.teamcode.command_factories.ShooterFactory; +import org.firstinspires.ftc.teamcode.command_factories.SuperstructureFactory; import org.firstinspires.ftc.teamcode.command_factories.TransferFactory; import org.firstinspires.ftc.teamcode.commands.AimTowardShootingRegion; +import org.firstinspires.ftc.teamcode.commands.MaintainShooterNumericals; import org.firstinspires.ftc.teamcode.commands.TeleopMecanum; +import org.firstinspires.ftc.teamcode.constants.DrivetrainConstants; import org.firstinspires.ftc.teamcode.constants.GlobalConstants; import org.firstinspires.ftc.teamcode.constants.LEDConstants; import org.firstinspires.ftc.teamcode.constants.ShooterConstants; @@ -26,37 +30,81 @@ private TeleopBindings() {} public static void configureBindings(GamepadEx driver, GamepadEx operator, Drivetrain drivetrain, Intake intake, Transfer transfer, Shooter shooter, Turret turret, Lift lift, LED led) { /* ------------------------------ Driver Controls ------------------------------ */ - new Trigger(() -> driver.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) - .whenActive(IntakeFactory.openLoopSetpointCommand(intake, () -> 1)) - .whenInactive(IntakeFactory.openLoopSetpointCommand(intake, () -> 0)); + if(GlobalConstants.getCurrentDriverType() == GlobalConstants.DriverType.HANISH) { + new Trigger(() -> driver.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) + .whenActive(IntakeFactory.openLoopSetpointCommand(intake, () -> 1)) + .whenInactive(IntakeFactory.openLoopSetpointCommand(intake, () -> 0)); - new Trigger(() -> driver.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) - .whenActive(ShooterFactory.velocitySetpointCommand(shooter, () -> 3500)) - .whenInactive(ShooterFactory.openLoopSetpointCommand(shooter, () -> 0)); + new Trigger(() -> driver.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) + .whenActive(ShooterFactory.velocitySetpointCommand(shooter, () -> 3500)) + .whenInactive(ShooterFactory.openLoopSetpointCommand(shooter, () -> 0)); + + driver.getGamepadButton(GamepadKeys.Button.SQUARE).toggleWhenActive( + () -> shooter.setHoodPosition(-2), + () -> shooter.setHoodPosition(2) + ); + + driver.getGamepadButton(GamepadKeys.Button.CIRCLE).whenPressed( + ShooterFactory.velocitySetpointCommand(shooter, () -> 3350).andThen(SuperstructureFactory.smartShootingCommand(intake, transfer, led)) + ); + + driver.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER) + .whenPressed(IntakeFactory.openLoopSetpointCommand(intake, () -> -1)) + .whenReleased(IntakeFactory.openLoopSetpointCommand(intake, () -> 0)); + + driver.getGamepadButton(GamepadKeys.Button.DPAD_UP) + .whenActive(() -> shooter.setHoodPosition(shooter.getHoodTargetPosition() + 0.001)); + + driver.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER) + .whenPressed(ShooterFactory.openLoopSetpointCommand(shooter, () -> -0.3)) + .whenReleased(ShooterFactory.openLoopSetpointCommand(shooter, () -> 0)); + } else { + new Trigger(() -> driver.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) + .whenActive(IntakeFactory.openLoopSetpointCommand(intake, () -> 1)) + .whenInactive(IntakeFactory.openLoopSetpointCommand(intake, () -> 0)); + + new Trigger(() -> driver.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) + .whenActive(SuperstructureFactory.smartShootingCommand(intake, transfer, led)); + + driver.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER) + .whenPressed(IntakeFactory.openLoopSetpointCommand(intake, () -> -1)) + .whenReleased(IntakeFactory.openLoopSetpointCommand(intake, () -> 0)); - driver.getGamepadButton(GamepadKeys.Button.SQUARE).toggleWhenActive( - () -> shooter.setHoodPosition(-2), - () -> shooter.setHoodPosition(2) - ); + driver.getGamepadButton(GamepadKeys.Button.RIGHT_STICK_BUTTON).toggleWhenPressed( + TransferFactory.engageBlocker(transfer, () -> TransferConstants.blockerAllowPosition), + TransferFactory.engageBlocker(transfer, () -> TransferConstants.blockerIdlePosition) + ); - driver.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER) - .whenPressed(IntakeFactory.openLoopSetpointCommand(intake, () -> -1)) - .whenReleased(IntakeFactory.openLoopSetpointCommand(intake, () -> 0)); + driver.getGamepadButton(GamepadKeys.Button.BACK).whenPressed( + SuperstructureFactory.auomaticallyParkAndLiftCommand(drivetrain, lift) + ); - driver.getGamepadButton(GamepadKeys.Button.DPAD_UP) - .whenActive(() -> shooter.setHoodPosition(shooter.getHoodTargetPosition() + 0.001)); + driver.getGamepadButton(GamepadKeys.Button.CIRCLE).whenPressed( + new InstantCommand(drivetrain::toggleRobotFieldCentric) + ); - driver.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER) - .whenPressed(ShooterFactory.openLoopSetpointCommand(shooter, () -> -0.3)) - .whenReleased(ShooterFactory.openLoopSetpointCommand(shooter, () -> 0)); + driver.getGamepadButton(GamepadKeys.Button.CROSS).whenPressed( + new InstantCommand(drivetrain::manualResetPoseForAlliance) + ); + + driver.getGamepadButton(GamepadKeys.Button.LEFT_STICK_BUTTON).whenPressed( + TransferFactory.runKickerCycle(transfer) + ); + } /* ------------------------------ Operator Controls ------------------------------ */ - new Trigger(() -> operator.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) - .whenActive(ShooterFactory.velocitySetpointCommand(shooter, () -> 4500)); + operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).and(operator.getGamepadButton(GamepadKeys.Button.DPAD_LEFT)) + .whenActive(new InstantCommand(() -> drivetrain.setDrivetrainParkingPose(GlobalConstants.getCurrentAllianceColor() == GlobalConstants.AllianceColor.BLUE ? DrivetrainConstants.kTopLeftParkingPoseBlue : DrivetrainConstants.kTopLeftParkingPoseRed))); + + operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).and(operator.getGamepadButton(GamepadKeys.Button.DPAD_RIGHT)) + .whenActive(new InstantCommand(() -> drivetrain.setDrivetrainParkingPose(GlobalConstants.getCurrentAllianceColor() == GlobalConstants.AllianceColor.BLUE ? DrivetrainConstants.kTopRightParkingPoseBlue : DrivetrainConstants.kTopRightParkingPoseRed))); - new Trigger(() -> operator.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) < 0.5) - .whenActive(ShooterFactory.velocitySetpointCommand(shooter, () -> 0)); + operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).and(operator.getGamepadButton(GamepadKeys.Button.DPAD_LEFT)) + .whenActive(new InstantCommand(() -> drivetrain.setDrivetrainParkingPose(GlobalConstants.getCurrentAllianceColor() == GlobalConstants.AllianceColor.BLUE ? DrivetrainConstants.kBottomLeftParkingPoseBlue : DrivetrainConstants.kBottomLeftParkingPoseRed))); + + operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).and(operator.getGamepadButton(GamepadKeys.Button.DPAD_RIGHT)) + .whenActive(new InstantCommand(() -> drivetrain.setDrivetrainParkingPose(GlobalConstants.getCurrentAllianceColor() == GlobalConstants.AllianceColor.BLUE ? DrivetrainConstants.kBottomRightParkingPoseBlue : DrivetrainConstants.kBottomRightParkingPoseRed))); operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER) .whenPressed(() -> lift.setPower(1)) @@ -69,24 +117,25 @@ public static void configureBindings(GamepadEx driver, GamepadEx operator, Drive operator.getGamepadButton(GamepadKeys.Button.SQUARE) .whenPressed(TransferFactory.runKickerCycle(transfer)); - operator.getGamepadButton(GamepadKeys.Button.DPAD_RIGHT).toggleWhenPressed( - () -> transfer.setBlockerPosition(TransferConstants.blockerAllowPosition), - () -> transfer.setBlockerPosition(TransferConstants.blockerIdlePosition) - ); - - new Trigger(() -> operator.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) + new Trigger(() -> operator.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) .whenActive(ShooterFactory.openLoopSetpointCommand(shooter, () -> 0.75)) .whenInactive(ShooterFactory.openLoopSetpointCommand(shooter, () -> 0)); + + new Trigger(transfer::doesTransferContainAnyBalls) + .whenActive(new InstantCommand(() -> shooter.setFlywheelCommanded(true))) + .whenInactive(new InstantCommand(() -> shooter.setFlywheelCommanded(false))); } public static void configureDefaultCommands(GamepadEx driver, GamepadEx operator, Drivetrain drivetrain, Intake intake, Transfer transfer, Shooter shooter, Turret turret, LED led) { - drivetrain.setDefaultCommand(new TeleopMecanum( - drivetrain, - driver::getLeftY, - driver::getLeftX, - driver::getRightX, - drivetrain::isRobotCentric - )); + if(GlobalConstants.getCurrentOpModeType() == GlobalConstants.OpModeType.TELEOP) { + drivetrain.setDefaultCommand(new TeleopMecanum( + drivetrain, + driver::getLeftY, + driver::getLeftX, + driver::getRightX, + drivetrain::isRobotCentric + )); + } turret.setDefaultCommand(new AimTowardShootingRegion( turret, @@ -94,5 +143,12 @@ public static void configureDefaultCommands(GamepadEx driver, GamepadEx operator GlobalConstants::getCurrentAllianceColor, turret::isTurretAutoTrackingEnabled )); + + shooter.setDefaultCommand(new MaintainShooterNumericals( + shooter, + () -> shooter.calculateFlywheelSpeeds(drivetrain.getDistanceToPose3D(shooter.getTargetPose(GlobalConstants.getCurrentAllianceColor()), 38, 12)), + () -> shooter.calculateHoodPosition(drivetrain.getDistanceToPose3D(shooter.getTargetPose(GlobalConstants.getCurrentAllianceColor()), 38, 12)), + shooter::isFlywheelCommanded + )); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestTeleop.java deleted file mode 100644 index a47e01d..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestTeleop.java +++ /dev/null @@ -1,117 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import com.bylazar.configurables.annotations.IgnoreConfigurable; -import com.bylazar.lights.LightsManager; -import com.bylazar.lights.PanelsLights; -import com.bylazar.telemetry.PanelsTelemetry; -import com.bylazar.telemetry.TelemetryManager; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.library.command.CommandOpMode; -import org.firstinspires.ftc.library.command.RunCommand; -import org.firstinspires.ftc.library.command.button.Trigger; -import org.firstinspires.ftc.library.gamepad.GamepadEx; -import org.firstinspires.ftc.library.gamepad.GamepadKeys; -import org.firstinspires.ftc.teamcode.command_factories.IntakeFactory; -import org.firstinspires.ftc.teamcode.command_factories.LEDFactory; -import org.firstinspires.ftc.teamcode.command_factories.ShooterFactory; -import org.firstinspires.ftc.teamcode.command_factories.TransferFactory; -import org.firstinspires.ftc.teamcode.command_factories.TurretFactory; -import org.firstinspires.ftc.teamcode.commands.TeleopMecanum; -import org.firstinspires.ftc.teamcode.constants.GlobalConstants; -import org.firstinspires.ftc.teamcode.constants.LEDConstants; -import org.firstinspires.ftc.teamcode.constants.ShooterConstants; -import org.firstinspires.ftc.teamcode.constants.TransferConstants; -import org.firstinspires.ftc.teamcode.subsystems.Drivetrain; -import org.firstinspires.ftc.teamcode.subsystems.Intake; -import org.firstinspires.ftc.teamcode.subsystems.LED; -import org.firstinspires.ftc.teamcode.subsystems.Shooter; -import org.firstinspires.ftc.teamcode.subsystems.Transfer; -import org.firstinspires.ftc.teamcode.subsystems.Turret; -import org.firstinspires.ftc.teamcode.subsystems.Vision; - -@TeleOp(name="TestTeleop", group = "TeleOp") -public class TestTeleop extends CommandOpMode { - - private Drivetrain drivetrain; - private Intake intake; - private Shooter shooter; - private Transfer transfer; - private Turret turret; - private Vision vision; - private LED led; - - private GamepadEx driverController; - - @IgnoreConfigurable - static TelemetryManager telemetryManager; - - @IgnoreConfigurable - static LightsManager lightsManager; - - @Override - public void initialize(){ - GlobalConstants.allianceColor = GlobalConstants.AllianceColor.BLUE; - - telemetryManager = PanelsTelemetry.INSTANCE.getTelemetry(); - lightsManager = PanelsLights.INSTANCE.getLights(); - - drivetrain = new Drivetrain(hardwareMap, telemetryManager); - intake = new Intake(hardwareMap, telemetryManager); - shooter = new Shooter(hardwareMap, telemetryManager); - transfer = new Transfer(hardwareMap, telemetryManager); - turret = new Turret(hardwareMap, telemetryManager); - vision = new Vision(hardwareMap, telemetryManager); - led = new LED(hardwareMap, telemetryManager, lightsManager); - - driverController = new GamepadEx(gamepad1); - - shooter.onInitialization(); - transfer.onInitialization(true, true); - - drivetrain.setDefaultCommand(new TeleopMecanum( - drivetrain, - () -> driverController.getLeftY(), - () -> driverController.getLeftX(), - () -> driverController.getRightX(), - () -> false - )); - - new Trigger(() -> driverController.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5 ) - .whenActive(IntakeFactory.openLoopSetpointCommand(intake, () -> 1)) - .whenInactive(IntakeFactory.openLoopSetpointCommand(intake, () -> 0) - ); - - driverController.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER) - .whenPressed(IntakeFactory.openLoopSetpointCommand(intake, () -> -1)) - .whenReleased(IntakeFactory.openLoopSetpointCommand(intake, () -> 0) - ); - - driverController.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER) - .toggleWhenActive( - TurretFactory.positionSetpointCommand(turret, () -> (1 * Math.PI) / 4), - TurretFactory.positionSetpointCommand(turret, () -> 0) - ); - - new Trigger(() -> driverController.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) - .whenActive(ShooterFactory.velocitySetpointCommand(shooter, () -> ShooterConstants.shooterRPM)) - .whenInactive(ShooterFactory.velocitySetpointCommand(shooter, () -> 0) - ); - - driverController.getGamepadButton(GamepadKeys.Button.SQUARE).toggleWhenActive( - ShooterFactory.hoodPositionCommand(shooter, () -> ShooterConstants.hoodPosition), - ShooterFactory.hoodPositionCommand(shooter, () -> 0) - ); - - driverController.getGamepadButton(GamepadKeys.Button.DPAD_UP).toggleWhenActive( - TransferFactory.engageBlocker(transfer, () -> TransferConstants.blockerEnabled), - TransferFactory.engageBlocker(transfer, () -> 0) - ); - - driverController.getGamepadButton(GamepadKeys.Button.DPAD_LEFT) - .whenPressed(LEDFactory.setConstantColorCommand(led, LEDConstants.ColorValue.VIOLET) - ); - - schedule(new RunCommand(() -> telemetryManager.update(telemetry))); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestingOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestingOpMode.java index 951138f..c2006ad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestingOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestingOpMode.java @@ -12,24 +12,33 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.firstinspires.ftc.teamcode.constants.DrivetrainConstants; +import org.firstinspires.ftc.teamcode.constants.GlobalConstants; import org.firstinspires.ftc.teamcode.constants.LEDConstants; +import org.firstinspires.ftc.teamcode.constants.ShooterConstants; import org.firstinspires.ftc.teamcode.constants.TurretConstants; import org.firstinspires.ftc.teamcode.pedropathing.Constants; +import org.firstinspires.ftc.teamcode.subsystems.Drivetrain; import org.firstinspires.ftc.teamcode.subsystems.Intake; import org.firstinspires.ftc.teamcode.subsystems.LED; import org.firstinspires.ftc.teamcode.subsystems.Shooter; import org.firstinspires.ftc.teamcode.subsystems.Turret; import org.firstinspires.ftc.teamcode.subsystems.Vision; +import java.util.Optional; + @TeleOp(name="TestingOpMode", group="TeleOp") public class TestingOpMode extends OpMode { public Intake intake; public Turret turret; public LED led; public Shooter shooter; - private Vision vison; - - public Follower follower; + public Drivetrain drivetrain; @IgnoreConfigurable private TelemetryManager telemetryManager; @@ -42,46 +51,55 @@ public void init() { telemetryManager = PanelsTelemetry.INSTANCE.getTelemetry(); lightsManager = PanelsLights.INSTANCE.getLights(); + drivetrain = new Drivetrain(hardwareMap, telemetryManager); intake = new Intake(hardwareMap, telemetryManager); shooter = new Shooter(hardwareMap, telemetryManager); turret = new Turret(hardwareMap, telemetryManager); led = new LED(hardwareMap, telemetryManager, lightsManager); - follower = Constants.createFollower(hardwareMap); - follower.setStartingPose(new Pose(0,0)); - follower.update(); - } + GlobalConstants.allianceColor= GlobalConstants.AllianceColor.RED; + GlobalConstants.opModeType = GlobalConstants.OpModeType.TELEOP; - @Override - public void start() { - follower.startTeleopDrive(true); + drivetrain.setStartingPose(DrivetrainConstants.decideToFlipPose(GlobalConstants.allianceColor, DrivetrainConstants.kCloseGoalStartingPoseBlue)); + drivetrain.update(); } @Override public void loop() { if(gamepad1.right_trigger > 0.5) { - shooter.setVelocitySetpoint(5200); + shooter.setVelocitySetpoint(ShooterConstants.kTuningFlywheelVelocitySetpoint); } else if(gamepad1.right_trigger < 0.5) { - shooter.setVelocitySetpoint(0); + shooter.setVelocitySetpoint(1); } - if(gamepad1.dpad_left) { - turret.setPosition(TurretConstants.tuningSetpoint); + if(gamepad1.left_trigger > 0.5) { + intake.setOpenLoopSetpoint(1); + } else if(gamepad1.left_bumper) { + intake.setOpenLoopSetpoint(-1); + } else if(gamepad1.left_trigger < 0.5) { + intake.setOpenLoopSetpoint(0); + } + + if(gamepad1.square) { + shooter.setHoodPosition(ShooterConstants.kTuningHoodPositionSetpoint); + } else if(!gamepad1.square) { + shooter.setHoodPosition(0); + } + + if(gamepad1.dpad_down) { + turret.setPosition(1.57); led.setSolid(LEDConstants.ColorValue.BLUE); - } else if(!gamepad1.dpad_left) { + } else if(!gamepad1.dpad_down) { turret.setPosition(0); led.setSolid(LEDConstants.ColorValue.GREEN); } + drivetrain.periodic(); shooter.periodic(); turret.periodic(); + drivetrain.update(); - follower.update(); - follower.setTeleOpDrive(gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x, true); - - telemetryManager.addData("Follower Pose X", follower.getPose().getX()); - telemetryManager.addData("Follower Pose Y", follower.getPose().getY()); - telemetryManager.addData("Follower Pose Rotation", follower.getPose().getHeading()); + telemetryManager.addData("Distance Reading", drivetrain.getDistanceToPose3D(turret.getTargetPose(GlobalConstants.getCurrentAllianceColor()), 38, 12)); telemetryManager.update(telemetry); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/GlobalConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/GlobalConstants.java index 8d4f569..cc358ce 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/GlobalConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/GlobalConstants.java @@ -21,10 +21,10 @@ public enum DriverType { public static OpModeType opModeType; public static AllianceColor allianceColor = AllianceColor.BLUE; - public static DriverType driverType = DriverType.HANISH; + public static DriverType driverType = DriverType.HECKER; public static boolean kTuningMode = true; - public static final Pose kBlueGoalPose = new Pose(9, 138); + public static final Pose kBlueGoalPose = new Pose(4, 140); public static final Pose kRedGoalPose = kBlueGoalPose.mirror(); public static AllianceColor getCurrentAllianceColor() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedropathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedropathing/Tuning.java index fe4e21a..5a472c6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedropathing/Tuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedropathing/Tuning.java @@ -1411,7 +1411,7 @@ public void loop() { * @author Lazar - 19234 * @version 1.1, 5/19/2025 */ -class Drawing { +public class Drawing { public static final double ROBOT_RADIUS = 9; // woah private static final FieldManager panelsField = PanelsField.INSTANCE.getField(); From 35b4c96a67a619e818e1864a90f3c613d4836833 Mon Sep 17 00:00:00 2001 From: ultimatehecker <86735991+ultimatehecker@users.noreply.github.com> Date: Thu, 5 Feb 2026 23:18:32 -0500 Subject: [PATCH 09/10] auto shooting command now works --- .../SuperstructureFactory.java | 101 +++++++++++++++--- .../teamcode/constants/ShooterConstants.java | 6 +- .../ftc/teamcode/subsystems/Shooter.java | 13 ++- 3 files changed, 100 insertions(+), 20 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/SuperstructureFactory.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/SuperstructureFactory.java index 97e067b..7382cc1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/SuperstructureFactory.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/SuperstructureFactory.java @@ -1,14 +1,25 @@ package org.firstinspires.ftc.teamcode.command_factories; +import androidx.core.math.MathUtils; + +import com.pedropathing.geometry.BezierCurve; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.paths.PathBuilder; + import org.firstinspires.ftc.library.command.Command; import org.firstinspires.ftc.library.command.Commands; import org.firstinspires.ftc.library.command.ConditionalCommand; import org.firstinspires.ftc.library.command.ParallelCommandGroup; +import org.firstinspires.ftc.library.command.ParallelDeadlineGroup; import org.firstinspires.ftc.library.command.WaitCommand; +import org.firstinspires.ftc.library.math.MathUtility; +import org.firstinspires.ftc.teamcode.constants.DrivetrainConstants; import org.firstinspires.ftc.teamcode.constants.LEDConstants; import org.firstinspires.ftc.teamcode.constants.TransferConstants; +import org.firstinspires.ftc.teamcode.subsystems.Drivetrain; import org.firstinspires.ftc.teamcode.subsystems.Intake; import org.firstinspires.ftc.teamcode.subsystems.LED; +import org.firstinspires.ftc.teamcode.subsystems.Lift; import org.firstinspires.ftc.teamcode.subsystems.Shooter; import org.firstinspires.ftc.teamcode.subsystems.Transfer; @@ -16,7 +27,8 @@ import java.util.function.DoubleSupplier; public class SuperstructureFactory { - public static Command shooterSmartVelocityRampCommand(Intake intake, Transfer transfer, Shooter shooter, LED led, DoubleSupplier shooterRPM, DoubleSupplier hoodPosition) { + @Deprecated + public static Command smartVelocityRampCommand(Intake intake, Transfer transfer, Shooter shooter, LED led, DoubleSupplier shooterRPM, DoubleSupplier hoodPosition) { return Commands.sequence( // TODO: Add a check to see if we have balls, if not then just return (maybe add it to the blink array) new ConditionalCommand( Commands.none(), @@ -36,28 +48,85 @@ public static Command shooterSmartVelocityRampCommand(Intake intake, Transfer tr ); } - public static Command controlledShootingCommand(Intake intake, Transfer transfer, Shooter shooter, LED led) { - return Commands.sequence( // TODO: Add a check to see if we have balls, if not then just return (maybe add it to the blink array) - new ConditionalCommand( - new ParallelCommandGroup( - TransferFactory.engageBlocker(transfer, () -> TransferConstants.blockerIdlePosition), - LEDFactory.setAdvancedStandardBlinkingCommand(led, LEDConstants.ColorValue.ORANGE, LEDConstants.ColorValue.VIOLET, () -> 50) + public static Command smartShootingCommand(Intake intake, Transfer transfer, LED led) { + return Commands.sequence( + Commands.either( + Commands.sequence( + new ParallelCommandGroup( + TransferFactory.engageBlocker(transfer, () -> TransferConstants.blockerAllowPosition), + LEDFactory.setAdvancedStandardBlinkingCommand(led, LEDConstants.ColorValue.ORANGE, LEDConstants.ColorValue.VIOLET, () -> 50) + ), + shootOneBallSensoredCommand(intake, transfer), + Commands.either( + Commands.sequence( + indexBallsSensoredCommand(intake, transfer), + shootOneBallSensoredCommand(intake, transfer) + ), + Commands.none(), + () -> transfer.getCurrentNumberOfBalls() > 2 // Check at sequence start + ), + Commands.either( + Commands.sequence( + indexBallsSensoredCommand(intake, transfer), + shootOneBallSensoredCommand(intake, transfer) + ), + Commands.none(), + () -> transfer.getCurrentNumberOfBalls() > 1 // Check at sequence start + ) ), Commands.none(), - transfer::isBlockerEngaged + () -> transfer.getCurrentNumberOfBalls() > 0 ), - IntakeFactory.openLoopSetpointCommand(intake, () -> 1), - new WaitCommand(250), - IntakeFactory.openLoopSetpointCommand(intake, () -> 0), - new WaitCommand(250) + TransferFactory.engageBlocker(transfer, () -> TransferConstants.blockerIdlePosition) + ); + } + + public static Command auomaticallyParkAndLiftCommand(Drivetrain drivetrain, Lift lift) { + return Commands.runEnd( + () -> { + PathBuilder otfToParkingPath = drivetrain.getPathBuilder(); + + otfToParkingPath.addPath( + new BezierLine( + drivetrain.getDrivetrainParkingPose(), + DrivetrainConstants.kTopRightParkingPoseBlue + ) + ); + otfToParkingPath.setLinearHeadingInterpolation(drivetrain.getPose().getAsPedroPose().getHeading(), DrivetrainConstants.kTopRightParkingPoseBlue.getHeading()); + + drivetrain.followTrajectory(otfToParkingPath.build(), true); + + if(drivetrain.isDrivetrainAtSetpoint() && drivetrain.getVelocity() == 0) { + lift.setPosition(1000); + } + }, + () -> lift.setPower(0.0), + drivetrain, lift + ).until(lift::isAtSetpoint); + } + + private static Command shootOneBallSensoredCommand(Intake intake, Transfer transfer) { + return Commands.sequence( + IntakeFactory.openLoopSetpointCommand(intake, () -> 1.0), + Commands.waitMillis(100), + Commands.waitUntil(transfer::isThirdBeamBroken).withTimeout(750), + new ConditionalCommand( + TransferFactory.runKickerCycle(transfer), + Commands.none(), + transfer::doesTransferContainSingleBall + ), + Commands.waitUntil(() -> !transfer.isThirdBeamBroken()).withTimeout(500), + IntakeFactory.openLoopSetpointCommand(intake, () -> 0.0) ); } - public static Command smartIntakingCommand(Intake intake, Transfer transfer, Shooter shooter, LED led, DoubleSupplier shooterRPM, DoubleSupplier hoodPosition) { + private static Command indexBallsSensoredCommand(Intake intake, Transfer transfer) { return Commands.sequence( - IntakeFactory.setFrontIntakeOpenLoopSetpointCommand(intake, () -> 1), - IntakeFactory.setRearIntakeOpenLoopSetpointCommand(intake, () -> 1) - //Commands.waitUntil(transfer::fir) + Commands.waitMillis(250), + IntakeFactory.openLoopSetpointCommand(intake, () -> 0.3), + Commands.waitUntil(transfer::isThirdBeamBroken).withTimeout(2000), + IntakeFactory.openLoopSetpointCommand(intake, () -> 0.0), + Commands.waitMillis(100) ); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterConstants.java index ea8f546..27f3a88 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterConstants.java @@ -9,8 +9,8 @@ public class ShooterConstants { public static final String hoodServoID = "hoodServo"; public static final String blockerServoID = "blockServo"; - public static final double kShooterIdleRPM = 600; - public static final double kShooterMaximumAchievableRPM = 5200; + public static final double kShooterIdleRPM = 2000; + public static final double kShooterMaximumAchievableRPM = 5600; public static final double kHoodMinimumPosition = 1.0; public static final double kHoodMaximumPosition = 0.0; @@ -20,7 +20,7 @@ public class ShooterConstants { public static final double shooterMotorCPR = 28; - public static double kP = 0.001; + public static double kP = 0.0015; public static double kI = 0.0; public static double kD = 0.0; public static double kF = 0.0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java index b52476d..9156c3b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java @@ -2,6 +2,7 @@ import com.bylazar.configurables.annotations.IgnoreConfigurable; import com.bylazar.telemetry.TelemetryManager; +import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -15,6 +16,9 @@ import org.firstinspires.ftc.teamcode.constants.GlobalConstants; import org.firstinspires.ftc.teamcode.constants.ShooterConstants; +import lombok.Getter; +import lombok.Setter; + public class Shooter extends SubsystemBase { private final Servo hoodServo; private final DcMotorEx shooterMotor; @@ -25,6 +29,9 @@ public class Shooter extends SubsystemBase { private final InterpLUT shooterInteroperableMap; private final InterpLUT hoodInteroperableMap; + @Getter @Setter + private boolean isFlywheelCommanded = true; + @IgnoreConfigurable static TelemetryManager telemetryM; @@ -35,7 +42,7 @@ public Shooter(HardwareMap hardwareMap, TelemetryManager telemetryM) { hoodServo = hardwareMap.get(Servo.class, ShooterConstants.hoodServoID); shooterInteroperableMap = new InterpLUT() - .add(20, 3200) + .add(20, 3150) .add(47.259, 3600) .add(63.864, 3800) .add(80.426, 4100) @@ -96,6 +103,10 @@ public void setOpenLoopSetpoint(double speed) { shooterMotor.setPower(speed); } + public Pose getTargetPose(GlobalConstants.AllianceColor allianceColor) { + return allianceColor == GlobalConstants.AllianceColor.BLUE ? GlobalConstants.kBlueGoalPose : GlobalConstants.kRedGoalPose; + } + public void setHoodPosition(double position) { hoodServo.setPosition(position); } From d519c9da3798f89cdad9df04ba30f262cbfaab0c Mon Sep 17 00:00:00 2001 From: ultimatehecker <86735991+ultimatehecker@users.noreply.github.com> Date: Thu, 5 Feb 2026 23:19:06 -0500 Subject: [PATCH 10/10] changes throughout the day --- .../ftc/teamcode/autonomous/AutoFactory.java | 64 +++++++++++-------- .../teamcode/constants/TransferConstants.java | 2 +- .../ftc/teamcode/subsystems/Drivetrain.java | 4 +- .../ftc/teamcode/subsystems/LED.java | 29 ++++++--- .../ftc/teamcode/subsystems/Lift.java | 12 ++++ .../ftc/teamcode/subsystems/Transfer.java | 9 +++ 6 files changed, 82 insertions(+), 38 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFactory.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFactory.java index 590fd55..6350a6e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFactory.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFactory.java @@ -17,6 +17,7 @@ import org.firstinspires.ftc.teamcode.command_factories.ShooterFactory; import org.firstinspires.ftc.teamcode.command_factories.SuperstructureFactory; import org.firstinspires.ftc.teamcode.command_factories.TransferFactory; +import org.firstinspires.ftc.teamcode.command_factories.TurretFactory; import org.firstinspires.ftc.teamcode.commands.FollowTrajectoryCommand; import org.firstinspires.ftc.teamcode.constants.DrivetrainConstants; import org.firstinspires.ftc.teamcode.constants.GlobalConstants; @@ -148,13 +149,18 @@ public Pair> initializeFarSixBall(GlobalConstants.Alli DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarParkingPositionBlue), Commands.sequence( new FollowTrajectoryCommand(drivetrain, createdPath.getPath(0), true, 1), - new WaitCommand(1000), + SuperstructureFactory.smartShootingCommand(intake, transfer, led), + IntakeFactory.openLoopSetpointCommand(intake, () -> 1), new FollowTrajectoryCommand(drivetrain, createdPath.getPath(1), true, 1), new WaitCommand(1000), new FollowTrajectoryCommand(drivetrain, createdPath.getPath(2), true, 1), - new WaitCommand(1000), - new FollowTrajectoryCommand(drivetrain, createdPath.getPath(3), true, 1), - new WaitCommand(1000) + SuperstructureFactory.smartShootingCommand(intake, transfer, led), + new InstantCommand(turret::disableTurretAutoTracking), + IntakeFactory.openLoopSetpointCommand(intake, () -> 0), + new ParallelCommandGroup( + new FollowTrajectoryCommand(drivetrain, createdPath.getPath(3), true, 1), + TurretFactory.positionSetpointCommand(turret, () -> 0.0) + ) ) ) ); @@ -228,25 +234,25 @@ public Pair> initializeFarNineBall(GlobalConstants.All DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarParkingPositionBlue), Commands.sequence( new FollowTrajectoryCommand(drivetrain, createdPath.getPath(0), true, 1), - IntakeFactory.openLoopSetpointCommand(intake, () -> -1), - new WaitCommand(2000), + SuperstructureFactory.smartShootingCommand(intake, transfer, led), IntakeFactory.openLoopSetpointCommand(intake, () -> 1), new FollowTrajectoryCommand(drivetrain, createdPath.getPath(1), true, 1), new WaitCommand(1000), new FollowTrajectoryCommand(drivetrain, createdPath.getPath(2), true, 1), - IntakeFactory.openLoopSetpointCommand(intake, () -> -1), - new WaitCommand(2000), + SuperstructureFactory.smartShootingCommand(intake, transfer, led), IntakeFactory.openLoopSetpointCommand(intake, () -> 1), new FollowTrajectoryCommand(drivetrain, createdPath.getPath(3), true, 1), new WaitCommand(1000), new FollowTrajectoryCommand(drivetrain, createdPath.getPath(4), true, 1), new WaitCommand(1000), new FollowTrajectoryCommand(drivetrain, createdPath.getPath(5), true, 1), - IntakeFactory.openLoopSetpointCommand(intake, () -> -1), - new WaitCommand(2000), - IntakeFactory.openLoopSetpointCommand(intake, () -> 1), - new FollowTrajectoryCommand(drivetrain, createdPath.getPath(6), true, 1), - new WaitCommand(1000) + SuperstructureFactory.smartShootingCommand(intake, transfer, led), + new InstantCommand(turret::disableTurretAutoTracking), + IntakeFactory.openLoopSetpointCommand(intake, () -> 0), + new ParallelCommandGroup( + new FollowTrajectoryCommand(drivetrain, createdPath.getPath(6), true, 1), + TurretFactory.positionSetpointCommand(turret, () -> 0.0) + ) ) ) ); @@ -314,23 +320,23 @@ public Pair> initializeCloseNineBall(GlobalConstants.A DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseParkingPositionBlue), Commands.sequence( new FollowTrajectoryCommand(drivetrain, createdPath.getPath(0), true, 1), - IntakeFactory.openLoopSetpointCommand(intake, () -> -1), - new WaitCommand(2000), + SuperstructureFactory.smartShootingCommand(intake, transfer, led), IntakeFactory.openLoopSetpointCommand(intake, () -> 1), new FollowTrajectoryCommand(drivetrain, createdPath.getPath(1), true, 1), new WaitCommand(1000), new FollowTrajectoryCommand(drivetrain, createdPath.getPath(2), true, 1), - IntakeFactory.openLoopSetpointCommand(intake, () -> -1), - new WaitCommand(2000), + SuperstructureFactory.smartShootingCommand(intake, transfer, led), IntakeFactory.openLoopSetpointCommand(intake, () -> 1), new FollowTrajectoryCommand(drivetrain, createdPath.getPath(3), true, 1), new WaitCommand(1000), new FollowTrajectoryCommand(drivetrain, createdPath.getPath(4), true, 1), - IntakeFactory.openLoopSetpointCommand(intake, () -> -1), - new WaitCommand(2000), + SuperstructureFactory.smartShootingCommand(intake, transfer, led), + new InstantCommand(turret::disableTurretAutoTracking), IntakeFactory.openLoopSetpointCommand(intake, () -> 0), - new FollowTrajectoryCommand(drivetrain, createdPath.getPath(5), true, 1), - new WaitCommand(1000) + new ParallelCommandGroup( + new FollowTrajectoryCommand(drivetrain, createdPath.getPath(5), true, 1), + TurretFactory.positionSetpointCommand(turret, () -> 0.0) + ) ) ) ); @@ -399,17 +405,23 @@ public Pair> initializeCloseNineBallPickup(GlobalConst DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupThreePositionBlue), Commands.sequence( new FollowTrajectoryCommand(drivetrain, createdPath.getPath(0), true, 1), - new WaitCommand(1000), + SuperstructureFactory.smartShootingCommand(intake, transfer, led), + IntakeFactory.openLoopSetpointCommand(intake, () -> 1), new FollowTrajectoryCommand(drivetrain, createdPath.getPath(1), true, 1), new WaitCommand(1000), new FollowTrajectoryCommand(drivetrain, createdPath.getPath(2), true, 1), - new WaitCommand(1000), + SuperstructureFactory.smartShootingCommand(intake, transfer, led), + IntakeFactory.openLoopSetpointCommand(intake, () -> 1), new FollowTrajectoryCommand(drivetrain, createdPath.getPath(3), true, 1), new WaitCommand(1000), new FollowTrajectoryCommand(drivetrain, createdPath.getPath(4), true, 1), - new WaitCommand(1000), - new FollowTrajectoryCommand(drivetrain, createdPath.getPath(5), true, 1), - new WaitCommand(1000) + SuperstructureFactory.smartShootingCommand(intake, transfer, led), + new InstantCommand(turret::disableTurretAutoTracking), + IntakeFactory.openLoopSetpointCommand(intake, () -> 0), + new ParallelCommandGroup( + new FollowTrajectoryCommand(drivetrain, createdPath.getPath(5), true, 1), + TurretFactory.positionSetpointCommand(turret, () -> 0.0) + ) ) ) ); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TransferConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TransferConstants.java index 5f980dd..e7a6ab2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TransferConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TransferConstants.java @@ -14,7 +14,7 @@ public class TransferConstants { public static final String secondBeamBreakID = "tBB"; public static final double blockerEnabled = 0; - public static final double blockerIdlePosition = 1; + public static final double blockerIdlePosition = 0.95; public static final double kickerIdlePosition = 0.56; public static final double blockerAllowPosition = 0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java index 9836b35..8378ab1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java @@ -2,12 +2,13 @@ import com.bylazar.configurables.annotations.IgnoreConfigurable; import com.bylazar.telemetry.TelemetryManager; + import com.pedropathing.follower.Follower; import com.pedropathing.geometry.Pose; - import com.pedropathing.paths.PathBuilder; import com.pedropathing.paths.PathChain; import com.pedropathing.util.PoseHistory; + import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -18,7 +19,6 @@ import org.firstinspires.ftc.library.controller.KalmanFilterParameters; import org.firstinspires.ftc.library.math.geometry.Pose2d; import org.firstinspires.ftc.library.math.geometry.Rotation2d; -import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; import org.firstinspires.ftc.teamcode.constants.DrivetrainConstants; import org.firstinspires.ftc.teamcode.constants.GlobalConstants; import org.firstinspires.ftc.teamcode.pedropathing.Constants; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/LED.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/LED.java index 336441e..b22c5d9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/LED.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/LED.java @@ -62,12 +62,24 @@ public void periodic() { telemetryM.addData(LEDConstants.kSubsystemName + "Pattern Step", patternStep); ledIndicator.update(currentColor.getColorPosition()); lightsManager.update(); + + update(); } + private void setColor(LEDConstants.ColorValue color) { + this.currentColor = color; + ledServo.setPosition(color.getColorPosition()); + } public void setSolid(LEDConstants.ColorValue color) { mode = LedState.SOLID; primaryColorA = color; + + if (ledTimer.isTimerOn()) { + ledTimer.pause(); + } + + patternStep = 0; } public void setSimpleBlink(LEDConstants.ColorValue colorA, LEDConstants.ColorValue colorB, long intervalMs) { @@ -76,11 +88,7 @@ public void setSimpleBlink(LEDConstants.ColorValue colorA, LEDConstants.ColorVal primaryColorA = colorA; primaryColorB = colorB; - this.intervalMs = intervalMs; - patternStep = 0; - - ledTimer = new Timing.Timer(intervalMs, TimeUnit.MILLISECONDS); - ledTimer.start(); + resetTimer(intervalMs); } public void setDefaultSimpleBlink(LEDConstants.ColorValue color, long intervalMs) { @@ -134,8 +142,9 @@ public void update() { } break; case OFF: - if (ledTimer.isTimerOn()) { // Might need to remove if it effects the state logic + if (ledTimer.isTimerOn()) { ledTimer.pause(); + ledTimer = new Timing.Timer(0, TimeUnit.MILLISECONDS); // Reset } patternStep = 0; @@ -147,8 +156,10 @@ public void update() { } } - private void setColor(LEDConstants.ColorValue color) { - this.currentColor = color; - ledServo.setPosition(color.getColorPosition()); + private void resetTimer(long intervalMs) { + this.intervalMs = intervalMs; + ledTimer = new Timing.Timer(intervalMs, TimeUnit.MILLISECONDS); + ledTimer.start(); + patternStep = 0; } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Lift.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Lift.java index 33a48c2..1cde760 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Lift.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Lift.java @@ -67,11 +67,23 @@ public void periodic() { updateRelativePosition(); } + public void onInitialization() { + resetToZero(); + + if(isHomingSwitchTriggered()) { + stopHoming(); + } + } + public void setPosition(double positionRotations) { double motorPower = positonController.calculate(getRelativePosition(), positionRotations); setPower(motorPower); } + public boolean isAtSetpoint() { + return positonController.atSetPoint(); + } + private void updateRelativePosition() { if (!isRelativeInitialized) { lastAbsolutePosition = liftEncoder.getCurrentPosition(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java index 42c7902..d3375f6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java @@ -167,6 +167,15 @@ public void setBlockerPosition(double position) { blockerServo.setPosition(position); } + public boolean doesTransferContainSingleBall() { + return currentNumberOfBalls == 1; + } + + public boolean doesTransferContainAnyBalls() { + return currentNumberOfBalls > 0; + } + + private double firstCSDistance() { return firstColorSensor.getDistance(DistanceUnit.INCH); }