From 436eb40322086a6e2b1db91f7e418a3c1cd7ee5b Mon Sep 17 00:00:00 2001 From: Lucas Date: Fri, 20 Feb 2026 21:28:13 -0500 Subject: [PATCH 01/18] Feat: Add voltageOverride --- .../climberhopper/ClimberHopper.java | 4 ++- .../climberhopper/ClimberHopperImpl.java | 24 +++++++++------- .../climberhopper/ClimberHopperSim.java | 28 ++++++++++++------- 3 files changed, 35 insertions(+), 21 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java index b2d70988..57b5b350 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java @@ -5,6 +5,8 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.climberhopper; +import java.util.Optional; + import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; @@ -62,7 +64,7 @@ public void setState(ClimberHopperState state) { public abstract double getCurrentHeight(); public abstract boolean atTargetHeight(); - // public abstract void setVoltageOverride(Optional voltage); + public abstract void setVoltageOverride(Optional voltage); @Override public void periodic() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java index de373ca9..65e43712 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java @@ -54,23 +54,27 @@ public boolean atTargetHeight() { return isWithinTolerance(Settings.ClimberHopper.HEIGHT_TOLERANCE_METERS); } - // @Override - // public void setVoltageOverride(Optional voltage) { - // this.voltageOverride = voltage; - // } + @Override + public void setVoltageOverride(Optional voltage) { + this.voltageOverride = voltage; + } @Override public void periodic() { super.periodic(); - if (!atTargetHeight()) { - if (getCurrentHeight() < getState().getTargetHeight()) { - voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; + if (voltageOverride.isPresent()) { + voltage = voltageOverride.get(); + } else { + if (!atTargetHeight()) { + if (getCurrentHeight() < getState().getTargetHeight()) { + voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; + } else { + voltage = - Settings.ClimberHopper.MOTOR_VOLTAGE; + } } else { - voltage = - Settings.ClimberHopper.MOTOR_VOLTAGE; + voltage = 0; } - } else { - voltage = 0; } // TODO: Figure out some way to reset the encoder reading when stall diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java index e6d6bbb6..f13ff12d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java @@ -6,6 +6,8 @@ package com.stuypulse.robot.subsystems.climberhopper; +import java.util.Optional; + import com.stuypulse.robot.constants.Settings; import edu.wpi.first.math.system.plant.DCMotor; @@ -19,6 +21,8 @@ public class ClimberHopperSim extends ClimberHopper { private final ClimberHopperVisualizer visualizer; private double voltage; + private Optional voltageOverride; + public ClimberHopperSim() { visualizer = ClimberHopperVisualizer.getInstance(); @@ -52,23 +56,27 @@ public boolean atTargetHeight() { return isWithinTolerance(Settings.ClimberHopper.HEIGHT_TOLERANCE_METERS); } - // @Override - // public void setVoltageOverride(Optional voltage) { - // this.voltageOverride = voltage; - // } + @Override + public void setVoltageOverride(Optional voltage) { + this.voltageOverride = voltage; + } @Override public void periodic() { super.periodic(); - if (!atTargetHeight()) { - if (getCurrentHeight() < getState().getTargetHeight()) { - voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; + if (voltageOverride.isPresent()) { + voltage = voltageOverride.get(); + } else { + if (!atTargetHeight()) { + if (getCurrentHeight() < getState().getTargetHeight()) { + voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; + } else { + voltage = - Settings.ClimberHopper.MOTOR_VOLTAGE; + } } else { - voltage = - Settings.ClimberHopper.MOTOR_VOLTAGE; + voltage = 0; } - } else { - voltage = 0; } // TODO: Figure out some way to reset the encoder reading when stall From f42916009786cd756e5d8d39a63911017610cbbc Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Sat, 21 Feb 2026 00:12:41 -0500 Subject: [PATCH 02/18] FEAT: add signed kS and rezeroing in periodic for hood --- .../com/stuypulse/robot/constants/Motors.java | 12 ++++++++- .../hoodedshooter/hood/HoodImpl.java | 27 ++++++++++++++----- 2 files changed, 31 insertions(+), 8 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index bcc80e26..6f8ebb35 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -24,6 +24,7 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; /*- * File containing all of the configurations that different motors require. @@ -70,8 +71,17 @@ public interface Hood { .withPIDConstants(Gains.HoodedShooter.Hood.kP, Gains.HoodedShooter.Hood.kI, Gains.HoodedShooter.Hood.kD, 0) .withFFConstants(Gains.HoodedShooter.Hood.kS, Gains.HoodedShooter.Hood.kV, Gains.HoodedShooter.Hood.kA, 0) .withSensorToMechanismRatio(Settings.HoodedShooter.Hood.GEAR_RATIO); + + Slot0Configs SLOT_0 = new Slot0Configs() + .withKP(Gains.HoodedShooter.Hood.kP) + .withKI(Gains.HoodedShooter.Hood.kI) + .withKD(Gains.HoodedShooter.Hood.kD) + .withKS(Gains.HoodedShooter.Hood.kS) + .withKV(Gains.HoodedShooter.Hood.kV) + .withKA(Gains.HoodedShooter.Hood.kA) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - SoftwareLimitSwitchConfigs hoodSoftwareLimitSwitchConfigs = new SoftwareLimitSwitchConfigs() + SoftwareLimitSwitchConfigs SOFT_LIMITS = new SoftwareLimitSwitchConfigs() .withForwardSoftLimitEnable(true) .withReverseSoftLimitEnable(true) .withForwardSoftLimitThreshold(Settings.HoodedShooter.Angles.MAX_ANGLE.getRotations()) diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java index 4cab66ce..fd86f36c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java @@ -1,4 +1,4 @@ -/************************ PROJECT TRIBECBOT *************************/ +/************************ PROJECT ALPHA *************************/ /* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ @@ -19,6 +19,7 @@ import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import java.util.Optional; + public class HoodImpl extends Hood { private final TalonFX hoodMotor; private final CANcoder hoodEncoder; @@ -26,7 +27,7 @@ public class HoodImpl extends Hood { private final PositionVoltage controller; private Optional voltageOverride; - + private boolean hasUsedAbsoluteEncoder; public HoodImpl() { @@ -35,10 +36,13 @@ public HoodImpl() { Motors.HoodedShooter.Hood.HOOD.configure(hoodMotor); - hoodMotor.getConfigurator().apply(Motors.HoodedShooter.Hood.hoodSoftwareLimitSwitchConfigs); + hoodMotor.getConfigurator().apply(Motors.HoodedShooter.Hood.SLOT_0); + + hoodMotor.getConfigurator().apply(Motors.HoodedShooter.Hood.SOFT_LIMITS); hoodEncoder.getConfigurator().apply(Motors.HoodedShooter.Hood.HOOD_ENCODER); - controller = new PositionVoltage(getTargetAngle().getRotations()); + controller = new PositionVoltage(getTargetAngle().getRotations()) + .withEnableFOC(true); voltageOverride = Optional.empty(); } @@ -54,21 +58,30 @@ public void periodic() { if (!hasUsedAbsoluteEncoder) { hoodMotor.setPosition(hoodEncoder.getAbsolutePosition().getValueAsDouble() / Settings.HoodedShooter.Hood.SENSOR_TO_HOOD_RATIO); + hasUsedAbsoluteEncoder = true; } if (EnabledSubsystems.HOOD.get()) { if (voltageOverride.isPresent()) { hoodMotor.setVoltage(voltageOverride.get()); } else { - hoodMotor.setControl(controller.withPosition(getTargetAngle().getRotations()).withEnableFOC(true)); + hoodMotor.setControl(controller.withPosition(getTargetAngle().getRotations())); } } else { hoodMotor.stopMotor(); } if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("HoodedShooter/Hood/Hood Absolute Angle (deg)", hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.HoodedShooter.Hood.SENSOR_TO_HOOD_RATIO); //* 360.0 / (360.0/35.0) / .97); - SmartDashboard.putNumber("HoodedShooter/Hood/Input Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("HoodedShooter/Hood/Hood Absolute Angle (deg)", hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.HoodedShooter.Hood.SENSOR_TO_HOOD_RATIO); + + SmartDashboard.putNumber("HoodedShooter/Hood/Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("HoodedShooter/Hood/Supply Current", hoodMotor.getSupplyCurrent().getValueAsDouble()); + + SmartDashboard.putNumber("HoodedShooter/Hood/Closed Loop Error (deg)", hoodMotor.getClosedLoopError().getValueAsDouble() * 360.0); + SmartDashboard.putBoolean("HoodedShooter/Hood/Has Used Absolute Encoder", hasUsedAbsoluteEncoder); + + SmartDashboard.putNumber("InterpolationTesting/Hood Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("InterpolationTesting/Hood Supply Current", hoodMotor.getSupplyCurrent().getValueAsDouble()); } } From 27a0bf7b080416c279e9d1993d20fbb8a103fefd Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Sat, 21 Feb 2026 00:25:00 -0500 Subject: [PATCH 03/18] FEAT: Interpolation state implementation --- .../com/stuypulse/robot/RobotContainer.java | 17 +++++---- .../com/stuypulse/robot/constants/Field.java | 2 +- .../stuypulse/robot/constants/Settings.java | 30 ++++++++++++--- .../hoodedshooter/HoodedShooter.java | 7 +++- .../subsystems/hoodedshooter/hood/Hood.java | 4 +- .../hoodedshooter/shooter/Shooter.java | 7 +++- .../hoodedshooter/shooter/ShooterImpl.java | 6 +++ .../swerve/CommandSwerveDrivetrain.java | 1 + .../hoodedshooter/HoodAngleCalculator.java | 38 +++++++++++++++---- 9 files changed, 84 insertions(+), 28 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 8411c15c..7d6ac1c7 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -116,17 +116,17 @@ private void configureDefaultCommands() { private void configureButtonBindings() { + driver.getDPadUp() + .onTrue(new SwerveResetHeading()); + driver.getDPadDown() .onTrue(new TurretIdle()) .onTrue(new TurretSeed()); - - driver.getDPadUp() - .onTrue(new SwerveResetHeading()); // SCORING ROUTINE driver.getTopButton() .whileTrue(new TurretShoot() - .alongWith(new HoodedShooterShoot()) + .alongWith(new HoodedShooterInterpolation()) .alongWith(new WaitUntilCommand(() -> hoodedShooter.bothAtTolerance())) .andThen(new HandoffRun().onlyIf(() -> hoodedShooter.bothAtTolerance()) .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) @@ -135,13 +135,14 @@ private void configureButtonBindings() { .alongWith(new HoodedShooterStow()) .alongWith(new HandoffStop())); - driver.getTopButton() - .whileTrue(new TurretShoot() - .alongWith(new HoodedShooterInterpolation()) + driver.getBottomButton() + .onTrue(new HoodedShooterFerry() + .alongWith(new TurretFerry()) .alongWith(new WaitUntilCommand(() -> hoodedShooter.bothAtTolerance())) .andThen(new HandoffRun().onlyIf(() -> hoodedShooter.bothAtTolerance()) .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) - .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && hoodedShooter.bothAtTolerance())))) + .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && hoodedShooter.bothAtTolerance()))) + ) .onFalse(new SpindexerStop() .alongWith(new HoodedShooterStow()) .alongWith(new HandoffStop())); diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index f5b4f0eb..8d45aed9 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -52,7 +52,7 @@ public static boolean closerToTop(){ // 1.0 meters from driverstation wall and field wall public final Pose2d leftFerryZone = new Pose2d(1.0, WIDTH - 1.0, new Rotation2d()); - public final Pose2d rightFerryZone = new Pose2d(1.0, 1.0, new Rotation2d()); + public final Pose2d rightFerryZone = new Pose2d(1.0 + Units.feetToMeters(6), 1.0 + Units.feetToMeters(3), new Rotation2d()); //TODO: GET ACTUAL POS public static Pose2d getFerryZonePose(Translation2d robot) { if (robot.getDistance(leftFerryZone.getTranslation()) > robot.getDistance(rightFerryZone.getTranslation())) { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 7675dea4..08e31ba1 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -69,23 +69,43 @@ public interface Constants { double GEAR_RATIO = 8.0 / 1.0; } } + + public interface ShootOnTheFly { + int MAX_ITERATIONS = 5; + double TIME_TOLERANCE = 0.01; + SmartNumber UPDATE_DELAY = new SmartNumber("HoodedShooter/SOTM/update delay", 0.00); + } public interface HoodedShooter { double SHOOTER_TOLERANCE_RPM = 50.0; double HOOD_TOLERANCE_DEG = 0.5; + public interface FerryRPMInterpolation { + double[][] distanceRPMInterpolationValues = { + {3.79, 3450} // DONE ON 2/20 + }; + } + public interface AngleInterpolation { double[][] distanceAngleInterpolationValues = { + {1.30, Units.degreesToRadians(16.5)}, {1.43, Units.degreesToRadians(21.0)}, // meters, radians + {2.15, Units.degreesToRadians(23.23)}, + {2.864967, Units.degreesToRadians(25.460189)}, {3.65, Units.degreesToRadians(28.0)}, + {4.43, Units.degreesToRadians(30.65)}, {5.32, Units.degreesToRadians(33.5)} }; } public interface RPMInterpolation{ double[][] distanceRPMInterpolationValues = { + {1.30, 3000.0}, {1.43, 3000.0}, // meters, RPM + {2.15, 3050.0}, + {2.864967, 3215.271125}, {3.65, 3400.0}, - {5.32, 3850.0} + {4.43, 3650.0}, + {5.32, 3950.0} }; } @@ -118,7 +138,10 @@ public interface Hood { } public interface Shooter { public final double GEAR_RATIO = 1.0; + public final double FLYWHEEL_RADIUS = Units.inchesToMeters(3.965 / 2); } + + } public interface Turret { @@ -199,11 +222,6 @@ public interface Vision { Vector MT2_STDEVS = VecBuilder.fill(0.7, 0.7, 694694); } - public interface ShootOnTheFly { - int MAX_ITERATIONS = 5; - double TIME_TOLERANCE = 0.01; - } - public interface Swerve { double MODULE_VELOCITY_DEADBAND_M_PER_S = 0.1; double ROTATIONAL_DEADBAND_RAD_PER_S = 0.1; diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java index 286dad3e..6491e296 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java @@ -1,4 +1,4 @@ -/************************ PROJECT TRIBECBOT *************************/ +/************************ PROJECT ALPHA *************************/ /* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ @@ -32,7 +32,7 @@ public static HoodedShooter getInstance(){ private final Shooter shooter; public HoodedShooter() { - state = HoodedShooterState.STOW; + state = HoodedShooterState.INTERPOLATION; hood = Hood.getInstance(); shooter = Shooter.getInstance(); } @@ -117,5 +117,8 @@ public void periodic() { SmartDashboard.putBoolean("HoodedShooter/Shooter At Tolerance?", isShooterAtTolerance()); SmartDashboard.putBoolean("HoodedShooter/Hood At Tolerance?", isHoodAtTolerance()); + + SmartDashboard.putNumber("InterpolationTesting/Hood Angle", getHoodAngle().getDegrees()); + SmartDashboard.putNumber("InterpolationTesting/Shooter RPM", getShooterRPM()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/Hood.java index c2ce8e7e..9a637d5a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/Hood.java @@ -33,8 +33,8 @@ public enum HoodState { HUB, LEFT_CORNER, RIGHT_CORNER, - IDLE, - INTERPOLATION; + INTERPOLATION, + IDLE; } public Hood() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java index 8dae4c8c..e727406d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java @@ -1,4 +1,4 @@ -/************************ PROJECT TRIBECBOT *************************/ +/************************ PROJECT ALPHA *************************/ /* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ @@ -7,6 +7,7 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; import com.stuypulse.robot.util.hoodedshooter.HoodAngleCalculator; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -57,7 +58,7 @@ public double getTargetRPM() { return switch(state) { case STOP -> 0; case SHOOT -> getShootRPM(); - case FERRY -> getFerryRPM(); + case FERRY -> HoodAngleCalculator.interpolateFerryingRPM().get(); case REVERSE -> Settings.HoodedShooter.RPMs.REVERSE; case HUB -> Settings.HoodedShooter.RPMs.HUB_RPM; case LEFT_CORNER -> Settings.HoodedShooter.RPMs.LEFT_CORNER_RPM; @@ -90,5 +91,7 @@ public void periodic() { SmartDashboard.putNumber("HoodedShooter/Shooter/Current RPM", getShooterRPM()); SmartDashboard.putNumber("HoodedShooter/Shooter/Target RPM", getTargetRPM()); + + SmartDashboard.putNumber("InterpolationTesting/Shooter Interpolated Target RPM", HoodAngleCalculator.interpolateShooterRPM().get()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java index 688826a0..0eb103fa 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java @@ -83,6 +83,12 @@ public void periodic() { SmartDashboard.putNumber("HoodedShooter/Shooter/Leader Voltage", shooterLeader.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("HoodedShooter/Shooter/Follower Voltage", shooterFollower.getMotorVoltage().getValueAsDouble()); + + SmartDashboard.putNumber("HoodedShooter/Shooter/Follower RPM", getFollowerRPM()); + + SmartDashboard.putNumber("InterpolationTesting/Shooter Closed Loop Error", shooterLeader.getClosedLoopError().getValueAsDouble() * 60.0); + SmartDashboard.putNumber("InterpolationTesting/Shooter Applied Voltage", shooterLeader.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("InterpolationTesting/Shooter Supply Current", shooterLeader.getSupplyCurrent().getValueAsDouble()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 4d79f238..65944d27 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -440,6 +440,7 @@ public void periodic() { turret2d.setPose(Robot.isBlue() ? turretPose : Field.transformToOppositeAlliance(turretPose)); SmartDashboard.putNumber("Turret/Dist From Hub", turretPose.getTranslation().getDistance(Field.hubCenter.getTranslation())); + SmartDashboard.putNumber("InterpolationTesting/Turret Dist From Hub", turretPose.getTranslation().getDistance(Field.hubCenter.getTranslation())); SmartDashboard.putNumber("Swerve/Pose/X", pose.getX()); SmartDashboard.putNumber("Swerve/Pose/Y", pose.getY()); diff --git a/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java b/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java index df3faf2f..519b613e 100644 --- a/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java @@ -1,13 +1,13 @@ -/************************ PROJECT TRIBECBOT *************************/ +/************************ PROJECT ALPHA *************************/ /* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ /***************************************************************/ package com.stuypulse.robot.util.hoodedshooter; - import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.HoodedShooter.AngleInterpolation; +import com.stuypulse.robot.constants.Settings.HoodedShooter.FerryRPMInterpolation; import com.stuypulse.robot.constants.Settings.HoodedShooter.RPMInterpolation; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -28,7 +28,8 @@ public class HoodAngleCalculator { public static InterpolatingDoubleTreeMap distanceAngleInterpolator; public static InterpolatingDoubleTreeMap distanceRPMInterpolator; - + public static InterpolatingDoubleTreeMap ferryingDistanceRPMInterpolator; + static { distanceAngleInterpolator = new InterpolatingDoubleTreeMap(); for (double[] pair : AngleInterpolation.distanceAngleInterpolationValues) { @@ -43,12 +44,19 @@ public class HoodAngleCalculator { } } + static { + ferryingDistanceRPMInterpolator = new InterpolatingDoubleTreeMap(); + for(double[] pair: FerryRPMInterpolation.distanceRPMInterpolationValues) { + ferryingDistanceRPMInterpolator.put(pair[0], pair[1]); + } + } + public static Supplier interpolateHoodAngle() { return () -> { CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); Translation2d hubPose = Field.getHubPose().getTranslation(); - Translation2d currentPose = swerve.getPose().getTranslation(); + Translation2d currentPose = swerve.getTurretPose().getTranslation(); double distanceMeters = hubPose.getDistance(currentPose); @@ -65,14 +73,30 @@ public static Supplier interpolateShooterRPM() { CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); Translation2d hubPose = Field.getHubPose().getTranslation(); - Translation2d currentPose = swerve.getPose().getTranslation(); + Translation2d currentPose = swerve.getTurretPose().getTranslation(); double distanceMeters = hubPose.getDistance(currentPose); - double targetRPM = distanceRPMInterpolator.get(distanceMeters); - SmartDashboard.putNumber("HoodedShooter/Interpolated RPM ", targetRPM); + SmartDashboard.putNumber("HoodedShooter/Interpolated RPM", targetRPM); + + return targetRPM; + }; + } + + public static Supplier interpolateFerryingRPM() { + return () -> { + CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); + + Translation2d currentPose = swerve.getTurretPose().getTranslation(); + Translation2d cornerPose = Field.getFerryZonePose(currentPose).getTranslation(); + + double distanceMeters = cornerPose.getDistance(currentPose); + + double targetRPM = ferryingDistanceRPMInterpolator.get(distanceMeters); + + SmartDashboard.putNumber("HoodedShooter/Interpolated Ferrying RPM", targetRPM); return targetRPM; }; From 244c2c177109da9ea8bd6929286801e5036e0aeb Mon Sep 17 00:00:00 2001 From: Lucas Date: Fri, 20 Feb 2026 21:28:13 -0500 Subject: [PATCH 04/18] Feat: Add voltageOverride --- .../climberhopper/ClimberHopper.java | 4 ++- .../climberhopper/ClimberHopperImpl.java | 24 +++++++++------- .../climberhopper/ClimberHopperSim.java | 28 ++++++++++++------- 3 files changed, 35 insertions(+), 21 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java index b2d70988..57b5b350 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java @@ -5,6 +5,8 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.climberhopper; +import java.util.Optional; + import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; @@ -62,7 +64,7 @@ public void setState(ClimberHopperState state) { public abstract double getCurrentHeight(); public abstract boolean atTargetHeight(); - // public abstract void setVoltageOverride(Optional voltage); + public abstract void setVoltageOverride(Optional voltage); @Override public void periodic() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java index de373ca9..65e43712 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java @@ -54,23 +54,27 @@ public boolean atTargetHeight() { return isWithinTolerance(Settings.ClimberHopper.HEIGHT_TOLERANCE_METERS); } - // @Override - // public void setVoltageOverride(Optional voltage) { - // this.voltageOverride = voltage; - // } + @Override + public void setVoltageOverride(Optional voltage) { + this.voltageOverride = voltage; + } @Override public void periodic() { super.periodic(); - if (!atTargetHeight()) { - if (getCurrentHeight() < getState().getTargetHeight()) { - voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; + if (voltageOverride.isPresent()) { + voltage = voltageOverride.get(); + } else { + if (!atTargetHeight()) { + if (getCurrentHeight() < getState().getTargetHeight()) { + voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; + } else { + voltage = - Settings.ClimberHopper.MOTOR_VOLTAGE; + } } else { - voltage = - Settings.ClimberHopper.MOTOR_VOLTAGE; + voltage = 0; } - } else { - voltage = 0; } // TODO: Figure out some way to reset the encoder reading when stall diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java index e6d6bbb6..f13ff12d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java @@ -6,6 +6,8 @@ package com.stuypulse.robot.subsystems.climberhopper; +import java.util.Optional; + import com.stuypulse.robot.constants.Settings; import edu.wpi.first.math.system.plant.DCMotor; @@ -19,6 +21,8 @@ public class ClimberHopperSim extends ClimberHopper { private final ClimberHopperVisualizer visualizer; private double voltage; + private Optional voltageOverride; + public ClimberHopperSim() { visualizer = ClimberHopperVisualizer.getInstance(); @@ -52,23 +56,27 @@ public boolean atTargetHeight() { return isWithinTolerance(Settings.ClimberHopper.HEIGHT_TOLERANCE_METERS); } - // @Override - // public void setVoltageOverride(Optional voltage) { - // this.voltageOverride = voltage; - // } + @Override + public void setVoltageOverride(Optional voltage) { + this.voltageOverride = voltage; + } @Override public void periodic() { super.periodic(); - if (!atTargetHeight()) { - if (getCurrentHeight() < getState().getTargetHeight()) { - voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; + if (voltageOverride.isPresent()) { + voltage = voltageOverride.get(); + } else { + if (!atTargetHeight()) { + if (getCurrentHeight() < getState().getTargetHeight()) { + voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; + } else { + voltage = - Settings.ClimberHopper.MOTOR_VOLTAGE; + } } else { - voltage = - Settings.ClimberHopper.MOTOR_VOLTAGE; + voltage = 0; } - } else { - voltage = 0; } // TODO: Figure out some way to reset the encoder reading when stall From 79097bb83175313fb21f3ce7b308931d2bfc269e Mon Sep 17 00:00:00 2001 From: Lucas Date: Sat, 21 Feb 2026 14:50:01 -0500 Subject: [PATCH 05/18] Feat: ClimberHopperReset command --- .../climberhopper/ClimberHopperReset.java | 21 +++++++++++++++++++ .../climberhopper/ClimberHopperImpl.java | 9 +------- 2 files changed, 22 insertions(+), 8 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java index 18bfb424..8b3f0707 100644 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java +++ b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java @@ -5,6 +5,8 @@ /***************************************************************/ package com.stuypulse.robot.commands.climberhopper; +import java.util.Optional; + import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; import edu.wpi.first.wpilibj2.command.Command; @@ -16,6 +18,25 @@ public ClimberHopperReset() { climberHopper = ClimberHopper.getInstance(); } + @Override + public void initialize() { + climberHopper.setVoltageOverride(Optional.of(2.0)); + } + + @Override + public boolean isFinished() { + if (climberHopper.getStalling()) { + // TODO: Reset encoder here + return true; + } + return false; + } + + @Override + public void end(boolean interrupted) { + climberHopper.setVoltageOverride(Optional.empty()); + } + @Override public void execute() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java index 65e43712..de287875 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java @@ -30,7 +30,7 @@ public ClimberHopperImpl() { motor = new TalonFX(Ports.ClimberHopper.CLIMBER_HOPPER); Motors.ClimberHopper.MOTOR.configure(motor); - motor.setPosition(0); + motor.setPosition(Settings.ClimberHopper.Constants.MIN_HEIGHT_METERS); stalling = BStream.create(() -> motor.getStatorCurrent().getValueAsDouble() > Settings.ClimberHopper.STALL) .filtered(new BDebounce.Both(Settings.ClimberHopper.DEBOUNCE)); } @@ -77,13 +77,6 @@ public void periodic() { } } - // TODO: Figure out some way to reset the encoder reading when stall - // if (atTargetHeight() && getState() == ClimberHopperState.HOPPER_DOWN) { - // if (voltageOverride.isPresent()) { - - // } - // } - motor.setControl(new VoltageOut(voltage).withEnableFOC(true)); SmartDashboard.putNumber("ClimberHopper/Voltage", voltage); From 655d859da5a483df0702abb0a9a63089b8d4d0e4 Mon Sep 17 00:00:00 2001 From: Lucas Date: Sat, 21 Feb 2026 15:10:55 -0500 Subject: [PATCH 06/18] Feat: Finish ClimberHopper encoder reset logic --- .../robot/commands/climberhopper/ClimberHopperReset.java | 6 ++++-- .../robot/subsystems/climberhopper/ClimberHopper.java | 5 +++++ .../robot/subsystems/climberhopper/ClimberHopperImpl.java | 7 +++++++ .../robot/subsystems/climberhopper/ClimberHopperSim.java | 6 ++++++ 4 files changed, 22 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java index 8b3f0707..6eb9335b 100644 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java +++ b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java @@ -7,6 +7,8 @@ import java.util.Optional; +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; import edu.wpi.first.wpilibj2.command.Command; @@ -20,13 +22,13 @@ public ClimberHopperReset() { @Override public void initialize() { - climberHopper.setVoltageOverride(Optional.of(2.0)); + climberHopper.setVoltageOverride(Optional.of(Settings.ClimberHopper.MOTOR_VOLTAGE)); } @Override public boolean isFinished() { if (climberHopper.getStalling()) { - // TODO: Reset encoder here + climberHopper.resetPostionUpper(); return true; } return false; diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java index 57b5b350..8075ced9 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java @@ -63,6 +63,11 @@ public void setState(ClimberHopperState state) { public abstract boolean getStalling(); public abstract double getCurrentHeight(); public abstract boolean atTargetHeight(); + + /** + * Resets the encoder postition to the upper hardstop + */ + public abstract void resetPostionUpper(); public abstract void setVoltageOverride(Optional voltage); diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java index de287875..d6bac385 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java @@ -59,6 +59,13 @@ public void setVoltageOverride(Optional voltage) { this.voltageOverride = voltage; } + /** + * Resets the encoder postition to the upper hardstop + */ + public void resetPostionUpper() { + motor.setPosition(Settings.ClimberHopper.Constants.MAX_HEIGHT_METERS); + } + @Override public void periodic() { super.periodic(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java index f13ff12d..13ef44dc 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java @@ -61,6 +61,12 @@ public void setVoltageOverride(Optional voltage) { this.voltageOverride = voltage; } + @Override + public void resetPostionUpper() { + // No encoder reset for sim + } + + @Override public void periodic() { super.periodic(); From fe73f6292e6ae2c2cc2a5f1ca2c985ac36377dbc Mon Sep 17 00:00:00 2001 From: Lucas Date: Sat, 21 Feb 2026 15:43:41 -0500 Subject: [PATCH 07/18] Fix - Set VoltageOverride to empty in constructor - Make ClimberHopperSetState an InstantCommand - Add ClimberHopper commands to button bindings --- src/main/java/com/stuypulse/robot/RobotContainer.java | 6 ++++-- .../robot/commands/climberhopper/ClimberHopperSetState.java | 4 ++-- .../robot/subsystems/climberhopper/ClimberHopperImpl.java | 2 ++ 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 7d6ac1c7..0cd132e2 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -11,7 +11,9 @@ import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.auton.DoNothingAuton; +import com.stuypulse.robot.commands.climberhopper.ClimberDown; import com.stuypulse.robot.commands.climberhopper.ClimberHopperDefaultCommand; +import com.stuypulse.robot.commands.climberhopper.ClimberUp; import com.stuypulse.robot.commands.handoff.HandoffReverse; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; @@ -228,11 +230,11 @@ private void configureButtonBindings() { // Climb Down Placeholder driver.getLeftBumper() - .onTrue(new BuzzController(driver)); + .onTrue(new BuzzController(driver).alongWith(new ClimberDown())); // Climb Up Placeholder driver.getRightBumper() - .onTrue(new BuzzController(driver)); + .onTrue(new BuzzController(driver).alongWith(new ClimberUp())); // Reset Heading driver.getDPadUp() diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperSetState.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperSetState.java index 6849ef97..85d8d6cd 100644 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperSetState.java +++ b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperSetState.java @@ -8,9 +8,9 @@ import com.stuypulse.robot.subsystems.climberhopper.*; import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; -import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; -public class ClimberHopperSetState extends Command { +public class ClimberHopperSetState extends InstantCommand { private final ClimberHopper climberHopper = ClimberHopper.getInstance(); private final ClimberHopperState state; diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java index d6bac385..68517bee 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java @@ -33,6 +33,8 @@ public ClimberHopperImpl() { motor.setPosition(Settings.ClimberHopper.Constants.MIN_HEIGHT_METERS); stalling = BStream.create(() -> motor.getStatorCurrent().getValueAsDouble() > Settings.ClimberHopper.STALL) .filtered(new BDebounce.Both(Settings.ClimberHopper.DEBOUNCE)); + + voltageOverride = Optional.empty(); } @Override From baaaa24c8a1576a333e79f9f7f02134a9dbd7076 Mon Sep 17 00:00:00 2001 From: Lucas Date: Sat, 21 Feb 2026 15:44:13 -0500 Subject: [PATCH 08/18] Feat: Comment out ClimberHopper vertical expansion stuff --- .../ClimberHopperDefaultCommand.java | 78 ++++++++++--------- .../climberhopper/ClimberHopperReset.java | 62 +++++++-------- .../commands/climberhopper/HopperUp.java | 12 +-- .../climberhopper/ClimberHopper.java | 2 +- .../climberhopper/ClimberHopperSim.java | 2 + 5 files changed, 82 insertions(+), 74 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java index ccd03002..203630dc 100644 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java @@ -17,14 +17,14 @@ public class ClimberHopperDefaultCommand extends Command { private final ClimberHopper climberHopper; - private final CommandSwerveDrivetrain swerve; - private Pose2d pose; - private boolean flag = false; // To prevent repeated stalling under trench + // private final CommandSwerveDrivetrain swerve; + // private Pose2d pose; + // private boolean flag = false; // To prevent repeated stalling under trench public ClimberHopperDefaultCommand() { climberHopper = ClimberHopper.getInstance(); - swerve = CommandSwerveDrivetrain.getInstance(); - pose = swerve.getPose(); + // swerve = CommandSwerveDrivetrain.getInstance(); + // pose = swerve.getPose(); addRequirements(climberHopper); @@ -34,43 +34,49 @@ public ClimberHopperDefaultCommand() { public void execute() { // === Robot Position Logic === // Reminder from driver's perspective, positive X to the opposite alliance and positive Y points to the left. - pose = swerve.getPose(); + // pose = swerve.getPose(); - boolean isBetweenRightTrenchesY = Field.NearRightTrench.rightEdge.getY() < pose.getY() && Field.NearRightTrench.leftEdge.getY() > pose.getY(); + // boolean isBetweenRightTrenchesY = Field.NearRightTrench.rightEdge.getY() < pose.getY() && Field.NearRightTrench.leftEdge.getY() > pose.getY(); - boolean isBetweenLeftTrenchesY = Field.NearLeftTrench.rightEdge.getY() < pose.getY() && Field.NearLeftTrench.leftEdge.getY() > pose.getY(); + // boolean isBetweenLeftTrenchesY = Field.NearLeftTrench.rightEdge.getY() < pose.getY() && Field.NearLeftTrench.leftEdge.getY() > pose.getY(); - boolean isCloseToNearTrenchesX = Math.abs(pose.getX() - Field.NearRightTrench.rightEdge.getX()) < Field.trenchXTolerance; + // boolean isCloseToNearTrenchesX = Math.abs(pose.getX() - Field.NearRightTrench.rightEdge.getX()) < Field.trenchXTolerance; - boolean isCloseToFarTrenchesX = Math.abs(pose.getX() - Field.FarRightTrench.rightEdge.getX()) < Field.trenchXTolerance; + // boolean isCloseToFarTrenchesX = Math.abs(pose.getX() - Field.FarRightTrench.rightEdge.getX()) < Field.trenchXTolerance; - boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) && (isCloseToNearTrenchesX || isCloseToFarTrenchesX); // Far X tolerance + // boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) && (isCloseToNearTrenchesX || isCloseToFarTrenchesX); // Far X tolerance - // === Climber Position and State Logic === - boolean inDownState = climberHopper.getState() == ClimberHopperState.CLIMBER_DOWN || climberHopper.getState() == ClimberHopperState.HOPPER_DOWN; - - boolean stalledByBalls = climberHopper.getStalling() && inDownState; - // boolean stalledByBalls = true; - - if (isUnderTrench) { - if (!stalledByBalls && !flag) { - climberHopper.setState(ClimberHopperState.HOPPER_DOWN); - } else { - climberHopper.setState(ClimberHopperState.HOPPER_UP); - // TODO: Flash LEDs or have Controller buzz when this happens. Also we need to somehow log this state!!! - flag = true; // prevent hopper from going back down while still under trench with too many balls - } - } else { // If not under trench... - if (climberHopper.getState() != ClimberHopperState.CLIMBER_DOWN) { - // Set the hopper up - climberHopper.setState(ClimberHopperState.HOPPER_UP); - } + // // === Climber Position and State Logic === + // boolean inDownState = climberHopper.getState() == ClimberHopperState.CLIMBER_DOWN || climberHopper.getState() == ClimberHopperState.HOPPER_DOWN; + + // boolean stalledByBalls = climberHopper.getStalling() && inDownState; + // // boolean stalledByBalls = true; + + // if (isUnderTrench) { + // if (!stalledByBalls && !flag) { + // climberHopper.setState(ClimberHopperState.HOPPER_DOWN); + // } else { + // climberHopper.setState(ClimberHopperState.HOPPER_UP); + // // TODO: Flash LEDs or have Controller buzz when this happens. Also we need to somehow log this state!!! + // flag = true; // prevent hopper from going back down while still under trench with too many balls + // } + // } else { // If not under trench... + // if (climberHopper.getState() != ClimberHopperState.CLIMBER_DOWN) { + // // Set the hopper up + // climberHopper.setState(ClimberHopperState.HOPPER_UP); + // } + // } + + // if (!isUnderTrench) { + // flag = false; + // } + + // SmartDashboard.putBoolean("ClimberHopper/UnderTrench", isUnderTrench); + + // !!! AFTER ABANDONING VERTICAL EXPANSION: + if (climberHopper.getState() != ClimberHopperState.CLIMBER_DOWN) { + // Set the hopper down + climberHopper.setState(ClimberHopperState.HOPPER_DOWN); } - - if (!isUnderTrench) { - flag = false; - } - - SmartDashboard.putBoolean("ClimberHopper/UnderTrench", isUnderTrench); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java index 6eb9335b..fd669f38 100644 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java +++ b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java @@ -5,42 +5,42 @@ /***************************************************************/ package com.stuypulse.robot.commands.climberhopper; -import java.util.Optional; +// import java.util.Optional; -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; +// import com.stuypulse.robot.Robot; +// import com.stuypulse.robot.constants.Settings; +// import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; import edu.wpi.first.wpilibj2.command.Command; public class ClimberHopperReset extends Command { - private final ClimberHopper climberHopper; + // private final ClimberHopper climberHopper; - public ClimberHopperReset() { - climberHopper = ClimberHopper.getInstance(); - } - - @Override - public void initialize() { - climberHopper.setVoltageOverride(Optional.of(Settings.ClimberHopper.MOTOR_VOLTAGE)); - } - - @Override - public boolean isFinished() { - if (climberHopper.getStalling()) { - climberHopper.resetPostionUpper(); - return true; - } - return false; - } - - @Override - public void end(boolean interrupted) { - climberHopper.setVoltageOverride(Optional.empty()); - } - - @Override - public void execute() { + // public ClimberHopperReset() { + // climberHopper = ClimberHopper.getInstance(); + // } + + // @Override + // public void initialize() { + // climberHopper.setVoltageOverride(Optional.of(Settings.ClimberHopper.MOTOR_VOLTAGE)); + // } + + // @Override + // public boolean isFinished() { + // if (climberHopper.getStalling()) { + // climberHopper.resetPostionUpper(); + // return true; + // } + // return false; + // } + + // @Override + // public void end(boolean interrupted) { + // climberHopper.setVoltageOverride(Optional.empty()); + // } + + // @Override + // public void execute() { - } + // } } diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperUp.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperUp.java index d4044578..03fca2a9 100644 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperUp.java +++ b/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperUp.java @@ -5,10 +5,10 @@ /***************************************************************/ package com.stuypulse.robot.commands.climberhopper; -import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; +// import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; -public class HopperUp extends ClimberHopperSetState { - public HopperUp() { - super(ClimberHopperState.HOPPER_UP); - } -} \ No newline at end of file +// public class HopperUp extends ClimberHopperSetState { +// public HopperUp() { +// super(ClimberHopperState.HOPPER_UP); +// } +// } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java index 8075ced9..cea9fe8e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java @@ -31,7 +31,7 @@ public static ClimberHopper getInstance() { public enum ClimberHopperState { CLIMBER_UP(Settings.ClimberHopper.CLIMBER_UP_HEIGHT_METERS), CLIMBER_DOWN(Settings.ClimberHopper.CLIMBER_DOWN_HEIGHT_METERS), - HOPPER_UP(Settings.ClimberHopper.HOPPER_UP_HEIGHT_METERS), + // HOPPER_UP(Settings.ClimberHopper.HOPPER_UP_HEIGHT_METERS), HOPPER_DOWN(Settings.ClimberHopper.HOPPER_DOWN_HEIGHT_METERS); private double targetHeight; diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java index 13ef44dc..401edfde 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java @@ -36,6 +36,8 @@ public ClimberHopperSim() { false, Settings.ClimberHopper.Constants.MIN_HEIGHT_METERS ); + + voltageOverride = Optional.empty(); } public boolean getStalling() { From 3e73f4593d5196074c75298edb6a9a6c28259319 Mon Sep 17 00:00:00 2001 From: Lucas Date: Sat, 21 Feb 2026 22:20:44 -0500 Subject: [PATCH 09/18] Fix: Convert between meters and rotations with handling ClimberHopper encoder Also: - Add softlimits - Update README --- README.md | 4 +-- .../com/stuypulse/robot/RobotContainer.java | 5 +-- .../com/stuypulse/robot/constants/Motors.java | 7 ++++ .../climberhopper/ClimberHopper.java | 2 +- .../climberhopper/ClimberHopperImpl.java | 32 ++++++++++++------- .../climberhopper/ClimberHopperSim.java | 2 -- 6 files changed, 33 insertions(+), 19 deletions(-) diff --git a/README.md b/README.md index 86f52fce..9ae71f9c 100644 --- a/README.md +++ b/README.md @@ -3,8 +3,8 @@ ### Mechanisms - Swerve -- L1 Climb + Vertically Expanding Hopper +- L1 Climb + ~~Vertically Expanding Hopper~~ - Handoff -> Turret + Hooded Shooter - Spindexer -- Intake + Horizontally Expanding Hopper +- Intake + Passive Horizontally Expanding Hopper - Vision with 3 LimeLight 4s diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 0cd132e2..7ad5aef9 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -170,7 +170,7 @@ private void configureButtonBindings() { //-------------------------------------------------------------------------------------------------------------------------\\ // Climb Align driver.getTopButton() - .whileTrue(new SwerveClimbAlign()); + .whileTrue(new SwerveClimbAlign().alongWith(new ClimberUp())); // Left Corner Shoot driver.getLeftButton() @@ -234,7 +234,8 @@ private void configureButtonBindings() { // Climb Up Placeholder driver.getRightBumper() - .onTrue(new BuzzController(driver).alongWith(new ClimberUp())); + .onTrue(new BuzzController(driver)) + .whileTrue(new ClimberUp()); // Reset Heading driver.getDPadUp() diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index 6f8ebb35..6c7fd5c2 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -44,6 +44,13 @@ public interface ClimberHopper { .withCurrentLimitAmps(50) .withSupplyCurrentLimitAmps(50) .withRampRate(Settings.ClimberHopper.RAMP_RATE); + + // TODO: This is currently assuming reverse is retracting. Swap when necessary!! + SoftwareLimitSwitchConfigs SOFT_LIMITS = new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true) + .withReverseSoftLimitEnable(true) + .withForwardSoftLimitThreshold(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM + Settings.ClimberHopper.Constants.NUM_ROTATIONS_TO_REACH_TOP) + .withReverseSoftLimitThreshold(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM); } public interface HoodedShooter { diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java index cea9fe8e..6b2e96cf 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java @@ -46,7 +46,7 @@ public double getTargetHeight() { } - protected ClimberHopperState state; + private ClimberHopperState state; protected ClimberHopper() { this.state = ClimberHopperState.CLIMBER_UP; diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java index 68517bee..8e7f41e1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java @@ -6,7 +6,7 @@ package com.stuypulse.robot.subsystems.climberhopper; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; - +import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; @@ -27,10 +27,12 @@ public class ClimberHopperImpl extends ClimberHopper { public ClimberHopperImpl() { super(); + motor = new TalonFX(Ports.ClimberHopper.CLIMBER_HOPPER); Motors.ClimberHopper.MOTOR.configure(motor); - - motor.setPosition(Settings.ClimberHopper.Constants.MIN_HEIGHT_METERS); + motor.getConfigurator().apply(Motors.ClimberHopper.SOFT_LIMITS); + + motor.setPosition(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM); stalling = BStream.create(() -> motor.getStatorCurrent().getValueAsDouble() > Settings.ClimberHopper.STALL) .filtered(new BDebounce.Both(Settings.ClimberHopper.DEBOUNCE)); @@ -43,7 +45,7 @@ public boolean getStalling() { } @Override - public double getCurrentHeight() { // TODO: convert motor encoder position to meters somehow + public double getCurrentHeight() { return this.motor.getPosition().getValueAsDouble() * Settings.ClimberHopper.Constants.POSITION_CONVERSION_FACTOR; } @@ -61,11 +63,8 @@ public void setVoltageOverride(Optional voltage) { this.voltageOverride = voltage; } - /** - * Resets the encoder postition to the upper hardstop - */ public void resetPostionUpper() { - motor.setPosition(Settings.ClimberHopper.Constants.MAX_HEIGHT_METERS); + motor.setPosition(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM + Settings.ClimberHopper.Constants.NUM_ROTATIONS_TO_REACH_TOP); } @Override @@ -85,11 +84,20 @@ public void periodic() { voltage = 0; } } + + if (EnabledSubsystems.CLIMBER_HOPPER.get()) { + motor.setControl(new VoltageOut(voltage).withEnableFOC(true)); + } else { + motor.stopMotor(); + } - motor.setControl(new VoltageOut(voltage).withEnableFOC(true)); - - SmartDashboard.putNumber("ClimberHopper/Voltage", voltage); - SmartDashboard.putNumber("ClimberHopper/Current", motor.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putBoolean("ClimberHopper/Stalling", getStalling()); + + if (Settings.DEBUG_MODE) { + SmartDashboard.putNumber("ClimberHopper/Current Height", getCurrentHeight()); + SmartDashboard.putNumber("ClimberHopper/Voltage", voltage); + SmartDashboard.putNumber("ClimberHopper/Applied Voltage", motor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("ClimberHopper/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); + } } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java index 401edfde..527567e4 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java @@ -87,8 +87,6 @@ public void periodic() { } } - // TODO: Figure out some way to reset the encoder reading when stall - sim.setInputVoltage(voltage); SmartDashboard.putNumber("ClimberHopper/Voltage", voltage); From 717ac87b8cf607abc7510261e6001267d28be30d Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Sat, 21 Feb 2026 23:03:54 -0500 Subject: [PATCH 10/18] CLEAN + FEAT: reorganize Settings and add CANCoderConfig wrapper --- .../com/stuypulse/robot/constants/Motors.java | 221 ++++++------ .../stuypulse/robot/constants/Settings.java | 335 +++++++++--------- 2 files changed, 282 insertions(+), 274 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index 6c7fd5c2..ba4db748 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -18,6 +18,7 @@ import com.ctre.phoenix6.configs.Slot2Configs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.GravityTypeValue; @@ -26,52 +27,38 @@ import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; -/*- - * File containing all of the configurations that different motors require. - * - * Such configurations include: - * - If it is Inverted - * - The Idle Mode of the Motor - * - The Current Limit - * - The Open Loop Ramp Rate - */ public interface Motors { public interface ClimberHopper { - TalonFXConfig MOTOR = new TalonFXConfig() + public final TalonFXConfig MOTOR = new TalonFXConfig() .withInvertedValue(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Brake) - .withCurrentLimitAmps(50) - .withSupplyCurrentLimitAmps(50) + .withCurrentLimitAmps(50.0) + .withSupplyCurrentLimitAmps(50.0) .withRampRate(Settings.ClimberHopper.RAMP_RATE); - - // TODO: This is currently assuming reverse is retracting. Swap when necessary!! - SoftwareLimitSwitchConfigs SOFT_LIMITS = new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitEnable(true) - .withReverseSoftLimitEnable(true) - .withForwardSoftLimitThreshold(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM + Settings.ClimberHopper.Constants.NUM_ROTATIONS_TO_REACH_TOP) - .withReverseSoftLimitThreshold(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM); + + public final SoftwareLimitSwitchConfigs SOFT_LIMITS = new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true) + .withReverseSoftLimitEnable(true) + .withForwardSoftLimitThreshold(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM + Settings.ClimberHopper.Constants.NUM_ROTATIONS_TO_REACH_TOP) + .withReverseSoftLimitThreshold(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM); } - public interface HoodedShooter { - public interface Shooter { - TalonFXConfig SHOOTER = new TalonFXConfig() - // .withSupplyCurrentLimitAmps(100.0) - // .withCurrentLimitAmps(100.0) - // .withRampRate(0.25) - .withCurrentLimitEnable(false) - .withNeutralMode(NeutralModeValue.Coast) - .withInvertedValue(InvertedValue.CounterClockwise_Positive) - .withPIDConstants(Gains.HoodedShooter.Shooter.kP, Gains.HoodedShooter.Shooter.kI, - Gains.HoodedShooter.Shooter.kD, 0) - .withFFConstants(Gains.HoodedShooter.Shooter.kS, Gains.HoodedShooter.Shooter.kV, - Gains.HoodedShooter.Shooter.kA, 0) - .withSensorToMechanismRatio(Settings.HoodedShooter.Shooter.GEAR_RATIO); - } + public interface Handoff { + public final TalonFXConfig HANDOFF = new TalonFXConfig() + .withCurrentLimitAmps(80.0) + .withRampRate(0.25) + .withNeutralMode(NeutralModeValue.Brake) + .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withFFConstants(Gains.Handoff.kS, Gains.Handoff.kV, Gains.Handoff.kA, 0) + .withPIDConstants(Gains.Handoff.kP, Gains.Handoff.kI, Gains.Handoff.kD, 0) + .withSensorToMechanismRatio(Settings.Handoff.GEAR_RATIO); + } + public interface HoodedShooter { public interface Hood { - TalonFXConfig HOOD = new TalonFXConfig() - .withCurrentLimitAmps(80) + public final TalonFXConfig HOOD = new TalonFXConfig() + .withCurrentLimitAmps(80.0) .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) .withInvertedValue(InvertedValue.Clockwise_Positive) @@ -79,7 +66,7 @@ public interface Hood { .withFFConstants(Gains.HoodedShooter.Hood.kS, Gains.HoodedShooter.Hood.kV, Gains.HoodedShooter.Hood.kA, 0) .withSensorToMechanismRatio(Settings.HoodedShooter.Hood.GEAR_RATIO); - Slot0Configs SLOT_0 = new Slot0Configs() + public final Slot0Configs SLOT_0 = new Slot0Configs() .withKP(Gains.HoodedShooter.Hood.kP) .withKI(Gains.HoodedShooter.Hood.kI) .withKD(Gains.HoodedShooter.Hood.kD) @@ -87,98 +74,125 @@ public interface Hood { .withKV(Gains.HoodedShooter.Hood.kV) .withKA(Gains.HoodedShooter.Hood.kA) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - - SoftwareLimitSwitchConfigs SOFT_LIMITS = new SoftwareLimitSwitchConfigs() + + public final SoftwareLimitSwitchConfigs SOFT_LIMITS = new SoftwareLimitSwitchConfigs() .withForwardSoftLimitEnable(true) .withReverseSoftLimitEnable(true) .withForwardSoftLimitThreshold(Settings.HoodedShooter.Angles.MAX_ANGLE.getRotations()) .withReverseSoftLimitThreshold(Settings.HoodedShooter.Angles.MIN_ANGLE.getRotations()); - CANcoderConfiguration HOOD_ENCODER = new CANcoderConfiguration() + public final CANcoderConfiguration HOOD_ENCODER = new CANcoderConfiguration() .withMagnetSensor(new MagnetSensorConfigs() - .withSensorDirection(SensorDirectionValue.Clockwise_Positive) - .withAbsoluteSensorDiscontinuityPoint(1) - .withMagnetOffset(Settings.HoodedShooter.Hood.ENCODER_OFFSET.getRotations())); + .withSensorDirection(SensorDirectionValue.Clockwise_Positive) + .withAbsoluteSensorDiscontinuityPoint(1.0) + .withMagnetOffset(Settings.HoodedShooter.Hood.ENCODER_OFFSET.getRotations())); + } + + public interface Shooter { + public final TalonFXConfig SHOOTER = new TalonFXConfig() + .withCurrentLimitEnable(false) + .withNeutralMode(NeutralModeValue.Coast) + .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withPIDConstants(Gains.HoodedShooter.Shooter.kP, Gains.HoodedShooter.Shooter.kI, + Gains.HoodedShooter.Shooter.kD, 0) + .withFFConstants(Gains.HoodedShooter.Shooter.kS, Gains.HoodedShooter.Shooter.kV, + Gains.HoodedShooter.Shooter.kA, 0) + .withSensorToMechanismRatio(Settings.HoodedShooter.Shooter.GEAR_RATIO); } } public interface Intake { - TalonFXConfig ROLLER = new TalonFXConfig() - .withCurrentLimitAmps(40) + public final TalonFXConfig ROLLER = new TalonFXConfig() + .withCurrentLimitAmps(40.0) .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Coast) - .withInvertedValue(InvertedValue.Clockwise_Positive); // TODO: add gear ratio, find inversions + .withInvertedValue(InvertedValue.Clockwise_Positive); - TalonFXConfig PIVOT = new TalonFXConfig() - .withCurrentLimitAmps(40) + public final TalonFXConfig PIVOT = new TalonFXConfig() + .withCurrentLimitAmps(40.0) .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.Clockwise_Positive) // TODO: find inversions + .withInvertedValue(InvertedValue.Clockwise_Positive) .withPIDConstants(Gains.Intake.Pivot.kP, Gains.Intake.Pivot.kI, Gains.Intake.Pivot.kD, 0) .withFFConstants(Gains.Intake.Pivot.kS, Gains.Intake.Pivot.kV, Gains.Intake.Pivot.kA, Gains.Intake.Pivot.kG, 0) .withMotionProfile(Settings.Intake.PIVOT_MAX_VEL.getRotations(), Settings.Intake.PIVOT_MAX_ACCEL.getRotations()); - } + } - public interface Spindexeer { - TalonFXConfig PIVOT = new TalonFXConfig() - .withCurrentLimitAmps(40) + public interface Spindexer { + public final TalonFXConfig SPINDEXER = new TalonFXConfig() + .withCurrentLimitEnable(false) .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.Clockwise_Positive) // TODO: find inversions - .withPIDConstants(Gains.Intake.Pivot.kP, Gains.Intake.Pivot.kI, Gains.Intake.Pivot.kD, 0) - .withFFConstants(Gains.Intake.Pivot.kS, Gains.Intake.Pivot.kV, Gains.Intake.Pivot.kA, Gains.Intake.Pivot.kG, 0) - .withMotionProfile(Settings.Intake.PIVOT_MAX_VEL.getRotations(), Settings.Intake.PIVOT_MAX_ACCEL.getRotations()); + .withInvertedValue(InvertedValue.Clockwise_Positive) + .withFFConstants(Gains.Spindexer.kS, Gains.Spindexer.kV, Gains.Spindexer.kA, 0) + .withPIDConstants(Gains.Spindexer.kP, Gains.Spindexer.kI, Gains.Spindexer.kD, 0) + .withSensorToMechanismRatio(Settings.Spindexer.Constants.GEAR_RATIO); } - public interface Spindexer { - TalonFXConfig SPINDEXER = new TalonFXConfig() - .withCurrentLimitEnable(false) - .withRampRate(0.25) - .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.Clockwise_Positive) - .withFFConstants(Gains.Spindexer.kS, Gains.Spindexer.kV, Gains.Spindexer.kA, 0) - .withPIDConstants(Gains.Spindexer.kP, Gains.Spindexer.kI, Gains.Spindexer.kD, 0) - .withSensorToMechanismRatio(Settings.Spindexer.Constants.GEAR_RATIO); + public interface Turret { + public final TalonFXConfig TURRET = new TalonFXConfig() + .withRampRate(0.25) + .withNeutralMode(NeutralModeValue.Brake) + .withInvertedValue(InvertedValue.Clockwise_Positive) + .withPIDConstants(Gains.Turret.kP, 0.0, Gains.Turret.kD, 0) + .withFFConstants(Gains.Turret.kS, 0.0, 0.0, 0) + .withSensorToMechanismRatio(Settings.Turret.Constants.GEAR_RATIO_MOTOR_TO_MECH); + + public final CANCoderConfig ENCODER_17T = new CANCoderConfig() + .withSensorDirection(SensorDirectionValue.Clockwise_Positive) + .withAbsoluteSensorDiscontinuityPoint(1.0); + + public final CANCoderConfig ENCODER_18T = new CANCoderConfig() + .withSensorDirection(SensorDirectionValue.Clockwise_Positive) + .withAbsoluteSensorDiscontinuityPoint(1.0); + + public final SoftwareLimitSwitchConfigs SOFT_LIMIT = new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true) + .withReverseSoftLimitEnable(true) + .withForwardSoftLimitThreshold(Settings.Turret.Constants.SoftwareLimit.FORWARD_MAX_ROTATIONS) + .withReverseSoftLimitThreshold(Settings.Turret.Constants.SoftwareLimit.BACKWARDS_MAX_ROTATIONS); } - public interface Turret { - TalonFXConfig TURRET = new TalonFXConfig() - .withCurrentLimitEnable(false) - .withRampRate(.25) - .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.Clockwise_Positive) - .withPIDConstants(Gains.Turret.kP, Gains.Turret.kI, Gains.Turret.kD, 0) - .withFFConstants(Gains.Turret.kS, Gains.Turret.kV, Gains.Turret.kA, 0) - .withSensorToMechanismRatio(Settings.Turret.Constants.GEAR_RATIO_MOTOR_TO_MECH); + public static class CANCoderConfig { + private final CANcoderConfiguration configuration = new CANcoderConfiguration(); + private final MagnetSensorConfigs magnetSensorConfigs = new MagnetSensorConfigs(); - SoftwareLimitSwitchConfigs SOFT_LIMIT = new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitEnable(true) - .withReverseSoftLimitEnable(true) - .withForwardSoftLimitThreshold(0.75) // 0.75 - .withReverseSoftLimitThreshold(-2.0 / 3.0); // -0.66 + public void configure(CANcoder encoder) { + CANcoderConfiguration defaultConfig = new CANcoderConfiguration(); + encoder.getConfigurator().apply(defaultConfig); - CANcoderConfiguration ENCODER_17T = new CANcoderConfiguration() - .withMagnetSensor(new MagnetSensorConfigs() - .withSensorDirection(SensorDirectionValue.Clockwise_Positive) - .withAbsoluteSensorDiscontinuityPoint(1) - .withMagnetOffset(Settings.Turret.Constants.Encoder17t.OFFSET.getRotations())); + encoder.getConfigurator().apply(configuration); + } - CANcoderConfiguration ENCODER_18T = new CANcoderConfiguration() - .withMagnetSensor(new MagnetSensorConfigs() - .withSensorDirection(SensorDirectionValue.Clockwise_Positive) - .withAbsoluteSensorDiscontinuityPoint(1) - .withMagnetOffset(Settings.Turret.Constants.Encoder18t.OFFSET.getRotations())); - } + public CANcoderConfiguration getConfiguration() { + return this.configuration; + } - public interface Handoff { - TalonFXConfig HANDOFF = new TalonFXConfig() - .withCurrentLimitAmps(80) - .withRampRate(0.25) - .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.CounterClockwise_Positive) - .withFFConstants(Gains.Handoff.kS, Gains.Handoff.kV, Gains.Handoff.kA, 0) - .withPIDConstants(Gains.Handoff.kP, Gains.Handoff.kI, Gains.Handoff.kD, 0) - .withSensorToMechanismRatio(Settings.Handoff.GEAR_RATIO); + // MAGNET SENSOR CONFIGS + + public CANCoderConfig withSensorDirection(SensorDirectionValue sensorDirection) { + magnetSensorConfigs.SensorDirection = sensorDirection; + + configuration.withMagnetSensor(magnetSensorConfigs); + + return this; + } + + public CANCoderConfig withAbsoluteSensorDiscontinuityPoint(double discontinuityPoint) { + magnetSensorConfigs.AbsoluteSensorDiscontinuityPoint = discontinuityPoint; + + configuration.withMagnetSensor(magnetSensorConfigs); + + return this; + } + + public CANCoderConfig withMagnetOffset(double magnetOffset) { + magnetSensorConfigs.MagnetOffset = magnetOffset; + + configuration.withMagnetSensor(magnetSensorConfigs); + + return this; + } } public static class TalonFXConfig { @@ -194,14 +208,13 @@ public static class TalonFXConfig { private final MotionMagicConfigs motionMagicConfigs = new MotionMagicConfigs(); public void configure(TalonFX motor) { - // We want to reset configs here before applying configs; prevents unwanted configs from persisting TalonFXConfiguration defaultConfig = new TalonFXConfiguration(); motor.getConfigurator().apply(defaultConfig); motor.getConfigurator().apply(configuration); } - // SLOT 0 CONFIGS + // SLOT CONFIGS public TalonFXConfig withPIDConstants(double kP, double kI, double kD, int slot) { switch (slot) { @@ -228,7 +241,7 @@ public TalonFXConfig withPIDConstants(double kP, double kI, double kD, int slot) } public TalonFXConfig withFFConstants(double kS, double kV, double kA, int slot) { - return withFFConstants(kS, kV, kA, 0, slot); + return withFFConstants(kS, kV, kA, 0.0, slot); } public TalonFXConfig withFFConstants(double kS, double kV, double kA, double kG, int slot) { @@ -317,7 +330,7 @@ public TalonFXConfig withCurrentLimitAmps(double currentLimitAmps) { } public TalonFXConfig withLowerLimitSupplyCurrent(double currentLowerLimitAmps) { - currentLimitsConfigs.SupplyCurrentLowerLimit= currentLowerLimitAmps; + currentLimitsConfigs.SupplyCurrentLowerLimit = currentLowerLimitAmps; currentLimitsConfigs.StatorCurrentLimitEnable = true; configuration.withCurrentLimits(currentLimitsConfigs); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 08e31ba1..4f775143 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -18,78 +18,77 @@ import com.ctre.phoenix6.CANBus; import com.pathplanner.lib.path.PathConstraints; -/*- - * File containing constants and tunable settings for every subsystem on the robot. - * - * We use StuyLib's SmartNumber / SmartBoolean in order to have tunable - * values that we can edit on Shuffleboard. - */ public interface Settings { - double DT = 0.020; - double SECONDS_IN_A_MINUTE = 60.0; - boolean DEBUG_MODE = true; - CANBus CANIVORE = new CANBus("canivore", "./logs/example.hoot"); + public final double DT = 0.020; + public final double SECONDS_IN_A_MINUTE = 60.0; + public final boolean DEBUG_MODE = true; + public final CANBus CANIVORE = new CANBus("canivore", "./logs/example.hoot"); - public interface Handoff { - double GEAR_RATIO = 1.0; + public interface ClimberHopper { + public interface Constants { + public final double GEAR_RATIO = 45.0; - double HANDOFF_STOP = 0.0; - double HANDOFF_MAX = 4800.0; - double HANDOFF_REVERSE = -500.0; - double RPM_TOLERANCE = 200.0; - public final SmartNumber HANDOFF_RPM = new SmartNumber("Handoff/Target RPM", HANDOFF_MAX); - } + public final double MIN_HEIGHT_METERS = 0.0; + public final double MAX_HEIGHT_METERS = 1.0; - public interface Intake { - Rotation2d PIVOT_STOW_ANGLE = Rotation2d.fromDegrees(90.0); - Rotation2d PIVOT_INTAKE_OUTAKE_ANGLE = Rotation2d.fromDegrees(0.0); + public final double MASS_KG = 1.0; - public final Rotation2d PIVOT_ANGLE_TOLERANCE = Rotation2d.fromDegrees(3.0); - public final double FORWARD_MAX_ROTATIONS = -30.0 / 360.0; - public final double BACKWARDS_MAX_ROTATIONS = 90.0 / 360.0; + public final double NUM_ROTATIONS_TO_REACH_TOP = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (0.480 / 13.0); + public final double POSITION_CONVERSION_FACTOR = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / NUM_ROTATIONS_TO_REACH_TOP; + public final double VELOCITY_CONVERSION_FACTOR = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / NUM_ROTATIONS_TO_REACH_TOP / 60.0; - Rotation2d PIVOT_ANGLE_OFFSET = new Rotation2d(); - Rotation2d PIVOT_MAX_ANGLE = Rotation2d.fromDegrees(190); - Rotation2d PIVOT_MIN_ANGLE = Rotation2d.fromDegrees(80); + public final double DRUM_RADIUS_METERS = ((MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (NUM_ROTATIONS_TO_REACH_TOP / GEAR_RATIO)) / 2.0 / Math.PI; + } - Rotation2d PIVOT_MAX_VEL = Rotation2d.fromDegrees(300.0); - Rotation2d PIVOT_MAX_ACCEL = Rotation2d.fromDegrees(300.0); + public final double CLIMBER_UP_HEIGHT_METERS = Constants.MAX_HEIGHT_METERS; + public final double CLIMBER_DOWN_HEIGHT_METERS = 0.2; + public final double HOPPER_DOWN_HEIGHT_METERS = Constants.MIN_HEIGHT_METERS; + public final double HOPPER_UP_HEIGHT_METERS = 0.5; - double GEAR_RATIO = 48.0; - } + public final double STALL = 10.0; - public interface Spindexer { - double FORWARD_SPEED = 6000.0; - double REVERSE_SPEED = -6000.0; - double STOP_SPEED = 0.0; + public final double ROTATIONS_AT_BOTTOM = 0.0; - double RPM_TOLERANCE = 400.0; + public final double DEBOUNCE = 0.25; - public interface Constants { - double GEAR_RATIO = 8.0 / 1.0; - } + public final double GYRO_TOLERANCE = 0.0; + + public final double HEIGHT_TOLERANCE_METERS = 0.02; + + public final double RAMP_RATE = 50.0; + + public final double MOTOR_VOLTAGE = 3.5; } - public interface ShootOnTheFly { - int MAX_ITERATIONS = 5; - double TIME_TOLERANCE = 0.01; - SmartNumber UPDATE_DELAY = new SmartNumber("HoodedShooter/SOTM/update delay", 0.00); + public interface Handoff { + public final double GEAR_RATIO = 3.0 / 1.0; + + public final double HANDOFF_STOP = 0.0; + public final double HANDOFF_MAX = 4800.0; + public final double HANDOFF_REVERSE = -500.0; + public final double RPM_TOLERANCE = 200.0; + public final SmartNumber HANDOFF_RPM = new SmartNumber("Handoff/Target RPM", HANDOFF_MAX); } - + public interface HoodedShooter { - double SHOOTER_TOLERANCE_RPM = 50.0; - double HOOD_TOLERANCE_DEG = 0.5; + public final double SHOOTER_TOLERANCE_RPM = 50.0; + public final double HOOD_TOLERANCE_DEG = 0.5; - public interface FerryRPMInterpolation { - double[][] distanceRPMInterpolationValues = { - {3.79, 3450} // DONE ON 2/20 - }; + public interface Angles { + public final SmartNumber SHOOT_ANGLE = new SmartNumber("HoodedShooter/Shoot State Target Angle (deg)", 15.0); + public final SmartNumber FERRY_ANGLE = new SmartNumber("HoodedShooter/Ferry State Target Angle (deg)", 20.0); + + public final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(7.0); + public final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(36.80); + public final Rotation2d HUB_ANGLE = Rotation2d.fromDegrees(12.0); + public final Rotation2d LEFT_CORNER_ANGLE = Rotation2d.fromDegrees(10.0); + public final Rotation2d RIGHT_CORNER_ANGLE = Rotation2d.fromDegrees(10.0); } public interface AngleInterpolation { - double[][] distanceAngleInterpolationValues = { + public final double[][] distanceAngleInterpolationValues = { {1.30, Units.degreesToRadians(16.5)}, - {1.43, Units.degreesToRadians(21.0)}, // meters, radians + {1.43, Units.degreesToRadians(21.0)}, {2.15, Units.degreesToRadians(23.23)}, {2.864967, Units.degreesToRadians(25.460189)}, {3.65, Units.degreesToRadians(28.0)}, @@ -97,10 +96,24 @@ public interface AngleInterpolation { {5.32, Units.degreesToRadians(33.5)} }; } - public interface RPMInterpolation{ - double[][] distanceRPMInterpolationValues = { + + public interface FerryRPMInterpolation { + public final double[][] distanceRPMInterpolationValues = { + {3.79, 3450.0} + }; + } + + public interface Hood { + public final double GEAR_RATIO = 17024.0 / 135.0; + public final double ENCODER_TO_MECH = 32.0 / 3.0; + + public final Rotation2d ENCODER_OFFSET = Rotation2d.fromDegrees(0.0); + } + + public interface RPMInterpolation { + public final double[][] distanceRPMInterpolationValues = { {1.30, 3000.0}, - {1.43, 3000.0}, // meters, RPM + {1.43, 3000.0}, {2.15, 3050.0}, {2.864967, 3215.271125}, {3.65, 3400.0}, @@ -110,159 +123,141 @@ public interface RPMInterpolation{ } public interface RPMs { - SmartNumber SHOOT_RPM = new SmartNumber("HoodedShooter/Shoot State Target RPM", 3400.0); - SmartNumber FERRY_RPM = new SmartNumber("HoodedShooter/Ferry State Target RPM", 2000.0); - public final double REVERSE = -0.0; - public final double HUB_RPM = 0.0; - public final double LEFT_CORNER_RPM = 0.0; // TBD - public final double RIGHT_CORNER_RPM = 0.0; // TBD - public final double STOW = 0.0; // TBD + public final SmartNumber SHOOT_RPM = new SmartNumber("HoodedShooter/Shoot State Target RPM", 3400.0); + public final SmartNumber FERRY_RPM = new SmartNumber("HoodedShooter/Ferry State Target RPM", 2000.0); + public final double REVERSE = 0.0; + public final double HUB_RPM = 0.0; + public final double LEFT_CORNER_RPM = 0.0; + public final double RIGHT_CORNER_RPM = 0.0; + public final double STOW = 0.0; } - public interface Angles { - SmartNumber SHOOT_ANGLE = new SmartNumber("HoodedShooter/Shoot State Target Angle (deg)", 15.0); - SmartNumber FERRY_ANGLE = new SmartNumber("HoodedShooter/Ferry State Target Angle (deg)", 20.0); - - public final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(7); - public final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(36.80); - public final Rotation2d HUB_ANGLE = Rotation2d.fromDegrees(12); - public final Rotation2d LEFT_CORNER_ANGLE = Rotation2d.fromDegrees(10); - public final Rotation2d RIGHT_CORNER_ANGLE = Rotation2d.fromDegrees(10); - } - - public interface Hood { - public final double GEAR_RATIO = 1290300.0 / 5967.0; - public final double SENSOR_TO_HOOD_RATIO = 360.0 / 36.0; - - public final Rotation2d ENCODER_OFFSET = Rotation2d.fromDegrees(0.0); - } public interface Shooter { public final double GEAR_RATIO = 1.0; - public final double FLYWHEEL_RADIUS = Units.inchesToMeters(3.965 / 2); } - - } - - public interface Turret { - Rotation2d MAX_VEL = new Rotation2d(Units.degreesToRadians(600.0)); - Rotation2d MAX_ACCEL = new Rotation2d(Units.degreesToRadians(600.0)); - double TOLERANCE_DEG = 2.0; - - Rotation2d HUB = Rotation2d.fromDegrees(0.0); - Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(0.0); - Rotation2d RIGHT_CORNER = Rotation2d.fromDegrees(0.0); - - public interface Constants { - double RANGE = 210.0; - Transform2d TURRET_OFFSET = new Transform2d(Units.inchesToMeters(0.0), Units.inchesToMeters(0.0), Rotation2d.kZero); - double TURRET_HEIGHT = Units.inchesToMeters(0.0); - public interface Encoder18t { - public final int TEETH = 18; - public final Rotation2d OFFSET = new Rotation2d(); - } + public interface Intake { + public final double GEAR_RATIO = (32.0 / 18.0) * (64.0 / 18.0) * (60.0 / 9.0); - public interface Encoder17t { - public final int TEETH = 17; - public final Rotation2d OFFSET = new Rotation2d(); - } + public final Rotation2d PIVOT_STOW_ANGLE = Rotation2d.fromDegrees(90.0); + public final Rotation2d PIVOT_INTAKE_OUTAKE_ANGLE = Rotation2d.fromDegrees(0.0); - public interface BigGear { - public final int TEETH = 95; - } + public final Rotation2d PIVOT_ANGLE_TOLERANCE = Rotation2d.fromDegrees(3.0); + public final double FORWARD_MAX_ROTATIONS = -30.0 / 360.0; + public final double BACKWARDS_MAX_ROTATIONS = 90.0 / 360.0; - public interface SoftwareLimit { - public final double FORWARD_MAX_ROTATIONS = 210.0 / 360.0; - public final double BACKWARDS_MAX_ROTATIONS = -210.0 / 360.0; - } + public final Rotation2d PIVOT_ANGLE_OFFSET = new Rotation2d(); + public final Rotation2d PIVOT_MAX_ANGLE = Rotation2d.fromDegrees(190.0); + public final Rotation2d PIVOT_MIN_ANGLE = Rotation2d.fromDegrees(80.0); - public final double GEAR_RATIO_MOTOR_TO_MECH = 1425.0 / 36.0; - } + public final Rotation2d PIVOT_MAX_VEL = Rotation2d.fromDegrees(300.0); + public final Rotation2d PIVOT_MAX_ACCEL = Rotation2d.fromDegrees(300.0); } - public interface ClimberHopper { - public interface Constants { - double GEAR_RATIO = 45.0; - - double MIN_HEIGHT_METERS = 0; - double MAX_HEIGHT_METERS = 1; + public interface ShootOnTheFly { + public final int MAX_ITERATIONS = 5; + public final double TIME_TOLERANCE = 0.01; + public final SmartNumber UPDATE_DELAY = new SmartNumber("HoodedShooter/SOTM/update delay", 0.00); + } - double MASS_KG = 1; + public interface Spindexer { + public final double FORWARD_SPEED = 6000.0; + public final double REVERSE_SPEED = -6000.0; + public final double STOP_SPEED = 0.0; - double NUM_ROTATIONS_TO_REACH_TOP = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (0.480 / 13); // Number of rotations that the motor has to spin, NOT the gear - double POSITION_CONVERSION_FACTOR = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / NUM_ROTATIONS_TO_REACH_TOP; - double VELOCITY_CONVERSION_FACTOR = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / NUM_ROTATIONS_TO_REACH_TOP / 60; + public final double RPM_TOLERANCE = 400.0; - double DRUM_RADIUS_METERS = ((MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (NUM_ROTATIONS_TO_REACH_TOP / GEAR_RATIO)) / 2 / Math.PI; + public interface Constants { + public final double GEAR_RATIO = 8.0 / 1.0; } + } - double CLIMBER_UP_HEIGHT_METERS = Constants.MAX_HEIGHT_METERS; - double CLIMBER_DOWN_HEIGHT_METERS = 0.2; - double HOPPER_DOWN_HEIGHT_METERS = Constants.MIN_HEIGHT_METERS; - double HOPPER_UP_HEIGHT_METERS = 0.5; - - double STALL = 10.0; + public interface Swerve { + public final double MODULE_VELOCITY_DEADBAND_M_PER_S = 0.1; + public final double ROTATIONAL_DEADBAND_RAD_PER_S = 0.1; - double ROTATIONS_AT_BOTTOM = 0.0; + public interface Alignment { + public interface Constraints { + public final double DEFAULT_MAX_VELOCITY = 4.3; + public final double DEFAULT_MAX_ACCELERATION = 15.0; + public final double DEFAULT_MAX_ANGULAR_VELOCITY = Units.degreesToRadians(400.0); + public final double DEFAULT_MAX_ANGULAR_ACCELERATION = Units.degreesToRadians(900.0); + } - double DEBOUNCE = 0.25; + public interface Targets {} - double GYRO_TOLERANCE = 0.0; + public interface Tolerances { + public final double X_TOLERANCE = Units.inchesToMeters(2.0); + public final double Y_TOLERANCE = Units.inchesToMeters(2.0); + public final double THETA_TOLERANCE_DEG = 2.0; - double HEIGHT_TOLERANCE_METERS = 0.02; + public final Pose2d POSE_TOLERANCE = new Pose2d( + Units.inchesToMeters(2.0), + Units.inchesToMeters(2.0), + Rotation2d.fromDegrees(2.0)); - double RAMP_RATE = 50.0; + public final double MAX_VELOCITY_WHEN_ALIGNED = 0.15; - double MOTOR_VOLTAGE = 3.5; - } + public final double ALIGNMENT_DEBOUNCE = 0.15; + } + } - public interface Vision { - Vector MT1_STDEVS = VecBuilder.fill(0.5, 0.5, 1.0); - Vector MT2_STDEVS = VecBuilder.fill(0.7, 0.7, 694694); - } + public interface Constraints { + public final double MAX_VELOCITY_M_PER_S = 4.3; + public final double MAX_ACCEL_M_PER_S_SQUARED = 15.0; + public final double MAX_ANGULAR_VEL_RAD_PER_S = Units.degreesToRadians(400.0); + public final double MAX_ANGULAR_ACCEL_RAD_PER_S = Units.degreesToRadians(900.0); - public interface Swerve { - double MODULE_VELOCITY_DEADBAND_M_PER_S = 0.1; - double ROTATIONAL_DEADBAND_RAD_PER_S = 0.1; - public interface Constraints { - double MAX_VELOCITY_M_PER_S = 4.3; - double MAX_ACCEL_M_PER_S_SQUARED = 15.0; - double MAX_ANGULAR_VEL_RAD_PER_S = Units.degreesToRadians(400); - double MAX_ANGULAR_ACCEL_RAD_PER_S = Units.degreesToRadians(900); - - PathConstraints DEFAULT_CONSTRAINTS = + public final PathConstraints DEFAULT_CONSTRAINTS = new PathConstraints( MAX_VELOCITY_M_PER_S, MAX_ACCEL_M_PER_S_SQUARED, MAX_ANGULAR_VEL_RAD_PER_S, MAX_ANGULAR_ACCEL_RAD_PER_S); } + } - public interface Alignment { - public interface Constraints { - double DEFAULT_MAX_VELOCITY = 4.3; - double DEFAULT_MAX_ACCELERATION = 15.0; - double DEFUALT_MAX_ANGULAR_VELOCITY = Units.degreesToRadians(400); - double DEFAULT_MAX_ANGULAR_ACCELERATION = Units.degreesToRadians(900); - } + public interface Turret { + public final Rotation2d MAX_VEL = new Rotation2d(Units.degreesToRadians(600.0)); + public final Rotation2d MAX_ACCEL = new Rotation2d(Units.degreesToRadians(600.0)); + public final double TOLERANCE_DEG = 2.0; - public interface Tolerances { - double X_TOLERANCE = Units.inchesToMeters(2.0); - double Y_TOLERANCE = Units.inchesToMeters(2.0); - double THETA_TOLERANCE_DEG = 2.0; + public final Rotation2d HUB = Rotation2d.fromDegrees(0.0); + public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(0.0); + public final Rotation2d RIGHT_CORNER = Rotation2d.fromDegrees(0.0); - Pose2d POSE_TOLERANCE = new Pose2d( - Units.inchesToMeters(2.0), - Units.inchesToMeters(2.0), - Rotation2d.fromDegrees(2.0)); + public interface Constants { + public final double RANGE = 210.0; - double MAX_VELOCITY_WHEN_ALIGNED = 0.15; + public final Transform2d TURRET_OFFSET = new Transform2d(Units.inchesToMeters(0.0), Units.inchesToMeters(0.0), Rotation2d.kZero); + public final double TURRET_HEIGHT = Units.inchesToMeters(0.0); - double ALIGNMENT_DEBOUNCE = 0.15; + public final double GEAR_RATIO_MOTOR_TO_MECH = 1425.0 / 36.0; + + public interface BigGear { + public final int TEETH = 95; } - public interface Targets {} + public interface Encoder17t { + public final int TEETH = 17; + public final Rotation2d OFFSET = new Rotation2d(); + } + + public interface Encoder18t { + public final int TEETH = 18; + public final Rotation2d OFFSET = new Rotation2d(); + } + + public interface SoftwareLimit { + public final double FORWARD_MAX_ROTATIONS = 210.0 / 360.0; + public final double BACKWARDS_MAX_ROTATIONS = -210.0 / 360.0; + } } } -} + + public interface Vision { + public final Vector MT1_STDEVS = VecBuilder.fill(0.5, 0.5, 1.0); + public final Vector MT2_STDEVS = VecBuilder.fill(0.7, 0.7, 694694.0); + } +} \ No newline at end of file From 346028b976e5e661dcf3534250795ce3169c77db Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Sat, 21 Feb 2026 23:06:54 -0500 Subject: [PATCH 11/18] FEAT: port over brute force approach to turret zeroing --- .../stuypulse/robot/constants/Settings.java | 6 +- .../robot/subsystems/turret/TurretImpl.java | 61 +++++++++---------- .../util/turret/TurretAngleCalculator.java | 59 ++++++++++++++++++ 3 files changed, 92 insertions(+), 34 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/util/turret/TurretAngleCalculator.java diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 4f775143..b1824012 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -227,6 +227,10 @@ public interface Turret { public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(0.0); public final Rotation2d RIGHT_CORNER = Rotation2d.fromDegrees(0.0); + double RESOLUTION_OF_ABSOLUTE_ENCODER = 0.1; + Rotation2d MAX_THEORETICAL_ROTATION = Rotation2d.fromDegrees(612); + Rotation2d MIN_THEORETICAL_ROTATION = Rotation2d.fromDegrees(-612); + public interface Constants { public final double RANGE = 210.0; @@ -236,7 +240,7 @@ public interface Constants { public final double GEAR_RATIO_MOTOR_TO_MECH = 1425.0 / 36.0; public interface BigGear { - public final int TEETH = 95; + public final int TEETH = 90; } public interface Encoder17t { diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java index b190e035..9b0b7cf3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java @@ -5,13 +5,13 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.turret; - import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Turret.Constants; import com.stuypulse.robot.util.SysId; +import com.stuypulse.robot.util.turret.TurretAngleCalculator; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -30,7 +30,7 @@ public class TurretImpl extends Turret { private boolean hasUsedAbsoluteEncoder; private Optional voltageOverride; private final PositionVoltage controller; - + public TurretImpl() { motor = new TalonFX(Ports.Turret.MOTOR, Ports.bus); @@ -38,10 +38,8 @@ public TurretImpl() { encoder18t = new CANcoder(Ports.Turret.ENCODER18T, Ports.bus); Motors.Turret.TURRET.configure(motor); - encoder17t.getConfigurator().apply(Motors.Turret.ENCODER_17T); - encoder18t.getConfigurator().apply(Motors.Turret.ENCODER_18T); - - // motor.getConfigurator().apply(Motors.Turret.turretSoftwareLimitSwitchConfigs); + Motors.Turret.ENCODER_17T.configure(encoder17t); + Motors.Turret.ENCODER_18T.configure(encoder18t); seedTurret(); @@ -58,33 +56,34 @@ private Rotation2d getEncoderPos18t() { return Rotation2d.fromRotations(this.encoder18t.getAbsolutePosition().getValueAsDouble()); } - public Rotation2d getAbsoluteTurretAngle() { - final int inverseMod17t = 1; - final int inverseMod18t = -1; + public Rotation2d getVectorSpaceAngle() { + return TurretAngleCalculator.getAbsoluteAngle(getEncoderPos17t().getDegrees(), getEncoderPos18t().getDegrees()); + } - final Rotation2d encoder17tPosition = getEncoderPos17t(); - final double numberOfGearTeethRotated17 = (encoder17tPosition.getRotations() - * (double) Constants.Encoder17t.TEETH); + public void zeroEncoders() { + double encoderPos17T = encoder17t.getAbsolutePosition().getValueAsDouble(); + double encoderPos18T = encoder18t.getAbsolutePosition().getValueAsDouble(); - final Rotation2d encoder18tPosition = getEncoderPos18t(); - final double numberOfGearTeethRotated18 = (encoder18tPosition.getRotations() - * (double) Constants.Encoder18t.TEETH); + encoder17t.getConfigurator().refresh(Motors.Turret.ENCODER_17T.getConfiguration().MagnetSensor); + encoder18t.getConfigurator().refresh(Motors.Turret.ENCODER_18T.getConfiguration().MagnetSensor); - final double crt_Partial17 = numberOfGearTeethRotated17 * inverseMod17t * Constants.Encoder17t.TEETH; - final double crt_Partial18 = numberOfGearTeethRotated18 * inverseMod18t * Constants.Encoder18t.TEETH; + double currentOffset17T = Motors.Turret.ENCODER_17T.getConfiguration().MagnetSensor.MagnetOffset; + double currentOffset18T = Motors.Turret.ENCODER_18T.getConfiguration().MagnetSensor.MagnetOffset; - double crt_pos = (crt_Partial17 + crt_Partial18) - % (Constants.Encoder17t.TEETH * Constants.Encoder18t.TEETH); + double newOffset17T = currentOffset17T - encoderPos17T; + double newOffset18T = currentOffset18T - encoderPos18T; - // Java's % operator is not actually the same as the modulo operator, the lines below account for that - crt_pos = (crt_pos < 0) ? (crt_pos + Constants.Encoder17t.TEETH * Constants.Encoder18t.TEETH) - : crt_pos; + Motors.Turret.ENCODER_17T.withMagnetOffset(newOffset17T); + Motors.Turret.ENCODER_18T.withMagnetOffset(newOffset18T); - final double turretAngle = (crt_pos / (double) Constants.BigGear.TEETH); + Motors.Turret.ENCODER_17T.configure(encoder17t); + Motors.Turret.ENCODER_18T.configure(encoder18t); + } - return Rotation2d.fromRotations(turretAngle); + public void seedTurret() { + motor.setPosition(getVectorSpaceAngle().getRotations()); } - + @Override public Rotation2d getAngle() { return Rotation2d.fromDegrees(motor.getPosition().getValueAsDouble()); @@ -102,20 +101,16 @@ private double getDelta(double target, double current) { else if (delta < -180) delta += 360; if (Math.abs(current + delta) < Constants.RANGE) return delta; - + return delta < 0 ? delta + 360 : delta - 360; } - public void seedTurret() { - motor.setPosition(0.0); - } - @Override public void periodic() { super.periodic(); - if (!hasUsedAbsoluteEncoder || getAbsoluteTurretAngle().getRotations() > 1.0 || getAngle().getRotations() < 0.0) { - motor.setPosition((getAbsoluteTurretAngle().getDegrees() % 360.0) / 360.0); + if (!hasUsedAbsoluteEncoder) { + motor.setPosition(getVectorSpaceAngle().getRotations()); hasUsedAbsoluteEncoder = true; System.out.println("Absolute Encoder Reset triggered"); } @@ -139,7 +134,7 @@ public void periodic() { if (Settings.DEBUG_MODE) { SmartDashboard.putNumber("Turret/Encoder18t Abs Position (Rot)", encoder18t.getAbsolutePosition().getValueAsDouble()); SmartDashboard.putNumber("Turret/Encoder17t Abs Position (Rot)", encoder17t.getAbsolutePosition().getValueAsDouble()); - SmartDashboard.putNumber("Turret/CRT Position (Rot)", getAbsoluteTurretAngle().getRotations()); + SmartDashboard.putNumber("Turret/Vector Space Position (Rot)", getVectorSpaceAngle().getRotations()); SmartDashboard.putNumber("Turret/Relative Encoder Position (Rot)", motor.getPosition().getValueAsDouble() * 360.0); SmartDashboard.putNumber("Turret/Voltage", motor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Turret/Error", motor.getClosedLoopError().getValueAsDouble() * 360.0); diff --git a/src/main/java/com/stuypulse/robot/util/turret/TurretAngleCalculator.java b/src/main/java/com/stuypulse/robot/util/turret/TurretAngleCalculator.java new file mode 100644 index 00000000..b0918323 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/turret/TurretAngleCalculator.java @@ -0,0 +1,59 @@ +package com.stuypulse.robot.util.turret; + +import com.stuypulse.robot.constants.Settings; + +import edu.wpi.first.math.geometry.Rotation2d; + +public class TurretAngleCalculator { + + private static final double MAX_ANGLE_DEGREES = Settings.Turret.MAX_THEORETICAL_ROTATION.getDegrees(); + private static final double MIN_ANGLE_DEGREES = Settings.Turret.MIN_THEORETICAL_ROTATION.getDegrees(); + private static final double RESOLUTION = Settings.Turret.RESOLUTION_OF_ABSOLUTE_ENCODER; + private static final int NUM_POINTS = (int) ((MAX_ANGLE_DEGREES - MIN_ANGLE_DEGREES) / RESOLUTION); + private static int leastDistanceIndex = 0; + + private static final double[] mechanismAngles = new double[NUM_POINTS]; + private static final double[] ARRAY_17T = generateEncoderValues(17); + private static final double[] ARRAY_18T = generateEncoderValues(18); + + private static double[] generateEncoderValues(int teeth) { + double[] values = new double[NUM_POINTS]; + double gearRatio = 1.0 * Settings.Turret.Constants.BigGear.TEETH / teeth; + int i = 0; + + for (double angle = MIN_ANGLE_DEGREES; angle < MAX_ANGLE_DEGREES; angle += RESOLUTION) { + mechanismAngles[i] = angle; + values[i] = (((angle * gearRatio) % 360.0) + 360.0) % 360.0; + i++; + } + + return values; + } + + public static Rotation2d getAbsoluteAngle(double encoder17TValue, double encoder18TValue) { + double leastDistance = Double.MAX_VALUE; + for (int i = 0; i < NUM_POINTS; i++) { + double diff17 = Math.abs(encoder17TValue - ARRAY_17T[i]); + double diff18 = Math.abs(encoder18TValue - ARRAY_18T[i]); + + diff17 = Math.min(diff17, 360.0 - diff17); + diff18 = Math.min(diff18, 360.0 - diff18); + + double distance17 = diff17 * diff17; + double distance18 = diff18 * diff18; + + double distance = distance17 + distance18; + + if (distance < leastDistance) { + leastDistance = distance; + leastDistanceIndex = i; + } + } + + return Rotation2d.fromDegrees(mechanismAngles[leastDistanceIndex]); + } + + public static int lowestDistanceIndex() { + return leastDistanceIndex; + } +} From d52acabc7f96b915de482e5d986c9919297084be Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Sat, 21 Feb 2026 23:44:30 -0500 Subject: [PATCH 12/18] FIX: hood configured and zeroing logic setup --- .../swerve/pidToPose/SwerveDrivePIDToPose.java | 2 +- .../com/stuypulse/robot/constants/Settings.java | 16 ++++++++++++++-- .../subsystems/hoodedshooter/hood/HoodImpl.java | 11 ++++++++--- 3 files changed, 23 insertions(+), 6 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java index 0fd73f71..b68b37f7 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java @@ -70,7 +70,7 @@ public SwerveDrivePIDToPose(Supplier targetPose) { new PIDController(Alignment.XY.kP, Alignment.XY.kI, Alignment.XY.kD), new PIDController(Alignment.XY.kP, Alignment.XY.kI, Alignment.XY.kD), new AnglePIDController(Alignment.THETA.kP, Alignment.THETA.kI, Alignment.THETA.kD) - .setSetpointFilter(new AMotionProfile(Settings.Swerve.Alignment.Constraints.DEFUALT_MAX_ANGULAR_VELOCITY, Settings.Swerve.Alignment.Constraints.DEFAULT_MAX_ANGULAR_ACCELERATION))); + .setSetpointFilter(new AMotionProfile(Settings.Swerve.Alignment.Constraints.DEFAULT_MAX_ANGULAR_VELOCITY, Settings.Swerve.Alignment.Constraints.DEFAULT_MAX_ANGULAR_ACCELERATION))); maxVelocity = Settings.Swerve.Alignment.Constraints.DEFAULT_MAX_VELOCITY; maxAcceleration = Settings.Swerve.Alignment.Constraints.DEFAULT_MAX_ACCELERATION; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index b1824012..2ac63b24 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -104,12 +104,24 @@ public interface FerryRPMInterpolation { } public interface Hood { + /** + * + * The absolute encoder is mounted on a 10.67:1 gear reduction relative to the + * hood mechanism. This means: + * + * - The encoder rotates 10.67 times for every 1 full rotation of the hood. + * - The hood's physical range of motion is only 33 degrees. + * + * Because 33° * 10.67 = ~352°, the encoder will never exceed 360° over the + * entire hood travel. Therefore, the absolute encoder reading (0–360°) + * uniquely maps to the hood’s 0–33° mechanical range without any ambiguity. + * + */ public final double GEAR_RATIO = 17024.0 / 135.0; public final double ENCODER_TO_MECH = 32.0 / 3.0; public final Rotation2d ENCODER_OFFSET = Rotation2d.fromDegrees(0.0); } - public interface RPMInterpolation { public final double[][] distanceRPMInterpolationValues = { {1.30, 3000.0}, @@ -240,7 +252,7 @@ public interface Constants { public final double GEAR_RATIO_MOTOR_TO_MECH = 1425.0 / 36.0; public interface BigGear { - public final int TEETH = 90; + public final int TEETH = 95; } public interface Encoder17t { diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java index fd86f36c..68731035 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java @@ -37,8 +37,8 @@ public HoodImpl() { Motors.HoodedShooter.Hood.HOOD.configure(hoodMotor); hoodMotor.getConfigurator().apply(Motors.HoodedShooter.Hood.SLOT_0); - hoodMotor.getConfigurator().apply(Motors.HoodedShooter.Hood.SOFT_LIMITS); + hoodEncoder.getConfigurator().apply(Motors.HoodedShooter.Hood.HOOD_ENCODER); controller = new PositionVoltage(getTargetAngle().getRotations()) @@ -57,7 +57,12 @@ public void periodic() { super.periodic(); if (!hasUsedAbsoluteEncoder) { - hoodMotor.setPosition(hoodEncoder.getAbsolutePosition().getValueAsDouble() / Settings.HoodedShooter.Hood.SENSOR_TO_HOOD_RATIO); + /* + * Example: + * Let's say the hood rotates 0.1 rotations. Then, the encoder has rotated 0.1 * 10.67 rotations + * To convert the encoder reading to the mechanism position, we simply do (0.1 * 10.67) / 10.67 = 0.1 + */ + hoodMotor.setPosition(hoodEncoder.getAbsolutePosition().getValueAsDouble() / Settings.HoodedShooter.Hood.ENCODER_TO_MECH); hasUsedAbsoluteEncoder = true; } @@ -72,7 +77,7 @@ public void periodic() { } if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("HoodedShooter/Hood/Hood Absolute Angle (deg)", hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.HoodedShooter.Hood.SENSOR_TO_HOOD_RATIO); + SmartDashboard.putNumber("HoodedShooter/Hood/Hood Absolute Angle (deg)", hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.HoodedShooter.Hood.ENCODER_TO_MECH); SmartDashboard.putNumber("HoodedShooter/Hood/Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("HoodedShooter/Hood/Supply Current", hoodMotor.getSupplyCurrent().getValueAsDouble()); From 93c40620fc5d7659dd795a9db4445d99dc9dedc1 Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Sat, 21 Feb 2026 23:47:56 -0500 Subject: [PATCH 13/18] CLEAN: spotlessApply --- .../commands/climberhopper/ClimberHopperDefaultCommand.java | 4 ---- src/main/java/com/stuypulse/robot/constants/Settings.java | 2 +- .../robot/subsystems/climberhopper/ClimberHopper.java | 4 ++-- .../robot/subsystems/climberhopper/ClimberHopperImpl.java | 1 + .../robot/subsystems/climberhopper/ClimberHopperSim.java | 4 ++-- .../robot/subsystems/hoodedshooter/HoodedShooter.java | 2 +- .../robot/subsystems/hoodedshooter/hood/HoodImpl.java | 2 +- .../robot/subsystems/hoodedshooter/shooter/Shooter.java | 3 +-- .../robot/util/hoodedshooter/HoodAngleCalculator.java | 2 +- .../stuypulse/robot/util/turret/TurretAngleCalculator.java | 5 +++++ 10 files changed, 15 insertions(+), 14 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java index 203630dc..434b1b25 100644 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java @@ -5,13 +5,9 @@ /***************************************************************/ package com.stuypulse.robot.commands.climberhopper; -import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 2ac63b24..06c17038 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -79,7 +79,7 @@ public interface Angles { public final SmartNumber FERRY_ANGLE = new SmartNumber("HoodedShooter/Ferry State Target Angle (deg)", 20.0); public final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(7.0); - public final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(36.80); + public final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(40.0); public final Rotation2d HUB_ANGLE = Rotation2d.fromDegrees(12.0); public final Rotation2d LEFT_CORNER_ANGLE = Rotation2d.fromDegrees(10.0); public final Rotation2d RIGHT_CORNER_ANGLE = Rotation2d.fromDegrees(10.0); diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java index 6b2e96cf..35197ae1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java @@ -5,14 +5,14 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.climberhopper; -import java.util.Optional; - import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.Optional; + public abstract class ClimberHopper extends SubsystemBase { private static final ClimberHopper instance; diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java index 8e7f41e1..7ce9c6b6 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java @@ -6,6 +6,7 @@ package com.stuypulse.robot.subsystems.climberhopper; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; + import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java index 527567e4..9ae11e3e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java @@ -6,14 +6,14 @@ package com.stuypulse.robot.subsystems.climberhopper; -import java.util.Optional; - import com.stuypulse.robot.constants.Settings; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.ElevatorSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.Optional; + public class ClimberHopperSim extends ClimberHopper { diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java index 6491e296..b1945ee9 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java @@ -1,4 +1,4 @@ -/************************ PROJECT ALPHA *************************/ +/************************ PROJECT TRIBECBOT *************************/ /* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java index 68731035..6465a053 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java @@ -1,4 +1,4 @@ -/************************ PROJECT ALPHA *************************/ +/************************ PROJECT TRIBECBOT *************************/ /* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java index e727406d..d2e862bc 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java @@ -1,4 +1,4 @@ -/************************ PROJECT ALPHA *************************/ +/************************ PROJECT TRIBECBOT *************************/ /* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ @@ -7,7 +7,6 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; import com.stuypulse.robot.util.hoodedshooter.HoodAngleCalculator; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; diff --git a/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java b/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java index 519b613e..8f87fb35 100644 --- a/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java @@ -1,4 +1,4 @@ -/************************ PROJECT ALPHA *************************/ +/************************ PROJECT TRIBECBOT *************************/ /* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ diff --git a/src/main/java/com/stuypulse/robot/util/turret/TurretAngleCalculator.java b/src/main/java/com/stuypulse/robot/util/turret/TurretAngleCalculator.java index b0918323..263ae98e 100644 --- a/src/main/java/com/stuypulse/robot/util/turret/TurretAngleCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/turret/TurretAngleCalculator.java @@ -1,3 +1,8 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ package com.stuypulse.robot.util.turret; import com.stuypulse.robot.constants.Settings; From a86ed3ef6b38c3b811464c4129d141b6f2c4805c Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Sun, 22 Feb 2026 00:18:30 -0500 Subject: [PATCH 14/18] FEAT: copy over SOTM calculation and shooter/ferry interp impl from alpha, clean up other files (todo: still need to add SOTM states to subsystems and SOTM button) --- .../stuypulse/robot/constants/Settings.java | 1 + .../hoodedshooter/shooter/Shooter.java | 8 +- .../robot/subsystems/turret/Turret.java | 4 +- .../hoodedshooter/HoodAngleCalculator.java | 105 +++++---- .../util/hoodedshooter/ShotCalculator.java | 199 ++++++++++-------- 5 files changed, 180 insertions(+), 137 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 06c17038..cf4880b7 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -146,6 +146,7 @@ public interface RPMs { public interface Shooter { public final double GEAR_RATIO = 1.0; + public final double FLYWHEEL_RADIUS = Units.inchesToMeters(3.965 / 2); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java index d2e862bc..c18ed7c3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java @@ -67,11 +67,7 @@ public double getTargetRPM() { } public double getShootRPM() { - return Settings.HoodedShooter.RPMs.SHOOT_RPM.get(); // will return different speeds in future based on distance to hub - } - - public double getFerryRPM() { - return Settings.HoodedShooter.RPMs.FERRY_RPM.get(); + return Settings.HoodedShooter.RPMs.SHOOT_RPM.get(); // Adjustable RPM on Glass } public boolean atTolerance() { @@ -91,6 +87,6 @@ public void periodic() { SmartDashboard.putNumber("HoodedShooter/Shooter/Current RPM", getShooterRPM()); SmartDashboard.putNumber("HoodedShooter/Shooter/Target RPM", getTargetRPM()); - SmartDashboard.putNumber("InterpolationTesting/Shooter Interpolated Target RPM", HoodAngleCalculator.interpolateShooterRPM().get()); + SmartDashboard.putNumber("InterpolationTesting/Shooter Interpolated Target Shoot RPM", HoodAngleCalculator.interpolateShooterRPM().get()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java index bfa74453..7dc79e49 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java @@ -60,10 +60,10 @@ public Rotation2d getTargetAngle() { case ZERO -> Rotation2d.kZero; case SHOOTING -> getScoringAngle(); case FERRYING -> getFerryAngle(); - case TESTING -> driverInputToAngle(); case HUB -> Settings.Turret.HUB; case LEFT_CORNER -> Settings.Turret.LEFT_CORNER; case RIGHT_CORNER -> Settings.Turret.RIGHT_CORNER; + case TESTING -> driverInputToAngle(); }; } @@ -117,7 +117,7 @@ public void periodic() { public Rotation2d getPointAtTargetAngle(Pose2d targetPose) { Pose2d robotPose = CommandSwerveDrivetrain.getInstance().getPose(); - Pose2d turretPose = CommandSwerveDrivetrain.getInstance().getTurretPose(); // TODO: TEST IF THIS PLUS SHOULD BE MINUS + Pose2d turretPose = CommandSwerveDrivetrain.getInstance().getTurretPose(); Vector2D turret = new Vector2D(turretPose.getTranslation()); Vector2D target = new Vector2D(targetPose.getTranslation()); diff --git a/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java b/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java index 8f87fb35..0d4abca6 100644 --- a/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java @@ -4,28 +4,34 @@ /* that can be found in the repository LICENSE file. */ /***************************************************************/ package com.stuypulse.robot.util.hoodedshooter; +import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.HoodedShooter.AngleInterpolation; import com.stuypulse.robot.constants.Settings.HoodedShooter.FerryRPMInterpolation; import com.stuypulse.robot.constants.Settings.HoodedShooter.RPMInterpolation; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.util.hoodedshooter.ShotCalculator.AlignAngleSolution; +import com.stuypulse.robot.util.hoodedshooter.ShotCalculator.SOTMSolution; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.function.Supplier; public class HoodAngleCalculator { + + public static SOTMSolution sol; + + private static FieldObject2d hubPose2d; + private static FieldObject2d virtualHubPose2d; + private static FieldObject2d futureTurretPose2d; + public static InterpolatingDoubleTreeMap distanceAngleInterpolator; public static InterpolatingDoubleTreeMap distanceRPMInterpolator; public static InterpolatingDoubleTreeMap ferryingDistanceRPMInterpolator; @@ -72,12 +78,10 @@ public static Supplier interpolateShooterRPM() { return () -> { CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); - Translation2d hubPose = Field.getHubPose().getTranslation(); - Translation2d currentPose = swerve.getTurretPose().getTranslation(); - - double distanceMeters = hubPose.getDistance(currentPose); + Pose2d hubPose = Field.getHubPose(); + Pose2d turretPose = swerve.getTurretPose(); - double targetRPM = distanceRPMInterpolator.get(distanceMeters); + double targetRPM = ShotCalculator.solveInterpolation(turretPose, hubPose).targetRPM(); SmartDashboard.putNumber("HoodedShooter/Interpolated RPM", targetRPM); @@ -102,40 +106,55 @@ public static Supplier interpolateFerryingRPM() { }; } + public static void updateSOTMSolution() { + + CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); + + Pose2d robotPose = swerve.getPose(); + Pose2d hubPose = Field.getHubPose(); + + ChassisSpeeds robotRelativeSpeeds = swerve.getChassisSpeeds(); + ChassisSpeeds fieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds( + robotRelativeSpeeds, + robotPose.getRotation() + ); + + Pose2d futureTurretPose = swerve.getTurretPose().exp( + new Twist2d( + robotRelativeSpeeds.vxMetersPerSecond * Settings.ShootOnTheFly.UPDATE_DELAY.doubleValue(), + robotRelativeSpeeds.vyMetersPerSecond * Settings.ShootOnTheFly.UPDATE_DELAY.doubleValue(), + 0 + ) + ); + + + SOTMSolution solution = ShotCalculator.solveShootOnTheMove( + futureTurretPose, + robotPose, + hubPose, + fieldRelativeSpeeds, + Settings.ShootOnTheFly.MAX_ITERATIONS, + Settings.ShootOnTheFly.TIME_TOLERANCE + ); + + sol = solution; + + hubPose2d.setPose(Robot.isBlue() ? hubPose : Field.transformToOppositeAlliance(hubPose)); + virtualHubPose2d.setPose((Robot.isBlue() ? sol.virtualPose() : Field.transformToOppositeAlliance(sol.virtualPose()))); + futureTurretPose2d.setPose((Robot.isBlue() ? futureTurretPose : Field.transformToOppositeAlliance(futureTurretPose))); + + + SmartDashboard.putNumber("HoodedShooter/SOTM/calculated turret angle", sol.targetTurretAngle().getDegrees()); + SmartDashboard.putNumber("HoodedShooter/SOTM/calculated hood angle", sol.targetHoodAngle().getDegrees()); + SmartDashboard.putNumber("HoodedShooter/SOTM/calculated flight time", sol.flightTime()); + SmartDashboard.putNumber("HoodedShooter/SOTM/turret dist to virtual pose", futureTurretPose.getTranslation().getDistance(sol.virtualPose().getTranslation())); + } + public static Supplier calculateHoodAngleSOTM() { - return () -> { - CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); - HoodedShooter hdsr = HoodedShooter.getInstance(); - - Pose2d currentPose = swerve.getPose(); - Pose2d turretPose = swerve.getTurretPose(); - - ChassisSpeeds robotRelSpeeds = swerve.getChassisSpeeds(); - ChassisSpeeds fieldRelSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds( - robotRelSpeeds, - currentPose.getRotation() - ); - - Pose3d targetPose = Field.hubCenter3d; - Pose3d turretPose3d = new Pose3d(new Translation3d(turretPose.getX(), turretPose.getY(), Settings.Turret.Constants.TURRET_HEIGHT), new Rotation3d()); + return () -> sol.targetHoodAngle(); + } - double axMetersPerSecondSquared = swerve.getPigeon2().getAccelerationX().getValueAsDouble(); - double ayMetersPerSecondSquared = swerve.getPigeon2().getAccelerationY().getValueAsDouble(); - - double shooterRPS = hdsr.getTargetRPM() / 60.0; - - AlignAngleSolution sol = ShotCalculator.solveShootOnTheFly( - turretPose3d, - targetPose, - axMetersPerSecondSquared, - ayMetersPerSecondSquared, - fieldRelSpeeds, // current speeds - shooterRPS, - Settings.ShootOnTheFly.MAX_ITERATIONS, - Settings.ShootOnTheFly.TIME_TOLERANCE - ); - - return sol.launchPitchAngle(); - }; + public static Supplier calculateTurretAngleSOTM() { + return () -> sol.targetTurretAngle(); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/hoodedshooter/ShotCalculator.java b/src/main/java/com/stuypulse/robot/util/hoodedshooter/ShotCalculator.java index 314ca5ed..0d5930cd 100644 --- a/src/main/java/com/stuypulse/robot/util/hoodedshooter/ShotCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/hoodedshooter/ShotCalculator.java @@ -5,133 +5,160 @@ /***************************************************************/ package com.stuypulse.robot.util.hoodedshooter; -import edu.wpi.first.math.geometry.Pose3d; +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.HoodedShooter.AngleInterpolation; +import com.stuypulse.robot.constants.Settings.HoodedShooter.RPMInterpolation; + +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.kinematics.ChassisSpeeds; public final class ShotCalculator { - public static final double g = 9.81; // gravity is not a number - - public record ShotSolution( - Rotation2d launchPitchAngle, - double flightTimeSeconds) { - } + public static final double g = 9.81; - public static ShotSolution solveBallisticWithSpeed( - Pose3d shooterPose, - Pose3d targetPose, - double launchSpeed) { + public static InterpolatingDoubleTreeMap distanceAngleInterpolator; + public static InterpolatingDoubleTreeMap distanceRPMInterpolator; - Translation3d s = shooterPose.getTranslation(); - Translation3d t = targetPose.getTranslation(); - - double dx = t.getX() - s.getX(); - double dy = t.getY() - s.getY(); - double dz = t.getZ() - s.getZ(); + static { + distanceAngleInterpolator = new InterpolatingDoubleTreeMap(); + for (double[] pair : AngleInterpolation.distanceAngleInterpolationValues) { + distanceAngleInterpolator.put(pair[0], pair[1]); + } + } - double d = Math.hypot(dx, dy); - if (d < 1e-9) { - throw new IllegalArgumentException("Horizontal distance too small"); + static { + distanceRPMInterpolator = new InterpolatingDoubleTreeMap(); + for (double[] pair : RPMInterpolation.distanceRPMInterpolationValues) { + distanceRPMInterpolator.put(pair[0], pair[1]); } + } - double v2 = launchSpeed * launchSpeed; + public record StationarySolution( + Rotation2d targetHoodAngle, + double targetRPM, + double flightTimeSeconds) { + } - double discriminant = v2 * v2 - g * (g * d * d + 2.0 * dz * v2); - if (discriminant < 0) { - return new ShotSolution(Rotation2d.kZero, 0); - } + public static StationarySolution solveInterpolation(Pose2d turretPose, Pose2d targetPose) { + + double distanceMeters = turretPose.getTranslation().getDistance(targetPose.getTranslation()); - // LOW-ARC solution (use + for high arc) - double tanTheta = (v2 - Math.sqrt(discriminant)) / (g * d); + // Interpolated Angle + Rotation2d targetHoodAngle = Rotation2d.fromRadians(distanceAngleInterpolator.get(distanceMeters)); - double launchPitch = Math.atan(tanTheta); + // Interpolated RPM + double targetRPM = distanceRPMInterpolator.get(distanceMeters); - double vHoriz = launchSpeed * Math.cos(launchPitch); - double time = d / vHoriz; + // Physics-based TOF + double launchSpeed = 0.5 * targetRPM * (2 * Math.PI / 60.0) * Settings.HoodedShooter.Shooter.FLYWHEEL_RADIUS; + Rotation2d launchAngle = Rotation2d.kCCW_Pi_2.minus(targetHoodAngle); - return new ShotSolution(Rotation2d.fromRadians(launchPitch), time); + double v_x = launchSpeed * Math.cos(launchAngle.getRadians()); + double flightTime = distanceMeters / v_x; + + return new StationarySolution( + targetHoodAngle, + targetRPM, + flightTime + ); } - - public record AlignAngleSolution( - Rotation2d launchPitchAngle, - Rotation2d requiredYaw, - Pose3d estimateTargetPose) { + public record SOTMSolution( + Rotation2d targetHoodAngle, + Rotation2d targetTurretAngle, + Pose2d virtualPose, + double flightTime) { } - public static AlignAngleSolution solveShootOnTheFly( - Pose3d shooterPose, - Pose3d targetPose, - double axMetersPerSecondSquared, - double ayMetersPerSecondSquared, - ChassisSpeeds fieldRelSpeeds, - double targetSpeedRps, + public static SOTMSolution solveShootOnTheMove( + Pose2d turretPose, + Pose2d robotPose, + Pose2d targetPose, + ChassisSpeeds fieldRelativeSpeeds, int maxIterations, double timeTolerance) { - ShotSolution sol = solveBallisticWithSpeed( - shooterPose, - targetPose, - targetSpeedRps + /* + * Start with v_ball * flightTime = distanceToTargetPose. + * + * We know that v_ball = v_robot + v_shooter, so + * (v_robot + v_shooter) * flightTime = distanceToTargetPose + * + * Rearranging, we can get + * (v_shooter) * flight_time = distanceToTargetPose - v_robot * flightTime + * + * So we can instead shoot at a virtual pose and treat the robot as stationary: + * distanceToVirtualPose = distanceToTargetPose - v_robot * flightTime + * (v_shooter) * flight_time = distanceToVirtualPose + * + * Looking at the first equation, we can find the virtual pose with the flight time, + * but looking at the second equation, to get the flight time we need to solveBallisticWithSpeed() + * using the virtual pose, so we have a circular dependence. + * + * Thus, we can make an initial guess for the flight time: the flight time if the robot were stationary + * We want our guess to converge such that the left side equals the right side: + * (v_shooter) * t_guess = distanceToVirtualPose = distance - v_robot * t_guess, which would make t_guess = flightTime + * + * We do the right side first using our inital guess, and then update t_guess with a new guess by + * calculating the flightTime to that virtualPose. + * + * The pose is that the flightTime converges within maxIterations. + */ + + StationarySolution sol = solveInterpolation( + turretPose, + targetPose ); + double t_guess = sol.flightTimeSeconds(); - double t = sol.flightTimeSeconds(); - - Pose3d effectiveTarget = targetPose; + Pose2d virtualPose = targetPose; - Translation3d s = shooterPose.getTranslation(); - + for (int i = 0; i < maxIterations; i++) { - double dx = fieldRelSpeeds.vxMetersPerSecond * t - + 0.5 * axMetersPerSecondSquared * t * t; + double dx = fieldRelativeSpeeds.vxMetersPerSecond * t_guess; + double dy = fieldRelativeSpeeds.vyMetersPerSecond * t_guess; - double dy = fieldRelSpeeds.vyMetersPerSecond * t - + 0.5 * ayMetersPerSecondSquared * t * t; - - effectiveTarget = new Pose3d( + virtualPose = new Pose2d( targetPose.getX() - dx, targetPose.getY() - dy, - targetPose.getZ(), targetPose.getRotation()); - - // SmartDashboard.putNumber("HoodedShooter/Target Pose X", targetPose.getX()); - // SmartDashboard.putNumber("HoodedShooter/Target Pose Y", targetPose.getY()); - - // SmartDashboard.putNumber("HoodedShooter/Virtual Pose X", effectiveTarget.getX()); - // SmartDashboard.putNumber("HoodedShooter/Virtual Pose Y", effectiveTarget.getY()); - - ShotSolution newSol = solveBallisticWithSpeed( - shooterPose, - effectiveTarget, - targetSpeedRps); + StationarySolution newSol = solveInterpolation( + turretPose, + virtualPose + ); - sol = newSol; - if (Math.abs(newSol.flightTimeSeconds() - t) < timeTolerance) { + if (Math.abs(newSol.flightTimeSeconds() - t_guess) < timeTolerance) { break; } - t = newSol.flightTimeSeconds(); + t_guess = newSol.flightTimeSeconds(); + sol = newSol; } - - Translation3d et = effectiveTarget.getTranslation(); + Translation2d virtualTranslation = virtualPose.getTranslation(); + Translation2d turretTranslation = turretPose.getTranslation(); - // all the poses we pass in are field relative, - // so calculated yaw (turret angle) should be field relative as well... right - - double yaw = Math.atan2( //atan2(dy, dx) - et.getY() - s.getY(), - et.getX() - s.getX() + double yaw = Math.atan2( + virtualTranslation.getY() - turretTranslation.getY(), + virtualTranslation.getX() - turretTranslation.getX() ); - return new AlignAngleSolution( - sol.launchPitchAngle(), - Rotation2d.fromRadians(yaw), - effectiveTarget); + Rotation2d targetTurretAngle = Robot.isReal() ? + Rotation2d.fromRadians(-yaw).plus(robotPose.getRotation()) : + Rotation2d.fromRadians(yaw).minus(robotPose.getRotation()); + + return new SOTMSolution( + sol.targetHoodAngle(), + targetTurretAngle, + virtualPose, + sol.flightTimeSeconds() + ); } } \ No newline at end of file From 294e9b67bafcb787493554a4dfaf578bb3398cef Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Tue, 24 Feb 2026 13:01:28 -0500 Subject: [PATCH 15/18] feat: burger intake routine + changes --- .../java/com/stuypulse/robot/RobotContainer.java | 15 +++++++++++++++ .../robot/commands/intake/IntakePivotIn.java | 11 +++++++++++ .../robot/commands/intake/IntakePivotOut.java | 10 ++++++++++ .../robot/commands/intake/IntakeStop.java | 2 +- .../com/stuypulse/robot/constants/Settings.java | 15 ++++++++------- .../stuypulse/robot/subsystems/intake/Intake.java | 8 ++++++-- .../robot/subsystems/intake/IntakeImpl.java | 5 +++++ 7 files changed, 56 insertions(+), 10 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakePivotIn.java create mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakePivotOut.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 8411c15c..0317a5ef 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -24,6 +24,8 @@ import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterStow; import com.stuypulse.robot.commands.intake.IntakeIntake; import com.stuypulse.robot.commands.intake.IntakeOutake; +import com.stuypulse.robot.commands.intake.IntakePivotIn; +import com.stuypulse.robot.commands.intake.IntakePivotOut; import com.stuypulse.robot.commands.intake.IntakeStop; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; @@ -115,6 +117,19 @@ private void configureDefaultCommands() { /***************/ private void configureButtonBindings() { + //INTAKING ROUTINE (OUT) + //TODO: change bindings, these are placeholders + // driver.getBottomButton() + // .onTrue(new IntakePivotOut() + // .alongWith(new WaitUntilCommand(() -> intake.pivotAtTolerance())) + // .andThen(new IntakeIntake())); + + //INTAKING ROUTINE (IN) - a bit redundant ngl + //TODO: button bindings!! + // driver.getBottomButton() + // .onTrue(new IntakeStop() + // .alongWith(new WaitUntilCommand(() -> intake.rollerStopped())) + // .andThen(new IntakePivotIn())); driver.getDPadDown() .onTrue(new TurretIdle()) diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakePivotIn.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakePivotIn.java new file mode 100644 index 00000000..b0a252bf --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakePivotIn.java @@ -0,0 +1,11 @@ +package com.stuypulse.robot.commands.intake; + +import com.stuypulse.robot.subsystems.intake.Intake.IntakeState; + +public class IntakePivotIn extends IntakeSetState{ + + public IntakePivotIn() { + super(IntakeState.PIVOTIN); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakePivotOut.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakePivotOut.java new file mode 100644 index 00000000..519d9014 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakePivotOut.java @@ -0,0 +1,10 @@ +package com.stuypulse.robot.commands.intake; + +import com.stuypulse.robot.subsystems.intake.Intake.IntakeState; + +public class IntakePivotOut extends IntakeSetState{ + public IntakePivotOut() { + super(IntakeState.PIVOTOUT); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeStop.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeStop.java index 472a2243..64da30a5 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeStop.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeStop.java @@ -12,6 +12,6 @@ public class IntakeStop extends IntakeSetState { * Sets the State of the Intake to Stowing */ public IntakeStop() { - super(IntakeState.STOW); + super(IntakeState.STOP); } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 7675dea4..3f10dd56 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -5,6 +5,8 @@ /***************************************************************/ package com.stuypulse.robot.constants; +import com.ctre.phoenix6.CANBus; +import com.pathplanner.lib.path.PathConstraints; import com.stuypulse.stuylib.network.SmartNumber; import edu.wpi.first.math.VecBuilder; @@ -15,9 +17,6 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; -import com.ctre.phoenix6.CANBus; -import com.pathplanner.lib.path.PathConstraints; - /*- * File containing constants and tunable settings for every subsystem on the robot. * @@ -37,16 +36,16 @@ public interface Handoff { double HANDOFF_MAX = 4800.0; double HANDOFF_REVERSE = -500.0; double RPM_TOLERANCE = 200.0; - public final SmartNumber HANDOFF_RPM = new SmartNumber("Handoff/Target RPM", HANDOFF_MAX); + SmartNumber HANDOFF_RPM = new SmartNumber("Handoff/Target RPM", HANDOFF_MAX); } public interface Intake { Rotation2d PIVOT_STOW_ANGLE = Rotation2d.fromDegrees(90.0); Rotation2d PIVOT_INTAKE_OUTAKE_ANGLE = Rotation2d.fromDegrees(0.0); - public final Rotation2d PIVOT_ANGLE_TOLERANCE = Rotation2d.fromDegrees(3.0); - public final double FORWARD_MAX_ROTATIONS = -30.0 / 360.0; - public final double BACKWARDS_MAX_ROTATIONS = 90.0 / 360.0; + Rotation2d PIVOT_ANGLE_TOLERANCE = Rotation2d.fromDegrees(3.0); + double FORWARD_MAX_ROTATIONS = -30.0 / 360.0; + double BACKWARDS_MAX_ROTATIONS = 90.0 / 360.0; Rotation2d PIVOT_ANGLE_OFFSET = new Rotation2d(); Rotation2d PIVOT_MAX_ANGLE = Rotation2d.fromDegrees(190); @@ -55,6 +54,8 @@ public interface Intake { Rotation2d PIVOT_MAX_VEL = Rotation2d.fromDegrees(300.0); Rotation2d PIVOT_MAX_ACCEL = Rotation2d.fromDegrees(300.0); + double ROLLER_RPS_TOLERANCE = .1; //TODO: find an appropriate tolerance for the roller + double GEAR_RATIO = 48.0; } diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java index 1798f11e..968b0811 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -27,13 +27,16 @@ public static Intake getInstance() { } public Intake() { - state = IntakeState.STOW; + state = IntakeState.STOP; } public enum IntakeState { + PIVOTOUT(Settings.Intake.PIVOT_INTAKE_OUTAKE_ANGLE, 0.0), + PIVOTIN(Settings.Intake.PIVOT_STOW_ANGLE, 0.0), + INTAKE(Settings.Intake.PIVOT_INTAKE_OUTAKE_ANGLE, 1.0), OUTAKE(Settings.Intake.PIVOT_INTAKE_OUTAKE_ANGLE, -1.0), - STOW(Settings.Intake.PIVOT_STOW_ANGLE, 0.0); + STOP(Settings.Intake.PIVOT_INTAKE_OUTAKE_ANGLE, 0.0); private double targetDutyCycle; private Rotation2d targetAngle; @@ -61,6 +64,7 @@ public void setState(IntakeState state) { } public abstract boolean pivotAtTolerance(); + public abstract boolean rollerStopped(); public abstract Rotation2d getPivotAngle(); public abstract SysIdRoutine getPivotSysIdRoutine(); public abstract void setPivotVoltageOverride(Optional voltage); diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index 43a9462c..d447b8d4 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -58,6 +58,11 @@ public boolean pivotAtTolerance() { < Settings.Intake.PIVOT_ANGLE_TOLERANCE.getRotations(); } + @Override + public boolean rollerStopped() { + return Math.abs(rollerLeader.getVelocity().getValueAsDouble()) <= Settings.Intake.ROLLER_RPS_TOLERANCE; + } + public Rotation2d getPivotAngle() { return Rotation2d.fromRotations(pivot.getPosition().getValueAsDouble()); } From 9492d8acf966498a4f13473e24867b42374133ce Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Tue, 24 Feb 2026 15:47:50 -0500 Subject: [PATCH 16/18] CLEAN: more cleaning up --- .../com/stuypulse/robot/RobotContainer.java | 15 +++-- .../com/stuypulse/robot/constants/Motors.java | 11 +++- .../stuypulse/robot/constants/Settings.java | 55 +++++++++---------- .../climberhopper/ClimberHopperImpl.java | 8 ++- .../robot/subsystems/handoff/HandoffImpl.java | 5 +- .../hoodedshooter/shooter/ShooterImpl.java | 5 +- .../robot/subsystems/intake/Intake.java | 1 - .../robot/subsystems/intake/IntakeImpl.java | 15 ++--- .../subsystems/spindexer/SpindexerImpl.java | 7 ++- .../robot/subsystems/turret/TurretImpl.java | 10 ++-- .../hoodedshooter/HoodAngleCalculator.java | 8 +-- 11 files changed, 76 insertions(+), 64 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 2b64b419..63b0be36 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -121,17 +121,16 @@ private void configureDefaultCommands() { private void configureButtonBindings() { //INTAKING ROUTINE (OUT) //TODO: change bindings, these are placeholders - // driver.getBottomButton() - // .onTrue(new IntakePivotOut() - // .alongWith(new WaitUntilCommand(() -> intake.pivotAtTolerance())) - // .andThen(new IntakeIntake())); + driver.getLeftTriggerButton() + .onTrue(new IntakePivotOut() + .alongWith(new WaitUntilCommand(() -> intake.pivotAtTolerance())) + .andThen(new IntakeIntake())); //INTAKING ROUTINE (IN) - a bit redundant ngl //TODO: button bindings!! - // driver.getBottomButton() - // .onTrue(new IntakeStop() - // .alongWith(new WaitUntilCommand(() -> intake.rollerStopped())) - // .andThen(new IntakePivotIn())); + driver.getRightTriggerButton() + .onTrue(new IntakeStop() + .andThen(new IntakePivotIn())); driver.getDPadUp() .onTrue(new SwerveResetHeading()); diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index ba4db748..61bf55e2 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -138,6 +138,15 @@ public interface Turret { .withFFConstants(Gains.Turret.kS, 0.0, 0.0, 0) .withSensorToMechanismRatio(Settings.Turret.Constants.GEAR_RATIO_MOTOR_TO_MECH); + public final Slot0Configs SLOT_0 = new Slot0Configs() + .withKP(Gains.Turret.kP) + .withKI(Gains.Turret.kI) + .withKD(Gains.Turret.kD) + .withKS(Gains.Turret.kS) + .withKV(Gains.Turret.kV) + .withKA(Gains.Turret.kA) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + public final CANCoderConfig ENCODER_17T = new CANCoderConfig() .withSensorDirection(SensorDirectionValue.Clockwise_Positive) .withAbsoluteSensorDiscontinuityPoint(1.0); @@ -146,7 +155,7 @@ public interface Turret { .withSensorDirection(SensorDirectionValue.Clockwise_Positive) .withAbsoluteSensorDiscontinuityPoint(1.0); - public final SoftwareLimitSwitchConfigs SOFT_LIMIT = new SoftwareLimitSwitchConfigs() + public final SoftwareLimitSwitchConfigs SOFT_LIMITS = new SoftwareLimitSwitchConfigs() .withForwardSoftLimitEnable(true) .withReverseSoftLimitEnable(true) .withForwardSoftLimitThreshold(Settings.Turret.Constants.SoftwareLimit.FORWARD_MAX_ROTATIONS) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index b9ce0873..0caa5baa 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -93,8 +93,6 @@ public interface Intake { Rotation2d PIVOT_MAX_VEL = Rotation2d.fromDegrees(300.0); Rotation2d PIVOT_MAX_ACCEL = Rotation2d.fromDegrees(300.0); - double ROLLER_RPS_TOLERANCE = .1; //TODO: find an appropriate tolerance for the roller - double GEAR_RATIO = 48.0; } @@ -114,6 +112,14 @@ public interface HoodedShooter { public final double SHOOTER_TOLERANCE_RPM = 50.0; public final double HOOD_TOLERANCE_DEG = 0.5; + public interface RPMs { + public final SmartNumber SHOOT_RPM = new SmartNumber("HoodedShooter/Shoot State Target RPM", 3400.0); + public final SmartNumber FERRY_RPM = new SmartNumber("HoodedShooter/Ferry State Target RPM", 2000.0); + public final double REVERSE = 0.0; + public final double HUB_RPM = 0.0; + public final double LEFT_CORNER_RPM = 0.0; + public final double RIGHT_CORNER_RPM = 0.0; + } public interface Angles { public final SmartNumber SHOOT_ANGLE = new SmartNumber("HoodedShooter/Shoot State Target Angle (deg)", 15.0); public final SmartNumber FERRY_ANGLE = new SmartNumber("HoodedShooter/Ferry State Target Angle (deg)", 20.0); @@ -137,12 +143,29 @@ public interface AngleInterpolation { }; } + public interface RPMInterpolation { + public final double[][] distanceRPMInterpolationValues = { + {1.30, 3000.0}, + {1.43, 3000.0}, + {2.15, 3050.0}, + {2.864967, 3215.271125}, + {3.65, 3400.0}, + {4.43, 3650.0}, + {5.32, 3950.0} + }; + } + public interface FerryRPMInterpolation { public final double[][] distanceRPMInterpolationValues = { {3.79, 3450.0} }; } + public interface Shooter { + public final double GEAR_RATIO = 1.0; + public final double FLYWHEEL_RADIUS = Units.inchesToMeters(3.965 / 2); + } + public interface Hood { /** * @@ -162,37 +185,11 @@ public interface Hood { public final Rotation2d ENCODER_OFFSET = Rotation2d.fromDegrees(0.0); } - public interface RPMInterpolation { - public final double[][] distanceRPMInterpolationValues = { - {1.30, 3000.0}, - {1.43, 3000.0}, - {2.15, 3050.0}, - {2.864967, 3215.271125}, - {3.65, 3400.0}, - {4.43, 3650.0}, - {5.32, 3950.0} - }; - } - - public interface RPMs { - public final SmartNumber SHOOT_RPM = new SmartNumber("HoodedShooter/Shoot State Target RPM", 3400.0); - public final SmartNumber FERRY_RPM = new SmartNumber("HoodedShooter/Ferry State Target RPM", 2000.0); - public final double REVERSE = 0.0; - public final double HUB_RPM = 0.0; - public final double LEFT_CORNER_RPM = 0.0; - public final double RIGHT_CORNER_RPM = 0.0; - public final double STOW = 0.0; - } - - public interface Shooter { - public final double GEAR_RATIO = 1.0; - public final double FLYWHEEL_RADIUS = Units.inchesToMeters(3.965 / 2); - } } public interface ShootOnTheFly { public final int MAX_ITERATIONS = 5; public final double TIME_TOLERANCE = 0.01; - public final SmartNumber UPDATE_DELAY = new SmartNumber("HoodedShooter/SOTM/update delay", 0.00); + public final SmartNumber UPDATE_DELAY = new SmartNumber("HoodedShooter/SOTM/Update Delay", 0.00); } public interface Swerve { diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java index 7ce9c6b6..1709888a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java @@ -21,6 +21,8 @@ public class ClimberHopperImpl extends ClimberHopper { private final TalonFX motor; + private final VoltageOut controller; + private final BStream stalling; private double voltage; @@ -37,6 +39,10 @@ public ClimberHopperImpl() { stalling = BStream.create(() -> motor.getStatorCurrent().getValueAsDouble() > Settings.ClimberHopper.STALL) .filtered(new BDebounce.Both(Settings.ClimberHopper.DEBOUNCE)); + // TODO: initialize voltage to default voltage and pass to controller initialization below + controller = new VoltageOut(0) + .withEnableFOC(true); + voltageOverride = Optional.empty(); } @@ -87,7 +93,7 @@ public void periodic() { } if (EnabledSubsystems.CLIMBER_HOPPER.get()) { - motor.setControl(new VoltageOut(voltage).withEnableFOC(true)); + motor.setControl(controller.withOutput(voltage)); } else { motor.stopMotor(); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java index d431d722..f650200f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -28,7 +28,8 @@ public HandoffImpl() { motor = new TalonFX(Ports.Handoff.HANDOFF); Motors.Handoff.HANDOFF.configure(motor); - controller = new VelocityVoltage(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE); + controller = new VelocityVoltage(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE) + .withEnableFOC(true); voltageOverride = Optional.empty(); } @@ -51,7 +52,7 @@ public void periodic() { } else if (voltageOverride.isPresent()) { motor.setVoltage(voltageOverride.get()); } else { - motor.setControl(controller.withVelocity(getTargetRPM() / 60.0).withEnableFOC(true)); + motor.setControl(controller.withVelocity(getTargetRPM() / 60.0)); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java index 0eb103fa..b91b5400 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java @@ -33,7 +33,8 @@ public ShooterImpl() { shooterLeader = new TalonFX(Ports.HoodedShooter.Shooter.MOTOR_LEAD); shooterFollower = new TalonFX(Ports.HoodedShooter.Shooter.MOTOR_FOLLOW); - shooterController = new VelocityVoltage(getTargetRPM() / 60.0); + shooterController = new VelocityVoltage(getTargetRPM() / 60.0) + .withEnableFOC(true); follower = new Follower(Ports.HoodedShooter.Shooter.MOTOR_LEAD, MotorAlignmentValue.Opposed); Motors.HoodedShooter.Shooter.SHOOTER.configure(shooterLeader); @@ -69,7 +70,7 @@ public void periodic() { shooterLeader.setVoltage(voltageOverride.get()); shooterFollower.setControl(follower); } else { - shooterLeader.setControl(shooterController.withVelocity(getTargetRPM() / 60.0).withEnableFOC(true)); + shooterLeader.setControl(shooterController.withVelocity(getTargetRPM() / 60.0)); shooterFollower.setControl(follower); } } else { diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java index 968b0811..54455b3b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -64,7 +64,6 @@ public void setState(IntakeState state) { } public abstract boolean pivotAtTolerance(); - public abstract boolean rollerStopped(); public abstract Rotation2d getPivotAngle(); public abstract SysIdRoutine getPivotSysIdRoutine(); public abstract void setPivotVoltageOverride(Optional voltage); diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index d447b8d4..bcd0bf85 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -44,8 +44,10 @@ public IntakeImpl() { rollerFollower = new TalonFX(Ports.Intake.ROLLER_FOLLOWER); Motors.Intake.ROLLER.configure(rollerFollower); - pivotController = new MotionMagicVoltage(getState().getTargetAngle().getRotations()); - rollerController = new DutyCycleOut(getState().getTargetDutyCycle()); + pivotController = new MotionMagicVoltage(getState().getTargetAngle().getRotations()) + .withEnableFOC(true); + rollerController = new DutyCycleOut(getState().getTargetDutyCycle()) + .withEnableFOC(true); follower = new Follower(Ports.Intake.ROLLER_LEADER, MotorAlignmentValue.Opposed); pivotVoltageOverride = Optional.empty(); @@ -58,11 +60,6 @@ public boolean pivotAtTolerance() { < Settings.Intake.PIVOT_ANGLE_TOLERANCE.getRotations(); } - @Override - public boolean rollerStopped() { - return Math.abs(rollerLeader.getVelocity().getValueAsDouble()) <= Settings.Intake.ROLLER_RPS_TOLERANCE; - } - public Rotation2d getPivotAngle() { return Rotation2d.fromRotations(pivot.getPosition().getValueAsDouble()); } @@ -73,8 +70,8 @@ public void periodic() { if (pivotVoltageOverride.isPresent()) { pivot.setVoltage(pivotVoltageOverride.get()); } else { - pivot.setControl(pivotController.withPosition(getState().getTargetAngle().getRotations()).withEnableFOC(true)); - rollerLeader.setControl(rollerController.withOutput(getState().getTargetDutyCycle()).withEnableFOC(true)); + pivot.setControl(pivotController.withPosition(getState().getTargetAngle().getRotations())); + rollerLeader.setControl(rollerController.withOutput(getState().getTargetDutyCycle())); rollerFollower.setControl(follower); } } else { diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java index af7e2a82..1b098441 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -36,9 +36,10 @@ public SpindexerImpl() { Motors.Spindexer.SPINDEXER.configure(leadMotor); Motors.Spindexer.SPINDEXER.configure(followerMotor); - follower = new Follower(Ports.Spindexer.SPINDEXER_LEAD_MOTOR, MotorAlignmentValue.Opposed); + controller = new VelocityVoltage(getTargetRPM()) + .withEnableFOC(true); - controller = new VelocityVoltage(getTargetRPM()); + follower = new Follower(Ports.Spindexer.SPINDEXER_LEAD_MOTOR, MotorAlignmentValue.Opposed); voltageOverride = Optional.empty(); } @@ -63,7 +64,7 @@ public void periodic() { leadMotor.setVoltage(voltageOverride.get()); followerMotor.setControl(follower); } else { - leadMotor.setControl(controller.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE).withEnableFOC(true)); + leadMotor.setControl(controller.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE)); followerMotor.setControl(follower); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java index 9b0b7cf3..18be43db 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java @@ -38,14 +38,16 @@ public TurretImpl() { encoder18t = new CANcoder(Ports.Turret.ENCODER18T, Ports.bus); Motors.Turret.TURRET.configure(motor); - Motors.Turret.ENCODER_17T.configure(encoder17t); - Motors.Turret.ENCODER_18T.configure(encoder18t); + + motor.getConfigurator().apply(Motors.Turret.SLOT_0); + motor.getConfigurator().apply(Motors.Turret.SOFT_LIMITS); seedTurret(); hasUsedAbsoluteEncoder = false; voltageOverride = Optional.empty(); - controller = new PositionVoltage(getTargetAngle().getRotations()); + controller = new PositionVoltage(getTargetAngle().getRotations()) + .withEnableFOC(true); } private Rotation2d getEncoderPos17t() { @@ -125,7 +127,7 @@ public void periodic() { if (voltageOverride.isPresent()) { motor.setVoltage(voltageOverride.get()); } else { - motor.setControl(controller.withPosition(actualTargetDeg / 360.0).withEnableFOC(true)); + motor.setControl(controller.withPosition(actualTargetDeg / 360.0)); } } else { motor.stopMotor(); diff --git a/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java b/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java index 0d4abca6..551f1ac7 100644 --- a/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java @@ -144,10 +144,10 @@ public static void updateSOTMSolution() { futureTurretPose2d.setPose((Robot.isBlue() ? futureTurretPose : Field.transformToOppositeAlliance(futureTurretPose))); - SmartDashboard.putNumber("HoodedShooter/SOTM/calculated turret angle", sol.targetTurretAngle().getDegrees()); - SmartDashboard.putNumber("HoodedShooter/SOTM/calculated hood angle", sol.targetHoodAngle().getDegrees()); - SmartDashboard.putNumber("HoodedShooter/SOTM/calculated flight time", sol.flightTime()); - SmartDashboard.putNumber("HoodedShooter/SOTM/turret dist to virtual pose", futureTurretPose.getTranslation().getDistance(sol.virtualPose().getTranslation())); + SmartDashboard.putNumber("HoodedShooter/SOTM/Calculated Turret Angle", sol.targetTurretAngle().getDegrees()); + SmartDashboard.putNumber("HoodedShooter/SOTM/Calculated Hood Angle", sol.targetHoodAngle().getDegrees()); + SmartDashboard.putNumber("HoodedShooter/SOTM/Calculated Flight time", sol.flightTime()); + SmartDashboard.putNumber("HoodedShooter/SOTM/Turret Dist to Virtual Pose", futureTurretPose.getTranslation().getDistance(sol.virtualPose().getTranslation())); } public static Supplier calculateHoodAngleSOTM() { From faba05a0f48ec83009439baaeede0dbfeae5db96 Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Tue, 24 Feb 2026 16:13:59 -0500 Subject: [PATCH 17/18] FIX: fix intake subsystem structure and IntakeSetState command --- .../com/stuypulse/robot/RobotContainer.java | 128 +++++++----------- .../{IntakeIntake.java => IntakeDeploy.java} | 12 +- .../robot/commands/intake/IntakePivotIn.java | 11 -- .../robot/commands/intake/IntakePivotOut.java | 10 -- .../robot/commands/intake/IntakeSetState.java | 39 +++++- ...IntakeStop.java => IntakeStopRollers.java} | 13 +- .../{IntakeOutake.java => IntakeStow.java} | 12 +- .../robot/subsystems/intake/Intake.java | 72 ++++++---- .../robot/subsystems/intake/IntakeImpl.java | 38 +++--- 9 files changed, 159 insertions(+), 176 deletions(-) rename src/main/java/com/stuypulse/robot/commands/intake/{IntakeIntake.java => IntakeDeploy.java} (59%) delete mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakePivotIn.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakePivotOut.java rename src/main/java/com/stuypulse/robot/commands/intake/{IntakeStop.java => IntakeStopRollers.java} (62%) rename src/main/java/com/stuypulse/robot/commands/intake/{IntakeOutake.java => IntakeStow.java} (60%) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 63b0be36..6f2128b1 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -24,11 +24,9 @@ import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterRightCorner; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterShoot; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterStow; -import com.stuypulse.robot.commands.intake.IntakeIntake; -import com.stuypulse.robot.commands.intake.IntakeOutake; -import com.stuypulse.robot.commands.intake.IntakePivotIn; -import com.stuypulse.robot.commands.intake.IntakePivotOut; -import com.stuypulse.robot.commands.intake.IntakeStop; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.intake.IntakeStopRollers; +import com.stuypulse.robot.commands.intake.IntakeStow; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.swerve.SwerveClimbAlign; @@ -119,30 +117,22 @@ private void configureDefaultCommands() { /***************/ private void configureButtonBindings() { - //INTAKING ROUTINE (OUT) - //TODO: change bindings, these are placeholders + // Intake Up and Off driver.getLeftTriggerButton() - .onTrue(new IntakePivotOut() - .alongWith(new WaitUntilCommand(() -> intake.pivotAtTolerance())) - .andThen(new IntakeIntake())); + .onTrue(new IntakeStow()); - //INTAKING ROUTINE (IN) - a bit redundant ngl - //TODO: button bindings!! + // Intake Down and On driver.getRightTriggerButton() - .onTrue(new IntakeStop() - .andThen(new IntakePivotIn())); + .onTrue(new IntakeDeploy()); + // Reset Heading driver.getDPadUp() .onTrue(new SwerveResetHeading()); - - driver.getDPadDown() - .onTrue(new TurretIdle()) - .onTrue(new TurretSeed()); - // SCORING ROUTINE + // Scoring Routine using Interpolation Settings driver.getTopButton() - .whileTrue(new TurretShoot() - .alongWith(new HoodedShooterInterpolation()) + .whileTrue(new HoodedShooterInterpolation() + .alongWith(new TurretShoot()) .alongWith(new WaitUntilCommand(() -> hoodedShooter.bothAtTolerance())) .andThen(new HandoffRun().onlyIf(() -> hoodedShooter.bothAtTolerance()) .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) @@ -151,6 +141,7 @@ private void configureButtonBindings() { .alongWith(new HoodedShooterStow()) .alongWith(new HandoffStop())); + // Ferry Routine using Interpolation Settings driver.getBottomButton() .onTrue(new HoodedShooterFerry() .alongWith(new TurretFerry()) @@ -163,25 +154,12 @@ private void configureButtonBindings() { .alongWith(new HoodedShooterStow()) .alongWith(new HandoffStop())); - // driver.getDPadDown() - // .onTrue(new HoodedShooterShoot()) - // .onFalse(new HoodedShooterStow()); - - // driver.getDPadUp() - // .onTrue(new HoodedShooterFerry()) - // .onFalse(new HoodedShooterStow()); - - // driver.getDPadUp().whileTrue(new HoodedShooterShoot() - // .alongWith(new WaitUntilCommand(() -> hoodedShooter.isShooterAtTolerance()) - // .andThen(new FeederFeed()))) - // .onFalse(new HoodedShooterStow() - // .alongWith(new FeederStop())); - //-------------------------------------------------------------------------------------------------------------------------\\ //-------------------------------------------------------------------------------------------------------------------------\\ //-------------------------------------------------------------------------------------------------------------------------\\ //-------------------------------------------------------------------------------------------------------------------------\\ //-------------------------------------------------------------------------------------------------------------------------\\ + /* // Climb Align driver.getTopButton() .whileTrue(new SwerveClimbAlign().alongWith(new ClimberUp())); @@ -234,13 +212,13 @@ private void configureButtonBindings() { new HandoffStop())) ); - // Intake On + // Intake Up and Off driver.getLeftTriggerButton() - .onTrue(new IntakeIntake()); + .onTrue(new IntakeStow()); - // Intake Off + // Intake Down and On driver.getRightTriggerButton() - .onTrue(new IntakeStop()); + .onTrue(new IntakeDeploy()); // Climb Down Placeholder driver.getLeftBumper() @@ -286,19 +264,7 @@ private void configureButtonBindings() { new SpindexerRun().alongWith( new HandoffStop())) ); - - // Unjam - driver.getDPadDown() - .whileTrue( - new HoodedShooterReverse().alongWith( - new HandoffReverse().alongWith( - new IntakeOutake()))) - .onFalse( - new HoodedShooterStow().alongWith( - new SpindexerRun().alongWith( - new HandoffStop().alongWith( - new IntakeStop()))) - ); + */ } /**************/ @@ -323,35 +289,35 @@ public void configureSysids() { // autonChooser.addOption("SysID Module Rotation Quasi Forwards", swerve.sysIdRotQuasi(Direction.kForward)); // autonChooser.addOption("SysID Module Rotation Quasi Backwards", swerve.sysIdRotQuasi(Direction.kReverse)); - SysIdRoutine shooterSysId = shooter.getShooterSysIdRoutine(); - autonChooser.addOption("SysID Shooter Dynamic Forward", shooterSysId.dynamic(Direction.kForward)); - autonChooser.addOption("SysID Shooter Dynamic Backwards", shooterSysId.dynamic(Direction.kReverse)); - autonChooser.addOption("SysID Shooter Quasi Forwards", shooterSysId.quasistatic(Direction.kForward)); - autonChooser.addOption("SysID Shooter Quasi Backwards", shooterSysId.quasistatic(Direction.kReverse)); - - SysIdRoutine hoodSysId = hood.getHoodSysIdRoutine(); - autonChooser.addOption("SysID Hood Dynamic Forward", hoodSysId.dynamic(Direction.kForward)); - autonChooser.addOption("SysID Hood Dynamic Backwards", hoodSysId.dynamic(Direction.kReverse)); - autonChooser.addOption("SysID Hood Quasi Forwards", hoodSysId.quasistatic(Direction.kForward)); - autonChooser.addOption("SysID Hood Quasi Backwards", hoodSysId.quasistatic(Direction.kReverse)); - - SysIdRoutine intakePivotSysId = intake.getPivotSysIdRoutine(); - autonChooser.addOption("SysID Intake Pivot Dynamic Forward", intakePivotSysId.dynamic(Direction.kForward)); - autonChooser.addOption("SysID Intake Pivot Dynamic Backwards", intakePivotSysId.dynamic(Direction.kReverse)); - autonChooser.addOption("SysID Intake Pivot Quasi Forwards", intakePivotSysId.quasistatic(Direction.kForward)); - autonChooser.addOption("SysID Intake Pivot Quasi Backwards", intakePivotSysId.quasistatic(Direction.kReverse)); - - SysIdRoutine spindexerSysId = spindexer.getSysIdRoutine(); - autonChooser.addOption("SysID Spindexer Dynamic Forward", spindexerSysId.dynamic(Direction.kForward)); - autonChooser.addOption("SysID Spindexer Dynamic Backwards", spindexerSysId.dynamic(Direction.kReverse)); - autonChooser.addOption("SysID Spindexer Quasi Forwards", spindexerSysId.quasistatic(Direction.kForward)); - autonChooser.addOption("SysID Spindexer Quasi Backwards", spindexerSysId.quasistatic(Direction.kReverse)); - - SysIdRoutine handoffSysId = handoff.getSysIdRoutine(); - autonChooser.addOption("SysID Handoff Forward", handoffSysId.dynamic(Direction.kForward)); - autonChooser.addOption("SysID Handoff Backwards", handoffSysId.dynamic(Direction.kReverse)); - autonChooser.addOption("SysID Handoff Forwards", handoffSysId.quasistatic(Direction.kForward)); - autonChooser.addOption("SysID Handoff Backwards", handoffSysId.quasistatic(Direction.kReverse)); + // SysIdRoutine shooterSysId = shooter.getShooterSysIdRoutine(); + // autonChooser.addOption("SysID Shooter Dynamic Forward", shooterSysId.dynamic(Direction.kForward)); + // autonChooser.addOption("SysID Shooter Dynamic Backwards", shooterSysId.dynamic(Direction.kReverse)); + // autonChooser.addOption("SysID Shooter Quasi Forwards", shooterSysId.quasistatic(Direction.kForward)); + // autonChooser.addOption("SysID Shooter Quasi Backwards", shooterSysId.quasistatic(Direction.kReverse)); + + // SysIdRoutine hoodSysId = hood.getHoodSysIdRoutine(); + // autonChooser.addOption("SysID Hood Dynamic Forward", hoodSysId.dynamic(Direction.kForward)); + // autonChooser.addOption("SysID Hood Dynamic Backwards", hoodSysId.dynamic(Direction.kReverse)); + // autonChooser.addOption("SysID Hood Quasi Forwards", hoodSysId.quasistatic(Direction.kForward)); + // autonChooser.addOption("SysID Hood Quasi Backwards", hoodSysId.quasistatic(Direction.kReverse)); + + // SysIdRoutine intakePivotSysId = intake.getPivotSysIdRoutine(); + // autonChooser.addOption("SysID Intake Pivot Dynamic Forward", intakePivotSysId.dynamic(Direction.kForward)); + // autonChooser.addOption("SysID Intake Pivot Dynamic Backwards", intakePivotSysId.dynamic(Direction.kReverse)); + // autonChooser.addOption("SysID Intake Pivot Quasi Forwards", intakePivotSysId.quasistatic(Direction.kForward)); + // autonChooser.addOption("SysID Intake Pivot Quasi Backwards", intakePivotSysId.quasistatic(Direction.kReverse)); + + // SysIdRoutine spindexerSysId = spindexer.getSysIdRoutine(); + // autonChooser.addOption("SysID Spindexer Dynamic Forward", spindexerSysId.dynamic(Direction.kForward)); + // autonChooser.addOption("SysID Spindexer Dynamic Backwards", spindexerSysId.dynamic(Direction.kReverse)); + // autonChooser.addOption("SysID Spindexer Quasi Forwards", spindexerSysId.quasistatic(Direction.kForward)); + // autonChooser.addOption("SysID Spindexer Quasi Backwards", spindexerSysId.quasistatic(Direction.kReverse)); + + // SysIdRoutine handoffSysId = handoff.getSysIdRoutine(); + // autonChooser.addOption("SysID Handoff Forward", handoffSysId.dynamic(Direction.kForward)); + // autonChooser.addOption("SysID Handoff Backwards", handoffSysId.dynamic(Direction.kReverse)); + // autonChooser.addOption("SysID Handoff Forwards", handoffSysId.quasistatic(Direction.kForward)); + // autonChooser.addOption("SysID Handoff Backwards", handoffSysId.quasistatic(Direction.kReverse)); } diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeIntake.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeploy.java similarity index 59% rename from src/main/java/com/stuypulse/robot/commands/intake/IntakeIntake.java rename to src/main/java/com/stuypulse/robot/commands/intake/IntakeDeploy.java index 2e89be86..de0fac60 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeIntake.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeploy.java @@ -5,13 +5,11 @@ /***************************************************************/ package com.stuypulse.robot.commands.intake; -import com.stuypulse.robot.subsystems.intake.Intake.IntakeState; +import com.stuypulse.robot.subsystems.intake.Intake.PivotState; +import com.stuypulse.robot.subsystems.intake.Intake.RollerState; -public class IntakeIntake extends IntakeSetState { - /** - * Sets the State of the Intake to Intaking - */ - public IntakeIntake() { - super(IntakeState.INTAKE); +public class IntakeDeploy extends IntakeSetState { + public IntakeDeploy() { + super(PivotState.DEPLOYED, RollerState.INTAKE); } } diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakePivotIn.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakePivotIn.java deleted file mode 100644 index b0a252bf..00000000 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakePivotIn.java +++ /dev/null @@ -1,11 +0,0 @@ -package com.stuypulse.robot.commands.intake; - -import com.stuypulse.robot.subsystems.intake.Intake.IntakeState; - -public class IntakePivotIn extends IntakeSetState{ - - public IntakePivotIn() { - super(IntakeState.PIVOTIN); - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakePivotOut.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakePivotOut.java deleted file mode 100644 index 519d9014..00000000 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakePivotOut.java +++ /dev/null @@ -1,10 +0,0 @@ -package com.stuypulse.robot.commands.intake; - -import com.stuypulse.robot.subsystems.intake.Intake.IntakeState; - -public class IntakePivotOut extends IntakeSetState{ - public IntakePivotOut() { - super(IntakeState.PIVOTOUT); - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeSetState.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeSetState.java index f26bff52..cde76db4 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeSetState.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeSetState.java @@ -6,23 +6,48 @@ package com.stuypulse.robot.commands.intake; import com.stuypulse.robot.subsystems.intake.Intake; -import com.stuypulse.robot.subsystems.intake.Intake.IntakeState; +import com.stuypulse.robot.subsystems.intake.Intake.PivotState; +import com.stuypulse.robot.subsystems.intake.Intake.RollerState; import edu.wpi.first.wpilibj2.command.InstantCommand; +import java.util.Optional; + public class IntakeSetState extends InstantCommand { - private Intake intake; - private IntakeState intakeState; - public IntakeSetState(IntakeState state) { + private final Intake intake; + private final Optional pivotState; + private final Optional rollerState; + + public IntakeSetState(RollerState rollerState) { + intake = Intake.getInstance(); + this.pivotState = Optional.empty(); + this.rollerState = Optional.of(rollerState); + addRequirements(intake); + } + + public IntakeSetState(PivotState pivotState) { intake = Intake.getInstance(); - this.intakeState = state; + this.pivotState = Optional.of(pivotState); + this.rollerState = Optional.empty(); + addRequirements(intake); + } + public IntakeSetState(PivotState pivotState, RollerState rollerState) { + intake = Intake.getInstance(); + this.pivotState = Optional.of(pivotState); + this.rollerState = Optional.of(rollerState); addRequirements(intake); } @Override public void initialize() { - intake.setState(intakeState); + if (pivotState.isPresent()) { + intake.setPivotState(pivotState.get()); + } + + if (rollerState.isPresent()) { + intake.setRollerState(rollerState.get()); + } } -} +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeStop.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeStopRollers.java similarity index 62% rename from src/main/java/com/stuypulse/robot/commands/intake/IntakeStop.java rename to src/main/java/com/stuypulse/robot/commands/intake/IntakeStopRollers.java index 64da30a5..319dfe3f 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeStop.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeStopRollers.java @@ -5,13 +5,10 @@ /***************************************************************/ package com.stuypulse.robot.commands.intake; -import com.stuypulse.robot.subsystems.intake.Intake.IntakeState; +import com.stuypulse.robot.subsystems.intake.Intake.RollerState; -public class IntakeStop extends IntakeSetState { - /** - * Sets the State of the Intake to Stowing - */ - public IntakeStop() { - super(IntakeState.STOP); +public class IntakeStopRollers extends IntakeSetState { + public IntakeStopRollers() { + super(RollerState.STOP); } -} +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeOutake.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeStow.java similarity index 60% rename from src/main/java/com/stuypulse/robot/commands/intake/IntakeOutake.java rename to src/main/java/com/stuypulse/robot/commands/intake/IntakeStow.java index 5b9e88fb..56f2cb0c 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeOutake.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeStow.java @@ -5,13 +5,11 @@ /***************************************************************/ package com.stuypulse.robot.commands.intake; -import com.stuypulse.robot.subsystems.intake.Intake.IntakeState; +import com.stuypulse.robot.subsystems.intake.Intake.PivotState; +import com.stuypulse.robot.subsystems.intake.Intake.RollerState; -public class IntakeOutake extends IntakeSetState { - /** - * Sets the State of the Intake to Outaking - */ - public IntakeOutake() { - super(IntakeState.OUTAKE); +public class IntakeStow extends IntakeSetState { + public IntakeStow() { + super(PivotState.STOWED, RollerState.STOP); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java index 54455b3b..bcde7954 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -16,7 +16,6 @@ public abstract class Intake extends SubsystemBase { private static final Intake instance; - private IntakeState state; static { instance = new IntakeImpl(); @@ -26,59 +25,78 @@ public static Intake getInstance() { return instance; } - public Intake() { - state = IntakeState.STOP; - } - - public enum IntakeState { - PIVOTOUT(Settings.Intake.PIVOT_INTAKE_OUTAKE_ANGLE, 0.0), - PIVOTIN(Settings.Intake.PIVOT_STOW_ANGLE, 0.0), - - INTAKE(Settings.Intake.PIVOT_INTAKE_OUTAKE_ANGLE, 1.0), - OUTAKE(Settings.Intake.PIVOT_INTAKE_OUTAKE_ANGLE, -1.0), - STOP(Settings.Intake.PIVOT_INTAKE_OUTAKE_ANGLE, 0.0); + public enum PivotState { + DEPLOYED(Settings.Intake.PIVOT_INTAKE_OUTAKE_ANGLE), + STOWED(Settings.Intake.PIVOT_STOW_ANGLE); - private double targetDutyCycle; - private Rotation2d targetAngle; + private final Rotation2d targetAngle; - private IntakeState(Rotation2d targetAngle, double targetDutyCycle) { + private PivotState(Rotation2d targetAngle) { this.targetAngle = targetAngle; - this.targetDutyCycle = targetDutyCycle; } public Rotation2d getTargetAngle() { return targetAngle; } + } + + public enum RollerState { + INTAKE(1.0), + OUTTAKE(-1.0), + STOP(0.0); + + private final double targetDutyCycle; + + private RollerState(double targetDutyCycle) { + this.targetDutyCycle = targetDutyCycle; + } public double getTargetDutyCycle() { return targetDutyCycle; } } - public IntakeState getState() { - return state; + private PivotState pivotState; + private RollerState rollerState; + + protected Intake() { + this.pivotState = PivotState.STOWED; + this.rollerState = RollerState.STOP; } - public void setState(IntakeState state) { - this.state = state; + public PivotState getPivotState() { + return pivotState; + } + + public void setPivotState(PivotState state) { + this.pivotState = state; + setPivotVoltageOverride(Optional.empty()); + } + + public RollerState getRollerState() { + return rollerState; + } + + public void setRollerState(RollerState state) { + this.rollerState = state; } public abstract boolean pivotAtTolerance(); public abstract Rotation2d getPivotAngle(); - public abstract SysIdRoutine getPivotSysIdRoutine(); public abstract void setPivotVoltageOverride(Optional voltage); - + public abstract SysIdRoutine getPivotSysIdRoutine(); + @Override public void periodic() { - SmartDashboard.putString("Intake/State", getState().toString()); - SmartDashboard.putString("Intake/State", getState().toString()); + SmartDashboard.putString("Intake/Pivot State", getPivotState().toString()); + SmartDashboard.putString("Intake/Roller State", getRollerState().toString()); SmartDashboard.putNumber("Intake/Current Angle (deg)", getPivotAngle().getDegrees()); - SmartDashboard.putNumber("Intake/Target Angle (deg)", getState().getTargetAngle().getDegrees()); + SmartDashboard.putNumber("Intake/Target Angle (deg)", getPivotState().getTargetAngle().getDegrees()); - SmartDashboard.putNumber("Intake/Target Duty Cycle", getState().getTargetDutyCycle()); + SmartDashboard.putNumber("Intake/Target Duty Cycle", getRollerState().getTargetDutyCycle()); SmartDashboard.putBoolean("Intake/Pivot At Tolerance?", pivotAtTolerance()); } - + } diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index bcd0bf85..abaec807 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -27,9 +27,8 @@ public class IntakeImpl extends Intake { private final TalonFX rollerLeader; private final TalonFX rollerFollower; - private final DutyCycleOut rollerController; private final MotionMagicVoltage pivotController; - + private final DutyCycleOut rollerController; private final Follower follower; private Optional pivotVoltageOverride; @@ -44,9 +43,9 @@ public IntakeImpl() { rollerFollower = new TalonFX(Ports.Intake.ROLLER_FOLLOWER); Motors.Intake.ROLLER.configure(rollerFollower); - pivotController = new MotionMagicVoltage(getState().getTargetAngle().getRotations()) + pivotController = new MotionMagicVoltage(getPivotState().getTargetAngle().getRotations()) .withEnableFOC(true); - rollerController = new DutyCycleOut(getState().getTargetDutyCycle()) + rollerController = new DutyCycleOut(getRollerState().getTargetDutyCycle()) .withEnableFOC(true); follower = new Follower(Ports.Intake.ROLLER_LEADER, MotorAlignmentValue.Opposed); @@ -56,22 +55,25 @@ public IntakeImpl() { @Override public boolean pivotAtTolerance() { return Math.abs( - (getPivotAngle().getRotations()) - getState().getTargetAngle().getRotations()) - < Settings.Intake.PIVOT_ANGLE_TOLERANCE.getRotations(); + getPivotAngle().getRotations() - getPivotState().getTargetAngle().getRotations()) + < Settings.Intake.PIVOT_ANGLE_TOLERANCE.getRotations(); } + @Override public Rotation2d getPivotAngle() { return Rotation2d.fromRotations(pivot.getPosition().getValueAsDouble()); } - + @Override public void periodic() { + super.periodic(); + if (EnabledSubsystems.INTAKE.get()) { if (pivotVoltageOverride.isPresent()) { pivot.setVoltage(pivotVoltageOverride.get()); } else { - pivot.setControl(pivotController.withPosition(getState().getTargetAngle().getRotations())); - rollerLeader.setControl(rollerController.withOutput(getState().getTargetDutyCycle())); + pivot.setControl(pivotController.withPosition(getPivotState().getTargetAngle().getRotations())); + rollerLeader.setControl(rollerController.withOutput(getRollerState().getTargetDutyCycle())); rollerFollower.setControl(follower); } } else { @@ -81,7 +83,7 @@ public void periodic() { } SmartDashboard.putNumber("Intake/Pivot Angle Error (deg)", - Math.abs(getState().getTargetAngle().getDegrees() - getPivotAngle().getDegrees())); + Math.abs(getPivotState().getTargetAngle().getDegrees() - getPivotAngle().getDegrees())); if (Settings.DEBUG_MODE) { // PIVOT @@ -105,13 +107,13 @@ public void setPivotVoltageOverride(Optional voltage) { @Override public SysIdRoutine getPivotSysIdRoutine() { return SysId.getRoutine( - 2, - 6, - "Intake Pivot", - voltage -> setPivotVoltageOverride(Optional.of(voltage)), - () -> getPivotAngle().getRotations(), - () -> pivot.getVelocity().getValueAsDouble(), - () -> pivot.getMotorVoltage().getValueAsDouble(), - getInstance()); + 2, + 6, + "Intake Pivot", + voltage -> setPivotVoltageOverride(Optional.of(voltage)), + () -> getPivotAngle().getRotations(), + () -> pivot.getVelocity().getValueAsDouble(), + () -> pivot.getMotorVoltage().getValueAsDouble(), + getInstance()); } } From 1b4d0e443876d7cdbb4f097635d0a0578641d113 Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Tue, 24 Feb 2026 16:26:37 -0500 Subject: [PATCH 18/18] FIX: update ports excluding swerve --- .../com/stuypulse/robot/RobotContainer.java | 1 - .../com/stuypulse/robot/constants/Ports.java | 34 +++++++++---------- .../climberhopper/ClimberHopperImpl.java | 2 +- .../robot/subsystems/handoff/HandoffImpl.java | 2 +- .../hoodedshooter/hood/HoodImpl.java | 4 +-- .../hoodedshooter/shooter/ShooterImpl.java | 4 +-- .../robot/subsystems/intake/IntakeImpl.java | 6 ++-- .../subsystems/spindexer/SpindexerImpl.java | 4 +-- .../robot/subsystems/turret/TurretImpl.java | 6 ++-- 9 files changed, 30 insertions(+), 33 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 6f2128b1..b7124041 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -75,7 +75,6 @@ public interface EnabledSubsystems { // Gamepads public final Gamepad driver = new AutoGamepad(Ports.Gamepad.DRIVER); - public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR); // Subsystem private final ClimberHopper climberHopper = ClimberHopper.getInstance(); diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 924ec08a..0d8e164a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -10,49 +10,47 @@ /** This file contains the different ports of motors, solenoids and sensors */ public interface Ports { // TODO: Get bus name - public CANBus bus = new CANBus("rio"); + public CANBus RIO = new CANBus("rio"); + public CANBus CANIVORE = new CANBus("CANIVORE"); public interface Gamepad { int DRIVER = 0; - int OPERATOR = 1; - int DEBUGGER = 2; } public interface ClimberHopper { - int CLIMBER_HOPPER = 3; + int CLIMBER_HOPPER = 60; } public interface Handoff { - int HANDOFF = 4; + int HANDOFF = 43; } public interface HoodedShooter { - // all these ports are copied from alphabot public interface Hood { - int MOTOR = 25; - int THROUGHBORE_ENCODER = 37; + int MOTOR = 45; + int THROUGHBORE_ENCODER = 44; } public interface Shooter { - int MOTOR_LEAD = 17; - int MOTOR_FOLLOW = 14; + int MOTOR_LEAD = 47; + int MOTOR_FOLLOW = 46; } } public interface Intake { - int PIVOT = 8; - int ROLLER_LEADER = 9; - int ROLLER_FOLLOWER = 10; + int PIVOT = 20; + int ROLLER_LEADER = 21; + int ROLLER_FOLLOWER = 22; } public interface Spindexer { - int SPINDEXER_LEAD_MOTOR = 11; - int SPINDEXER_FOLLOW_MOTOR = 12; + int SPINDEXER_LEAD_MOTOR = 30; + int SPINDEXER_FOLLOW_MOTOR = 31; } public interface Turret { - int MOTOR = 50; - int ENCODER17T = 38; - int ENCODER18T = 21; + int MOTOR = 40; + int ENCODER17T = 42; + int ENCODER18T = 41; } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java index 1709888a..fed7859b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java @@ -31,7 +31,7 @@ public class ClimberHopperImpl extends ClimberHopper { public ClimberHopperImpl() { super(); - motor = new TalonFX(Ports.ClimberHopper.CLIMBER_HOPPER); + motor = new TalonFX(Ports.ClimberHopper.CLIMBER_HOPPER, Ports.CANIVORE); Motors.ClimberHopper.MOTOR.configure(motor); motor.getConfigurator().apply(Motors.ClimberHopper.SOFT_LIMITS); diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java index f650200f..2bb053ad 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -25,7 +25,7 @@ public class HandoffImpl extends Handoff { private Optional voltageOverride; public HandoffImpl() { - motor = new TalonFX(Ports.Handoff.HANDOFF); + motor = new TalonFX(Ports.Handoff.HANDOFF, Ports.RIO); Motors.Handoff.HANDOFF.configure(motor); controller = new VelocityVoltage(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE) diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java index 6465a053..6b8c814e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java @@ -31,8 +31,8 @@ public class HoodImpl extends Hood { private boolean hasUsedAbsoluteEncoder; public HoodImpl() { - hoodMotor = new TalonFX(Ports.HoodedShooter.Hood.MOTOR); - hoodEncoder = new CANcoder(Ports.HoodedShooter.Hood.THROUGHBORE_ENCODER); + hoodMotor = new TalonFX(Ports.HoodedShooter.Hood.MOTOR, Ports.RIO); + hoodEncoder = new CANcoder(Ports.HoodedShooter.Hood.THROUGHBORE_ENCODER, Ports.RIO); Motors.HoodedShooter.Hood.HOOD.configure(hoodMotor); diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java index b91b5400..2a88eff0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java @@ -30,8 +30,8 @@ public class ShooterImpl extends Shooter { private Optional voltageOverride; public ShooterImpl() { - shooterLeader = new TalonFX(Ports.HoodedShooter.Shooter.MOTOR_LEAD); - shooterFollower = new TalonFX(Ports.HoodedShooter.Shooter.MOTOR_FOLLOW); + shooterLeader = new TalonFX(Ports.HoodedShooter.Shooter.MOTOR_LEAD, Ports.RIO); + shooterFollower = new TalonFX(Ports.HoodedShooter.Shooter.MOTOR_FOLLOW, Ports.RIO); shooterController = new VelocityVoltage(getTargetRPM() / 60.0) .withEnableFOC(true); diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index abaec807..b89ccd0a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -34,13 +34,13 @@ public class IntakeImpl extends Intake { private Optional pivotVoltageOverride; public IntakeImpl() { - pivot = new TalonFX(Ports.Intake.PIVOT); + pivot = new TalonFX(Ports.Intake.PIVOT, Ports.RIO); Motors.Intake.PIVOT.configure(pivot); - rollerLeader = new TalonFX(Ports.Intake.ROLLER_LEADER); + rollerLeader = new TalonFX(Ports.Intake.ROLLER_LEADER, Ports.RIO); Motors.Intake.ROLLER.configure(rollerLeader); - rollerFollower = new TalonFX(Ports.Intake.ROLLER_FOLLOWER); + rollerFollower = new TalonFX(Ports.Intake.ROLLER_FOLLOWER, Ports.RIO); Motors.Intake.ROLLER.configure(rollerFollower); pivotController = new MotionMagicVoltage(getPivotState().getTargetAngle().getRotations()) diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java index 1b098441..7eb88853 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -30,8 +30,8 @@ public class SpindexerImpl extends Spindexer { private Optional voltageOverride; public SpindexerImpl() { - leadMotor = new TalonFX(Ports.Spindexer.SPINDEXER_LEAD_MOTOR); - followerMotor = new TalonFX(Ports.Spindexer.SPINDEXER_FOLLOW_MOTOR); + leadMotor = new TalonFX(Ports.Spindexer.SPINDEXER_LEAD_MOTOR, Ports.CANIVORE); + followerMotor = new TalonFX(Ports.Spindexer.SPINDEXER_FOLLOW_MOTOR, Ports.CANIVORE); Motors.Spindexer.SPINDEXER.configure(leadMotor); Motors.Spindexer.SPINDEXER.configure(followerMotor); diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java index 18be43db..97d58657 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java @@ -33,9 +33,9 @@ public class TurretImpl extends Turret { public TurretImpl() { - motor = new TalonFX(Ports.Turret.MOTOR, Ports.bus); - encoder17t = new CANcoder(Ports.Turret.ENCODER17T, Ports.bus); - encoder18t = new CANcoder(Ports.Turret.ENCODER18T, Ports.bus); + motor = new TalonFX(Ports.Turret.MOTOR, Ports.RIO); + encoder17t = new CANcoder(Ports.Turret.ENCODER17T, Ports.RIO); + encoder18t = new CANcoder(Ports.Turret.ENCODER18T, Ports.RIO); Motors.Turret.TURRET.configure(motor);