From 416f0e05f8bb6708ee05dae9b1ad13e08478125d Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Wed, 25 Feb 2026 13:10:15 -0500 Subject: [PATCH 001/150] FEAT: TunerConstants and driving good --- .../com/stuypulse/robot/RobotContainer.java | 2 +- .../subsystems/swerve/TunerConstants.java | 86 ++++++++++--------- 2 files changed, 47 insertions(+), 41 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index b7124041..90b18db6 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -96,7 +96,7 @@ public interface EnabledSubsystems { // Robot container public RobotContainer() { configureDefaultCommands(); - configureButtonBindings(); + // configureButtonBindings(); configureAutons(); SmartDashboard.putData("Field", Field.FIELD2D); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java index f5a896ff..d5a7e598 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java @@ -7,6 +7,13 @@ import static edu.wpi.first.units.Units.*; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.*; +import com.ctre.phoenix6.hardware.*; +import com.ctre.phoenix6.signals.*; +import com.ctre.phoenix6.swerve.*; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + import com.stuypulse.robot.constants.Gains; import com.stuypulse.robot.constants.Settings; @@ -15,12 +22,11 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.configs.*; -import com.ctre.phoenix6.hardware.*; -import com.ctre.phoenix6.signals.*; -import com.ctre.phoenix6.swerve.*; -import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.measure.*; + // Generated by the 2026 Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html @@ -30,14 +36,14 @@ public class TunerConstants { // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(Gains.Swerve.Turn.kP).withKI(Gains.Swerve.Turn.kI).withKD(Gains.Swerve.Turn.kD) - .withKS(Gains.Swerve.Turn.kS).withKV(Gains.Swerve.Turn.kV).withKA(Gains.Swerve.Turn.kA) + .withKP(100).withKI(0).withKD(0.5) + .withKS(0.1).withKV(1.16).withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(Gains.Swerve.Drive.kP).withKI(Gains.Swerve.Drive.kI).withKD(Gains.Swerve.Drive.kV) - .withKS(Gains.Swerve.Drive.kS).withKV(Gains.Swerve.Drive.kV); + .withKP(0.1).withKI(0).withKD(0) + .withKS(0).withKV(0.124); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors @@ -76,24 +82,24 @@ public class TunerConstants { // CAN bus that the devices are located on; // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = Settings.CANIVORE; + public static final CANBus kCANBus = new CANBus("CANIVORE", "./logs/example.hoot"); // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.76); + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.76); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.125; + private static final double kCoupleRatio = 5.4; - private static final double kDriveGearRatio = 5.357142857142857; - private static final double kSteerGearRatio = 21.428571428571427; + private static final double kDriveGearRatio = 6.48; + private static final double kSteerGearRatio = 12.1; private static final Distance kWheelRadius = Inches.of(2); private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; - private static final int kPigeonId = 5; + private static final int kPigeonId = 0; // These are only used for simulation private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); @@ -132,48 +138,48 @@ public class TunerConstants { // Front Left - private static final int kFrontLeftDriveMotorId = 32; - private static final int kFrontLeftSteerMotorId = 42; - private static final int kFrontLeftEncoderId = 4; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.09375); + private static final int kFrontLeftDriveMotorId = 14; + private static final int kFrontLeftSteerMotorId = 15; + private static final int kFrontLeftEncoderId = 2; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.21435546875); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; - private static final Distance kFrontLeftXPos = Inches.of(17.15); - private static final Distance kFrontLeftYPos = Inches.of(19.7); + private static final Distance kFrontLeftXPos = Inches.of(4.5); + private static final Distance kFrontLeftYPos = Inches.of(6.5); // Front Right - private static final int kFrontRightDriveMotorId = 50; - private static final int kFrontRightSteerMotorId = 13; - private static final int kFrontRightEncoderId = 2; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.14306640625); + private static final int kFrontRightDriveMotorId = 13; + private static final int kFrontRightSteerMotorId = 12; + private static final int kFrontRightEncoderId = 3; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.291259765625); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; - private static final Distance kFrontRightXPos = Inches.of(17.15); - private static final Distance kFrontRightYPos = Inches.of(-19.7); + private static final Distance kFrontRightXPos = Inches.of(4.5); + private static final Distance kFrontRightYPos = Inches.of(-6.5); // Back Left - private static final int kBackLeftDriveMotorId = 22; - private static final int kBackLeftSteerMotorId = 11; - private static final int kBackLeftEncoderId = 3; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.251708984375); + private static final int kBackLeftDriveMotorId = 16; + private static final int kBackLeftSteerMotorId = 17; + private static final int kBackLeftEncoderId = 1; + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.484375); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; - private static final Distance kBackLeftXPos = Inches.of(-17.15); - private static final Distance kBackLeftYPos = Inches.of(19.7); + private static final Distance kBackLeftXPos = Inches.of(-4.5); + private static final Distance kBackLeftYPos = Inches.of(6.5); // Back Right - private static final int kBackRightDriveMotorId = 21; - private static final int kBackRightSteerMotorId = 0; - private static final int kBackRightEncoderId = 1; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.17822265625); + private static final int kBackRightDriveMotorId = 11; + private static final int kBackRightSteerMotorId = 10; + private static final int kBackRightEncoderId = 4; + private static final Angle kBackRightEncoderOffset = Rotations.of(0.462890625); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; - private static final Distance kBackRightXPos = Inches.of(-17.15); - private static final Distance kBackRightYPos = Inches.of(-19.7); + private static final Distance kBackRightXPos = Inches.of(-4.5); + private static final Distance kBackRightYPos = Inches.of(-6.5); public static final SwerveModuleConstants FrontLeft = From 282622f7610fbd6139735e782951fa479c5df689 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 26 Feb 2026 12:38:23 -0500 Subject: [PATCH 002/150] pre-pd. 7 --- .../com/stuypulse/robot/RobotContainer.java | 63 +++++++++++-------- .../robot/commands/turret/TurretZero.java | 20 ++++-- .../stuypulse/robot/constants/Cameras.java | 1 + .../com/stuypulse/robot/constants/Gains.java | 2 +- .../com/stuypulse/robot/constants/Motors.java | 4 +- .../stuypulse/robot/constants/Settings.java | 2 +- .../robot/subsystems/turret/Turret.java | 1 + .../robot/subsystems/turret/TurretImpl.java | 3 +- .../robot/subsystems/turret/TurretSim.java | 5 ++ 9 files changed, 67 insertions(+), 34 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 90b18db6..9e25dc29 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -33,12 +33,14 @@ import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveXMode; +import com.stuypulse.robot.commands.turret.TurretAnalog; import com.stuypulse.robot.commands.turret.TurretFerry; import com.stuypulse.robot.commands.turret.TurretIdle; import com.stuypulse.robot.commands.turret.TurretLeftCorner; import com.stuypulse.robot.commands.turret.TurretRightCorner; import com.stuypulse.robot.commands.turret.TurretSeed; import com.stuypulse.robot.commands.turret.TurretShoot; +import com.stuypulse.robot.commands.turret.TurretZero; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; @@ -62,7 +64,7 @@ public class RobotContainer { public interface EnabledSubsystems { - SmartBoolean SWERVE = new SmartBoolean("Enabled Subsystems/Swerve Is Enabled", true); + SmartBoolean SWERVE = new SmartBoolean("Enabled Subsystems/Swerve Is Enabled", false); SmartBoolean TURRET = new SmartBoolean("Enabled Subsystems/Turret Is Enabled", false); SmartBoolean HANDOFF = new SmartBoolean("Enabled Subsystems/Handoff Is Enabled", false); SmartBoolean INTAKE = new SmartBoolean("Enabled Subsystems/Intake Is Enabled", false); @@ -96,10 +98,13 @@ public interface EnabledSubsystems { // Robot container public RobotContainer() { configureDefaultCommands(); - // configureButtonBindings(); + configureButtonBindings(); configureAutons(); + configureSysids(); SmartDashboard.putData("Field", Field.FIELD2D); + SmartDashboard.putData("Zero Encoders", new TurretZero()); + // SmartDashboard.putData("Seed Encoders", new TurretSee()); } /****************/ @@ -127,31 +132,34 @@ private void configureButtonBindings() { // Reset Heading driver.getDPadUp() .onTrue(new SwerveResetHeading()); + + driver.getTopButton() + .whileTrue(new TurretAnalog(driver)); // Scoring Routine using Interpolation Settings - driver.getTopButton() - .whileTrue(new HoodedShooterInterpolation() - .alongWith(new TurretShoot()) - .alongWith(new WaitUntilCommand(() -> hoodedShooter.bothAtTolerance())) - .andThen(new HandoffRun().onlyIf(() -> hoodedShooter.bothAtTolerance()) - .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) - .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && hoodedShooter.bothAtTolerance())))) - .onFalse(new SpindexerStop() - .alongWith(new HoodedShooterStow()) - .alongWith(new HandoffStop())); - - // Ferry Routine using Interpolation Settings - 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()))) - ) - .onFalse(new SpindexerStop() - .alongWith(new HoodedShooterStow()) - .alongWith(new HandoffStop())); + // driver.getTopButton() + // .whileTrue(new HoodedShooterInterpolation() + // .alongWith(new TurretShoot()) + // .alongWith(new WaitUntilCommand(() -> hoodedShooter.bothAtTolerance())) + // .andThen(new HandoffRun().onlyIf(() -> hoodedShooter.bothAtTolerance()) + // .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) + // .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && hoodedShooter.bothAtTolerance())))) + // .onFalse(new SpindexerStop() + // .alongWith(new HoodedShooterStow()) + // .alongWith(new HandoffStop())); + + // // Ferry Routine using Interpolation Settings + // 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()))) + // ) + // .onFalse(new SpindexerStop() + // .alongWith(new HoodedShooterStow()) + // .alongWith(new HandoffStop())); //-------------------------------------------------------------------------------------------------------------------------\\ //-------------------------------------------------------------------------------------------------------------------------\\ @@ -283,6 +291,11 @@ public void configureSysids() { // autonChooser.addOption("SysID Module Translation Quasi Forwards", swerve.sysIdQuasistatic(Direction.kForward)); // autonChooser.addOption("SysID Module Translation Quasi Backwards", swerve.sysIdQuasistatic(Direction.kReverse)); + autonChooser.addOption("SysID Turret Dynamic Forward", turret.getSysIdRoutine().dynamic(Direction.kForward)); + autonChooser.addOption("SysID Turret Dynamic Reverse", turret.getSysIdRoutine().dynamic(Direction.kReverse)); + autonChooser.addOption("SysID Turret Quasistatic Forward", turret.getSysIdRoutine().quasistatic(Direction.kForward)); + autonChooser.addOption("SysID Turret Quasistatic Reverse", turret.getSysIdRoutine().quasistatic(Direction.kReverse)); + // autonChooser.addOption("SysID Module Rotation Dynamic Forwards", swerve.sysIdRotDynamic(Direction.kForward)); // autonChooser.addOption("SysID Module Rotation Dynamic Backwards", swerve.sysIdRotDynamic(Direction.kReverse)); // autonChooser.addOption("SysID Module Rotation Quasi Forwards", swerve.sysIdRotQuasi(Direction.kForward)); diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretZero.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretZero.java index 8ad2e8d7..e133c156 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretZero.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretZero.java @@ -1,14 +1,26 @@ -/************************ 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.commands.turret; -import com.stuypulse.robot.subsystems.turret.Turret.TurretState; +import com.stuypulse.robot.subsystems.turret.Turret; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class TurretZero extends InstantCommand { + + private final Turret turret; -public class TurretZero extends TurretSetState { public TurretZero() { - super(TurretState.ZERO); + this.turret = Turret.getInstance(); + + addRequirements(turret); + } + + @Override + public void initialize() { + turret.zeroEncoders(); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index b264f114..38cb3e09 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -13,6 +13,7 @@ public interface Cameras { public Camera[] LimelightCameras = new Camera[] { + // LimelightCameras[0] = new Camera("limelight", new Pose3d) }; diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index d172cde0..a7287b3a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -81,7 +81,7 @@ public interface Turret { double kI = 0.0; double kD = 140.0; - double kS = 0.179; + double kS = 0.23; // FOUND ON 2/25 PD 8 double kV = 0.0; double kA = 0.0; } diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index 61bf55e2..3eb622ee 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -156,8 +156,8 @@ public interface Turret { .withAbsoluteSensorDiscontinuityPoint(1.0); public final SoftwareLimitSwitchConfigs SOFT_LIMITS = new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitEnable(true) - .withReverseSoftLimitEnable(true) + .withForwardSoftLimitEnable(false) + .withReverseSoftLimitEnable(false) .withForwardSoftLimitThreshold(Settings.Turret.Constants.SoftwareLimit.FORWARD_MAX_ROTATIONS) .withReverseSoftLimitThreshold(Settings.Turret.Constants.SoftwareLimit.BACKWARDS_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 0caa5baa..f6fc1ca8 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -256,7 +256,7 @@ public interface Constants { 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); - public final double GEAR_RATIO_MOTOR_TO_MECH = 1425.0 / 36.0; + public final double GEAR_RATIO_MOTOR_TO_MECH = (60.0 / 9.0) * (95.0 / 12.0); //1425.0 / 36.0; public interface BigGear { public final int TEETH = 95; 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 7dc79e49..1bfdf4a9 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java @@ -90,6 +90,7 @@ public Rotation2d getFerryAngle() { public abstract SysIdRoutine getSysIdRoutine(); public abstract void seedTurret(); + public abstract void zeroEncoders(); public void setState(TurretState state) { this.state = state; 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 97d58657..c5e7016b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java @@ -123,7 +123,8 @@ public void periodic() { SmartDashboard.putNumber("Turret/Delta (deg)", getDelta(getTargetAngle().getDegrees(), getAngle().getDegrees())); SmartDashboard.putNumber("Turret/Actual Target (deg)", actualTargetDeg); - if (EnabledSubsystems.TURRET.get() && getState() != TurretState.IDLE) { + // if (EnabledSubsystems.TURRET.get() && getState() != TurretState.IDLE) { + if(EnabledSubsystems.TURRET.get()) { if (voltageOverride.isPresent()) { motor.setVoltage(voltageOverride.get()); } else { diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java index 5354d939..48729621 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java @@ -84,6 +84,11 @@ public void seedTurret() { return; } + @Override + public void zeroEncoders() { + return; + } + private double getAngularVelocityRadPerSec() { return sim.getOutput(1); } From f16c89a485483e3a4897edf36646d2eb73aaefac Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 26 Feb 2026 14:07:33 -0500 Subject: [PATCH 003/150] feat: intake pivot gains --- .../java/com/stuypulse/robot/constants/Gains.java | 9 +++++---- .../java/com/stuypulse/robot/constants/Motors.java | 12 ++++++++++-- .../java/com/stuypulse/robot/constants/Settings.java | 2 +- .../robot/subsystems/intake/IntakeImpl.java | 4 ++++ 4 files changed, 20 insertions(+), 7 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index a7287b3a..13c57868 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -54,12 +54,13 @@ public interface Spindexer { public interface Intake { public interface Pivot { - double kP = 1.0; + // commented out values are proposed values found during , need to dampen slightly more though. Also might need to update the max velocity + double kP = 40.0; //450 double kI = 0.0; - double kD = 0.0; + double kD = 0.0; //7.5 - double kS = 0.0; - double kV = 0.0; + double kS = 0.5; + double kV = 0.12; //1.2 double kA = 0.0; double kG = 0.0; diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index 3eb622ee..a7ebda31 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -113,9 +113,17 @@ public interface Intake { .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) .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) + .withSensorToMechanismRatio(Settings.Intake.GEAR_RATIO) .withMotionProfile(Settings.Intake.PIVOT_MAX_VEL.getRotations(), Settings.Intake.PIVOT_MAX_ACCEL.getRotations()); + + public final Slot0Configs PIVOT_SLOT_0 = new Slot0Configs() + .withKP(Gains.Intake.Pivot.kP) + .withKI(Gains.Intake.Pivot.kI) + .withKD(Gains.Intake.Pivot.kD) + .withKS(Gains.Intake.Pivot.kS) + .withKV(Gains.Intake.Pivot.kV) + .withKA(Gains.Intake.Pivot.kA) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); } public interface Spindexer { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index f6fc1ca8..8021a89d 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -93,7 +93,7 @@ public interface Intake { Rotation2d PIVOT_MAX_VEL = Rotation2d.fromDegrees(300.0); Rotation2d PIVOT_MAX_ACCEL = Rotation2d.fromDegrees(300.0); - double GEAR_RATIO = 48.0; + double GEAR_RATIO = 37.93; } public interface Spindexer { 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 b89ccd0a..9e26757e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -37,6 +37,8 @@ public IntakeImpl() { pivot = new TalonFX(Ports.Intake.PIVOT, Ports.RIO); Motors.Intake.PIVOT.configure(pivot); + pivot.getConfigurator().apply(Motors.Intake.PIVOT_SLOT_0); + rollerLeader = new TalonFX(Ports.Intake.ROLLER_LEADER, Ports.RIO); Motors.Intake.ROLLER.configure(rollerLeader); @@ -50,6 +52,8 @@ public IntakeImpl() { follower = new Follower(Ports.Intake.ROLLER_LEADER, MotorAlignmentValue.Opposed); pivotVoltageOverride = Optional.empty(); + + pivot.setPosition(0); } @Override From 7eb8fb84c8af9b1c8180ad086c05903bbac9ddbb Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 26 Feb 2026 19:22:19 -0500 Subject: [PATCH 004/150] feat: intake commands, limelight imu stuff, drivetrain stuff --- .../com/stuypulse/robot/RobotContainer.java | 27 +++++++++++-------- .../stuypulse/robot/constants/Cameras.java | 11 +++++--- .../com/stuypulse/robot/constants/Gains.java | 10 +++---- .../com/stuypulse/robot/constants/Motors.java | 6 ++--- .../stuypulse/robot/constants/Settings.java | 1 + .../robot/subsystems/intake/IntakeImpl.java | 4 +-- .../subsystems/swerve/TunerConstants.java | 4 +-- 7 files changed, 37 insertions(+), 26 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 9e25dc29..1115c26e 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -25,6 +25,7 @@ import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterShoot; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterStow; import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.intake.IntakeRunRollers; import com.stuypulse.robot.commands.intake.IntakeStopRollers; import com.stuypulse.robot.commands.intake.IntakeStow; import com.stuypulse.robot.commands.spindexer.SpindexerRun; @@ -41,6 +42,8 @@ import com.stuypulse.robot.commands.turret.TurretSeed; import com.stuypulse.robot.commands.turret.TurretShoot; import com.stuypulse.robot.commands.turret.TurretZero; +import com.stuypulse.robot.commands.vision.ResetLimelightIMU; +import com.stuypulse.robot.commands.vision.SetIMUMode; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; @@ -104,7 +107,6 @@ public RobotContainer() { SmartDashboard.putData("Field", Field.FIELD2D); SmartDashboard.putData("Zero Encoders", new TurretZero()); - // SmartDashboard.putData("Seed Encoders", new TurretSee()); } /****************/ @@ -123,7 +125,8 @@ private void configureDefaultCommands() { private void configureButtonBindings() { // Intake Up and Off driver.getLeftTriggerButton() - .onTrue(new IntakeStow()); + .onTrue(new IntakeRunRollers()) + .onFalse(new IntakeStopRollers()); // Intake Down and On driver.getRightTriggerButton() @@ -131,7 +134,9 @@ private void configureButtonBindings() { // Reset Heading driver.getDPadUp() - .onTrue(new SwerveResetHeading()); + .onTrue(new SwerveResetHeading()) + .onTrue(new ResetLimelightIMU()) + .onFalse(new SetIMUMode(0)); driver.getTopButton() .whileTrue(new TurretAnalog(driver)); @@ -286,15 +291,15 @@ public void configureAutons() { public void configureSysids() { - // autonChooser.addOption("SysID Module Translation Dynamic Forward", swerve.sysIdDynamic(Direction.kForward)); - // autonChooser.addOption("SysID Module Translation Dynamic Backwards", swerve.sysIdDynamic(Direction.kReverse)); - // autonChooser.addOption("SysID Module Translation Quasi Forwards", swerve.sysIdQuasistatic(Direction.kForward)); - // autonChooser.addOption("SysID Module Translation Quasi Backwards", swerve.sysIdQuasistatic(Direction.kReverse)); + autonChooser.addOption("SysID Module Translation Dynamic Forward", swerve.sysIdDynamic(Direction.kForward)); + autonChooser.addOption("SysID Module Translation Dynamic Backwards", swerve.sysIdDynamic(Direction.kReverse)); + autonChooser.addOption("SysID Module Translation Quasi Forwards", swerve.sysIdQuasistatic(Direction.kForward)); + autonChooser.addOption("SysID Module Translation Quasi Backwards", swerve.sysIdQuasistatic(Direction.kReverse)); - autonChooser.addOption("SysID Turret Dynamic Forward", turret.getSysIdRoutine().dynamic(Direction.kForward)); - autonChooser.addOption("SysID Turret Dynamic Reverse", turret.getSysIdRoutine().dynamic(Direction.kReverse)); - autonChooser.addOption("SysID Turret Quasistatic Forward", turret.getSysIdRoutine().quasistatic(Direction.kForward)); - autonChooser.addOption("SysID Turret Quasistatic Reverse", turret.getSysIdRoutine().quasistatic(Direction.kReverse)); + // autonChooser.addOption("SysID Turret Dynamic Forward", turret.getSysIdRoutine().dynamic(Direction.kForward)); + // autonChooser.addOption("SysID Turret Dynamic Reverse", turret.getSysIdRoutine().dynamic(Direction.kReverse)); + // autonChooser.addOption("SysID Turret Quasistatic Forward", turret.getSysIdRoutine().quasistatic(Direction.kForward)); + // autonChooser.addOption("SysID Turret Quasistatic Reverse", turret.getSysIdRoutine().quasistatic(Direction.kReverse)); // autonChooser.addOption("SysID Module Rotation Dynamic Forwards", swerve.sysIdRotDynamic(Direction.kForward)); // autonChooser.addOption("SysID Module Rotation Dynamic Backwards", swerve.sysIdRotDynamic(Direction.kReverse)); diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 38cb3e09..5ac223db 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -5,16 +5,21 @@ /***************************************************************/ package com.stuypulse.robot.constants; +import com.stuypulse.robot.RobotContainer; import com.stuypulse.stuylib.network.SmartBoolean; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; /** This interface stores information about each camera. */ public interface Cameras { - public Camera[] LimelightCameras = new Camera[] { - // LimelightCameras[0] = new Camera("limelight", new Pose3d) - + public Camera[] LimelightCameras = { + new Camera("limelight-climber", + new Pose3d(-0.375959, -0.233, 0.20368, + new Rotation3d(Units.degreesToRadians(180), Units.degreesToRadians(31.08), Units.degreesToRadians(-80))), + RobotContainer.EnabledSubsystems.LIMELIGHT) }; public static class Camera { diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 13c57868..eb2ec845 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -63,7 +63,7 @@ public interface Pivot { double kV = 0.12; //1.2 double kA = 0.0; - double kG = 0.0; + double kG = 0.0; } } @@ -89,13 +89,13 @@ public interface Turret { public interface Swerve { public interface Drive { - double kP = 0.1; + double kP = 0.3838; double kI = 0.0; double kD = 0.0; - double kS = 0.0; - double kV = 0.124; - double kA = 0.0; + double kS = 0.20896; + double kV = 0.12464; + double kA = 0.014877; } public interface Turn { diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index a7ebda31..92e1d112 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -31,7 +31,7 @@ public interface Motors { public interface ClimberHopper { public final TalonFXConfig MOTOR = new TalonFXConfig() - .withInvertedValue(InvertedValue.Clockwise_Positive) + .withInvertedValue(InvertedValue.CounterClockwise_Positive) .withNeutralMode(NeutralModeValue.Brake) .withCurrentLimitAmps(50.0) .withSupplyCurrentLimitAmps(50.0) @@ -106,7 +106,7 @@ public interface Intake { .withCurrentLimitAmps(40.0) .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Coast) - .withInvertedValue(InvertedValue.Clockwise_Positive); + .withInvertedValue(InvertedValue.CounterClockwise_Positive); public final TalonFXConfig PIVOT = new TalonFXConfig() .withCurrentLimitAmps(40.0) @@ -131,7 +131,7 @@ public interface Spindexer { .withCurrentLimitEnable(false) .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.Clockwise_Positive) + .withInvertedValue(InvertedValue.CounterClockwise_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); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 8021a89d..633d5305 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -282,5 +282,6 @@ public interface SoftwareLimit { 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); + public final int RESET_IMU_INDEX = 2; } } \ No newline at end of file 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 9e26757e..5ffb5e58 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -49,7 +49,7 @@ public IntakeImpl() { .withEnableFOC(true); rollerController = new DutyCycleOut(getRollerState().getTargetDutyCycle()) .withEnableFOC(true); - follower = new Follower(Ports.Intake.ROLLER_LEADER, MotorAlignmentValue.Opposed); + follower = new Follower(Ports.Intake.ROLLER_LEADER, MotorAlignmentValue.Aligned); pivotVoltageOverride = Optional.empty(); @@ -76,7 +76,7 @@ public void periodic() { if (pivotVoltageOverride.isPresent()) { pivot.setVoltage(pivotVoltageOverride.get()); } else { - pivot.setControl(pivotController.withPosition(getPivotState().getTargetAngle().getRotations())); + // pivot.setControl(pivotController.withPosition(getPivotState().getTargetAngle().getRotations())); rollerLeader.setControl(rollerController.withOutput(getRollerState().getTargetDutyCycle())); rollerFollower.setControl(follower); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java index d5a7e598..72349796 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java @@ -42,8 +42,8 @@ public class TunerConstants { // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.1).withKI(0).withKD(0) - .withKS(0).withKV(0.124); + .withKP(0.3838).withKI(0).withKD(0) + .withKS(0.20896).withKV(0.12464).withKA(0.014877); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors From 66f2e164ea6dc6b12fd4470f86e886c5a4657f67 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 26 Feb 2026 19:23:02 -0500 Subject: [PATCH 005/150] fix previous commit w/ new files --- .../commands/intake/IntakeRunRollers.java | 14 +++++++++++++ .../commands/vision/ResetLimelightIMU.java | 9 ++++++++ .../robot/commands/vision/SetIMUMode.java | 21 +++++++++++++++++++ 3 files changed, 44 insertions(+) create mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeRunRollers.java create mode 100644 src/main/java/com/stuypulse/robot/commands/vision/ResetLimelightIMU.java create mode 100644 src/main/java/com/stuypulse/robot/commands/vision/SetIMUMode.java diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeRunRollers.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeRunRollers.java new file mode 100644 index 00000000..544f8fdb --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeRunRollers.java @@ -0,0 +1,14 @@ +/************************ 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.commands.intake; + +import com.stuypulse.robot.subsystems.intake.Intake.RollerState; + +public class IntakeRunRollers extends IntakeSetState { + public IntakeRunRollers() { + super(RollerState.INTAKE); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/vision/ResetLimelightIMU.java b/src/main/java/com/stuypulse/robot/commands/vision/ResetLimelightIMU.java new file mode 100644 index 00000000..5ffa8fe2 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/vision/ResetLimelightIMU.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.vision; + +import com.stuypulse.robot.constants.Settings; + +public class ResetLimelightIMU extends SetIMUMode { + public ResetLimelightIMU() { + super(1); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/vision/SetIMUMode.java b/src/main/java/com/stuypulse/robot/commands/vision/SetIMUMode.java new file mode 100644 index 00000000..87a07af8 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/vision/SetIMUMode.java @@ -0,0 +1,21 @@ +package com.stuypulse.robot.commands.vision; + +import com.stuypulse.robot.subsystems.vision.LimelightVision; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class SetIMUMode extends InstantCommand { + private final LimelightVision vision; + private final int index; + + public SetIMUMode(int IMUIndex) { + vision = LimelightVision.getInstance(); + index = IMUIndex; + } + + @Override + public void initialize() { + super.initialize(); + vision.setIMUMode(index); + } +} From d0ad584ecb3706ec070029b4a24a8acaa38a8627 Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Fri, 27 Feb 2026 00:31:08 -0500 Subject: [PATCH 006/150] FIX: add match time logging, update drivetrain max velocity --- src/main/java/com/stuypulse/robot/Robot.java | 2 ++ src/main/java/com/stuypulse/robot/RobotContainer.java | 8 +++----- .../robot/commands/vision/ResetLimelightIMU.java | 2 -- src/main/java/com/stuypulse/robot/constants/Settings.java | 4 +--- 4 files changed, 6 insertions(+), 10 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 4fa0dcc5..32fb7451 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -52,6 +52,8 @@ public void robotPeriodic() { if (DriverStation.getAlliance().isPresent()) { alliance = DriverStation.getAlliance().get(); } + + SmartDashboard.putNumber("Match Time", DriverStation.getMatchTime()); } /*********************/ diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 1115c26e..937682fb 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -60,14 +60,12 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; public class RobotContainer { public interface EnabledSubsystems { - SmartBoolean SWERVE = new SmartBoolean("Enabled Subsystems/Swerve Is Enabled", false); + SmartBoolean SWERVE = new SmartBoolean("Enabled Subsystems/Swerve Is Enabled", true); SmartBoolean TURRET = new SmartBoolean("Enabled Subsystems/Turret Is Enabled", false); SmartBoolean HANDOFF = new SmartBoolean("Enabled Subsystems/Handoff Is Enabled", false); SmartBoolean INTAKE = new SmartBoolean("Enabled Subsystems/Intake Is Enabled", false); @@ -75,7 +73,7 @@ public interface EnabledSubsystems { SmartBoolean CLIMBER_HOPPER = new SmartBoolean("Enabled Subsystems/Climber-Hopper Is Enabled", false); SmartBoolean HOOD = new SmartBoolean("Enabled Subsystems/Hood Is Enabled", false); SmartBoolean SHOOTER = new SmartBoolean("Enabled Subsystems/Shooter Is Enabled", false); - SmartBoolean LIMELIGHT = new SmartBoolean("Enabled Subsystems/Limelight Is Enabled", false); + SmartBoolean LIMELIGHT = new SmartBoolean("Enabled Subsystems/Limelight Is Enabled", true); } // Gamepads @@ -123,7 +121,7 @@ private void configureDefaultCommands() { /***************/ private void configureButtonBindings() { - // Intake Up and Off + // Intake Run Rollers driver.getLeftTriggerButton() .onTrue(new IntakeRunRollers()) .onFalse(new IntakeStopRollers()); diff --git a/src/main/java/com/stuypulse/robot/commands/vision/ResetLimelightIMU.java b/src/main/java/com/stuypulse/robot/commands/vision/ResetLimelightIMU.java index 5ffa8fe2..9389b5cb 100644 --- a/src/main/java/com/stuypulse/robot/commands/vision/ResetLimelightIMU.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/ResetLimelightIMU.java @@ -1,7 +1,5 @@ package com.stuypulse.robot.commands.vision; -import com.stuypulse.robot.constants.Settings; - public class ResetLimelightIMU extends SetIMUMode { public ResetLimelightIMU() { super(1); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 633d5305..40ca0d6d 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -23,8 +23,6 @@ * We use StuyLib's SmartNumber / SmartBoolean in order to have tunable * values that we can edit on Shuffleboard. */ -import com.ctre.phoenix6.CANBus; -import com.pathplanner.lib.path.PathConstraints; public interface Settings { public final double DT = 0.020; @@ -198,7 +196,7 @@ public interface Swerve { public interface Alignment { public interface Constraints { - public final double DEFAULT_MAX_VELOCITY = 4.3; + public final double DEFAULT_MAX_VELOCITY = 4.93; 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); From 671486f062647ace66e9dac0874e0c1d3f55e7df Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Fri, 27 Feb 2026 00:32:05 -0500 Subject: [PATCH 007/150] FEAT: add tunable constants ONLY for spindexer (revert to prev if broken) --- .../com/stuypulse/robot/constants/Gains.java | 21 +++-- .../com/stuypulse/robot/constants/Motors.java | 91 ++++++++++++++++++- .../subsystems/spindexer/SpindexerImpl.java | 28 +++++- 3 files changed, 128 insertions(+), 12 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index eb2ec845..3c630af1 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -6,6 +6,7 @@ package com.stuypulse.robot.constants; import com.pathplanner.lib.config.PIDConstants; +import com.stuypulse.stuylib.network.SmartNumber; public class Gains { @@ -43,13 +44,21 @@ public interface Hood { } public interface Spindexer { - double kP = 1.20; - double kI = 0.0; - double kD = 0.0; + // double kP = 1.20; + // double kI = 0.0; + // double kD = 0.0; + + // double kS = 0.019444; + // double kA = 0.010876; + // double kV = 0.38546; + + SmartNumber kP = new SmartNumber("Spindexer/Gains/kP", 1.20); + SmartNumber kI = new SmartNumber("Spindexer/Gains/kI", 0.0); + SmartNumber kD = new SmartNumber("Spindexer/Gains/kD", 0.0); - double kS = 0.019444; - double kA = 0.010876; - double kV = 0.38546; + SmartNumber kS = new SmartNumber("Spindexer/Gains/kS", 0.019444); + SmartNumber kA = new SmartNumber("Spindexer/Gains/kA", 0.010876); + SmartNumber kV = new SmartNumber("Spindexer/Gains/kV", 0.38546); } public interface Intake { diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index 92e1d112..1765e2c3 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -5,6 +5,8 @@ /***************************************************************/ package com.stuypulse.robot.constants; +import java.util.function.DoubleSupplier; + import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; @@ -16,6 +18,7 @@ import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.Slot1Configs; import com.ctre.phoenix6.configs.Slot2Configs; +import com.ctre.phoenix6.configs.SlotConfigs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.CANcoder; @@ -26,6 +29,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import com.stuypulse.stuylib.network.SmartNumber; public interface Motors { @@ -104,7 +108,7 @@ public interface Shooter { public interface Intake { public final TalonFXConfig ROLLER = new TalonFXConfig() .withCurrentLimitAmps(40.0) - .withRampRate(0.25) + .withRampRate(0.50) .withNeutralMode(NeutralModeValue.Coast) .withInvertedValue(InvertedValue.CounterClockwise_Positive); @@ -127,13 +131,22 @@ public interface Intake { } public interface Spindexer { - public final TalonFXConfig SPINDEXER = new TalonFXConfig() + public final TalonFXConfig SPINDEXER_LEAD = new TalonFXConfig() .withCurrentLimitEnable(false) .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) .withInvertedValue(InvertedValue.CounterClockwise_Positive) - .withFFConstants(Gains.Spindexer.kS, Gains.Spindexer.kV, Gains.Spindexer.kA, 0) - .withPIDConstants(Gains.Spindexer.kP, Gains.Spindexer.kI, Gains.Spindexer.kD, 0) + .withFFConstants(Gains.Spindexer.kS.get(), Gains.Spindexer.kV.get(), Gains.Spindexer.kA.get(), 0) + .withPIDConstants(Gains.Spindexer.kP.get(), Gains.Spindexer.kI.get(), Gains.Spindexer.kD.get(), 0) + .withSensorToMechanismRatio(Settings.Spindexer.Constants.GEAR_RATIO); + + public final TalonFXConfig SPINDEXER_FOLLOWER = new TalonFXConfig() + .withCurrentLimitEnable(false) + .withRampRate(0.25) + .withNeutralMode(NeutralModeValue.Brake) + .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withFFConstants(Gains.Spindexer.kS.get(), Gains.Spindexer.kV.get(), Gains.Spindexer.kA.get(), 0) + .withPIDConstants(Gains.Spindexer.kP.get(), Gains.Spindexer.kI.get(), Gains.Spindexer.kD.get(), 0) .withSensorToMechanismRatio(Settings.Spindexer.Constants.GEAR_RATIO); } @@ -224,6 +237,13 @@ public static class TalonFXConfig { private final FeedbackConfigs feedbackConfigs = new FeedbackConfigs(); private final MotionMagicConfigs motionMagicConfigs = new MotionMagicConfigs(); + private final double[] lastKP = new double[3]; + private final double[] lastKI = new double[3]; + private final double[] lastKD = new double[3]; + private final double[] lastKS = new double[3]; + private final double[] lastKV = new double[3]; + private final double[] lastKA = new double[3]; + public void configure(TalonFX motor) { TalonFXConfiguration defaultConfig = new TalonFXConfiguration(); motor.getConfigurator().apply(defaultConfig); @@ -231,6 +251,69 @@ public void configure(TalonFX motor) { motor.getConfigurator().apply(configuration); } + public TalonFXConfiguration getConfiguration() { + return this.configuration; + } + + // SMARTNUMBER TUNABLE GAINS FOR TALONFX MOTOR CONTROLLERS + // Note that this should ONLY be used during testing/debugging and not for competition code + // Provide a double supplier using () -> {SmartNumberObject}.get(). Use () -> {constant} for terms that do not need to be tuned + public void updateGainsConfig(TalonFX motor, int slot, SmartNumber kP, SmartNumber kI, SmartNumber kD, SmartNumber kS, SmartNumber kV, SmartNumber kA) { + if (slot != 0 && slot != 1 && slot != 2) { + return; + } + + double currentKP = kP.getAsDouble(); + double currentKI = kI.getAsDouble(); + double currentKD = kD.getAsDouble(); + double currentKS = kS.getAsDouble(); + double currentKV = kV.getAsDouble(); + double currentKA = kA.getAsDouble(); + + boolean changed = + currentKP != lastKP[slot] || + currentKI != lastKI[slot] || + currentKD != lastKD[slot] || + currentKS != lastKS[slot] || + currentKV != lastKV[slot] || + currentKA != lastKA[slot]; + + if (!changed) { + return; + } + + SlotConfigs gainConfig = new SlotConfigs() + .withKP(currentKP) + .withKI(currentKI) + .withKD(currentKD) + .withKS(currentKS) + .withKV(currentKV) + .withKA(currentKA); + + gainConfig.SlotNumber = slot; + + motor.getConfigurator().apply(gainConfig); + + lastKP[slot] = currentKP; + lastKI[slot] = currentKI; + lastKD[slot] = currentKD; + lastKS[slot] = currentKS; + lastKV[slot] = currentKV; + lastKA[slot] = currentKA; + + switch (slot) { + case 0: + motor.getConfigurator().refresh(this.getConfiguration().Slot0); + break; + case 1: + motor.getConfigurator().refresh(this.getConfiguration().Slot1); + break; + case 2: + motor.getConfigurator().refresh(this.getConfiguration().Slot2); + break; + } + } + // SLOT CONFIGS public TalonFXConfig withPIDConstants(double kP, double kI, double kD, int slot) { 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 7eb88853..066489fe 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -6,6 +6,7 @@ package com.stuypulse.robot.subsystems.spindexer; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.constants.Gains; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; @@ -33,8 +34,8 @@ public SpindexerImpl() { 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); + Motors.Spindexer.SPINDEXER_LEAD.configure(leadMotor); + Motors.Spindexer.SPINDEXER_FOLLOWER.configure(followerMotor); controller = new VelocityVoltage(getTargetRPM()) .withEnableFOC(true); @@ -59,6 +60,29 @@ public boolean atTolerance() { @Override public void periodic() { super.periodic(); + + Motors.Spindexer.SPINDEXER_LEAD.updateGainsConfig( + leadMotor, + 0, + Gains.Spindexer.kP, + Gains.Spindexer.kI, + Gains.Spindexer.kD, + Gains.Spindexer.kS, + Gains.Spindexer.kV, + Gains.Spindexer.kA + ); + + Motors.Spindexer.SPINDEXER_FOLLOWER.updateGainsConfig( + followerMotor, + 0, + Gains.Spindexer.kP, + Gains.Spindexer.kI, + Gains.Spindexer.kD, + Gains.Spindexer.kS, + Gains.Spindexer.kV, + Gains.Spindexer.kA + ); + if (EnabledSubsystems.SPINDEXER.get()) { if (voltageOverride.isPresent()){ leadMotor.setVoltage(voltageOverride.get()); From 2c3a3905b41cfb7b90352f7c33e0a8e9d6f3904a Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Fri, 27 Feb 2026 00:39:57 -0500 Subject: [PATCH 008/150] FIX: add back imu mode and robot yaw logging in vision subsystem --- .../subsystems/vision/LimelightVision.java | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 79efc83d..b43f6d6c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -5,14 +5,14 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.vision; -import com.stuypulse.stuylib.network.SmartBoolean; - import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Cameras; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.vision.LimelightHelpers; +import com.stuypulse.robot.util.vision.LimelightHelpers.IMUData; import com.stuypulse.robot.util.vision.LimelightHelpers.PoseEstimate; +import com.stuypulse.stuylib.network.SmartBoolean; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -102,6 +102,17 @@ public void setIMUMode(int mode) { } } + public IMUData[] getIMUData() { + IMUData[] data = new IMUData[Cameras.LimelightCameras.length]; + + for (int i = 0; i < Cameras.LimelightCameras.length; i++) { + data[i] = LimelightHelpers.getIMUData(Cameras.LimelightCameras[i].getName()); + } + + return data; + } + + @Override public void periodic() { if (enabled.get()) { @@ -151,6 +162,9 @@ public void periodic() { } SmartDashboard.putString("Vision/MegaTag Mode", megaTagMode.toString()); + SmartDashboard.putNumber("Vision/Robot Yaw", LimelightHelpers.getIMUData(limelightName).robotYaw); + + SmartDashboard.putString("Vision/IMU mode", getIMUData()[0].toString()); //only 1 camera on alpha, so will need to change on Big T } } } From fcd9c77bd259f22597296bb04b969648fb80f7f5 Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Fri, 27 Feb 2026 00:53:21 -0500 Subject: [PATCH 009/150] FEAT: add motion profile limit switching based on state, increase current limits on intake, add logging --- .../robot/commands/intake/IntakeDeploy.java | 2 +- .../robot/commands/intake/IntakeStow.java | 2 +- .../com/stuypulse/robot/constants/Motors.java | 6 +-- .../stuypulse/robot/constants/Settings.java | 7 ++- .../robot/subsystems/intake/Intake.java | 6 +-- .../robot/subsystems/intake/IntakeImpl.java | 25 +++++++++-- .../stuypulse/robot/util/SettableNumber.java | 44 +++++++++++++++++++ 7 files changed, 79 insertions(+), 13 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/util/SettableNumber.java diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeploy.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeploy.java index de0fac60..c1f70b71 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeploy.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeploy.java @@ -10,6 +10,6 @@ public class IntakeDeploy extends IntakeSetState { public IntakeDeploy() { - super(PivotState.DEPLOYED, RollerState.INTAKE); + super(PivotState.DEPLOY, RollerState.INTAKE); } } diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeStow.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeStow.java index 56f2cb0c..bfd917ad 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeStow.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeStow.java @@ -10,6 +10,6 @@ public class IntakeStow extends IntakeSetState { public IntakeStow() { - super(PivotState.STOWED, RollerState.STOP); + super(PivotState.STOW, RollerState.STOP); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index 1765e2c3..c3b461fd 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -107,18 +107,18 @@ public interface Shooter { public interface Intake { public final TalonFXConfig ROLLER = new TalonFXConfig() - .withCurrentLimitAmps(40.0) + .withCurrentLimitAmps(60.0) .withRampRate(0.50) .withNeutralMode(NeutralModeValue.Coast) .withInvertedValue(InvertedValue.CounterClockwise_Positive); public final TalonFXConfig PIVOT = new TalonFXConfig() - .withCurrentLimitAmps(40.0) + .withCurrentLimitAmps(60.0) .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) .withInvertedValue(InvertedValue.Clockwise_Positive) .withSensorToMechanismRatio(Settings.Intake.GEAR_RATIO) - .withMotionProfile(Settings.Intake.PIVOT_MAX_VEL.getRotations(), Settings.Intake.PIVOT_MAX_ACCEL.getRotations()); + .withMotionProfile(Settings.Intake.PIVOT_MAX_VEL_STOW.getRotations(), Settings.Intake.PIVOT_MAX_ACCEL_STOW.getRotations()); public final Slot0Configs PIVOT_SLOT_0 = new Slot0Configs() .withKP(Gains.Intake.Pivot.kP) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 40ca0d6d..a4fd99bd 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -88,8 +88,11 @@ public interface Intake { Rotation2d PIVOT_MAX_ANGLE = Rotation2d.fromDegrees(190); Rotation2d PIVOT_MIN_ANGLE = Rotation2d.fromDegrees(80); - Rotation2d PIVOT_MAX_VEL = Rotation2d.fromDegrees(300.0); - Rotation2d PIVOT_MAX_ACCEL = Rotation2d.fromDegrees(300.0); + Rotation2d PIVOT_MAX_VEL_DEPLOY = Rotation2d.fromDegrees(360.0); + Rotation2d PIVOT_MAX_ACCEL_DEPLOY = Rotation2d.fromDegrees(1200.0); + + Rotation2d PIVOT_MAX_VEL_STOW = Rotation2d.fromDegrees(360.0); + Rotation2d PIVOT_MAX_ACCEL_STOW = Rotation2d.fromDegrees(600.0); double GEAR_RATIO = 37.93; } 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 bcde7954..ffa948cb 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -26,8 +26,8 @@ public static Intake getInstance() { } public enum PivotState { - DEPLOYED(Settings.Intake.PIVOT_INTAKE_OUTAKE_ANGLE), - STOWED(Settings.Intake.PIVOT_STOW_ANGLE); + DEPLOY(Settings.Intake.PIVOT_INTAKE_OUTAKE_ANGLE), + STOW(Settings.Intake.PIVOT_STOW_ANGLE); private final Rotation2d targetAngle; @@ -60,7 +60,7 @@ public double getTargetDutyCycle() { private RollerState rollerState; protected Intake() { - this.pivotState = PivotState.STOWED; + this.pivotState = PivotState.STOW; this.rollerState = RollerState.STOP; } 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 5ffb5e58..c27d2021 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -9,6 +9,7 @@ import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.SettableNumber; import com.stuypulse.robot.util.SysId; import edu.wpi.first.math.geometry.Rotation2d; @@ -31,6 +32,9 @@ public class IntakeImpl extends Intake { private final DutyCycleOut rollerController; private final Follower follower; + private SettableNumber velLimitDegreesPerSecond; + private SettableNumber accelLimitDegreesPerSecondSquared; + private Optional pivotVoltageOverride; public IntakeImpl() { @@ -67,11 +71,24 @@ public boolean pivotAtTolerance() { public Rotation2d getPivotAngle() { return Rotation2d.fromRotations(pivot.getPosition().getValueAsDouble()); } + + public void setMotionProfileConstraints(Rotation2d velLimit, Rotation2d accelLimit) { + this.velLimitDegreesPerSecond.set(velLimit.getDegrees()); + this.accelLimitDegreesPerSecondSquared.set(accelLimit.getDegrees()); + Motors.Intake.PIVOT.withMotionProfile(velLimit.getRotations(), accelLimit.getRotations()); + Motors.Intake.PIVOT.configure(pivot); + } @Override public void periodic() { super.periodic(); + if (getPivotState() == PivotState.STOW) { + setMotionProfileConstraints(Settings.Intake.PIVOT_MAX_VEL_STOW, Settings.Intake.PIVOT_MAX_ACCEL_STOW); + } else if (getPivotState() == PivotState.DEPLOY) { + setMotionProfileConstraints(Settings.Intake.PIVOT_MAX_VEL_DEPLOY, Settings.Intake.PIVOT_MAX_ACCEL_DEPLOY); + } + if (EnabledSubsystems.INTAKE.get()) { if (pivotVoltageOverride.isPresent()) { pivot.setVoltage(pivotVoltageOverride.get()); @@ -86,14 +103,16 @@ public void periodic() { rollerFollower.stopMotor(); } - SmartDashboard.putNumber("Intake/Pivot Angle Error (deg)", - Math.abs(getPivotState().getTargetAngle().getDegrees() - getPivotAngle().getDegrees())); - if (Settings.DEBUG_MODE) { // PIVOT SmartDashboard.putNumber("Intake/Pivot Voltage (volts)", pivot.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Intake/Pivot Current (amps)", pivot.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Pivot Max Velocity Limit (deg/s)", velLimitDegreesPerSecond.get()); + SmartDashboard.putNumber("Intake/Pivot Max Accel Limit (deg/s^2)", accelLimitDegreesPerSecondSquared.get()); + + SmartDashboard.putNumber("Intake/Pivot Angle Error (deg)", Math.abs(getPivotState().getTargetAngle().getDegrees() - getPivotAngle().getDegrees())); + // ROLLERS SmartDashboard.putNumber("Intake/Roller Leader Voltage (volts)", rollerLeader.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Intake/Roller Follower Voltage (volts)", rollerFollower.getMotorVoltage().getValueAsDouble()); diff --git a/src/main/java/com/stuypulse/robot/util/SettableNumber.java b/src/main/java/com/stuypulse/robot/util/SettableNumber.java new file mode 100644 index 00000000..2e877efa --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/SettableNumber.java @@ -0,0 +1,44 @@ + +/************************ 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; + +public class SettableNumber extends Number { + private double value; + + public SettableNumber(double value) { + this.value = value; + } + + public void set(double value) { + this.value = value; + } + + public double get() { + return value; + } + + @Override + public int intValue() { + return (int) value; + } + + @Override + public long longValue() { + return (long) value; + } + + @Override + public float floatValue() { + return (float) value; + } + + @Override + public double doubleValue() { + return (double) value; + } +} From 6e703e93d0c454f8dd4d1e978b2f444d8570da81 Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Fri, 27 Feb 2026 00:53:42 -0500 Subject: [PATCH 010/150] CLEAN: spotless --- .../robot/commands/turret/TurretZero.java | 2 +- .../commands/vision/ResetLimelightIMU.java | 5 +++++ .../robot/commands/vision/SetIMUMode.java | 5 +++++ .../com/stuypulse/robot/constants/Cameras.java | 3 ++- .../com/stuypulse/robot/constants/Gains.java | 3 ++- .../com/stuypulse/robot/constants/Motors.java | 3 +-- .../stuypulse/robot/constants/Settings.java | 5 +++-- .../subsystems/swerve/TunerConstants.java | 18 +++++------------- .../subsystems/vision/LimelightVision.java | 3 ++- .../stuypulse/robot/util/SettableNumber.java | 2 -- 10 files changed, 26 insertions(+), 23 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretZero.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretZero.java index e133c156..62bece3b 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretZero.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretZero.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/commands/vision/ResetLimelightIMU.java b/src/main/java/com/stuypulse/robot/commands/vision/ResetLimelightIMU.java index 9389b5cb..16ebb206 100644 --- a/src/main/java/com/stuypulse/robot/commands/vision/ResetLimelightIMU.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/ResetLimelightIMU.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.commands.vision; public class ResetLimelightIMU extends SetIMUMode { diff --git a/src/main/java/com/stuypulse/robot/commands/vision/SetIMUMode.java b/src/main/java/com/stuypulse/robot/commands/vision/SetIMUMode.java index 87a07af8..c979e0b6 100644 --- a/src/main/java/com/stuypulse/robot/commands/vision/SetIMUMode.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/SetIMUMode.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.commands.vision; import com.stuypulse.robot.subsystems.vision.LimelightVision; diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 5ac223db..a095c89d 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -5,9 +5,10 @@ /***************************************************************/ package com.stuypulse.robot.constants; -import com.stuypulse.robot.RobotContainer; import com.stuypulse.stuylib.network.SmartBoolean; +import com.stuypulse.robot.RobotContainer; + import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 3c630af1..25bbe41c 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -5,9 +5,10 @@ /***************************************************************/ package com.stuypulse.robot.constants; -import com.pathplanner.lib.config.PIDConstants; import com.stuypulse.stuylib.network.SmartNumber; +import com.pathplanner.lib.config.PIDConstants; + public class Gains { public interface ClimberHopper { diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index c3b461fd..43f96839 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -5,7 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.constants; -import java.util.function.DoubleSupplier; +import com.stuypulse.stuylib.network.SmartNumber; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; @@ -29,7 +29,6 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; -import com.stuypulse.stuylib.network.SmartNumber; public interface Motors { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index a4fd99bd..2c3748df 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -5,8 +5,6 @@ /***************************************************************/ 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; @@ -17,6 +15,9 @@ 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. * diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java index 72349796..bdbcc0c5 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java @@ -7,6 +7,11 @@ import static edu.wpi.first.units.Units.*; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.measure.*; + import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.*; import com.ctre.phoenix6.hardware.*; @@ -14,19 +19,6 @@ import com.ctre.phoenix6.swerve.*; import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; -import com.stuypulse.robot.constants.Gains; -import com.stuypulse.robot.constants.Settings; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.units.measure.*; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.units.measure.*; - // Generated by the 2026 Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index b43f6d6c..1324af89 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -5,6 +5,8 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.vision; +import com.stuypulse.stuylib.network.SmartBoolean; + import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Cameras; import com.stuypulse.robot.constants.Settings; @@ -12,7 +14,6 @@ import com.stuypulse.robot.util.vision.LimelightHelpers; import com.stuypulse.robot.util.vision.LimelightHelpers.IMUData; import com.stuypulse.robot.util.vision.LimelightHelpers.PoseEstimate; -import com.stuypulse.stuylib.network.SmartBoolean; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; diff --git a/src/main/java/com/stuypulse/robot/util/SettableNumber.java b/src/main/java/com/stuypulse/robot/util/SettableNumber.java index 2e877efa..b458bf3a 100644 --- a/src/main/java/com/stuypulse/robot/util/SettableNumber.java +++ b/src/main/java/com/stuypulse/robot/util/SettableNumber.java @@ -1,10 +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; public class SettableNumber extends Number { From 65df8cb8b25c1ab9f9aceacd3cac2375f9ae6ab5 Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Fri, 27 Feb 2026 01:06:32 -0500 Subject: [PATCH 011/150] FIX: intake min/max angles and build error; update glass layout --- simgui.json | 3 ++- .../com/stuypulse/robot/RobotContainer.java | 22 ++++++++++++------- .../com/stuypulse/robot/constants/Ports.java | 1 - .../stuypulse/robot/constants/Settings.java | 9 +++----- .../robot/subsystems/intake/Intake.java | 2 +- .../robot/subsystems/intake/IntakeImpl.java | 17 ++++++++------ 6 files changed, 30 insertions(+), 24 deletions(-) diff --git a/simgui.json b/simgui.json index d01bfcf5..68c2c46d 100644 --- a/simgui.json +++ b/simgui.json @@ -11,7 +11,8 @@ "/FMSInfo": "FMSInfo", "/SmartDashboard/Autonomous": "String Chooser", "/SmartDashboard/Field": "Field2d", - "/SmartDashboard/Visualizers/ClimberHopper": "Mechanism2d" + "/SmartDashboard/Visualizers/ClimberHopper": "Mechanism2d", + "/SmartDashboard/Visualizers/Turret": "Mechanism2d" }, "windows": { "/SmartDashboard/Field": { diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 937682fb..aba2f9f3 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -80,7 +80,7 @@ public interface EnabledSubsystems { public final Gamepad driver = new AutoGamepad(Ports.Gamepad.DRIVER); // Subsystem - private final ClimberHopper climberHopper = ClimberHopper.getInstance(); + // private final ClimberHopper climberHopper = ClimberHopper.getInstance(); private final Handoff handoff = Handoff.getInstance(); private final Intake intake = Intake.getInstance(); private final Spindexer spindexer = Spindexer.getInstance(); @@ -104,7 +104,7 @@ public RobotContainer() { configureSysids(); SmartDashboard.putData("Field", Field.FIELD2D); - SmartDashboard.putData("Zero Encoders", new TurretZero()); + // SmartDashboard.putData("Zero Encoders", new TurretZero()); } /****************/ @@ -113,7 +113,7 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); - climberHopper.setDefaultCommand(new ClimberHopperDefaultCommand()); + // climberHopper.setDefaultCommand(new ClimberHopperDefaultCommand()); } /***************/ @@ -122,14 +122,20 @@ private void configureDefaultCommands() { private void configureButtonBindings() { // Intake Run Rollers - driver.getLeftTriggerButton() - .onTrue(new IntakeRunRollers()) - .onFalse(new IntakeStopRollers()); + // driver.getLeftTriggerButton() + // .onTrue(new IntakeRunRollers()) + // .onFalse(new IntakeStopRollers()); - // Intake Down and On - driver.getRightTriggerButton() + driver.getRightButton() .onTrue(new IntakeDeploy()); + driver.getLeftButton() + .onTrue(new IntakeStow()); + + // Intake Down and On + // driver.getRightTriggerButton() + // .onTrue(new IntakeDeploy()); + // Reset Heading driver.getDPadUp() .onTrue(new SwerveResetHeading()) diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 0d8e164a..45c4722f 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -9,7 +9,6 @@ /** This file contains the different ports of motors, solenoids and sensors */ public interface Ports { - // TODO: Get bus name public CANBus RIO = new CANBus("rio"); public CANBus CANIVORE = new CANBus("CANIVORE"); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 2c3748df..5c31a45b 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -79,15 +79,12 @@ public interface Handoff { public interface Intake { Rotation2d PIVOT_STOW_ANGLE = Rotation2d.fromDegrees(90.0); - Rotation2d PIVOT_INTAKE_OUTAKE_ANGLE = Rotation2d.fromDegrees(0.0); + Rotation2d PIVOT_DEPLOY_ANGLE = Rotation2d.fromDegrees(0.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); - Rotation2d PIVOT_MIN_ANGLE = Rotation2d.fromDegrees(80); + Rotation2d PIVOT_MAX_ANGLE = Rotation2d.fromDegrees(75); + Rotation2d PIVOT_MIN_ANGLE = Rotation2d.fromDegrees(20); Rotation2d PIVOT_MAX_VEL_DEPLOY = Rotation2d.fromDegrees(360.0); Rotation2d PIVOT_MAX_ACCEL_DEPLOY = Rotation2d.fromDegrees(1200.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 ffa948cb..97ad6ed3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -26,7 +26,7 @@ public static Intake getInstance() { } public enum PivotState { - DEPLOY(Settings.Intake.PIVOT_INTAKE_OUTAKE_ANGLE), + DEPLOY(Settings.Intake.PIVOT_DEPLOY_ANGLE), STOW(Settings.Intake.PIVOT_STOW_ANGLE); private final Rotation2d targetAngle; 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 c27d2021..0a71fc01 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -32,8 +32,8 @@ public class IntakeImpl extends Intake { private final DutyCycleOut rollerController; private final Follower follower; - private SettableNumber velLimitDegreesPerSecond; - private SettableNumber accelLimitDegreesPerSecondSquared; + private SettableNumber velLimit; + private SettableNumber accelLimit; private Optional pivotVoltageOverride; @@ -55,9 +55,12 @@ public IntakeImpl() { .withEnableFOC(true); follower = new Follower(Ports.Intake.ROLLER_LEADER, MotorAlignmentValue.Aligned); + velLimit = new SettableNumber(Settings.Intake.PIVOT_MAX_VEL_DEPLOY.getDegrees()); + accelLimit = new SettableNumber(Settings.Intake.PIVOT_MAX_ACCEL_DEPLOY.getDegrees()); + pivotVoltageOverride = Optional.empty(); - pivot.setPosition(0); + pivot.setPosition(Settings.Intake.PIVOT_MAX_ANGLE.getRotations()); } @Override @@ -73,8 +76,8 @@ public Rotation2d getPivotAngle() { } public void setMotionProfileConstraints(Rotation2d velLimit, Rotation2d accelLimit) { - this.velLimitDegreesPerSecond.set(velLimit.getDegrees()); - this.accelLimitDegreesPerSecondSquared.set(accelLimit.getDegrees()); + this.velLimit.set(velLimit.getDegrees()); + this.accelLimit.set(accelLimit.getDegrees()); Motors.Intake.PIVOT.withMotionProfile(velLimit.getRotations(), accelLimit.getRotations()); Motors.Intake.PIVOT.configure(pivot); } @@ -108,8 +111,8 @@ public void periodic() { SmartDashboard.putNumber("Intake/Pivot Voltage (volts)", pivot.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Intake/Pivot Current (amps)", pivot.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("Intake/Pivot Max Velocity Limit (deg/s)", velLimitDegreesPerSecond.get()); - SmartDashboard.putNumber("Intake/Pivot Max Accel Limit (deg/s^2)", accelLimitDegreesPerSecondSquared.get()); + SmartDashboard.putNumber("Intake/Pivot Max Velocity Limit (deg/s)", velLimit.get()); + SmartDashboard.putNumber("Intake/Pivot Max Accel Limit (deg/s^2)", accelLimit.get()); SmartDashboard.putNumber("Intake/Pivot Angle Error (deg)", Math.abs(getPivotState().getTargetAngle().getDegrees() - getPivotAngle().getDegrees())); From 2c8e938cc20060948f4897a06a99dea77753e550 Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Fri, 27 Feb 2026 13:46:23 -0500 Subject: [PATCH 012/150] FEAT: add intakeAnalog for kG tuning --- .../com/stuypulse/robot/RobotContainer.java | 12 +++--- .../robot/commands/intake/IntakeAnalog.java | 32 ++++++++++++++ .../robot/subsystems/intake/Intake.java | 42 +++++++++++++++---- .../robot/subsystems/intake/IntakeImpl.java | 8 +++- 4 files changed, 79 insertions(+), 15 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeAnalog.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index aba2f9f3..491d28cd 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -24,6 +24,7 @@ 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.IntakeAnalog; import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.intake.IntakeRunRollers; import com.stuypulse.robot.commands.intake.IntakeStopRollers; @@ -126,16 +127,16 @@ private void configureButtonBindings() { // .onTrue(new IntakeRunRollers()) // .onFalse(new IntakeStopRollers()); + // Intake Down and On + // driver.getRightTriggerButton() + // .onTrue(new IntakeDeploy()); + driver.getRightButton() .onTrue(new IntakeDeploy()); driver.getLeftButton() .onTrue(new IntakeStow()); - // Intake Down and On - // driver.getRightTriggerButton() - // .onTrue(new IntakeDeploy()); - // Reset Heading driver.getDPadUp() .onTrue(new SwerveResetHeading()) @@ -143,7 +144,8 @@ private void configureButtonBindings() { .onFalse(new SetIMUMode(0)); driver.getTopButton() - .whileTrue(new TurretAnalog(driver)); + // .whileTrue(new TurretAnalog(driver)); + .whileTrue(new IntakeAnalog(driver)); // will be useful for finding kG // Scoring Routine using Interpolation Settings // driver.getTopButton() diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAnalog.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAnalog.java new file mode 100644 index 00000000..2fe1f24b --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAnalog.java @@ -0,0 +1,32 @@ +/************************ 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.commands.intake; + +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.intake.Intake.PivotState; + +import edu.wpi.first.wpilibj2.command.Command; + +public class IntakeAnalog extends Command { + private Gamepad gamepad; + private Intake intake; + public IntakeAnalog(Gamepad gamepad){ + intake = Intake.getInstance(); + this.gamepad = gamepad; + } + + @Override + public void initialize() { + super.initialize(); + intake.setPivotState(PivotState.ANALOG); + } + @Override + public void execute() { + super.execute(); + intake.setDriverInput(gamepad); + } +} \ 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 97ad6ed3..7975152e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -6,6 +6,8 @@ package com.stuypulse.robot.subsystems.intake; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.math.Vector2D; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -17,6 +19,11 @@ public abstract class Intake extends SubsystemBase { private static final Intake instance; + private PivotState pivotState; + private RollerState rollerState; + + private Vector2D driverInput; + static { instance = new IntakeImpl(); } @@ -25,9 +32,17 @@ public static Intake getInstance() { return instance; } + protected Intake() { + this.pivotState = PivotState.STOW; + this.rollerState = RollerState.STOP; + + driverInput = new Vector2D(0, 0); + } + public enum PivotState { DEPLOY(Settings.Intake.PIVOT_DEPLOY_ANGLE), - STOW(Settings.Intake.PIVOT_STOW_ANGLE); + STOW(Settings.Intake.PIVOT_STOW_ANGLE), + ANALOG(Settings.Intake.PIVOT_STOW_ANGLE); private final Rotation2d targetAngle; @@ -56,14 +71,6 @@ public double getTargetDutyCycle() { } } - private PivotState pivotState; - private RollerState rollerState; - - protected Intake() { - this.pivotState = PivotState.STOW; - this.rollerState = RollerState.STOP; - } - public PivotState getPivotState() { return pivotState; } @@ -81,6 +88,23 @@ public void setRollerState(RollerState state) { this.rollerState = state; } + public void setDriverInput(Gamepad gamepad) { + this.driverInput = gamepad.getLeftStick(); + } + + public Rotation2d driverInputToAngle() { + SmartDashboard.putNumber("Intake/Driver Input", driverInput.x); + + double minAngle = Settings.Intake.PIVOT_MIN_ANGLE.getDegrees(); + double maxAngle = Settings.Intake.PIVOT_MAX_ANGLE.getDegrees(); + + // Maps driver input [-1, 1] to pivot angle [MIN, MAX]. + // A driver input of 0 (i.e. centered joystick) will drive the pivot to the midpoint of the range of motion. + double scaledAngle = minAngle + (driverInput.x + 1.0) * (maxAngle - minAngle) / 2.0; + + return Rotation2d.fromDegrees(scaledAngle); + } + public abstract boolean pivotAtTolerance(); public abstract Rotation2d getPivotAngle(); 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 0a71fc01..4a6f6d69 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -23,6 +23,8 @@ import com.ctre.phoenix6.signals.MotorAlignmentValue; import java.util.Optional; +import javax.xml.xpath.XPathExpressionException; + public class IntakeImpl extends Intake { private final TalonFX pivot; private final TalonFX rollerLeader; @@ -93,7 +95,11 @@ public void periodic() { } if (EnabledSubsystems.INTAKE.get()) { - if (pivotVoltageOverride.isPresent()) { + if (getPivotState() == PivotState.ANALOG) { // comment out the setControl line if it breaks + pivot.setControl(pivotController.withPosition(driverInputToAngle().getRotations())); + } + + else if (pivotVoltageOverride.isPresent()) { pivot.setVoltage(pivotVoltageOverride.get()); } else { // pivot.setControl(pivotController.withPosition(getPivotState().getTargetAngle().getRotations())); From 3f49394e74e9922d9c643e3b59baf662ff351748 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 27 Feb 2026 13:51:13 -0500 Subject: [PATCH 013/150] fix: comment out creating a new shooter + update set pivot state to be the only time when the motion profiler constraints are changed instead of reconfiguring the motor periodically --- .../java/com/stuypulse/robot/RobotContainer.java | 2 +- .../robot/subsystems/intake/IntakeImpl.java | 12 +++++++++--- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index aba2f9f3..43b870d1 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -90,7 +90,7 @@ public interface EnabledSubsystems { private final Turret turret = Turret.getInstance(); private final HoodedShooter hoodedShooter = HoodedShooter.getInstance(); - private final Shooter shooter = Shooter.getInstance(); + // private final Shooter shooter = Shooter.getInstance(); private final Hood hood = Hood.getInstance(); // Autons 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 0a71fc01..30cbbeaf 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -83,14 +83,20 @@ public void setMotionProfileConstraints(Rotation2d velLimit, Rotation2d accelLim } @Override - public void periodic() { - super.periodic(); - + public void setPivotState(PivotState pivotState) { + super.setPivotState(pivotState); if (getPivotState() == PivotState.STOW) { + SmartDashboard.putString("Intake/Profile Constraints", "S"); setMotionProfileConstraints(Settings.Intake.PIVOT_MAX_VEL_STOW, Settings.Intake.PIVOT_MAX_ACCEL_STOW); } else if (getPivotState() == PivotState.DEPLOY) { + SmartDashboard.putString("Intake/Profile Constraints", "D"); setMotionProfileConstraints(Settings.Intake.PIVOT_MAX_VEL_DEPLOY, Settings.Intake.PIVOT_MAX_ACCEL_DEPLOY); } + } + + @Override + public void periodic() { + super.periodic(); if (EnabledSubsystems.INTAKE.get()) { if (pivotVoltageOverride.isPresent()) { From 52c636ee2e6247b9a398fa5685eed0a83b5622db Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 27 Feb 2026 15:42:10 -0500 Subject: [PATCH 014/150] feat: added bangbang and continued tuning. Added gains for handoff and spindexer --- .../com/stuypulse/robot/RobotContainer.java | 24 ++++++++--------- .../robot/commands/intake/IntakeBangBang.java | 9 +++++++ .../com/stuypulse/robot/constants/Gains.java | 12 ++++----- .../stuypulse/robot/constants/Settings.java | 8 +++--- .../robot/subsystems/handoff/HandoffImpl.java | 6 ++--- .../robot/subsystems/intake/Intake.java | 1 + .../robot/subsystems/intake/IntakeImpl.java | 27 ++++++++++++++++--- .../subsystems/spindexer/SpindexerImpl.java | 3 +-- 8 files changed, 60 insertions(+), 30 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeBangBang.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index e083367c..efd583f2 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -61,6 +61,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; @@ -330,18 +331,17 @@ public void configureSysids() { // 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 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 DF", handoffSysId.dynamic(Direction.kForward)); + autonChooser.addOption("SysID Handoff DB", handoffSysId.dynamic(Direction.kReverse)); + autonChooser.addOption("SysID Handoff QF", handoffSysId.quasistatic(Direction.kForward)); + autonChooser.addOption("SysID Handoff QB", handoffSysId.quasistatic(Direction.kReverse)); } public Command getAutonomousCommand() { diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeBangBang.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeBangBang.java new file mode 100644 index 00000000..4ab99901 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeBangBang.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.intake; + +import com.stuypulse.robot.subsystems.intake.Intake.PivotState; + +public class IntakeBangBang extends IntakeSetState { + public IntakeBangBang() { + super(PivotState.BANGBANG); + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 25bbe41c..675a8343 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -57,9 +57,9 @@ public interface Spindexer { SmartNumber kI = new SmartNumber("Spindexer/Gains/kI", 0.0); SmartNumber kD = new SmartNumber("Spindexer/Gains/kD", 0.0); - SmartNumber kS = new SmartNumber("Spindexer/Gains/kS", 0.019444); + SmartNumber kS = new SmartNumber("Spindexer/Gains/kS", 0.25); SmartNumber kA = new SmartNumber("Spindexer/Gains/kA", 0.010876); - SmartNumber kV = new SmartNumber("Spindexer/Gains/kV", 0.38546); + SmartNumber kV = new SmartNumber("Spindexer/Gains/kV", 0.9413); } public interface Intake { @@ -78,13 +78,13 @@ public interface Pivot { } public interface Handoff { - double kP = 0.00015508; + double kP = 0.00015508; //0.016973 from sysid double kI = 0.0; double kD = 0.0; - double kS = 0.1728; - double kA = 0.0028428; - double kV = 0.11725; + double kS = 0.21149; //0.1728 from alpha + double kA = 0.016329; + double kV = 0.3652; } public interface Turret { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 5c31a45b..6ba9bbb4 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -78,13 +78,13 @@ public interface Handoff { } public interface Intake { - Rotation2d PIVOT_STOW_ANGLE = Rotation2d.fromDegrees(90.0); - Rotation2d PIVOT_DEPLOY_ANGLE = Rotation2d.fromDegrees(0.0); + Rotation2d PIVOT_STOW_ANGLE = Rotation2d.fromDegrees(75.0); + Rotation2d PIVOT_DEPLOY_ANGLE = Rotation2d.fromDegrees(20.0); Rotation2d PIVOT_ANGLE_TOLERANCE = Rotation2d.fromDegrees(3.0); - Rotation2d PIVOT_MAX_ANGLE = Rotation2d.fromDegrees(75); - Rotation2d PIVOT_MIN_ANGLE = Rotation2d.fromDegrees(20); + Rotation2d PIVOT_MAX_ANGLE = Rotation2d.fromDegrees(90.0); //Rotation2d.fromRotations(-0.0) + Rotation2d PIVOT_MIN_ANGLE = Rotation2d.fromDegrees(0.0); // Rotation2d.fromRotations(-0.2) Rotation2d PIVOT_MAX_VEL_DEPLOY = Rotation2d.fromDegrees(360.0); Rotation2d PIVOT_MAX_ACCEL_DEPLOY = Rotation2d.fromDegrees(1200.0); 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 2bb053ad..90baf152 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -47,10 +47,10 @@ public void periodic() { super.periodic(); if (EnabledSubsystems.HANDOFF.get()) { - if (getState() == HandoffState.STOP) { - motor.stopMotor(); - } else if (voltageOverride.isPresent()) { + if (voltageOverride.isPresent()) { motor.setVoltage(voltageOverride.get()); + } else if (getState() == HandoffState.STOP) { + motor.stopMotor(); } else { motor.setControl(controller.withVelocity(getTargetRPM() / 60.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 7975152e..5b6940ad 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -42,6 +42,7 @@ protected Intake() { public enum PivotState { DEPLOY(Settings.Intake.PIVOT_DEPLOY_ANGLE), STOW(Settings.Intake.PIVOT_STOW_ANGLE), + BANGBANG(Settings.Intake.PIVOT_DEPLOY_ANGLE), ANALOG(Settings.Intake.PIVOT_STOW_ANGLE); private final Rotation2d targetAngle; 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 2dae92d2..787279fa 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -12,6 +12,7 @@ import com.stuypulse.robot.util.SettableNumber; import com.stuypulse.robot.util.SysId; +import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -19,18 +20,19 @@ import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.MotorAlignmentValue; import java.util.Optional; -import javax.xml.xpath.XPathExpressionException; - public class IntakeImpl extends Intake { private final TalonFX pivot; private final TalonFX rollerLeader; private final TalonFX rollerFollower; private final MotionMagicVoltage pivotController; + private final BangBangController pivotAggressiveController; + private final DutyCycleOut rollerController; private final Follower follower; @@ -53,6 +55,10 @@ public IntakeImpl() { pivotController = new MotionMagicVoltage(getPivotState().getTargetAngle().getRotations()) .withEnableFOC(true); + + //TODO: add tolerance + pivotAggressiveController = new BangBangController(Settings.Intake.PIVOT_ANGLE_TOLERANCE.getRotations()); + rollerController = new DutyCycleOut(getRollerState().getTargetDutyCycle()) .withEnableFOC(true); follower = new Follower(Ports.Intake.ROLLER_LEADER, MotorAlignmentValue.Aligned); @@ -107,7 +113,15 @@ public void periodic() { else if (pivotVoltageOverride.isPresent()) { pivot.setVoltage(pivotVoltageOverride.get()); - } else { + } + // else if (getPivotState() == PivotState.BANGBANG) { + // pivotAggressiveController.calculate(getPivotAngle().getRotations(), getPivotState().getTargetAngle().getRotations()); + // pivot.setControl(new VelocityVoltage(pivotAggressiveController.getMeasurement() * 12)); + + // TODO: might have to calculate inside of pivot.setcontrol + // //TODO: finish and apply to motor -> motor config conflict + // } + else { // pivot.setControl(pivotController.withPosition(getPivotState().getTargetAngle().getRotations())); rollerLeader.setControl(rollerController.withOutput(getRollerState().getTargetDutyCycle())); rollerFollower.setControl(follower); @@ -128,6 +142,13 @@ else if (pivotVoltageOverride.isPresent()) { SmartDashboard.putNumber("Intake/Pivot Angle Error (deg)", Math.abs(getPivotState().getTargetAngle().getDegrees() - getPivotAngle().getDegrees())); + //BANGBANG + SmartDashboard.putNumber("Intake/Pivot BANGANG tolerance", pivotAggressiveController.getSetpoint()); + SmartDashboard.putNumber("Intake/Pivot BANGANG measurement (0 - 1)", pivotAggressiveController.getMeasurement()); + SmartDashboard.putNumber("Intake/Pivot BANGANG Distance between target and setpoint", pivotAggressiveController.getError()); + //TODO: add whatever was still not added + + // ROLLERS SmartDashboard.putNumber("Intake/Roller Leader Voltage (volts)", rollerLeader.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Intake/Roller Follower Voltage (volts)", rollerFollower.getMotorVoltage().getValueAsDouble()); 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 066489fe..3140dde5 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -86,11 +86,10 @@ public void periodic() { if (EnabledSubsystems.SPINDEXER.get()) { if (voltageOverride.isPresent()){ leadMotor.setVoltage(voltageOverride.get()); - followerMotor.setControl(follower); } else { leadMotor.setControl(controller.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE)); - followerMotor.setControl(follower); } + followerMotor.setControl(follower); } if (Settings.DEBUG_MODE) { From 1791a3d637c8b16e70560b3f38fdcf08940df599 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 27 Feb 2026 19:25:36 -0500 Subject: [PATCH 015/150] feat: intake tuning + climber stuff (REVERT CLIMBER AFTER TESTING IT) --- .../com/stuypulse/robot/RobotContainer.java | 25 +++++++++++++------ .../com/stuypulse/robot/constants/Gains.java | 11 ++++---- .../com/stuypulse/robot/constants/Motors.java | 8 +++--- .../stuypulse/robot/constants/Settings.java | 10 ++++---- .../climberhopper/ClimberHopperImpl.java | 17 +++++++------ .../robot/subsystems/intake/IntakeImpl.java | 14 ++++++++--- .../robot/subsystems/turret/TurretImpl.java | 1 + 7 files changed, 53 insertions(+), 33 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index efd583f2..7ba9539b 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -14,6 +14,7 @@ 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.climberhopper.HopperDown; import com.stuypulse.robot.commands.handoff.HandoffReverse; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; @@ -25,6 +26,7 @@ import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterShoot; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterStow; import com.stuypulse.robot.commands.intake.IntakeAnalog; +import com.stuypulse.robot.commands.intake.IntakeBangBang; import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.intake.IntakeRunRollers; import com.stuypulse.robot.commands.intake.IntakeStopRollers; @@ -70,7 +72,7 @@ public interface EnabledSubsystems { SmartBoolean SWERVE = new SmartBoolean("Enabled Subsystems/Swerve Is Enabled", true); SmartBoolean TURRET = new SmartBoolean("Enabled Subsystems/Turret Is Enabled", false); SmartBoolean HANDOFF = new SmartBoolean("Enabled Subsystems/Handoff Is Enabled", false); - SmartBoolean INTAKE = new SmartBoolean("Enabled Subsystems/Intake Is Enabled", false); + SmartBoolean INTAKE = new SmartBoolean("Enabled Subsystems/Intake Is Enabled", true); SmartBoolean SPINDEXER = new SmartBoolean("Enabled Subsystems/Spindexer Is Enabled", false); SmartBoolean CLIMBER_HOPPER = new SmartBoolean("Enabled Subsystems/Climber-Hopper Is Enabled", false); SmartBoolean HOOD = new SmartBoolean("Enabled Subsystems/Hood Is Enabled", false); @@ -124,14 +126,19 @@ private void configureDefaultCommands() { private void configureButtonBindings() { // Intake Run Rollers - // driver.getLeftTriggerButton() - // .onTrue(new IntakeRunRollers()) - // .onFalse(new IntakeStopRollers()); + driver.getLeftTriggerButton() + .onTrue(new IntakeRunRollers()) + .onFalse(new IntakeStopRollers()); // Intake Down and On // driver.getRightTriggerButton() // .onTrue(new IntakeDeploy()); + //TODO: COMMENT OUT AFTER (POTENTIAL) TESTING + + // driver.getBottomButton() + // .onTrue(new IntakeBangBang()); + driver.getRightButton() .onTrue(new IntakeDeploy()); @@ -238,16 +245,18 @@ private void configureButtonBindings() { // Intake Down and On driver.getRightTriggerButton() .onTrue(new IntakeDeploy()); - +*/ // Climb Down Placeholder driver.getLeftBumper() - .onTrue(new BuzzController(driver).alongWith(new ClimberDown())); + .onTrue(new BuzzController(driver).alongWith(new ClimberDown())) + .onFalse(new HopperDown()); // Climb Up Placeholder driver.getRightBumper() .onTrue(new BuzzController(driver)) - .whileTrue(new ClimberUp()); - + .whileTrue(new ClimberUp()) + .onFalse(new HopperDown()); +/* // Reset Heading driver.getDPadUp() .onTrue(new SwerveResetHeading()); diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 675a8343..76d2a4c5 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -64,16 +64,15 @@ public interface Spindexer { public interface Intake { public interface Pivot { - // commented out values are proposed values found during , need to dampen slightly more though. Also might need to update the max velocity - double kP = 40.0; //450 + double kP = 100.0; double kI = 0.0; - double kD = 0.0; //7.5 + double kD = 10.0; - double kS = 0.5; - double kV = 0.12; //1.2 + double kS = 0.0; + double kV = 0.12; double kA = 0.0; - double kG = 0.0; + double kG = 0.5; } } diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index 43f96839..8e43979a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -41,8 +41,8 @@ public interface ClimberHopper { .withRampRate(Settings.ClimberHopper.RAMP_RATE); public final SoftwareLimitSwitchConfigs SOFT_LIMITS = new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitEnable(true) - .withReverseSoftLimitEnable(true) + .withForwardSoftLimitEnable(false) + .withReverseSoftLimitEnable(false) .withForwardSoftLimitThreshold(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM + Settings.ClimberHopper.Constants.NUM_ROTATIONS_TO_REACH_TOP) .withReverseSoftLimitThreshold(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM); } @@ -126,7 +126,9 @@ public interface Intake { .withKS(Gains.Intake.Pivot.kS) .withKV(Gains.Intake.Pivot.kV) .withKA(Gains.Intake.Pivot.kA) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + .withKG(Gains.Intake.Pivot.kG) + .withGravityType(GravityTypeValue.Arm_Cosine) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign); } public interface Spindexer { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 6ba9bbb4..bc70152a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -64,7 +64,7 @@ public interface Constants { public final double RAMP_RATE = 50.0; - public final double MOTOR_VOLTAGE = 3.5; + public final double MOTOR_VOLTAGE = 12; } public interface Handoff { @@ -78,16 +78,16 @@ public interface Handoff { } public interface Intake { - Rotation2d PIVOT_STOW_ANGLE = Rotation2d.fromDegrees(75.0); - Rotation2d PIVOT_DEPLOY_ANGLE = Rotation2d.fromDegrees(20.0); + Rotation2d PIVOT_STOW_ANGLE = Rotation2d.fromDegrees(90.0); + Rotation2d PIVOT_DEPLOY_ANGLE = Rotation2d.fromDegrees(0.0); Rotation2d PIVOT_ANGLE_TOLERANCE = Rotation2d.fromDegrees(3.0); Rotation2d PIVOT_MAX_ANGLE = Rotation2d.fromDegrees(90.0); //Rotation2d.fromRotations(-0.0) Rotation2d PIVOT_MIN_ANGLE = Rotation2d.fromDegrees(0.0); // Rotation2d.fromRotations(-0.2) - Rotation2d PIVOT_MAX_VEL_DEPLOY = Rotation2d.fromDegrees(360.0); - Rotation2d PIVOT_MAX_ACCEL_DEPLOY = Rotation2d.fromDegrees(1200.0); + Rotation2d PIVOT_MAX_VEL_DEPLOY = Rotation2d.fromDegrees(720.0); + Rotation2d PIVOT_MAX_ACCEL_DEPLOY = Rotation2d.fromDegrees(1440.0); Rotation2d PIVOT_MAX_VEL_STOW = Rotation2d.fromDegrees(360.0); Rotation2d PIVOT_MAX_ACCEL_STOW = Rotation2d.fromDegrees(600.0); 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 fed7859b..43d3c5cb 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java @@ -81,13 +81,16 @@ public void periodic() { 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 { + if(getState() == ClimberHopperState.CLIMBER_DOWN) voltage = -Settings.ClimberHopper.MOTOR_VOLTAGE; + else if(getState() == ClimberHopperState.CLIMBER_UP) voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; + // else if (!atTargetHeight()) { + // if (getCurrentHeight() < getState().getTargetHeight()) { + // voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; + // } else { + // voltage = - Settings.ClimberHopper.MOTOR_VOLTAGE; + // } + // } else { + else { voltage = 0; } } 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 787279fa..00cd0a00 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -20,6 +20,7 @@ import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.MotorAlignmentValue; @@ -122,18 +123,23 @@ else if (pivotVoltageOverride.isPresent()) { // //TODO: finish and apply to motor -> motor config conflict // } else { - // pivot.setControl(pivotController.withPosition(getPivotState().getTargetAngle().getRotations())); + pivot.setControl(new PositionVoltage(getPivotState().getTargetAngle().getRotations())); + //pivot.setControl(pivotController.withPosition(getPivotState().getTargetAngle().getRotations())); rollerLeader.setControl(rollerController.withOutput(getRollerState().getTargetDutyCycle())); rollerFollower.setControl(follower); } } else { - pivot.stopMotor(); - rollerLeader.stopMotor(); - rollerFollower.stopMotor(); + // pivot.stopMotor(); + // rollerLeader.stopMotor(); + // rollerFollower.stopMotor(); } if (Settings.DEBUG_MODE) { // PIVOT + SmartDashboard.putBoolean("Intake/Voltage Override", pivotVoltageOverride.isPresent()); + + SmartDashboard.putNumber("Intake/Debug", getPivotState().getTargetAngle().getRotations()); + SmartDashboard.putNumber("Intake/Pivot Voltage (volts)", pivot.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Intake/Pivot Current (amps)", pivot.getSupplyCurrent().getValueAsDouble()); 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 c5e7016b..942e15fe 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java @@ -138,6 +138,7 @@ public void periodic() { SmartDashboard.putNumber("Turret/Encoder18t Abs Position (Rot)", encoder18t.getAbsolutePosition().getValueAsDouble()); SmartDashboard.putNumber("Turret/Encoder17t Abs Position (Rot)", encoder17t.getAbsolutePosition().getValueAsDouble()); SmartDashboard.putNumber("Turret/Vector Space Position (Rot)", getVectorSpaceAngle().getRotations()); + SmartDashboard.putNumber("Turret/Vector Space Position (Deg)", getVectorSpaceAngle().getDegrees()); 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); From 7f3f378ba27893c7a5a624741ee4ceb6a6d419e4 Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Sat, 28 Feb 2026 03:19:08 -0500 Subject: [PATCH 016/150] REFACTOR: motor configs now initialized in subsystem impls --- .../com/stuypulse/robot/constants/Motors.java | 187 ++++-------------- .../climberhopper/ClimberHopperImpl.java | 42 ++-- .../robot/subsystems/handoff/HandoffImpl.java | 16 +- .../hoodedshooter/hood/HoodImpl.java | 37 +++- .../hoodedshooter/shooter/ShooterImpl.java | 39 ++-- .../robot/subsystems/intake/IntakeImpl.java | 41 +++- .../subsystems/spindexer/SpindexerImpl.java | 45 +++-- .../robot/subsystems/turret/TurretImpl.java | 57 ++++-- 8 files changed, 236 insertions(+), 228 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index 8e43979a..b289641a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -32,158 +32,6 @@ public interface Motors { - public interface ClimberHopper { - public final TalonFXConfig MOTOR = new TalonFXConfig() - .withInvertedValue(InvertedValue.CounterClockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake) - .withCurrentLimitAmps(50.0) - .withSupplyCurrentLimitAmps(50.0) - .withRampRate(Settings.ClimberHopper.RAMP_RATE); - - public final SoftwareLimitSwitchConfigs SOFT_LIMITS = new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitEnable(false) - .withReverseSoftLimitEnable(false) - .withForwardSoftLimitThreshold(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM + Settings.ClimberHopper.Constants.NUM_ROTATIONS_TO_REACH_TOP) - .withReverseSoftLimitThreshold(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM); - } - - 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 { - public final TalonFXConfig HOOD = new TalonFXConfig() - .withCurrentLimitAmps(80.0) - .withRampRate(0.25) - .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.Clockwise_Positive) - .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); - - public final 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); - - 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()); - - public final CANcoderConfiguration HOOD_ENCODER = new CANcoderConfiguration() - .withMagnetSensor(new MagnetSensorConfigs() - .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 { - public final TalonFXConfig ROLLER = new TalonFXConfig() - .withCurrentLimitAmps(60.0) - .withRampRate(0.50) - .withNeutralMode(NeutralModeValue.Coast) - .withInvertedValue(InvertedValue.CounterClockwise_Positive); - - public final TalonFXConfig PIVOT = new TalonFXConfig() - .withCurrentLimitAmps(60.0) - .withRampRate(0.25) - .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.Clockwise_Positive) - .withSensorToMechanismRatio(Settings.Intake.GEAR_RATIO) - .withMotionProfile(Settings.Intake.PIVOT_MAX_VEL_STOW.getRotations(), Settings.Intake.PIVOT_MAX_ACCEL_STOW.getRotations()); - - public final Slot0Configs PIVOT_SLOT_0 = new Slot0Configs() - .withKP(Gains.Intake.Pivot.kP) - .withKI(Gains.Intake.Pivot.kI) - .withKD(Gains.Intake.Pivot.kD) - .withKS(Gains.Intake.Pivot.kS) - .withKV(Gains.Intake.Pivot.kV) - .withKA(Gains.Intake.Pivot.kA) - .withKG(Gains.Intake.Pivot.kG) - .withGravityType(GravityTypeValue.Arm_Cosine) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign); - } - - public interface Spindexer { - public final TalonFXConfig SPINDEXER_LEAD = new TalonFXConfig() - .withCurrentLimitEnable(false) - .withRampRate(0.25) - .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.CounterClockwise_Positive) - .withFFConstants(Gains.Spindexer.kS.get(), Gains.Spindexer.kV.get(), Gains.Spindexer.kA.get(), 0) - .withPIDConstants(Gains.Spindexer.kP.get(), Gains.Spindexer.kI.get(), Gains.Spindexer.kD.get(), 0) - .withSensorToMechanismRatio(Settings.Spindexer.Constants.GEAR_RATIO); - - public final TalonFXConfig SPINDEXER_FOLLOWER = new TalonFXConfig() - .withCurrentLimitEnable(false) - .withRampRate(0.25) - .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.CounterClockwise_Positive) - .withFFConstants(Gains.Spindexer.kS.get(), Gains.Spindexer.kV.get(), Gains.Spindexer.kA.get(), 0) - .withPIDConstants(Gains.Spindexer.kP.get(), Gains.Spindexer.kI.get(), Gains.Spindexer.kD.get(), 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 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); - - public final CANCoderConfig ENCODER_18T = new CANCoderConfig() - .withSensorDirection(SensorDirectionValue.Clockwise_Positive) - .withAbsoluteSensorDiscontinuityPoint(1.0); - - public final SoftwareLimitSwitchConfigs SOFT_LIMITS = new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitEnable(false) - .withReverseSoftLimitEnable(false) - .withForwardSoftLimitThreshold(Settings.Turret.Constants.SoftwareLimit.FORWARD_MAX_ROTATIONS) - .withReverseSoftLimitThreshold(Settings.Turret.Constants.SoftwareLimit.BACKWARDS_MAX_ROTATIONS); - } - public static class CANCoderConfig { private final CANcoderConfiguration configuration = new CANcoderConfiguration(); private final MagnetSensorConfigs magnetSensorConfigs = new MagnetSensorConfigs(); @@ -237,6 +85,7 @@ public static class TalonFXConfig { private final CurrentLimitsConfigs currentLimitsConfigs = new CurrentLimitsConfigs(); private final FeedbackConfigs feedbackConfigs = new FeedbackConfigs(); private final MotionMagicConfigs motionMagicConfigs = new MotionMagicConfigs(); + private final SoftwareLimitSwitchConfigs softwareLimitSwitchConfigs = new SoftwareLimitSwitchConfigs(); private final double[] lastKP = new double[3]; private final double[] lastKI = new double[3]; @@ -258,7 +107,6 @@ public TalonFXConfiguration getConfiguration() { // SMARTNUMBER TUNABLE GAINS FOR TALONFX MOTOR CONTROLLERS // Note that this should ONLY be used during testing/debugging and not for competition code - // Provide a double supplier using () -> {SmartNumberObject}.get(). Use () -> {constant} for terms that do not need to be tuned public void updateGainsConfig(TalonFX motor, int slot, SmartNumber kP, SmartNumber kI, SmartNumber kD, SmartNumber kS, SmartNumber kV, SmartNumber kA) { if (slot != 0 && slot != 1 && slot != 2) { return; @@ -301,7 +149,7 @@ public void updateGainsConfig(TalonFX motor, int slot, SmartNumber kP, SmartNumb lastKS[slot] = currentKS; lastKV[slot] = currentKV; lastKA[slot] = currentKA; - + switch (slot) { case 0: motor.getConfigurator().refresh(this.getConfiguration().Slot0); @@ -372,6 +220,24 @@ public TalonFXConfig withFFConstants(double kS, double kV, double kA, double kG, return this; } + public TalonFXConfig withStaticFeedforwardSign(StaticFeedforwardSignValue staticFeedforwardSign, int slot) { + switch (slot) { + case 0: + slot0Configs.StaticFeedforwardSign = staticFeedforwardSign; + configuration.withSlot0(slot0Configs); + break; + case 1: + slot1Configs.StaticFeedforwardSign = staticFeedforwardSign; + configuration.withSlot1(slot1Configs); + break; + case 2: + slot2Configs.StaticFeedforwardSign = staticFeedforwardSign; + configuration.withSlot2(slot2Configs); + break; + } + return this; + } + public TalonFXConfig withGravityType(GravityTypeValue gravityType) { slot0Configs.GravityType = gravityType; slot1Configs.GravityType = gravityType; @@ -457,6 +323,19 @@ public TalonFXConfig withCurrentLimitEnable(boolean enabled) { return this; } + // SOFTWARE LIMIT CONFIGS + + public TalonFXConfig withSoftLimits(boolean forwardEnable, boolean reverseEnable, double forwardThreshold, double reverseThreshold) { + softwareLimitSwitchConfigs.ForwardSoftLimitEnable = forwardEnable; + softwareLimitSwitchConfigs.ReverseSoftLimitEnable = reverseEnable; + softwareLimitSwitchConfigs.ForwardSoftLimitThreshold = forwardThreshold; + softwareLimitSwitchConfigs.ReverseSoftLimitThreshold = reverseThreshold; + + configuration.withSoftwareLimitSwitch(softwareLimitSwitchConfigs); + + return this; + } + // MOTION MAGIC CONFIGS public TalonFXConfig withMotionProfile(double maxVelocity, double maxAcceleration) { 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 43d3c5cb..cf231eab 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java @@ -16,10 +16,14 @@ import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import java.util.Optional; public class ClimberHopperImpl extends ClimberHopper { + private final Motors.TalonFXConfig climberHopperConfig; + private final TalonFX motor; private final VoltageOut controller; @@ -30,23 +34,32 @@ public class ClimberHopperImpl extends ClimberHopper { public ClimberHopperImpl() { super(); - + + climberHopperConfig = new Motors.TalonFXConfig() + .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) + .withCurrentLimitAmps(50.0) + .withSupplyCurrentLimitAmps(50.0) + .withRampRate(Settings.ClimberHopper.RAMP_RATE) + .withSoftLimits( + false, false, + Settings.ClimberHopper.ROTATIONS_AT_BOTTOM + Settings.ClimberHopper.Constants.NUM_ROTATIONS_TO_REACH_TOP, + Settings.ClimberHopper.ROTATIONS_AT_BOTTOM); + motor = new TalonFX(Ports.ClimberHopper.CLIMBER_HOPPER, Ports.CANIVORE); - Motors.ClimberHopper.MOTOR.configure(motor); - motor.getConfigurator().apply(Motors.ClimberHopper.SOFT_LIMITS); + climberHopperConfig.configure(motor); motor.setPosition(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM); 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(); } - @Override + @Override public boolean getStalling() { return stalling.getAsBoolean(); } @@ -79,22 +92,13 @@ public void periodic() { super.periodic(); if (voltageOverride.isPresent()) { - voltage = voltageOverride.get(); + voltage = voltageOverride.get(); } else { - if(getState() == ClimberHopperState.CLIMBER_DOWN) voltage = -Settings.ClimberHopper.MOTOR_VOLTAGE; - else if(getState() == ClimberHopperState.CLIMBER_UP) voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; - // else if (!atTargetHeight()) { - // if (getCurrentHeight() < getState().getTargetHeight()) { - // voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; - // } else { - // voltage = - Settings.ClimberHopper.MOTOR_VOLTAGE; - // } - // } else { - else { - voltage = 0; - } + if (getState() == ClimberHopperState.CLIMBER_DOWN) voltage = -Settings.ClimberHopper.MOTOR_VOLTAGE; + else if (getState() == ClimberHopperState.CLIMBER_UP) voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; + else voltage = 0; } - + if (EnabledSubsystems.CLIMBER_HOPPER.get()) { motor.setControl(controller.withOutput(voltage)); } else { 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 90baf152..66028391 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -6,6 +6,7 @@ package com.stuypulse.robot.subsystems.handoff; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.constants.Gains; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; @@ -16,17 +17,30 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import java.util.Optional; public class HandoffImpl extends Handoff { + private final Motors.TalonFXConfig handoffConfig; + private final TalonFX motor; private final VelocityVoltage controller; private Optional voltageOverride; public HandoffImpl() { + handoffConfig = new Motors.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); + motor = new TalonFX(Ports.Handoff.HANDOFF, Ports.RIO); - Motors.Handoff.HANDOFF.configure(motor); + handoffConfig.configure(motor); controller = new VelocityVoltage(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE) .withEnableFOC(true); 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 6b8c814e..646ff743 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 @@ -6,6 +6,7 @@ package com.stuypulse.robot.subsystems.hoodedshooter.hood; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.constants.Gains; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; @@ -18,9 +19,16 @@ import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import java.util.Optional; public class HoodImpl extends Hood { + private final Motors.TalonFXConfig hoodConfig; + private final Motors.CANCoderConfig hoodEncoderConfig; + private final TalonFX hoodMotor; private final CANcoder hoodEncoder; @@ -31,15 +39,30 @@ public class HoodImpl extends Hood { private boolean hasUsedAbsoluteEncoder; public HoodImpl() { + hoodConfig = new Motors.TalonFXConfig() + .withCurrentLimitAmps(80.0) + .withRampRate(0.25) + .withNeutralMode(NeutralModeValue.Brake) + .withInvertedValue(InvertedValue.Clockwise_Positive) + .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) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 0) + .withSensorToMechanismRatio(Settings.HoodedShooter.Hood.GEAR_RATIO) + .withSoftLimits( + true, true, + Settings.HoodedShooter.Angles.MAX_ANGLE.getRotations(), + Settings.HoodedShooter.Angles.MIN_ANGLE.getRotations()); + + hoodEncoderConfig = new Motors.CANCoderConfig() + .withSensorDirection(SensorDirectionValue.Clockwise_Positive) + .withAbsoluteSensorDiscontinuityPoint(1.0) + .withMagnetOffset(Settings.HoodedShooter.Hood.ENCODER_OFFSET.getRotations()); + 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); - - hoodMotor.getConfigurator().apply(Motors.HoodedShooter.Hood.SLOT_0); - hoodMotor.getConfigurator().apply(Motors.HoodedShooter.Hood.SOFT_LIMITS); - - hoodEncoder.getConfigurator().apply(Motors.HoodedShooter.Hood.HOOD_ENCODER); + hoodConfig.configure(hoodMotor); + hoodEncoderConfig.configure(hoodEncoder); controller = new PositionVoltage(getTargetAngle().getRotations()) .withEnableFOC(true); @@ -107,5 +130,5 @@ public SysIdRoutine getHoodSysIdRoutine() { getInstance() ); } - + } \ No newline at end of file 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 2a88eff0..2885e19f 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 @@ -6,6 +6,7 @@ package com.stuypulse.robot.subsystems.hoodedshooter.shooter; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.constants.Gains; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; @@ -17,10 +18,14 @@ import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import java.util.Optional; public class ShooterImpl extends Shooter { + private final Motors.TalonFXConfig shooterConfig; + private final TalonFX shooterLeader; private final TalonFX shooterFollower; @@ -30,16 +35,26 @@ public class ShooterImpl extends Shooter { private Optional voltageOverride; public ShooterImpl() { + shooterConfig = new Motors.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); + shooterLeader = new TalonFX(Ports.HoodedShooter.Shooter.MOTOR_LEAD, Ports.RIO); shooterFollower = new TalonFX(Ports.HoodedShooter.Shooter.MOTOR_FOLLOW, Ports.RIO); + shooterConfig.configure(shooterLeader); + shooterConfig.configure(shooterFollower); + shooterController = new VelocityVoltage(getTargetRPM() / 60.0) .withEnableFOC(true); follower = new Follower(Ports.HoodedShooter.Shooter.MOTOR_LEAD, MotorAlignmentValue.Opposed); - Motors.HoodedShooter.Shooter.SHOOTER.configure(shooterLeader); - Motors.HoodedShooter.Shooter.SHOOTER.configure(shooterFollower); - shooterFollower.setControl(follower); voltageOverride = Optional.empty(); @@ -58,7 +73,7 @@ public double getFollowerRPM() { return shooterFollower.getVelocity().getValueAsDouble() * 60.0; } - @Override + @Override public void periodic() { super.periodic(); @@ -77,7 +92,7 @@ public void periodic() { shooterLeader.stopMotor(); shooterFollower.stopMotor(); } - + if (Settings.DEBUG_MODE) { SmartDashboard.putNumber("HoodedShooter/Shooter/Leader Current (amps)", shooterLeader.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("HoodedShooter/Shooter/Follower Current (amps)", shooterFollower.getSupplyCurrent().getValueAsDouble()); @@ -100,13 +115,13 @@ public void setVoltageOverride(Optional voltageOverride) { @Override public SysIdRoutine getShooterSysIdRoutine() { return SysId.getRoutine( - 1, - 5, - "Shooter", - voltage -> setVoltageOverride(Optional.of(voltage)), - () -> shooterLeader.getPosition().getValueAsDouble(), - () -> shooterLeader.getVelocity().getValueAsDouble(), - () -> shooterLeader.getMotorVoltage().getValueAsDouble(), + 1, + 5, + "Shooter", + voltage -> setVoltageOverride(Optional.of(voltage)), + () -> shooterLeader.getPosition().getValueAsDouble(), + () -> shooterLeader.getVelocity().getValueAsDouble(), + () -> shooterLeader.getMotorVoltage().getValueAsDouble(), getInstance() ); } 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 00cd0a00..4021dc79 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -6,6 +6,7 @@ package com.stuypulse.robot.subsystems.intake; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.constants.Gains; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; @@ -21,12 +22,18 @@ import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import java.util.Optional; public class IntakeImpl extends Intake { + private final Motors.TalonFXConfig pivotConfig; + private final Motors.TalonFXConfig rollerConfig; + private final TalonFX pivot; private final TalonFX rollerLeader; private final TalonFX rollerFollower; @@ -43,21 +50,36 @@ public class IntakeImpl extends Intake { private Optional pivotVoltageOverride; public IntakeImpl() { - pivot = new TalonFX(Ports.Intake.PIVOT, Ports.RIO); - Motors.Intake.PIVOT.configure(pivot); + pivotConfig = new Motors.TalonFXConfig() + .withCurrentLimitAmps(60.0) + .withRampRate(0.25) + .withNeutralMode(NeutralModeValue.Brake) + .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) + .withGravityType(GravityTypeValue.Arm_Cosine) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign, 0) + .withSensorToMechanismRatio(Settings.Intake.GEAR_RATIO) + .withMotionProfile(Settings.Intake.PIVOT_MAX_VEL_STOW.getRotations(), Settings.Intake.PIVOT_MAX_ACCEL_STOW.getRotations()); + + rollerConfig = new Motors.TalonFXConfig() + .withCurrentLimitAmps(60.0) + .withRampRate(0.50) + .withNeutralMode(NeutralModeValue.Coast) + .withInvertedValue(InvertedValue.CounterClockwise_Positive); - pivot.getConfigurator().apply(Motors.Intake.PIVOT_SLOT_0); + pivot = new TalonFX(Ports.Intake.PIVOT, Ports.RIO); + pivotConfig.configure(pivot); rollerLeader = new TalonFX(Ports.Intake.ROLLER_LEADER, Ports.RIO); - Motors.Intake.ROLLER.configure(rollerLeader); + rollerConfig.configure(rollerLeader); rollerFollower = new TalonFX(Ports.Intake.ROLLER_FOLLOWER, Ports.RIO); - Motors.Intake.ROLLER.configure(rollerFollower); + rollerConfig.configure(rollerFollower); pivotController = new MotionMagicVoltage(getPivotState().getTargetAngle().getRotations()) .withEnableFOC(true); - //TODO: add tolerance pivotAggressiveController = new BangBangController(Settings.Intake.PIVOT_ANGLE_TOLERANCE.getRotations()); rollerController = new DutyCycleOut(getRollerState().getTargetDutyCycle()) @@ -87,8 +109,8 @@ public Rotation2d getPivotAngle() { public void setMotionProfileConstraints(Rotation2d velLimit, Rotation2d accelLimit) { this.velLimit.set(velLimit.getDegrees()); this.accelLimit.set(accelLimit.getDegrees()); - Motors.Intake.PIVOT.withMotionProfile(velLimit.getRotations(), accelLimit.getRotations()); - Motors.Intake.PIVOT.configure(pivot); + pivotConfig.withMotionProfile(velLimit.getRotations(), accelLimit.getRotations()); + pivotConfig.configure(pivot); } @Override @@ -115,6 +137,7 @@ public void periodic() { else if (pivotVoltageOverride.isPresent()) { pivot.setVoltage(pivotVoltageOverride.get()); } + // else if (getPivotState() == PivotState.BANGBANG) { // pivotAggressiveController.calculate(getPivotAngle().getRotations(), getPivotState().getTargetAngle().getRotations()); // pivot.setControl(new VelocityVoltage(pivotAggressiveController.getMeasurement() * 12)); 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 3140dde5..bc3d3ff8 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -18,10 +18,15 @@ import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import java.util.Optional; public class SpindexerImpl extends Spindexer { + private final Motors.TalonFXConfig spindexerLeadConfig; + private final Motors.TalonFXConfig spindexerFollowerConfig; + private final TalonFX leadMotor; private final TalonFX followerMotor; @@ -31,11 +36,29 @@ public class SpindexerImpl extends Spindexer { private Optional voltageOverride; public SpindexerImpl() { + spindexerLeadConfig = new Motors.TalonFXConfig() + .withCurrentLimitEnable(false) + .withRampRate(0.25) + .withNeutralMode(NeutralModeValue.Brake) + .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withFFConstants(Gains.Spindexer.kS.get(), Gains.Spindexer.kV.get(), Gains.Spindexer.kA.get(), 0) + .withPIDConstants(Gains.Spindexer.kP.get(), Gains.Spindexer.kI.get(), Gains.Spindexer.kD.get(), 0) + .withSensorToMechanismRatio(Settings.Spindexer.Constants.GEAR_RATIO); + + spindexerFollowerConfig = new Motors.TalonFXConfig() + .withCurrentLimitEnable(false) + .withRampRate(0.25) + .withNeutralMode(NeutralModeValue.Brake) + .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withFFConstants(Gains.Spindexer.kS.get(), Gains.Spindexer.kV.get(), Gains.Spindexer.kA.get(), 0) + .withPIDConstants(Gains.Spindexer.kP.get(), Gains.Spindexer.kI.get(), Gains.Spindexer.kD.get(), 0) + .withSensorToMechanismRatio(Settings.Spindexer.Constants.GEAR_RATIO); + leadMotor = new TalonFX(Ports.Spindexer.SPINDEXER_LEAD_MOTOR, Ports.CANIVORE); followerMotor = new TalonFX(Ports.Spindexer.SPINDEXER_FOLLOW_MOTOR, Ports.CANIVORE); - Motors.Spindexer.SPINDEXER_LEAD.configure(leadMotor); - Motors.Spindexer.SPINDEXER_FOLLOWER.configure(followerMotor); + spindexerLeadConfig.configure(leadMotor); + spindexerFollowerConfig.configure(followerMotor); controller = new VelocityVoltage(getTargetRPM()) .withEnableFOC(true); @@ -61,10 +84,10 @@ public boolean atTolerance() { public void periodic() { super.periodic(); - Motors.Spindexer.SPINDEXER_LEAD.updateGainsConfig( - leadMotor, - 0, - Gains.Spindexer.kP, + spindexerLeadConfig.updateGainsConfig( + leadMotor, + 0, + Gains.Spindexer.kP, Gains.Spindexer.kI, Gains.Spindexer.kD, Gains.Spindexer.kS, @@ -72,10 +95,10 @@ public void periodic() { Gains.Spindexer.kA ); - Motors.Spindexer.SPINDEXER_FOLLOWER.updateGainsConfig( - followerMotor, - 0, - Gains.Spindexer.kP, + spindexerFollowerConfig.updateGainsConfig( + followerMotor, + 0, + Gains.Spindexer.kP, Gains.Spindexer.kI, Gains.Spindexer.kD, Gains.Spindexer.kS, @@ -84,7 +107,7 @@ public void periodic() { ); if (EnabledSubsystems.SPINDEXER.get()) { - if (voltageOverride.isPresent()){ + if (voltageOverride.isPresent()) { leadMotor.setVoltage(voltageOverride.get()); } else { leadMotor.setControl(controller.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE)); 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 942e15fe..b47848bc 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java @@ -6,6 +6,7 @@ package com.stuypulse.robot.subsystems.turret; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.constants.Gains; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; @@ -20,9 +21,17 @@ import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import java.util.Optional; public class TurretImpl extends Turret { + private final Motors.TalonFXConfig turretConfig; + private final Motors.CANCoderConfig encoder17tConfig; + private final Motors.CANCoderConfig encoder18tConfig; + private final TalonFX motor; private final CANcoder encoder17t; private final CANcoder encoder18t; @@ -31,16 +40,35 @@ public class TurretImpl extends Turret { private Optional voltageOverride; private final PositionVoltage controller; - public TurretImpl() { + turretConfig = new Motors.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) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 0) + .withSensorToMechanismRatio(Settings.Turret.Constants.GEAR_RATIO_MOTOR_TO_MECH) + .withSoftLimits( + false, false, + Settings.Turret.Constants.SoftwareLimit.FORWARD_MAX_ROTATIONS, + Settings.Turret.Constants.SoftwareLimit.BACKWARDS_MAX_ROTATIONS); + + encoder17tConfig = new Motors.CANCoderConfig() + .withSensorDirection(SensorDirectionValue.Clockwise_Positive) + .withAbsoluteSensorDiscontinuityPoint(1.0); + + encoder18tConfig = new Motors.CANCoderConfig() + .withSensorDirection(SensorDirectionValue.Clockwise_Positive) + .withAbsoluteSensorDiscontinuityPoint(1.0); + 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); - - motor.getConfigurator().apply(Motors.Turret.SLOT_0); - motor.getConfigurator().apply(Motors.Turret.SOFT_LIMITS); + turretConfig.configure(motor); + encoder17tConfig.configure(encoder17t); + encoder18tConfig.configure(encoder18t); seedTurret(); @@ -66,20 +94,20 @@ public void zeroEncoders() { double encoderPos17T = encoder17t.getAbsolutePosition().getValueAsDouble(); double encoderPos18T = encoder18t.getAbsolutePosition().getValueAsDouble(); - encoder17t.getConfigurator().refresh(Motors.Turret.ENCODER_17T.getConfiguration().MagnetSensor); - encoder18t.getConfigurator().refresh(Motors.Turret.ENCODER_18T.getConfiguration().MagnetSensor); + encoder17t.getConfigurator().refresh(encoder17tConfig.getConfiguration().MagnetSensor); + encoder18t.getConfigurator().refresh(encoder18tConfig.getConfiguration().MagnetSensor); - double currentOffset17T = Motors.Turret.ENCODER_17T.getConfiguration().MagnetSensor.MagnetOffset; - double currentOffset18T = Motors.Turret.ENCODER_18T.getConfiguration().MagnetSensor.MagnetOffset; + double currentOffset17T = encoder17tConfig.getConfiguration().MagnetSensor.MagnetOffset; + double currentOffset18T = encoder18tConfig.getConfiguration().MagnetSensor.MagnetOffset; double newOffset17T = currentOffset17T - encoderPos17T; double newOffset18T = currentOffset18T - encoderPos18T; - Motors.Turret.ENCODER_17T.withMagnetOffset(newOffset17T); - Motors.Turret.ENCODER_18T.withMagnetOffset(newOffset18T); + encoder17tConfig.withMagnetOffset(newOffset17T); + encoder18tConfig.withMagnetOffset(newOffset18T); - Motors.Turret.ENCODER_17T.configure(encoder17t); - Motors.Turret.ENCODER_18T.configure(encoder18t); + encoder17tConfig.configure(encoder17t); + encoder18tConfig.configure(encoder18t); } public void seedTurret() { @@ -123,8 +151,7 @@ public void periodic() { SmartDashboard.putNumber("Turret/Delta (deg)", getDelta(getTargetAngle().getDegrees(), getAngle().getDegrees())); SmartDashboard.putNumber("Turret/Actual Target (deg)", actualTargetDeg); - // if (EnabledSubsystems.TURRET.get() && getState() != TurretState.IDLE) { - if(EnabledSubsystems.TURRET.get()) { + if (EnabledSubsystems.TURRET.get()) { if (voltageOverride.isPresent()) { motor.setVoltage(voltageOverride.get()); } else { From a79f5ceb1d2f8f0af7704f4c9be53a7e9e52a53a Mon Sep 17 00:00:00 2001 From: Apetrock24 Date: Sat, 28 Feb 2026 09:46:35 -0500 Subject: [PATCH 017/150] Add: supply and stator current logging --- .../subsystems/climberhopper/ClimberHopperImpl.java | 1 + .../stuypulse/robot/subsystems/handoff/HandoffImpl.java | 1 + .../robot/subsystems/hoodedshooter/hood/HoodImpl.java | 3 ++- .../subsystems/hoodedshooter/shooter/ShooterImpl.java | 4 ++-- .../stuypulse/robot/subsystems/intake/IntakeImpl.java | 9 ++++++--- .../robot/subsystems/spindexer/SpindexerImpl.java | 1 + .../stuypulse/robot/subsystems/turret/TurretImpl.java | 2 ++ 7 files changed, 15 insertions(+), 6 deletions(-) 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 cf231eab..47957af4 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java @@ -112,6 +112,7 @@ public void periodic() { SmartDashboard.putNumber("ClimberHopper/Voltage", voltage); SmartDashboard.putNumber("ClimberHopper/Applied Voltage", motor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("ClimberHopper/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("ClimberHopper/Stator Current", motor.getStatorCurrent().getValueAsDouble()); } } } \ No newline at end of file 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 66028391..5f3b16b1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -74,6 +74,7 @@ public void periodic() { SmartDashboard.putNumber("Handoff/Current (amps)", motor.getStatorCurrent().getValueAsDouble()); SmartDashboard.putNumber("Handoff/Voltage", motor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Handoff/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Handoff/Stator Current", motor.getStatorCurrent().getValueAsDouble()); } } 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 646ff743..733de053 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 @@ -104,12 +104,13 @@ public void periodic() { SmartDashboard.putNumber("HoodedShooter/Hood/Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("HoodedShooter/Hood/Supply Current", hoodMotor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("HoodedShooter/Hood/Stator Current", hoodMotor.getStatorCurrent().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()); } } 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 2885e19f..c56d5fb0 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 @@ -95,7 +95,8 @@ public void periodic() { if (Settings.DEBUG_MODE) { SmartDashboard.putNumber("HoodedShooter/Shooter/Leader Current (amps)", shooterLeader.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("HoodedShooter/Shooter/Follower Current (amps)", shooterFollower.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("HoodedShooter/Shooter/Follower Supply Current (amps)", shooterFollower.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("HoodedShooter/Shooter/Follower Stator Current", shooterFollower.getStatorCurrent().getValueAsDouble()); SmartDashboard.putNumber("HoodedShooter/Shooter/Leader Voltage", shooterLeader.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("HoodedShooter/Shooter/Follower Voltage", shooterFollower.getMotorVoltage().getValueAsDouble()); @@ -104,7 +105,6 @@ public void periodic() { 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/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index 4021dc79..e7f0bcdd 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -164,7 +164,8 @@ else if (pivotVoltageOverride.isPresent()) { SmartDashboard.putNumber("Intake/Debug", getPivotState().getTargetAngle().getRotations()); SmartDashboard.putNumber("Intake/Pivot Voltage (volts)", pivot.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("Intake/Pivot Current (amps)", pivot.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Pivot Supply Current (amps)", pivot.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Pivot Stator Current (amps)", pivot.getStatorCurrent().getValueAsDouble()); SmartDashboard.putNumber("Intake/Pivot Max Velocity Limit (deg/s)", velLimit.get()); SmartDashboard.putNumber("Intake/Pivot Max Accel Limit (deg/s^2)", accelLimit.get()); @@ -182,8 +183,10 @@ else if (pivotVoltageOverride.isPresent()) { SmartDashboard.putNumber("Intake/Roller Leader Voltage (volts)", rollerLeader.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Intake/Roller Follower Voltage (volts)", rollerFollower.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("Intake/Roller Leader Current (amps)", rollerLeader.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("Intake/Roller Follower Current (amps)", rollerFollower.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Roller Leader Supply Current (amps)", rollerLeader.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Roller Leader Stator Current (amps)", rollerLeader.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Roller Follower Supply Current (amps)", rollerFollower.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Roller Follower Stator Current (amps)", rollerFollower.getStatorCurrent().getValueAsDouble()); } } 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 bc3d3ff8..d770c0ba 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -128,6 +128,7 @@ public void periodic() { SmartDashboard.putNumber("Spindexer/Follower Current (amps)", followerMotor.getStatorCurrent().getValueAsDouble()); SmartDashboard.putNumber("Spindexer/Follower Voltage", followerMotor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Spindexer/Follower Supply Current", followerMotor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Spindexer/Follower Stator Current", followerMotor.getStatorCurrent().getValueAsDouble()); } } 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 b47848bc..8895f6bd 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java @@ -169,6 +169,8 @@ public void periodic() { 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); + SmartDashboard.putNumber("Turret/Stator Current", motor.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Turret/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); } } From 0667bbd0a75c0316341b86d9d1e866c37d421779 Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Sat, 28 Feb 2026 10:17:14 -0500 Subject: [PATCH 018/150] ADD: More autons --- simgui.json | 3 +- .../pathplanner/autos/Bottom 1 Cycle (P).auto | 25 ++++++ .../pathplanner/autos/Bottom 2 Cycle (P).auto | 37 +++++++++ .../pathplanner/autos/Bottom 2 Cycle.auto | 37 +++++++++ .../deploy/pathplanner/autos/Box Test.auto | 37 +++++++++ .../deploy/pathplanner/autos/Depot Auto.auto | 25 ++++++ .../pathplanner/autos/Straight Line Test.auto | 19 +++++ .../pathplanner/autos/Top 1 Cycle (P).auto | 25 ++++++ .../pathplanner/autos/Top 2 Cycle (P).auto | 37 +++++++++ .../deploy/pathplanner/autos/Top 2 Cycle.auto | 37 +++++++++ src/main/deploy/pathplanner/navgrid.json | 1 + .../paths/Bottom NZ To Score (P).path | 54 +++++++++++++ .../pathplanner/paths/Bottom NZ To Score.path | 54 +++++++++++++ .../paths/Bottom NZ To Tower Right (P).path | 79 +++++++++++++++++++ .../paths/Bottom NZ To Tower Right.path | 79 +++++++++++++++++++ .../paths/Bottom Score To NZ (P).path | 68 ++++++++++++++++ .../pathplanner/paths/Bottom Score To NZ.path | 68 ++++++++++++++++ .../Bottom Trench Score To Tower Right.path | 59 ++++++++++++++ .../paths/Bottom Trench To NZ (P).path | 68 ++++++++++++++++ .../paths/Bottom Trench To NZ.path | 68 ++++++++++++++++ src/main/deploy/pathplanner/paths/Box 1.path | 54 +++++++++++++ src/main/deploy/pathplanner/paths/Box 2.path | 54 +++++++++++++ src/main/deploy/pathplanner/paths/Box 3.path | 54 +++++++++++++ src/main/deploy/pathplanner/paths/Box 4.path | 54 +++++++++++++ .../paths/Depot To Tower Left.path | 54 +++++++++++++ .../pathplanner/paths/Straight Line.path | 54 +++++++++++++ .../pathplanner/paths/Top Bump To Depot.path | 54 +++++++++++++ .../paths/Top NZ To Score (P).path | 54 +++++++++++++ .../pathplanner/paths/Top NZ To Score.path | 54 +++++++++++++ .../paths/Top NZ To Tower Left (P).path | 75 ++++++++++++++++++ .../paths/Top NZ To Tower Left.path | 75 ++++++++++++++++++ .../paths/Top Score To NZ (P).path | 68 ++++++++++++++++ .../pathplanner/paths/Top Score To NZ.path | 68 ++++++++++++++++ .../paths/Top Trench Score To Tower Left.path | 54 +++++++++++++ .../paths/Top Trench To NZ (P).path | 68 ++++++++++++++++ .../pathplanner/paths/Top Trench To NZ.path | 68 ++++++++++++++++ src/main/deploy/pathplanner/settings.json | 43 ++++++++++ src/main/java/com/stuypulse/robot/Robot.java | 1 + .../com/stuypulse/robot/RobotContainer.java | 53 +++++++++++++ .../PoachingAutons/BottomOneCyclePoach.java | 56 +++++++++++++ .../PoachingAutons/BottomTwoCyclePoach.java | 78 ++++++++++++++++++ .../PoachingAutons/TopOneCyclePoach.java | 56 +++++++++++++ .../PoachingAutons/TopTwoCyclePoach.java | 78 ++++++++++++++++++ .../auton/RegularAutons/BottomTwoCycle.java | 78 ++++++++++++++++++ .../auton/RegularAutons/DepotAuton.java | 55 +++++++++++++ .../auton/RegularAutons/EightFuel.java | 36 +++++++++ .../auton/RegularAutons/TopTwoCycle.java | 78 ++++++++++++++++++ .../robot/commands/auton/Test/BoxTest.java | 23 ++++++ .../hoodedshooter/HoodedShooterKB.java | 14 ++++ .../stuypulse/robot/constants/Settings.java | 4 +- .../hoodedshooter/HoodedShooter.java | 2 +- .../subsystems/hoodedshooter/hood/Hood.java | 4 +- .../hoodedshooter/shooter/Shooter.java | 4 +- .../swerve/CommandSwerveDrivetrain.java | 2 +- 54 files changed, 2498 insertions(+), 9 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Bottom 1 Cycle (P).auto create mode 100644 src/main/deploy/pathplanner/autos/Bottom 2 Cycle (P).auto create mode 100644 src/main/deploy/pathplanner/autos/Bottom 2 Cycle.auto create mode 100644 src/main/deploy/pathplanner/autos/Box Test.auto create mode 100644 src/main/deploy/pathplanner/autos/Depot Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Straight Line Test.auto create mode 100644 src/main/deploy/pathplanner/autos/Top 1 Cycle (P).auto create mode 100644 src/main/deploy/pathplanner/autos/Top 2 Cycle (P).auto create mode 100644 src/main/deploy/pathplanner/autos/Top 2 Cycle.auto create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/Bottom NZ To Score (P).path create mode 100644 src/main/deploy/pathplanner/paths/Bottom NZ To Score.path create mode 100644 src/main/deploy/pathplanner/paths/Bottom NZ To Tower Right (P).path create mode 100644 src/main/deploy/pathplanner/paths/Bottom NZ To Tower Right.path create mode 100644 src/main/deploy/pathplanner/paths/Bottom Score To NZ (P).path create mode 100644 src/main/deploy/pathplanner/paths/Bottom Score To NZ.path create mode 100644 src/main/deploy/pathplanner/paths/Bottom Trench Score To Tower Right.path create mode 100644 src/main/deploy/pathplanner/paths/Bottom Trench To NZ (P).path create mode 100644 src/main/deploy/pathplanner/paths/Bottom Trench To NZ.path create mode 100644 src/main/deploy/pathplanner/paths/Box 1.path create mode 100644 src/main/deploy/pathplanner/paths/Box 2.path create mode 100644 src/main/deploy/pathplanner/paths/Box 3.path create mode 100644 src/main/deploy/pathplanner/paths/Box 4.path create mode 100644 src/main/deploy/pathplanner/paths/Depot To Tower Left.path create mode 100644 src/main/deploy/pathplanner/paths/Straight Line.path create mode 100644 src/main/deploy/pathplanner/paths/Top Bump To Depot.path create mode 100644 src/main/deploy/pathplanner/paths/Top NZ To Score (P).path create mode 100644 src/main/deploy/pathplanner/paths/Top NZ To Score.path create mode 100644 src/main/deploy/pathplanner/paths/Top NZ To Tower Left (P).path create mode 100644 src/main/deploy/pathplanner/paths/Top NZ To Tower Left.path create mode 100644 src/main/deploy/pathplanner/paths/Top Score To NZ (P).path create mode 100644 src/main/deploy/pathplanner/paths/Top Score To NZ.path create mode 100644 src/main/deploy/pathplanner/paths/Top Trench Score To Tower Left.path create mode 100644 src/main/deploy/pathplanner/paths/Top Trench To NZ (P).path create mode 100644 src/main/deploy/pathplanner/paths/Top Trench To NZ.path create mode 100644 src/main/deploy/pathplanner/settings.json create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomOneCyclePoach.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomTwoCyclePoach.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopOneCyclePoach.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopTwoCyclePoach.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/BottomTwoCycle.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/DepotAuton.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/EightFuel.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/TopTwoCycle.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/Test/BoxTest.java create mode 100644 src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterKB.java diff --git a/simgui.json b/simgui.json index d01bfcf5..68c2c46d 100644 --- a/simgui.json +++ b/simgui.json @@ -11,7 +11,8 @@ "/FMSInfo": "FMSInfo", "/SmartDashboard/Autonomous": "String Chooser", "/SmartDashboard/Field": "Field2d", - "/SmartDashboard/Visualizers/ClimberHopper": "Mechanism2d" + "/SmartDashboard/Visualizers/ClimberHopper": "Mechanism2d", + "/SmartDashboard/Visualizers/Turret": "Mechanism2d" }, "windows": { "/SmartDashboard/Field": { diff --git a/src/main/deploy/pathplanner/autos/Bottom 1 Cycle (P).auto b/src/main/deploy/pathplanner/autos/Bottom 1 Cycle (P).auto new file mode 100644 index 00000000..129a618e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Bottom 1 Cycle (P).auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Bottom Trench To NZ (P)" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom NZ To Tower Right (P)" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Bottom 2 Cycle (P).auto b/src/main/deploy/pathplanner/autos/Bottom 2 Cycle (P).auto new file mode 100644 index 00000000..cbe54ab7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Bottom 2 Cycle (P).auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Bottom Trench To NZ (P)" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom NZ To Score (P)" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Score To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom NZ To Tower Right" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Bottom 2 Cycle.auto b/src/main/deploy/pathplanner/autos/Bottom 2 Cycle.auto new file mode 100644 index 00000000..c748089e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Bottom 2 Cycle.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Bottom Trench To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Score To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom NZ To Tower Right" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Box Test.auto b/src/main/deploy/pathplanner/autos/Box Test.auto new file mode 100644 index 00000000..34e6aefd --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Box Test.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Box 1" + } + }, + { + "type": "path", + "data": { + "pathName": "Box 2" + } + }, + { + "type": "path", + "data": { + "pathName": "Box 3" + } + }, + { + "type": "path", + "data": { + "pathName": "Box 4" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Tests", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Depot Auto.auto b/src/main/deploy/pathplanner/autos/Depot Auto.auto new file mode 100644 index 00000000..8e5b974e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Depot Auto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Top Bump To Depot" + } + }, + { + "type": "path", + "data": { + "pathName": "Depot To Tower Left" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Straight Line Test.auto b/src/main/deploy/pathplanner/autos/Straight Line Test.auto new file mode 100644 index 00000000..92bbd21a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Straight Line Test.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Straight Line" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Tests", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Top 1 Cycle (P).auto b/src/main/deploy/pathplanner/autos/Top 1 Cycle (P).auto new file mode 100644 index 00000000..be7edc36 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Top 1 Cycle (P).auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Top Trench To NZ (P)" + } + }, + { + "type": "path", + "data": { + "pathName": "Top NZ To Tower Left (P)" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Top 2 Cycle (P).auto b/src/main/deploy/pathplanner/autos/Top 2 Cycle (P).auto new file mode 100644 index 00000000..c96a26b2 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Top 2 Cycle (P).auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Top Trench To NZ (P)" + } + }, + { + "type": "path", + "data": { + "pathName": "Top NZ To Score (P)" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Score To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Top NZ To Tower Left" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Top 2 Cycle.auto b/src/main/deploy/pathplanner/autos/Top 2 Cycle.auto new file mode 100644 index 00000000..667286b6 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Top 2 Cycle.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Top Trench To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Top NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Score To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Top NZ To Tower Left" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 00000000..ac5f5216 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom NZ To Score (P).path b/src/main/deploy/pathplanner/paths/Bottom NZ To Score (P).path new file mode 100644 index 00000000..9f8790a0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bottom NZ To Score (P).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.28075919335706, + "y": 3.6638078291814944 + }, + "prevControl": null, + "nextControl": { + "x": 7.0757295373665485, + "y": 0.1778291814946611 + }, + "isLocked": false, + "linkedName": "NZ Bottom Center (P)" + }, + { + "anchor": { + "x": 4.370362595419848, + "y": 0.4282538167938924 + }, + "prevControl": { + "x": 6.871304863582444, + "y": 0.3392170818505338 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Bottom Trench Score" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom NZ To Score.path b/src/main/deploy/pathplanner/paths/Bottom NZ To Score.path new file mode 100644 index 00000000..c7e4179c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bottom NZ To Score.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.8073546856465015, + "y": 3.696085409252669 + }, + "prevControl": null, + "nextControl": { + "x": 6.763712930011864, + "y": 0.32845788849347635 + }, + "isLocked": false, + "linkedName": "NZ Bottom Center" + }, + { + "anchor": { + "x": 4.370362595419848, + "y": 0.4282538167938924 + }, + "prevControl": { + "x": 7.274169847328244, + "y": 0.47009541984732894 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Bottom Trench Score" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom NZ To Tower Right (P).path b/src/main/deploy/pathplanner/paths/Bottom NZ To Tower Right (P).path new file mode 100644 index 00000000..f14aad0f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bottom NZ To Tower Right (P).path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.28075919335706, + "y": 3.6638078291814944 + }, + "prevControl": null, + "nextControl": { + "x": 7.333950177935943, + "y": 0.41453143534993897 + }, + "isLocked": false, + "linkedName": "NZ Bottom Center (P)" + }, + { + "anchor": { + "x": 4.579596678529064, + "y": 0.43604982206405685 + }, + "prevControl": { + "x": 6.882064056939502, + "y": 0.4468090154211143 + }, + "nextControl": { + "x": 2.9648389542439793, + "y": 0.4285042252216036 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.1258956109134053, + "y": 2.5448517200474496 + }, + "prevControl": { + "x": 2.1695373665480435, + "y": 0.4037722419928822 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Tower Right 1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0510046367851622, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.3261205564142173, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "To Tower", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom NZ To Tower Right.path b/src/main/deploy/pathplanner/paths/Bottom NZ To Tower Right.path new file mode 100644 index 00000000..793b2f81 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bottom NZ To Tower Right.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.8073546856465015, + "y": 3.696085409252669 + }, + "prevControl": null, + "nextControl": { + "x": 6.5485290628707, + "y": 0.4683274021352313 + }, + "isLocked": false, + "linkedName": "NZ Bottom Center" + }, + { + "anchor": { + "x": 4.579596678529064, + "y": 0.43604982206405685 + }, + "prevControl": { + "x": 6.558452312157588, + "y": 0.5020116765183412 + }, + "nextControl": { + "x": 2.965717674970344, + "y": 0.382253855278766 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.1258956109134053, + "y": 2.5448517200474496 + }, + "prevControl": { + "x": 2.1695373665480435, + "y": 0.4037722419928822 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Tower Right 1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0510046367851622, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.3261205564142173, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "To Tower", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Score To NZ (P).path b/src/main/deploy/pathplanner/paths/Bottom Score To NZ (P).path new file mode 100644 index 00000000..5bcab21d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bottom Score To NZ (P).path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.370362595419848, + "y": 0.4282538167938924 + }, + "prevControl": null, + "nextControl": { + "x": 8.807959667852908, + "y": 0.4898457888493476 + }, + "isLocked": false, + "linkedName": "Bottom Trench Score" + }, + { + "anchor": { + "x": 8.28075919335706, + "y": 3.6638078291814944 + }, + "prevControl": { + "x": 8.22218094908225, + "y": 0.7767372184944703 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "NZ Bottom Center (P)" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7228003784295178, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.5, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Score To NZ.path b/src/main/deploy/pathplanner/paths/Bottom Score To NZ.path new file mode 100644 index 00000000..66693c25 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bottom Score To NZ.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.370362595419848, + "y": 0.4282538167938924 + }, + "prevControl": null, + "nextControl": { + "x": 8.248481613285884, + "y": 0.4468090154211143 + }, + "isLocked": false, + "linkedName": "Bottom Trench Score" + }, + { + "anchor": { + "x": 7.8073546856465015, + "y": 3.696085409252669 + }, + "prevControl": { + "x": 7.748776441371692, + "y": 0.8090147985656451 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "NZ Bottom Center" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7228003784295178, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.5, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Trench Score To Tower Right.path b/src/main/deploy/pathplanner/paths/Bottom Trench Score To Tower Right.path new file mode 100644 index 00000000..f31aaaac --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bottom Trench Score To Tower Right.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.370362595419848, + "y": 0.4282538167938924 + }, + "prevControl": null, + "nextControl": { + "x": 1.3195610913404519, + "y": 0.6404744958481603 + }, + "isLocked": false, + "linkedName": "Bottom Trench Score" + }, + { + "anchor": { + "x": 1.1258956109134053, + "y": 2.5448517200474496 + }, + "prevControl": { + "x": 1.534744958481614, + "y": 1.3183036773428227 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Tower Right 1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.21947449768161098, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "To Tower", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Trench To NZ (P).path b/src/main/deploy/pathplanner/paths/Bottom Trench To NZ (P).path new file mode 100644 index 00000000..fd178db9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bottom Trench To NZ (P).path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.370362595419848, + "y": 0.411517175572518 + }, + "prevControl": null, + "nextControl": { + "x": 8.689608540925267, + "y": 0.47908659549228905 + }, + "isLocked": false, + "linkedName": "Bottom Trench Auto Start" + }, + { + "anchor": { + "x": 8.28075919335706, + "y": 3.6638078291814944 + }, + "prevControl": { + "x": 8.205444839857652, + "y": 0.3714946619217079 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "NZ Bottom Center (P)" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6830652790917692, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.5, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Trench To NZ.path b/src/main/deploy/pathplanner/paths/Bottom Trench To NZ.path new file mode 100644 index 00000000..2c3fbfe8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bottom Trench To NZ.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.370362595419848, + "y": 0.411517175572518 + }, + "prevControl": null, + "nextControl": { + "x": 7.968742586002373, + "y": 0.4468090154211143 + }, + "isLocked": false, + "linkedName": "Bottom Trench Auto Start" + }, + { + "anchor": { + "x": 7.8073546856465015, + "y": 3.696085409252669 + }, + "prevControl": { + "x": 7.936465005931199, + "y": 0.7157888493475686 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "NZ Bottom Center" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6830652790917692, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.5, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Box 1.path b/src/main/deploy/pathplanner/paths/Box 1.path new file mode 100644 index 00000000..fa4bd833 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Box 1.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 2.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0000000000000018, + "y": 2.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.0, + "y": 2.0 + }, + "prevControl": { + "x": 2.0, + "y": 2.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Tests", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Box 2.path b/src/main/deploy/pathplanner/paths/Box 2.path new file mode 100644 index 00000000..c539bd92 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Box 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.0, + "y": 2.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.003409325748791, + "y": 1.720136381756153 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.0, + "y": 1.0 + }, + "prevControl": { + "x": 3.0045204355743644, + "y": 1.6922222258449668 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Tests", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Box 3.path b/src/main/deploy/pathplanner/paths/Box 3.path new file mode 100644 index 00000000..53675b86 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Box 3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.0, + "y": 1.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.77619604200323, + "y": 0.9977414835927592 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 1.0 + }, + "prevControl": { + "x": 3.0717077172960874, + "y": 0.9943971527224802 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Tests", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Box 4.path b/src/main/deploy/pathplanner/paths/Box 4.path new file mode 100644 index 00000000..69af045f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Box 4.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 1.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.0299091229524393, + "y": 1.2482044406617574 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 2.0 + }, + "prevControl": { + "x": 2.0255951320775263, + "y": 1.751313673045875 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Tests", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot To Tower Left.path b/src/main/deploy/pathplanner/paths/Depot To Tower Left.path new file mode 100644 index 00000000..1e1adf3a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Depot To Tower Left.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.6632502965545264, + "y": 5.968082061068702 + }, + "prevControl": null, + "nextControl": { + "x": 2.4169988137603804, + "y": 6.020071174377224 + }, + "isLocked": false, + "linkedName": "Depot Collect" + }, + { + "anchor": { + "x": 1.1043772241992889, + "y": 4.933392645314354 + }, + "prevControl": { + "x": 2.051186239620404, + "y": 5.030225385527876 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Tower Left 1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Misc", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Straight Line.path b/src/main/deploy/pathplanner/paths/Straight Line.path new file mode 100644 index 00000000..bb367655 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Straight Line.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.48276393831554, + "y": 7.0 + }, + "prevControl": { + "x": 3.48276393831554, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Tests", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Bump To Depot.path b/src/main/deploy/pathplanner/paths/Top Bump To Depot.path new file mode 100644 index 00000000..dfe32110 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top Bump To Depot.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.7296204033160913, + "y": 5.968082061068702 + }, + "prevControl": null, + "nextControl": { + "x": 3.9920262726131046, + "y": 5.956602493249752 + }, + "isLocked": false, + "linkedName": "Top Bump Start" + }, + { + "anchor": { + "x": 0.6632502965545264, + "y": 5.968082061068702 + }, + "prevControl": { + "x": 1.613412361420885, + "y": 5.968117899160344 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Depot Collect" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Misc", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top NZ To Score (P).path b/src/main/deploy/pathplanner/paths/Top NZ To Score (P).path new file mode 100644 index 00000000..cd31ab00 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top NZ To Score (P).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.259240806642943, + "y": 4.43846975088968 + }, + "prevControl": null, + "nextControl": { + "x": 6.849786476868328, + "y": 7.956725978647688 + }, + "isLocked": false, + "linkedName": "NZ Top Center (P)" + }, + { + "anchor": { + "x": 3.5921087786259545, + "y": 7.63337786259542 + }, + "prevControl": { + "x": 6.623843416370107, + "y": 7.655468564650059 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Top Trench Score" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top NZ To Score.path b/src/main/deploy/pathplanner/paths/Top NZ To Score.path new file mode 100644 index 00000000..fac618a4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top NZ To Score.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.785836298932384, + "y": 4.4169513641755636 + }, + "prevControl": null, + "nextControl": { + "x": 6.882064056939502, + "y": 7.924448398576512 + }, + "isLocked": false, + "linkedName": "NZ Top Center" + }, + { + "anchor": { + "x": 3.5921087786259545, + "y": 7.63337786259542 + }, + "prevControl": { + "x": 6.623843416370107, + "y": 7.655468564650059 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Top Trench Score" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top NZ To Tower Left (P).path b/src/main/deploy/pathplanner/paths/Top NZ To Tower Left (P).path new file mode 100644 index 00000000..e490fd44 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top NZ To Tower Left (P).path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.259240806642943, + "y": 4.43846975088968 + }, + "prevControl": null, + "nextControl": { + "x": 7.097247924080665, + "y": 7.763060498220641 + }, + "isLocked": false, + "linkedName": "NZ Top Center (P)" + }, + { + "anchor": { + "x": 4.558078291814947, + "y": 7.655468564650059 + }, + "prevControl": { + "x": 6.73143534994069, + "y": 7.666227758007118 + }, + "nextControl": { + "x": 2.6534508870348223, + "y": 7.646039716111543 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.1043772241992889, + "y": 4.933392645314354 + }, + "prevControl": { + "x": 2.3524436536180313, + "y": 7.3434519572953745 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Tower Left 1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.15, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Tower", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top NZ To Tower Left.path b/src/main/deploy/pathplanner/paths/Top NZ To Tower Left.path new file mode 100644 index 00000000..ff5882dd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top NZ To Tower Left.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.785836298932384, + "y": 4.4169513641755636 + }, + "prevControl": null, + "nextControl": { + "x": 7.333950177935943, + "y": 7.859893238434164 + }, + "isLocked": false, + "linkedName": "NZ Top Center" + }, + { + "anchor": { + "x": 4.558078291814947, + "y": 7.655468564650059 + }, + "prevControl": { + "x": 5.980083146752088, + "y": 7.631366787447734 + }, + "nextControl": { + "x": 2.653701067615659, + "y": 7.687746144721235 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.1043772241992889, + "y": 4.933392645314354 + }, + "prevControl": { + "x": 2.5783867141162524, + "y": 6.9668801897983395 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Tower Left 1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.15, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Tower", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Score To NZ (P).path b/src/main/deploy/pathplanner/paths/Top Score To NZ (P).path new file mode 100644 index 00000000..89e1f1cf --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top Score To NZ (P).path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5921087786259545, + "y": 7.63337786259542 + }, + "prevControl": null, + "nextControl": { + "x": 8.50670225385528, + "y": 7.515599051008303 + }, + "isLocked": false, + "linkedName": "Top Trench Score" + }, + { + "anchor": { + "x": 8.259240806642943, + "y": 4.43846975088968 + }, + "prevControl": { + "x": 8.366832740213523, + "y": 7.6985053380782915 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "NZ Top Center (P)" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7587511825922443, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.5, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Score To NZ.path b/src/main/deploy/pathplanner/paths/Top Score To NZ.path new file mode 100644 index 00000000..31cf5bb4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top Score To NZ.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5921087786259545, + "y": 7.63337786259542 + }, + "prevControl": null, + "nextControl": { + "x": 8.32379596678529, + "y": 7.569395017793594 + }, + "isLocked": false, + "linkedName": "Top Trench Score" + }, + { + "anchor": { + "x": 7.785836298932384, + "y": 4.4169513641755636 + }, + "prevControl": { + "x": 7.763125401827354, + "y": 7.670407928336639 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "NZ Top Center" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7587511825922443, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.5, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Trench Score To Tower Left.path b/src/main/deploy/pathplanner/paths/Top Trench Score To Tower Left.path new file mode 100644 index 00000000..d5821799 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top Trench Score To Tower Left.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5921087786259545, + "y": 7.63337786259542 + }, + "prevControl": null, + "nextControl": { + "x": 1.4326293622109532, + "y": 6.100108466886093 + }, + "isLocked": false, + "linkedName": "Top Trench Score" + }, + { + "anchor": { + "x": 1.1043772241992889, + "y": 4.933392645314354 + }, + "prevControl": { + "x": 1.0438041131203828, + "y": 5.495921921420095 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Tower Left 1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Tower", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Trench To NZ (P).path b/src/main/deploy/pathplanner/paths/Top Trench To NZ (P).path new file mode 100644 index 00000000..b294751f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top Trench To NZ (P).path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.030877817319099, + "y": 7.633950177935944 + }, + "prevControl": null, + "nextControl": { + "x": 8.67884934756821, + "y": 7.590913404507711 + }, + "isLocked": false, + "linkedName": "Top Trench Auto Start" + }, + { + "anchor": { + "x": 8.259240806642943, + "y": 4.43846975088968 + }, + "prevControl": { + "x": 8.302277580071175, + "y": 7.8060972716488735 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "NZ Top Center (P)" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7152317880794704, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.5, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Trench To NZ.path b/src/main/deploy/pathplanner/paths/Top Trench To NZ.path new file mode 100644 index 00000000..b4045124 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top Trench To NZ.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.030877817319099, + "y": 7.633950177935944 + }, + "prevControl": null, + "nextControl": { + "x": 8.226963226571769, + "y": 7.590913404507711 + }, + "isLocked": false, + "linkedName": "Top Trench Auto Start" + }, + { + "anchor": { + "x": 7.785836298932384, + "y": 4.4169513641755636 + }, + "prevControl": { + "x": 7.882669039145908, + "y": 7.730782918149465 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "NZ Top Center" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7152317880794704, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.5, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 00000000..0fb0623e --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,43 @@ +{ + "robotWidth": 0.84, + "robotLength": 0.66, + "holonomicMode": true, + "pathFolders": [ + "Misc", + "Tests", + "To NZ", + "To Score", + "To Tower" + ], + "autoFolders": [ + "Tests" + ], + "defaultMaxVel": 5.75, + "defaultMaxAccel": 6.75, + "defaultMaxAngVel": 400.0, + "defaultMaxAngAccel": 900.0, + "defaultNominalVoltage": 12.0, + "robotMass": 49.4, + "robotMOI": 6.883, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.051, + "driveGearing": 5.35714285714, + "maxDriveSpeed": 4.93, + "driveMotorType": "krakenX60FOC", + "driveCurrentLimit": 120.0, + "wheelCOF": 1.542, + "flModuleX": 0.2, + "flModuleY": 0.12, + "frModuleX": 0.2, + "frModuleY": -0.3, + "blModuleX": -0.2, + "blModuleY": 0.12, + "brModuleX": -0.2, + "brModuleY": -0.3, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [ + "{\"name\":\"Intake\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.475,\"y\":0.0},\"size\":{\"width\":0.84,\"length\":0.29},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":true}}", + "{\"name\":\"Turret\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":-0.1,\"y\":0.2},\"size\":{\"width\":0.3,\"length\":0.35},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}" + ] +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 4fa0dcc5..5b78a442 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -72,6 +72,7 @@ public void disabledPeriodic() {} @Override public void autonomousInit() { + CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); auto = robot.getAutonomousCommand(); diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index b7124041..1310ae8e 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -11,6 +11,15 @@ import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.auton.DoNothingAuton; +import com.stuypulse.robot.commands.auton.PoachingAutons.BottomOneCyclePoach; +import com.stuypulse.robot.commands.auton.PoachingAutons.BottomTwoCyclePoach; +import com.stuypulse.robot.commands.auton.PoachingAutons.TopOneCyclePoach; +import com.stuypulse.robot.commands.auton.PoachingAutons.TopTwoCyclePoach; +import com.stuypulse.robot.commands.auton.RegularAutons.BottomTwoCycle; +import com.stuypulse.robot.commands.auton.RegularAutons.DepotAuton; +import com.stuypulse.robot.commands.auton.RegularAutons.EightFuel; +import com.stuypulse.robot.commands.auton.RegularAutons.TopTwoCycle; +import com.stuypulse.robot.commands.auton.Test.BoxTest; import com.stuypulse.robot.commands.climberhopper.ClimberDown; import com.stuypulse.robot.commands.climberhopper.ClimberHopperDefaultCommand; import com.stuypulse.robot.commands.climberhopper.ClimberUp; @@ -51,6 +60,7 @@ import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.subsystems.turret.Turret; import com.stuypulse.robot.subsystems.vision.LimelightVision; +import com.stuypulse.robot.util.PathUtil.AutonConfig; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -271,9 +281,52 @@ private void configureButtonBindings() { /**************/ public void configureAutons() { + autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); + // TESTS + AutonConfig BOX_TEST = new AutonConfig("Box Test", BoxTest::new, + "Box 1", "Box 2", "Box 3", "Box 4"); + BOX_TEST.register(autonChooser); + + // // BASE + // AutonConfig EIGHT_FUEL = new AutonConfig("Eight Fuel", EightFuel::new, + // ""); + // EIGHT_FUEL.register(autonChooser); + + // // DEPOT + // AutonConfig DEPOT_AUTON = new AutonConfig("Depot Auton", DepotAuton::new, + // "Top Bump To Depot", "Depot To Tower Left"); + // DEPOT_AUTON.register(autonChooser); + + // // ONE CYCLES + // AutonConfig TOP_ONE_CYCLE_POACH = new AutonConfig("Top One Cycle (Poach)", TopOneCyclePoach::new, + // "Top Trench To NZ (P)", "Top NZ To Tower Left (P)"); + // TOP_ONE_CYCLE_POACH.register(autonChooser); + + // AutonConfig BOTTOM_ONE_CYCLE_POACH = new AutonConfig("Bottom One Cycle (Poach)", BottomOneCyclePoach::new, + // "Bottom Trench To NZ (P)", "Bottom NZ To Tower Right (P)"); + // BOTTOM_ONE_CYCLE_POACH.register(autonChooser); + + // // TWO CYCLES + // AutonConfig TOP_TWO_CYCLE = new AutonConfig("Top Two Cycle", TopTwoCycle::new, + // "Top Trench To NZ", "Top NZ To Score", "Top Score To NZ", "Top NZ To Tower Left"); + // TOP_TWO_CYCLE.register(autonChooser); + + // AutonConfig BOTTOM_TWO_CYCLE = new AutonConfig("Bottom Two Cycle", BottomTwoCycle::new, + // "Bottom Trench To NZ", "Bottom NZ To Score", "Bottom Score To NZ", "Bottom NZ To Tower Right"); + // BOTTOM_TWO_CYCLE.register(autonChooser); + + // AutonConfig TOP_TWO_CYCLE_POACH = new AutonConfig("Top Two Cycle (Poach)", TopTwoCyclePoach::new, + // "Top Trench To NZ (P)", "Top NZ To Score (P)", "Top Score To NZ", "Top NZ To Tower Left"); + // TOP_TWO_CYCLE_POACH.register(autonChooser); + + // AutonConfig BOTTOM_TWO_CYCLE_POACH = new AutonConfig("Bottom Two Cycle (Poach)", BottomTwoCyclePoach::new, + // "Bottom Trench To NZ (P)", "Bottom NZ To Score (P)", "Bottom Score To NZ", "Bottom NZ To Tower Right"); + // BOTTOM_TWO_CYCLE_POACH.register(autonChooser); + SmartDashboard.putData("Autonomous", autonChooser); + } public void configureSysids() { diff --git a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomOneCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomOneCyclePoach.java new file mode 100644 index 00000000..c4980e03 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomOneCyclePoach.java @@ -0,0 +1,56 @@ +package com.stuypulse.robot.commands.auton.PoachingAutons; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.climberhopper.ClimberDown; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +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; +import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; +import com.stuypulse.robot.subsystems.spindexer.Spindexer; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class BottomOneCyclePoach extends SequentialCommandGroup { + + public BottomOneCyclePoach(PathPlannerPath... paths) { + + addCommands( + + // NZ Trip 1 + new IntakeDeploy().alongWith( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new IntakeStow() + ), + new ParallelCommandGroup( + new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()), + new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()), + new SwerveClimbAlign() + ), + new SpindexerRun().alongWith( + new HandoffRun() + ).until(() -> DriverStation.getMatchTime() < 2).andThen( + new ParallelCommandGroup( + new HandoffStop(), + new SpindexerStop(), + new ClimberDown() + ) + ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomTwoCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomTwoCyclePoach.java new file mode 100644 index 00000000..d4e7f26c --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomTwoCyclePoach.java @@ -0,0 +1,78 @@ +package com.stuypulse.robot.commands.auton.PoachingAutons; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.climberhopper.ClimberDown; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +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; +import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; +import com.stuypulse.robot.subsystems.spindexer.Spindexer; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class BottomTwoCyclePoach extends SequentialCommandGroup { + + public BottomTwoCyclePoach(PathPlannerPath... paths) { + + addCommands( + + // NZ Trip 1 + new IntakeDeploy().alongWith( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new IntakeStow() + ), + new ParallelCommandGroup( + new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()), + new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()) + ), + new SpindexerRun().alongWith( + new HandoffRun() + ).withTimeout(5.0), + + // NZ Trip 2 + new IntakeDeploy().alongWith( + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ) + ), + + // Trip 2 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).alongWith( + new IntakeStow() + ), + new ParallelCommandGroup( + new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()), + new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()), + new SwerveClimbAlign() + ), + new SpindexerRun().alongWith( + new HandoffRun() + ).until(() -> DriverStation.getMatchTime() < 2).andThen( + new ParallelCommandGroup( + new HandoffStop(), + new SpindexerStop(), + new ClimberDown() + ) + ) + + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopOneCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopOneCyclePoach.java new file mode 100644 index 00000000..ae4fd460 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopOneCyclePoach.java @@ -0,0 +1,56 @@ +package com.stuypulse.robot.commands.auton.PoachingAutons; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.climberhopper.ClimberDown; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +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; +import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; +import com.stuypulse.robot.subsystems.spindexer.Spindexer; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class TopOneCyclePoach extends SequentialCommandGroup { + + public TopOneCyclePoach(PathPlannerPath... paths) { + + addCommands( + + // NZ Trip 1 + new IntakeDeploy().alongWith( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new IntakeStow() + ), + new ParallelCommandGroup( + new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()), + new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()), + new SwerveClimbAlign() + ), + new SpindexerRun().alongWith( + new HandoffRun() + ).until(() -> DriverStation.getMatchTime() < 2).andThen( + new ParallelCommandGroup( + new HandoffStop(), + new SpindexerStop(), + new ClimberDown() + ) + ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopTwoCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopTwoCyclePoach.java new file mode 100644 index 00000000..b105937b --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopTwoCyclePoach.java @@ -0,0 +1,78 @@ +package com.stuypulse.robot.commands.auton.PoachingAutons; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.climberhopper.ClimberDown; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +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; +import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; +import com.stuypulse.robot.subsystems.spindexer.Spindexer; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class TopTwoCyclePoach extends SequentialCommandGroup { + + public TopTwoCyclePoach(PathPlannerPath... paths) { + + addCommands( + + // NZ Trip 1 + new IntakeDeploy().alongWith( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new IntakeStow() + ), + new ParallelCommandGroup( + new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()), + new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()) + ), + new SpindexerRun().alongWith( + new HandoffRun() + ).withTimeout(5.0), + + // NZ Trip 2 + new IntakeDeploy().alongWith( + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ) + ), + + // Trip 2 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).alongWith( + new IntakeStow() + ), + new ParallelCommandGroup( + new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()), + new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()), + new SwerveClimbAlign() + ), + new SpindexerRun().alongWith( + new HandoffRun() + ).until(() -> DriverStation.getMatchTime() < 2).andThen( + new ParallelCommandGroup( + new HandoffStop(), + new SpindexerStop(), + new ClimberDown() + ) + ) + + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/BottomTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/BottomTwoCycle.java new file mode 100644 index 00000000..08234a92 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/BottomTwoCycle.java @@ -0,0 +1,78 @@ +package com.stuypulse.robot.commands.auton.RegularAutons; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.climberhopper.ClimberDown; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +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; +import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; +import com.stuypulse.robot.subsystems.spindexer.Spindexer; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class BottomTwoCycle extends SequentialCommandGroup { + + public BottomTwoCycle(PathPlannerPath... paths) { + + addCommands( + + // NZ Trip 1 + new IntakeDeploy().alongWith( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new IntakeStow() + ), + new ParallelCommandGroup( + new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()), + new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()) + ), + new SpindexerRun().alongWith( + new HandoffRun() + ).withTimeout(5.0), + + // NZ Trip 2 + new IntakeDeploy().alongWith( + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ) + ), + + // Trip 2 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).alongWith( + new IntakeStow() + ), + new ParallelCommandGroup( + new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()), + new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()), + new SwerveClimbAlign() + ), + new SpindexerRun().alongWith( + new HandoffRun() + ).until(() -> DriverStation.getMatchTime() < 2).andThen( + new ParallelCommandGroup( + new HandoffStop(), + new SpindexerStop(), + new ClimberDown() + ) + ) + + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/DepotAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/DepotAuton.java new file mode 100644 index 00000000..7ed1909a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/DepotAuton.java @@ -0,0 +1,55 @@ +package com.stuypulse.robot.commands.auton.RegularAutons; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.climberhopper.ClimberDown; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +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; +import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; +import com.stuypulse.robot.subsystems.spindexer.Spindexer; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class DepotAuton extends SequentialCommandGroup { + + public DepotAuton(PathPlannerPath... paths) { + + addCommands( + + // To Depot + new IntakeDeploy().alongWith( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + ), + + new IntakeStow().alongWith( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]) + ), + + new ParallelCommandGroup( + new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()), + new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()) + ), + new SpindexerRun().alongWith( + new HandoffRun() + ).until(() -> DriverStation.getMatchTime() < 2).andThen( + new ParallelCommandGroup( + new HandoffStop(), + new SpindexerStop(), + new ClimberDown() + ) + ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/EightFuel.java b/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/EightFuel.java new file mode 100644 index 00000000..f774dc1c --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/EightFuel.java @@ -0,0 +1,36 @@ +package com.stuypulse.robot.commands.auton.RegularAutons; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterKB; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class EightFuel extends SequentialCommandGroup { + + public EightFuel(PathPlannerPath... paths) { + + addCommands( + + new HoodedShooterKB().alongWith( + new ParallelCommandGroup( + + new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()), + new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()) + + ) + ), + + new SpindexerRun().alongWith( + new HandoffRun() + ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/TopTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/TopTwoCycle.java new file mode 100644 index 00000000..f87b916c --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/TopTwoCycle.java @@ -0,0 +1,78 @@ +package com.stuypulse.robot.commands.auton.RegularAutons; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.climberhopper.ClimberDown; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +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; +import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; +import com.stuypulse.robot.subsystems.spindexer.Spindexer; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class TopTwoCycle extends SequentialCommandGroup { + + public TopTwoCycle(PathPlannerPath... paths) { + + addCommands( + + // NZ Trip 1 + new IntakeDeploy().alongWith( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new IntakeStow() + ), + new ParallelCommandGroup( + new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()), + new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()) + ), + new SpindexerRun().alongWith( + new HandoffRun() + ).withTimeout(5.0), + + // NZ Trip 2 + new IntakeDeploy().alongWith( + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ) + ), + + // Trip 2 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).alongWith( + new IntakeStow() + ), + new ParallelCommandGroup( + new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()), + new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()), + new SwerveClimbAlign() + ), + new SpindexerRun().alongWith( + new HandoffRun() + ).until(() -> DriverStation.getMatchTime() < 2).andThen( + new ParallelCommandGroup( + new HandoffStop(), + new SpindexerStop(), + new ClimberDown() + ) + ) + + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/Test/BoxTest.java b/src/main/java/com/stuypulse/robot/commands/auton/Test/BoxTest.java new file mode 100644 index 00000000..366a4f34 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/Test/BoxTest.java @@ -0,0 +1,23 @@ +package com.stuypulse.robot.commands.auton.Test; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class BoxTest extends SequentialCommandGroup { + + public BoxTest(PathPlannerPath... paths) { + + addCommands( + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterKB.java b/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterKB.java new file mode 100644 index 00000000..88a05bdd --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterKB.java @@ -0,0 +1,14 @@ +/************************ 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.commands.hoodedshooter; + +import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; + +public class HoodedShooterKB extends HoodedShooterSetState { + public HoodedShooterKB() { + super(HoodedShooterState.KB); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 0caa5baa..a7e2a3e6 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -116,7 +116,7 @@ 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 KB_RPM = 0.0; public final double LEFT_CORNER_RPM = 0.0; public final double RIGHT_CORNER_RPM = 0.0; } @@ -126,7 +126,7 @@ public interface Angles { public final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(7.0); public final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(40.0); - public final Rotation2d HUB_ANGLE = Rotation2d.fromDegrees(12.0); + public final Rotation2d KB_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/hoodedshooter/HoodedShooter.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java index b1945ee9..df6396fb 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java @@ -42,7 +42,7 @@ public enum HoodedShooterState { SHOOT(HoodState.SHOOT, ShooterState.SHOOT), FERRY(HoodState.FERRY, ShooterState.FERRY), REVERSE(HoodState.SHOOT, ShooterState.REVERSE), - HUB(HoodState.HUB, ShooterState.HUB), + KB(HoodState.KB, ShooterState.KB), LEFT_CORNER(HoodState.LEFT_CORNER, ShooterState.LEFT_CORNER), RIGHT_CORNER(HoodState.RIGHT_CORNER, ShooterState.RIGHT_CORNER), INTERPOLATION(HoodState.INTERPOLATION, ShooterState.INTERPOLATION); 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 9a637d5a..4185a6a9 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 @@ -30,7 +30,7 @@ public enum HoodState { STOW, FERRY, SHOOT, - HUB, + KB, LEFT_CORNER, RIGHT_CORNER, INTERPOLATION, @@ -54,7 +54,7 @@ public Rotation2d getTargetAngle() { case STOW -> Settings.HoodedShooter.Angles.MIN_ANGLE; case FERRY -> Rotation2d.fromDegrees(30); case SHOOT -> Rotation2d.fromDegrees(Settings.HoodedShooter.Angles.SHOOT_ANGLE.get()); - case HUB -> Settings.HoodedShooter.Angles.HUB_ANGLE; + case KB -> Settings.HoodedShooter.Angles.KB_ANGLE; case LEFT_CORNER -> Settings.HoodedShooter.Angles.LEFT_CORNER_ANGLE; case RIGHT_CORNER -> Settings.HoodedShooter.Angles.RIGHT_CORNER_ANGLE; case INTERPOLATION -> HoodAngleCalculator.interpolateHoodAngle().get(); 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 c18ed7c3..4a1bbdd0 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 @@ -35,7 +35,7 @@ public enum ShooterState { SHOOT, FERRY, REVERSE, - HUB, + KB, LEFT_CORNER, RIGHT_CORNER, INTERPOLATION; @@ -59,7 +59,7 @@ public double getTargetRPM() { case SHOOT -> getShootRPM(); case FERRY -> HoodAngleCalculator.interpolateFerryingRPM().get(); case REVERSE -> Settings.HoodedShooter.RPMs.REVERSE; - case HUB -> Settings.HoodedShooter.RPMs.HUB_RPM; + case KB -> Settings.HoodedShooter.RPMs.KB_RPM; case LEFT_CORNER -> Settings.HoodedShooter.RPMs.LEFT_CORNER_RPM; case RIGHT_CORNER -> Settings.HoodedShooter.RPMs.RIGHT_CORNER_RPM; case INTERPOLATION -> HoodAngleCalculator.interpolateShooterRPM().get(); 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 65944d27..56d549fb 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -480,4 +480,4 @@ public void periodic() { SmartDashboard.putNumber("Swerve/Distance From Hub (meters)", Field.hubCenter.getTranslation().getDistance(getPose().getTranslation())); } -} +} \ No newline at end of file From d0e29770f4b53e1cf8b0bd1b5e3d178b93cbd994 Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Sat, 28 Feb 2026 10:28:42 -0500 Subject: [PATCH 019/150] FIX: configured autons --- simgui.json | 14 ++++ .../com/stuypulse/robot/RobotContainer.java | 71 +++++++++--------- .../subsystems/swerve/TunerConstants.java | 74 +++++++++---------- 3 files changed, 86 insertions(+), 73 deletions(-) diff --git a/simgui.json b/simgui.json index 68c2c46d..1aa1d769 100644 --- a/simgui.json +++ b/simgui.json @@ -11,10 +11,16 @@ "/FMSInfo": "FMSInfo", "/SmartDashboard/Autonomous": "String Chooser", "/SmartDashboard/Field": "Field2d", + "/SmartDashboard/PathPlanner": "Alerts", "/SmartDashboard/Visualizers/ClimberHopper": "Mechanism2d", "/SmartDashboard/Visualizers/Turret": "Mechanism2d" }, "windows": { + "/SmartDashboard/Autonomous": { + "window": { + "visible": true + } + }, "/SmartDashboard/Field": { "bottom": 1914, "height": 8.069275856018066, @@ -30,6 +36,11 @@ "window": { "visible": true } + }, + "/SmartDashboard/Visualizers/Turret": { + "window": { + "visible": true + } } } }, @@ -45,6 +56,9 @@ "ClimberHopper": { "open": true }, + "States": { + "open": true + }, "open": true } } diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 1310ae8e..7f69bf4c 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -105,6 +105,7 @@ public interface EnabledSubsystems { // Robot container public RobotContainer() { + swerve.configureAutoBuilder(); configureDefaultCommands(); configureButtonBindings(); configureAutons(); @@ -289,41 +290,41 @@ public void configureAutons() { "Box 1", "Box 2", "Box 3", "Box 4"); BOX_TEST.register(autonChooser); - // // BASE - // AutonConfig EIGHT_FUEL = new AutonConfig("Eight Fuel", EightFuel::new, - // ""); - // EIGHT_FUEL.register(autonChooser); - - // // DEPOT - // AutonConfig DEPOT_AUTON = new AutonConfig("Depot Auton", DepotAuton::new, - // "Top Bump To Depot", "Depot To Tower Left"); - // DEPOT_AUTON.register(autonChooser); - - // // ONE CYCLES - // AutonConfig TOP_ONE_CYCLE_POACH = new AutonConfig("Top One Cycle (Poach)", TopOneCyclePoach::new, - // "Top Trench To NZ (P)", "Top NZ To Tower Left (P)"); - // TOP_ONE_CYCLE_POACH.register(autonChooser); - - // AutonConfig BOTTOM_ONE_CYCLE_POACH = new AutonConfig("Bottom One Cycle (Poach)", BottomOneCyclePoach::new, - // "Bottom Trench To NZ (P)", "Bottom NZ To Tower Right (P)"); - // BOTTOM_ONE_CYCLE_POACH.register(autonChooser); - - // // TWO CYCLES - // AutonConfig TOP_TWO_CYCLE = new AutonConfig("Top Two Cycle", TopTwoCycle::new, - // "Top Trench To NZ", "Top NZ To Score", "Top Score To NZ", "Top NZ To Tower Left"); - // TOP_TWO_CYCLE.register(autonChooser); - - // AutonConfig BOTTOM_TWO_CYCLE = new AutonConfig("Bottom Two Cycle", BottomTwoCycle::new, - // "Bottom Trench To NZ", "Bottom NZ To Score", "Bottom Score To NZ", "Bottom NZ To Tower Right"); - // BOTTOM_TWO_CYCLE.register(autonChooser); - - // AutonConfig TOP_TWO_CYCLE_POACH = new AutonConfig("Top Two Cycle (Poach)", TopTwoCyclePoach::new, - // "Top Trench To NZ (P)", "Top NZ To Score (P)", "Top Score To NZ", "Top NZ To Tower Left"); - // TOP_TWO_CYCLE_POACH.register(autonChooser); - - // AutonConfig BOTTOM_TWO_CYCLE_POACH = new AutonConfig("Bottom Two Cycle (Poach)", BottomTwoCyclePoach::new, - // "Bottom Trench To NZ (P)", "Bottom NZ To Score (P)", "Bottom Score To NZ", "Bottom NZ To Tower Right"); - // BOTTOM_TWO_CYCLE_POACH.register(autonChooser); + // BASE + AutonConfig EIGHT_FUEL = new AutonConfig("Eight Fuel", EightFuel::new, + ""); + EIGHT_FUEL.register(autonChooser); + + // DEPOT + AutonConfig DEPOT_AUTON = new AutonConfig("Depot Auton", DepotAuton::new, + "Top Bump To Depot", "Depot To Tower Left"); + DEPOT_AUTON.register(autonChooser); + + // ONE CYCLES + AutonConfig TOP_ONE_CYCLE_POACH = new AutonConfig("Top One Cycle (Poach)", TopOneCyclePoach::new, + "Top Trench To NZ (P)", "Top NZ To Tower Left (P)"); + TOP_ONE_CYCLE_POACH.register(autonChooser); + + AutonConfig BOTTOM_ONE_CYCLE_POACH = new AutonConfig("Bottom One Cycle (Poach)", BottomOneCyclePoach::new, + "Bottom Trench To NZ (P)", "Bottom NZ To Tower Right (P)"); + BOTTOM_ONE_CYCLE_POACH.register(autonChooser); + + // TWO CYCLES + AutonConfig TOP_TWO_CYCLE = new AutonConfig("Top Two Cycle", TopTwoCycle::new, + "Top Trench To NZ", "Top NZ To Score", "Top Score To NZ", "Top NZ To Tower Left"); + TOP_TWO_CYCLE.register(autonChooser); + + AutonConfig BOTTOM_TWO_CYCLE = new AutonConfig("Bottom Two Cycle", BottomTwoCycle::new, + "Bottom Trench To NZ", "Bottom NZ To Score", "Bottom Score To NZ", "Bottom NZ To Tower Right"); + BOTTOM_TWO_CYCLE.register(autonChooser); + + AutonConfig TOP_TWO_CYCLE_POACH = new AutonConfig("Top Two Cycle (Poach)", TopTwoCyclePoach::new, + "Top Trench To NZ (P)", "Top NZ To Score (P)", "Top Score To NZ", "Top NZ To Tower Left"); + TOP_TWO_CYCLE_POACH.register(autonChooser); + + AutonConfig BOTTOM_TWO_CYCLE_POACH = new AutonConfig("Bottom Two Cycle (Poach)", BottomTwoCyclePoach::new, + "Bottom Trench To NZ (P)", "Bottom NZ To Score (P)", "Bottom Score To NZ", "Bottom NZ To Tower Right"); + BOTTOM_TWO_CYCLE_POACH.register(autonChooser); SmartDashboard.putData("Autonomous", autonChooser); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java index f5a896ff..0c83c339 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java @@ -7,9 +7,6 @@ import static edu.wpi.first.units.Units.*; -import com.stuypulse.robot.constants.Gains; -import com.stuypulse.robot.constants.Settings; - import edu.wpi.first.math.Matrix; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -22,6 +19,7 @@ import com.ctre.phoenix6.swerve.*; import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + // Generated by the 2026 Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { @@ -30,14 +28,14 @@ public class TunerConstants { // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(Gains.Swerve.Turn.kP).withKI(Gains.Swerve.Turn.kI).withKD(Gains.Swerve.Turn.kD) - .withKS(Gains.Swerve.Turn.kS).withKV(Gains.Swerve.Turn.kV).withKA(Gains.Swerve.Turn.kA) + .withKP(100).withKI(0).withKD(0.5) + .withKS(0.1).withKV(1.16).withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(Gains.Swerve.Drive.kP).withKI(Gains.Swerve.Drive.kI).withKD(Gains.Swerve.Drive.kV) - .withKS(Gains.Swerve.Drive.kS).withKV(Gains.Swerve.Drive.kV); + .withKP(0.3838).withKI(0).withKD(0) + .withKS(0.20896).withKV(0.12464).withKA(0.014877); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors @@ -76,24 +74,24 @@ public class TunerConstants { // CAN bus that the devices are located on; // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = Settings.CANIVORE; + public static final CANBus kCANBus = new CANBus("CANIVORE", "./logs/example.hoot"); // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.76); + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.76); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.125; + private static final double kCoupleRatio = 5.4; - private static final double kDriveGearRatio = 5.357142857142857; - private static final double kSteerGearRatio = 21.428571428571427; + private static final double kDriveGearRatio = 6.48; + private static final double kSteerGearRatio = 12.1; private static final Distance kWheelRadius = Inches.of(2); private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; - private static final int kPigeonId = 5; + private static final int kPigeonId = 0; // These are only used for simulation private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); @@ -132,48 +130,48 @@ public class TunerConstants { // Front Left - private static final int kFrontLeftDriveMotorId = 32; - private static final int kFrontLeftSteerMotorId = 42; - private static final int kFrontLeftEncoderId = 4; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.09375); + private static final int kFrontLeftDriveMotorId = 14; + private static final int kFrontLeftSteerMotorId = 15; + private static final int kFrontLeftEncoderId = 2; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.21435546875); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; - private static final Distance kFrontLeftXPos = Inches.of(17.15); - private static final Distance kFrontLeftYPos = Inches.of(19.7); + private static final Distance kFrontLeftXPos = Inches.of(4.5); + private static final Distance kFrontLeftYPos = Inches.of(6.5); // Front Right - private static final int kFrontRightDriveMotorId = 50; - private static final int kFrontRightSteerMotorId = 13; - private static final int kFrontRightEncoderId = 2; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.14306640625); + private static final int kFrontRightDriveMotorId = 13; + private static final int kFrontRightSteerMotorId = 12; + private static final int kFrontRightEncoderId = 3; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.291259765625); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; - private static final Distance kFrontRightXPos = Inches.of(17.15); - private static final Distance kFrontRightYPos = Inches.of(-19.7); + private static final Distance kFrontRightXPos = Inches.of(4.5); + private static final Distance kFrontRightYPos = Inches.of(-6.5); // Back Left - private static final int kBackLeftDriveMotorId = 22; - private static final int kBackLeftSteerMotorId = 11; - private static final int kBackLeftEncoderId = 3; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.251708984375); + private static final int kBackLeftDriveMotorId = 16; + private static final int kBackLeftSteerMotorId = 17; + private static final int kBackLeftEncoderId = 1; + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.484375); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; - private static final Distance kBackLeftXPos = Inches.of(-17.15); - private static final Distance kBackLeftYPos = Inches.of(19.7); + private static final Distance kBackLeftXPos = Inches.of(-4.5); + private static final Distance kBackLeftYPos = Inches.of(6.5); // Back Right - private static final int kBackRightDriveMotorId = 21; - private static final int kBackRightSteerMotorId = 0; - private static final int kBackRightEncoderId = 1; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.17822265625); + private static final int kBackRightDriveMotorId = 11; + private static final int kBackRightSteerMotorId = 10; + private static final int kBackRightEncoderId = 4; + private static final Angle kBackRightEncoderOffset = Rotations.of(0.462890625); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; - private static final Distance kBackRightXPos = Inches.of(-17.15); - private static final Distance kBackRightYPos = Inches.of(-19.7); + private static final Distance kBackRightXPos = Inches.of(-4.5); + private static final Distance kBackRightYPos = Inches.of(-6.5); public static final SwerveModuleConstants FrontLeft = @@ -289,4 +287,4 @@ public TunerSwerveDrivetrain( ); } } -} +} \ No newline at end of file From 2caf6dadfc64e5c3444ba05c047162af685196d6 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 28 Feb 2026 10:42:29 -0500 Subject: [PATCH 020/150] feat: allow for seeding pivot, adding voltage to push down the intake --- .../com/stuypulse/robot/RobotContainer.java | 2 + .../robot/commands/intake/SeedPivot.java | 18 ++++++ .../stuypulse/robot/constants/Settings.java | 6 ++ .../robot/subsystems/intake/Intake.java | 2 + .../robot/subsystems/intake/IntakeImpl.java | 62 +++++++++++-------- 5 files changed, 64 insertions(+), 26 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/intake/SeedPivot.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 7ba9539b..8a794a40 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -31,6 +31,7 @@ import com.stuypulse.robot.commands.intake.IntakeRunRollers; import com.stuypulse.robot.commands.intake.IntakeStopRollers; import com.stuypulse.robot.commands.intake.IntakeStow; +import com.stuypulse.robot.commands.intake.SeedPivot; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.swerve.SwerveClimbAlign; @@ -108,6 +109,7 @@ public RobotContainer() { configureSysids(); SmartDashboard.putData("Field", Field.FIELD2D); + SmartDashboard.putData("Intake/Reset Pivot", new SeedPivot()); // SmartDashboard.putData("Zero Encoders", new TurretZero()); } diff --git a/src/main/java/com/stuypulse/robot/commands/intake/SeedPivot.java b/src/main/java/com/stuypulse/robot/commands/intake/SeedPivot.java new file mode 100644 index 00000000..81c8be3f --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/SeedPivot.java @@ -0,0 +1,18 @@ +package com.stuypulse.robot.commands.intake; + +import com.stuypulse.robot.subsystems.intake.Intake; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class SeedPivot extends InstantCommand { + private Intake intake; + + public SeedPivot() { + intake = Intake.getInstance(); + } + + @Override + public void initialize() { + intake.seedPivot(); + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index bc70152a..bbf1b8cc 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -92,6 +92,12 @@ public interface Intake { Rotation2d PIVOT_MAX_VEL_STOW = Rotation2d.fromDegrees(360.0); Rotation2d PIVOT_MAX_ACCEL_STOW = Rotation2d.fromDegrees(600.0); + Rotation2d THRESHHOLD_TO_START_ROLLERS = Rotation2d.fromDegrees(60.0); + + Rotation2d ARBITRARY_VOLTAGE_THRESHOLD = Rotation2d.fromDegrees(15.0); + + double PUSHDOWN_VOLTAGE = 3.0; + double GEAR_RATIO = 37.93; } 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 5b6940ad..cf6aa142 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -111,6 +111,8 @@ public Rotation2d driverInputToAngle() { public abstract void setPivotVoltageOverride(Optional voltage); public abstract SysIdRoutine getPivotSysIdRoutine(); + public abstract void seedPivot(); + @Override public void periodic() { SmartDashboard.putString("Intake/Pivot State", getPivotState().toString()); 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 4021dc79..7f813b2a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -13,8 +13,8 @@ import com.stuypulse.robot.util.SettableNumber; import com.stuypulse.robot.util.SysId; -import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -22,6 +22,7 @@ import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; @@ -39,7 +40,6 @@ public class IntakeImpl extends Intake { private final TalonFX rollerFollower; private final MotionMagicVoltage pivotController; - private final BangBangController pivotAggressiveController; private final DutyCycleOut rollerController; private final Follower follower; @@ -50,9 +50,10 @@ public class IntakeImpl extends Intake { private Optional pivotVoltageOverride; public IntakeImpl() { + //TODO: refactor pivotConfig = new Motors.TalonFXConfig() - .withCurrentLimitAmps(60.0) - .withRampRate(0.25) + .withCurrentLimitEnable(false) + //.withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) .withInvertedValue(InvertedValue.Clockwise_Positive) .withPIDConstants(Gains.Intake.Pivot.kP, Gains.Intake.Pivot.kI, Gains.Intake.Pivot.kD, 0) @@ -63,8 +64,8 @@ public IntakeImpl() { .withMotionProfile(Settings.Intake.PIVOT_MAX_VEL_STOW.getRotations(), Settings.Intake.PIVOT_MAX_ACCEL_STOW.getRotations()); rollerConfig = new Motors.TalonFXConfig() - .withCurrentLimitAmps(60.0) - .withRampRate(0.50) + .withCurrentLimitEnable(false) + //.withRampRate(0.50) .withNeutralMode(NeutralModeValue.Coast) .withInvertedValue(InvertedValue.CounterClockwise_Positive); @@ -80,7 +81,6 @@ public IntakeImpl() { pivotController = new MotionMagicVoltage(getPivotState().getTargetAngle().getRotations()) .withEnableFOC(true); - pivotAggressiveController = new BangBangController(Settings.Intake.PIVOT_ANGLE_TOLERANCE.getRotations()); rollerController = new DutyCycleOut(getRollerState().getTargetDutyCycle()) .withEnableFOC(true); @@ -125,11 +125,17 @@ public void setPivotState(PivotState pivotState) { } } + @Override + public void seedPivot() { + pivot.setPosition(Settings.Intake.PIVOT_MAX_ANGLE.getRotations()); + } + @Override public void periodic() { super.periodic(); if (EnabledSubsystems.INTAKE.get()) { + if (getPivotState() == PivotState.ANALOG) { // comment out the setControl line if it breaks pivot.setControl(pivotController.withPosition(driverInputToAngle().getRotations())); } @@ -138,23 +144,27 @@ else if (pivotVoltageOverride.isPresent()) { pivot.setVoltage(pivotVoltageOverride.get()); } - // else if (getPivotState() == PivotState.BANGBANG) { - // pivotAggressiveController.calculate(getPivotAngle().getRotations(), getPivotState().getTargetAngle().getRotations()); - // pivot.setControl(new VelocityVoltage(pivotAggressiveController.getMeasurement() * 12)); - - // TODO: might have to calculate inside of pivot.setcontrol - // //TODO: finish and apply to motor -> motor config conflict - // } else { - pivot.setControl(new PositionVoltage(getPivotState().getTargetAngle().getRotations())); - //pivot.setControl(pivotController.withPosition(getPivotState().getTargetAngle().getRotations())); - rollerLeader.setControl(rollerController.withOutput(getRollerState().getTargetDutyCycle())); - rollerFollower.setControl(follower); + if (getPivotState() == PivotState.DEPLOY && getPivotAngle().getDegrees() <= Settings.Intake.ARBITRARY_VOLTAGE_THRESHOLD.getDegrees()) { + pivot.setControl(new VoltageOut(-Settings.Intake.PUSHDOWN_VOLTAGE)); // applying 3 volts + } + + else { + pivot.setControl(new PositionVoltage(getPivotState().getTargetAngle().getRotations())); + } + + if (getPivotAngle().getDegrees() <= Settings.Intake.THRESHHOLD_TO_START_ROLLERS.getDegrees()) { + rollerLeader.setControl(rollerController.withOutput(getRollerState().getTargetDutyCycle())); + } + + else { + rollerLeader.stopMotor(); + } } } else { - // pivot.stopMotor(); - // rollerLeader.stopMotor(); - // rollerFollower.stopMotor(); + pivot.stopMotor(); + rollerLeader.stopMotor(); + rollerFollower.stopMotor(); } if (Settings.DEBUG_MODE) { @@ -171,11 +181,7 @@ else if (pivotVoltageOverride.isPresent()) { SmartDashboard.putNumber("Intake/Pivot Angle Error (deg)", Math.abs(getPivotState().getTargetAngle().getDegrees() - getPivotAngle().getDegrees())); - //BANGBANG - SmartDashboard.putNumber("Intake/Pivot BANGANG tolerance", pivotAggressiveController.getSetpoint()); - SmartDashboard.putNumber("Intake/Pivot BANGANG measurement (0 - 1)", pivotAggressiveController.getMeasurement()); - SmartDashboard.putNumber("Intake/Pivot BANGANG Distance between target and setpoint", pivotAggressiveController.getError()); - //TODO: add whatever was still not added + SmartDashboard.putNumber("Intake/Pivot Closed Loop Error (deg)", pivot.getClosedLoopError().getValueAsDouble() * 360.0); // ROLLERS @@ -184,6 +190,10 @@ else if (pivotVoltageOverride.isPresent()) { SmartDashboard.putNumber("Intake/Roller Leader Current (amps)", rollerLeader.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Intake/Roller Follower Current (amps)", rollerFollower.getSupplyCurrent().getValueAsDouble()); + + SmartDashboard.putNumber("Intake/Pivot Temperature (C)", pivot.getDeviceTemp().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Leader Temperature (C)", rollerLeader.getDeviceTemp().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Follower Temperature (C)", rollerFollower.getDeviceTemp().getValueAsDouble()); } } From 7c58b9b46c8eaf9e6d515a704563cdc5a834520d Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Sat, 28 Feb 2026 11:55:18 -0500 Subject: [PATCH 021/150] FIX: FMS logic and Tolerance logic --- simgui.json | 3 ++ src/main/java/com/stuypulse/robot/Robot.java | 2 ++ .../PoachingAutons/BottomOneCyclePoach.java | 3 +- .../PoachingAutons/BottomTwoCyclePoach.java | 8 ++---- .../PoachingAutons/TopOneCyclePoach.java | 3 +- .../PoachingAutons/TopTwoCyclePoach.java | 8 ++---- .../auton/RegularAutons/BottomTwoCycle.java | 8 ++---- .../auton/RegularAutons/DepotAuton.java | 5 +--- .../auton/RegularAutons/EightFuel.java | 9 ++---- .../robot/commands/auton/Test/BoxTest.java | 7 ++++- .../hoodedshooter/HoodedShooter.java | 9 +++++- .../hoodedshooter/HoodedShooterSim.java | 28 +++++++++++++++++++ 12 files changed, 58 insertions(+), 35 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooterSim.java diff --git a/simgui.json b/simgui.json index 1aa1d769..499b2554 100644 --- a/simgui.json +++ b/simgui.json @@ -56,6 +56,9 @@ "ClimberHopper": { "open": true }, + "HoodedShooter": { + "open": true + }, "States": { "open": true }, diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 5b78a442..c0676ea1 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -49,6 +49,8 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); SmartDashboard.putNumber("Robot/Voltage of Robot", powerDistribution.getVoltage()); + SmartDashboard.putNumber("Match Time", DriverStation.getMatchTime()); + if (DriverStation.getAlliance().isPresent()) { alliance = DriverStation.getAlliance().get(); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomOneCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomOneCyclePoach.java index c4980e03..be56a4bd 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomOneCyclePoach.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomOneCyclePoach.java @@ -35,8 +35,7 @@ public BottomOneCyclePoach(PathPlannerPath... paths) { new IntakeStow() ), new ParallelCommandGroup( - new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()), - new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()), + new WaitUntilCommand(() -> HoodedShooter.getInstance().bothAtTolerance()), new SwerveClimbAlign() ), new SpindexerRun().alongWith( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomTwoCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomTwoCyclePoach.java index d4e7f26c..dfde0b1e 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomTwoCyclePoach.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomTwoCyclePoach.java @@ -34,10 +34,7 @@ public BottomTwoCyclePoach(PathPlannerPath... paths) { CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( new IntakeStow() ), - new ParallelCommandGroup( - new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()), - new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()) - ), + new WaitUntilCommand(() -> HoodedShooter.getInstance().bothAtTolerance()), new SpindexerRun().alongWith( new HandoffRun() ).withTimeout(5.0), @@ -56,8 +53,7 @@ public BottomTwoCyclePoach(PathPlannerPath... paths) { new IntakeStow() ), new ParallelCommandGroup( - new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()), - new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()), + new WaitUntilCommand(() -> HoodedShooter.getInstance().bothAtTolerance()), new SwerveClimbAlign() ), new SpindexerRun().alongWith( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopOneCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopOneCyclePoach.java index ae4fd460..4b9ddcf9 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopOneCyclePoach.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopOneCyclePoach.java @@ -35,8 +35,7 @@ public TopOneCyclePoach(PathPlannerPath... paths) { new IntakeStow() ), new ParallelCommandGroup( - new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()), - new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()), + new WaitUntilCommand(() -> HoodedShooter.getInstance().bothAtTolerance()), new SwerveClimbAlign() ), new SpindexerRun().alongWith( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopTwoCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopTwoCyclePoach.java index b105937b..c481447b 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopTwoCyclePoach.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopTwoCyclePoach.java @@ -34,10 +34,7 @@ public TopTwoCyclePoach(PathPlannerPath... paths) { CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( new IntakeStow() ), - new ParallelCommandGroup( - new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()), - new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()) - ), + new WaitUntilCommand(() -> HoodedShooter.getInstance().bothAtTolerance()), new SpindexerRun().alongWith( new HandoffRun() ).withTimeout(5.0), @@ -56,8 +53,7 @@ public TopTwoCyclePoach(PathPlannerPath... paths) { new IntakeStow() ), new ParallelCommandGroup( - new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()), - new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()), + new WaitUntilCommand(() -> HoodedShooter.getInstance().bothAtTolerance()), new SwerveClimbAlign() ), new SpindexerRun().alongWith( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/BottomTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/BottomTwoCycle.java index 08234a92..718845f9 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/BottomTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/BottomTwoCycle.java @@ -34,10 +34,7 @@ public BottomTwoCycle(PathPlannerPath... paths) { CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( new IntakeStow() ), - new ParallelCommandGroup( - new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()), - new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()) - ), + new WaitUntilCommand(() -> HoodedShooter.getInstance().bothAtTolerance()), new SpindexerRun().alongWith( new HandoffRun() ).withTimeout(5.0), @@ -56,8 +53,7 @@ public BottomTwoCycle(PathPlannerPath... paths) { new IntakeStow() ), new ParallelCommandGroup( - new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()), - new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()), + new WaitUntilCommand(() -> HoodedShooter.getInstance().bothAtTolerance()), new SwerveClimbAlign() ), new SpindexerRun().alongWith( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/DepotAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/DepotAuton.java index 7ed1909a..d256c875 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/DepotAuton.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/DepotAuton.java @@ -34,10 +34,7 @@ public DepotAuton(PathPlannerPath... paths) { CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]) ), - new ParallelCommandGroup( - new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()), - new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()) - ), + new WaitUntilCommand(() -> HoodedShooter.getInstance().bothAtTolerance()), new SpindexerRun().alongWith( new HandoffRun() ).until(() -> DriverStation.getMatchTime() < 2).andThen( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/EightFuel.java b/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/EightFuel.java index f774dc1c..b31a67da 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/EightFuel.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/EightFuel.java @@ -16,13 +16,8 @@ public EightFuel(PathPlannerPath... paths) { addCommands( - new HoodedShooterKB().alongWith( - new ParallelCommandGroup( - - new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()), - new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()) - - ) + new HoodedShooterKB().until( + () -> HoodedShooter.getInstance().bothAtTolerance() ), new SpindexerRun().alongWith( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/Test/BoxTest.java b/src/main/java/com/stuypulse/robot/commands/auton/Test/BoxTest.java index 366a4f34..6b5ae850 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/Test/BoxTest.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/Test/BoxTest.java @@ -1,9 +1,13 @@ package com.stuypulse.robot.commands.auton.Test; import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterKB; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class BoxTest extends SequentialCommandGroup { @@ -11,7 +15,8 @@ public BoxTest(PathPlannerPath... paths) { addCommands( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new HoodedShooterKB().until(() -> DriverStation.getMatchTime() < 18).andThen( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0])), CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]) 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 df6396fb..dc549948 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java @@ -5,6 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.hoodedshooter; +import com.stuypulse.robot.Robot; import com.stuypulse.robot.subsystems.hoodedshooter.hood.Hood; import com.stuypulse.robot.subsystems.hoodedshooter.hood.Hood.HoodState; import com.stuypulse.robot.subsystems.hoodedshooter.shooter.Shooter; @@ -19,8 +20,14 @@ public class HoodedShooter extends SubsystemBase { private static final HoodedShooter instance; static { - instance = new HoodedShooter(); + if (Robot.isReal()) { + instance = new HoodedShooter(); + } + else { + instance = new HoodedShooterSim(); + } } + public static HoodedShooter getInstance(){ return instance; diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooterSim.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooterSim.java new file mode 100644 index 00000000..4722fbfd --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooterSim.java @@ -0,0 +1,28 @@ +/************************ 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.subsystems.hoodedshooter; + +import com.stuypulse.stuylib.network.SmartBoolean; + +public class HoodedShooterSim extends HoodedShooter { + + private final SmartBoolean bothAtTolerance; + + protected HoodedShooterSim() { + super(); + bothAtTolerance = new SmartBoolean("HoodedShooter/Both At Tolerance", false); + } + + @Override + public boolean bothAtTolerance() { + return bothAtTolerance.get(); + } + + @Override + public void periodic() { + super.periodic(); + } +} \ No newline at end of file From d321fc7bef0d7875fd30846387a49c924bcac04b Mon Sep 17 00:00:00 2001 From: Lucas Date: Sat, 28 Feb 2026 12:58:45 -0500 Subject: [PATCH 022/150] turret tracking to hub/ferry based on pose Co-authored-by: Tahmidd2 Co-authored-by: Xuan Hao Cen Co-authored-by: Lucas Li --- simgui.json | 3 ++ .../commands/turret/TurretDefaultCommand.java | 38 +++++++++++++++++++ 2 files changed, 41 insertions(+) create mode 100644 src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java diff --git a/simgui.json b/simgui.json index 499b2554..2f962283 100644 --- a/simgui.json +++ b/simgui.json @@ -56,6 +56,9 @@ "ClimberHopper": { "open": true }, + "Enabled Subsystems": { + "open": true + }, "HoodedShooter": { "open": true }, diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java new file mode 100644 index 00000000..f9f826a6 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java @@ -0,0 +1,38 @@ +package com.stuypulse.robot.commands.turret; + +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.subsystems.turret.Turret; +import com.stuypulse.robot.subsystems.turret.Turret.TurretState; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; + +public class TurretDefaultCommand extends Command { + private final Turret turret; + private final CommandSwerveDrivetrain swerve; + + public TurretDefaultCommand() { + turret = Turret.getInstance(); + swerve = CommandSwerveDrivetrain.getInstance(); + + addRequirements(turret); + } + + @Override + public void execute() { + // === Robot Position Logic === + // Reminder from driver's perspective, positive X to the opposite alliance and positive Y points to the left. + + boolean isInAllianceZone = swerve.getPose().getX() < Field.getHubPose().getX(); + + if (isInAllianceZone) { + turret.setState(TurretState.FERRYING); + } + else { + turret.setState(TurretState.SHOOTING); + } + } + +} \ No newline at end of file From b26b41621f05d01f03b0f3ddea78b183bd91083f Mon Sep 17 00:00:00 2001 From: Apetrock24 Date: Sat, 28 Feb 2026 13:09:11 -0500 Subject: [PATCH 023/150] Feat: Prelim fms Util Co-authored-by: Delta Co-authored-by: Ryan Bergman --- src/main/java/com/stuypulse/robot/Robot.java | 4 +- .../stuypulse/robot/constants/Settings.java | 6 + .../com/stuypulse/robot/util/FMSutil.java | 106 ++++++++++++++++++ 3 files changed, 115 insertions(+), 1 deletion(-) create mode 100644 src/main/java/com/stuypulse/robot/util/FMSutil.java diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index c0676ea1..5f7749a7 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -7,6 +7,7 @@ import com.stuypulse.robot.commands.vision.SetMegaTagMode; import com.stuypulse.robot.subsystems.vision.LimelightVision; +import com.stuypulse.robot.util.FMSutil; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; @@ -96,7 +97,8 @@ public void autonomousExit() {} @Override public void teleopInit() { - CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); + FMSutil fmSutil = new FMSutil(); + CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); if (auto != null) { auto.cancel(); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index a7e2a3e6..b51d9761 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -32,6 +32,12 @@ public interface Settings { public final boolean DEBUG_MODE = true; public final CANBus CANIVORE = new CANBus("canivore", "./logs/example.hoot"); + public interface HubDATA { + public final double[] shiftStartTimes = {0.0, 10.0, 35.0, 60.0, 85.0, 110.0}; + public final double[] shiftEndTimes = {10.0, 35.0, 60.0, 85.0, 110.0, 140.0}; + + } + public interface ClimberHopper { public interface Constants { public final double GEAR_RATIO = 45.0; diff --git a/src/main/java/com/stuypulse/robot/util/FMSutil.java b/src/main/java/com/stuypulse/robot/util/FMSutil.java new file mode 100644 index 00000000..03631a79 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/FMSutil.java @@ -0,0 +1,106 @@ +package com.stuypulse.robot.util; + +import com.stuypulse.robot.constants.Field; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +public class FMSutil { + private Timer timer = new Timer(); + private FieldState fieldState; + private double timeLeft; + private FieldState[] fieldStates = { fieldState.AUTO, fieldState.TRANSITION, fieldState.SHIFT1, fieldState.SHIFT2, + fieldState.SHIFT3, fieldState.SHIFT4, fieldState.ENDGAME }; + + public static enum FieldState { + AUTO(0.0, 20.0), + TRANSITION(0.0, 10.0), + SHIFT1(10.0, 35.0), + SHIFT2(35.0, 60.0), + SHIFT3(60.0, 85.0), + SHIFT4(85.0, 110.0), + ENDGAME(110.0, 140); + + // public final do.0uble[] shiftStartTimes = {0.0, 10.0, 35.0, 60.0, 85.0, 110.0}; + // public final double[] shiftEndTimes = {10.0, 35.0, 60.0, 85.0, 110.0, 140.0}; + private double startT; + private double endT; + + private FieldState(double startT, double endT) { + this.startT = startT; + this.endT = endT; + } + + public boolean isActive(double time) { + if (startT <= time && time < endT) { + return true; + } else { + return false; + } + } + + public double timeLeft(double time) { + return endT - time; + } + + public double timeElapsed(double time) { + return time - startT; + } + }; + + + public FMSutil() { + timer = new Timer(); + timer.start(); + } + + public void resetTimer() { + timer.reset(); + } + + public FieldState getFieldState() { + FieldState activeState = FieldState.AUTO; + for (FieldState state : fieldStates) { + if (state.isActive(timer.get())) { activeState = state; break;} + } + return activeState; + } + + public boolean isActiveShift() { + boolean wonAuto = didWinAuto(); + switch (getFieldState()) { + case SHIFT1: + return (wonAuto) ? true : false; + case SHIFT2: + return (wonAuto) ? false : true; + case SHIFT3: + return (wonAuto) ? true : false; + case SHIFT4: + return (wonAuto) ? false : false; + case ENDGAME: + return true; + default: + return false; + } + } + + public boolean didWinAuto() { + String winner = DriverStation.getGameSpecificMessage(); //TODO: find out what message is + String currentAlliance = (DriverStation.getAlliance().get() == Alliance.Blue) ? "B" : "R"; + if (winner.isEmpty()) { + DriverStation.reportWarning("Arena Fault, no alliance won data", true); + return true; // Assume we won :) + } else if (currentAlliance.equalsIgnoreCase(winner)) { + return true; + } else { + return false; + } + } + + public double getTimeLeftInSHift() { + return getFieldState().endT - timer.get(); + } + + +} From b52418fd083eb41c9bf57bc6e49b5d6cc9935872 Mon Sep 17 00:00:00 2001 From: Apetrock24 Date: Sat, 28 Feb 2026 13:33:59 -0500 Subject: [PATCH 024/150] FIX: account for delay of teleop and auto --- src/main/java/com/stuypulse/robot/Robot.java | 5 ++-- .../com/stuypulse/robot/util/FMSutil.java | 23 +++++++++++++------ 2 files changed, 19 insertions(+), 9 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 5f7749a7..8760a0da 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -55,6 +55,7 @@ public void robotPeriodic() { if (DriverStation.getAlliance().isPresent()) { alliance = DriverStation.getAlliance().get(); } + } /*********************/ @@ -75,7 +76,7 @@ public void disabledPeriodic() {} @Override public void autonomousInit() { - + FMSutil fmSutil = new FMSutil(true); CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); auto = robot.getAutonomousCommand(); @@ -98,7 +99,7 @@ public void autonomousExit() {} @Override public void teleopInit() { FMSutil fmSutil = new FMSutil(); - CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); + CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); if (auto != null) { auto.cancel(); diff --git a/src/main/java/com/stuypulse/robot/util/FMSutil.java b/src/main/java/com/stuypulse/robot/util/FMSutil.java index 03631a79..3cac5108 100644 --- a/src/main/java/com/stuypulse/robot/util/FMSutil.java +++ b/src/main/java/com/stuypulse/robot/util/FMSutil.java @@ -1,7 +1,9 @@ package com.stuypulse.robot.util; +import com.fasterxml.jackson.annotation.JsonFormat.Feature; import com.stuypulse.robot.constants.Field; +import edu.wpi.first.util.function.BooleanConsumer; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -10,17 +12,18 @@ public class FMSutil { private Timer timer = new Timer(); private FieldState fieldState; private double timeLeft; + boolean auto; private FieldState[] fieldStates = { fieldState.AUTO, fieldState.TRANSITION, fieldState.SHIFT1, fieldState.SHIFT2, fieldState.SHIFT3, fieldState.SHIFT4, fieldState.ENDGAME }; public static enum FieldState { AUTO(0.0, 20.0), - TRANSITION(0.0, 10.0), - SHIFT1(10.0, 35.0), - SHIFT2(35.0, 60.0), - SHIFT3(60.0, 85.0), - SHIFT4(85.0, 110.0), - ENDGAME(110.0, 140); + TRANSITION(0.0, 25.0), + SHIFT1(25.0, 50.0), + SHIFT2(50.0, 75.0), + SHIFT3(75.0, 100.0), + SHIFT4(100.0, 125.0), + ENDGAME(125, 155.0); // public final do.0uble[] shiftStartTimes = {0.0, 10.0, 35.0, 60.0, 85.0, 110.0}; // public final double[] shiftEndTimes = {10.0, 35.0, 60.0, 85.0, 110.0, 140.0}; @@ -50,9 +53,14 @@ public double timeElapsed(double time) { }; - public FMSutil() { + public FMSutil(boolean auto) { timer = new Timer(); timer.start(); + this.auto = true; + } + + public FMSutil() { + this(false); } public void resetTimer() { @@ -61,6 +69,7 @@ public void resetTimer() { public FieldState getFieldState() { FieldState activeState = FieldState.AUTO; + if (auto) return FieldState.AUTO; for (FieldState state : fieldStates) { if (state.isActive(timer.get())) { activeState = state; break;} } From f8ccbc43ae816dd7d9accfa385ca010322c51689 Mon Sep 17 00:00:00 2001 From: Lucas Date: Sat, 28 Feb 2026 13:36:08 -0500 Subject: [PATCH 025/150] Feat: Set TurretDefaultCommand as default command --- src/main/java/com/stuypulse/robot/RobotContainer.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 7f69bf4c..f7b22412 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -42,6 +42,7 @@ import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveXMode; +import com.stuypulse.robot.commands.turret.TurretDefaultCommand; import com.stuypulse.robot.commands.turret.TurretFerry; import com.stuypulse.robot.commands.turret.TurretIdle; import com.stuypulse.robot.commands.turret.TurretLeftCorner; @@ -120,6 +121,7 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); climberHopper.setDefaultCommand(new ClimberHopperDefaultCommand()); + turret.setDefaultCommand(new TurretDefaultCommand()); } /***************/ From 17c1f14f70d90c049e42eed5827b6212bf419aa5 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 28 Feb 2026 14:07:15 -0500 Subject: [PATCH 026/150] Feat+Fix: Preliminary gains switching and offsets for turret, Limelight offsets, temporary turret testing state. --- .../com/stuypulse/robot/RobotContainer.java | 23 +++-- .../robot/commands/turret/TurretZero.java | 1 + .../stuypulse/robot/constants/Cameras.java | 2 +- .../com/stuypulse/robot/constants/Gains.java | 40 +++++---- .../com/stuypulse/robot/constants/Motors.java | 29 +++++++ .../stuypulse/robot/constants/Settings.java | 10 ++- .../robot/subsystems/turret/TurretImpl.java | 83 ++++++++++++++----- 7 files changed, 141 insertions(+), 47 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 8a794a40..a17b57d5 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -40,6 +40,7 @@ import com.stuypulse.robot.commands.swerve.SwerveXMode; import com.stuypulse.robot.commands.turret.TurretAnalog; import com.stuypulse.robot.commands.turret.TurretFerry; +import com.stuypulse.robot.commands.turret.TurretHub; import com.stuypulse.robot.commands.turret.TurretIdle; import com.stuypulse.robot.commands.turret.TurretLeftCorner; import com.stuypulse.robot.commands.turret.TurretRightCorner; @@ -71,9 +72,9 @@ public class RobotContainer { public interface EnabledSubsystems { SmartBoolean SWERVE = new SmartBoolean("Enabled Subsystems/Swerve Is Enabled", true); - SmartBoolean TURRET = new SmartBoolean("Enabled Subsystems/Turret Is Enabled", false); + SmartBoolean TURRET = new SmartBoolean("Enabled Subsystems/Turret Is Enabled", true); SmartBoolean HANDOFF = new SmartBoolean("Enabled Subsystems/Handoff Is Enabled", false); - SmartBoolean INTAKE = new SmartBoolean("Enabled Subsystems/Intake Is Enabled", true); + SmartBoolean INTAKE = new SmartBoolean("Enabled Subsystems/Intake Is Enabled", false); SmartBoolean SPINDEXER = new SmartBoolean("Enabled Subsystems/Spindexer Is Enabled", false); SmartBoolean CLIMBER_HOPPER = new SmartBoolean("Enabled Subsystems/Climber-Hopper Is Enabled", false); SmartBoolean HOOD = new SmartBoolean("Enabled Subsystems/Hood Is Enabled", false); @@ -110,7 +111,7 @@ public RobotContainer() { SmartDashboard.putData("Field", Field.FIELD2D); SmartDashboard.putData("Intake/Reset Pivot", new SeedPivot()); - // SmartDashboard.putData("Zero Encoders", new TurretZero()); + SmartDashboard.putData("Zero Encoders", new TurretZero()); } /****************/ @@ -152,11 +153,15 @@ private void configureButtonBindings() { .onTrue(new SwerveResetHeading()) .onTrue(new ResetLimelightIMU()) .onFalse(new SetIMUMode(0)); - - driver.getTopButton() - // .whileTrue(new TurretAnalog(driver)); - .whileTrue(new IntakeAnalog(driver)); // will be useful for finding kG + driver.getTopButton() + .onTrue(new TurretShoot()) + .onFalse(new TurretIdle()); + + driver.getBottomButton() + .onTrue(new TurretAnalog(driver)) + .onFalse(new TurretIdle()); + // Scoring Routine using Interpolation Settings // driver.getTopButton() // .whileTrue(new HoodedShooterInterpolation() @@ -247,7 +252,7 @@ private void configureButtonBindings() { // Intake Down and On driver.getRightTriggerButton() .onTrue(new IntakeDeploy()); -*/ + // Climb Down Placeholder driver.getLeftBumper() .onTrue(new BuzzController(driver).alongWith(new ClimberDown())) @@ -258,7 +263,7 @@ private void configureButtonBindings() { .onTrue(new BuzzController(driver)) .whileTrue(new ClimberUp()) .onFalse(new HopperDown()); -/* + // Reset Heading driver.getDPadUp() .onTrue(new SwerveResetHeading()); diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretZero.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretZero.java index 62bece3b..0fbd07d7 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretZero.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretZero.java @@ -22,5 +22,6 @@ public TurretZero() { @Override public void initialize() { turret.zeroEncoders(); + turret.seedTurret(); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index a095c89d..e6532689 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -18,7 +18,7 @@ public interface Cameras { public Camera[] LimelightCameras = { new Camera("limelight-climber", - new Pose3d(-0.375959, -0.233, 0.20368, + new Pose3d(-0.233, 0.375959, 0.20368, new Rotation3d(Units.degreesToRadians(180), Units.degreesToRadians(31.08), Units.degreesToRadians(-80))), RobotContainer.EnabledSubsystems.LIMELIGHT) }; diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 76d2a4c5..f97d90d9 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -10,7 +10,7 @@ import com.pathplanner.lib.config.PIDConstants; public class Gains { - + public interface ClimberHopper { double kP = 1.0; double kI = 0.0; @@ -41,14 +41,14 @@ public interface Hood { double kV = 0.0; double kA = 0.0; } - + } public interface Spindexer { // double kP = 1.20; // double kI = 0.0; // double kD = 0.0; - + // double kS = 0.019444; // double kA = 0.010876; // double kV = 0.38546; @@ -56,7 +56,7 @@ public interface Spindexer { SmartNumber kP = new SmartNumber("Spindexer/Gains/kP", 1.20); SmartNumber kI = new SmartNumber("Spindexer/Gains/kI", 0.0); SmartNumber kD = new SmartNumber("Spindexer/Gains/kD", 0.0); - + SmartNumber kS = new SmartNumber("Spindexer/Gains/kS", 0.25); SmartNumber kA = new SmartNumber("Spindexer/Gains/kA", 0.010876); SmartNumber kV = new SmartNumber("Spindexer/Gains/kV", 0.9413); @@ -77,23 +77,35 @@ public interface Pivot { } public interface Handoff { - double kP = 0.00015508; //0.016973 from sysid + double kP = 0.00015508; // 0.016973 from sysid double kI = 0.0; double kD = 0.0; - - double kS = 0.21149; //0.1728 from alpha + + double kS = 0.21149; // 0.1728 from alpha double kA = 0.016329; double kV = 0.3652; } public interface Turret { - double kP = 1300.0; - double kI = 0.0; - double kD = 140.0; + public interface slot0 { + double kP = 1300.0; + double kI = 0.0; + double kD = 140.0; - double kS = 0.23; // FOUND ON 2/25 PD 8 - double kV = 0.0; - double kA = 0.0; + double kS = 0.23; // FOUND ON 2/25 PD 8 + double kV = 0.0; + double kA = 0.0; + } + + public interface slot1 { + double kP = 0.0; + double kI = 0.0; + double kD = 0.0; + + double kS = 0.0; // FOUND ON 2/25 PD 8 + double kV = 0.0; + double kA = 0.0; + } } public interface Swerve { @@ -118,7 +130,7 @@ public interface Turn { } public interface Alignment { - public interface Rotation { + public interface Rotation { double kp = 112.3; double ki = 0.0; double kd = 2.3758; diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index b289641a..73a82224 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -8,6 +8,7 @@ import com.stuypulse.stuylib.network.SmartNumber; import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; @@ -24,6 +25,7 @@ import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.GainSchedBehaviorValue; import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -86,6 +88,7 @@ public static class TalonFXConfig { private final FeedbackConfigs feedbackConfigs = new FeedbackConfigs(); private final MotionMagicConfigs motionMagicConfigs = new MotionMagicConfigs(); private final SoftwareLimitSwitchConfigs softwareLimitSwitchConfigs = new SoftwareLimitSwitchConfigs(); + private final ClosedLoopGeneralConfigs closedLoopGeneralConfigs = new ClosedLoopGeneralConfigs(); private final double[] lastKP = new double[3]; private final double[] lastKI = new double[3]; @@ -235,6 +238,7 @@ public TalonFXConfig withStaticFeedforwardSign(StaticFeedforwardSignValue static configuration.withSlot2(slot2Configs); break; } + return this; } @@ -250,6 +254,31 @@ public TalonFXConfig withGravityType(GravityTypeValue gravityType) { return this; } + public TalonFXConfig withGainSchedBehavior(GainSchedBehaviorValue value, double threshold, int slot) { + closedLoopGeneralConfigs.GainSchedErrorThreshold = threshold; + configuration.withClosedLoopGeneral(closedLoopGeneralConfigs); + + switch(slot) { + case 0: { + slot0Configs.GainSchedBehavior = value; + configuration.withSlot0(slot0Configs); + } + break; + case 1: { + slot1Configs.GainSchedBehavior = value; + configuration.withSlot1(slot1Configs); + } + break; + case 2: { + slot2Configs.GainSchedBehavior = value; + configuration.withSlot2(slot2Configs); + } + break; + } + + return this; + } + // MOTOR OUTPUT CONFIGS public TalonFXConfig withInvertedValue(InvertedValue invertedValue) { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index bbf1b8cc..5527c640 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. * @@ -252,13 +251,16 @@ public interface Turret { public final Rotation2d RIGHT_CORNER = Rotation2d.fromDegrees(0.0); double RESOLUTION_OF_ABSOLUTE_ENCODER = 0.1; + double WRAP_DEBOUNCE = 0.5; Rotation2d MAX_THEORETICAL_ROTATION = Rotation2d.fromDegrees(612); Rotation2d MIN_THEORETICAL_ROTATION = Rotation2d.fromDegrees(-612); public interface Constants { public final double RANGE = 210.0; - public final Transform2d TURRET_OFFSET = new Transform2d(Units.inchesToMeters(0.0), Units.inchesToMeters(0.0), Rotation2d.kZero); + public final double SLOT_SWITCHING_THRESHOLD_ROT = .5; + + public final Transform2d TURRET_OFFSET = new Transform2d(Units.inchesToMeters(-4.0), Units.inchesToMeters(8.0), Rotation2d.kZero); public final double TURRET_HEIGHT = Units.inchesToMeters(0.0); public final double GEAR_RATIO_MOTOR_TO_MECH = (60.0 / 9.0) * (95.0 / 12.0); //1425.0 / 36.0; 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 8895f6bd..93b41872 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java @@ -13,14 +13,18 @@ import com.stuypulse.robot.constants.Settings.Turret.Constants; import com.stuypulse.robot.util.SysId; import com.stuypulse.robot.util.turret.TurretAngleCalculator; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import com.ctre.phoenix6.configs.SlotConfigs; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GainSchedBehaviorValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; @@ -40,15 +44,25 @@ public class TurretImpl extends Turret { private Optional voltageOverride; private final PositionVoltage controller; + private boolean wrapping; + private BStream isWrapping; + public TurretImpl() { turretConfig = new Motors.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) + + .withPIDConstants(Gains.Turret.slot0.kP, Gains.Turret.slot0.kI, Gains.Turret.slot0.kD, 0) + .withFFConstants(Gains.Turret.slot0.kS, Gains.Turret.slot0.kV, Gains.Turret.slot0.kA, 0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 0) + + .withPIDConstants(Gains.Turret.slot1.kP, Gains.Turret.slot1.kI, Gains.Turret.slot1.kD, 1) + .withFFConstants(Gains.Turret.slot1.kS, Gains.Turret.slot1.kV, Gains.Turret.slot1.kA, 1) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 1) + .withSensorToMechanismRatio(Settings.Turret.Constants.GEAR_RATIO_MOTOR_TO_MECH) + // .withGainSchedBehavior(GainSchedBehaviorValue.UseSlot0, Settings.Turret.Constants.SLOT_SWITCHING_THRESHOLD_ROT, 1) .withSoftLimits( false, false, Settings.Turret.Constants.SoftwareLimit.FORWARD_MAX_ROTATIONS, @@ -70,33 +84,35 @@ public TurretImpl() { encoder17tConfig.configure(encoder17t); encoder18tConfig.configure(encoder18t); - seedTurret(); - hasUsedAbsoluteEncoder = false; voltageOverride = Optional.empty(); controller = new PositionVoltage(getTargetAngle().getRotations()) - .withEnableFOC(true); - } + .withEnableFOC(true); + isWrapping = BStream + .create(() -> checkForWrapping()) + .filtered(new BDebounce.Both(Settings.Turret.WRAP_DEBOUNCE)); + } + private Rotation2d getEncoderPos17t() { return Rotation2d.fromRotations(this.encoder17t.getAbsolutePosition().getValueAsDouble()); } - + private Rotation2d getEncoderPos18t() { return Rotation2d.fromRotations(this.encoder18t.getAbsolutePosition().getValueAsDouble()); } - + public Rotation2d getVectorSpaceAngle() { return TurretAngleCalculator.getAbsoluteAngle(getEncoderPos17t().getDegrees(), getEncoderPos18t().getDegrees()); } - + public void zeroEncoders() { double encoderPos17T = encoder17t.getAbsolutePosition().getValueAsDouble(); double encoderPos18T = encoder18t.getAbsolutePosition().getValueAsDouble(); - + encoder17t.getConfigurator().refresh(encoder17tConfig.getConfiguration().MagnetSensor); encoder18t.getConfigurator().refresh(encoder18tConfig.getConfiguration().MagnetSensor); - + double currentOffset17T = encoder17tConfig.getConfiguration().MagnetSensor.MagnetOffset; double currentOffset18T = encoder18tConfig.getConfiguration().MagnetSensor.MagnetOffset; @@ -105,28 +121,33 @@ public void zeroEncoders() { encoder17tConfig.withMagnetOffset(newOffset17T); encoder18tConfig.withMagnetOffset(newOffset18T); - + encoder17tConfig.configure(encoder17t); encoder18tConfig.configure(encoder18t); } public void seedTurret() { - motor.setPosition(getVectorSpaceAngle().getRotations()); + motor.setPosition(0); //TODO: SEED USING CRT INSTEAD OF TS, TS IS TEMP + // motor.setPosition(getVectorSpaceAngle().getRotations()); } - + @Override public Rotation2d getAngle() { - return Rotation2d.fromDegrees(motor.getPosition().getValueAsDouble()); + return Rotation2d.fromRotations(motor.getPosition().getValueAsDouble()); } - + @Override public boolean atTargetAngle() { return Math.abs(getAngle().minus(getTargetAngle()).getDegrees() + 180.0) < Settings.Turret.TOLERANCE_DEG; } - + private double getDelta(double target, double current) { double delta = (target - current) % 360; + SmartDashboard.putNumber("Turret/Debugging: Delta", delta); + SmartDashboard.putNumber("Turret/Debugging: Target", target); + SmartDashboard.putNumber("Turret/Debugging: Current", current); + if (delta > 180.0) delta -= 360; else if (delta < -180) delta += 360; @@ -135,12 +156,25 @@ private double getDelta(double target, double current) { return delta < 0 ? delta + 360 : delta - 360; } + private boolean checkForWrapping() { + double currentAngle = getAngle().getDegrees(); + double actualTargetDeg = currentAngle + getDelta(getTargetAngle().getDegrees(), currentAngle); + + if(!isWrapping.get()) { + return (Math.abs(currentAngle - actualTargetDeg) > 180.0); + } else if(atTargetAngle()) { + return false; + } + + return isWrapping.get(); + } + @Override public void periodic() { super.periodic(); if (!hasUsedAbsoluteEncoder) { - motor.setPosition(getVectorSpaceAngle().getRotations()); + seedTurret(); hasUsedAbsoluteEncoder = true; System.out.println("Absolute Encoder Reset triggered"); } @@ -150,18 +184,29 @@ public void periodic() { SmartDashboard.putNumber("Turret/Delta (deg)", getDelta(getTargetAngle().getDegrees(), getAngle().getDegrees())); SmartDashboard.putNumber("Turret/Actual Target (deg)", actualTargetDeg); + SmartDashboard.putNumber("Turret/Closed Loop Slot" , motor.getClosedLoopSlot().getValue()); if (EnabledSubsystems.TURRET.get()) { if (voltageOverride.isPresent()) { motor.setVoltage(voltageOverride.get()); } else { - motor.setControl(controller.withPosition(actualTargetDeg / 360.0)); + if(getState() == TurretState.TESTING) { + motor.setControl(controller.withPosition(currentAngle + 1.0).withSlot(1)); + } else + if (isWrapping.getAsBoolean()) { + motor.setControl(controller.withPosition(actualTargetDeg / 360.0).withSlot(1)); + } else { + motor.setControl(controller.withPosition(actualTargetDeg / 360.0).withSlot(0)); + } } } else { motor.stopMotor(); } if (Settings.DEBUG_MODE) { + SmartDashboard.putNumber("Turret/Closed Loop Error (deg)", motor.getClosedLoopError().getValueAsDouble() * 360.0); + SmartDashboard.putNumber("Turret/Wrap Point +210", Settings.Turret.Constants.RANGE); + SmartDashboard.putNumber("Turret/Wrap Point -210", -Settings.Turret.Constants.RANGE); SmartDashboard.putNumber("Turret/Encoder18t Abs Position (Rot)", encoder18t.getAbsolutePosition().getValueAsDouble()); SmartDashboard.putNumber("Turret/Encoder17t Abs Position (Rot)", encoder17t.getAbsolutePosition().getValueAsDouble()); SmartDashboard.putNumber("Turret/Vector Space Position (Rot)", getVectorSpaceAngle().getRotations()); From 987e7b9cb63908c613b9acee7071aa4dee597f7e Mon Sep 17 00:00:00 2001 From: Apetrock24 Date: Sat, 28 Feb 2026 14:18:41 -0500 Subject: [PATCH 027/150] FIX; add timings and msc fixes --- simgui.json | 2 +- src/main/java/com/stuypulse/robot/Robot.java | 18 ++++++++--- .../com/stuypulse/robot/util/FMSutil.java | 32 ++++++++++++------- 3 files changed, 35 insertions(+), 17 deletions(-) diff --git a/simgui.json b/simgui.json index 499b2554..a34946be 100644 --- a/simgui.json +++ b/simgui.json @@ -56,7 +56,7 @@ "ClimberHopper": { "open": true }, - "HoodedShooter": { + "FMSUtil": { "open": true }, "States": { diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 8760a0da..5b9d3a30 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -27,6 +27,8 @@ public class Robot extends TimedRobot { private static Alliance alliance; private PowerDistribution powerDistribution; + private FMSutil fmSutil; + public static boolean isBlue() { return alliance == Alliance.Blue; } @@ -76,7 +78,7 @@ public void disabledPeriodic() {} @Override public void autonomousInit() { - FMSutil fmSutil = new FMSutil(true); + fmSutil = new FMSutil(true); CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); auto = robot.getAutonomousCommand(); @@ -87,7 +89,11 @@ public void autonomousInit() { } @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + SmartDashboard.putNumber("FMSUtil/time left in shift", fmSutil.getTimeLeftInShift()); + SmartDashboard.putBoolean("FMSUtil/is current shift?", fmSutil.isActiveShift()); + SmartDashboard.putString("FMSUtil/Field State", fmSutil.getFieldState().toString()); + } @Override public void autonomousExit() {} @@ -98,7 +104,7 @@ public void autonomousExit() {} @Override public void teleopInit() { - FMSutil fmSutil = new FMSutil(); + fmSutil = new FMSutil(); CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); if (auto != null) { @@ -107,7 +113,11 @@ public void teleopInit() { } @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + SmartDashboard.putNumber("FMSUtil/time left in shift", fmSutil.getTimeLeftInShift()); + SmartDashboard.putBoolean("FMSUtil/is current shift?", fmSutil.isActiveShift()); + SmartDashboard.putString("FMSUtil/Field State", fmSutil.getFieldState().toString()); + } @Override public void teleopExit() {} diff --git a/src/main/java/com/stuypulse/robot/util/FMSutil.java b/src/main/java/com/stuypulse/robot/util/FMSutil.java index 3cac5108..52193680 100644 --- a/src/main/java/com/stuypulse/robot/util/FMSutil.java +++ b/src/main/java/com/stuypulse/robot/util/FMSutil.java @@ -1,5 +1,7 @@ package com.stuypulse.robot.util; +import java.util.concurrent.TransferQueue; + import com.fasterxml.jackson.annotation.JsonFormat.Feature; import com.stuypulse.robot.constants.Field; @@ -7,23 +9,24 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class FMSutil { private Timer timer = new Timer(); private FieldState fieldState; private double timeLeft; boolean auto; - private FieldState[] fieldStates = { fieldState.AUTO, fieldState.TRANSITION, fieldState.SHIFT1, fieldState.SHIFT2, + private FieldState[] fieldStates = {fieldState.TRANSITION, fieldState.SHIFT1, fieldState.SHIFT2, fieldState.SHIFT3, fieldState.SHIFT4, fieldState.ENDGAME }; public static enum FieldState { AUTO(0.0, 20.0), - TRANSITION(0.0, 25.0), - SHIFT1(25.0, 50.0), - SHIFT2(50.0, 75.0), - SHIFT3(75.0, 100.0), - SHIFT4(100.0, 125.0), - ENDGAME(125, 155.0); + TRANSITION(0.0, 10), + SHIFT1(10.0, 35.0), + SHIFT2(35.0, 60.0), + SHIFT3(60.0, 85.0), + SHIFT4(85.0, 110.0), + ENDGAME(110.0, 140.0); // public final do.0uble[] shiftStartTimes = {0.0, 10.0, 35.0, 60.0, 85.0, 110.0}; // public final double[] shiftEndTimes = {10.0, 35.0, 60.0, 85.0, 110.0, 140.0}; @@ -56,7 +59,7 @@ public double timeElapsed(double time) { public FMSutil(boolean auto) { timer = new Timer(); timer.start(); - this.auto = true; + this.auto = auto; } public FMSutil() { @@ -79,6 +82,12 @@ public FieldState getFieldState() { public boolean isActiveShift() { boolean wonAuto = didWinAuto(); switch (getFieldState()) { + case AUTO: + return true; + case TRANSITION: + return false; + case ENDGAME: + return true; case SHIFT1: return (wonAuto) ? true : false; case SHIFT2: @@ -87,18 +96,17 @@ public boolean isActiveShift() { return (wonAuto) ? true : false; case SHIFT4: return (wonAuto) ? false : false; - case ENDGAME: - return true; default: return false; } } public boolean didWinAuto() { - String winner = DriverStation.getGameSpecificMessage(); //TODO: find out what message is + String winner = DriverStation.getGameSpecificMessage(); String currentAlliance = (DriverStation.getAlliance().get() == Alliance.Blue) ? "B" : "R"; if (winner.isEmpty()) { DriverStation.reportWarning("Arena Fault, no alliance won data", true); + SmartDashboard.putBoolean("FMSUtil/nodata?", true); return true; // Assume we won :) } else if (currentAlliance.equalsIgnoreCase(winner)) { return true; @@ -107,7 +115,7 @@ public boolean didWinAuto() { } } - public double getTimeLeftInSHift() { + public double getTimeLeftInShift() { return getFieldState().endT - timer.get(); } From 9cebbd66348a380fb4f27c609d3117abf3f31983 Mon Sep 17 00:00:00 2001 From: Mekot Sarder Date: Sat, 28 Feb 2026 14:23:32 -0500 Subject: [PATCH 028/150] FEAT: Added Wheel Radius --- .../swerve/SwerveWheelCharacterization.java | 97 +++++++++++++++++++ .../swerve/CommandSwerveDrivetrain.java | 44 +++++---- 2 files changed, 123 insertions(+), 18 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelCharacterization.java diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelCharacterization.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelCharacterization.java new file mode 100644 index 00000000..8563e029 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelCharacterization.java @@ -0,0 +1,97 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveWheelCharacterization extends Command { + + private static final double ROTATIONAL_RATE = 1.0; + private static final double DRIVE_RADIUS = Math.sqrt(4.5 * 4.5 + 6.5 * 6.5) * 0.0254; + private static final double RAMP_RATE = 0.05; + + private final Timer timer = new Timer(); + private final SlewRateLimiter limiter = new SlewRateLimiter(RAMP_RATE); + + private final CommandSwerveDrivetrain swerve; + + private double[] wheelInitial; + private Rotation2d lastAngle; + private double gyroDelta; + private boolean initalReading; + + public SwerveWheelCharacterization() { + swerve = CommandSwerveDrivetrain.getInstance(); + addRequirements(swerve); + } + + @Override + public void initialize() { + timer.restart(); + limiter.reset(0.0); + gyroDelta = 0.0; + initalReading = false; + } + + @Override + public void execute() { + double speed = limiter.calculate(ROTATIONAL_RATE); + swerve.setControl(swerve.getFieldCentricSwerveRequest() + .withVelocityX(0.0) + .withVelocityY(0.0) + .withRotationalRate(speed) + ); + + if (timer.get() > 1.0) { + if (!initalReading) { + wheelInitial = swerve.getRadiusCharacterizationModulePositions(); + lastAngle = swerve.getPigeon2().getRotation2d(); + gyroDelta = 0.0; + initalReading = true; + } + + Rotation2d currentAngle = swerve.getPigeon2().getRotation2d(); + gyroDelta += Math.abs(currentAngle.minus(lastAngle).getRadians()); + lastAngle = currentAngle; + + double[] wheelCurrent = swerve.getRadiusCharacterizationModulePositions(); + double wheelDelta = 0.0; + for (int i = 0; i < 4; i++) { + wheelDelta += Math.abs(wheelCurrent[i] - wheelInitial[i]) / 4.0; + } + + double wheelRadius = (gyroDelta * DRIVE_RADIUS) / wheelDelta; + + SmartDashboard.putNumber("Radius Characterization/Radius (m)", wheelRadius); + SmartDashboard.putNumber("Radius Characterization/Radius (in.)", wheelRadius * 39.3701); + SmartDashboard.putNumber("Radius Characterization/Gyro Delta", gyroDelta); + SmartDashboard.putNumber("Radius Characterization/Wheel Delta", wheelDelta);} + } + + @Override + public void end(boolean interrupted) { + swerve.setControl(swerve.getFieldCentricSwerveRequest() + .withVelocityX(0).withVelocityY(0).withRotationalRate(0)); + double[] wheelCurrent = swerve.getRadiusCharacterizationModulePositions(); + + double wheelDelta = 0.0; + for (int i = 0; i < 4; i++) { + wheelDelta += (Math.abs(wheelCurrent[i] - wheelInitial[i]) / 4.0); + } + + double wheelRadius = (gyroDelta * DRIVE_RADIUS) / wheelDelta; + + System.out.println("********** Wheel Radius Characterization Results **********"); + System.out.printf("\tWheel Delta: %.9f radians%n", wheelDelta); + System.out.printf("\tGyro Delta: %.9f radians%n", gyroDelta); + System.out.printf("\tWheel Radius: %.9f meters / %.9f inches%n", wheelRadius, wheelRadius * 39.3701); + } + + @Override + public boolean isFinished() { + return false; // driver cancels manually + } } \ No newline at end of file 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 56d549fb..b71c1a09 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -5,12 +5,17 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.swerve; -import static edu.wpi.first.units.Units.Second; -import static edu.wpi.first.units.Units.Volts; - -import com.stuypulse.stuylib.math.Angle; -import com.stuypulse.stuylib.math.Vector2D; - +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.PathPlannerLogging; import com.stuypulse.robot.Robot; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Field; @@ -18,6 +23,8 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.swerve.TunerConstants.TunerSwerveDrivetrain; import com.stuypulse.robot.subsystems.turret.Turret; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.math.Vector2D; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; @@ -27,6 +34,8 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; @@ -34,18 +43,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.swerve.SwerveRequest; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.util.PathPlannerLogging; /** * Class that extends the Phoenix 6 SwerveDrivetrain class and implements * Subsystem so it can easily be used in command-based projects. @@ -405,6 +402,8 @@ private void setChassisSpeeds(ChassisSpeeds robotSpeeds) { .withRotationalRate(robotSpeeds.omegaRadiansPerSecond)); } + + public void drive(Vector2D velocity, double rotation) { ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds( Robot.isBlue() ? velocity.y : -velocity.y, @@ -429,6 +428,15 @@ public Pose2d getTurretPose() { return turretPose; } + public double[] getRadiusCharacterizationModulePositions(){ + double[] positions = new double[4]; + double kDriveGearRatio = 6.48; + for(int i = 0; i < 4; i++){ + positions[i] = getModule(i).getDriveMotor().getPosition().getValueAsDouble() * 2 * Math.PI / kDriveGearRatio; + } + return positions; + } + @Override public void periodic() { Pose2d pose = getPose(); From 582710b9a2681dd5edef5cba1fc5bee5d1cf3028 Mon Sep 17 00:00:00 2001 From: Mekot Sarder Date: Sat, 28 Feb 2026 14:23:52 -0500 Subject: [PATCH 029/150] FEAT: Added Wheel Radius Characterizarion --- .../com/stuypulse/robot/RobotContainer.java | 26 ++++--------------- 1 file changed, 5 insertions(+), 21 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index f7b22412..df0f34a0 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -5,11 +5,6 @@ /***************************************************************/ package com.stuypulse.robot; -import com.stuypulse.stuylib.input.Gamepad; -import com.stuypulse.stuylib.input.gamepads.AutoGamepad; -import com.stuypulse.stuylib.network.SmartBoolean; - -import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.auton.PoachingAutons.BottomOneCyclePoach; import com.stuypulse.robot.commands.auton.PoachingAutons.BottomTwoCyclePoach; @@ -20,34 +15,20 @@ import com.stuypulse.robot.commands.auton.RegularAutons.EightFuel; import com.stuypulse.robot.commands.auton.RegularAutons.TopTwoCycle; import com.stuypulse.robot.commands.auton.Test.BoxTest; -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; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterFerry; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterLeftCorner; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterReverse; -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.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; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; -import com.stuypulse.robot.commands.swerve.SwerveXMode; import com.stuypulse.robot.commands.turret.TurretDefaultCommand; import com.stuypulse.robot.commands.turret.TurretFerry; -import com.stuypulse.robot.commands.turret.TurretIdle; -import com.stuypulse.robot.commands.turret.TurretLeftCorner; -import com.stuypulse.robot.commands.turret.TurretRightCorner; -import com.stuypulse.robot.commands.turret.TurretSeed; import com.stuypulse.robot.commands.turret.TurretShoot; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Ports; @@ -62,13 +43,14 @@ import com.stuypulse.robot.subsystems.turret.Turret; import com.stuypulse.robot.subsystems.vision.LimelightVision; import com.stuypulse.robot.util.PathUtil.AutonConfig; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.input.gamepads.AutoGamepad; +import com.stuypulse.stuylib.network.SmartBoolean; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; public class RobotContainer { @@ -374,6 +356,8 @@ public void configureSysids() { // autonChooser.addOption("SysID Handoff Forwards", handoffSysId.quasistatic(Direction.kForward)); // autonChooser.addOption("SysID Handoff Backwards", handoffSysId.quasistatic(Direction.kReverse)); + // Wheel Radius Characterization + // autonChooser.addOption("Wheel Characterization", new SwerveWheelCharacterization()); } public Command getAutonomousCommand() { From c01e1065bc81d49b36740f4d6632bf661d996c33 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 28 Feb 2026 14:25:29 -0500 Subject: [PATCH 030/150] Feat: Add voltage override commands for ClimberHopper --- .../com/stuypulse/robot/RobotContainer.java | 7 +++++ .../climberhopper/ClimberOverrideDown.java | 27 +++++++++++++++++++ .../climberhopper/ClimberOverrideStop.java | 25 +++++++++++++++++ .../climberhopper/ClimberOverrideUp.java | 26 ++++++++++++++++++ 4 files changed, 85 insertions(+) create mode 100644 src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideDown.java create mode 100644 src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideStop.java create mode 100644 src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideUp.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index a17b57d5..033812dc 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -13,6 +13,9 @@ 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.ClimberOverrideDown; +import com.stuypulse.robot.commands.climberhopper.ClimberOverrideStop; +import com.stuypulse.robot.commands.climberhopper.ClimberOverrideUp; import com.stuypulse.robot.commands.climberhopper.ClimberUp; import com.stuypulse.robot.commands.climberhopper.HopperDown; import com.stuypulse.robot.commands.handoff.HandoffReverse; @@ -112,6 +115,10 @@ public RobotContainer() { SmartDashboard.putData("Field", Field.FIELD2D); SmartDashboard.putData("Intake/Reset Pivot", new SeedPivot()); SmartDashboard.putData("Zero Encoders", new TurretZero()); + + SmartDashboard.putData("ClimberHopper/Override Up", new ClimberOverrideUp()); + SmartDashboard.putData("ClimberHopper/Override Down", new ClimberOverrideDown()); + SmartDashboard.putData("ClimberHopper/Override Stop", new ClimberOverrideStop()); } /****************/ diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideDown.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideDown.java new file mode 100644 index 00000000..e2e137bd --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideDown.java @@ -0,0 +1,27 @@ + +package com.stuypulse.robot.commands.climberhopper; + +import java.util.Optional; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ClimberOverrideDown extends Command { + private final ClimberHopper climberHopper; + + public ClimberOverrideDown() { + climberHopper = ClimberHopper.getInstance(); + } + + @Override + public void initialize() { + climberHopper.setVoltageOverride(Optional.of(-Settings.ClimberHopper.MOTOR_VOLTAGE)); + } + + @Override + public void end(boolean interrupted) { + climberHopper.setVoltageOverride(Optional.empty()); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideStop.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideStop.java new file mode 100644 index 00000000..de1fc321 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideStop.java @@ -0,0 +1,25 @@ +package com.stuypulse.robot.commands.climberhopper; + +import java.util.Optional; + +import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ClimberOverrideStop extends Command { + private final ClimberHopper climberHopper; + + public ClimberOverrideStop() { + climberHopper = ClimberHopper.getInstance(); + } + + @Override + public void initialize() { + climberHopper.setVoltageOverride(Optional.of(0.0)); + } + + @Override + public void end(boolean interrupted) { + climberHopper.setVoltageOverride(Optional.empty()); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideUp.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideUp.java new file mode 100644 index 00000000..a0f25dbe --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideUp.java @@ -0,0 +1,26 @@ +package com.stuypulse.robot.commands.climberhopper; + +import java.util.Optional; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ClimberOverrideUp extends Command { + private final ClimberHopper climberHopper; + + public ClimberOverrideUp() { + climberHopper = ClimberHopper.getInstance(); + } + + @Override + public void initialize() { + climberHopper.setVoltageOverride(Optional.of(Settings.ClimberHopper.MOTOR_VOLTAGE)); + } + + @Override + public void end(boolean interrupted) { + climberHopper.setVoltageOverride(Optional.empty()); + } +} From 419c13053ca201e3d71d2e541a2dab8f0a0c0a45 Mon Sep 17 00:00:00 2001 From: Apetrock24 Date: Sat, 28 Feb 2026 14:29:59 -0500 Subject: [PATCH 031/150] Fix: optomize object creation and add auto override --- src/main/java/com/stuypulse/robot/Robot.java | 4 ++-- src/main/java/com/stuypulse/robot/util/FMSutil.java | 11 +++++++++-- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 5b9d3a30..3a7e46d4 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -104,9 +104,9 @@ public void autonomousExit() {} @Override public void teleopInit() { - fmSutil = new FMSutil(); + if (fmSutil == null) fmSutil = new FMSutil(); + fmSutil.resetTimer(false); CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); - if (auto != null) { auto.cancel(); } diff --git a/src/main/java/com/stuypulse/robot/util/FMSutil.java b/src/main/java/com/stuypulse/robot/util/FMSutil.java index 52193680..638982bc 100644 --- a/src/main/java/com/stuypulse/robot/util/FMSutil.java +++ b/src/main/java/com/stuypulse/robot/util/FMSutil.java @@ -5,6 +5,7 @@ import com.fasterxml.jackson.annotation.JsonFormat.Feature; import com.stuypulse.robot.constants.Field; +import edu.wpi.first.networktables.BooleanEntry; import edu.wpi.first.util.function.BooleanConsumer; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; @@ -16,6 +17,7 @@ public class FMSutil { private FieldState fieldState; private double timeLeft; boolean auto; + private boolean autoOveride = false; private FieldState[] fieldStates = {fieldState.TRANSITION, fieldState.SHIFT1, fieldState.SHIFT2, fieldState.SHIFT3, fieldState.SHIFT4, fieldState.ENDGAME }; @@ -66,8 +68,9 @@ public FMSutil() { this(false); } - public void resetTimer() { + public void resetTimer(boolean auto) { timer.reset(); + this.auto = auto; } public FieldState getFieldState() { @@ -101,13 +104,17 @@ public boolean isActiveShift() { } } + public void overrideFMSAutoVictor(boolean didwin) { + this.autoOveride = didwin; + } + public boolean didWinAuto() { String winner = DriverStation.getGameSpecificMessage(); String currentAlliance = (DriverStation.getAlliance().get() == Alliance.Blue) ? "B" : "R"; if (winner.isEmpty()) { DriverStation.reportWarning("Arena Fault, no alliance won data", true); SmartDashboard.putBoolean("FMSUtil/nodata?", true); - return true; // Assume we won :) + return autoOveride; } else if (currentAlliance.equalsIgnoreCase(winner)) { return true; } else { From 63535672ea2d28a3a26f33dd103e788ca7b7d287 Mon Sep 17 00:00:00 2001 From: Mekot Sarder Date: Sat, 28 Feb 2026 14:30:53 -0500 Subject: [PATCH 032/150] Revert "FEAT: Added Wheel Radius Characterizarion" This reverts commit 582710b9a2681dd5edef5cba1fc5bee5d1cf3028. --- .../com/stuypulse/robot/RobotContainer.java | 26 +++++++++++++++---- 1 file changed, 21 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index df0f34a0..f7b22412 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -5,6 +5,11 @@ /***************************************************************/ package com.stuypulse.robot; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.input.gamepads.AutoGamepad; +import com.stuypulse.stuylib.network.SmartBoolean; + +import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.auton.PoachingAutons.BottomOneCyclePoach; import com.stuypulse.robot.commands.auton.PoachingAutons.BottomTwoCyclePoach; @@ -15,20 +20,34 @@ import com.stuypulse.robot.commands.auton.RegularAutons.EightFuel; import com.stuypulse.robot.commands.auton.RegularAutons.TopTwoCycle; import com.stuypulse.robot.commands.auton.Test.BoxTest; +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; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterFerry; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; +import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterLeftCorner; +import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterReverse; +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.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; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; +import com.stuypulse.robot.commands.swerve.SwerveXMode; import com.stuypulse.robot.commands.turret.TurretDefaultCommand; import com.stuypulse.robot.commands.turret.TurretFerry; +import com.stuypulse.robot.commands.turret.TurretIdle; +import com.stuypulse.robot.commands.turret.TurretLeftCorner; +import com.stuypulse.robot.commands.turret.TurretRightCorner; +import com.stuypulse.robot.commands.turret.TurretSeed; import com.stuypulse.robot.commands.turret.TurretShoot; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Ports; @@ -43,14 +62,13 @@ import com.stuypulse.robot.subsystems.turret.Turret; import com.stuypulse.robot.subsystems.vision.LimelightVision; import com.stuypulse.robot.util.PathUtil.AutonConfig; -import com.stuypulse.stuylib.input.Gamepad; -import com.stuypulse.stuylib.input.gamepads.AutoGamepad; -import com.stuypulse.stuylib.network.SmartBoolean; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; public class RobotContainer { @@ -356,8 +374,6 @@ public void configureSysids() { // autonChooser.addOption("SysID Handoff Forwards", handoffSysId.quasistatic(Direction.kForward)); // autonChooser.addOption("SysID Handoff Backwards", handoffSysId.quasistatic(Direction.kReverse)); - // Wheel Radius Characterization - // autonChooser.addOption("Wheel Characterization", new SwerveWheelCharacterization()); } public Command getAutonomousCommand() { From e05159a46d992abc8f0d3309cd73dfa540be5428 Mon Sep 17 00:00:00 2001 From: Mekot Sarder Date: Sat, 28 Feb 2026 14:43:47 -0500 Subject: [PATCH 033/150] CLEAN: Added auton for wheel --- src/main/java/com/stuypulse/robot/RobotContainer.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index f7b22412..ebaa7adb 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -374,6 +374,9 @@ public void configureSysids() { // autonChooser.addOption("SysID Handoff Forwards", handoffSysId.quasistatic(Direction.kForward)); // autonChooser.addOption("SysID Handoff Backwards", handoffSysId.quasistatic(Direction.kReverse)); + // Wheel Radius Characterization + // autonChooser.addOption("Wheel Characterization", new SwerveWheelCharacterization()); + } public Command getAutonomousCommand() { From 506d6528e186c801785e8644502d0a911f505d96 Mon Sep 17 00:00:00 2001 From: Apetrock24 Date: Sat, 28 Feb 2026 14:53:58 -0500 Subject: [PATCH 034/150] Fix: shift 4 and logging --- simgui-ds.json | 5 +++++ src/main/java/com/stuypulse/robot/Robot.java | 9 +++++---- src/main/java/com/stuypulse/robot/util/FMSutil.java | 8 ++++---- 3 files changed, 14 insertions(+), 8 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 69b1a3cb..00784de4 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "System Joysticks": { + "window": { + "enabled": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 3a7e46d4..605f3096 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -90,8 +90,8 @@ public void autonomousInit() { @Override public void autonomousPeriodic() { - SmartDashboard.putNumber("FMSUtil/time left in shift", fmSutil.getTimeLeftInShift()); - SmartDashboard.putBoolean("FMSUtil/is current shift?", fmSutil.isActiveShift()); + SmartDashboard.putNumber("FMSUtil/Time left in shift", fmSutil.getTimeLeftInShift()); + SmartDashboard.putBoolean("FMSUtil/Is current shift?", fmSutil.isActiveShift()); SmartDashboard.putString("FMSUtil/Field State", fmSutil.getFieldState().toString()); } @@ -114,8 +114,9 @@ public void teleopInit() { @Override public void teleopPeriodic() { - SmartDashboard.putNumber("FMSUtil/time left in shift", fmSutil.getTimeLeftInShift()); - SmartDashboard.putBoolean("FMSUtil/is current shift?", fmSutil.isActiveShift()); + SmartDashboard.putNumber("FMSUtil/Time left in shift", fmSutil.getTimeLeftInShift()); + SmartDashboard.putBoolean("FMSUtil/Is current shift?", fmSutil.isActiveShift()); + SmartDashboard.putBoolean("FMSUtil/Won Auto?", fmSutil.didWinAuto()); SmartDashboard.putString("FMSUtil/Field State", fmSutil.getFieldState().toString()); } diff --git a/src/main/java/com/stuypulse/robot/util/FMSutil.java b/src/main/java/com/stuypulse/robot/util/FMSutil.java index 638982bc..ba334066 100644 --- a/src/main/java/com/stuypulse/robot/util/FMSutil.java +++ b/src/main/java/com/stuypulse/robot/util/FMSutil.java @@ -88,7 +88,7 @@ public boolean isActiveShift() { case AUTO: return true; case TRANSITION: - return false; + return true; case ENDGAME: return true; case SHIFT1: @@ -98,7 +98,7 @@ public boolean isActiveShift() { case SHIFT3: return (wonAuto) ? true : false; case SHIFT4: - return (wonAuto) ? false : false; + return (wonAuto) ? false : true; default: return false; } @@ -113,7 +113,7 @@ public boolean didWinAuto() { String currentAlliance = (DriverStation.getAlliance().get() == Alliance.Blue) ? "B" : "R"; if (winner.isEmpty()) { DriverStation.reportWarning("Arena Fault, no alliance won data", true); - SmartDashboard.putBoolean("FMSUtil/nodata?", true); + SmartDashboard.putBoolean("FMSUtil/No Data on Auto Winner?", true); return autoOveride; } else if (currentAlliance.equalsIgnoreCase(winner)) { return true; @@ -123,7 +123,7 @@ public boolean didWinAuto() { } public double getTimeLeftInShift() { - return getFieldState().endT - timer.get(); + return Math.max(0, getFieldState().endT - timer.get()); } From 962a9c32424012afdf4f3f17f4c69a9641d2d998 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 28 Feb 2026 15:08:53 -0500 Subject: [PATCH 035/150] fix: revert complicated code, turret works, only 1 gain slot, tracking works --- .../robot/subsystems/turret/TurretImpl.java | 44 +++++++++---------- 1 file changed, 22 insertions(+), 22 deletions(-) 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 93b41872..fef3c92d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java @@ -44,8 +44,8 @@ public class TurretImpl extends Turret { private Optional voltageOverride; private final PositionVoltage controller; - private boolean wrapping; - private BStream isWrapping; + // private boolean wrapping; + // private BStream isWrapping; public TurretImpl() { turretConfig = new Motors.TalonFXConfig() @@ -89,9 +89,9 @@ public TurretImpl() { controller = new PositionVoltage(getTargetAngle().getRotations()) .withEnableFOC(true); - isWrapping = BStream - .create(() -> checkForWrapping()) - .filtered(new BDebounce.Both(Settings.Turret.WRAP_DEBOUNCE)); + // isWrapping = BStream + // .create(() -> checkForWrapping()) + // .filtered(new BDebounce.Both(Settings.Turret.WRAP_DEBOUNCE)); } private Rotation2d getEncoderPos17t() { @@ -156,18 +156,18 @@ private double getDelta(double target, double current) { return delta < 0 ? delta + 360 : delta - 360; } - private boolean checkForWrapping() { - double currentAngle = getAngle().getDegrees(); - double actualTargetDeg = currentAngle + getDelta(getTargetAngle().getDegrees(), currentAngle); + // private boolean checkForWrapping() { + // double currentAngle = getAngle().getDegrees(); + // double actualTargetDeg = currentAngle + getDelta(getTargetAngle().getDegrees(), currentAngle); - if(!isWrapping.get()) { - return (Math.abs(currentAngle - actualTargetDeg) > 180.0); - } else if(atTargetAngle()) { - return false; - } + // if(!isWrapping.get()) { + // return (Math.abs(currentAngle - actualTargetDeg) > 180.0); + // } else if(atTargetAngle()) { + // return false; + // } - return isWrapping.get(); - } + // return isWrapping.get(); + // } @Override public void periodic() { @@ -190,14 +190,14 @@ public void periodic() { if (voltageOverride.isPresent()) { motor.setVoltage(voltageOverride.get()); } else { - if(getState() == TurretState.TESTING) { - motor.setControl(controller.withPosition(currentAngle + 1.0).withSlot(1)); - } else - if (isWrapping.getAsBoolean()) { - motor.setControl(controller.withPosition(actualTargetDeg / 360.0).withSlot(1)); - } else { + // if(getState() == TurretState.TESTING) { + // motor.setControl(controller.withPosition(currentAngle + 1.0).withSlot(1)); + // } else + // if (isWrapping.getAsBoolean()) { + // motor.setControl(controller.withPosition(actualTargetDeg / 360.0).withSlot(1)); + // } else { motor.setControl(controller.withPosition(actualTargetDeg / 360.0).withSlot(0)); - } + // } } } else { motor.stopMotor(); From dffb76326f23e71d73e6e71b510a3202943d0748 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 28 Feb 2026 16:25:48 -0500 Subject: [PATCH 036/150] feat: voltage configs for turret + add real button bindings for intake --- .../java/com/stuypulse/robot/RobotContainer.java | 10 +++++----- .../java/com/stuypulse/robot/constants/Motors.java | 13 +++++++++++++ .../subsystems/climberhopper/ClimberHopper.java | 2 +- .../robot/subsystems/turret/TurretImpl.java | 4 +++- 4 files changed, 22 insertions(+), 7 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 033812dc..b40e7f28 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -136,9 +136,9 @@ private void configureDefaultCommands() { private void configureButtonBindings() { // Intake Run Rollers - driver.getLeftTriggerButton() - .onTrue(new IntakeRunRollers()) - .onFalse(new IntakeStopRollers()); + // driver.getLeftTriggerButton() + // .onTrue(new IntakeRunRollers()) + // .onFalse(new IntakeStopRollers()); // Intake Down and On // driver.getRightTriggerButton() @@ -149,10 +149,10 @@ private void configureButtonBindings() { // driver.getBottomButton() // .onTrue(new IntakeBangBang()); - driver.getRightButton() + driver.getRightTriggerButton() .onTrue(new IntakeDeploy()); - driver.getLeftButton() + driver.getLeftTriggerButton() .onTrue(new IntakeStow()); // Reset Heading diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index 73a82224..4084eed6 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -22,6 +22,7 @@ import com.ctre.phoenix6.configs.SlotConfigs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.VoltageConfigs; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; @@ -89,6 +90,7 @@ public static class TalonFXConfig { private final MotionMagicConfigs motionMagicConfigs = new MotionMagicConfigs(); private final SoftwareLimitSwitchConfigs softwareLimitSwitchConfigs = new SoftwareLimitSwitchConfigs(); private final ClosedLoopGeneralConfigs closedLoopGeneralConfigs = new ClosedLoopGeneralConfigs(); + private final VoltageConfigs voltageConfigs = new VoltageConfigs(); private final double[] lastKP = new double[3]; private final double[] lastKI = new double[3]; @@ -352,6 +354,17 @@ public TalonFXConfig withCurrentLimitEnable(boolean enabled) { return this; } + // VOLTAGE LIMIT CONFIGS + + public TalonFXConfig withMaxVoltage(double forwardPeak, double reversePeak) { + voltageConfigs.PeakForwardVoltage = forwardPeak; + voltageConfigs.PeakReverseVoltage = reversePeak; + + configuration.withVoltage(voltageConfigs); + + return this; + } + // SOFTWARE LIMIT CONFIGS public TalonFXConfig withSoftLimits(boolean forwardEnable, boolean reverseEnable, double forwardThreshold, double reverseThreshold) { 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 35197ae1..63160b9b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java @@ -49,7 +49,7 @@ public double getTargetHeight() { private ClimberHopperState state; protected ClimberHopper() { - this.state = ClimberHopperState.CLIMBER_UP; + this.state = ClimberHopperState.HOPPER_DOWN; } public ClimberHopperState getState() { 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 fef3c92d..132776b5 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java @@ -66,7 +66,9 @@ public TurretImpl() { .withSoftLimits( false, false, Settings.Turret.Constants.SoftwareLimit.FORWARD_MAX_ROTATIONS, - Settings.Turret.Constants.SoftwareLimit.BACKWARDS_MAX_ROTATIONS); + Settings.Turret.Constants.SoftwareLimit.BACKWARDS_MAX_ROTATIONS) + + .withMaxVoltage(6, -6); encoder17tConfig = new Motors.CANCoderConfig() .withSensorDirection(SensorDirectionValue.Clockwise_Positive) From 92bb0d957719e1c292d63d0bea2a46fdda205878 Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Sun, 1 Mar 2026 15:31:22 -0500 Subject: [PATCH 037/150] FEAT: ramp omega manually to ensure consistency in tests (deterministic approach), increase target omega --- .../com/stuypulse/robot/RobotContainer.java | 3 +- .../swerve/SwerveWheelCharacterization.java | 97 ------------- .../SwerveWheelRadiusCharacterization.java | 128 ++++++++++++++++++ .../swerve/CommandSwerveDrivetrain.java | 2 +- 4 files changed, 131 insertions(+), 99 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelCharacterization.java create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index c4d5e715..7c704f7a 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -49,6 +49,7 @@ import com.stuypulse.robot.commands.swerve.SwerveClimbAlign; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; +import com.stuypulse.robot.commands.swerve.SwerveWheelRadiusCharacterization; import com.stuypulse.robot.commands.swerve.SwerveXMode; import com.stuypulse.robot.commands.turret.TurretDefaultCommand; import com.stuypulse.robot.commands.turret.TurretAnalog; @@ -417,7 +418,7 @@ public void configureSysids() { autonChooser.addOption("SysID Spindexer Quasi Backwards", spindexerSysId.quasistatic(Direction.kReverse)); // Wheel Radius Characterization - // autonChooser.addOption("Wheel Characterization", new SwerveWheelCharacterization()); + autonChooser.addOption("Wheel Characterization", new SwerveWheelRadiusCharacterization()); SysIdRoutine handoffSysId = handoff.getSysIdRoutine(); autonChooser.addOption("SysID Handoff Dynamic Forward", handoffSysId.dynamic(Direction.kForward)); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelCharacterization.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelCharacterization.java deleted file mode 100644 index 8563e029..00000000 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelCharacterization.java +++ /dev/null @@ -1,97 +0,0 @@ -package com.stuypulse.robot.commands.swerve; - -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; - -public class SwerveWheelCharacterization extends Command { - - private static final double ROTATIONAL_RATE = 1.0; - private static final double DRIVE_RADIUS = Math.sqrt(4.5 * 4.5 + 6.5 * 6.5) * 0.0254; - private static final double RAMP_RATE = 0.05; - - private final Timer timer = new Timer(); - private final SlewRateLimiter limiter = new SlewRateLimiter(RAMP_RATE); - - private final CommandSwerveDrivetrain swerve; - - private double[] wheelInitial; - private Rotation2d lastAngle; - private double gyroDelta; - private boolean initalReading; - - public SwerveWheelCharacterization() { - swerve = CommandSwerveDrivetrain.getInstance(); - addRequirements(swerve); - } - - @Override - public void initialize() { - timer.restart(); - limiter.reset(0.0); - gyroDelta = 0.0; - initalReading = false; - } - - @Override - public void execute() { - double speed = limiter.calculate(ROTATIONAL_RATE); - swerve.setControl(swerve.getFieldCentricSwerveRequest() - .withVelocityX(0.0) - .withVelocityY(0.0) - .withRotationalRate(speed) - ); - - if (timer.get() > 1.0) { - if (!initalReading) { - wheelInitial = swerve.getRadiusCharacterizationModulePositions(); - lastAngle = swerve.getPigeon2().getRotation2d(); - gyroDelta = 0.0; - initalReading = true; - } - - Rotation2d currentAngle = swerve.getPigeon2().getRotation2d(); - gyroDelta += Math.abs(currentAngle.minus(lastAngle).getRadians()); - lastAngle = currentAngle; - - double[] wheelCurrent = swerve.getRadiusCharacterizationModulePositions(); - double wheelDelta = 0.0; - for (int i = 0; i < 4; i++) { - wheelDelta += Math.abs(wheelCurrent[i] - wheelInitial[i]) / 4.0; - } - - double wheelRadius = (gyroDelta * DRIVE_RADIUS) / wheelDelta; - - SmartDashboard.putNumber("Radius Characterization/Radius (m)", wheelRadius); - SmartDashboard.putNumber("Radius Characterization/Radius (in.)", wheelRadius * 39.3701); - SmartDashboard.putNumber("Radius Characterization/Gyro Delta", gyroDelta); - SmartDashboard.putNumber("Radius Characterization/Wheel Delta", wheelDelta);} - } - - @Override - public void end(boolean interrupted) { - swerve.setControl(swerve.getFieldCentricSwerveRequest() - .withVelocityX(0).withVelocityY(0).withRotationalRate(0)); - double[] wheelCurrent = swerve.getRadiusCharacterizationModulePositions(); - - double wheelDelta = 0.0; - for (int i = 0; i < 4; i++) { - wheelDelta += (Math.abs(wheelCurrent[i] - wheelInitial[i]) / 4.0); - } - - double wheelRadius = (gyroDelta * DRIVE_RADIUS) / wheelDelta; - - System.out.println("********** Wheel Radius Characterization Results **********"); - System.out.printf("\tWheel Delta: %.9f radians%n", wheelDelta); - System.out.printf("\tGyro Delta: %.9f radians%n", gyroDelta); - System.out.printf("\tWheel Radius: %.9f meters / %.9f inches%n", wheelRadius, wheelRadius * 39.3701); - } - - @Override - public boolean isFinished() { - return false; // driver cancels manually - } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java new file mode 100644 index 00000000..2c68d4a1 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java @@ -0,0 +1,128 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveWheelRadiusCharacterization extends Command { + + /* + * - Triple check the track width and length + * - Double check the gear ratio in getWheelDrivePositionsRadians() + * - Allow the robot to rotate 3-5 times when running the routine + */ + + private static final double HALF_TRACK_WIDTH_INCHES = 9.0 / 2; + private static final double HALF_TRACK_LENGTH_INCHES = 13.0 / 2; + + private static final double DRIVE_RADIUS_INCHES = Math.sqrt(HALF_TRACK_WIDTH_INCHES * HALF_TRACK_WIDTH_INCHES + HALF_TRACK_LENGTH_INCHES * HALF_TRACK_LENGTH_INCHES); + private static final double DRIVE_RADIUS_METERS = Units.inchesToMeters(DRIVE_RADIUS_INCHES); + + private static final double TARGET_ROTATIONAL_RATE = 1.5; // rad/s about center of rotation + private static final double RAMP_TIME = 0.75; // seconds to reach target + // private static final double RAMP_RATE = 0.05; + + private final Timer timer = new Timer(); + // private final SlewRateLimiter limiter = new SlewRateLimiter(RAMP_RATE); + + private final CommandSwerveDrivetrain swerve; + + private double[] wheelInitialRad; + private Rotation2d lastAngle; + private double gyroDeltaRad; + private boolean initalReading; + + public SwerveWheelRadiusCharacterization() { + swerve = CommandSwerveDrivetrain.getInstance(); + addRequirements(swerve); + } + + @Override + public void initialize() { + timer.restart(); + // limiter.reset(0.0); + gyroDeltaRad = 0.0; + initalReading = false; + } + + @Override + public void execute() { + double elapsed = timer.get(); + + double commandedRate; + + if (elapsed < RAMP_TIME) { + commandedRate = TARGET_ROTATIONAL_RATE * (elapsed / RAMP_TIME); // Ramping omega proportional to RAMP_TIME + } else { + commandedRate = TARGET_ROTATIONAL_RATE; // Steady state omega + } + + // Go back to WPILib SlewRateLimiter if manual ramping doesn't work well + // double speed = limiter.calculate(TARGET_ROTATIONAL_RATE); + + swerve.setControl(swerve.getFieldCentricSwerveRequest() + .withVelocityX(0.0) + .withVelocityY(0.0) + .withRotationalRate(commandedRate) + ); + + // Wait half a second after reaching target omega to start data collection + if (timer.get() > RAMP_TIME + 0.5) { + if (!initalReading) { + wheelInitialRad = swerve.getWheelDrivePositionsRadians(); + lastAngle = swerve.getPigeon2().getRotation2d(); + gyroDeltaRad = 0.0; + initalReading = true; + } + + Rotation2d currentAngle = swerve.getPigeon2().getRotation2d(); + gyroDeltaRad += Math.abs(currentAngle.minus(lastAngle).getRadians()); + lastAngle = currentAngle; + + double[] wheelCurrentRad = swerve.getWheelDrivePositionsRadians(); + double wheelDeltaRad = 0.0; + for (int i = 0; i < 4; i++) { + wheelDeltaRad += Math.abs(wheelCurrentRad[i] - wheelInitialRad[i]) / 4.0; + } + + double wheelRadius = (gyroDeltaRad * DRIVE_RADIUS_METERS) / wheelDeltaRad; + + SmartDashboard.putNumber("Radius Characterization/Radius (m)", wheelRadius); + SmartDashboard.putNumber("Radius Characterization/Radius (in.)", Units.metersToInches(wheelRadius)); + SmartDashboard.putNumber("Radius Characterization/Gyro Delta (rad)", gyroDeltaRad); + SmartDashboard.putNumber("Radius Characterization/Wheel Delta (rad)", wheelDeltaRad); + } + } + + @Override + public void end(boolean interrupted) { + swerve.setControl(swerve.getFieldCentricSwerveRequest() + .withVelocityX(0) + .withVelocityY(0) + .withRotationalRate(0)); + + double[] wheelCurrentRad = swerve.getWheelDrivePositionsRadians(); + + double wheelDeltaRad = 0.0; + for (int i = 0; i < 4; i++) { + wheelDeltaRad += (Math.abs(wheelCurrentRad[i] - wheelInitialRad[i]) / 4.0); + } + + double wheelRadiusMeters = (gyroDeltaRad * DRIVE_RADIUS_METERS) / wheelDeltaRad; + double wheelRadiusInches = Units.metersToInches(wheelRadiusMeters); + + System.out.println("********** Wheel Radius Characterization Results **********"); + System.out.printf("\tWheel Delta: %.9f radians%n", wheelDeltaRad); + System.out.printf("\tGyro Delta: %.9f radians%n", gyroDeltaRad); + System.out.printf("\tWheel Radius: %.9f meters / %.9f inches%n", wheelRadiusMeters, wheelRadiusInches); + } + + @Override + public boolean isFinished() { + return false; // driver cancels manually + } } \ No newline at end of file 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 b71c1a09..23e2d87f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -428,7 +428,7 @@ public Pose2d getTurretPose() { return turretPose; } - public double[] getRadiusCharacterizationModulePositions(){ + public double[] getWheelDrivePositionsRadians(){ double[] positions = new double[4]; double kDriveGearRatio = 6.48; for(int i = 0; i < 4; i++){ From 1c055f55784ddc8c21149a4c98b2df854a8ce9eb Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Sun, 1 Mar 2026 16:03:11 -0500 Subject: [PATCH 038/150] FIX: add current draw key to logging --- .../climberhopper/ClimberHopperImpl.java | 6 ++- .../robot/subsystems/handoff/HandoffImpl.java | 2 + .../hoodedshooter/hood/HoodImpl.java | 2 + .../hoodedshooter/shooter/ShooterImpl.java | 3 ++ .../robot/subsystems/intake/IntakeImpl.java | 4 ++ .../subsystems/spindexer/SpindexerImpl.java | 3 ++ .../robot/subsystems/turret/TurretImpl.java | 51 ++----------------- 7 files changed, 22 insertions(+), 49 deletions(-) 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 47957af4..8be584a3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java @@ -105,14 +105,16 @@ public void periodic() { motor.stopMotor(); } - SmartDashboard.putBoolean("ClimberHopper/Stalling", getStalling()); - if (Settings.DEBUG_MODE) { + SmartDashboard.putBoolean("ClimberHopper/Stalling", getStalling()); + 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()); SmartDashboard.putNumber("ClimberHopper/Stator Current", motor.getStatorCurrent().getValueAsDouble()); + + SmartDashboard.putNumber("Current Draws/ClimberHopper (amps)", motor.getSupplyCurrent().getValueAsDouble()); } } } \ No newline at end of file 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 5f3b16b1..ce8dd761 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -75,6 +75,8 @@ public void periodic() { SmartDashboard.putNumber("Handoff/Voltage", motor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Handoff/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Handoff/Stator Current", motor.getStatorCurrent().getValueAsDouble()); + + SmartDashboard.putNumber("Current Draws/Handoff (amps)", motor.getSupplyCurrent().getValueAsDouble()); } } 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 733de053..dd0d6116 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 @@ -111,6 +111,8 @@ public void periodic() { SmartDashboard.putBoolean("HoodedShooter/Hood/Has Used Absolute Encoder", hasUsedAbsoluteEncoder); SmartDashboard.putNumber("InterpolationTesting/Hood Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); + + SmartDashboard.putNumber("Current Draws/Hood (amps)", hoodMotor.getSupplyCurrent().getValueAsDouble()); } } 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 c56d5fb0..d74245d2 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 @@ -105,6 +105,9 @@ public void periodic() { SmartDashboard.putNumber("InterpolationTesting/Shooter Closed Loop Error", shooterLeader.getClosedLoopError().getValueAsDouble() * 60.0); SmartDashboard.putNumber("InterpolationTesting/Shooter Applied Voltage", shooterLeader.getMotorVoltage().getValueAsDouble()); + + SmartDashboard.putNumber("Current Draws/Shooter Leader (amps)", shooterLeader.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Current Draws/Shooter Follower (amps)", shooterFollower.getSupplyCurrent().getValueAsDouble()); } } 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 30393724..9e6a81ca 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -195,6 +195,10 @@ else if (pivotVoltageOverride.isPresent()) { SmartDashboard.putNumber("Intake/Pivot Temperature (C)", pivot.getDeviceTemp().getValueAsDouble()); SmartDashboard.putNumber("Intake/Leader Temperature (C)", rollerLeader.getDeviceTemp().getValueAsDouble()); SmartDashboard.putNumber("Intake/Follower Temperature (C)", rollerFollower.getDeviceTemp().getValueAsDouble()); + + SmartDashboard.putNumber("Current Draws/Intake Pivot (amps)", pivot.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Current Draws/Intake Roller Leader (amps)", rollerLeader.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Current Draws/Intake Roller Follower (amps)", rollerFollower.getSupplyCurrent().getValueAsDouble()); } } 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 d770c0ba..f2e048e1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -129,6 +129,9 @@ public void periodic() { SmartDashboard.putNumber("Spindexer/Follower Voltage", followerMotor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Spindexer/Follower Supply Current", followerMotor.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Spindexer/Follower Stator Current", followerMotor.getStatorCurrent().getValueAsDouble()); + + SmartDashboard.putNumber("Current Draws/Spindexer Leader (amps)", leadMotor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Current Draws/Spindexer Follower (amps)", followerMotor.getSupplyCurrent().getValueAsDouble()); } } 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 132776b5..6a0b132e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java @@ -13,18 +13,13 @@ import com.stuypulse.robot.constants.Settings.Turret.Constants; import com.stuypulse.robot.util.SysId; import com.stuypulse.robot.util.turret.TurretAngleCalculator; -import com.stuypulse.stuylib.streams.booleans.BStream; -import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; - import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import com.ctre.phoenix6.configs.SlotConfigs; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.GainSchedBehaviorValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; @@ -44,9 +39,6 @@ public class TurretImpl extends Turret { private Optional voltageOverride; private final PositionVoltage controller; - // private boolean wrapping; - // private BStream isWrapping; - public TurretImpl() { turretConfig = new Motors.TalonFXConfig() .withRampRate(0.25) @@ -62,12 +54,11 @@ public TurretImpl() { .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 1) .withSensorToMechanismRatio(Settings.Turret.Constants.GEAR_RATIO_MOTOR_TO_MECH) - // .withGainSchedBehavior(GainSchedBehaviorValue.UseSlot0, Settings.Turret.Constants.SLOT_SWITCHING_THRESHOLD_ROT, 1) + .withSoftLimits( false, false, Settings.Turret.Constants.SoftwareLimit.FORWARD_MAX_ROTATIONS, Settings.Turret.Constants.SoftwareLimit.BACKWARDS_MAX_ROTATIONS) - .withMaxVoltage(6, -6); encoder17tConfig = new Motors.CANCoderConfig() @@ -90,10 +81,6 @@ public TurretImpl() { voltageOverride = Optional.empty(); controller = new PositionVoltage(getTargetAngle().getRotations()) .withEnableFOC(true); - - // isWrapping = BStream - // .create(() -> checkForWrapping()) - // .filtered(new BDebounce.Both(Settings.Turret.WRAP_DEBOUNCE)); } private Rotation2d getEncoderPos17t() { @@ -145,10 +132,6 @@ public boolean atTargetAngle() { private double getDelta(double target, double current) { double delta = (target - current) % 360; - - SmartDashboard.putNumber("Turret/Debugging: Delta", delta); - SmartDashboard.putNumber("Turret/Debugging: Target", target); - SmartDashboard.putNumber("Turret/Debugging: Current", current); if (delta > 180.0) delta -= 360; else if (delta < -180) delta += 360; @@ -158,19 +141,6 @@ private double getDelta(double target, double current) { return delta < 0 ? delta + 360 : delta - 360; } - // private boolean checkForWrapping() { - // double currentAngle = getAngle().getDegrees(); - // double actualTargetDeg = currentAngle + getDelta(getTargetAngle().getDegrees(), currentAngle); - - // if(!isWrapping.get()) { - // return (Math.abs(currentAngle - actualTargetDeg) > 180.0); - // } else if(atTargetAngle()) { - // return false; - // } - - // return isWrapping.get(); - // } - @Override public void periodic() { super.periodic(); @@ -184,22 +154,11 @@ public void periodic() { double currentAngle = getAngle().getDegrees(); double actualTargetDeg = currentAngle + getDelta(getTargetAngle().getDegrees(), currentAngle); - SmartDashboard.putNumber("Turret/Delta (deg)", getDelta(getTargetAngle().getDegrees(), getAngle().getDegrees())); - SmartDashboard.putNumber("Turret/Actual Target (deg)", actualTargetDeg); - SmartDashboard.putNumber("Turret/Closed Loop Slot" , motor.getClosedLoopSlot().getValue()); - if (EnabledSubsystems.TURRET.get()) { if (voltageOverride.isPresent()) { motor.setVoltage(voltageOverride.get()); } else { - // if(getState() == TurretState.TESTING) { - // motor.setControl(controller.withPosition(currentAngle + 1.0).withSlot(1)); - // } else - // if (isWrapping.getAsBoolean()) { - // motor.setControl(controller.withPosition(actualTargetDeg / 360.0).withSlot(1)); - // } else { - motor.setControl(controller.withPosition(actualTargetDeg / 360.0).withSlot(0)); - // } + motor.setControl(controller.withPosition(actualTargetDeg / 360.0).withSlot(0)); } } else { motor.stopMotor(); @@ -207,17 +166,15 @@ public void periodic() { if (Settings.DEBUG_MODE) { SmartDashboard.putNumber("Turret/Closed Loop Error (deg)", motor.getClosedLoopError().getValueAsDouble() * 360.0); - SmartDashboard.putNumber("Turret/Wrap Point +210", Settings.Turret.Constants.RANGE); - SmartDashboard.putNumber("Turret/Wrap Point -210", -Settings.Turret.Constants.RANGE); SmartDashboard.putNumber("Turret/Encoder18t Abs Position (Rot)", encoder18t.getAbsolutePosition().getValueAsDouble()); SmartDashboard.putNumber("Turret/Encoder17t Abs Position (Rot)", encoder17t.getAbsolutePosition().getValueAsDouble()); - SmartDashboard.putNumber("Turret/Vector Space Position (Rot)", getVectorSpaceAngle().getRotations()); SmartDashboard.putNumber("Turret/Vector Space Position (Deg)", getVectorSpaceAngle().getDegrees()); 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); SmartDashboard.putNumber("Turret/Stator Current", motor.getStatorCurrent().getValueAsDouble()); SmartDashboard.putNumber("Turret/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); + + SmartDashboard.putNumber("Current Draws/Turret (amps)", motor.getSupplyCurrent().getValueAsDouble()); } } From 1d615297b84d49c347f4090c6a312f6112c07d12 Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Sun, 1 Mar 2026 16:09:07 -0500 Subject: [PATCH 039/150] CLEAN: cleanup logging --- src/main/java/com/stuypulse/robot/Robot.java | 12 +++++------- .../java/com/stuypulse/robot/RobotContainer.java | 12 ++++++------ .../robot/commands/swerve/SwerveDriveDrive.java | 6 +++--- .../stuypulse/robot/constants/DriverConstants.java | 14 ++++++-------- .../stuypulse/robot/subsystems/intake/Intake.java | 3 +++ .../stuypulse/robot/subsystems/turret/Turret.java | 9 +-------- .../robot/subsystems/turret/TurretSim.java | 4 +--- 7 files changed, 25 insertions(+), 35 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index bd513a6c..4c192d32 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -7,7 +7,7 @@ import com.stuypulse.robot.commands.vision.SetMegaTagMode; import com.stuypulse.robot.subsystems.vision.LimelightVision; -import com.stuypulse.robot.util.FMSutil; +import com.stuypulse.robot.util.FMSUtil; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; @@ -27,7 +27,7 @@ public class Robot extends TimedRobot { private static Alliance alliance; private PowerDistribution powerDistribution; - private FMSutil FMSUtil; + private FMSUtil FMSUtil; public static boolean isBlue() { return alliance == Alliance.Blue; @@ -52,13 +52,11 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); SmartDashboard.putNumber("Robot/Voltage of Robot", powerDistribution.getVoltage()); - SmartDashboard.putNumber("Match Time", DriverStation.getMatchTime()); + SmartDashboard.putNumber("Robot/Match Time", DriverStation.getMatchTime()); if (DriverStation.getAlliance().isPresent()) { alliance = DriverStation.getAlliance().get(); } - - SmartDashboard.putNumber("Match Time", DriverStation.getMatchTime()); } /*********************/ @@ -79,7 +77,7 @@ public void disabledPeriodic() {} @Override public void autonomousInit() { - FMSUtil = new FMSutil(true); + FMSUtil = new FMSUtil(true); CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); auto = robot.getAutonomousCommand(); @@ -105,7 +103,7 @@ public void autonomousExit() {} @Override public void teleopInit() { - if (FMSUtil == null) FMSUtil = new FMSutil(); + if (FMSUtil == null) FMSUtil = new FMSUtil(); FMSUtil.resetTimer(false); CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); if (auto != null) { diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 7c704f7a..1ceaa8de 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -111,7 +111,7 @@ public interface EnabledSubsystems { private final Turret turret = Turret.getInstance(); private final HoodedShooter hoodedShooter = HoodedShooter.getInstance(); - // private final Shooter shooter = Shooter.getInstance(); + private final Shooter shooter = Shooter.getInstance(); private final Hood hood = Hood.getInstance(); // Autons @@ -126,12 +126,12 @@ public RobotContainer() { configureSysids(); SmartDashboard.putData("Field", Field.FIELD2D); - SmartDashboard.putData("Intake/Reset Pivot", new SeedPivot()); - SmartDashboard.putData("Zero Encoders", new TurretZero()); + SmartDashboard.putData("Robot/Reset Pivot", new SeedPivot()); + SmartDashboard.putData("Robot/Zero Encoders", new TurretZero()); - SmartDashboard.putData("ClimberHopper/Override Up", new ClimberOverrideUp()); - SmartDashboard.putData("ClimberHopper/Override Down", new ClimberOverrideDown()); - SmartDashboard.putData("ClimberHopper/Override Stop", new ClimberOverrideStop()); + SmartDashboard.putData("Robot/Override Up", new ClimberOverrideUp()); + SmartDashboard.putData("Robot/Override Down", new ClimberOverrideDown()); + SmartDashboard.putData("Robot/Override Stop", new ClimberOverrideStop()); } /****************/ diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java index df7fb7a9..33fd1e47 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java @@ -38,7 +38,7 @@ public SwerveDriveDrive(Gamepad driver) { .filtered( new VDeadZone(Drive.DEADBAND), x -> x.clamp(1), - x -> x.pow(Drive.POWER.get()), + x -> x.pow(Drive.POWER), x -> x.mul(Swerve.Constraints.MAX_VELOCITY_M_PER_S), new VRateLimit(Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED), new VLowPassFilter(Drive.RC) @@ -46,8 +46,8 @@ public SwerveDriveDrive(Gamepad driver) { turn = IStream.create(driver::getRightX) .filtered( - x -> SLMath.deadband(x, Turn.DEADBAND.get()), - x -> SLMath.spow(x, Turn.POWER.get()), + x -> SLMath.deadband(x, Turn.DEADBAND), + x -> SLMath.spow(x, Turn.POWER), x -> x * Swerve.Constraints.MAX_ANGULAR_VEL_RAD_PER_S, new LowPassFilter(Turn.RC) ); diff --git a/src/main/java/com/stuypulse/robot/constants/DriverConstants.java b/src/main/java/com/stuypulse/robot/constants/DriverConstants.java index a997419c..b0067fe5 100644 --- a/src/main/java/com/stuypulse/robot/constants/DriverConstants.java +++ b/src/main/java/com/stuypulse/robot/constants/DriverConstants.java @@ -14,16 +14,14 @@ public interface Driver { double BUZZ_INTENSITY = 1.0; public interface Drive { - SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.05); - - SmartNumber RC = new SmartNumber("Driver Settings/Drive/RC", 0.05); - SmartNumber POWER = new SmartNumber("Driver Settings/Drive/Power", 2); + double DEADBAND = 0.05; + double RC = 0.05; + int POWER = 2; } public interface Turn { - SmartNumber DEADBAND = new SmartNumber("Driver Settings/Turn/Deadband", 0.05); - - SmartNumber RC = new SmartNumber("Driver Settings/Turn/RC", 0.05); - SmartNumber POWER = new SmartNumber("Driver Settings/Turn/Power", 2); + double DEADBAND = 0.05; + double RC = 0.05; + int POWER = 2; } } } \ 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 cf6aa142..707e7a36 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -115,6 +115,9 @@ public Rotation2d driverInputToAngle() { @Override public void periodic() { + SmartDashboard.putString("States/Intake Pivot", getPivotState().toString()); + SmartDashboard.putString("States/Intake Roller", getRollerState().toString()); + SmartDashboard.putString("Intake/Pivot State", getPivotState().toString()); SmartDashboard.putString("Intake/Roller State", getRollerState().toString()); 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 1bfdf4a9..bedf2a75 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java @@ -105,6 +105,7 @@ public void periodic() { SmartDashboard.putString("Turret/State", state.name()); SmartDashboard.putString("States/Turret", state.name()); SmartDashboard.putNumber("Turret/Target Angle", getTargetAngle().getDegrees()); + SmartDashboard.putBoolean("Turret/At Target Angle?", atTargetAngle()); if (Settings.DEBUG_MODE) { if (EnabledSubsystems.TURRET.get()) { @@ -130,14 +131,6 @@ public Rotation2d getPointAtTargetAngle(Pose2d targetPose) { double crossProduct = zeroVector.x * turretToTarget.y - zeroVector.y * turretToTarget.x; double dotProduct = zeroVector.dot(turretToTarget); - SmartDashboard.putNumber("Turret/Turret to Target Vector X", turretToTarget.x); - SmartDashboard.putNumber("Turret/Turret to Target Vector Y", turretToTarget.y); - - SmartDashboard.putNumber("Turret/Target Pose X", targetPose.getX()); - SmartDashboard.putNumber("Turret/Target Pose Y", targetPose.getY()); - SmartDashboard.putNumber("Turret/Zero Vector X", zeroVector.x); - SmartDashboard.putNumber("Turret/Zero Vector Y", zeroVector.y); - Rotation2d targetAngle = (Robot.isReal() ? Rotation2d.fromRadians(-Math.atan2(crossProduct, dotProduct)) : Rotation2d.fromRadians(Math.atan2(crossProduct, dotProduct))); diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java index 48729621..b6348f06 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java @@ -107,9 +107,7 @@ public void periodic() { SmartDashboard.putNumber("Turret/Constraints/Max Vel (deg per s)", Units.radiansToDegrees(maxAngularVelRadiansPerSecond)); SmartDashboard.putNumber("Turret/Constraints/Max Accel (deg per s per s)", Units.radiansToDegrees(maxAngularAccelRadiansPerSecondSquared)); - SmartDashboard.putNumber("Turret/Setpoint (deg)", Units.radiansToDegrees(setpoint.position)); - SmartDashboard.putNumber("Turret/Target (deg)", Units.radiansToDegrees(getTargetAngle().getRadians())); - SmartDashboard.putBoolean("Turret/At Target", atTargetAngle()); + SmartDashboard.putNumber("Turret/Motion Profile Setpoint (deg)", Units.radiansToDegrees(setpoint.position)); SmartDashboard.putNumber("Turret/Error: abs(turret - target) (deg)", Math.abs(getAngle().minus(getTargetAngle()).getDegrees())); SmartDashboard.putNumber("Turret/Current Angle (deg)", sim.getOutput(0)); From 662c48c74e34ef63388cb904c358dc9c9dd15c86 Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Sun, 1 Mar 2026 16:45:37 -0500 Subject: [PATCH 040/150] CLEAN: FMSUtil --- src/main/java/com/stuypulse/robot/Robot.java | 29 ++-- .../com/stuypulse/robot/util/FMSutil.java | 151 ++++++++---------- 2 files changed, 86 insertions(+), 94 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 4c192d32..056ef9b2 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -27,7 +27,7 @@ public class Robot extends TimedRobot { private static Alliance alliance; private PowerDistribution powerDistribution; - private FMSUtil FMSUtil; + private FMSUtil fmsUtil; public static boolean isBlue() { return alliance == Alliance.Blue; @@ -40,11 +40,12 @@ public static boolean isBlue() { @Override public void robotInit() { robot = new RobotContainer(); + powerDistribution = new PowerDistribution(); + fmsUtil = new FMSUtil(false); DataLogManager.start(); SignalLogger.start(); - powerDistribution = new PowerDistribution(); } @Override @@ -57,6 +58,10 @@ public void robotPeriodic() { if (DriverStation.getAlliance().isPresent()) { alliance = DriverStation.getAlliance().get(); } + + SmartDashboard.putNumber("FMSUtil/Time Left In Shift", fmsUtil.getTimeLeftInShift()); + SmartDashboard.putBoolean("FMSUtil/Is Active Shift?", fmsUtil.isActiveShift()); + SmartDashboard.putString("FMSUtil/Field State", fmsUtil.getCurrentFieldState().toString()); } /*********************/ @@ -77,7 +82,8 @@ public void disabledPeriodic() {} @Override public void autonomousInit() { - FMSUtil = new FMSUtil(true); + fmsUtil.restartTimer(true); + CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); auto = robot.getAutonomousCommand(); @@ -88,11 +94,7 @@ public void autonomousInit() { } @Override - public void autonomousPeriodic() { - SmartDashboard.putNumber("FMSUtil/Time left in shift", FMSUtil.getTimeLeftInShift()); - SmartDashboard.putBoolean("FMSUtil/Is current shift?", FMSUtil.isActiveShift()); - SmartDashboard.putString("FMSUtil/Field State", FMSUtil.getFieldState().toString()); - } + public void autonomousPeriodic() {} @Override public void autonomousExit() {} @@ -103,20 +105,19 @@ public void autonomousExit() {} @Override public void teleopInit() { - if (FMSUtil == null) FMSUtil = new FMSUtil(); - FMSUtil.resetTimer(false); + fmsUtil.restartTimer(false); + CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); + if (auto != null) { auto.cancel(); } + + SmartDashboard.putBoolean("FMSUtil/Won Auto?", fmsUtil.didWinAuto()); } @Override public void teleopPeriodic() { - SmartDashboard.putNumber("FMSUtil/Time left in shift", FMSUtil.getTimeLeftInShift()); - SmartDashboard.putBoolean("FMSUtil/Is current shift?", FMSUtil.isActiveShift()); - SmartDashboard.putBoolean("FMSUtil/Won Auto?", FMSUtil.didWinAuto()); - SmartDashboard.putString("FMSUtil/Field State", FMSUtil.getFieldState().toString()); } @Override diff --git a/src/main/java/com/stuypulse/robot/util/FMSutil.java b/src/main/java/com/stuypulse/robot/util/FMSutil.java index ba334066..bb1de2d5 100644 --- a/src/main/java/com/stuypulse/robot/util/FMSutil.java +++ b/src/main/java/com/stuypulse/robot/util/FMSutil.java @@ -1,130 +1,121 @@ package com.stuypulse.robot.util; -import java.util.concurrent.TransferQueue; +import java.util.Optional; -import com.fasterxml.jackson.annotation.JsonFormat.Feature; -import com.stuypulse.robot.constants.Field; - -import edu.wpi.first.networktables.BooleanEntry; -import edu.wpi.first.util.function.BooleanConsumer; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -public class FMSutil { - private Timer timer = new Timer(); - private FieldState fieldState; - private double timeLeft; - boolean auto; - private boolean autoOveride = false; - private FieldState[] fieldStates = {fieldState.TRANSITION, fieldState.SHIFT1, fieldState.SHIFT2, - fieldState.SHIFT3, fieldState.SHIFT4, fieldState.ENDGAME }; +public class FMSUtil { - public static enum FieldState { + private final Timer timer = new Timer(); + private boolean autoMode; + private boolean autoOverride = false; + + public enum FieldState { AUTO(0.0, 20.0), - TRANSITION(0.0, 10), - SHIFT1(10.0, 35.0), - SHIFT2(35.0, 60.0), - SHIFT3(60.0, 85.0), - SHIFT4(85.0, 110.0), + TRANSITION(0.0, 10.0), + SHIFT_1(10.0, 35.0), + SHIFT_2(35.0, 60.0), + SHIFT_3(60.0, 85.0), + SHIFT_4(85.0, 110.0), ENDGAME(110.0, 140.0); - // public final do.0uble[] shiftStartTimes = {0.0, 10.0, 35.0, 60.0, 85.0, 110.0}; - // public final double[] shiftEndTimes = {10.0, 35.0, 60.0, 85.0, 110.0, 140.0}; - private double startT; - private double endT; + private final double startTime; + private final double endTime; - private FieldState(double startT, double endT) { - this.startT = startT; - this.endT = endT; + FieldState(double startTime, double endTime) { + this.startTime = startTime; + this.endTime = endTime; } public boolean isActive(double time) { - if (startT <= time && time < endT) { - return true; - } else { - return false; - } + return startTime <= time && time < endTime; } public double timeLeft(double time) { - return endT - time; + return Math.max(0.0, endTime - time); } public double timeElapsed(double time) { - return time - startT; + return Math.max(0.0, time - startTime); } - }; - - - public FMSutil(boolean auto) { - timer = new Timer(); + } + + public FMSUtil(boolean autoMode) { + this.autoMode = autoMode; timer.start(); - this.auto = auto; } - public FMSutil() { - this(false); + public FMSUtil() { + this(true); } - public void resetTimer(boolean auto) { - timer.reset(); - this.auto = auto; + public void restartTimer(boolean autoMode) { + timer.restart(); + this.autoMode = autoMode; } - public FieldState getFieldState() { - FieldState activeState = FieldState.AUTO; - if (auto) return FieldState.AUTO; - for (FieldState state : fieldStates) { - if (state.isActive(timer.get())) { activeState = state; break;} + public FieldState getCurrentFieldState() { + if (autoMode) { + return FieldState.AUTO; + } + + double time = timer.get(); + + for (FieldState state : FieldState.values()) { + if (state != FieldState.AUTO && state.isActive(time)) { + return state; + } } - return activeState; + + return FieldState.ENDGAME; } - + public boolean isActiveShift() { boolean wonAuto = didWinAuto(); - switch (getFieldState()) { - case AUTO: + + switch (getCurrentFieldState()) { + case AUTO: return true; - case TRANSITION: + case TRANSITION: return true; case ENDGAME: return true; - case SHIFT1: - return (wonAuto) ? true : false; - case SHIFT2: - return (wonAuto) ? false : true; - case SHIFT3: - return (wonAuto) ? true : false; - case SHIFT4: - return (wonAuto) ? false : true; + case SHIFT_1: + return wonAuto; + case SHIFT_2: + return !wonAuto; + case SHIFT_3: + return wonAuto; + case SHIFT_4: + return !wonAuto; default: return false; } } - public void overrideFMSAutoVictor(boolean didwin) { - this.autoOveride = didwin; + public void overrideFMSAutoVictor(boolean didWin) { + this.autoOverride = didWin; } public boolean didWinAuto() { - String winner = DriverStation.getGameSpecificMessage(); - String currentAlliance = (DriverStation.getAlliance().get() == Alliance.Blue) ? "B" : "R"; - if (winner.isEmpty()) { - DriverStation.reportWarning("Arena Fault, no alliance won data", true); - SmartDashboard.putBoolean("FMSUtil/No Data on Auto Winner?", true); - return autoOveride; - } else if (currentAlliance.equalsIgnoreCase(winner)) { - return true; - } else { - return false; + String winner = DriverStation.getGameSpecificMessage(); + Optional allianceOpt = DriverStation.getAlliance(); + + if (winner == null || winner.isEmpty() || allianceOpt.isEmpty()) { + DriverStation.reportWarning("No FMS auto winner data available", false); + SmartDashboard.putBoolean("FMSUtil/No Auto Winner Data", true); + return autoOverride; } + + String allianceLetter = allianceOpt.get() == Alliance.Blue ? "B" : "R"; + + return allianceLetter.equalsIgnoreCase(winner); } public double getTimeLeftInShift() { - return Math.max(0, getFieldState().endT - timer.get()); + return getCurrentFieldState().timeLeft(timer.get()); } - - -} +} \ No newline at end of file From ef03e8c94d9608a27e88c8897f78d3a559daf72e Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Sun, 1 Mar 2026 16:59:57 -0500 Subject: [PATCH 041/150] BUG: TurretDefaultCommand is broken, removed from RobotContainer --- src/main/java/com/stuypulse/robot/RobotContainer.java | 9 ++++----- .../robot/commands/turret/TurretDefaultCommand.java | 2 -- src/main/java/com/stuypulse/robot/constants/Field.java | 3 ++- 3 files changed, 6 insertions(+), 8 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 1ceaa8de..e96067b7 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -140,7 +140,6 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); - turret.setDefaultCommand(new TurretDefaultCommand()); // climberHopper.setDefaultCommand(new ClimberHopperDefaultCommand()); } @@ -176,12 +175,12 @@ private void configureButtonBindings() { .onFalse(new SetIMUMode(0)); driver.getTopButton() - .onTrue(new TurretShoot()) + .whileTrue(new TurretShoot()) .onFalse(new TurretIdle()); - driver.getBottomButton() - .onTrue(new TurretAnalog(driver)) - .onFalse(new TurretIdle()); + // driver.getBottomButton() + // .onTrue(new TurretAnalog(driver)) + // .onFalse(new TurretIdle()); // Scoring Routine using Interpolation Settings // driver.getTopButton() diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java index f9f826a6..0210a6c8 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java @@ -1,12 +1,10 @@ package com.stuypulse.robot.commands.turret; import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.subsystems.turret.Turret; import com.stuypulse.robot.subsystems.turret.Turret.TurretState; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Command; public class TurretDefaultCommand extends Command { diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 8d45aed9..1c956ac3 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -52,7 +52,8 @@ 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 + Units.feetToMeters(6), 1.0 + Units.feetToMeters(3), new Rotation2d()); //TODO: GET ACTUAL POS + 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())) { From 5964336289185b3fc24791b547ea3acdc8c7bfce Mon Sep 17 00:00:00 2001 From: Daniel M Date: Sun, 1 Mar 2026 20:32:18 -0500 Subject: [PATCH 042/150] feat: add hot swappable gains to non-swerve subsystems --- .../robot/constants/DriverConstants.java | 2 - .../com/stuypulse/robot/constants/Gains.java | 130 ++++++++++++------ .../robot/subsystems/handoff/HandoffImpl.java | 15 +- .../hoodedshooter/HoodedShooter.java | 1 - .../hoodedshooter/hood/HoodImpl.java | 15 +- .../hoodedshooter/shooter/ShooterImpl.java | 30 +++- .../robot/subsystems/intake/IntakeImpl.java | 16 ++- .../robot/subsystems/turret/TurretImpl.java | 17 ++- .../robot/util/{FMSutil.java => FMSUtil.java} | 0 9 files changed, 164 insertions(+), 62 deletions(-) rename src/main/java/com/stuypulse/robot/util/{FMSutil.java => FMSUtil.java} (100%) diff --git a/src/main/java/com/stuypulse/robot/constants/DriverConstants.java b/src/main/java/com/stuypulse/robot/constants/DriverConstants.java index b0067fe5..e2c9886d 100644 --- a/src/main/java/com/stuypulse/robot/constants/DriverConstants.java +++ b/src/main/java/com/stuypulse/robot/constants/DriverConstants.java @@ -5,8 +5,6 @@ /***************************************************************/ package com.stuypulse.robot.constants; -import com.stuypulse.stuylib.network.SmartNumber; - public interface DriverConstants { public interface Driver { diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index f97d90d9..73232aa9 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -12,47 +12,63 @@ public class Gains { public interface ClimberHopper { - double kP = 1.0; - double kI = 0.0; - double kD = 0.20; + SmartNumber kP = new SmartNumber("ClimberHopper/Gains/kP", 1.0); + SmartNumber kI = new SmartNumber("ClimberHopper/Gains/kI", 0.0); + SmartNumber kD = new SmartNumber("ClimberHopper/Gains/kD", 0.20); - double kS = 0.0; - double kV = 0.123; - double kA = 0.0; + SmartNumber kS = new SmartNumber("ClimberHopper/Gains/kS", 0.0); + SmartNumber kA = new SmartNumber("ClimberHopper/Gains/kA", 0.123); + SmartNumber kV = new SmartNumber("ClimberHopper/Gains/kV", 0.0); + + // double kP = 1.0; + // double kI = 0.0; + // double kD = 0.20; + + // double kS = 0.0; + // double kV = 0.123; + // double kA = 0.0; } public interface HoodedShooter { public interface Shooter { - double kP = 0.45; - double kI = 0.0; - double kD = 0.0; + SmartNumber kP = new SmartNumber("HoodedShooter/Shooter/Gains/kP", 0.45); + SmartNumber kI = new SmartNumber("HoodedShooter/Shooter/Gains/kI", 0.0); + SmartNumber kD = new SmartNumber("HoodedShooter/Shooter/Gains/kD", 0.0); - double kS = 0.0; - double kV = 0.123; - double kA = 0.0; + SmartNumber kS = new SmartNumber("HoodedShooter/Shooter/Gains/kS", 0.0); + SmartNumber kA = new SmartNumber("HoodedShooter/Shooter/Gains/kA", 0.123); + SmartNumber kV = new SmartNumber("HoodedShooter/Shooter/Gains/kV", 0.0); + + // double kP = 0.45; + // double kI = 0.0; + // double kD = 0.0; + + // double kS = 0.0; + // double kV = 0.123; + // double kA = 0.0; } public interface Hood { - double kP = 300.0; - double kI = 0.0; - double kD = 0.0; + SmartNumber kP = new SmartNumber("HoodedShooter/Hood/Gains/kP", 300.0); + SmartNumber kI = new SmartNumber("HoodedShooter/Hood/Gains/kI", 0.0); + SmartNumber kD = new SmartNumber("HoodedShooter/Hood/Gains/kD", 0.0); - double kS = 0.0; - double kV = 0.0; - double kA = 0.0; + SmartNumber kS = new SmartNumber("HoodedShooter/Hood/Gains/kS", 0.0); + SmartNumber kA = new SmartNumber("HoodedShooter/Hood/Gains/kA", 0.0); + SmartNumber kV = new SmartNumber("HoodedShooter/Hood/Gains/kV", 0.0); + + // double kP = 300.0; + // double kI = 0.0; + // double kD = 0.0; + + // double kS = 0.0; + // double kV = 0.0; + // double kA = 0.0; } } public interface Spindexer { - // double kP = 1.20; - // double kI = 0.0; - // double kD = 0.0; - - // double kS = 0.019444; - // double kA = 0.010876; - // double kV = 0.38546; - SmartNumber kP = new SmartNumber("Spindexer/Gains/kP", 1.20); SmartNumber kI = new SmartNumber("Spindexer/Gains/kI", 0.0); SmartNumber kD = new SmartNumber("Spindexer/Gains/kD", 0.0); @@ -64,37 +80,61 @@ public interface Spindexer { public interface Intake { public interface Pivot { - double kP = 100.0; - double kI = 0.0; - double kD = 10.0; + SmartNumber kP = new SmartNumber("Intake/Pivot/Gains/kP", 100.0); + SmartNumber kI = new SmartNumber("Intake/Pivot/Gains/kI", 0.0); + SmartNumber kD = new SmartNumber("Intake/Pivot/Gains/kD", 10.0); - double kS = 0.0; - double kV = 0.12; - double kA = 0.0; + SmartNumber kS = new SmartNumber("Intake/Pivot/Gains/kS", 0.0); + SmartNumber kV = new SmartNumber("Intake/Pivot/Gains/kA", 0.12); + SmartNumber kA = new SmartNumber("Intake/Pivot/Gains/kV", 0.0); + + // double kP = 100.0; // 0.016973 from sysid + // double kI = 0.0; + // double kD = 10.0; + + // double kS = 0.0; // 0.1728 from alpha + // double kA = 0.12; + // double kV = 0.0; double kG = 0.5; } } public interface Handoff { - double kP = 0.00015508; // 0.016973 from sysid - double kI = 0.0; - double kD = 0.0; + SmartNumber kP = new SmartNumber("Handoff/Gains/kP", 0.00015508); + SmartNumber kI = new SmartNumber("Handoff/Gains/kI", 0.0); + SmartNumber kD = new SmartNumber("Handoff/Gains/kD", 140.0); + + SmartNumber kS = new SmartNumber("Handoff/Gains/kS", 0.21149); + SmartNumber kV = new SmartNumber("Handoff/Gains/kV", 0.016329); + SmartNumber kA = new SmartNumber("Handoff/Gains/kA", 0.3652); + + // double kP = 0.00015508; // 0.016973 from sysid + // double kI = 0.0; + // double kD = 0.0; - double kS = 0.21149; // 0.1728 from alpha - double kA = 0.016329; - double kV = 0.3652; + // double kS = 0.21149; // 0.1728 from alpha + // double kA = 0.016329; + // double kV = 0.3652; } public interface Turret { public interface slot0 { - double kP = 1300.0; - double kI = 0.0; - double kD = 140.0; - - double kS = 0.23; // FOUND ON 2/25 PD 8 - double kV = 0.0; - double kA = 0.0; + SmartNumber kP = new SmartNumber("Turret/Gains/kP", 1300.0); + SmartNumber kI = new SmartNumber("Turret/Gains/kI", 0.0); + SmartNumber kD = new SmartNumber("Turret/Gains/kD", 140.0); + + SmartNumber kS = new SmartNumber("Turret/Gains/kS", 0.23); + SmartNumber kV = new SmartNumber("Turret/Gains/kV", 0.0); + SmartNumber kA = new SmartNumber("Turret/Gains/kA", 0.0); + + // double kP = 1300.0; + // double kI = 0.0; + // double kD = 140.0; + + // double kS = 0.23; // FOUND ON 2/25 PD 8 + // double kV = 0.0; + // double kA = 0.0; } public interface slot1 { 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 ce8dd761..c0447923 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -35,8 +35,8 @@ public HandoffImpl() { .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) + .withFFConstants(Gains.Handoff.kS.get(), Gains.Handoff.kV.get(), Gains.Handoff.kA.get(), 0) + .withPIDConstants(Gains.Handoff.kP.get(), Gains.Handoff.kI.get(), Gains.Handoff.kD.get(), 0) .withSensorToMechanismRatio(Settings.Handoff.GEAR_RATIO); motor = new TalonFX(Ports.Handoff.HANDOFF, Ports.RIO); @@ -60,6 +60,17 @@ public void setVoltageOverride(Optional voltage) { public void periodic() { super.periodic(); + handoffConfig.updateGainsConfig( + motor, + 0, + Gains.Handoff.kP, + Gains.Handoff.kI, + Gains.Handoff.kD, + Gains.Handoff.kS, + Gains.Handoff.kV, + Gains.Handoff.kA + ); + if (EnabledSubsystems.HANDOFF.get()) { if (voltageOverride.isPresent()) { motor.setVoltage(voltageOverride.get()); 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 dc549948..fe4a0609 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java @@ -27,7 +27,6 @@ public class HoodedShooter extends SubsystemBase { instance = new HoodedShooterSim(); } } - public static HoodedShooter getInstance(){ return instance; 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 dd0d6116..defc2773 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 @@ -44,8 +44,8 @@ public HoodImpl() { .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) .withInvertedValue(InvertedValue.Clockwise_Positive) - .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) + .withPIDConstants(Gains.HoodedShooter.Hood.kP.get(), Gains.HoodedShooter.Hood.kI.get(), Gains.HoodedShooter.Hood.kD.get(), 0) + .withFFConstants(Gains.HoodedShooter.Hood.kS.get(), Gains.HoodedShooter.Hood.kV.get(), Gains.HoodedShooter.Hood.kA.get(), 0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 0) .withSensorToMechanismRatio(Settings.HoodedShooter.Hood.GEAR_RATIO) .withSoftLimits( @@ -79,6 +79,17 @@ public Rotation2d getHoodAngle() { public void periodic() { super.periodic(); + hoodConfig.updateGainsConfig( + hoodMotor, + 0, + Gains.HoodedShooter.Hood.kP, + Gains.HoodedShooter.Hood.kI, + Gains.HoodedShooter.Hood.kD, + Gains.HoodedShooter.Hood.kS, + Gains.HoodedShooter.Hood.kV, + Gains.HoodedShooter.Hood.kA + ); + if (!hasUsedAbsoluteEncoder) { /* * Example: 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 d74245d2..896ee6a7 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 @@ -39,10 +39,10 @@ public ShooterImpl() { .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) + .withPIDConstants(Gains.HoodedShooter.Shooter.kP.get(), Gains.HoodedShooter.Shooter.kI.get(), + Gains.HoodedShooter.Shooter.kD.get(), 0) + .withFFConstants(Gains.HoodedShooter.Shooter.kS.get(), Gains.HoodedShooter.Shooter.kV.get(), + Gains.HoodedShooter.Shooter.kA.get(), 0) .withSensorToMechanismRatio(Settings.HoodedShooter.Shooter.GEAR_RATIO); shooterLeader = new TalonFX(Ports.HoodedShooter.Shooter.MOTOR_LEAD, Ports.RIO); @@ -77,6 +77,28 @@ public double getFollowerRPM() { public void periodic() { super.periodic(); + shooterConfig.updateGainsConfig( + shooterLeader, + 0, + Gains.HoodedShooter.Shooter.kP, + Gains.HoodedShooter.Shooter.kI, + Gains.HoodedShooter.Shooter.kD, + Gains.HoodedShooter.Shooter.kS, + Gains.HoodedShooter.Shooter.kV, + Gains.HoodedShooter.Shooter.kA + ); + + shooterConfig.updateGainsConfig( + shooterFollower, + 0, + Gains.HoodedShooter.Shooter.kP, + Gains.HoodedShooter.Shooter.kI, + Gains.HoodedShooter.Shooter.kD, + Gains.HoodedShooter.Shooter.kS, + Gains.HoodedShooter.Shooter.kV, + Gains.HoodedShooter.Shooter.kA + ); + if (EnabledSubsystems.SHOOTER.get()) { if (getState() == ShooterState.STOP) { shooterLeader.stopMotor(); 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 9e6a81ca..ae382f71 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -56,8 +56,8 @@ public IntakeImpl() { //.withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) .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) + .withPIDConstants(Gains.Intake.Pivot.kP.get(), Gains.Intake.Pivot.kI.get(), Gains.Intake.Pivot.kD.get(), 0) + .withFFConstants(Gains.Intake.Pivot.kS.get(), Gains.Intake.Pivot.kV.get(), Gains.Intake.Pivot.kA.get(), Gains.Intake.Pivot.kG, 0) .withGravityType(GravityTypeValue.Arm_Cosine) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign, 0) .withSensorToMechanismRatio(Settings.Intake.GEAR_RATIO) @@ -81,7 +81,6 @@ public IntakeImpl() { pivotController = new MotionMagicVoltage(getPivotState().getTargetAngle().getRotations()) .withEnableFOC(true); - rollerController = new DutyCycleOut(getRollerState().getTargetDutyCycle()) .withEnableFOC(true); follower = new Follower(Ports.Intake.ROLLER_LEADER, MotorAlignmentValue.Aligned); @@ -134,6 +133,17 @@ public void seedPivot() { public void periodic() { super.periodic(); + pivotConfig.updateGainsConfig( + pivot, + 0, + Gains.Intake.Pivot.kP, + Gains.Intake.Pivot.kI, + Gains.Intake.Pivot.kD, + Gains.Intake.Pivot.kS, + Gains.Intake.Pivot.kV, + Gains.Intake.Pivot.kA + ); + if (EnabledSubsystems.INTAKE.get()) { if (getPivotState() == PivotState.ANALOG) { // comment out the setControl line if it breaks 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 6a0b132e..9793fd15 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java @@ -45,8 +45,8 @@ public TurretImpl() { .withNeutralMode(NeutralModeValue.Brake) .withInvertedValue(InvertedValue.Clockwise_Positive) - .withPIDConstants(Gains.Turret.slot0.kP, Gains.Turret.slot0.kI, Gains.Turret.slot0.kD, 0) - .withFFConstants(Gains.Turret.slot0.kS, Gains.Turret.slot0.kV, Gains.Turret.slot0.kA, 0) + .withPIDConstants(Gains.Turret.slot0.kP.get(), Gains.Turret.slot0.kI.get(), Gains.Turret.slot0.kD.get(), 0) + .withFFConstants(Gains.Turret.slot0.kS.get(), Gains.Turret.slot0.kV.get(), Gains.Turret.slot0.kA.get(), 0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 0) .withPIDConstants(Gains.Turret.slot1.kP, Gains.Turret.slot1.kI, Gains.Turret.slot1.kD, 1) @@ -59,7 +59,7 @@ public TurretImpl() { false, false, Settings.Turret.Constants.SoftwareLimit.FORWARD_MAX_ROTATIONS, Settings.Turret.Constants.SoftwareLimit.BACKWARDS_MAX_ROTATIONS) - .withMaxVoltage(6, -6); + .withMaxVoltage(6, -6); //TODO: VERIFY MAX VOLTAGE encoder17tConfig = new Motors.CANCoderConfig() .withSensorDirection(SensorDirectionValue.Clockwise_Positive) @@ -145,6 +145,17 @@ private double getDelta(double target, double current) { public void periodic() { super.periodic(); + turretConfig.updateGainsConfig( + motor, + 0, + Gains.Turret.slot0.kP, + Gains.Turret.slot0.kI, + Gains.Turret.slot0.kD, + Gains.Turret.slot0.kS, + Gains.Turret.slot0.kV, + Gains.Turret.slot0.kA + ); + if (!hasUsedAbsoluteEncoder) { seedTurret(); hasUsedAbsoluteEncoder = true; diff --git a/src/main/java/com/stuypulse/robot/util/FMSutil.java b/src/main/java/com/stuypulse/robot/util/FMSUtil.java similarity index 100% rename from src/main/java/com/stuypulse/robot/util/FMSutil.java rename to src/main/java/com/stuypulse/robot/util/FMSUtil.java From ea9d7509bdd67d22f847ae321b2e3fd1d679e17f Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Mon, 2 Mar 2026 11:00:26 -0500 Subject: [PATCH 043/150] FEAT: add SwerveDriveAlignTurretToHub --- simgui.json | 12 ++++ .../com/stuypulse/robot/RobotContainer.java | 6 +- .../PoachingAutons/BottomOneCyclePoach.java | 2 +- .../PoachingAutons/BottomTwoCyclePoach.java | 2 +- .../PoachingAutons/TopOneCyclePoach.java | 2 +- .../PoachingAutons/TopTwoCyclePoach.java | 2 +- .../auton/RegularAutons/BottomTwoCycle.java | 2 +- .../auton/RegularAutons/DepotAuton.java | 2 +- .../auton/RegularAutons/TopTwoCycle.java | 2 +- .../swerve/SwerveDriveAlignTurretToHub.java | 68 +++++++++++++++++++ .../{ => climbAlign}/SwerveClimbAlign.java | 2 +- .../{ => climbAlign}/SwerveClimbAlignBot.java | 2 +- .../{ => climbAlign}/SwerveClimbAlignTop.java | 2 +- .../stuypulse/robot/constants/Settings.java | 2 +- 14 files changed, 96 insertions(+), 12 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java rename src/main/java/com/stuypulse/robot/commands/swerve/{ => climbAlign}/SwerveClimbAlign.java (91%) rename src/main/java/com/stuypulse/robot/commands/swerve/{ => climbAlign}/SwerveClimbAlignBot.java (93%) rename src/main/java/com/stuypulse/robot/commands/swerve/{ => climbAlign}/SwerveClimbAlignTop.java (94%) diff --git a/simgui.json b/simgui.json index 3718da7b..1deed979 100644 --- a/simgui.json +++ b/simgui.json @@ -11,6 +11,12 @@ "/FMSInfo": "FMSInfo", "/SmartDashboard/Autonomous": "String Chooser", "/SmartDashboard/Field": "Field2d", + "/SmartDashboard/PathPlanner": "Alerts", + "/SmartDashboard/Robot/Override Down": "Command", + "/SmartDashboard/Robot/Override Stop": "Command", + "/SmartDashboard/Robot/Override Up": "Command", + "/SmartDashboard/Robot/Reset Pivot": "Command", + "/SmartDashboard/Robot/Zero Encoders": "Command", "/SmartDashboard/Visualizers/ClimberHopper": "Mechanism2d", "/SmartDashboard/Visualizers/Turret": "Mechanism2d" }, @@ -64,6 +70,12 @@ "States": { "open": true }, + "Swerve": { + "open": true + }, + "Turret": { + "open": true + }, "open": true } } diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index e96067b7..2ea02f94 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -46,11 +46,12 @@ import com.stuypulse.robot.commands.intake.SeedPivot; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.swerve.SwerveClimbAlign; +import com.stuypulse.robot.commands.swerve.SwerveDriveAlignTurretToHub; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveWheelRadiusCharacterization; import com.stuypulse.robot.commands.swerve.SwerveXMode; +import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.commands.turret.TurretDefaultCommand; import com.stuypulse.robot.commands.turret.TurretAnalog; import com.stuypulse.robot.commands.turret.TurretFerry; @@ -178,6 +179,9 @@ private void configureButtonBindings() { .whileTrue(new TurretShoot()) .onFalse(new TurretIdle()); + driver.getBottomButton() + .whileTrue(new SwerveDriveAlignTurretToHub()); + // driver.getBottomButton() // .onTrue(new TurretAnalog(driver)) // .onFalse(new TurretIdle()); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomOneCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomOneCyclePoach.java index be56a4bd..814d018c 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomOneCyclePoach.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomOneCyclePoach.java @@ -9,7 +9,7 @@ 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; +import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomTwoCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomTwoCyclePoach.java index dfde0b1e..c0171c13 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomTwoCyclePoach.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomTwoCyclePoach.java @@ -9,7 +9,7 @@ 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; +import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopOneCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopOneCyclePoach.java index 4b9ddcf9..37cd0157 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopOneCyclePoach.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopOneCyclePoach.java @@ -9,7 +9,7 @@ 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; +import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopTwoCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopTwoCyclePoach.java index c481447b..b772f6e6 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopTwoCyclePoach.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopTwoCyclePoach.java @@ -9,7 +9,7 @@ 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; +import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/BottomTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/BottomTwoCycle.java index 718845f9..72b9e399 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/BottomTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/BottomTwoCycle.java @@ -9,7 +9,7 @@ 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; +import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/DepotAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/DepotAuton.java index d256c875..a38b448d 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/DepotAuton.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/DepotAuton.java @@ -9,7 +9,7 @@ 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; +import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/TopTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/TopTwoCycle.java index f87b916c..679cd765 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/TopTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/TopTwoCycle.java @@ -9,7 +9,7 @@ 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; +import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java new file mode 100644 index 00000000..198bb5bf --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java @@ -0,0 +1,68 @@ +/************************ 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.commands.swerve; + +import com.stuypulse.stuylib.control.angle.AngleController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.streams.angles.filters.AMotionProfile; + +import java.util.function.Supplier; + +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Gains.Swerve.Alignment; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.subsystems.turret.Turret; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveAlignTurretToHub extends Command { + + private final CommandSwerveDrivetrain swerve; + private final Turret turret; + private final AngleController angleController; + + private Pose2d robot; + + public SwerveDriveAlignTurretToHub() { + swerve = CommandSwerveDrivetrain.getInstance(); + turret = Turret.getInstance(); + + angleController = new AnglePIDController(Alignment.THETA.kP, Alignment.THETA.kI, Alignment.THETA.kD) + .setSetpointFilter(new AMotionProfile(Settings.Swerve.Alignment.Constraints.DEFAULT_MAX_ANGULAR_VELOCITY, Settings.Swerve.Alignment.Constraints.DEFAULT_MAX_ANGULAR_ACCELERATION)); + + addRequirements(swerve); + } + + protected double getAngleError() { + return angleController.getError().getRotation2d().getDegrees(); + } + + @Override + public boolean isFinished() { + return angleController.isDoneDegrees(Settings.Swerve.Alignment.Tolerances.THETA_TOLERANCE_DEG); + } + + @Override + public void execute() { + robot = swerve.getPose(); + + swerve.setControl(swerve.getFieldCentricSwerveRequest() + .withVelocityX(0) + .withVelocityY(0) + .withRotationalRate(angleController.update( + Angle.fromRotation2d(turret.getPointAtTargetAngle(Field.getHubPose())), + Angle.fromRotation2d(robot.getRotation())))); + + SmartDashboard.putNumber("Swerve/Target angle hub deg", turret.getPointAtTargetAngle(Field.getHubPose()).getDegrees()); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveClimbAlign.java b/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlign.java similarity index 91% rename from src/main/java/com/stuypulse/robot/commands/swerve/SwerveClimbAlign.java rename to src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlign.java index 41444f68..2cc837de 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveClimbAlign.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlign.java @@ -3,7 +3,7 @@ /* 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.commands.swerve; +package com.stuypulse.robot.commands.swerve.climbAlign; import com.stuypulse.robot.constants.Field; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveClimbAlignBot.java b/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlignBot.java similarity index 93% rename from src/main/java/com/stuypulse/robot/commands/swerve/SwerveClimbAlignBot.java rename to src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlignBot.java index 835fdd5c..f07cde7f 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveClimbAlignBot.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlignBot.java @@ -3,7 +3,7 @@ /* 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.commands.swerve; +package com.stuypulse.robot.commands.swerve.climbAlign; import com.stuypulse.robot.commands.swerve.pidToPose.SwerveDrivePIDToPose; import com.stuypulse.robot.constants.Field; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveClimbAlignTop.java b/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlignTop.java similarity index 94% rename from src/main/java/com/stuypulse/robot/commands/swerve/SwerveClimbAlignTop.java rename to src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlignTop.java index 3b908c21..7a5e38e0 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveClimbAlignTop.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlignTop.java @@ -3,7 +3,7 @@ /* 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.commands.swerve; +package com.stuypulse.robot.commands.swerve.climbAlign; import com.stuypulse.robot.commands.swerve.pidToPose.SwerveDrivePIDToPose; import com.stuypulse.robot.constants.Field; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index e0b2a0ed..cfcc6dab 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -210,7 +210,7 @@ public interface Alignment { public interface Constraints { public final double DEFAULT_MAX_VELOCITY = 4.93; 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_VELOCITY = Units.degreesToRadians(600.0); public final double DEFAULT_MAX_ANGULAR_ACCELERATION = Units.degreesToRadians(900.0); } From 665365cc074cb8c68907b6a8a110a4c11e66fbd4 Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Mon, 2 Mar 2026 12:33:42 -0500 Subject: [PATCH 044/150] FIX: used wrong target angle before oops --- .../commands/swerve/SwerveDriveAlignTurretToHub.java | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java index 198bb5bf..3bb3b4ff 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java @@ -11,8 +11,6 @@ import com.stuypulse.stuylib.math.Angle; import com.stuypulse.stuylib.streams.angles.filters.AMotionProfile; -import java.util.function.Supplier; - import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Gains.Swerve.Alignment; import com.stuypulse.robot.constants.Settings; @@ -52,6 +50,12 @@ public boolean isFinished() { return angleController.isDoneDegrees(Settings.Swerve.Alignment.Tolerances.THETA_TOLERANCE_DEG); } + public Rotation2d getTargetAngle() { + Translation2d robot = CommandSwerveDrivetrain.getInstance().getPose().getTranslation(); + Translation2d hub = Field.getHubPose().getTranslation(); + return robot.minus(hub).getAngle().plus(Rotation2d.k180deg); + } + @Override public void execute() { robot = swerve.getPose(); @@ -60,7 +64,7 @@ public void execute() { .withVelocityX(0) .withVelocityY(0) .withRotationalRate(angleController.update( - Angle.fromRotation2d(turret.getPointAtTargetAngle(Field.getHubPose())), + Angle.fromRotation2d(getTargetAngle()), Angle.fromRotation2d(robot.getRotation())))); SmartDashboard.putNumber("Swerve/Target angle hub deg", turret.getPointAtTargetAngle(Field.getHubPose()).getDegrees()); From cb3b1d4077e8fc12cab59c0bacb6715795b3f397 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Mon, 2 Mar 2026 13:33:12 -0500 Subject: [PATCH 045/150] feat: shooter limelight configured --- src/main/java/com/stuypulse/robot/constants/Cameras.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index e6532689..8ef90a3a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -20,6 +20,10 @@ public interface Cameras { new Camera("limelight-climber", new Pose3d(-0.233, 0.375959, 0.20368, new Rotation3d(Units.degreesToRadians(180), Units.degreesToRadians(31.08), Units.degreesToRadians(-80))), + RobotContainer.EnabledSubsystems.LIMELIGHT), + new Camera("limelight-shooter", + new Pose3d(Units.inchesToMeters(2.490), Units.inchesToMeters(14.8620), Units.inchesToMeters(5.920157), + new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15.0), Units.degreesToRadians(251.))), RobotContainer.EnabledSubsystems.LIMELIGHT) }; From df10e74e9c7cb3dad9dd48cfa8a41f70bceabce3 Mon Sep 17 00:00:00 2001 From: Lucas Date: Mon, 2 Mar 2026 16:04:03 -0500 Subject: [PATCH 046/150] FIX: turret command logic --- src/main/java/com/stuypulse/robot/RobotContainer.java | 4 ++-- .../robot/commands/turret/TurretDefaultCommand.java | 6 +++--- .../com/stuypulse/robot/commands/turret/TurretSetState.java | 5 +++-- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 2ea02f94..dbc9a220 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -142,6 +142,7 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); // climberHopper.setDefaultCommand(new ClimberHopperDefaultCommand()); + turret.setDefaultCommand(new TurretDefaultCommand()); } /***************/ @@ -176,8 +177,7 @@ private void configureButtonBindings() { .onFalse(new SetIMUMode(0)); driver.getTopButton() - .whileTrue(new TurretShoot()) - .onFalse(new TurretIdle()); + .whileTrue(new TurretShoot()); driver.getBottomButton() .whileTrue(new SwerveDriveAlignTurretToHub()); diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java index 0210a6c8..bc89a640 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java @@ -25,11 +25,11 @@ public void execute() { boolean isInAllianceZone = swerve.getPose().getX() < Field.getHubPose().getX(); - if (isInAllianceZone) { - turret.setState(TurretState.FERRYING); + if (isInAllianceZone) { + turret.setState(TurretState.SHOOTING); } else { - turret.setState(TurretState.SHOOTING); + turret.setState(TurretState.FERRYING); } } diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretSetState.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretSetState.java index 0fc2099d..12197f7c 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretSetState.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretSetState.java @@ -8,9 +8,10 @@ import com.stuypulse.robot.subsystems.turret.Turret; import com.stuypulse.robot.subsystems.turret.Turret.TurretState; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj2.command.InstantCommand; -public class TurretSetState extends InstantCommand { +public class TurretSetState extends Command { private final Turret turret; private final TurretState state; From 96f8cee294908e9630324e47fbb550b91f2b9398 Mon Sep 17 00:00:00 2001 From: Lucas Date: Mon, 2 Mar 2026 16:10:39 -0500 Subject: [PATCH 047/150] FIX: auton package naming conventions --- .../com/stuypulse/robot/RobotContainer.java | 18 +++++++++--------- .../robot/commands/auton/Test/BoxTest.java | 2 +- .../BottomOneCyclePoach.java | 2 +- .../BottomTwoCyclePoach.java | 2 +- .../TopOneCyclePoach.java | 2 +- .../TopTwoCyclePoach.java | 2 +- .../BottomTwoCycle.java | 2 +- .../{RegularAutons => regular}/DepotAuton.java | 2 +- .../{RegularAutons => regular}/EightFuel.java | 2 +- .../TopTwoCycle.java | 2 +- 10 files changed, 18 insertions(+), 18 deletions(-) rename src/main/java/com/stuypulse/robot/commands/auton/{PoachingAutons => poaching}/BottomOneCyclePoach.java (97%) rename src/main/java/com/stuypulse/robot/commands/auton/{PoachingAutons => poaching}/BottomTwoCyclePoach.java (97%) rename src/main/java/com/stuypulse/robot/commands/auton/{PoachingAutons => poaching}/TopOneCyclePoach.java (97%) rename src/main/java/com/stuypulse/robot/commands/auton/{PoachingAutons => poaching}/TopTwoCyclePoach.java (97%) rename src/main/java/com/stuypulse/robot/commands/auton/{RegularAutons => regular}/BottomTwoCycle.java (97%) rename src/main/java/com/stuypulse/robot/commands/auton/{RegularAutons => regular}/DepotAuton.java (97%) rename src/main/java/com/stuypulse/robot/commands/auton/{RegularAutons => regular}/EightFuel.java (93%) rename src/main/java/com/stuypulse/robot/commands/auton/{RegularAutons => regular}/TopTwoCycle.java (98%) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index dbc9a220..d2958c88 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -11,15 +11,15 @@ import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.auton.DoNothingAuton; -import com.stuypulse.robot.commands.auton.PoachingAutons.BottomOneCyclePoach; -import com.stuypulse.robot.commands.auton.PoachingAutons.BottomTwoCyclePoach; -import com.stuypulse.robot.commands.auton.PoachingAutons.TopOneCyclePoach; -import com.stuypulse.robot.commands.auton.PoachingAutons.TopTwoCyclePoach; -import com.stuypulse.robot.commands.auton.RegularAutons.BottomTwoCycle; -import com.stuypulse.robot.commands.auton.RegularAutons.DepotAuton; -import com.stuypulse.robot.commands.auton.RegularAutons.EightFuel; -import com.stuypulse.robot.commands.auton.RegularAutons.TopTwoCycle; -import com.stuypulse.robot.commands.auton.Test.BoxTest; +import com.stuypulse.robot.commands.auton.poaching.BottomOneCyclePoach; +import com.stuypulse.robot.commands.auton.poaching.BottomTwoCyclePoach; +import com.stuypulse.robot.commands.auton.poaching.TopOneCyclePoach; +import com.stuypulse.robot.commands.auton.poaching.TopTwoCyclePoach; +import com.stuypulse.robot.commands.auton.regular.BottomTwoCycle; +import com.stuypulse.robot.commands.auton.regular.DepotAuton; +import com.stuypulse.robot.commands.auton.regular.EightFuel; +import com.stuypulse.robot.commands.auton.regular.TopTwoCycle; +import com.stuypulse.robot.commands.auton.test.BoxTest; import com.stuypulse.robot.commands.climberhopper.ClimberDown; import com.stuypulse.robot.commands.climberhopper.ClimberHopperDefaultCommand; import com.stuypulse.robot.commands.climberhopper.ClimberOverrideDown; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/Test/BoxTest.java b/src/main/java/com/stuypulse/robot/commands/auton/Test/BoxTest.java index 6b5ae850..faac48cf 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/Test/BoxTest.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/Test/BoxTest.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.auton.Test; +package com.stuypulse.robot.commands.auton.test; import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterKB; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomOneCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomOneCyclePoach.java similarity index 97% rename from src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomOneCyclePoach.java rename to src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomOneCyclePoach.java index 814d018c..c55bc2e0 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomOneCyclePoach.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomOneCyclePoach.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.auton.PoachingAutons; +package com.stuypulse.robot.commands.auton.poaching; import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.climberhopper.ClimberDown; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomTwoCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomTwoCyclePoach.java similarity index 97% rename from src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomTwoCyclePoach.java rename to src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomTwoCyclePoach.java index c0171c13..9b38668a 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/BottomTwoCyclePoach.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomTwoCyclePoach.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.auton.PoachingAutons; +package com.stuypulse.robot.commands.auton.poaching; import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.climberhopper.ClimberDown; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopOneCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopOneCyclePoach.java similarity index 97% rename from src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopOneCyclePoach.java rename to src/main/java/com/stuypulse/robot/commands/auton/poaching/TopOneCyclePoach.java index 37cd0157..2454b2e8 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopOneCyclePoach.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopOneCyclePoach.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.auton.PoachingAutons; +package com.stuypulse.robot.commands.auton.poaching; import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.climberhopper.ClimberDown; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopTwoCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopTwoCyclePoach.java similarity index 97% rename from src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopTwoCyclePoach.java rename to src/main/java/com/stuypulse/robot/commands/auton/poaching/TopTwoCyclePoach.java index b772f6e6..6d028a2f 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/PoachingAutons/TopTwoCyclePoach.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopTwoCyclePoach.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.auton.PoachingAutons; +package com.stuypulse.robot.commands.auton.poaching; import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.climberhopper.ClimberDown; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/BottomTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/BottomTwoCycle.java similarity index 97% rename from src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/BottomTwoCycle.java rename to src/main/java/com/stuypulse/robot/commands/auton/regular/BottomTwoCycle.java index 72b9e399..124205a0 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/BottomTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/BottomTwoCycle.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.auton.RegularAutons; +package com.stuypulse.robot.commands.auton.regular; import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.climberhopper.ClimberDown; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/DepotAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java similarity index 97% rename from src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/DepotAuton.java rename to src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java index a38b448d..f776459a 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/DepotAuton.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.auton.RegularAutons; +package com.stuypulse.robot.commands.auton.regular; import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.climberhopper.ClimberDown; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/EightFuel.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java similarity index 93% rename from src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/EightFuel.java rename to src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java index b31a67da..f9cca46d 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/EightFuel.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.auton.RegularAutons; +package com.stuypulse.robot.commands.auton.regular; import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.handoff.HandoffRun; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/TopTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/TopTwoCycle.java similarity index 98% rename from src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/TopTwoCycle.java rename to src/main/java/com/stuypulse/robot/commands/auton/regular/TopTwoCycle.java index 679cd765..118ba420 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/RegularAutons/TopTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/TopTwoCycle.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.auton.RegularAutons; +package com.stuypulse.robot.commands.auton.regular; import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.climberhopper.ClimberDown; From d1b2680a4a9a4d6cdeec3fb0288836fa65737c79 Mon Sep 17 00:00:00 2001 From: Lucas O <150559208+lucas01b0x@users.noreply.github.com> Date: Mon, 2 Mar 2026 16:14:29 -0500 Subject: [PATCH 048/150] FIX: rename auton test directory to package name --- .../stuypulse/robot/commands/auton/{Test => test}/BoxTest.java | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/main/java/com/stuypulse/robot/commands/auton/{Test => test}/BoxTest.java (100%) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/Test/BoxTest.java b/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java similarity index 100% rename from src/main/java/com/stuypulse/robot/commands/auton/Test/BoxTest.java rename to src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java From edfb0de566cd57e59af319aefec36636d736011b Mon Sep 17 00:00:00 2001 From: Ryan Bergman Date: Mon, 2 Mar 2026 16:31:43 -0500 Subject: [PATCH 049/150] feat: hood analog, hood zero, hood seed, intake stalling, some other stalling, some other stuff. Zeroing and seeding not tested, double check. Look over intake stalling and the debug enum constant, double check that is what we want. --- simgui-ds.json | 8 +-- simgui.json | 13 ++--- .../com/stuypulse/robot/RobotContainer.java | 56 ++++++------------- .../commands/hoodedshooter/HoodAnalog.java | 29 ++++++++++ .../commands/hoodedshooter/ZeroHood.java | 21 +++++++ .../swerve/SwerveDriveAlignTurretToHub.java | 11 ++-- .../stuypulse/robot/constants/Settings.java | 3 + .../subsystems/hoodedshooter/hood/Hood.java | 20 +++++++ .../hoodedshooter/hood/HoodImpl.java | 29 ++++++++++ .../robot/subsystems/intake/Intake.java | 4 +- .../robot/subsystems/intake/IntakeImpl.java | 46 ++++++++++----- 11 files changed, 167 insertions(+), 73 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodAnalog.java create mode 100644 src/main/java/com/stuypulse/robot/commands/hoodedshooter/ZeroHood.java diff --git a/simgui-ds.json b/simgui-ds.json index 00784de4..c4b7efd3 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,9 +1,4 @@ { - "System Joysticks": { - "window": { - "enabled": false - } - }, "keyboardJoysticks": [ { "axisConfig": [ @@ -96,7 +91,8 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0" + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ] } diff --git a/simgui.json b/simgui.json index 1deed979..1b956d58 100644 --- a/simgui.json +++ b/simgui.json @@ -17,6 +17,8 @@ "/SmartDashboard/Robot/Override Up": "Command", "/SmartDashboard/Robot/Reset Pivot": "Command", "/SmartDashboard/Robot/Zero Encoders": "Command", + "/SmartDashboard/Robot/Zero Hood Encoder": "Command", + "/SmartDashboard/Robot/Zero Turret Encoders": "Command", "/SmartDashboard/Visualizers/ClimberHopper": "Mechanism2d", "/SmartDashboard/Visualizers/Turret": "Mechanism2d" }, @@ -37,11 +39,6 @@ "visible": true } }, - "/SmartDashboard/Visualizers/ClimberHopper": { - "window": { - "visible": true - } - }, "/SmartDashboard/Visualizers/Turret": { "window": { "visible": true @@ -58,13 +55,13 @@ }, "transitory": { "SmartDashboard": { - "ClimberHopper": { - "open": true - }, "Enabled Subsystems": { "open": true }, "HoodedShooter": { + "Hood": { + "open": true + }, "open": true }, "States": { diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index d2958c88..b28583cf 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -5,11 +5,6 @@ /***************************************************************/ package com.stuypulse.robot; -import com.stuypulse.stuylib.input.Gamepad; -import com.stuypulse.stuylib.input.gamepads.AutoGamepad; -import com.stuypulse.stuylib.network.SmartBoolean; - -import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.auton.poaching.BottomOneCyclePoach; import com.stuypulse.robot.commands.auton.poaching.BottomTwoCyclePoach; @@ -20,53 +15,24 @@ import com.stuypulse.robot.commands.auton.regular.EightFuel; import com.stuypulse.robot.commands.auton.regular.TopTwoCycle; import com.stuypulse.robot.commands.auton.test.BoxTest; -import com.stuypulse.robot.commands.climberhopper.ClimberDown; -import com.stuypulse.robot.commands.climberhopper.ClimberHopperDefaultCommand; import com.stuypulse.robot.commands.climberhopper.ClimberOverrideDown; import com.stuypulse.robot.commands.climberhopper.ClimberOverrideStop; import com.stuypulse.robot.commands.climberhopper.ClimberOverrideUp; -import com.stuypulse.robot.commands.climberhopper.ClimberUp; -import com.stuypulse.robot.commands.climberhopper.HopperDown; -import com.stuypulse.robot.commands.handoff.HandoffReverse; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterFerry; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterLeftCorner; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterReverse; -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.IntakeAnalog; -import com.stuypulse.robot.commands.intake.IntakeBangBang; +import com.stuypulse.robot.commands.hoodedshooter.HoodAnalog; +import com.stuypulse.robot.commands.hoodedshooter.ZeroHood; import com.stuypulse.robot.commands.intake.IntakeDeploy; -import com.stuypulse.robot.commands.intake.IntakeRunRollers; -import com.stuypulse.robot.commands.intake.IntakeStopRollers; import com.stuypulse.robot.commands.intake.IntakeStow; import com.stuypulse.robot.commands.intake.SeedPivot; -import com.stuypulse.robot.commands.spindexer.SpindexerRun; -import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.swerve.SwerveDriveAlignTurretToHub; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveWheelRadiusCharacterization; -import com.stuypulse.robot.commands.swerve.SwerveXMode; -import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.commands.turret.TurretDefaultCommand; -import com.stuypulse.robot.commands.turret.TurretAnalog; -import com.stuypulse.robot.commands.turret.TurretFerry; -import com.stuypulse.robot.commands.turret.TurretHub; -import com.stuypulse.robot.commands.turret.TurretIdle; -import com.stuypulse.robot.commands.turret.TurretLeftCorner; -import com.stuypulse.robot.commands.turret.TurretRightCorner; -import com.stuypulse.robot.commands.turret.TurretSeed; import com.stuypulse.robot.commands.turret.TurretShoot; import com.stuypulse.robot.commands.turret.TurretZero; import com.stuypulse.robot.commands.vision.ResetLimelightIMU; import com.stuypulse.robot.commands.vision.SetIMUMode; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Ports; -import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; import com.stuypulse.robot.subsystems.hoodedshooter.hood.Hood; @@ -77,6 +43,9 @@ import com.stuypulse.robot.subsystems.turret.Turret; import com.stuypulse.robot.subsystems.vision.LimelightVision; import com.stuypulse.robot.util.PathUtil.AutonConfig; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.input.gamepads.AutoGamepad; +import com.stuypulse.stuylib.network.SmartBoolean; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -128,11 +97,15 @@ public RobotContainer() { SmartDashboard.putData("Field", Field.FIELD2D); SmartDashboard.putData("Robot/Reset Pivot", new SeedPivot()); - SmartDashboard.putData("Robot/Zero Encoders", new TurretZero()); + SmartDashboard.putData("Robot/Zero Turret Encoders", new TurretZero()); SmartDashboard.putData("Robot/Override Up", new ClimberOverrideUp()); SmartDashboard.putData("Robot/Override Down", new ClimberOverrideDown()); SmartDashboard.putData("Robot/Override Stop", new ClimberOverrideStop()); + + + SmartDashboard.putData("Robot/Zero Hood Encoder", new ZeroHood()); + } /****************/ @@ -150,6 +123,11 @@ private void configureDefaultCommands() { /***************/ private void configureButtonBindings() { + //hood analog + driver.getBottomButton() + .whileTrue(new HoodAnalog(driver)); + + // Intake Run Rollers // driver.getLeftTriggerButton() // .onTrue(new IntakeRunRollers()) @@ -179,8 +157,8 @@ private void configureButtonBindings() { driver.getTopButton() .whileTrue(new TurretShoot()); - driver.getBottomButton() - .whileTrue(new SwerveDriveAlignTurretToHub()); + // driver.getBottomButton() + // .whileTrue(new SwerveDriveAlignTurretToHub()); // driver.getBottomButton() // .onTrue(new TurretAnalog(driver)) diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodAnalog.java b/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodAnalog.java new file mode 100644 index 00000000..e536c2db --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodAnalog.java @@ -0,0 +1,29 @@ +package com.stuypulse.robot.commands.hoodedshooter; + +import com.stuypulse.robot.subsystems.hoodedshooter.hood.Hood; +import com.stuypulse.stuylib.input.Gamepad; + +import edu.wpi.first.wpilibj2.command.Command; + +public class HoodAnalog extends Command{ + private final Hood hood; + private final Gamepad driver; + + public HoodAnalog(Gamepad driver) { + this.driver = driver; + hood = Hood.getInstance(); + + addRequirements(hood); + } + + @Override + public void initialize() { + hood.setState(Hood.HoodState.ANALOG); + } + + @Override + public void execute() { + hood.hoodAnalogToInput(driver); + //SmartDashboard.putNumber("HoodedShooter/Hood/Analog", hood.hoodAnalogToOutput().getDegrees()); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/ZeroHood.java b/src/main/java/com/stuypulse/robot/commands/hoodedshooter/ZeroHood.java new file mode 100644 index 00000000..5ef71949 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/hoodedshooter/ZeroHood.java @@ -0,0 +1,21 @@ +package com.stuypulse.robot.commands.hoodedshooter; + +import com.stuypulse.robot.subsystems.hoodedshooter.hood.Hood; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ZeroHood extends InstantCommand{ + private final Hood hood; + + public ZeroHood() { + hood = Hood.getInstance(); + + addRequirements(hood); + } + + @Override + public void initialize() { + hood.zeroHoodEncoder(); + hood.seedHood(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java index 3bb3b4ff..ccf32039 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java @@ -6,16 +6,15 @@ package com.stuypulse.robot.commands.swerve; -import com.stuypulse.stuylib.control.angle.AngleController; -import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; -import com.stuypulse.stuylib.math.Angle; -import com.stuypulse.stuylib.streams.angles.filters.AMotionProfile; - import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Gains.Swerve.Alignment; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.subsystems.turret.Turret; +import com.stuypulse.stuylib.control.angle.AngleController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.streams.angles.filters.AMotionProfile; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -51,7 +50,7 @@ public boolean isFinished() { } public Rotation2d getTargetAngle() { - Translation2d robot = CommandSwerveDrivetrain.getInstance().getPose().getTranslation(); + Translation2d robot = swerve.getPose().getTranslation(); Translation2d hub = Field.getHubPose().getTranslation(); return robot.minus(hub).getAngle().plus(Rotation2d.k180deg); } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index cfcc6dab..2f9329d6 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -104,6 +104,9 @@ public interface Intake { double PUSHDOWN_VOLTAGE = 3.0; double GEAR_RATIO = 37.93; + + double debugVoltage = 0; //TODO: set value + double STALL_CURRENT_LIMIT = 0; //TODO: set value } public interface Spindexer { 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 4185a6a9..6d162bf2 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 @@ -7,6 +7,7 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.hoodedshooter.HoodAngleCalculator; +import com.stuypulse.stuylib.input.Gamepad; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -18,6 +19,8 @@ public abstract class Hood extends SubsystemBase{ private HoodState state; + private Rotation2d driverInput; + static { instance = new HoodImpl(); } @@ -34,6 +37,7 @@ public enum HoodState { LEFT_CORNER, RIGHT_CORNER, INTERPOLATION, + ANALOG, IDLE; } @@ -58,6 +62,7 @@ public Rotation2d getTargetAngle() { case LEFT_CORNER -> Settings.HoodedShooter.Angles.LEFT_CORNER_ANGLE; case RIGHT_CORNER -> Settings.HoodedShooter.Angles.RIGHT_CORNER_ANGLE; case INTERPOLATION -> HoodAngleCalculator.interpolateHoodAngle().get(); + case ANALOG -> hoodAnalogToOutput(); case IDLE -> getHoodAngle(); }; } @@ -68,7 +73,20 @@ public boolean atTolerance() { public abstract Rotation2d getHoodAngle(); + public void hoodAnalogToInput(Gamepad gamepad) { + double hoodMin = Settings.HoodedShooter.Angles.MIN_ANGLE.getDegrees(); + double hoodMax = Settings.HoodedShooter.Angles.MAX_ANGLE.getDegrees(); + + this.driverInput = Rotation2d.fromDegrees(hoodMin + (gamepad.getLeftX() + 1.0) * ((hoodMax - hoodMin) / 2)); + } + + public Rotation2d hoodAnalogToOutput() { + return this.driverInput; + } + public abstract SysIdRoutine getHoodSysIdRoutine(); + public abstract void zeroHoodEncoder(); + public abstract void seedHood(); @Override public void periodic() { @@ -77,5 +95,7 @@ public void periodic() { SmartDashboard.putNumber("HoodedShooter/Hood/Target Angle", getTargetAngle().getDegrees()); SmartDashboard.putNumber("HoodedShooter/Hood/Current Angle", getHoodAngle().getDegrees()); + + //SmartDashboard.putNumber("HoodedShooter/Hood/Analog Target Angle", hoodAnalogToOutput().getDegrees()); } } \ No newline at end of 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 defc2773..731fe3c6 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 @@ -24,6 +24,16 @@ import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import java.util.Optional; +import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.constants.Gains; +import com.stuypulse.robot.constants.Motors; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.SysId; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class HoodImpl extends Hood { private final Motors.TalonFXConfig hoodConfig; @@ -75,6 +85,25 @@ public Rotation2d getHoodAngle() { return Rotation2d.fromRotations(hoodMotor.getPosition().getValueAsDouble()); } + @Override + public void zeroHoodEncoder() { + double currentPos = hoodEncoder.getAbsolutePosition().getValueAsDouble(); + + hoodEncoder.getConfigurator().refresh(hoodEncoderConfig.getConfiguration().MagnetSensor); + + double currentOffset = hoodEncoderConfig.getConfiguration().MagnetSensor.MagnetOffset; + double newOffset = currentOffset - currentPos; + + hoodEncoderConfig.withMagnetOffset(newOffset); + + hoodEncoderConfig.configure(hoodEncoder); + } + + @Override + public void seedHood() { + hoodMotor.setPosition(hoodEncoder.getAbsolutePosition().getValueAsDouble()); + } + @Override public void periodic() { super.periodic(); 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 707e7a36..b04b5920 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -43,7 +43,8 @@ public enum PivotState { DEPLOY(Settings.Intake.PIVOT_DEPLOY_ANGLE), STOW(Settings.Intake.PIVOT_STOW_ANGLE), BANGBANG(Settings.Intake.PIVOT_DEPLOY_ANGLE), - ANALOG(Settings.Intake.PIVOT_STOW_ANGLE); + ANALOG(Settings.Intake.PIVOT_STOW_ANGLE), + DEBUG(Settings.Intake.PIVOT_STOW_ANGLE); //filler argument private final Rotation2d targetAngle; @@ -110,6 +111,7 @@ public Rotation2d driverInputToAngle() { public abstract Rotation2d getPivotAngle(); public abstract void setPivotVoltageOverride(Optional voltage); public abstract SysIdRoutine getPivotSysIdRoutine(); + public abstract boolean pivotStalling(); public abstract void seedPivot(); 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 ae382f71..4b976690 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -5,18 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.intake; -import com.stuypulse.robot.RobotContainer.EnabledSubsystems; -import com.stuypulse.robot.constants.Gains; -import com.stuypulse.robot.constants.Motors; -import com.stuypulse.robot.constants.Ports; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.util.SettableNumber; -import com.stuypulse.robot.util.SysId; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import java.util.Optional; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; @@ -29,7 +18,19 @@ import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; -import java.util.Optional; +import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.constants.Gains; +import com.stuypulse.robot.constants.Motors; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.SettableNumber; +import com.stuypulse.robot.util.SysId; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class IntakeImpl extends Intake { private final Motors.TalonFXConfig pivotConfig; @@ -49,6 +50,8 @@ public class IntakeImpl extends Intake { private Optional pivotVoltageOverride; + private BStream pivotStalling; + public IntakeImpl() { //TODO: refactor pivotConfig = new Motors.TalonFXConfig() @@ -91,6 +94,17 @@ public IntakeImpl() { pivotVoltageOverride = Optional.empty(); pivot.setPosition(Settings.Intake.PIVOT_MAX_ANGLE.getRotations()); + + pivotStalling = BStream.create( + () -> Math.abs(pivot.getSupplyCurrent().getValueAsDouble()) > Settings.Intake.STALL_CURRENT_LIMIT) + .filtered(new BDebounce.Both(1.0)); + + } + + @Override + public boolean pivotStalling() { + // return Math.abs(pivot.getSupplyCurrent().getValueAsDouble()) > Settings.Intake.debugCurrentLimit; + return pivotStalling.get(); } @Override @@ -149,6 +163,12 @@ public void periodic() { if (getPivotState() == PivotState.ANALOG) { // comment out the setControl line if it breaks pivot.setControl(pivotController.withPosition(driverInputToAngle().getRotations())); } + else if (getPivotState() == PivotState.DEBUG) { + pivot.setControl(new VoltageOut(Settings.Intake.debugVoltage)); //TODO: check if we want DEBUG enum/state + if (pivotStalling.get()) { //TODO: update what we want the intake to do if + pivot.setPosition(0); + } + } else if (pivotVoltageOverride.isPresent()) { pivot.setVoltage(pivotVoltageOverride.get()); From e6bcd0cf1b1f1d1d8fad3fe55d2b76e3212fe769 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Mon, 2 Mar 2026 19:25:18 -0500 Subject: [PATCH 050/150] FIX: hood zeroing, testing scoring routine --- src/main/java/com/stuypulse/robot/Robot.java | 18 ++--- .../com/stuypulse/robot/RobotContainer.java | 65 +++++++++++-------- .../stuypulse/robot/constants/Cameras.java | 5 ++ .../com/stuypulse/robot/constants/Gains.java | 20 +++--- .../stuypulse/robot/constants/Settings.java | 8 +-- .../robot/subsystems/handoff/HandoffImpl.java | 2 +- .../hoodedshooter/hood/HoodImpl.java | 14 ++-- .../hoodedshooter/shooter/ShooterImpl.java | 4 ++ .../robot/subsystems/intake/IntakeImpl.java | 4 ++ .../subsystems/spindexer/SpindexerImpl.java | 10 +-- .../robot/subsystems/turret/TurretImpl.java | 2 +- 11 files changed, 90 insertions(+), 62 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 056ef9b2..dc9f28aa 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -27,7 +27,7 @@ public class Robot extends TimedRobot { private static Alliance alliance; private PowerDistribution powerDistribution; - private FMSUtil fmsUtil; + // private FMSUtil fmsUtil; public static boolean isBlue() { return alliance == Alliance.Blue; @@ -41,7 +41,7 @@ public static boolean isBlue() { public void robotInit() { robot = new RobotContainer(); powerDistribution = new PowerDistribution(); - fmsUtil = new FMSUtil(false); + // fmsUtil = new FMSUtil(false); DataLogManager.start(); SignalLogger.start(); @@ -51,7 +51,7 @@ public void robotInit() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); - SmartDashboard.putNumber("Robot/Voltage of Robot", powerDistribution.getVoltage()); + // SmartDashboard.putNumber("Robot/Voltage of Robot", powerDistribution.getVoltage()); SmartDashboard.putNumber("Robot/Match Time", DriverStation.getMatchTime()); @@ -59,9 +59,9 @@ public void robotPeriodic() { alliance = DriverStation.getAlliance().get(); } - SmartDashboard.putNumber("FMSUtil/Time Left In Shift", fmsUtil.getTimeLeftInShift()); - SmartDashboard.putBoolean("FMSUtil/Is Active Shift?", fmsUtil.isActiveShift()); - SmartDashboard.putString("FMSUtil/Field State", fmsUtil.getCurrentFieldState().toString()); + // SmartDashboard.putNumber("FMSUtil/Time Left In Shift", fmsUtil.getTimeLeftInShift()); + // SmartDashboard.putBoolean("FMSUtil/Is Active Shift?", fmsUtil.isActiveShift()); + // SmartDashboard.putString("FMSUtil/Field State", fmsUtil.getCurrentFieldState().toString()); } /*********************/ @@ -82,7 +82,7 @@ public void disabledPeriodic() {} @Override public void autonomousInit() { - fmsUtil.restartTimer(true); + // fmsUtil.restartTimer(true); CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); @@ -105,7 +105,7 @@ public void autonomousExit() {} @Override public void teleopInit() { - fmsUtil.restartTimer(false); + // fmsUtil.restartTimer(false); CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); @@ -113,7 +113,7 @@ public void teleopInit() { auto.cancel(); } - SmartDashboard.putBoolean("FMSUtil/Won Auto?", fmsUtil.didWinAuto()); + // SmartDashboard.putBoolean("FMSUtil/Won Auto?", fmsUtil.didWinAuto()); } @Override diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index b28583cf..5489a062 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -14,15 +14,24 @@ import com.stuypulse.robot.commands.auton.regular.DepotAuton; import com.stuypulse.robot.commands.auton.regular.EightFuel; import com.stuypulse.robot.commands.auton.regular.TopTwoCycle; -import com.stuypulse.robot.commands.auton.test.BoxTest; +// import com.stuypulse.robot.commands.auton.test.BoxTest; import com.stuypulse.robot.commands.climberhopper.ClimberOverrideDown; import com.stuypulse.robot.commands.climberhopper.ClimberOverrideStop; import com.stuypulse.robot.commands.climberhopper.ClimberOverrideUp; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.hoodedshooter.HoodAnalog; +import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; +import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterShoot; +import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterStow; import com.stuypulse.robot.commands.hoodedshooter.ZeroHood; import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.intake.IntakeRunRollers; +import com.stuypulse.robot.commands.intake.IntakeStopRollers; import com.stuypulse.robot.commands.intake.IntakeStow; import com.stuypulse.robot.commands.intake.SeedPivot; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveWheelRadiusCharacterization; @@ -50,6 +59,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; @@ -57,13 +67,13 @@ public class RobotContainer { public interface EnabledSubsystems { SmartBoolean SWERVE = new SmartBoolean("Enabled Subsystems/Swerve Is Enabled", true); - SmartBoolean TURRET = new SmartBoolean("Enabled Subsystems/Turret Is Enabled", true); - SmartBoolean HANDOFF = new SmartBoolean("Enabled Subsystems/Handoff Is Enabled", false); + SmartBoolean TURRET = new SmartBoolean("Enabled Subsystems/Turret Is Enabled", false); + SmartBoolean HANDOFF = new SmartBoolean("Enabled Subsystems/Handoff Is Enabled", true); SmartBoolean INTAKE = new SmartBoolean("Enabled Subsystems/Intake Is Enabled", false); - SmartBoolean SPINDEXER = new SmartBoolean("Enabled Subsystems/Spindexer Is Enabled", false); + SmartBoolean SPINDEXER = new SmartBoolean("Enabled Subsystems/Spindexer Is Enabled", true); SmartBoolean CLIMBER_HOPPER = new SmartBoolean("Enabled Subsystems/Climber-Hopper Is Enabled", false); SmartBoolean HOOD = new SmartBoolean("Enabled Subsystems/Hood Is Enabled", false); - SmartBoolean SHOOTER = new SmartBoolean("Enabled Subsystems/Shooter Is Enabled", false); + SmartBoolean SHOOTER = new SmartBoolean("Enabled Subsystems/Shooter Is Enabled", true); SmartBoolean LIMELIGHT = new SmartBoolean("Enabled Subsystems/Limelight Is Enabled", true); } @@ -92,7 +102,7 @@ public RobotContainer() { swerve.configureAutoBuilder(); configureDefaultCommands(); configureButtonBindings(); - configureAutons(); + //configureAutons(); configureSysids(); SmartDashboard.putData("Field", Field.FIELD2D); @@ -102,7 +112,6 @@ public RobotContainer() { SmartDashboard.putData("Robot/Override Up", new ClimberOverrideUp()); SmartDashboard.putData("Robot/Override Down", new ClimberOverrideDown()); SmartDashboard.putData("Robot/Override Stop", new ClimberOverrideStop()); - SmartDashboard.putData("Robot/Zero Hood Encoder", new ZeroHood()); @@ -115,7 +124,7 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); // climberHopper.setDefaultCommand(new ClimberHopperDefaultCommand()); - turret.setDefaultCommand(new TurretDefaultCommand()); + // turret.setDefaultCommand(new TurretDefaultCommand()); } /***************/ @@ -124,11 +133,11 @@ private void configureDefaultCommands() { private void configureButtonBindings() { //hood analog - driver.getBottomButton() - .whileTrue(new HoodAnalog(driver)); + // driver.getBottomButton() + // .whileTrue(new HoodAnalog(driver)); - // Intake Run Rollers + // // Intake Run Rollers // driver.getLeftTriggerButton() // .onTrue(new IntakeRunRollers()) // .onFalse(new IntakeStopRollers()); @@ -142,6 +151,10 @@ private void configureButtonBindings() { // driver.getBottomButton() // .onTrue(new IntakeBangBang()); + // driver.getTopButton() + // .whileTrue(new HoodedShooterInterpolation()) + // .onFalse(new HoodedShooterStow()); + driver.getRightTriggerButton() .onTrue(new IntakeDeploy()); @@ -154,8 +167,8 @@ private void configureButtonBindings() { .onTrue(new ResetLimelightIMU()) .onFalse(new SetIMUMode(0)); - driver.getTopButton() - .whileTrue(new TurretShoot()); + // driver.getTopButton() + // .whileTrue(new TurretShoot()); // driver.getBottomButton() // .whileTrue(new SwerveDriveAlignTurretToHub()); @@ -165,16 +178,16 @@ private void configureButtonBindings() { // .onFalse(new TurretIdle()); // Scoring Routine using Interpolation Settings - // driver.getTopButton() - // .whileTrue(new HoodedShooterInterpolation() - // .alongWith(new TurretShoot()) - // .alongWith(new WaitUntilCommand(() -> hoodedShooter.bothAtTolerance())) - // .andThen(new HandoffRun().onlyIf(() -> hoodedShooter.bothAtTolerance()) - // .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) - // .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && hoodedShooter.bothAtTolerance())))) - // .onFalse(new SpindexerStop() - // .alongWith(new HoodedShooterStow()) - // .alongWith(new HandoffStop())); + driver.getTopButton() + .whileTrue(new HoodedShooterShoot() + // .alongWith(new TurretShoot()) + .alongWith(new WaitUntilCommand(() -> hoodedShooter.isShooterAtTolerance())) + .andThen(new HandoffRun().onlyIf(() -> hoodedShooter.isShooterAtTolerance()) + .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) + .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && hoodedShooter.isShooterAtTolerance())))) + .onFalse(new SpindexerStop() + .alongWith(new HoodedShooterStow()) + .alongWith(new HandoffStop())); // // Ferry Routine using Interpolation Settings // driver.getBottomButton() @@ -313,9 +326,9 @@ public void configureAutons() { autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); // TESTS - AutonConfig BOX_TEST = new AutonConfig("Box Test", BoxTest::new, - "Box 1", "Box 2", "Box 3", "Box 4"); - BOX_TEST.register(autonChooser); + // AutonConfig BOX_TEST = new AutonConfig("Box Test", BoxTest::new, + // "Box 1", "Box 2", "Box 3", "Box 4"); + // BOX_TEST.register(autonChooser); // BASE AutonConfig EIGHT_FUEL = new AutonConfig("Eight Fuel", EightFuel::new, diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 8ef90a3a..f51ed9a6 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -22,9 +22,14 @@ public interface Cameras { new Rotation3d(Units.degreesToRadians(180), Units.degreesToRadians(31.08), Units.degreesToRadians(-80))), RobotContainer.EnabledSubsystems.LIMELIGHT), new Camera("limelight-shooter", + new Pose3d(Units.inchesToMeters(2.490), Units.inchesToMeters(14.8620), Units.inchesToMeters(5.920157), + new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15.0), Units.degreesToRadians(251.))), + RobotContainer.EnabledSubsystems.LIMELIGHT), + new Camera("limelight-module", new Pose3d(Units.inchesToMeters(2.490), Units.inchesToMeters(14.8620), Units.inchesToMeters(5.920157), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15.0), Units.degreesToRadians(251.))), RobotContainer.EnabledSubsystems.LIMELIGHT) + }; public static class Camera { diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 73232aa9..93d36472 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -36,8 +36,8 @@ public interface Shooter { SmartNumber kD = new SmartNumber("HoodedShooter/Shooter/Gains/kD", 0.0); SmartNumber kS = new SmartNumber("HoodedShooter/Shooter/Gains/kS", 0.0); - SmartNumber kA = new SmartNumber("HoodedShooter/Shooter/Gains/kA", 0.123); - SmartNumber kV = new SmartNumber("HoodedShooter/Shooter/Gains/kV", 0.0); + SmartNumber kA = new SmartNumber("HoodedShooter/Shooter/Gains/kA", 0.0); + SmartNumber kV = new SmartNumber("HoodedShooter/Shooter/Gains/kV", 0.123); // double kP = 0.45; // double kI = 0.0; @@ -49,11 +49,11 @@ public interface Shooter { } public interface Hood { - SmartNumber kP = new SmartNumber("HoodedShooter/Hood/Gains/kP", 300.0); + SmartNumber kP = new SmartNumber("HoodedShooter/Hood/Gains/kP", 400.0); SmartNumber kI = new SmartNumber("HoodedShooter/Hood/Gains/kI", 0.0); SmartNumber kD = new SmartNumber("HoodedShooter/Hood/Gains/kD", 0.0); - SmartNumber kS = new SmartNumber("HoodedShooter/Hood/Gains/kS", 0.0); + SmartNumber kS = new SmartNumber("HoodedShooter/Hood/Gains/kS", 0.25); SmartNumber kA = new SmartNumber("HoodedShooter/Hood/Gains/kA", 0.0); SmartNumber kV = new SmartNumber("HoodedShooter/Hood/Gains/kV", 0.0); @@ -75,7 +75,7 @@ public interface Spindexer { SmartNumber kS = new SmartNumber("Spindexer/Gains/kS", 0.25); SmartNumber kA = new SmartNumber("Spindexer/Gains/kA", 0.010876); - SmartNumber kV = new SmartNumber("Spindexer/Gains/kV", 0.9413); + SmartNumber kV = new SmartNumber("Spindexer/Gains/kV", 1.20); //0.9413); } public interface Intake { @@ -101,13 +101,13 @@ public interface Pivot { } public interface Handoff { - SmartNumber kP = new SmartNumber("Handoff/Gains/kP", 0.00015508); + SmartNumber kP = new SmartNumber("Handoff/Gains/kP", 0.000155808); SmartNumber kI = new SmartNumber("Handoff/Gains/kI", 0.0); - SmartNumber kD = new SmartNumber("Handoff/Gains/kD", 140.0); + SmartNumber kD = new SmartNumber("Handoff/Gains/kD", 0.0); - SmartNumber kS = new SmartNumber("Handoff/Gains/kS", 0.21149); - SmartNumber kV = new SmartNumber("Handoff/Gains/kV", 0.016329); - SmartNumber kA = new SmartNumber("Handoff/Gains/kA", 0.3652); + SmartNumber kS = new SmartNumber("Handoff/Gains/kS", 0.1728); + SmartNumber kV = new SmartNumber("Handoff/Gains/kV", 0.12); + SmartNumber kA = new SmartNumber("Handoff/Gains/kA", 0.00284); // double kP = 0.00015508; // 0.016973 from sysid // double kI = 0.0; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 2f9329d6..21b3c160 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -126,7 +126,7 @@ public interface HoodedShooter { 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 SHOOT_RPM = new SmartNumber("HoodedShooter/Shoot State Target RPM", 3500.0); public final SmartNumber FERRY_RPM = new SmartNumber("HoodedShooter/Ferry State Target RPM", 2000.0); public final double REVERSE = 0.0; public final double KB_RPM = 0.0; @@ -193,11 +193,11 @@ public interface Hood { * 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 GEAR_RATIO = 1064.0 / 9.0; public final double ENCODER_TO_MECH = 32.0 / 3.0; - public final Rotation2d ENCODER_OFFSET = Rotation2d.fromDegrees(0.0); - } + public final Rotation2d ENCODER_OFFSET = Rotation2d.fromRotations(0.01344); + } } public interface ShootOnTheFly { public final int MAX_ITERATIONS = 5; 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 c0447923..7e99a4ce 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -48,7 +48,7 @@ public HandoffImpl() { } public double getCurrentRPM() { - return motor.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE; + return motor.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE * Settings.Handoff.GEAR_RATIO; } @Override 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 731fe3c6..4dbfd4c2 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 @@ -53,18 +53,18 @@ public HoodImpl() { .withCurrentLimitAmps(80.0) .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.Clockwise_Positive) + .withInvertedValue(InvertedValue.CounterClockwise_Positive) .withPIDConstants(Gains.HoodedShooter.Hood.kP.get(), Gains.HoodedShooter.Hood.kI.get(), Gains.HoodedShooter.Hood.kD.get(), 0) .withFFConstants(Gains.HoodedShooter.Hood.kS.get(), Gains.HoodedShooter.Hood.kV.get(), Gains.HoodedShooter.Hood.kA.get(), 0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 0) .withSensorToMechanismRatio(Settings.HoodedShooter.Hood.GEAR_RATIO) .withSoftLimits( - true, true, + false, false, // TODO: make true Settings.HoodedShooter.Angles.MAX_ANGLE.getRotations(), Settings.HoodedShooter.Angles.MIN_ANGLE.getRotations()); hoodEncoderConfig = new Motors.CANCoderConfig() - .withSensorDirection(SensorDirectionValue.Clockwise_Positive) + .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) .withAbsoluteSensorDiscontinuityPoint(1.0) .withMagnetOffset(Settings.HoodedShooter.Hood.ENCODER_OFFSET.getRotations()); @@ -85,6 +85,7 @@ public Rotation2d getHoodAngle() { return Rotation2d.fromRotations(hoodMotor.getPosition().getValueAsDouble()); } + // TODO: INCORRECT RIGHT NOW @Override public void zeroHoodEncoder() { double currentPos = hoodEncoder.getAbsolutePosition().getValueAsDouble(); @@ -101,7 +102,7 @@ public void zeroHoodEncoder() { @Override public void seedHood() { - hoodMotor.setPosition(hoodEncoder.getAbsolutePosition().getValueAsDouble()); + hoodMotor.setPosition(Settings.HoodedShooter.Angles.MIN_ANGLE.getRotations() + hoodEncoder.getAbsolutePosition().getValueAsDouble() / Settings.HoodedShooter.Hood.ENCODER_TO_MECH); } @Override @@ -125,7 +126,8 @@ public void periodic() { * 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); + hoodMotor.setPosition(Settings.HoodedShooter.Angles.MIN_ANGLE.getRotations() + hoodEncoder.getAbsolutePosition().getValueAsDouble() / Settings.HoodedShooter.Hood.ENCODER_TO_MECH); + // seedHood(); hasUsedAbsoluteEncoder = true; } @@ -140,7 +142,7 @@ public void periodic() { } if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("HoodedShooter/Hood/Hood Absolute Angle (deg)", hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.HoodedShooter.Hood.ENCODER_TO_MECH); + SmartDashboard.putNumber("HoodedShooter/Hood/Hood Absolute Angle (deg)", Settings.HoodedShooter.Angles.MIN_ANGLE.getDegrees() + 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()); 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 896ee6a7..ad65eab9 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 @@ -43,6 +43,10 @@ public ShooterImpl() { Gains.HoodedShooter.Shooter.kD.get(), 0) .withFFConstants(Gains.HoodedShooter.Shooter.kS.get(), Gains.HoodedShooter.Shooter.kV.get(), Gains.HoodedShooter.Shooter.kA.get(), 0) + + // .withPIDConstants(Gains.HoodedShooter.Shooter.kP, Gains.HoodedShooter.Shooter.kI, Gains.HoodedShooter.Shooter.kD, 0) + // .withFFConstants(Gains.HoodedShoothooter.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); shooterLeader = new TalonFX(Ports.HoodedShooter.Shooter.MOTOR_LEAD, Ports.RIO); 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 4b976690..9d89e8d0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -88,6 +88,8 @@ public IntakeImpl() { .withEnableFOC(true); follower = new Follower(Ports.Intake.ROLLER_LEADER, MotorAlignmentValue.Aligned); + rollerFollower.setControl(follower); + velLimit = new SettableNumber(Settings.Intake.PIVOT_MAX_VEL_DEPLOY.getDegrees()); accelLimit = new SettableNumber(Settings.Intake.PIVOT_MAX_ACCEL_DEPLOY.getDegrees()); @@ -185,10 +187,12 @@ else if (pivotVoltageOverride.isPresent()) { if (getPivotAngle().getDegrees() <= Settings.Intake.THRESHHOLD_TO_START_ROLLERS.getDegrees()) { rollerLeader.setControl(rollerController.withOutput(getRollerState().getTargetDutyCycle())); + rollerFollower.setControl(follower); } else { rollerLeader.stopMotor(); + rollerFollower.stopMotor(); } } } 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 f2e048e1..9ff66f3b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -40,7 +40,7 @@ public SpindexerImpl() { .withCurrentLimitEnable(false) .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withInvertedValue(InvertedValue.Clockwise_Positive) .withFFConstants(Gains.Spindexer.kS.get(), Gains.Spindexer.kV.get(), Gains.Spindexer.kA.get(), 0) .withPIDConstants(Gains.Spindexer.kP.get(), Gains.Spindexer.kI.get(), Gains.Spindexer.kD.get(), 0) .withSensorToMechanismRatio(Settings.Spindexer.Constants.GEAR_RATIO); @@ -49,7 +49,7 @@ public SpindexerImpl() { .withCurrentLimitEnable(false) .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withInvertedValue(InvertedValue.Clockwise_Positive) .withFFConstants(Gains.Spindexer.kS.get(), Gains.Spindexer.kV.get(), Gains.Spindexer.kA.get(), 0) .withPIDConstants(Gains.Spindexer.kP.get(), Gains.Spindexer.kI.get(), Gains.Spindexer.kD.get(), 0) .withSensorToMechanismRatio(Settings.Spindexer.Constants.GEAR_RATIO); @@ -63,17 +63,17 @@ public SpindexerImpl() { controller = new VelocityVoltage(getTargetRPM()) .withEnableFOC(true); - follower = new Follower(Ports.Spindexer.SPINDEXER_LEAD_MOTOR, MotorAlignmentValue.Opposed); + follower = new Follower(Ports.Spindexer.SPINDEXER_LEAD_MOTOR, MotorAlignmentValue.Aligned); voltageOverride = Optional.empty(); } public double getCurrentLeadMotorRPM() { - return leadMotor.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE; + return leadMotor.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE * Settings.Spindexer.Constants.GEAR_RATIO; } public double getCurrentFollowerMotorRPM() { - return followerMotor.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE; + return followerMotor.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE * Settings.Spindexer.Constants.GEAR_RATIO; } public boolean atTolerance() { 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 9793fd15..636ab808 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java @@ -70,7 +70,7 @@ public TurretImpl() { .withAbsoluteSensorDiscontinuityPoint(1.0); motor = new TalonFX(Ports.Turret.MOTOR, Ports.RIO); - encoder17t = new CANcoder(Ports.Turret.ENCODER17T, Ports.RIO); + encoder17t = new CANcoder(Ports.Turret.ENCODER17T, Ports.RIO); encoder18t = new CANcoder(Ports.Turret.ENCODER18T, Ports.RIO); turretConfig.configure(motor); From 969fae6709d5ac68f22508e132d28b174d08b0cd Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Tue, 3 Mar 2026 16:02:49 -0500 Subject: [PATCH 051/150] FEAT: hood tuning, hood zeroing, scoring routine with swerve alignment, removed other limelights --- .../com/stuypulse/robot/RobotContainer.java | 23 +++++--- .../commands/hoodedshooter/ZeroHood.java | 2 +- .../swerve/SwerveDriveAlignTurretToHub.java | 3 +- .../stuypulse/robot/constants/Cameras.java | 16 +++--- .../com/stuypulse/robot/constants/Gains.java | 6 +- .../stuypulse/robot/constants/Settings.java | 17 ++++-- .../subsystems/hoodedshooter/hood/Hood.java | 2 + .../hoodedshooter/hood/HoodImpl.java | 55 ++++++++++++++----- .../robot/subsystems/intake/IntakeImpl.java | 10 ++-- .../swerve/CommandSwerveDrivetrain.java | 4 +- .../subsystems/vision/LimelightVision.java | 2 +- .../hoodedshooter/HoodAngleCalculator.java | 2 +- 12 files changed, 91 insertions(+), 51 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 5489a062..e749436d 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -32,6 +32,7 @@ import com.stuypulse.robot.commands.intake.SeedPivot; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.swerve.SwerveDriveAlignTurretToHub; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveWheelRadiusCharacterization; @@ -69,10 +70,10 @@ public interface EnabledSubsystems { SmartBoolean SWERVE = new SmartBoolean("Enabled Subsystems/Swerve Is Enabled", true); SmartBoolean TURRET = new SmartBoolean("Enabled Subsystems/Turret Is Enabled", false); SmartBoolean HANDOFF = new SmartBoolean("Enabled Subsystems/Handoff Is Enabled", true); - SmartBoolean INTAKE = new SmartBoolean("Enabled Subsystems/Intake Is Enabled", false); + SmartBoolean INTAKE = new SmartBoolean("Enabled Subsystems/Intake Is Enabled", true); SmartBoolean SPINDEXER = new SmartBoolean("Enabled Subsystems/Spindexer Is Enabled", true); SmartBoolean CLIMBER_HOPPER = new SmartBoolean("Enabled Subsystems/Climber-Hopper Is Enabled", false); - SmartBoolean HOOD = new SmartBoolean("Enabled Subsystems/Hood Is Enabled", false); + SmartBoolean HOOD = new SmartBoolean("Enabled Subsystems/Hood Is Enabled", true); SmartBoolean SHOOTER = new SmartBoolean("Enabled Subsystems/Shooter Is Enabled", true); SmartBoolean LIMELIGHT = new SmartBoolean("Enabled Subsystems/Limelight Is Enabled", true); } @@ -107,13 +108,13 @@ public RobotContainer() { SmartDashboard.putData("Field", Field.FIELD2D); SmartDashboard.putData("Robot/Reset Pivot", new SeedPivot()); - SmartDashboard.putData("Robot/Zero Turret Encoders", new TurretZero()); + SmartDashboard.putData("Robot/Zero Turret Encoders", new TurretZero().ignoringDisable(true)); SmartDashboard.putData("Robot/Override Up", new ClimberOverrideUp()); SmartDashboard.putData("Robot/Override Down", new ClimberOverrideDown()); SmartDashboard.putData("Robot/Override Stop", new ClimberOverrideStop()); - SmartDashboard.putData("Robot/Zero Hood Encoder", new ZeroHood()); + SmartDashboard.putData("Robot/Zero Hood Encoder", new ZeroHood().ignoringDisable(true)); } @@ -165,8 +166,11 @@ private void configureButtonBindings() { driver.getDPadUp() .onTrue(new SwerveResetHeading()) .onTrue(new ResetLimelightIMU()) - .onFalse(new SetIMUMode(0)); + .onFalse(new SetIMUMode(0)); + driver.getRightMenuButton() + .onTrue(new SpindexerRun()) + .onFalse(new SpindexerStop()); // driver.getTopButton() // .whileTrue(new TurretShoot()); @@ -179,12 +183,13 @@ private void configureButtonBindings() { // Scoring Routine using Interpolation Settings driver.getTopButton() - .whileTrue(new HoodedShooterShoot() + .whileTrue(new HoodedShooterInterpolation() + .alongWith(new SwerveDriveAlignTurretToHub()) // .alongWith(new TurretShoot()) - .alongWith(new WaitUntilCommand(() -> hoodedShooter.isShooterAtTolerance())) - .andThen(new HandoffRun().onlyIf(() -> hoodedShooter.isShooterAtTolerance()) + .andThen(new WaitUntilCommand(() -> hoodedShooter.bothAtTolerance() )) + .andThen(new HandoffRun().onlyIf(() -> hoodedShooter.bothAtTolerance()) .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) - .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && hoodedShooter.isShooterAtTolerance())))) + .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/commands/hoodedshooter/ZeroHood.java b/src/main/java/com/stuypulse/robot/commands/hoodedshooter/ZeroHood.java index 5ef71949..f9f20fa1 100644 --- a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/ZeroHood.java +++ b/src/main/java/com/stuypulse/robot/commands/hoodedshooter/ZeroHood.java @@ -16,6 +16,6 @@ public ZeroHood() { @Override public void initialize() { hood.zeroHoodEncoder(); - hood.seedHood(); + // hood.seedHood(); } } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java index ccf32039..dd0e50fb 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java @@ -66,6 +66,7 @@ public void execute() { Angle.fromRotation2d(getTargetAngle()), Angle.fromRotation2d(robot.getRotation())))); - SmartDashboard.putNumber("Swerve/Target angle hub deg", turret.getPointAtTargetAngle(Field.getHubPose()).getDegrees()); + SmartDashboard.putNumber("Swerve/Angle Error", angleController.getError().toDegrees()); + SmartDashboard.putNumber("Swerve/Target Angle Hub Deg", turret.getPointAtTargetAngle(Field.getHubPose()).getDegrees()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index f51ed9a6..069de364 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -21,14 +21,14 @@ public interface Cameras { new Pose3d(-0.233, 0.375959, 0.20368, new Rotation3d(Units.degreesToRadians(180), Units.degreesToRadians(31.08), Units.degreesToRadians(-80))), RobotContainer.EnabledSubsystems.LIMELIGHT), - new Camera("limelight-shooter", - new Pose3d(Units.inchesToMeters(2.490), Units.inchesToMeters(14.8620), Units.inchesToMeters(5.920157), - new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15.0), Units.degreesToRadians(251.))), - RobotContainer.EnabledSubsystems.LIMELIGHT), - new Camera("limelight-module", - new Pose3d(Units.inchesToMeters(2.490), Units.inchesToMeters(14.8620), Units.inchesToMeters(5.920157), - new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15.0), Units.degreesToRadians(251.))), - RobotContainer.EnabledSubsystems.LIMELIGHT) + // new Camera("limelight-shooter", + // new Pose3d(Units.inchesToMeters(2.490), Units.inchesToMeters(14.8620), Units.inchesToMeters(5.920157), + // new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15.0), Units.degreesToRadians(251.))), + // RobotContainer.EnabledSubsystems.LIMELIGHT), + // new Camera("limelight-module", + // new Pose3d(Units.inchesToMeters(2.490), Units.inchesToMeters(14.8620), Units.inchesToMeters(5.920157), + // new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15.0), Units.degreesToRadians(251.))), + // RobotContainer.EnabledSubsystems.LIMELIGHT) }; diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 93d36472..fae4c13b 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -49,9 +49,9 @@ public interface Shooter { } public interface Hood { - SmartNumber kP = new SmartNumber("HoodedShooter/Hood/Gains/kP", 400.0); + SmartNumber kP = new SmartNumber("HoodedShooter/Hood/Gains/kP", 250.0); //300.0); SmartNumber kI = new SmartNumber("HoodedShooter/Hood/Gains/kI", 0.0); - SmartNumber kD = new SmartNumber("HoodedShooter/Hood/Gains/kD", 0.0); + SmartNumber kD = new SmartNumber("HoodedShooter/Hood/Gains/kD", 2.0); //0.5); SmartNumber kS = new SmartNumber("HoodedShooter/Hood/Gains/kS", 0.25); SmartNumber kA = new SmartNumber("HoodedShooter/Hood/Gains/kA", 0.0); @@ -187,7 +187,7 @@ public interface Rotation { double akD = 0.0; PIDConstants XY = new PIDConstants(3.0, 0.0, 0.2); - PIDConstants THETA = new PIDConstants(3.0, 0.0, 0.2); + PIDConstants THETA = new PIDConstants(13.0, 0.0, 0.5); } } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 21b3c160..42050c75 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -110,8 +110,8 @@ public interface Intake { } public interface Spindexer { - double FORWARD_SPEED = 6000.0; - double REVERSE_SPEED = -6000.0; + double FORWARD_SPEED = 4500.0; + double REVERSE_SPEED = -4500.0; double STOP_SPEED = 0.0; double RPM_TOLERANCE = 400.0; @@ -122,7 +122,7 @@ public interface Constants { } public interface HoodedShooter { - public final double SHOOTER_TOLERANCE_RPM = 50.0; + public final double SHOOTER_TOLERANCE_RPM = 100.0; public final double HOOD_TOLERANCE_DEG = 0.5; public interface RPMs { @@ -137,7 +137,7 @@ 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 MIN_ANGLE= Rotation2d.fromDegrees(7.0); public final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(40.0); public final Rotation2d KB_ANGLE = Rotation2d.fromDegrees(12.0); public final Rotation2d LEFT_CORNER_ANGLE = Rotation2d.fromDegrees(10.0); @@ -196,7 +196,12 @@ public interface Hood { public final double GEAR_RATIO = 1064.0 / 9.0; public final double ENCODER_TO_MECH = 32.0 / 3.0; - public final Rotation2d ENCODER_OFFSET = Rotation2d.fromRotations(0.01344); + public final Rotation2d ENCODER_OFFSET = Rotation2d.fromRotations(-0.043); + + public final Rotation2d FORWARD_SOFT_LIMIT = Rotation2d.fromDegrees(37.0); + public final Rotation2d REVERSE_SOFT_LIMIT = Rotation2d.fromDegrees(10.0); + + public final double HOOD_STALL_CURRENT_LIMIT = 20.0; } } public interface ShootOnTheFly { @@ -222,7 +227,7 @@ public interface Targets {} 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; + public final double THETA_TOLERANCE_DEG = 3.0; public final Pose2d POSE_TOLERANCE = new Pose2d( Units.inchesToMeters(2.0), 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 6d162bf2..7e15a43f 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 @@ -84,6 +84,8 @@ public Rotation2d hoodAnalogToOutput() { return this.driverInput; } + + public abstract boolean isStalling(); public abstract SysIdRoutine getHoodSysIdRoutine(); public abstract void zeroHoodEncoder(); public abstract void seedHood(); 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 4dbfd4c2..1de313e4 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 @@ -11,6 +11,8 @@ import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.SysId; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -48,6 +50,8 @@ public class HoodImpl extends Hood { private boolean hasUsedAbsoluteEncoder; + private final BStream isStalling; + public HoodImpl() { hoodConfig = new Motors.TalonFXConfig() .withCurrentLimitAmps(80.0) @@ -59,9 +63,9 @@ public HoodImpl() { .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 0) .withSensorToMechanismRatio(Settings.HoodedShooter.Hood.GEAR_RATIO) .withSoftLimits( - false, false, // TODO: make true - Settings.HoodedShooter.Angles.MAX_ANGLE.getRotations(), - Settings.HoodedShooter.Angles.MIN_ANGLE.getRotations()); + true, true, + Settings.HoodedShooter.Hood.FORWARD_SOFT_LIMIT.getRotations(), + Settings.HoodedShooter.Hood.REVERSE_SOFT_LIMIT.getRotations()); hoodEncoderConfig = new Motors.CANCoderConfig() .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) @@ -78,6 +82,12 @@ public HoodImpl() { .withEnableFOC(true); voltageOverride = Optional.empty(); + + isStalling = BStream.create(() -> hoodMotor.getSupplyCurrent().getValueAsDouble() > Settings.HoodedShooter.Hood.HOOD_STALL_CURRENT_LIMIT) //TODO: update value in Settings after testing + .filtered(new BDebounce.Both(0.5)); + + seedHood(); + } @Override @@ -87,28 +97,48 @@ public Rotation2d getHoodAngle() { // TODO: INCORRECT RIGHT NOW @Override - public void zeroHoodEncoder() { - double currentPos = hoodEncoder.getAbsolutePosition().getValueAsDouble(); + //TODO: DEBUGGGG!!! + public void zeroHoodEncoder() { hoodEncoder.getConfigurator().refresh(hoodEncoderConfig.getConfiguration().MagnetSensor); double currentOffset = hoodEncoderConfig.getConfiguration().MagnetSensor.MagnetOffset; - double newOffset = currentOffset - currentPos; + + double positionWithCurrentOffset = hoodEncoder.getPosition().getValueAsDouble(); + double newOffset = -((positionWithCurrentOffset - currentOffset) - Settings.HoodedShooter.Angles.MIN_ANGLE.getRotations()); hoodEncoderConfig.withMagnetOffset(newOffset); hoodEncoderConfig.configure(hoodEncoder); } + + @Override + public boolean isStalling() { + return isStalling.getAsBoolean(); + } + /* + * 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 + */ @Override public void seedHood() { hoodMotor.setPosition(Settings.HoodedShooter.Angles.MIN_ANGLE.getRotations() + hoodEncoder.getAbsolutePosition().getValueAsDouble() / Settings.HoodedShooter.Hood.ENCODER_TO_MECH); } + public double getCorrectHoodAngle() { + return Settings.HoodedShooter.Angles.MIN_ANGLE.getDegrees() + hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.HoodedShooter.Hood.ENCODER_TO_MECH; + } + @Override public void periodic() { super.periodic(); + if (isStalling()) { + // zeroHoodEncoder(); + } + hoodConfig.updateGainsConfig( hoodMotor, 0, @@ -121,14 +151,8 @@ public void periodic() { ); if (!hasUsedAbsoluteEncoder) { - /* - * 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(Settings.HoodedShooter.Angles.MIN_ANGLE.getRotations() + hoodEncoder.getAbsolutePosition().getValueAsDouble() / Settings.HoodedShooter.Hood.ENCODER_TO_MECH); - // seedHood(); - hasUsedAbsoluteEncoder = true; + //seedHood(); + //hasUsedAbsoluteEncoder = true; } if (EnabledSubsystems.HOOD.get()) { @@ -142,7 +166,7 @@ public void periodic() { } if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("HoodedShooter/Hood/Hood Absolute Angle (deg)", Settings.HoodedShooter.Angles.MIN_ANGLE.getDegrees() + hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.HoodedShooter.Hood.ENCODER_TO_MECH); + SmartDashboard.putNumber("HoodedShooter/Hood/Correct Hood Angle (deg)", Settings.HoodedShooter.Angles.MIN_ANGLE.getDegrees() + 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()); @@ -153,6 +177,7 @@ public void periodic() { SmartDashboard.putBoolean("HoodedShooter/Hood/Has Used Absolute Encoder", hasUsedAbsoluteEncoder); SmartDashboard.putNumber("InterpolationTesting/Hood Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("HoodedShooter/Hood/Raw Motor Encoder Value", hoodMotor.getPosition().getValueAsDouble()); SmartDashboard.putNumber("Current Draws/Hood (amps)", hoodMotor.getSupplyCurrent().getValueAsDouble()); } 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 9d89e8d0..94e69068 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -55,8 +55,9 @@ public class IntakeImpl extends Intake { public IntakeImpl() { //TODO: refactor pivotConfig = new Motors.TalonFXConfig() - .withCurrentLimitEnable(false) - //.withRampRate(0.25) + .withCurrentLimitEnable(true) + .withRampRate(0.25) + .withCurrentLimitAmps(60) .withNeutralMode(NeutralModeValue.Brake) .withInvertedValue(InvertedValue.Clockwise_Positive) .withPIDConstants(Gains.Intake.Pivot.kP.get(), Gains.Intake.Pivot.kI.get(), Gains.Intake.Pivot.kD.get(), 0) @@ -67,8 +68,9 @@ public IntakeImpl() { .withMotionProfile(Settings.Intake.PIVOT_MAX_VEL_STOW.getRotations(), Settings.Intake.PIVOT_MAX_ACCEL_STOW.getRotations()); rollerConfig = new Motors.TalonFXConfig() - .withCurrentLimitEnable(false) - //.withRampRate(0.50) + .withCurrentLimitEnable(true) + .withRampRate(0.50) + .withCurrentLimitAmps(45) .withNeutralMode(NeutralModeValue.Coast) .withInvertedValue(InvertedValue.CounterClockwise_Positive); 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 23e2d87f..ff452b37 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -50,7 +50,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem { private final static CommandSwerveDrivetrain instance; - private FieldObject2d turret2d = Field.FIELD2D.getObject("Turret 2D"); + // private FieldObject2d turret2d = Field.FIELD2D.getObject("Turret 2D"); private Pose2d turretPose = new Pose2d(); static { @@ -445,7 +445,7 @@ public void periodic() { pose.getRotation().plus(Turret.getInstance().getAngle()) ); - turret2d.setPose(Robot.isBlue() ? turretPose : Field.transformToOppositeAlliance(turretPose)); + // 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())); diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 1324af89..43822d21 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -152,7 +152,7 @@ public void periodic() { double timestamp = poseEstimate.timestampSeconds; CommandSwerveDrivetrain.getInstance().addVisionMeasurement(robotPose, timestamp, Settings.Vision.MT1_STDEVS); - + SmartDashboard.putNumber("Vision/Pose X Component", robotPose.getX()); SmartDashboard.putNumber("Vision/Pose Y Component", robotPose.getY()); SmartDashboard.putNumber("Vision/Pose Theta (Degrees)", robotPose.getRotation().getDegrees()); 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 551f1ac7..0abb933f 100644 --- a/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java @@ -64,7 +64,7 @@ public static Supplier interpolateHoodAngle() { Translation2d hubPose = Field.getHubPose().getTranslation(); Translation2d currentPose = swerve.getTurretPose().getTranslation(); - double distanceMeters = hubPose.getDistance(currentPose); + double distanceMeters = hubPose.getDistance(currentPose); Rotation2d targetAngle = Rotation2d.fromRadians(distanceAngleInterpolator.get(distanceMeters)); From 4adf5d7e7447f9ebd7836737dae61265a5850626 Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Tue, 3 Mar 2026 21:29:57 -0500 Subject: [PATCH 052/150] FIX: current limits - changed spindexer supply current limit from disabled to 60A, drive motor supply current limit set to 80A --- .../com/stuypulse/robot/constants/Motors.java | 27 ++++++++++++------- .../climberhopper/ClimberHopperImpl.java | 4 +-- .../robot/subsystems/handoff/HandoffImpl.java | 3 ++- .../hoodedshooter/hood/HoodImpl.java | 14 ++-------- .../hoodedshooter/shooter/ShooterImpl.java | 7 ++--- .../robot/subsystems/intake/IntakeImpl.java | 9 +++---- .../subsystems/spindexer/SpindexerImpl.java | 6 +++-- .../subsystems/swerve/TunerConstants.java | 6 ++++- .../robot/subsystems/turret/TurretImpl.java | 3 +++ 9 files changed, 41 insertions(+), 38 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index 4084eed6..0f2c9e64 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -318,15 +318,6 @@ public TalonFXConfig withRampRate(double rampRate) { // CURRENT LIMIT CONFIGS - public TalonFXConfig withCurrentLimitAmps(double currentLimitAmps) { - currentLimitsConfigs.StatorCurrentLimit = currentLimitAmps; - currentLimitsConfigs.StatorCurrentLimitEnable = true; - - configuration.withCurrentLimits(currentLimitsConfigs); - - return this; - } - public TalonFXConfig withLowerLimitSupplyCurrent(double currentLowerLimitAmps) { currentLimitsConfigs.SupplyCurrentLowerLimit = currentLowerLimitAmps; currentLimitsConfigs.StatorCurrentLimitEnable = true; @@ -345,8 +336,24 @@ public TalonFXConfig withSupplyCurrentLimitAmps(double currentLimitAmps) { return this; } - public TalonFXConfig withCurrentLimitEnable(boolean enabled) { + public TalonFXConfig withSupplyCurrentLimitEnabled(boolean enabled) { currentLimitsConfigs.SupplyCurrentLimitEnable = enabled; + + configuration.withCurrentLimits(currentLimitsConfigs); + + return this; + } + + public TalonFXConfig withStatorCurrentLimitAmps(double currentLimitAmps) { + currentLimitsConfigs.StatorCurrentLimit = currentLimitAmps; + currentLimitsConfigs.StatorCurrentLimitEnable = true; + + configuration.withCurrentLimits(currentLimitsConfigs); + + return this; + } + + public TalonFXConfig withStatorCurrentLimitEnabled(boolean enabled) { currentLimitsConfigs.StatorCurrentLimitEnable = enabled; configuration.withCurrentLimits(currentLimitsConfigs); 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 8be584a3..cb524f22 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java @@ -36,10 +36,10 @@ public ClimberHopperImpl() { super(); climberHopperConfig = new Motors.TalonFXConfig() + .withSupplyCurrentLimitAmps(50.0) + .withStatorCurrentLimitEnabled(false) .withInvertedValue(InvertedValue.CounterClockwise_Positive) .withNeutralMode(NeutralModeValue.Brake) - .withCurrentLimitAmps(50.0) - .withSupplyCurrentLimitAmps(50.0) .withRampRate(Settings.ClimberHopper.RAMP_RATE) .withSoftLimits( false, false, 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 7e99a4ce..b70d7324 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -31,7 +31,8 @@ public class HandoffImpl extends Handoff { public HandoffImpl() { handoffConfig = new Motors.TalonFXConfig() - .withCurrentLimitAmps(80.0) + .withSupplyCurrentLimitAmps(80.0) + .withStatorCurrentLimitEnabled(false) .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) .withInvertedValue(InvertedValue.CounterClockwise_Positive) 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 1de313e4..6fb2eedd 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 @@ -26,17 +26,6 @@ import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import java.util.Optional; -import com.stuypulse.robot.RobotContainer.EnabledSubsystems; -import com.stuypulse.robot.constants.Gains; -import com.stuypulse.robot.constants.Motors; -import com.stuypulse.robot.constants.Ports; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.util.SysId; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; - public class HoodImpl extends Hood { private final Motors.TalonFXConfig hoodConfig; private final Motors.CANCoderConfig hoodEncoderConfig; @@ -54,7 +43,8 @@ public class HoodImpl extends Hood { public HoodImpl() { hoodConfig = new Motors.TalonFXConfig() - .withCurrentLimitAmps(80.0) + .withSupplyCurrentLimitAmps(80.0) + .withStatorCurrentLimitEnabled(false) .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) .withInvertedValue(InvertedValue.CounterClockwise_Positive) 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 ad65eab9..5f77405e 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 @@ -36,17 +36,14 @@ public class ShooterImpl extends Shooter { public ShooterImpl() { shooterConfig = new Motors.TalonFXConfig() - .withCurrentLimitEnable(false) + .withStatorCurrentLimitEnabled(false) + .withSupplyCurrentLimitEnabled(false) .withNeutralMode(NeutralModeValue.Coast) .withInvertedValue(InvertedValue.CounterClockwise_Positive) .withPIDConstants(Gains.HoodedShooter.Shooter.kP.get(), Gains.HoodedShooter.Shooter.kI.get(), Gains.HoodedShooter.Shooter.kD.get(), 0) .withFFConstants(Gains.HoodedShooter.Shooter.kS.get(), Gains.HoodedShooter.Shooter.kV.get(), Gains.HoodedShooter.Shooter.kA.get(), 0) - - // .withPIDConstants(Gains.HoodedShooter.Shooter.kP, Gains.HoodedShooter.Shooter.kI, Gains.HoodedShooter.Shooter.kD, 0) - // .withFFConstants(Gains.HoodedShoothooter.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); shooterLeader = new TalonFX(Ports.HoodedShooter.Shooter.MOTOR_LEAD, Ports.RIO); 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 94e69068..5c5b62b7 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -53,11 +53,10 @@ public class IntakeImpl extends Intake { private BStream pivotStalling; public IntakeImpl() { - //TODO: refactor pivotConfig = new Motors.TalonFXConfig() - .withCurrentLimitEnable(true) + .withSupplyCurrentLimitAmps(60) + .withStatorCurrentLimitEnabled(false) .withRampRate(0.25) - .withCurrentLimitAmps(60) .withNeutralMode(NeutralModeValue.Brake) .withInvertedValue(InvertedValue.Clockwise_Positive) .withPIDConstants(Gains.Intake.Pivot.kP.get(), Gains.Intake.Pivot.kI.get(), Gains.Intake.Pivot.kD.get(), 0) @@ -68,9 +67,9 @@ public IntakeImpl() { .withMotionProfile(Settings.Intake.PIVOT_MAX_VEL_STOW.getRotations(), Settings.Intake.PIVOT_MAX_ACCEL_STOW.getRotations()); rollerConfig = new Motors.TalonFXConfig() - .withCurrentLimitEnable(true) + .withSupplyCurrentLimitAmps(45) + .withStatorCurrentLimitEnabled(false) .withRampRate(0.50) - .withCurrentLimitAmps(45) .withNeutralMode(NeutralModeValue.Coast) .withInvertedValue(InvertedValue.CounterClockwise_Positive); 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 9ff66f3b..c77b32cb 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -37,7 +37,8 @@ public class SpindexerImpl extends Spindexer { public SpindexerImpl() { spindexerLeadConfig = new Motors.TalonFXConfig() - .withCurrentLimitEnable(false) + .withSupplyCurrentLimitAmps(60) + .withStatorCurrentLimitEnabled(false) .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) .withInvertedValue(InvertedValue.Clockwise_Positive) @@ -46,7 +47,8 @@ public SpindexerImpl() { .withSensorToMechanismRatio(Settings.Spindexer.Constants.GEAR_RATIO); spindexerFollowerConfig = new Motors.TalonFXConfig() - .withCurrentLimitEnable(false) + .withSupplyCurrentLimitAmps(60) + .withStatorCurrentLimitEnabled(false) .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) .withInvertedValue(InvertedValue.Clockwise_Positive) diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java index 0c83c339..ce81aad8 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java @@ -59,7 +59,11 @@ public class TunerConstants { // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withSupplyCurrentLimit(Amps.of(80)) + ); private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() 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 636ab808..0e264d04 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java @@ -41,6 +41,9 @@ public class TurretImpl extends Turret { public TurretImpl() { turretConfig = new Motors.TalonFXConfig() + .withSupplyCurrentLimitAmps(80) + .withStatorCurrentLimitEnabled(false) + .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) .withInvertedValue(InvertedValue.Clockwise_Positive) From 2f7a2b8861f1fad3e81bfde475eb5d52705ed95b Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Tue, 3 Mar 2026 22:14:32 -0500 Subject: [PATCH 053/150] CLEAN: clean up zeroing for hood and intake pivot --- .../com/stuypulse/robot/RobotContainer.java | 77 ++++++------------- ...Hood.java => ZeroHoodAtLowerHardstop.java} | 8 +- .../commands/intake/ZeroPivotDeployed.java | 18 +++++ .../{SeedPivot.java => ZeroPivotStowed.java} | 6 +- .../{TurretZero.java => ZeroTurret.java} | 4 +- .../stuypulse/robot/constants/Settings.java | 6 +- .../subsystems/hoodedshooter/hood/Hood.java | 2 +- .../hoodedshooter/hood/HoodImpl.java | 21 +++-- .../robot/subsystems/intake/Intake.java | 3 +- .../robot/subsystems/intake/IntakeImpl.java | 7 +- 10 files changed, 70 insertions(+), 82 deletions(-) rename src/main/java/com/stuypulse/robot/commands/hoodedshooter/{ZeroHood.java => ZeroHoodAtLowerHardstop.java} (65%) create mode 100644 src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotDeployed.java rename src/main/java/com/stuypulse/robot/commands/intake/{SeedPivot.java => ZeroPivotStowed.java} (69%) rename src/main/java/com/stuypulse/robot/commands/turret/{TurretZero.java => ZeroTurret.java} (90%) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index e749436d..9e00bd0e 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -24,12 +24,13 @@ import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterShoot; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterStow; -import com.stuypulse.robot.commands.hoodedshooter.ZeroHood; +import com.stuypulse.robot.commands.hoodedshooter.ZeroHoodAtLowerHardstop; import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.intake.IntakeRunRollers; import com.stuypulse.robot.commands.intake.IntakeStopRollers; import com.stuypulse.robot.commands.intake.IntakeStow; -import com.stuypulse.robot.commands.intake.SeedPivot; +import com.stuypulse.robot.commands.intake.ZeroPivotDeployed; +import com.stuypulse.robot.commands.intake.ZeroPivotStowed; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.swerve.SwerveDriveAlignTurretToHub; @@ -38,7 +39,7 @@ import com.stuypulse.robot.commands.swerve.SwerveWheelRadiusCharacterization; import com.stuypulse.robot.commands.turret.TurretDefaultCommand; import com.stuypulse.robot.commands.turret.TurretShoot; -import com.stuypulse.robot.commands.turret.TurretZero; +import com.stuypulse.robot.commands.turret.ZeroTurret; import com.stuypulse.robot.commands.vision.ResetLimelightIMU; import com.stuypulse.robot.commands.vision.SetIMUMode; import com.stuypulse.robot.constants.Field; @@ -107,14 +108,14 @@ public RobotContainer() { configureSysids(); SmartDashboard.putData("Field", Field.FIELD2D); - SmartDashboard.putData("Robot/Reset Pivot", new SeedPivot()); - SmartDashboard.putData("Robot/Zero Turret Encoders", new TurretZero().ignoringDisable(true)); + SmartDashboard.putData("Robot/Zero Pivot Encoder at Lower Limit (Deployed)", new ZeroPivotDeployed().ignoringDisable(true)); + SmartDashboard.putData("Robot/Zero Pivot Encoder at Upper Limit (Stowed)", new ZeroPivotStowed().ignoringDisable(true)); + SmartDashboard.putData("Robot/Zero Turret Encoders", new ZeroTurret().ignoringDisable(true)); + SmartDashboard.putData("Robot/Zero Hood Encoder", new ZeroHoodAtLowerHardstop().ignoringDisable(true)); SmartDashboard.putData("Robot/Override Up", new ClimberOverrideUp()); SmartDashboard.putData("Robot/Override Down", new ClimberOverrideDown()); SmartDashboard.putData("Robot/Override Stop", new ClimberOverrideStop()); - - SmartDashboard.putData("Robot/Zero Hood Encoder", new ZeroHood().ignoringDisable(true)); } @@ -133,54 +134,6 @@ private void configureDefaultCommands() { /***************/ private void configureButtonBindings() { - //hood analog - // driver.getBottomButton() - // .whileTrue(new HoodAnalog(driver)); - - - // // Intake Run Rollers - // driver.getLeftTriggerButton() - // .onTrue(new IntakeRunRollers()) - // .onFalse(new IntakeStopRollers()); - - // Intake Down and On - // driver.getRightTriggerButton() - // .onTrue(new IntakeDeploy()); - - //TODO: COMMENT OUT AFTER (POTENTIAL) TESTING - - // driver.getBottomButton() - // .onTrue(new IntakeBangBang()); - - // driver.getTopButton() - // .whileTrue(new HoodedShooterInterpolation()) - // .onFalse(new HoodedShooterStow()); - - driver.getRightTriggerButton() - .onTrue(new IntakeDeploy()); - - driver.getLeftTriggerButton() - .onTrue(new IntakeStow()); - - // Reset Heading - driver.getDPadUp() - .onTrue(new SwerveResetHeading()) - .onTrue(new ResetLimelightIMU()) - .onFalse(new SetIMUMode(0)); - - driver.getRightMenuButton() - .onTrue(new SpindexerRun()) - .onFalse(new SpindexerStop()); - // driver.getTopButton() - // .whileTrue(new TurretShoot()); - - // driver.getBottomButton() - // .whileTrue(new SwerveDriveAlignTurretToHub()); - - // driver.getBottomButton() - // .onTrue(new TurretAnalog(driver)) - // .onFalse(new TurretIdle()); - // Scoring Routine using Interpolation Settings driver.getTopButton() .whileTrue(new HoodedShooterInterpolation() @@ -194,6 +147,20 @@ private void configureButtonBindings() { .alongWith(new HoodedShooterStow()) .alongWith(new HandoffStop())); + // Intake Deploy + driver.getRightTriggerButton() + .onTrue(new IntakeDeploy()); + + // Intake Stow + driver.getLeftTriggerButton() + .onTrue(new IntakeStow()); + + // Reset Heading + driver.getDPadUp() + .onTrue(new SwerveResetHeading()) + .onTrue(new ResetLimelightIMU()) + .onFalse(new SetIMUMode(0)); + // // Ferry Routine using Interpolation Settings // driver.getBottomButton() // .onTrue(new HoodedShooterFerry() diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/ZeroHood.java b/src/main/java/com/stuypulse/robot/commands/hoodedshooter/ZeroHoodAtLowerHardstop.java similarity index 65% rename from src/main/java/com/stuypulse/robot/commands/hoodedshooter/ZeroHood.java rename to src/main/java/com/stuypulse/robot/commands/hoodedshooter/ZeroHoodAtLowerHardstop.java index f9f20fa1..ad7f410f 100644 --- a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/ZeroHood.java +++ b/src/main/java/com/stuypulse/robot/commands/hoodedshooter/ZeroHoodAtLowerHardstop.java @@ -4,10 +4,10 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; -public class ZeroHood extends InstantCommand{ +public class ZeroHoodAtLowerHardstop extends InstantCommand{ private final Hood hood; - public ZeroHood() { + public ZeroHoodAtLowerHardstop() { hood = Hood.getInstance(); addRequirements(hood); @@ -15,7 +15,7 @@ public ZeroHood() { @Override public void initialize() { - hood.zeroHoodEncoder(); - // hood.seedHood(); + hood.zeroHoodEncoderAtLowerHardstop(); + hood.seedHood(); } } diff --git a/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotDeployed.java b/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotDeployed.java new file mode 100644 index 00000000..40015c04 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotDeployed.java @@ -0,0 +1,18 @@ +package com.stuypulse.robot.commands.intake; + +import com.stuypulse.robot.subsystems.intake.Intake; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ZeroPivotDeployed extends InstantCommand { + private Intake intake; + + public ZeroPivotDeployed() { + intake = Intake.getInstance(); + } + + @Override + public void initialize() { + intake.zeroPivotDeployed(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/intake/SeedPivot.java b/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotStowed.java similarity index 69% rename from src/main/java/com/stuypulse/robot/commands/intake/SeedPivot.java rename to src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotStowed.java index 81c8be3f..71e2c559 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/SeedPivot.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotStowed.java @@ -4,15 +4,15 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; -public class SeedPivot extends InstantCommand { +public class ZeroPivotStowed extends InstantCommand { private Intake intake; - public SeedPivot() { + public ZeroPivotStowed() { intake = Intake.getInstance(); } @Override public void initialize() { - intake.seedPivot(); + intake.zeroPivotDeployed(); } } diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretZero.java b/src/main/java/com/stuypulse/robot/commands/turret/ZeroTurret.java similarity index 90% rename from src/main/java/com/stuypulse/robot/commands/turret/TurretZero.java rename to src/main/java/com/stuypulse/robot/commands/turret/ZeroTurret.java index 0fbd07d7..8afbe74e 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretZero.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/ZeroTurret.java @@ -9,11 +9,11 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; -public class TurretZero extends InstantCommand { +public class ZeroTurret extends InstantCommand { private final Turret turret; - public TurretZero() { + public ZeroTurret() { this.turret = Turret.getInstance(); addRequirements(turret); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 42050c75..b8a3b611 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -88,8 +88,8 @@ public interface Intake { Rotation2d PIVOT_ANGLE_TOLERANCE = Rotation2d.fromDegrees(3.0); - Rotation2d PIVOT_MAX_ANGLE = Rotation2d.fromDegrees(90.0); //Rotation2d.fromRotations(-0.0) - Rotation2d PIVOT_MIN_ANGLE = Rotation2d.fromDegrees(0.0); // Rotation2d.fromRotations(-0.2) + Rotation2d PIVOT_MAX_ANGLE = Rotation2d.fromDegrees(90.0); + Rotation2d PIVOT_MIN_ANGLE = Rotation2d.fromDegrees(0.0); Rotation2d PIVOT_MAX_VEL_DEPLOY = Rotation2d.fromDegrees(720.0); Rotation2d PIVOT_MAX_ACCEL_DEPLOY = Rotation2d.fromDegrees(1440.0); @@ -202,7 +202,7 @@ public interface Hood { public final Rotation2d REVERSE_SOFT_LIMIT = Rotation2d.fromDegrees(10.0); public final double HOOD_STALL_CURRENT_LIMIT = 20.0; - } + } } public interface ShootOnTheFly { public final int MAX_ITERATIONS = 5; 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 7e15a43f..9fcdd6f7 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 @@ -87,7 +87,7 @@ public Rotation2d hoodAnalogToOutput() { public abstract boolean isStalling(); public abstract SysIdRoutine getHoodSysIdRoutine(); - public abstract void zeroHoodEncoder(); + public abstract void zeroHoodEncoderAtLowerHardstop(); public abstract void seedHood(); @Override 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 6fb2eedd..dcd2680b 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 @@ -85,11 +85,10 @@ public Rotation2d getHoodAngle() { return Rotation2d.fromRotations(hoodMotor.getPosition().getValueAsDouble()); } - // TODO: INCORRECT RIGHT NOW + //TODO: Implementation as of 3/3 but not yet tested + // Should ONLY be called at the lower hardstop! @Override - - //TODO: DEBUGGGG!!! - public void zeroHoodEncoder() { + public void zeroHoodEncoderAtLowerHardstop() { hoodEncoder.getConfigurator().refresh(hoodEncoderConfig.getConfiguration().MagnetSensor); double currentOffset = hoodEncoderConfig.getConfiguration().MagnetSensor.MagnetOffset; @@ -114,7 +113,8 @@ public boolean isStalling() { */ @Override public void seedHood() { - hoodMotor.setPosition(Settings.HoodedShooter.Angles.MIN_ANGLE.getRotations() + hoodEncoder.getAbsolutePosition().getValueAsDouble() / Settings.HoodedShooter.Hood.ENCODER_TO_MECH); + hoodMotor.setPosition(getCorrectHoodAngle()); + // hoodMotor.setPosition(Settings.HoodedShooter.Angles.MIN_ANGLE.getRotations() + hoodEncoder.getAbsolutePosition().getValueAsDouble() / Settings.HoodedShooter.Hood.ENCODER_TO_MECH); } public double getCorrectHoodAngle() { @@ -125,10 +125,6 @@ public double getCorrectHoodAngle() { public void periodic() { super.periodic(); - if (isStalling()) { - // zeroHoodEncoder(); - } - hoodConfig.updateGainsConfig( hoodMotor, 0, @@ -141,8 +137,8 @@ public void periodic() { ); if (!hasUsedAbsoluteEncoder) { - //seedHood(); - //hasUsedAbsoluteEncoder = true; + seedHood(); + hasUsedAbsoluteEncoder = true; } if (EnabledSubsystems.HOOD.get()) { @@ -156,7 +152,8 @@ public void periodic() { } if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("HoodedShooter/Hood/Correct Hood Angle (deg)", Settings.HoodedShooter.Angles.MIN_ANGLE.getDegrees() + hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.HoodedShooter.Hood.ENCODER_TO_MECH); + SmartDashboard.putNumber("HoodedShooter/Hood/Correct Hood Angle (deg)", getCorrectHoodAngle()); + // SmartDashboard.putNumber("HoodedShooter/Hood/Correct Hood Angle (deg)", Settings.HoodedShooter.Angles.MIN_ANGLE.getDegrees() + 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()); 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 b04b5920..b04936bd 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -113,7 +113,8 @@ public Rotation2d driverInputToAngle() { public abstract SysIdRoutine getPivotSysIdRoutine(); public abstract boolean pivotStalling(); - public abstract void seedPivot(); + public abstract void zeroPivotDeployed(); + public abstract void zeroPivotStowed(); @Override public void periodic() { 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 5c5b62b7..595f45dd 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -142,8 +142,13 @@ public void setPivotState(PivotState pivotState) { } @Override - public void seedPivot() { + public void zeroPivotStowed() { pivot.setPosition(Settings.Intake.PIVOT_MAX_ANGLE.getRotations()); + } + + @Override + public void zeroPivotDeployed() { + pivot.setPosition(Settings.Intake.PIVOT_MIN_ANGLE.getRotations()); } @Override From 6b1c65432727452f6fce0dc5e3a1ea9429eaff4e Mon Sep 17 00:00:00 2001 From: Daniel M Date: Wed, 4 Mar 2026 00:19:03 -0500 Subject: [PATCH 054/150] clean: remove dynamic gains for all subsystems except for climberhopper & shooter --- .../com/stuypulse/robot/constants/Gains.java | 100 ++++++------------ .../robot/subsystems/handoff/HandoffImpl.java | 15 +-- .../hoodedshooter/hood/HoodImpl.java | 24 +---- .../robot/subsystems/intake/IntakeImpl.java | 15 +-- .../subsystems/spindexer/SpindexerImpl.java | 30 +----- .../robot/subsystems/turret/TurretImpl.java | 15 +-- 6 files changed, 49 insertions(+), 150 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index fae4c13b..0f117934 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -17,8 +17,8 @@ public interface ClimberHopper { SmartNumber kD = new SmartNumber("ClimberHopper/Gains/kD", 0.20); SmartNumber kS = new SmartNumber("ClimberHopper/Gains/kS", 0.0); - SmartNumber kA = new SmartNumber("ClimberHopper/Gains/kA", 0.123); - SmartNumber kV = new SmartNumber("ClimberHopper/Gains/kV", 0.0); + SmartNumber kV = new SmartNumber("ClimberHopper/Gains/kV", 0.123); + SmartNumber kA = new SmartNumber("ClimberHopper/Gains/kA", 0.0); // double kP = 1.0; // double kI = 0.0; @@ -36,8 +36,8 @@ public interface Shooter { SmartNumber kD = new SmartNumber("HoodedShooter/Shooter/Gains/kD", 0.0); SmartNumber kS = new SmartNumber("HoodedShooter/Shooter/Gains/kS", 0.0); - SmartNumber kA = new SmartNumber("HoodedShooter/Shooter/Gains/kA", 0.0); SmartNumber kV = new SmartNumber("HoodedShooter/Shooter/Gains/kV", 0.123); + SmartNumber kA = new SmartNumber("HoodedShooter/Shooter/Gains/kA", 0.0); // double kP = 0.45; // double kI = 0.0; @@ -49,92 +49,60 @@ public interface Shooter { } public interface Hood { - SmartNumber kP = new SmartNumber("HoodedShooter/Hood/Gains/kP", 250.0); //300.0); - SmartNumber kI = new SmartNumber("HoodedShooter/Hood/Gains/kI", 0.0); - SmartNumber kD = new SmartNumber("HoodedShooter/Hood/Gains/kD", 2.0); //0.5); - - SmartNumber kS = new SmartNumber("HoodedShooter/Hood/Gains/kS", 0.25); - SmartNumber kA = new SmartNumber("HoodedShooter/Hood/Gains/kA", 0.0); - SmartNumber kV = new SmartNumber("HoodedShooter/Hood/Gains/kV", 0.0); - - // double kP = 300.0; - // double kI = 0.0; - // double kD = 0.0; + double kP = 250.0; //300.0; + double kI = 0.0; + double kD = 2.0; //0.5 - // double kS = 0.0; - // double kV = 0.0; - // double kA = 0.0; + double kS = 0.25; + double kV = 0.0; + double kA = 0.0; } } public interface Spindexer { - SmartNumber kP = new SmartNumber("Spindexer/Gains/kP", 1.20); - SmartNumber kI = new SmartNumber("Spindexer/Gains/kI", 0.0); - SmartNumber kD = new SmartNumber("Spindexer/Gains/kD", 0.0); + double kP = 1.2; + double kI = 0.0; + double kD = 10.0; - SmartNumber kS = new SmartNumber("Spindexer/Gains/kS", 0.25); - SmartNumber kA = new SmartNumber("Spindexer/Gains/kA", 0.010876); - SmartNumber kV = new SmartNumber("Spindexer/Gains/kV", 1.20); //0.9413); + double kS = 0.25; + double kV = 1.2; //0.9413 + double kA = 0.010876; } public interface Intake { public interface Pivot { - SmartNumber kP = new SmartNumber("Intake/Pivot/Gains/kP", 100.0); - SmartNumber kI = new SmartNumber("Intake/Pivot/Gains/kI", 0.0); - SmartNumber kD = new SmartNumber("Intake/Pivot/Gains/kD", 10.0); - - SmartNumber kS = new SmartNumber("Intake/Pivot/Gains/kS", 0.0); - SmartNumber kV = new SmartNumber("Intake/Pivot/Gains/kA", 0.12); - SmartNumber kA = new SmartNumber("Intake/Pivot/Gains/kV", 0.0); - - // double kP = 100.0; // 0.016973 from sysid - // double kI = 0.0; - // double kD = 10.0; + double kP = 100.0; + double kI = 0.0; + double kD = 10.0; - // double kS = 0.0; // 0.1728 from alpha - // double kA = 0.12; - // double kV = 0.0; + double kS = 0.0; + double kV = 0.12; + double kA = 0.0; double kG = 0.5; } } public interface Handoff { - SmartNumber kP = new SmartNumber("Handoff/Gains/kP", 0.000155808); - SmartNumber kI = new SmartNumber("Handoff/Gains/kI", 0.0); - SmartNumber kD = new SmartNumber("Handoff/Gains/kD", 0.0); - - SmartNumber kS = new SmartNumber("Handoff/Gains/kS", 0.1728); - SmartNumber kV = new SmartNumber("Handoff/Gains/kV", 0.12); - SmartNumber kA = new SmartNumber("Handoff/Gains/kA", 0.00284); - - // double kP = 0.00015508; // 0.016973 from sysid - // double kI = 0.0; - // double kD = 0.0; + double kP = 0.00015508; // 0.016973 from sysid + double kI = 0.0; + double kD = 0.0; - // double kS = 0.21149; // 0.1728 from alpha - // double kA = 0.016329; - // double kV = 0.3652; + double kS = 0.1728; + double kV = 0.12; + double kA = 0.00284; } public interface Turret { public interface slot0 { - SmartNumber kP = new SmartNumber("Turret/Gains/kP", 1300.0); - SmartNumber kI = new SmartNumber("Turret/Gains/kI", 0.0); - SmartNumber kD = new SmartNumber("Turret/Gains/kD", 140.0); - - SmartNumber kS = new SmartNumber("Turret/Gains/kS", 0.23); - SmartNumber kV = new SmartNumber("Turret/Gains/kV", 0.0); - SmartNumber kA = new SmartNumber("Turret/Gains/kA", 0.0); - - // double kP = 1300.0; - // double kI = 0.0; - // double kD = 140.0; + double kP = 1300.0; + double kI = 0.0; + double kD = 140.0; - // double kS = 0.23; // FOUND ON 2/25 PD 8 - // double kV = 0.0; - // double kA = 0.0; + double kS = 0.23; // FOUND ON 2/25 PD 8 + double kV = 0.0; + double kA = 0.0; } public interface slot1 { @@ -142,7 +110,7 @@ public interface slot1 { double kI = 0.0; double kD = 0.0; - double kS = 0.0; // FOUND ON 2/25 PD 8 + double kS = 0.0; double kV = 0.0; double kA = 0.0; } 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 b70d7324..9d59ce33 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -36,8 +36,8 @@ public HandoffImpl() { .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) .withInvertedValue(InvertedValue.CounterClockwise_Positive) - .withFFConstants(Gains.Handoff.kS.get(), Gains.Handoff.kV.get(), Gains.Handoff.kA.get(), 0) - .withPIDConstants(Gains.Handoff.kP.get(), Gains.Handoff.kI.get(), Gains.Handoff.kD.get(), 0) + .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); motor = new TalonFX(Ports.Handoff.HANDOFF, Ports.RIO); @@ -61,17 +61,6 @@ public void setVoltageOverride(Optional voltage) { public void periodic() { super.periodic(); - handoffConfig.updateGainsConfig( - motor, - 0, - Gains.Handoff.kP, - Gains.Handoff.kI, - Gains.Handoff.kD, - Gains.Handoff.kS, - Gains.Handoff.kV, - Gains.Handoff.kA - ); - if (EnabledSubsystems.HANDOFF.get()) { if (voltageOverride.isPresent()) { motor.setVoltage(voltageOverride.get()); 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 dcd2680b..8575d776 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 @@ -48,8 +48,8 @@ public HoodImpl() { .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) .withInvertedValue(InvertedValue.CounterClockwise_Positive) - .withPIDConstants(Gains.HoodedShooter.Hood.kP.get(), Gains.HoodedShooter.Hood.kI.get(), Gains.HoodedShooter.Hood.kD.get(), 0) - .withFFConstants(Gains.HoodedShooter.Hood.kS.get(), Gains.HoodedShooter.Hood.kV.get(), Gains.HoodedShooter.Hood.kA.get(), 0) + .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) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 0) .withSensorToMechanismRatio(Settings.HoodedShooter.Hood.GEAR_RATIO) .withSoftLimits( @@ -113,11 +113,10 @@ public boolean isStalling() { */ @Override public void seedHood() { - hoodMotor.setPosition(getCorrectHoodAngle()); - // hoodMotor.setPosition(Settings.HoodedShooter.Angles.MIN_ANGLE.getRotations() + hoodEncoder.getAbsolutePosition().getValueAsDouble() / Settings.HoodedShooter.Hood.ENCODER_TO_MECH); + hoodMotor.setPosition(getAbsoluteHoodAngle()); } - public double getCorrectHoodAngle() { + private double getAbsoluteHoodAngle() { return Settings.HoodedShooter.Angles.MIN_ANGLE.getDegrees() + hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.HoodedShooter.Hood.ENCODER_TO_MECH; } @@ -125,17 +124,6 @@ public double getCorrectHoodAngle() { public void periodic() { super.periodic(); - hoodConfig.updateGainsConfig( - hoodMotor, - 0, - Gains.HoodedShooter.Hood.kP, - Gains.HoodedShooter.Hood.kI, - Gains.HoodedShooter.Hood.kD, - Gains.HoodedShooter.Hood.kS, - Gains.HoodedShooter.Hood.kV, - Gains.HoodedShooter.Hood.kA - ); - if (!hasUsedAbsoluteEncoder) { seedHood(); hasUsedAbsoluteEncoder = true; @@ -152,14 +140,12 @@ public void periodic() { } if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("HoodedShooter/Hood/Correct Hood Angle (deg)", getCorrectHoodAngle()); - // SmartDashboard.putNumber("HoodedShooter/Hood/Correct Hood Angle (deg)", Settings.HoodedShooter.Angles.MIN_ANGLE.getDegrees() + hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.HoodedShooter.Hood.ENCODER_TO_MECH); + SmartDashboard.putNumber("HoodedShooter/Hood/Correct Hood Angle (deg)", getAbsoluteHoodAngle()); SmartDashboard.putNumber("HoodedShooter/Hood/Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("HoodedShooter/Hood/Supply Current", hoodMotor.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("HoodedShooter/Hood/Stator Current", hoodMotor.getStatorCurrent().getValueAsDouble()); - SmartDashboard.putNumber("HoodedShooter/Hood/Closed Loop Error (deg)", hoodMotor.getClosedLoopError().getValueAsDouble() * 360.0); SmartDashboard.putBoolean("HoodedShooter/Hood/Has Used Absolute Encoder", hasUsedAbsoluteEncoder); 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 595f45dd..e3d493c6 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -59,8 +59,8 @@ public IntakeImpl() { .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) .withInvertedValue(InvertedValue.Clockwise_Positive) - .withPIDConstants(Gains.Intake.Pivot.kP.get(), Gains.Intake.Pivot.kI.get(), Gains.Intake.Pivot.kD.get(), 0) - .withFFConstants(Gains.Intake.Pivot.kS.get(), Gains.Intake.Pivot.kV.get(), Gains.Intake.Pivot.kA.get(), Gains.Intake.Pivot.kG, 0) + .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) .withGravityType(GravityTypeValue.Arm_Cosine) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign, 0) .withSensorToMechanismRatio(Settings.Intake.GEAR_RATIO) @@ -155,17 +155,6 @@ public void zeroPivotDeployed() { public void periodic() { super.periodic(); - pivotConfig.updateGainsConfig( - pivot, - 0, - Gains.Intake.Pivot.kP, - Gains.Intake.Pivot.kI, - Gains.Intake.Pivot.kD, - Gains.Intake.Pivot.kS, - Gains.Intake.Pivot.kV, - Gains.Intake.Pivot.kA - ); - if (EnabledSubsystems.INTAKE.get()) { if (getPivotState() == PivotState.ANALOG) { // comment out the setControl line if it breaks 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 c77b32cb..99fdc4bd 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -42,8 +42,8 @@ public SpindexerImpl() { .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) .withInvertedValue(InvertedValue.Clockwise_Positive) - .withFFConstants(Gains.Spindexer.kS.get(), Gains.Spindexer.kV.get(), Gains.Spindexer.kA.get(), 0) - .withPIDConstants(Gains.Spindexer.kP.get(), Gains.Spindexer.kI.get(), Gains.Spindexer.kD.get(), 0) + .withPIDConstants(Gains.Spindexer.kP, Gains.Spindexer.kI, Gains.Spindexer.kD, 0) + .withFFConstants(Gains.Spindexer.kS, Gains.Spindexer.kV, Gains.Spindexer.kA, 0) .withSensorToMechanismRatio(Settings.Spindexer.Constants.GEAR_RATIO); spindexerFollowerConfig = new Motors.TalonFXConfig() @@ -52,8 +52,8 @@ public SpindexerImpl() { .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) .withInvertedValue(InvertedValue.Clockwise_Positive) - .withFFConstants(Gains.Spindexer.kS.get(), Gains.Spindexer.kV.get(), Gains.Spindexer.kA.get(), 0) - .withPIDConstants(Gains.Spindexer.kP.get(), Gains.Spindexer.kI.get(), Gains.Spindexer.kD.get(), 0) + .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); leadMotor = new TalonFX(Ports.Spindexer.SPINDEXER_LEAD_MOTOR, Ports.CANIVORE); @@ -86,28 +86,6 @@ public boolean atTolerance() { public void periodic() { super.periodic(); - spindexerLeadConfig.updateGainsConfig( - leadMotor, - 0, - Gains.Spindexer.kP, - Gains.Spindexer.kI, - Gains.Spindexer.kD, - Gains.Spindexer.kS, - Gains.Spindexer.kV, - Gains.Spindexer.kA - ); - - spindexerFollowerConfig.updateGainsConfig( - followerMotor, - 0, - Gains.Spindexer.kP, - Gains.Spindexer.kI, - Gains.Spindexer.kD, - Gains.Spindexer.kS, - Gains.Spindexer.kV, - Gains.Spindexer.kA - ); - if (EnabledSubsystems.SPINDEXER.get()) { if (voltageOverride.isPresent()) { leadMotor.setVoltage(voltageOverride.get()); 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 0e264d04..8cf529ce 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java @@ -48,8 +48,8 @@ public TurretImpl() { .withNeutralMode(NeutralModeValue.Brake) .withInvertedValue(InvertedValue.Clockwise_Positive) - .withPIDConstants(Gains.Turret.slot0.kP.get(), Gains.Turret.slot0.kI.get(), Gains.Turret.slot0.kD.get(), 0) - .withFFConstants(Gains.Turret.slot0.kS.get(), Gains.Turret.slot0.kV.get(), Gains.Turret.slot0.kA.get(), 0) + .withPIDConstants(Gains.Turret.slot0.kP, Gains.Turret.slot0.kI, Gains.Turret.slot0.kD, 0) + .withFFConstants(Gains.Turret.slot0.kS, Gains.Turret.slot0.kV, Gains.Turret.slot0.kA, 0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 0) .withPIDConstants(Gains.Turret.slot1.kP, Gains.Turret.slot1.kI, Gains.Turret.slot1.kD, 1) @@ -148,17 +148,6 @@ private double getDelta(double target, double current) { public void periodic() { super.periodic(); - turretConfig.updateGainsConfig( - motor, - 0, - Gains.Turret.slot0.kP, - Gains.Turret.slot0.kI, - Gains.Turret.slot0.kD, - Gains.Turret.slot0.kS, - Gains.Turret.slot0.kV, - Gains.Turret.slot0.kA - ); - if (!hasUsedAbsoluteEncoder) { seedTurret(); hasUsedAbsoluteEncoder = true; From 23e7c7f1e05b9aa375b892fd6d9f45a33fac5c2a Mon Sep 17 00:00:00 2001 From: Daniel M Date: Wed, 4 Mar 2026 00:36:27 -0500 Subject: [PATCH 055/150] clean: reorder configs --- .../com/stuypulse/robot/constants/Motors.java | 6 ++--- .../climberhopper/ClimberHopperImpl.java | 2 +- .../robot/subsystems/handoff/HandoffImpl.java | 10 ++++++--- .../hoodedshooter/hood/HoodImpl.java | 11 +++++++--- .../hoodedshooter/shooter/ShooterImpl.java | 17 +++++++++----- .../robot/subsystems/intake/IntakeImpl.java | 22 ++++++++++++------- .../subsystems/spindexer/SpindexerImpl.java | 20 ++++++++++++----- .../robot/subsystems/turret/TurretImpl.java | 17 +++++++------- 8 files changed, 67 insertions(+), 38 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index 0f2c9e64..e5cc39ab 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -363,9 +363,9 @@ public TalonFXConfig withStatorCurrentLimitEnabled(boolean enabled) { // VOLTAGE LIMIT CONFIGS - public TalonFXConfig withMaxVoltage(double forwardPeak, double reversePeak) { - voltageConfigs.PeakForwardVoltage = forwardPeak; - voltageConfigs.PeakReverseVoltage = reversePeak; + public TalonFXConfig withVoltageLimits(double peakForwardVoltage, double peakReverseVoltage) { + voltageConfigs.PeakForwardVoltage = peakForwardVoltage; + voltageConfigs.PeakReverseVoltage = peakReverseVoltage; configuration.withVoltage(voltageConfigs); 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 cb524f22..0fdd39e5 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java @@ -36,9 +36,9 @@ public ClimberHopperImpl() { super(); climberHopperConfig = new Motors.TalonFXConfig() + .withInvertedValue(InvertedValue.CounterClockwise_Positive) .withSupplyCurrentLimitAmps(50.0) .withStatorCurrentLimitEnabled(false) - .withInvertedValue(InvertedValue.CounterClockwise_Positive) .withNeutralMode(NeutralModeValue.Brake) .withRampRate(Settings.ClimberHopper.RAMP_RATE) .withSoftLimits( 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 9d59ce33..ef5755de 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -31,13 +31,17 @@ public class HandoffImpl extends Handoff { public HandoffImpl() { handoffConfig = new Motors.TalonFXConfig() + .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withSupplyCurrentLimitAmps(80.0) .withStatorCurrentLimitEnabled(false) + + .withPIDConstants(Gains.Handoff.kP, Gains.Handoff.kI, Gains.Handoff.kD, 0) + .withFFConstants(Gains.Handoff.kS, Gains.Handoff.kV, Gains.Handoff.kA, 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); motor = new TalonFX(Ports.Handoff.HANDOFF, Ports.RIO); 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 8575d776..5a7fb055 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 @@ -43,15 +43,20 @@ public class HoodImpl extends Hood { public HoodImpl() { hoodConfig = new Motors.TalonFXConfig() + .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withSupplyCurrentLimitAmps(80.0) .withStatorCurrentLimitEnabled(false) - .withRampRate(0.25) - .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .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) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 0) + + .withRampRate(0.25) + .withNeutralMode(NeutralModeValue.Brake) + .withSensorToMechanismRatio(Settings.HoodedShooter.Hood.GEAR_RATIO) + .withSoftLimits( true, true, Settings.HoodedShooter.Hood.FORWARD_SOFT_LIMIT.getRotations(), 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 5f77405e..6b15cdb9 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 @@ -36,14 +36,19 @@ public class ShooterImpl extends Shooter { public ShooterImpl() { shooterConfig = new Motors.TalonFXConfig() - .withStatorCurrentLimitEnabled(false) + .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withSupplyCurrentLimitEnabled(false) + .withStatorCurrentLimitEnabled(false) + + .withPIDConstants(Gains.HoodedShooter.Shooter.kP.get(), + Gains.HoodedShooter.Shooter.kI.get(), + Gains.HoodedShooter.Shooter.kD.get(), 0) + .withFFConstants(Gains.HoodedShooter.Shooter.kS.get(), + Gains.HoodedShooter.Shooter.kV.get(), + Gains.HoodedShooter.Shooter.kA.get(), 0) + .withNeutralMode(NeutralModeValue.Coast) - .withInvertedValue(InvertedValue.CounterClockwise_Positive) - .withPIDConstants(Gains.HoodedShooter.Shooter.kP.get(), Gains.HoodedShooter.Shooter.kI.get(), - Gains.HoodedShooter.Shooter.kD.get(), 0) - .withFFConstants(Gains.HoodedShooter.Shooter.kS.get(), Gains.HoodedShooter.Shooter.kV.get(), - Gains.HoodedShooter.Shooter.kA.get(), 0) .withSensorToMechanismRatio(Settings.HoodedShooter.Shooter.GEAR_RATIO); shooterLeader = new TalonFX(Ports.HoodedShooter.Shooter.MOTOR_LEAD, Ports.RIO); 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 e3d493c6..0a96d10e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -54,24 +54,30 @@ public class IntakeImpl extends Intake { public IntakeImpl() { pivotConfig = new Motors.TalonFXConfig() + .withInvertedValue(InvertedValue.Clockwise_Positive) + .withSupplyCurrentLimitAmps(60) .withStatorCurrentLimitEnabled(false) - .withRampRate(0.25) - .withNeutralMode(NeutralModeValue.Brake) - .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) - .withGravityType(GravityTypeValue.Arm_Cosine) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign, 0) - .withSensorToMechanismRatio(Settings.Intake.GEAR_RATIO) - .withMotionProfile(Settings.Intake.PIVOT_MAX_VEL_STOW.getRotations(), Settings.Intake.PIVOT_MAX_ACCEL_STOW.getRotations()); + .withGravityType(GravityTypeValue.Arm_Cosine) + .withMotionProfile(Settings.Intake.PIVOT_MAX_VEL_STOW.getRotations(), Settings.Intake.PIVOT_MAX_ACCEL_STOW.getRotations()) + + .withNeutralMode(NeutralModeValue.Brake) + .withRampRate(0.25) + + .withSensorToMechanismRatio(Settings.Intake.GEAR_RATIO); rollerConfig = new Motors.TalonFXConfig() + .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withSupplyCurrentLimitAmps(45) .withStatorCurrentLimitEnabled(false) + .withRampRate(0.50) - .withNeutralMode(NeutralModeValue.Coast) - .withInvertedValue(InvertedValue.CounterClockwise_Positive); + .withNeutralMode(NeutralModeValue.Coast); pivot = new TalonFX(Ports.Intake.PIVOT, Ports.RIO); pivotConfig.configure(pivot); 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 99fdc4bd..9e726dd7 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -37,23 +37,31 @@ public class SpindexerImpl extends Spindexer { public SpindexerImpl() { spindexerLeadConfig = new Motors.TalonFXConfig() + .withInvertedValue(InvertedValue.Clockwise_Positive) + .withSupplyCurrentLimitAmps(60) .withStatorCurrentLimitEnabled(false) - .withRampRate(0.25) - .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.Clockwise_Positive) + .withPIDConstants(Gains.Spindexer.kP, Gains.Spindexer.kI, Gains.Spindexer.kD, 0) .withFFConstants(Gains.Spindexer.kS, Gains.Spindexer.kV, Gains.Spindexer.kA, 0) + + .withNeutralMode(NeutralModeValue.Brake) + .withRampRate(0.25) + .withSensorToMechanismRatio(Settings.Spindexer.Constants.GEAR_RATIO); spindexerFollowerConfig = new Motors.TalonFXConfig() + .withInvertedValue(InvertedValue.Clockwise_Positive) + .withSupplyCurrentLimitAmps(60) .withStatorCurrentLimitEnabled(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) + + .withNeutralMode(NeutralModeValue.Brake) + .withRampRate(0.25) + .withSensorToMechanismRatio(Settings.Spindexer.Constants.GEAR_RATIO); leadMotor = new TalonFX(Ports.Spindexer.SPINDEXER_LEAD_MOTOR, Ports.CANIVORE); 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 8cf529ce..ffff25bb 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java @@ -41,12 +41,10 @@ public class TurretImpl extends Turret { public TurretImpl() { turretConfig = new Motors.TalonFXConfig() + .withInvertedValue(InvertedValue.Clockwise_Positive) + .withSupplyCurrentLimitAmps(80) .withStatorCurrentLimitEnabled(false) - - .withRampRate(0.25) - .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.Clockwise_Positive) .withPIDConstants(Gains.Turret.slot0.kP, Gains.Turret.slot0.kI, Gains.Turret.slot0.kD, 0) .withFFConstants(Gains.Turret.slot0.kS, Gains.Turret.slot0.kV, Gains.Turret.slot0.kA, 0) @@ -56,13 +54,17 @@ public TurretImpl() { .withFFConstants(Gains.Turret.slot1.kS, Gains.Turret.slot1.kV, Gains.Turret.slot1.kA, 1) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 1) + .withNeutralMode(NeutralModeValue.Brake) + .withRampRate(0.25) + .withSensorToMechanismRatio(Settings.Turret.Constants.GEAR_RATIO_MOTOR_TO_MECH) .withSoftLimits( false, false, Settings.Turret.Constants.SoftwareLimit.FORWARD_MAX_ROTATIONS, Settings.Turret.Constants.SoftwareLimit.BACKWARDS_MAX_ROTATIONS) - .withMaxVoltage(6, -6); //TODO: VERIFY MAX VOLTAGE + + .withVoltageLimits(6, -6); //TODO: VERIFY MAX VOLTAGE encoder17tConfig = new Motors.CANCoderConfig() .withSensorDirection(SensorDirectionValue.Clockwise_Positive) @@ -73,7 +75,7 @@ public TurretImpl() { .withAbsoluteSensorDiscontinuityPoint(1.0); motor = new TalonFX(Ports.Turret.MOTOR, Ports.RIO); - encoder17t = new CANcoder(Ports.Turret.ENCODER17T, Ports.RIO); + encoder17t = new CANcoder(Ports.Turret.ENCODER17T, Ports.RIO); encoder18t = new CANcoder(Ports.Turret.ENCODER18T, Ports.RIO); turretConfig.configure(motor); @@ -82,8 +84,7 @@ public TurretImpl() { hasUsedAbsoluteEncoder = false; voltageOverride = Optional.empty(); - controller = new PositionVoltage(getTargetAngle().getRotations()) - .withEnableFOC(true); + controller = new PositionVoltage(getTargetAngle().getRotations()).withEnableFOC(true); } private Rotation2d getEncoderPos17t() { From 92c902128615b0da7d5a4ab5f008620f083cac14 Mon Sep 17 00:00:00 2001 From: Alex Wang Date: Wed, 4 Mar 2026 01:05:02 -0500 Subject: [PATCH 056/150] feat: change the default LL IMU mode, add constants to settings for IMU modes --- .../commands/vision/ResetLimelightIMU.java | 4 +++- .../commands/vision/SetIMUAssistValue.java | 19 +++++++++++++++ .../stuypulse/robot/constants/Settings.java | 3 ++- .../subsystems/vision/LimelightVision.java | 24 +++++++++++++++---- 4 files changed, 43 insertions(+), 7 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/vision/SetIMUAssistValue.java diff --git a/src/main/java/com/stuypulse/robot/commands/vision/ResetLimelightIMU.java b/src/main/java/com/stuypulse/robot/commands/vision/ResetLimelightIMU.java index 16ebb206..071e6c3e 100644 --- a/src/main/java/com/stuypulse/robot/commands/vision/ResetLimelightIMU.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/ResetLimelightIMU.java @@ -5,8 +5,10 @@ /***************************************************************/ package com.stuypulse.robot.commands.vision; +import com.stuypulse.robot.constants.Settings; + public class ResetLimelightIMU extends SetIMUMode { public ResetLimelightIMU() { - super(1); + super(Settings.Vision.RESET_IMU_INDEX); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/vision/SetIMUAssistValue.java b/src/main/java/com/stuypulse/robot/commands/vision/SetIMUAssistValue.java new file mode 100644 index 00000000..864e8a32 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/vision/SetIMUAssistValue.java @@ -0,0 +1,19 @@ +package com.stuypulse.robot.commands.vision; + +import com.stuypulse.robot.subsystems.vision.LimelightVision; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class SetIMUAssistValue extends InstantCommand { + private LimelightVision vision; + private double assistValue; + + public SetIMUAssistValue(double assistValue) { + vision = LimelightVision.getInstance(); + } + + @Override + public void initialize() { + vision.setIMUAssistValue(assistValue); + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index b8a3b611..7406f4cd 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -303,6 +303,7 @@ public interface SoftwareLimit { 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); - public final int RESET_IMU_INDEX = 2; + public final int RESET_IMU_INDEX = 1; + public final int INTERNAL_EXTERNAL_ASSIST_INDEX = 4; } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 43822d21..57d04170 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -5,8 +5,6 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.vision; -import com.stuypulse.stuylib.network.SmartBoolean; - import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Cameras; import com.stuypulse.robot.constants.Settings; @@ -14,6 +12,7 @@ import com.stuypulse.robot.util.vision.LimelightHelpers; import com.stuypulse.robot.util.vision.LimelightHelpers.IMUData; import com.stuypulse.robot.util.vision.LimelightHelpers.PoseEstimate; +import com.stuypulse.stuylib.network.SmartBoolean; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -63,7 +62,7 @@ public LimelightVision() { for (int i = 0; i < camerasEnabled.length; i++) { camerasEnabled[i] = new SmartBoolean("Vision/" + names[i] + " Is Enabled", true); - LimelightHelpers.SetIMUMode(names[i], 0); + LimelightHelpers.SetIMUMode(names[i], Settings.Vision.INTERNAL_EXTERNAL_ASSIST_INDEX); SmartDashboard.putBoolean("Vision/" + names[i] + " Has Data", false); } @@ -103,6 +102,19 @@ public void setIMUMode(int mode) { } } + /** + * Allows you to set the convergence speed of the internal LL IMU and robot gyro. + * + * @param assistValue, an double that sets the correction speed of the complementary filter for the IMU. IMU Mode 4 + * uses the fusing of the internal IMU (1khz) with the external gyro reading as well. Higher values ranging towards 1 + * indicate a faster convergence of internal IMU to the robot IMU mode. Defaults to 0.001. + */ + public void setIMUAssistValue(double assistValue) { + for (String name : names) { + LimelightHelpers.SetIMUAssistAlpha(name, assistValue); + } + } + public IMUData[] getIMUData() { IMUData[] data = new IMUData[Cameras.LimelightCameras.length]; @@ -163,9 +175,11 @@ public void periodic() { } SmartDashboard.putString("Vision/MegaTag Mode", megaTagMode.toString()); - SmartDashboard.putNumber("Vision/Robot Yaw", LimelightHelpers.getIMUData(limelightName).robotYaw); + // this yaw is seems to be the robot yaw passed into the LL + SmartDashboard.putNumber("Vision/Limelight Robot Yaw", LimelightHelpers.getIMUData(limelightName).robotYaw); + // this is just the yaw of the internal imu + SmartDashboard.putNumber("Vision/Limelight Yaw", LimelightHelpers.getIMUData(limelightName).Yaw); - SmartDashboard.putString("Vision/IMU mode", getIMUData()[0].toString()); //only 1 camera on alpha, so will need to change on Big T } } } From 5cd8efc5729fa83a49b1c77406a9907371416dbb Mon Sep 17 00:00:00 2001 From: Daniel M Date: Wed, 4 Mar 2026 01:17:03 -0500 Subject: [PATCH 057/150] clean: reorder pt. 2, and a lot of minor refactoring changes to make code a little cleaner and more consistent across subsystems overall. --- .../stuypulse/robot/constants/Settings.java | 8 ++- .../climberhopper/ClimberHopperImpl.java | 28 +++++----- .../robot/subsystems/handoff/Handoff.java | 1 + .../robot/subsystems/handoff/HandoffImpl.java | 55 +++++++++---------- .../subsystems/hoodedshooter/hood/Hood.java | 3 +- .../hoodedshooter/hood/HoodImpl.java | 25 ++++----- .../hoodedshooter/shooter/ShooterImpl.java | 21 +++---- .../robot/subsystems/intake/IntakeImpl.java | 36 +++++------- .../subsystems/spindexer/SpindexerImpl.java | 29 +++++----- .../robot/subsystems/turret/Turret.java | 4 +- .../robot/subsystems/turret/TurretImpl.java | 25 +++++---- 11 files changed, 111 insertions(+), 124 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index b8a3b611..9636e400 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -107,6 +107,7 @@ public interface Intake { double debugVoltage = 0; //TODO: set value double STALL_CURRENT_LIMIT = 0; //TODO: set value + double STALL_DEBOUNCE = 1.0; //TODO: VERIFY } public interface Spindexer { @@ -123,7 +124,7 @@ public interface Constants { public interface HoodedShooter { public final double SHOOTER_TOLERANCE_RPM = 100.0; - public final double HOOD_TOLERANCE_DEG = 0.5; + public final Rotation2d HOOD_TOLERANCE = Rotation2d.fromDegrees(0.5); public interface RPMs { public final SmartNumber SHOOT_RPM = new SmartNumber("HoodedShooter/Shoot State Target RPM", 3500.0); @@ -201,7 +202,8 @@ public interface Hood { public final Rotation2d FORWARD_SOFT_LIMIT = Rotation2d.fromDegrees(37.0); public final Rotation2d REVERSE_SOFT_LIMIT = Rotation2d.fromDegrees(10.0); - public final double HOOD_STALL_CURRENT_LIMIT = 20.0; + public final double STALL_CURRENT_LIMIT = 20.0; + public final double STALL_DEBOUNCE = 0.5; } } public interface ShootOnTheFly { @@ -258,7 +260,7 @@ public interface Constraints { 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 final Rotation2d TOLERANCE = Rotation2d.fromDegrees(2.0); public final Rotation2d HUB = Rotation2d.fromDegrees(0.0); public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(0.0); 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 0fdd39e5..b4368e53 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java @@ -37,10 +37,12 @@ public ClimberHopperImpl() { climberHopperConfig = new Motors.TalonFXConfig() .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) + .withSupplyCurrentLimitAmps(50.0) .withStatorCurrentLimitEnabled(false) - .withNeutralMode(NeutralModeValue.Brake) .withRampRate(Settings.ClimberHopper.RAMP_RATE) + .withSoftLimits( false, false, Settings.ClimberHopper.ROTATIONS_AT_BOTTOM + Settings.ClimberHopper.Constants.NUM_ROTATIONS_TO_REACH_TOP, @@ -77,20 +79,15 @@ private boolean isWithinTolerance(double toleranceMeters) { public boolean atTargetHeight() { return isWithinTolerance(Settings.ClimberHopper.HEIGHT_TOLERANCE_METERS); } - - @Override - public void setVoltageOverride(Optional voltage) { - this.voltageOverride = voltage; - } - + public void resetPostionUpper() { motor.setPosition(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM + Settings.ClimberHopper.Constants.NUM_ROTATIONS_TO_REACH_TOP); } - + @Override public void periodic() { super.periodic(); - + if (voltageOverride.isPresent()) { voltage = voltageOverride.get(); } else { @@ -98,23 +95,28 @@ public void periodic() { else if (getState() == ClimberHopperState.CLIMBER_UP) voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; else voltage = 0; } - + if (EnabledSubsystems.CLIMBER_HOPPER.get()) { motor.setControl(controller.withOutput(voltage)); } else { motor.stopMotor(); } - + if (Settings.DEBUG_MODE) { SmartDashboard.putBoolean("ClimberHopper/Stalling", getStalling()); - + 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()); SmartDashboard.putNumber("ClimberHopper/Stator Current", motor.getStatorCurrent().getValueAsDouble()); - + SmartDashboard.putNumber("Current Draws/ClimberHopper (amps)", motor.getSupplyCurrent().getValueAsDouble()); } } + + @Override + public void setVoltageOverride(Optional voltage) { + this.voltageOverride = voltage; + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java index 469febbb..db900f03 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java @@ -69,5 +69,6 @@ public void periodic() { SmartDashboard.putNumber("Handoff/Target RPM", getTargetRPM()); SmartDashboard.putNumber("Handoff/Current RPM", getCurrentRPM()); + SmartDashboard.putBoolean("Handoff/At Tolerance?", atTolerance()); } } 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 ef5755de..74ee0359 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -32,69 +32,66 @@ public class HandoffImpl extends Handoff { public HandoffImpl() { handoffConfig = new Motors.TalonFXConfig() .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) .withSupplyCurrentLimitAmps(80.0) .withStatorCurrentLimitEnabled(false) + .withRampRate(0.25) .withPIDConstants(Gains.Handoff.kP, Gains.Handoff.kI, Gains.Handoff.kD, 0) .withFFConstants(Gains.Handoff.kS, Gains.Handoff.kV, Gains.Handoff.kA, 0) - .withRampRate(0.25) - .withNeutralMode(NeutralModeValue.Brake) - .withSensorToMechanismRatio(Settings.Handoff.GEAR_RATIO); motor = new TalonFX(Ports.Handoff.HANDOFF, Ports.RIO); handoffConfig.configure(motor); - controller = new VelocityVoltage(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE) - .withEnableFOC(true); + controller = new VelocityVoltage(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE).withEnableFOC(true); voltageOverride = Optional.empty(); } public double getCurrentRPM() { return motor.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE * Settings.Handoff.GEAR_RATIO; } - - @Override - public void setVoltageOverride(Optional voltage) { - this.voltageOverride = voltage; - } - + @Override public void periodic() { super.periodic(); - - if (EnabledSubsystems.HANDOFF.get()) { + + if (EnabledSubsystems.HANDOFF.get() && getState() != HandoffState.STOP) { if (voltageOverride.isPresent()) { motor.setVoltage(voltageOverride.get()); - } else if (getState() == HandoffState.STOP) { - motor.stopMotor(); - } else { - motor.setControl(controller.withVelocity(getTargetRPM() / 60.0)); + } else { + motor.setControl(controller.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE)); } + } else { + motor.stopMotor(); } - + if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("Handoff/Current (amps)", motor.getStatorCurrent().getValueAsDouble()); SmartDashboard.putNumber("Handoff/Voltage", motor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Handoff/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Handoff/Stator Current", motor.getStatorCurrent().getValueAsDouble()); - + SmartDashboard.putNumber("Current Draws/Handoff (amps)", motor.getSupplyCurrent().getValueAsDouble()); } } - + + @Override + public void setVoltageOverride(Optional voltage) { + this.voltageOverride = voltage; + } + @Override public SysIdRoutine getSysIdRoutine() { return SysId.getRoutine( - 2, - 8, - "Handoff", - voltage -> setVoltageOverride(Optional.of(voltage)), - () -> motor.getPosition().getValueAsDouble(), - () -> motor.getVelocity().getValueAsDouble(), - () -> motor.getMotorVoltage().getValueAsDouble(), - getInstance()); + 2, + 8, + "Handoff", + voltage -> setVoltageOverride(Optional.of(voltage)), + () -> motor.getPosition().getValueAsDouble(), + () -> motor.getVelocity().getValueAsDouble(), + () -> motor.getMotorVoltage().getValueAsDouble(), + getInstance()); } } \ 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 9fcdd6f7..1d49d4ba 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 @@ -68,7 +68,8 @@ public Rotation2d getTargetAngle() { } public boolean atTolerance() { - return Math.abs(getHoodAngle().getDegrees() - getTargetAngle().getDegrees()) < Settings.HoodedShooter.HOOD_TOLERANCE_DEG; + double error = getHoodAngle().minus(getTargetAngle()).getRotations(); + return Math.abs(error) < Settings.HoodedShooter.HOOD_TOLERANCE.getRotations(); } public abstract Rotation2d getHoodAngle(); 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 5a7fb055..5ac0bfeb 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 @@ -44,19 +44,18 @@ public class HoodImpl extends Hood { public HoodImpl() { hoodConfig = new Motors.TalonFXConfig() .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) .withSupplyCurrentLimitAmps(80.0) .withStatorCurrentLimitEnabled(false) + .withRampRate(0.25) .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) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 0) - .withRampRate(0.25) - .withNeutralMode(NeutralModeValue.Brake) - .withSensorToMechanismRatio(Settings.HoodedShooter.Hood.GEAR_RATIO) - + .withSoftLimits( true, true, Settings.HoodedShooter.Hood.FORWARD_SOFT_LIMIT.getRotations(), @@ -78,11 +77,8 @@ public HoodImpl() { voltageOverride = Optional.empty(); - isStalling = BStream.create(() -> hoodMotor.getSupplyCurrent().getValueAsDouble() > Settings.HoodedShooter.Hood.HOOD_STALL_CURRENT_LIMIT) //TODO: update value in Settings after testing - .filtered(new BDebounce.Both(0.5)); - - seedHood(); - + isStalling = BStream.create(() -> hoodMotor.getSupplyCurrent().getValueAsDouble() > Settings.HoodedShooter.Hood.STALL_CURRENT_LIMIT) //TODO: update value in Settings after testing + .filtered(new BDebounce.Both(Settings.HoodedShooter.Hood.STALL_DEBOUNCE)); } @Override @@ -118,10 +114,10 @@ public boolean isStalling() { */ @Override public void seedHood() { - hoodMotor.setPosition(getAbsoluteHoodAngle()); + hoodMotor.setPosition(getAbsoluteHoodAngleDeg() / 360.0); } - private double getAbsoluteHoodAngle() { + private double getAbsoluteHoodAngleDeg() { return Settings.HoodedShooter.Angles.MIN_ANGLE.getDegrees() + hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.HoodedShooter.Hood.ENCODER_TO_MECH; } @@ -145,7 +141,7 @@ public void periodic() { } if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("HoodedShooter/Hood/Correct Hood Angle (deg)", getAbsoluteHoodAngle()); + SmartDashboard.putNumber("HoodedShooter/Hood/Correct Hood Angle (deg)", getAbsoluteHoodAngleDeg()); SmartDashboard.putNumber("HoodedShooter/Hood/Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("HoodedShooter/Hood/Supply Current", hoodMotor.getSupplyCurrent().getValueAsDouble()); @@ -154,14 +150,15 @@ public void periodic() { 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("HoodedShooter/Hood/Raw Motor Encoder Value", hoodMotor.getPosition().getValueAsDouble()); + + SmartDashboard.putNumber("InterpolationTesting/Hood Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Current Draws/Hood (amps)", hoodMotor.getSupplyCurrent().getValueAsDouble()); } } - public void setVoltageOverride(Optional voltageOverride) { + private void setVoltageOverride(Optional voltageOverride) { this.voltageOverride = voltageOverride; } 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 6b15cdb9..576b234c 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 @@ -37,18 +37,14 @@ public class ShooterImpl extends Shooter { public ShooterImpl() { shooterConfig = new Motors.TalonFXConfig() .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast) .withSupplyCurrentLimitEnabled(false) .withStatorCurrentLimitEnabled(false) - .withPIDConstants(Gains.HoodedShooter.Shooter.kP.get(), - Gains.HoodedShooter.Shooter.kI.get(), - Gains.HoodedShooter.Shooter.kD.get(), 0) - .withFFConstants(Gains.HoodedShooter.Shooter.kS.get(), - Gains.HoodedShooter.Shooter.kV.get(), - Gains.HoodedShooter.Shooter.kA.get(), 0) + .withPIDConstants(Gains.HoodedShooter.Shooter.kP.get(), Gains.HoodedShooter.Shooter.kI.get(), Gains.HoodedShooter.Shooter.kD.get(), 0) + .withFFConstants(Gains.HoodedShooter.Shooter.kS.get(), Gains.HoodedShooter.Shooter.kV.get(), Gains.HoodedShooter.Shooter.kA.get(), 0) - .withNeutralMode(NeutralModeValue.Coast) .withSensorToMechanismRatio(Settings.HoodedShooter.Shooter.GEAR_RATIO); shooterLeader = new TalonFX(Ports.HoodedShooter.Shooter.MOTOR_LEAD, Ports.RIO); @@ -57,8 +53,7 @@ public ShooterImpl() { shooterConfig.configure(shooterLeader); shooterConfig.configure(shooterFollower); - shooterController = new VelocityVoltage(getTargetRPM() / 60.0) - .withEnableFOC(true); + shooterController = new VelocityVoltage(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE).withEnableFOC(true); follower = new Follower(Ports.HoodedShooter.Shooter.MOTOR_LEAD, MotorAlignmentValue.Opposed); shooterFollower.setControl(follower); @@ -72,11 +67,11 @@ public double getShooterRPM() { } public double getLeaderRPM() { - return shooterLeader.getVelocity().getValueAsDouble() * 60.0; + return shooterLeader.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE; } public double getFollowerRPM() { - return shooterFollower.getVelocity().getValueAsDouble() * 60.0; + return shooterFollower.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE; } @Override @@ -113,7 +108,7 @@ public void periodic() { shooterLeader.setVoltage(voltageOverride.get()); shooterFollower.setControl(follower); } else { - shooterLeader.setControl(shooterController.withVelocity(getTargetRPM() / 60.0)); + shooterLeader.setControl(shooterController.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE)); shooterFollower.setControl(follower); } } else { @@ -139,7 +134,7 @@ public void periodic() { } } - public void setVoltageOverride(Optional voltageOverride) { + private void setVoltageOverride(Optional voltageOverride) { this.voltageOverride = voltageOverride; } 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 0a96d10e..b76dcb2f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -55,9 +55,11 @@ public class IntakeImpl extends Intake { public IntakeImpl() { pivotConfig = new Motors.TalonFXConfig() .withInvertedValue(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) .withSupplyCurrentLimitAmps(60) .withStatorCurrentLimitEnabled(false) + .withRampRate(0.25) .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) @@ -65,19 +67,15 @@ public IntakeImpl() { .withGravityType(GravityTypeValue.Arm_Cosine) .withMotionProfile(Settings.Intake.PIVOT_MAX_VEL_STOW.getRotations(), Settings.Intake.PIVOT_MAX_ACCEL_STOW.getRotations()) - .withNeutralMode(NeutralModeValue.Brake) - .withRampRate(0.25) - .withSensorToMechanismRatio(Settings.Intake.GEAR_RATIO); rollerConfig = new Motors.TalonFXConfig() .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast) .withSupplyCurrentLimitAmps(45) .withStatorCurrentLimitEnabled(false) - - .withRampRate(0.50) - .withNeutralMode(NeutralModeValue.Coast); + .withRampRate(0.50); pivot = new TalonFX(Ports.Intake.PIVOT, Ports.RIO); pivotConfig.configure(pivot); @@ -88,11 +86,8 @@ public IntakeImpl() { rollerFollower = new TalonFX(Ports.Intake.ROLLER_FOLLOWER, Ports.RIO); rollerConfig.configure(rollerFollower); - pivotController = new MotionMagicVoltage(getPivotState().getTargetAngle().getRotations()) - .withEnableFOC(true); - - rollerController = new DutyCycleOut(getRollerState().getTargetDutyCycle()) - .withEnableFOC(true); + pivotController = new MotionMagicVoltage(getPivotState().getTargetAngle().getRotations()).withEnableFOC(true); + rollerController = new DutyCycleOut(getRollerState().getTargetDutyCycle()).withEnableFOC(true); follower = new Follower(Ports.Intake.ROLLER_LEADER, MotorAlignmentValue.Aligned); rollerFollower.setControl(follower); @@ -106,21 +101,18 @@ public IntakeImpl() { pivotStalling = BStream.create( () -> Math.abs(pivot.getSupplyCurrent().getValueAsDouble()) > Settings.Intake.STALL_CURRENT_LIMIT) - .filtered(new BDebounce.Both(1.0)); - + .filtered(new BDebounce.Both(Settings.Intake.STALL_DEBOUNCE)); } @Override public boolean pivotStalling() { - // return Math.abs(pivot.getSupplyCurrent().getValueAsDouble()) > Settings.Intake.debugCurrentLimit; return pivotStalling.get(); } @Override public boolean pivotAtTolerance() { - return Math.abs( - getPivotAngle().getRotations() - getPivotState().getTargetAngle().getRotations()) - < Settings.Intake.PIVOT_ANGLE_TOLERANCE.getRotations(); + double error = getPivotAngle().minus(getPivotState().getTargetAngle()).getRotations(); + return Math.abs(error) < Settings.Intake.PIVOT_ANGLE_TOLERANCE.getRotations(); } @Override @@ -128,7 +120,7 @@ public Rotation2d getPivotAngle() { return Rotation2d.fromRotations(pivot.getPosition().getValueAsDouble()); } - public void setMotionProfileConstraints(Rotation2d velLimit, Rotation2d accelLimit) { + private void setMotionProfileConstraints(Rotation2d velLimit, Rotation2d accelLimit) { this.velLimit.set(velLimit.getDegrees()); this.accelLimit.set(accelLimit.getDegrees()); pivotConfig.withMotionProfile(velLimit.getRotations(), accelLimit.getRotations()); @@ -138,13 +130,14 @@ public void setMotionProfileConstraints(Rotation2d velLimit, Rotation2d accelLim @Override public void setPivotState(PivotState pivotState) { super.setPivotState(pivotState); + if (getPivotState() == PivotState.STOW) { - SmartDashboard.putString("Intake/Profile Constraints", "S"); setMotionProfileConstraints(Settings.Intake.PIVOT_MAX_VEL_STOW, Settings.Intake.PIVOT_MAX_ACCEL_STOW); } else if (getPivotState() == PivotState.DEPLOY) { - SmartDashboard.putString("Intake/Profile Constraints", "D"); setMotionProfileConstraints(Settings.Intake.PIVOT_MAX_VEL_DEPLOY, Settings.Intake.PIVOT_MAX_ACCEL_DEPLOY); } + + SmartDashboard.putString("Intake/Profile Constraints", getPivotState().name()); } @Override @@ -163,7 +156,7 @@ public void periodic() { if (EnabledSubsystems.INTAKE.get()) { - if (getPivotState() == PivotState.ANALOG) { // comment out the setControl line if it breaks + if (getPivotState() == PivotState.ANALOG) { //TODO: comment out the setControl line if it breaks pivot.setControl(pivotController.withPosition(driverInputToAngle().getRotations())); } else if (getPivotState() == PivotState.DEBUG) { @@ -218,7 +211,6 @@ else if (pivotVoltageOverride.isPresent()) { SmartDashboard.putNumber("Intake/Pivot Angle Error (deg)", Math.abs(getPivotState().getTargetAngle().getDegrees() - getPivotAngle().getDegrees())); SmartDashboard.putNumber("Intake/Pivot Closed Loop Error (deg)", pivot.getClosedLoopError().getValueAsDouble() * 360.0); - // ROLLERS SmartDashboard.putNumber("Intake/Roller Leader Voltage (volts)", rollerLeader.getMotorVoltage().getValueAsDouble()); 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 9e726dd7..74705366 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -38,30 +38,28 @@ public class SpindexerImpl extends Spindexer { public SpindexerImpl() { spindexerLeadConfig = new Motors.TalonFXConfig() .withInvertedValue(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) .withSupplyCurrentLimitAmps(60) .withStatorCurrentLimitEnabled(false) + .withRampRate(0.25) .withPIDConstants(Gains.Spindexer.kP, Gains.Spindexer.kI, Gains.Spindexer.kD, 0) .withFFConstants(Gains.Spindexer.kS, Gains.Spindexer.kV, Gains.Spindexer.kA, 0) - .withNeutralMode(NeutralModeValue.Brake) - .withRampRate(0.25) - .withSensorToMechanismRatio(Settings.Spindexer.Constants.GEAR_RATIO); spindexerFollowerConfig = new Motors.TalonFXConfig() .withInvertedValue(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) .withSupplyCurrentLimitAmps(60) .withStatorCurrentLimitEnabled(false) + .withRampRate(0.25) .withFFConstants(Gains.Spindexer.kS, Gains.Spindexer.kV, Gains.Spindexer.kA, 0) .withPIDConstants(Gains.Spindexer.kP, Gains.Spindexer.kI, Gains.Spindexer.kD, 0) - .withNeutralMode(NeutralModeValue.Brake) - .withRampRate(0.25) - .withSensorToMechanismRatio(Settings.Spindexer.Constants.GEAR_RATIO); leadMotor = new TalonFX(Ports.Spindexer.SPINDEXER_LEAD_MOTOR, Ports.CANIVORE); @@ -70,24 +68,23 @@ public SpindexerImpl() { spindexerLeadConfig.configure(leadMotor); spindexerFollowerConfig.configure(followerMotor); - controller = new VelocityVoltage(getTargetRPM()) - .withEnableFOC(true); - + controller = new VelocityVoltage(getTargetRPM()).withEnableFOC(true); follower = new Follower(Ports.Spindexer.SPINDEXER_LEAD_MOTOR, MotorAlignmentValue.Aligned); voltageOverride = Optional.empty(); } - public double getCurrentLeadMotorRPM() { + private double getCurrentLeadMotorRPM() { return leadMotor.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE * Settings.Spindexer.Constants.GEAR_RATIO; } - public double getCurrentFollowerMotorRPM() { + private double getCurrentFollowerMotorRPM() { return followerMotor.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE * Settings.Spindexer.Constants.GEAR_RATIO; } - public boolean atTolerance() { - return Math.abs(getCurrentLeadMotorRPM() - getTargetRPM()) <= Settings.Spindexer.RPM_TOLERANCE; + private boolean atTolerance() { + double error = getCurrentLeadMotorRPM() - getTargetRPM(); + return Math.abs(error) <= Settings.Spindexer.RPM_TOLERANCE; } @Override @@ -101,6 +98,9 @@ public void periodic() { leadMotor.setControl(controller.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE)); } followerMotor.setControl(follower); + } else { + leadMotor.stopMotor(); + followerMotor.stopMotor(); } if (Settings.DEBUG_MODE) { @@ -109,11 +109,10 @@ public void periodic() { SmartDashboard.putBoolean("Spindexer/At Tolerance", atTolerance()); - SmartDashboard.putNumber("Spindexer/Lead Current (amps)", leadMotor.getStatorCurrent().getValueAsDouble()); SmartDashboard.putNumber("Spindexer/Lead Voltage", leadMotor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Spindexer/Lead Stator Current", leadMotor.getStatorCurrent().getValueAsDouble()); SmartDashboard.putNumber("Spindexer/Lead Supply Current", leadMotor.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("Spindexer/Follower Current (amps)", followerMotor.getStatorCurrent().getValueAsDouble()); SmartDashboard.putNumber("Spindexer/Follower Voltage", followerMotor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Spindexer/Follower Supply Current", followerMotor.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Spindexer/Follower Stator Current", followerMotor.getStatorCurrent().getValueAsDouble()); 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 bedf2a75..18dc7f64 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java @@ -35,7 +35,7 @@ public static Turret getInstance() { } public Turret() { - driverInput = new Vector2D(0, 0); + driverInput = Vector2D.kOrigin; state = TurretState.IDLE; } @@ -73,7 +73,7 @@ public Rotation2d driverInputToAngle() { } public boolean atTargetAngle() { - return Math.abs(getAngle().minus(getTargetAngle()).getDegrees()) < Settings.Turret.TOLERANCE_DEG; + return Math.abs(getAngle().minus(getTargetAngle()).getDegrees()) < Settings.Turret.TOLERANCE; } public Rotation2d getScoringAngle() { 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 ffff25bb..f30364cb 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java @@ -42,9 +42,12 @@ public class TurretImpl extends Turret { public TurretImpl() { turretConfig = new Motors.TalonFXConfig() .withInvertedValue(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) .withSupplyCurrentLimitAmps(80) .withStatorCurrentLimitEnabled(false) + .withRampRate(0.25) + .withVoltageLimits(6, -6) //TODO: VERIFY MAX VOLTAGE .withPIDConstants(Gains.Turret.slot0.kP, Gains.Turret.slot0.kI, Gains.Turret.slot0.kD, 0) .withFFConstants(Gains.Turret.slot0.kS, Gains.Turret.slot0.kV, Gains.Turret.slot0.kA, 0) @@ -54,17 +57,12 @@ public TurretImpl() { .withFFConstants(Gains.Turret.slot1.kS, Gains.Turret.slot1.kV, Gains.Turret.slot1.kA, 1) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 1) - .withNeutralMode(NeutralModeValue.Brake) - .withRampRate(0.25) - .withSensorToMechanismRatio(Settings.Turret.Constants.GEAR_RATIO_MOTOR_TO_MECH) .withSoftLimits( false, false, Settings.Turret.Constants.SoftwareLimit.FORWARD_MAX_ROTATIONS, - Settings.Turret.Constants.SoftwareLimit.BACKWARDS_MAX_ROTATIONS) - - .withVoltageLimits(6, -6); //TODO: VERIFY MAX VOLTAGE + Settings.Turret.Constants.SoftwareLimit.BACKWARDS_MAX_ROTATIONS); encoder17tConfig = new Motors.CANCoderConfig() .withSensorDirection(SensorDirectionValue.Clockwise_Positive) @@ -131,7 +129,8 @@ public Rotation2d getAngle() { @Override public boolean atTargetAngle() { - return Math.abs(getAngle().minus(getTargetAngle()).getDegrees() + 180.0) < Settings.Turret.TOLERANCE_DEG; + double error = getAngle().minus(getTargetAngle()).getRotations() + 0.5; + return Math.abs(error) < Settings.Turret.TOLERANCE.getRotations(); } private double getDelta(double target, double current) { @@ -169,11 +168,13 @@ public void periodic() { } if (Settings.DEBUG_MODE) { + SmartDashboard.putNumber("Turret/Relative Encoder Position (Rot)", motor.getPosition().getValueAsDouble() * 360.0); SmartDashboard.putNumber("Turret/Closed Loop Error (deg)", motor.getClosedLoopError().getValueAsDouble() * 360.0); + SmartDashboard.putNumber("Turret/Encoder18t Abs Position (Rot)", encoder18t.getAbsolutePosition().getValueAsDouble()); SmartDashboard.putNumber("Turret/Encoder17t Abs Position (Rot)", encoder17t.getAbsolutePosition().getValueAsDouble()); SmartDashboard.putNumber("Turret/Vector Space Position (Deg)", getVectorSpaceAngle().getDegrees()); - SmartDashboard.putNumber("Turret/Relative Encoder Position (Rot)", motor.getPosition().getValueAsDouble() * 360.0); + SmartDashboard.putNumber("Turret/Voltage", motor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Turret/Stator Current", motor.getStatorCurrent().getValueAsDouble()); SmartDashboard.putNumber("Turret/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); @@ -181,6 +182,10 @@ public void periodic() { SmartDashboard.putNumber("Current Draws/Turret (amps)", motor.getSupplyCurrent().getValueAsDouble()); } } + + private void setVoltageOverride(Optional volts) { + this.voltageOverride = volts; + } @Override public SysIdRoutine getSysIdRoutine() { @@ -194,8 +199,4 @@ public SysIdRoutine getSysIdRoutine() { () -> this.motor.getMotorVoltage().getValueAsDouble(), getInstance()); } - - private void setVoltageOverride(Optional volts) { - this.voltageOverride = volts; - } } \ No newline at end of file From aa2c992d5b96a2c92f7e8db224ed11c05c466fee Mon Sep 17 00:00:00 2001 From: Daniel M Date: Wed, 4 Mar 2026 01:25:00 -0500 Subject: [PATCH 058/150] clean: remove pivot analog/debug states --- .../robot/commands/intake/IntakeAnalog.java | 32 ------------------- .../robot/commands/intake/IntakeBangBang.java | 9 ------ .../robot/subsystems/intake/Intake.java | 28 +--------------- .../robot/subsystems/intake/IntakeImpl.java | 29 ++++------------- 4 files changed, 8 insertions(+), 90 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeAnalog.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeBangBang.java diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAnalog.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAnalog.java deleted file mode 100644 index 2fe1f24b..00000000 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAnalog.java +++ /dev/null @@ -1,32 +0,0 @@ -/************************ 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.commands.intake; - -import com.stuypulse.stuylib.input.Gamepad; -import com.stuypulse.robot.subsystems.intake.Intake; -import com.stuypulse.robot.subsystems.intake.Intake.PivotState; - -import edu.wpi.first.wpilibj2.command.Command; - -public class IntakeAnalog extends Command { - private Gamepad gamepad; - private Intake intake; - public IntakeAnalog(Gamepad gamepad){ - intake = Intake.getInstance(); - this.gamepad = gamepad; - } - - @Override - public void initialize() { - super.initialize(); - intake.setPivotState(PivotState.ANALOG); - } - @Override - public void execute() { - super.execute(); - intake.setDriverInput(gamepad); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeBangBang.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeBangBang.java deleted file mode 100644 index 4ab99901..00000000 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeBangBang.java +++ /dev/null @@ -1,9 +0,0 @@ -package com.stuypulse.robot.commands.intake; - -import com.stuypulse.robot.subsystems.intake.Intake.PivotState; - -public class IntakeBangBang extends IntakeSetState { - public IntakeBangBang() { - super(PivotState.BANGBANG); - } -} 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 b04936bd..20db72de 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -6,8 +6,6 @@ package com.stuypulse.robot.subsystems.intake; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.stuylib.input.Gamepad; -import com.stuypulse.stuylib.math.Vector2D; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -22,8 +20,6 @@ public abstract class Intake extends SubsystemBase { private PivotState pivotState; private RollerState rollerState; - private Vector2D driverInput; - static { instance = new IntakeImpl(); } @@ -35,16 +31,11 @@ public static Intake getInstance() { protected Intake() { this.pivotState = PivotState.STOW; this.rollerState = RollerState.STOP; - - driverInput = new Vector2D(0, 0); } public enum PivotState { DEPLOY(Settings.Intake.PIVOT_DEPLOY_ANGLE), - STOW(Settings.Intake.PIVOT_STOW_ANGLE), - BANGBANG(Settings.Intake.PIVOT_DEPLOY_ANGLE), - ANALOG(Settings.Intake.PIVOT_STOW_ANGLE), - DEBUG(Settings.Intake.PIVOT_STOW_ANGLE); //filler argument + STOW(Settings.Intake.PIVOT_STOW_ANGLE); private final Rotation2d targetAngle; @@ -90,23 +81,6 @@ public void setRollerState(RollerState state) { this.rollerState = state; } - public void setDriverInput(Gamepad gamepad) { - this.driverInput = gamepad.getLeftStick(); - } - - public Rotation2d driverInputToAngle() { - SmartDashboard.putNumber("Intake/Driver Input", driverInput.x); - - double minAngle = Settings.Intake.PIVOT_MIN_ANGLE.getDegrees(); - double maxAngle = Settings.Intake.PIVOT_MAX_ANGLE.getDegrees(); - - // Maps driver input [-1, 1] to pivot angle [MIN, MAX]. - // A driver input of 0 (i.e. centered joystick) will drive the pivot to the midpoint of the range of motion. - double scaledAngle = minAngle + (driverInput.x + 1.0) * (maxAngle - minAngle) / 2.0; - - return Rotation2d.fromDegrees(scaledAngle); - } - public abstract boolean pivotAtTolerance(); public abstract Rotation2d getPivotAngle(); 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 b76dcb2f..8f716c50 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -155,36 +155,21 @@ public void periodic() { super.periodic(); if (EnabledSubsystems.INTAKE.get()) { - - if (getPivotState() == PivotState.ANALOG) { //TODO: comment out the setControl line if it breaks - pivot.setControl(pivotController.withPosition(driverInputToAngle().getRotations())); - } - else if (getPivotState() == PivotState.DEBUG) { - pivot.setControl(new VoltageOut(Settings.Intake.debugVoltage)); //TODO: check if we want DEBUG enum/state - if (pivotStalling.get()) { //TODO: update what we want the intake to do if - pivot.setPosition(0); - } - } - - else if (pivotVoltageOverride.isPresent()) { + if (pivotVoltageOverride.isPresent()) { pivot.setVoltage(pivotVoltageOverride.get()); - } - - else { + } else { + // PIVOT if (getPivotState() == PivotState.DEPLOY && getPivotAngle().getDegrees() <= Settings.Intake.ARBITRARY_VOLTAGE_THRESHOLD.getDegrees()) { pivot.setControl(new VoltageOut(-Settings.Intake.PUSHDOWN_VOLTAGE)); // applying 3 volts - } - - else { + } else { pivot.setControl(new PositionVoltage(getPivotState().getTargetAngle().getRotations())); } + // ROLLERS if (getPivotAngle().getDegrees() <= Settings.Intake.THRESHHOLD_TO_START_ROLLERS.getDegrees()) { rollerLeader.setControl(rollerController.withOutput(getRollerState().getTargetDutyCycle())); rollerFollower.setControl(follower); - } - - else { + } else { rollerLeader.stopMotor(); rollerFollower.stopMotor(); } @@ -199,7 +184,7 @@ else if (pivotVoltageOverride.isPresent()) { // PIVOT SmartDashboard.putBoolean("Intake/Voltage Override", pivotVoltageOverride.isPresent()); - SmartDashboard.putNumber("Intake/Debug", getPivotState().getTargetAngle().getRotations()); + SmartDashboard.putNumber("Intake/Pivot Target Angle", getPivotState().getTargetAngle().getRotations()); SmartDashboard.putNumber("Intake/Pivot Voltage (volts)", pivot.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Intake/Pivot Supply Current (amps)", pivot.getSupplyCurrent().getValueAsDouble()); From 5867e091db24820f3d9d213af6471071f93a3ad2 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Wed, 4 Mar 2026 10:08:42 -0500 Subject: [PATCH 059/150] Add: rotation sysid - Yunus --- .../java/com/stuypulse/robot/RobotContainer.java | 16 ++++++++++++++-- .../swerve/CommandSwerveDrivetrain.java | 8 ++++++++ 2 files changed, 22 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 9e00bd0e..863f5484 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -22,6 +22,7 @@ import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.hoodedshooter.HoodAnalog; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; +import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterKB; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterShoot; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterStow; import com.stuypulse.robot.commands.hoodedshooter.ZeroHoodAtLowerHardstop; @@ -161,6 +162,11 @@ private void configureButtonBindings() { .onTrue(new ResetLimelightIMU()) .onFalse(new SetIMUMode(0)); + driver.getLeftButton() + .whileTrue(new HoodedShooterKB() + .andThen(new WaitUntilCommand(() -> hoodedShooter.bothAtTolerance())) + .andThen(new HoodedShooterShoot())); + // // Ferry Routine using Interpolation Settings // driver.getBottomButton() // .onTrue(new HoodedShooterFerry() @@ -331,7 +337,7 @@ public void configureAutons() { BOTTOM_TWO_CYCLE.register(autonChooser); AutonConfig TOP_TWO_CYCLE_POACH = new AutonConfig("Top Two Cycle (Poach)", TopTwoCyclePoach::new, - "Top Trench To NZ (P)", "Top NZ To Score (P)", "Top Score To NZ", "Top NZ To Tower Left"); + "Top Trench To NZ (P)", "Top NZ To Score (P)", "Top Score To NZ", "Top NZ To Tower Left"); TOP_TWO_CYCLE_POACH.register(autonChooser); AutonConfig BOTTOM_TWO_CYCLE_POACH = new AutonConfig("Bottom Two Cycle (Poach)", BottomTwoCyclePoach::new, @@ -347,7 +353,13 @@ public void configureSysids() { autonChooser.addOption("SysID Module Translation Dynamic Forwards", swerve.sysIdDynamic(Direction.kForward)); autonChooser.addOption("SysID Module Translation Dynamic Backwards", swerve.sysIdDynamic(Direction.kReverse)); autonChooser.addOption("SysID Module Translation Quasi Forwards", swerve.sysIdQuasistatic(Direction.kForward)); - autonChooser.addOption("SysID Module Translation Quasi Backwards", swerve.sysIdQuasistatic(Direction.kReverse)); + autonChooser.addOption("SysID Module Translation Quasi Backwards", swerve.sysIdQuasistatic(Direction.kReverse)); + + autonChooser.addOption("SysID Rotation Translation Dynamic Forwards", swerve.sysidRotationDynamic(Direction.kForward)); + autonChooser.addOption("SysID Rotation Translation Dynamic Backwards", swerve.sysidRotationDynamic(Direction.kReverse)); + autonChooser.addOption("SysID Rotation Translation Quasi Forwards", swerve.sysidRotationQuasiStatic(Direction.kForward)); + autonChooser.addOption("SysID Rotation Translation Quasi Backwards", swerve.sysidRotationQuasiStatic(Direction.kReverse)); + // autonChooser.addOption("SysID Turret Dynamic Forwards", turret.getSysIdRoutine().dynamic(Direction.kForward)); // autonChooser.addOption("SysID Turret Dynamic Reverse", turret.getSysIdRoutine().dynamic(Direction.kReverse)); 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 ff452b37..568fa8fc 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -287,6 +287,14 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { return m_sysIdRoutineToApply.dynamic(direction); } + public Command sysidRotationDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineRotation.dynamic(direction); + } + + public Command sysidRotationQuasiStatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineRotation.quasistatic(direction); + } + private void startSimThread() { m_lastSimTime = Utils.getCurrentTimeSeconds(); From 539c704ef9de4c1273c9c9223f9422a4cf846085 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Wed, 4 Mar 2026 12:20:40 -0500 Subject: [PATCH 060/150] FIX: spindexer stops once within tolerance, upper soft limit to 39 degrees --- .../java/com/stuypulse/robot/RobotContainer.java | 8 ++++++-- .../java/com/stuypulse/robot/constants/Cameras.java | 7 ++++--- .../com/stuypulse/robot/constants/Settings.java | 2 +- .../robot/subsystems/spindexer/SpindexerImpl.java | 13 +++++++++---- 4 files changed, 20 insertions(+), 10 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 863f5484..8e890bf7 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -135,9 +135,9 @@ private void configureDefaultCommands() { /***************/ private void configureButtonBindings() { - // Scoring Routine using Interpolation Settings + // Scoring Routine driver.getTopButton() - .whileTrue(new HoodedShooterInterpolation() + .whileTrue(new HoodedShooterShoot() .alongWith(new SwerveDriveAlignTurretToHub()) // .alongWith(new TurretShoot()) .andThen(new WaitUntilCommand(() -> hoodedShooter.bothAtTolerance() )) @@ -156,6 +156,10 @@ private void configureButtonBindings() { driver.getLeftTriggerButton() .onTrue(new IntakeStow()); + driver.getLeftButton() + .whileTrue(new SpindexerRun()) + .onFalse(new SpindexerStop()); + // Reset Heading driver.getDPadUp() .onTrue(new SwerveResetHeading()) diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 069de364..4f71fbac 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -25,10 +25,11 @@ public interface Cameras { // new Pose3d(Units.inchesToMeters(2.490), Units.inchesToMeters(14.8620), Units.inchesToMeters(5.920157), // new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15.0), Units.degreesToRadians(251.))), // RobotContainer.EnabledSubsystems.LIMELIGHT), - // new Camera("limelight-module", - // new Pose3d(Units.inchesToMeters(2.490), Units.inchesToMeters(14.8620), Units.inchesToMeters(5.920157), - // new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15.0), Units.degreesToRadians(251.))), + // new Camera("limelight-module", + // new Pose3d(Units.inchesToMeters(-12.963), Units.inchesToMeters(-11.0845), Units.inchesToMeters(7.743595), + // new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(31), Units.degreesToRadians(180))), // RobotContainer.EnabledSubsystems.LIMELIGHT) + //THIS ROTATION IS NOT EXACTLY 180 DEGREES... }; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 6025e46a..e8fb50a5 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -199,7 +199,7 @@ public interface Hood { public final Rotation2d ENCODER_OFFSET = Rotation2d.fromRotations(-0.043); - public final Rotation2d FORWARD_SOFT_LIMIT = Rotation2d.fromDegrees(37.0); + public final Rotation2d FORWARD_SOFT_LIMIT = Rotation2d.fromDegrees(39.0); public final Rotation2d REVERSE_SOFT_LIMIT = Rotation2d.fromDegrees(10.0); public final double STALL_CURRENT_LIMIT = 20.0; 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 74705366..1922d2e3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -40,7 +40,7 @@ public SpindexerImpl() { .withInvertedValue(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Brake) - .withSupplyCurrentLimitAmps(60) + .withSupplyCurrentLimitAmps(80) .withStatorCurrentLimitEnabled(false) .withRampRate(0.25) @@ -53,7 +53,7 @@ public SpindexerImpl() { .withInvertedValue(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Brake) - .withSupplyCurrentLimitAmps(60) + .withSupplyCurrentLimitAmps(80) .withStatorCurrentLimitEnabled(false) .withRampRate(0.25) @@ -95,9 +95,14 @@ public void periodic() { if (voltageOverride.isPresent()) { leadMotor.setVoltage(voltageOverride.get()); } else { - leadMotor.setControl(controller.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE)); + if (atTolerance() && getState() == SpindexerState.STOP) { + leadMotor.stopMotor(); + followerMotor.stopMotor(); + } else { + leadMotor.setControl(controller.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE)); + followerMotor.setControl(follower); + } } - followerMotor.setControl(follower); } else { leadMotor.stopMotor(); followerMotor.stopMotor(); From 2744f8fc6a1657f894e2c7ec7ffd42f1ce68b2a4 Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Wed, 4 Mar 2026 13:23:34 -0500 Subject: [PATCH 061/150] FEAT: hood automatically retracts to STOW when going under trench, regardless of state --- .../com/stuypulse/robot/RobotContainer.java | 11 ++--- .../com/stuypulse/robot/constants/Field.java | 1 + .../stuypulse/robot/constants/Settings.java | 43 ++++++++++--------- .../hoodedshooter/HoodedShooter.java | 4 ++ .../subsystems/hoodedshooter/hood/Hood.java | 32 +++++++++++--- .../hoodedshooter/hood/HoodImpl.java | 12 ++++-- .../hoodedshooter/shooter/Shooter.java | 10 ++--- 7 files changed, 70 insertions(+), 43 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 8e890bf7..a809fdb5 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -137,7 +137,7 @@ private void configureDefaultCommands() { private void configureButtonBindings() { // Scoring Routine driver.getTopButton() - .whileTrue(new HoodedShooterShoot() + .whileTrue(new HoodedShooterShoot().onlyIf(() -> !hoodedShooter.isHoodUnderTrench()) .alongWith(new SwerveDriveAlignTurretToHub()) // .alongWith(new TurretShoot()) .andThen(new WaitUntilCommand(() -> hoodedShooter.bothAtTolerance() )) @@ -157,8 +157,8 @@ private void configureButtonBindings() { .onTrue(new IntakeStow()); driver.getLeftButton() - .whileTrue(new SpindexerRun()) - .onFalse(new SpindexerStop()); + .whileTrue(new HoodedShooterShoot().onlyIf(() -> !hoodedShooter.isHoodUnderTrench())) + .onFalse(new HoodedShooterStow()); // Reset Heading driver.getDPadUp() @@ -166,11 +166,6 @@ private void configureButtonBindings() { .onTrue(new ResetLimelightIMU()) .onFalse(new SetIMUMode(0)); - driver.getLeftButton() - .whileTrue(new HoodedShooterKB() - .andThen(new WaitUntilCommand(() -> hoodedShooter.bothAtTolerance())) - .andThen(new HoodedShooterShoot())); - // // Ferry Routine using Interpolation Settings // driver.getBottomButton() // .onTrue(new HoodedShooterFerry() diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 1c956ac3..7804a00d 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -31,6 +31,7 @@ public interface Field { double LENGTH = Units.inchesToMeters(651.200); public static final double trenchXTolerance = Units.inchesToMeters(50); + public static final double trenchHoodTolerance = Units.inchesToMeters(15); // Alliance relative hub center coordinates public static final Pose2d hubCenter = new Pose2d(Units.inchesToMeters(182.11), WIDTH / 2.0, new Rotation2d()); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index e8fb50a5..938146d2 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -126,25 +126,6 @@ public interface HoodedShooter { public final double SHOOTER_TOLERANCE_RPM = 100.0; public final Rotation2d HOOD_TOLERANCE = Rotation2d.fromDegrees(0.5); - public interface RPMs { - public final SmartNumber SHOOT_RPM = new SmartNumber("HoodedShooter/Shoot State Target RPM", 3500.0); - public final SmartNumber FERRY_RPM = new SmartNumber("HoodedShooter/Ferry State Target RPM", 2000.0); - public final double REVERSE = 0.0; - public final double KB_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); - - public final Rotation2d MIN_ANGLE= Rotation2d.fromDegrees(7.0); - public final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(40.0); - public final Rotation2d KB_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 { public final double[][] distanceAngleInterpolationValues = { {1.30, Units.degreesToRadians(16.5)}, @@ -178,6 +159,15 @@ public interface FerryRPMInterpolation { public interface Shooter { public final double GEAR_RATIO = 1.0; public final double FLYWHEEL_RADIUS = Units.inchesToMeters(3.965 / 2); + + public interface RPMs { + public final SmartNumber SHOOT_RPM = new SmartNumber("HoodedShooter/Shoot State Target RPM", 3500.0); + public final SmartNumber FERRY_RPM = new SmartNumber("HoodedShooter/Ferry State Target RPM", 2000.0); + public final double REVERSE = 0.0; + public final double KB_RPM = 0.0; + public final double LEFT_CORNER_RPM = 0.0; + public final double RIGHT_CORNER_RPM = 0.0; + } } public interface Hood { @@ -200,10 +190,23 @@ public interface Hood { public final Rotation2d ENCODER_OFFSET = Rotation2d.fromRotations(-0.043); public final Rotation2d FORWARD_SOFT_LIMIT = Rotation2d.fromDegrees(39.0); - public final Rotation2d REVERSE_SOFT_LIMIT = Rotation2d.fromDegrees(10.0); + public final Rotation2d REVERSE_SOFT_LIMIT = Rotation2d.fromDegrees(8.0); public final double STALL_CURRENT_LIMIT = 20.0; public final double STALL_DEBOUNCE = 0.5; + + 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(40.0); + + public final Rotation2d UNDER_TRENCH_ANGLE = Rotation2d.fromDegrees(11.0); + public final Rotation2d KB_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 ShootOnTheFly { 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 fe4a0609..f2d1752b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java @@ -84,6 +84,10 @@ public boolean bothAtTolerance() { return isShooterAtTolerance() && isHoodAtTolerance(); } + public boolean isHoodUnderTrench() { + return hood.isHoodUnderTrench(); + } + public boolean isShooterAtTolerance() { return shooter.atTolerance(); } 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 1d49d4ba..66bf7b69 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 @@ -5,10 +5,13 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.hoodedshooter.hood; +import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.hoodedshooter.HoodAngleCalculator; import com.stuypulse.stuylib.input.Gamepad; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -55,12 +58,12 @@ public void setState(HoodState state){ public Rotation2d getTargetAngle() { return switch(state) { - case STOW -> Settings.HoodedShooter.Angles.MIN_ANGLE; + case STOW -> Settings.HoodedShooter.Hood.Angles.MIN_ANGLE; case FERRY -> Rotation2d.fromDegrees(30); - case SHOOT -> Rotation2d.fromDegrees(Settings.HoodedShooter.Angles.SHOOT_ANGLE.get()); - case KB -> Settings.HoodedShooter.Angles.KB_ANGLE; - case LEFT_CORNER -> Settings.HoodedShooter.Angles.LEFT_CORNER_ANGLE; - case RIGHT_CORNER -> Settings.HoodedShooter.Angles.RIGHT_CORNER_ANGLE; + case SHOOT -> Rotation2d.fromDegrees(Settings.HoodedShooter.Hood.Angles.SHOOT_ANGLE.get()); + case KB -> Settings.HoodedShooter.Hood.Angles.KB_ANGLE; + case LEFT_CORNER -> Settings.HoodedShooter.Hood.Angles.LEFT_CORNER_ANGLE; + case RIGHT_CORNER -> Settings.HoodedShooter.Hood.Angles.RIGHT_CORNER_ANGLE; case INTERPOLATION -> HoodAngleCalculator.interpolateHoodAngle().get(); case ANALOG -> hoodAnalogToOutput(); case IDLE -> getHoodAngle(); @@ -75,8 +78,8 @@ public boolean atTolerance() { public abstract Rotation2d getHoodAngle(); public void hoodAnalogToInput(Gamepad gamepad) { - double hoodMin = Settings.HoodedShooter.Angles.MIN_ANGLE.getDegrees(); - double hoodMax = Settings.HoodedShooter.Angles.MAX_ANGLE.getDegrees(); + double hoodMin = Settings.HoodedShooter.Hood.Angles.MIN_ANGLE.getDegrees(); + double hoodMax = Settings.HoodedShooter.Hood.Angles.MAX_ANGLE.getDegrees(); this.driverInput = Rotation2d.fromDegrees(hoodMin + (gamepad.getLeftX() + 1.0) * ((hoodMax - hoodMin) / 2)); } @@ -85,7 +88,22 @@ public Rotation2d hoodAnalogToOutput() { return this.driverInput; } + public boolean isHoodUnderTrench() { + Pose2d pose = CommandSwerveDrivetrain.getInstance().getTurretPose(); + 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 isCloseToAllianceSideTrenchX = Math.abs(pose.getX() - Field.NearRightTrench.rightEdge.getX()) < Field.trenchHoodTolerance; + + boolean isCloseToNeutralSideTrenchX = Math.abs(pose.getX() - Field.FarRightTrench.rightEdge.getX()) < Field.trenchHoodTolerance; + + boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) && (isCloseToAllianceSideTrenchX || isCloseToNeutralSideTrenchX); + + return isUnderTrench; + } + public abstract boolean isStalling(); public abstract SysIdRoutine getHoodSysIdRoutine(); public abstract void zeroHoodEncoderAtLowerHardstop(); 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 5ac0bfeb..68448b53 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 @@ -10,6 +10,7 @@ import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; import com.stuypulse.robot.util.SysId; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; @@ -95,7 +96,7 @@ public void zeroHoodEncoderAtLowerHardstop() { double currentOffset = hoodEncoderConfig.getConfiguration().MagnetSensor.MagnetOffset; double positionWithCurrentOffset = hoodEncoder.getPosition().getValueAsDouble(); - double newOffset = -((positionWithCurrentOffset - currentOffset) - Settings.HoodedShooter.Angles.MIN_ANGLE.getRotations()); + double newOffset = -((positionWithCurrentOffset - currentOffset) - Settings.HoodedShooter.Hood.Angles.MIN_ANGLE.getRotations()); hoodEncoderConfig.withMagnetOffset(newOffset); @@ -118,7 +119,7 @@ public void seedHood() { } private double getAbsoluteHoodAngleDeg() { - return Settings.HoodedShooter.Angles.MIN_ANGLE.getDegrees() + hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.HoodedShooter.Hood.ENCODER_TO_MECH; + return Settings.HoodedShooter.Hood.Angles.MIN_ANGLE.getDegrees() + hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.HoodedShooter.Hood.ENCODER_TO_MECH; } @Override @@ -134,7 +135,12 @@ public void periodic() { if (voltageOverride.isPresent()) { hoodMotor.setVoltage(voltageOverride.get()); } else { - hoodMotor.setControl(controller.withPosition(getTargetAngle().getRotations())); + if (!isHoodUnderTrench()) { + hoodMotor.setControl(controller.withPosition(getTargetAngle().getRotations())); + } else { + setState(HoodState.STOW); + hoodMotor.setControl(controller.withPosition(Settings.HoodedShooter.Hood.Angles.UNDER_TRENCH_ANGLE.getRotations())); + } } } else { hoodMotor.stopMotor(); 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 4a1bbdd0..a107970f 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 @@ -58,16 +58,16 @@ public double getTargetRPM() { case STOP -> 0; case SHOOT -> getShootRPM(); case FERRY -> HoodAngleCalculator.interpolateFerryingRPM().get(); - case REVERSE -> Settings.HoodedShooter.RPMs.REVERSE; - case KB -> Settings.HoodedShooter.RPMs.KB_RPM; - case LEFT_CORNER -> Settings.HoodedShooter.RPMs.LEFT_CORNER_RPM; - case RIGHT_CORNER -> Settings.HoodedShooter.RPMs.RIGHT_CORNER_RPM; + case REVERSE -> Settings.HoodedShooter.Shooter.RPMs.REVERSE; + case KB -> Settings.HoodedShooter.Shooter.RPMs.KB_RPM; + case LEFT_CORNER -> Settings.HoodedShooter.Shooter.RPMs.LEFT_CORNER_RPM; + case RIGHT_CORNER -> Settings.HoodedShooter.Shooter.RPMs.RIGHT_CORNER_RPM; case INTERPOLATION -> HoodAngleCalculator.interpolateShooterRPM().get(); }; } public double getShootRPM() { - return Settings.HoodedShooter.RPMs.SHOOT_RPM.get(); // Adjustable RPM on Glass + return Settings.HoodedShooter.Shooter.RPMs.SHOOT_RPM.get(); // Adjustable RPM on Glass } public boolean atTolerance() { From 2fba0efefd04788981e0508f72f58373e2e15cf1 Mon Sep 17 00:00:00 2001 From: Lucas Date: Wed, 4 Mar 2026 17:27:16 -0500 Subject: [PATCH 062/150] fix: add right button, log isUnderTrench, simplify underTrench logic for Hood --- simgui-ds.json | 3 +-- simgui.json | 2 ++ src/main/java/com/stuypulse/robot/RobotContainer.java | 8 +++++++- .../robot/subsystems/hoodedshooter/hood/Hood.java | 2 ++ .../robot/subsystems/hoodedshooter/hood/HoodImpl.java | 6 ++---- 5 files changed, 14 insertions(+), 7 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index c4b7efd3..69b1a3cb 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -91,8 +91,7 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "Keyboard0" } ] } diff --git a/simgui.json b/simgui.json index 1b956d58..849c61c5 100644 --- a/simgui.json +++ b/simgui.json @@ -18,6 +18,8 @@ "/SmartDashboard/Robot/Reset Pivot": "Command", "/SmartDashboard/Robot/Zero Encoders": "Command", "/SmartDashboard/Robot/Zero Hood Encoder": "Command", + "/SmartDashboard/Robot/Zero Pivot Encoder at Lower Limit (Deployed)": "Command", + "/SmartDashboard/Robot/Zero Pivot Encoder at Upper Limit (Stowed)": "Command", "/SmartDashboard/Robot/Zero Turret Encoders": "Command", "/SmartDashboard/Visualizers/ClimberHopper": "Mechanism2d", "/SmartDashboard/Visualizers/Turret": "Mechanism2d" diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index a809fdb5..2969830d 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -20,7 +20,7 @@ import com.stuypulse.robot.commands.climberhopper.ClimberOverrideUp; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.hoodedshooter.HoodAnalog; +import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterFerry; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterKB; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterShoot; @@ -160,6 +160,12 @@ private void configureButtonBindings() { .whileTrue(new HoodedShooterShoot().onlyIf(() -> !hoodedShooter.isHoodUnderTrench())) .onFalse(new HoodedShooterStow()); + + driver.getRightButton() + .whileTrue(new HoodedShooterFerry().onlyIf( + () -> CommandSwerveDrivetrain.getInstance().getPose().getX() > Field.getHubPose().getX())) + .onFalse(new HoodedShooterStow()); + // Reset Heading driver.getDPadUp() .onTrue(new SwerveResetHeading()) 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 66bf7b69..9214f49a 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 @@ -118,5 +118,7 @@ public void periodic() { SmartDashboard.putNumber("HoodedShooter/Hood/Current Angle", getHoodAngle().getDegrees()); //SmartDashboard.putNumber("HoodedShooter/Hood/Analog Target Angle", hoodAnalogToOutput().getDegrees()); + + SmartDashboard.putBoolean("HoodedShooter/Hood/Under Trench", isHoodUnderTrench()); } } \ No newline at end of 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 68448b53..d12485ce 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 @@ -135,12 +135,10 @@ public void periodic() { if (voltageOverride.isPresent()) { hoodMotor.setVoltage(voltageOverride.get()); } else { - if (!isHoodUnderTrench()) { - hoodMotor.setControl(controller.withPosition(getTargetAngle().getRotations())); - } else { + if (isHoodUnderTrench()) { setState(HoodState.STOW); - hoodMotor.setControl(controller.withPosition(Settings.HoodedShooter.Hood.Angles.UNDER_TRENCH_ANGLE.getRotations())); } + hoodMotor.setControl(controller.withPosition(getTargetAngle().getRotations())); } } else { hoodMotor.stopMotor(); From e9131e742ff2cf55d88f23b981aff1127ddf00e6 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Wed, 4 Mar 2026 17:29:17 -0500 Subject: [PATCH 063/150] feat: data points --- src/main/java/com/stuypulse/robot/constants/Settings.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index e8fb50a5..87bb6670 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -147,6 +147,7 @@ public interface Angles { public interface AngleInterpolation { public final double[][] distanceAngleInterpolationValues = { + {1.22, Units.degreesToRadians(20)}, //AGAINST THE HUB {1.30, Units.degreesToRadians(16.5)}, {1.43, Units.degreesToRadians(21.0)}, {2.15, Units.degreesToRadians(23.23)}, @@ -159,6 +160,7 @@ public interface AngleInterpolation { public interface RPMInterpolation { public final double[][] distanceRPMInterpolationValues = { + {1.22, 2850.0}, //AGAINST THE HUB {1.30, 3000.0}, {1.43, 3000.0}, {2.15, 3050.0}, From 9af29a458bd2ac2254f8383b37f27809844ee509 Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Wed, 4 Mar 2026 18:09:04 -0500 Subject: [PATCH 064/150] REFACTOR: Superstructure for Hood, Shooter, and Turret Co-authored-by: Lucas O Co-authored-by: Xuan Hao Cen --- .../com/stuypulse/robot/RobotContainer.java | 32 ++-- .../commands/hoodedshooter/HoodAnalog.java | 2 +- .../hoodedshooter/HoodedShooterSetState.java | 28 ---- .../commands/superstructure/HoodAnalog.java | 29 ++++ .../HoodedShooterFerry.java | 4 +- .../HoodedShooterInterpolation.java | 4 +- .../HoodedShooterKB.java | 4 +- .../HoodedShooterReverse.java | 4 +- .../HoodedShooterRightCorner.java | 4 +- .../HoodedShooterShoot.java} | 10 +- .../HoodedShooterStow.java | 4 +- .../SuperstructureLeftCorner.java | 14 ++ .../SuperstructureSetState.java | 28 ++++ .../SuperstructureShoot.java} | 10 +- .../ZeroHoodAtLowerHardstop.java | 2 +- .../commands/turret/TurretDefaultCommand.java | 4 +- .../robot/commands/turret/TurretFerry.java | 2 +- .../robot/commands/turret/TurretShoot.java | 2 +- .../{hoodedshooter => }/hood/Hood.java | 10 +- .../{hoodedshooter => }/hood/HoodImpl.java | 5 +- .../hoodedshooter/HoodedShooter.java | 134 --------------- .../hoodedshooter/HoodedShooterSim.java | 28 ---- .../{hoodedshooter => }/shooter/Shooter.java | 8 +- .../shooter/ShooterImpl.java | 4 +- .../shooter/ShooterSim.java | 4 +- .../superstructure/Superstructure.java | 155 ++++++++++++++++++ .../robot/subsystems/turret/Turret.java | 10 +- 27 files changed, 289 insertions(+), 256 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterSetState.java create mode 100644 src/main/java/com/stuypulse/robot/commands/superstructure/HoodAnalog.java rename src/main/java/com/stuypulse/robot/commands/{hoodedshooter => superstructure}/HoodedShooterFerry.java (84%) rename src/main/java/com/stuypulse/robot/commands/{hoodedshooter => superstructure}/HoodedShooterInterpolation.java (84%) rename src/main/java/com/stuypulse/robot/commands/{hoodedshooter => superstructure}/HoodedShooterKB.java (85%) rename src/main/java/com/stuypulse/robot/commands/{hoodedshooter => superstructure}/HoodedShooterReverse.java (84%) rename src/main/java/com/stuypulse/robot/commands/{hoodedshooter => superstructure}/HoodedShooterRightCorner.java (84%) rename src/main/java/com/stuypulse/robot/commands/{hoodedshooter/HoodedShooterLeftCorner.java => superstructure/HoodedShooterShoot.java} (54%) rename src/main/java/com/stuypulse/robot/commands/{hoodedshooter => superstructure}/HoodedShooterStow.java (84%) create mode 100644 src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureLeftCorner.java create mode 100644 src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSetState.java rename src/main/java/com/stuypulse/robot/commands/{hoodedshooter/HoodedShooterShoot.java => superstructure/SuperstructureShoot.java} (55%) rename src/main/java/com/stuypulse/robot/commands/{hoodedshooter => superstructure}/ZeroHoodAtLowerHardstop.java (89%) rename src/main/java/com/stuypulse/robot/subsystems/{hoodedshooter => }/hood/Hood.java (94%) rename src/main/java/com/stuypulse/robot/subsystems/{hoodedshooter => }/hood/HoodImpl.java (97%) delete mode 100644 src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java delete mode 100644 src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooterSim.java rename src/main/java/com/stuypulse/robot/subsystems/{hoodedshooter => }/shooter/Shooter.java (93%) rename src/main/java/com/stuypulse/robot/subsystems/{hoodedshooter => }/shooter/ShooterImpl.java (98%) rename src/main/java/com/stuypulse/robot/subsystems/{hoodedshooter => }/shooter/ShooterSim.java (97%) create mode 100644 src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 2969830d..0bf04425 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -20,12 +20,6 @@ import com.stuypulse.robot.commands.climberhopper.ClimberOverrideUp; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterFerry; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterKB; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterShoot; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterStow; -import com.stuypulse.robot.commands.hoodedshooter.ZeroHoodAtLowerHardstop; import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.intake.IntakeRunRollers; import com.stuypulse.robot.commands.intake.IntakeStopRollers; @@ -34,6 +28,12 @@ import com.stuypulse.robot.commands.intake.ZeroPivotStowed; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.HoodedShooterFerry; +import com.stuypulse.robot.commands.superstructure.HoodedShooterInterpolation; +import com.stuypulse.robot.commands.superstructure.HoodedShooterKB; +import com.stuypulse.robot.commands.superstructure.HoodedShooterShoot; +import com.stuypulse.robot.commands.superstructure.HoodedShooterStow; +import com.stuypulse.robot.commands.superstructure.ZeroHoodAtLowerHardstop; import com.stuypulse.robot.commands.swerve.SwerveDriveAlignTurretToHub; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; @@ -46,9 +46,9 @@ import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.subsystems.handoff.Handoff; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; -import com.stuypulse.robot.subsystems.hoodedshooter.hood.Hood; -import com.stuypulse.robot.subsystems.hoodedshooter.shooter.Shooter; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.hood.Hood; +import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.intake.Intake; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -93,7 +93,7 @@ public interface EnabledSubsystems { private final LimelightVision vision = LimelightVision.getInstance(); private final Turret turret = Turret.getInstance(); - private final HoodedShooter hoodedShooter = HoodedShooter.getInstance(); + private final Superstructure superstructure = Superstructure.getInstance(); private final Shooter shooter = Shooter.getInstance(); private final Hood hood = Hood.getInstance(); @@ -137,13 +137,13 @@ private void configureDefaultCommands() { private void configureButtonBindings() { // Scoring Routine driver.getTopButton() - .whileTrue(new HoodedShooterShoot().onlyIf(() -> !hoodedShooter.isHoodUnderTrench()) + .whileTrue(new HoodedShooterShoot().onlyIf(() -> !superstructure.isHoodUnderTrench()) .alongWith(new SwerveDriveAlignTurretToHub()) // .alongWith(new TurretShoot()) - .andThen(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 WaitUntilCommand(superstructure::atTolerance)) + .andThen(new HandoffRun().onlyIf(superstructure::atTolerance) + .alongWith(new WaitUntilCommand(handoff::atTolerance)) + .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) .onFalse(new SpindexerStop() .alongWith(new HoodedShooterStow()) .alongWith(new HandoffStop())); @@ -157,7 +157,7 @@ private void configureButtonBindings() { .onTrue(new IntakeStow()); driver.getLeftButton() - .whileTrue(new HoodedShooterShoot().onlyIf(() -> !hoodedShooter.isHoodUnderTrench())) + .whileTrue(new HoodedShooterShoot().onlyIf(() -> !superstructure.isHoodUnderTrench())) .onFalse(new HoodedShooterStow()); diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodAnalog.java b/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodAnalog.java index e536c2db..dab637ea 100644 --- a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodAnalog.java +++ b/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodAnalog.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.hoodedshooter; +package com.stuypulse.robot.commands.superstructure; import com.stuypulse.robot.subsystems.hoodedshooter.hood.Hood; import com.stuypulse.stuylib.input.Gamepad; diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterSetState.java b/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterSetState.java deleted file mode 100644 index e110761f..00000000 --- a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterSetState.java +++ /dev/null @@ -1,28 +0,0 @@ -/************************ 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.commands.hoodedshooter; - -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; - -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class HoodedShooterSetState extends InstantCommand { - private final HoodedShooter hoodedshooter; - private final HoodedShooterState state; - - public HoodedShooterSetState(HoodedShooterState state) { - hoodedshooter = HoodedShooter.getInstance(); - this.state = state; - - addRequirements(hoodedshooter); - } - - @Override - public void execute() { - hoodedshooter.setState(state); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/HoodAnalog.java b/src/main/java/com/stuypulse/robot/commands/superstructure/HoodAnalog.java new file mode 100644 index 00000000..dab637ea --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/HoodAnalog.java @@ -0,0 +1,29 @@ +package com.stuypulse.robot.commands.superstructure; + +import com.stuypulse.robot.subsystems.hoodedshooter.hood.Hood; +import com.stuypulse.stuylib.input.Gamepad; + +import edu.wpi.first.wpilibj2.command.Command; + +public class HoodAnalog extends Command{ + private final Hood hood; + private final Gamepad driver; + + public HoodAnalog(Gamepad driver) { + this.driver = driver; + hood = Hood.getInstance(); + + addRequirements(hood); + } + + @Override + public void initialize() { + hood.setState(Hood.HoodState.ANALOG); + } + + @Override + public void execute() { + hood.hoodAnalogToInput(driver); + //SmartDashboard.putNumber("HoodedShooter/Hood/Analog", hood.hoodAnalogToOutput().getDegrees()); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterFerry.java b/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterFerry.java similarity index 84% rename from src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterFerry.java rename to src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterFerry.java index 7bd7c239..37f9acc5 100644 --- a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterFerry.java @@ -3,12 +3,12 @@ /* 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.commands.hoodedshooter; +package com.stuypulse.robot.commands.superstructure; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; public class HoodedShooterFerry extends HoodedShooterSetState { public HoodedShooterFerry(){ - super(HoodedShooterState.FERRY); + super(SuperstructureState.FERRY); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterInterpolation.java b/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterInterpolation.java similarity index 84% rename from src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterInterpolation.java rename to src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterInterpolation.java index 26ad75b4..dbf694f0 100644 --- a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterInterpolation.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterInterpolation.java @@ -3,12 +3,12 @@ /* 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.commands.hoodedshooter; +package com.stuypulse.robot.commands.superstructure; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; public class HoodedShooterInterpolation extends HoodedShooterSetState { public HoodedShooterInterpolation() { - super(HoodedShooterState.INTERPOLATION); + super(SuperstructureState.INTERPOLATION); } } diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterKB.java b/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterKB.java similarity index 85% rename from src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterKB.java rename to src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterKB.java index 88a05bdd..ef352e6c 100644 --- a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterKB.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterKB.java @@ -3,12 +3,12 @@ /* 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.commands.hoodedshooter; +package com.stuypulse.robot.commands.superstructure; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; public class HoodedShooterKB extends HoodedShooterSetState { public HoodedShooterKB() { - super(HoodedShooterState.KB); + super(SuperstructureState.KB); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterReverse.java b/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterReverse.java similarity index 84% rename from src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterReverse.java rename to src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterReverse.java index c0b62b07..6c6eb9b6 100644 --- a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterReverse.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterReverse.java @@ -3,12 +3,12 @@ /* 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.commands.hoodedshooter; +package com.stuypulse.robot.commands.superstructure; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; public class HoodedShooterReverse extends HoodedShooterSetState { public HoodedShooterReverse() { - super(HoodedShooterState.REVERSE); + super(SuperstructureState.REVERSE); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterRightCorner.java b/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterRightCorner.java similarity index 84% rename from src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterRightCorner.java rename to src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterRightCorner.java index a189811c..d3395896 100644 --- a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterRightCorner.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterRightCorner.java @@ -3,12 +3,12 @@ /* 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.commands.hoodedshooter; +package com.stuypulse.robot.commands.superstructure; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; public class HoodedShooterRightCorner extends HoodedShooterSetState { public HoodedShooterRightCorner() { - super(HoodedShooterState.RIGHT_CORNER); + super(SuperstructureState.RIGHT_CORNER); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterLeftCorner.java b/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterShoot.java similarity index 54% rename from src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterLeftCorner.java rename to src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterShoot.java index 9c1556b3..79d87ce7 100644 --- a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterLeftCorner.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterShoot.java @@ -3,12 +3,12 @@ /* 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.commands.hoodedshooter; +package com.stuypulse.robot.commands.superstructure; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; +import com.stuypulse.robot.subsystems.Superstructure.Superstructure.SuperstructureState; -public class HoodedShooterLeftCorner extends HoodedShooterSetState { - public HoodedShooterLeftCorner() { - super(HoodedShooterState.LEFT_CORNER); +public class SuperstructureShoot extends SuperstructureSetState { + public SuperstructureShoot() { + super(SuperstructureState.SHOOT); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterStow.java b/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterStow.java similarity index 84% rename from src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterStow.java rename to src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterStow.java index aad5c5c4..ea6ab0af 100644 --- a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterStow.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterStow.java @@ -3,12 +3,12 @@ /* 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.commands.hoodedshooter; +package com.stuypulse.robot.commands.superstructure; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; public class HoodedShooterStow extends HoodedShooterSetState{ public HoodedShooterStow(){ - super(HoodedShooterState.STOW); + super(SuperstructureState.STOW); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureLeftCorner.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureLeftCorner.java new file mode 100644 index 00000000..ba823f11 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureLeftCorner.java @@ -0,0 +1,14 @@ +/************************ 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.commands.superstructure; + +import com.stuypulse.robot.subsystems.superstructure.SuperstructureState; + +public class SuperstructureLeftCorner extends SuperstructureSetState { + public SuperstructureLeftCorner() { + super(SuperstructureState.LEFT_CORNER); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSetState.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSetState.java new file mode 100644 index 00000000..5694f94c --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSetState.java @@ -0,0 +1,28 @@ +/************************ 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.commands.superstructure; + +import com.stuypulse.robot.subsystems.Superstructure.Superstructure; +import com.stuypulse.robot.subsystems.Superstructure.Superstructure.SuperstructureState; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class SuperstructureSetState extends InstantCommand { + private final Superstructure Superstructure; + private final SuperstructureState state; + + public SuperstructureSetState(SuperstructureState state) { + Superstructure = Superstructure.getInstance(); + this.state = state; + + addRequirements(Superstructure); + } + + @Override + public void execute() { + Superstructure.setState(state); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterShoot.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureShoot.java similarity index 55% rename from src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterShoot.java rename to src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureShoot.java index 6850da29..91add107 100644 --- a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureShoot.java @@ -3,12 +3,12 @@ /* 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.commands.hoodedshooter; +package com.stuypulse.robot.commands.superstructure; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; +import com.stuypulse.robot.subsystems.superstructure.SuperstructureState; -public class HoodedShooterShoot extends HoodedShooterSetState { - public HoodedShooterShoot() { - super(HoodedShooterState.SHOOT); +public class SuperstructureShoot extends SuperstructureSetState { + public SuperstructureShoot() { + super(SuperstructureState.SHOOT); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/ZeroHoodAtLowerHardstop.java b/src/main/java/com/stuypulse/robot/commands/superstructure/ZeroHoodAtLowerHardstop.java similarity index 89% rename from src/main/java/com/stuypulse/robot/commands/hoodedshooter/ZeroHoodAtLowerHardstop.java rename to src/main/java/com/stuypulse/robot/commands/superstructure/ZeroHoodAtLowerHardstop.java index ad7f410f..da38ffac 100644 --- a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/ZeroHoodAtLowerHardstop.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/ZeroHoodAtLowerHardstop.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.hoodedshooter; +package com.stuypulse.robot.commands.superstructure; import com.stuypulse.robot.subsystems.hoodedshooter.hood.Hood; diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java index bc89a640..8f7c5886 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java @@ -26,10 +26,10 @@ public void execute() { boolean isInAllianceZone = swerve.getPose().getX() < Field.getHubPose().getX(); if (isInAllianceZone) { - turret.setState(TurretState.SHOOTING); + turret.setState(TurretState.SHOOT); } else { - turret.setState(TurretState.FERRYING); + turret.setState(TurretState.FERRY); } } diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretFerry.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretFerry.java index a576df14..8fb76a39 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretFerry.java @@ -9,6 +9,6 @@ public class TurretFerry extends TurretSetState { public TurretFerry() { - super(TurretState.FERRYING); + super(TurretState.FERRY); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretShoot.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretShoot.java index 29dcf0ad..e210b4a2 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretShoot.java @@ -9,6 +9,6 @@ public class TurretShoot extends TurretSetState { public TurretShoot() { - super(TurretState.SHOOTING); + super(TurretState.SHOOT); } } \ 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/hood/Hood.java similarity index 94% rename from src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/Hood.java rename to src/main/java/com/stuypulse/robot/subsystems/hood/Hood.java index 9214f49a..c0e08922 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hood/Hood.java @@ -3,7 +3,7 @@ /* 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.subsystems.hoodedshooter.hood; +package com.stuypulse.robot.subsystems.hood; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; @@ -66,16 +66,16 @@ public Rotation2d getTargetAngle() { case RIGHT_CORNER -> Settings.HoodedShooter.Hood.Angles.RIGHT_CORNER_ANGLE; case INTERPOLATION -> HoodAngleCalculator.interpolateHoodAngle().get(); case ANALOG -> hoodAnalogToOutput(); - case IDLE -> getHoodAngle(); + case IDLE -> getAngle(); }; } public boolean atTolerance() { - double error = getHoodAngle().minus(getTargetAngle()).getRotations(); + double error = getAngle().minus(getTargetAngle()).getRotations(); return Math.abs(error) < Settings.HoodedShooter.HOOD_TOLERANCE.getRotations(); } - public abstract Rotation2d getHoodAngle(); + public abstract Rotation2d getAngle(); public void hoodAnalogToInput(Gamepad gamepad) { double hoodMin = Settings.HoodedShooter.Hood.Angles.MIN_ANGLE.getDegrees(); @@ -115,7 +115,7 @@ public void periodic() { SmartDashboard.putString("States/Hood", state.name()); SmartDashboard.putNumber("HoodedShooter/Hood/Target Angle", getTargetAngle().getDegrees()); - SmartDashboard.putNumber("HoodedShooter/Hood/Current Angle", getHoodAngle().getDegrees()); + SmartDashboard.putNumber("HoodedShooter/Hood/Current Angle", getAngle().getDegrees()); //SmartDashboard.putNumber("HoodedShooter/Hood/Analog Target Angle", hoodAnalogToOutput().getDegrees()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/hood/HoodImpl.java similarity index 97% rename from src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java rename to src/main/java/com/stuypulse/robot/subsystems/hood/HoodImpl.java index d12485ce..2e9b14d8 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hood/HoodImpl.java @@ -3,14 +3,13 @@ /* 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.subsystems.hoodedshooter.hood; +package com.stuypulse.robot.subsystems.hood; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Gains; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; import com.stuypulse.robot.util.SysId; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; @@ -83,7 +82,7 @@ public HoodImpl() { } @Override - public Rotation2d getHoodAngle() { + public Rotation2d getAngle() { return Rotation2d.fromRotations(hoodMotor.getPosition().getValueAsDouble()); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java deleted file mode 100644 index f2d1752b..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java +++ /dev/null @@ -1,134 +0,0 @@ -/************************ 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.subsystems.hoodedshooter; - -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.subsystems.hoodedshooter.hood.Hood; -import com.stuypulse.robot.subsystems.hoodedshooter.hood.Hood.HoodState; -import com.stuypulse.robot.subsystems.hoodedshooter.shooter.Shooter; -import com.stuypulse.robot.subsystems.hoodedshooter.shooter.Shooter.ShooterState; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class HoodedShooter extends SubsystemBase { - - private static final HoodedShooter instance; - - static { - if (Robot.isReal()) { - instance = new HoodedShooter(); - } - else { - instance = new HoodedShooterSim(); - } - } - - public static HoodedShooter getInstance(){ - return instance; - } - - private HoodedShooterState state; - - private final Hood hood; - private final Shooter shooter; - - public HoodedShooter() { - state = HoodedShooterState.INTERPOLATION; - hood = Hood.getInstance(); - shooter = Shooter.getInstance(); - } - - public enum HoodedShooterState { - STOW(HoodState.STOW, ShooterState.SHOOT), - SHOOT(HoodState.SHOOT, ShooterState.SHOOT), - FERRY(HoodState.FERRY, ShooterState.FERRY), - REVERSE(HoodState.SHOOT, ShooterState.REVERSE), - KB(HoodState.KB, ShooterState.KB), - LEFT_CORNER(HoodState.LEFT_CORNER, ShooterState.LEFT_CORNER), - RIGHT_CORNER(HoodState.RIGHT_CORNER, ShooterState.RIGHT_CORNER), - INTERPOLATION(HoodState.INTERPOLATION, ShooterState.INTERPOLATION); - - private HoodState hoodState; - private ShooterState shooterState; - - private HoodedShooterState(HoodState hoodState, ShooterState shooterState) { - this.hoodState = hoodState; - this.shooterState = shooterState; - } - - public HoodState getHoodState() { - return hoodState; - } - - public ShooterState getShooterState() { - return shooterState; - } - } - - public void setState(HoodedShooterState state){ - this.state = state; - hood.setState(state.getHoodState()); - shooter.setState(state.getShooterState()); - } - - public HoodedShooterState getState(){ - return state; - } - - public boolean bothAtTolerance() { - return isShooterAtTolerance() && isHoodAtTolerance(); - } - - public boolean isHoodUnderTrench() { - return hood.isHoodUnderTrench(); - } - - public boolean isShooterAtTolerance() { - return shooter.atTolerance(); - } - - public boolean isHoodAtTolerance() { - return hood.atTolerance(); - } - - public double getTargetRPM() { - return shooter.getTargetRPM(); - } - - public Rotation2d getTargetAngle() { - return hood.getTargetAngle(); - } - - public double getShooterRPM() { - return shooter.getShooterRPM(); - } - - public Rotation2d getHoodAngle() { - return hood.getHoodAngle(); - } - - @Override - public void periodic() { - SmartDashboard.putString("HoodedShooter/State", state.name()); - SmartDashboard.putString("States/HoodedShooter", state.name()); - - SmartDashboard.putNumber("HoodedShooter/Target RPM", shooter.getTargetRPM()); - SmartDashboard.putNumber("HoodedShooter/Target Angle", hood.getTargetAngle().getDegrees()); - - SmartDashboard.putNumber("HoodedShooter/Current RPM", getShooterRPM()); - SmartDashboard.putNumber("HoodedShooter/Current Angle", getHoodAngle().getDegrees()); - - SmartDashboard.putNumber("HoodedShooter/Angle Error (Deg)", hood.getTargetAngle().getDegrees() - getHoodAngle().getDegrees()); - - 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/HoodedShooterSim.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooterSim.java deleted file mode 100644 index 4722fbfd..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooterSim.java +++ /dev/null @@ -1,28 +0,0 @@ -/************************ 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.subsystems.hoodedshooter; - -import com.stuypulse.stuylib.network.SmartBoolean; - -public class HoodedShooterSim extends HoodedShooter { - - private final SmartBoolean bothAtTolerance; - - protected HoodedShooterSim() { - super(); - bothAtTolerance = new SmartBoolean("HoodedShooter/Both At Tolerance", false); - } - - @Override - public boolean bothAtTolerance() { - return bothAtTolerance.get(); - } - - @Override - public void periodic() { - super.periodic(); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java similarity index 93% rename from src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java rename to src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java index a107970f..9622bcab 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java @@ -3,7 +3,7 @@ /* 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.subsystems.hoodedshooter.shooter; +package com.stuypulse.robot.subsystems.shooter; import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; @@ -71,11 +71,11 @@ public double getShootRPM() { } public boolean atTolerance() { - double diff = Math.abs(getTargetRPM() - getShooterRPM()); + double diff = Math.abs(getTargetRPM() - getRPM()); return diff < Settings.HoodedShooter.SHOOTER_TOLERANCE_RPM; } - public abstract double getShooterRPM(); + public abstract double getRPM(); public abstract SysIdRoutine getShooterSysIdRoutine(); @@ -84,7 +84,7 @@ public void periodic() { SmartDashboard.putString("HoodedShooter/Shooter/State", state.name()); SmartDashboard.putString("States/Shooter", state.name()); - SmartDashboard.putNumber("HoodedShooter/Shooter/Current RPM", getShooterRPM()); + SmartDashboard.putNumber("HoodedShooter/Shooter/Current RPM", getRPM()); SmartDashboard.putNumber("HoodedShooter/Shooter/Target RPM", getTargetRPM()); SmartDashboard.putNumber("InterpolationTesting/Shooter Interpolated Target Shoot 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/shooter/ShooterImpl.java similarity index 98% rename from src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java rename to src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java index 576b234c..b5155f82 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -3,7 +3,7 @@ /* 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.subsystems.hoodedshooter.shooter; +package com.stuypulse.robot.subsystems.shooter; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Gains; @@ -62,7 +62,7 @@ public ShooterImpl() { } @Override - public double getShooterRPM() { + public double getRPM() { return getLeaderRPM(); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterSim.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java similarity index 97% rename from src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterSim.java rename to src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java index c0fefef9..2e506a85 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java @@ -3,7 +3,7 @@ /* 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.subsystems.hoodedshooter.shooter; +package com.stuypulse.robot.subsystems.shooter; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Settings; @@ -56,7 +56,7 @@ public ShooterSim() { } @Override - public double getShooterRPM() { + public double getRPM() { return sim.getOutput(0) * 60.0 / (2.0 * Math.PI); // convert to RPM } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java new file mode 100644 index 00000000..0ee83a95 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -0,0 +1,155 @@ +/************************ 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.subsystems.superstructure; + +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.subsystems.shooter.Shooter; +import com.stuypulse.robot.subsystems.shooter.Shooter.ShooterState; +import com.stuypulse.robot.subsystems.hood.Hood; +import com.stuypulse.robot.subsystems.hood.Hood.HoodState; +import com.stuypulse.robot.subsystems.turret.Turret; +import com.stuypulse.robot.subsystems.turret.Turret.TurretState; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Superstructure extends SubsystemBase { + + private static final Superstructure instance; + + static { + instance = new Superstructure(); + } + + public static Superstructure getInstance(){ + return instance; + } + + private SuperstructureState state; + + private final Hood hood; + private final Shooter shooter; + private final Turret turret; + + public Superstructure() { + state = SuperstructureState.INTERPOLATION; + hood = Hood.getInstance(); + shooter = Shooter.getInstance(); + turret = Turret.getInstance(); + } + + public enum SuperstructureState { + STOW(HoodState.STOW, ShooterState.SHOOT, TurretState.SHOOT), + SHOOT(HoodState.SHOOT, ShooterState.SHOOT, TurretState.SHOOT), + FERRY(HoodState.FERRY, ShooterState.FERRY, TurretState.FERRY), + REVERSE(HoodState.SHOOT, ShooterState.REVERSE, TurretState.SHOOT), + KB(HoodState.KB, ShooterState.KB, TurretState.SHOOT), + LEFT_CORNER(HoodState.LEFT_CORNER, ShooterState.LEFT_CORNER, TurretState.SHOOT), + RIGHT_CORNER(HoodState.RIGHT_CORNER, ShooterState.RIGHT_CORNER, TurretState.SHOOT), + INTERPOLATION(HoodState.INTERPOLATION, ShooterState.INTERPOLATION, TurretState.SHOOT); + + private HoodState hoodState; + private ShooterState shooterState; + private TurretState turretState; + + private SuperstructureState(HoodState hoodState, ShooterState shooterState, TurretState TurretState) { + this.hoodState = hoodState; + this.shooterState = shooterState; + this.turretState = TurretState; + } + + public HoodState getHoodState() { + return hoodState; + } + + public ShooterState getShooterState() { + return shooterState; + } + + public TurretState getTurretState(){ + return turretState; + } + } + + public void setState(SuperstructureState state){ + this.state = state; + hood.setState(state.getHoodState()); + shooter.setState(state.getShooterState()); + turret.setState(state.getTurretState()); + } + + public SuperstructureState getState(){ + return state; + } + + public boolean atTolerance() { + return isShooterAtTolerance() && isHoodAtTolerance() && isTurretAtTolerance(); + } + + public boolean isHoodUnderTrench() { + return hood.isHoodUnderTrench(); + } + + public boolean isShooterAtTolerance() { + return shooter.atTolerance(); + } + + public boolean isHoodAtTolerance() { + return hood.atTolerance(); + } + + public boolean isTurretAtTolerance(){ + return turret.atTargetAngle(); + } + + public double getTargetRPM() { + return shooter.getTargetRPM(); + } + + public Rotation2d getTargetYaw() { + return hood.getTargetAngle(); + } + + public Rotation2d getTargetPitch(){ + return turret.getTargetAngle(); + } + + public double getShooterRPM() { + return shooter.getRPM(); + } + + public Rotation2d getHoodAngle() { + return hood.getAngle(); + } + + public Rotation2d getTurretAngle(){ + return turret.getAngle(); + } + + @Override + public void periodic() { + SmartDashboard.putString("SuperStructure/State", state.name()); + SmartDashboard.putString("States/SuperStructure", state.name()); + + SmartDashboard.putNumber("SuperStructure/Target RPM", getTargetRPM()); + SmartDashboard.putNumber("SuperStructure/Target Yaw", getTargetYaw().getDegrees()); + SmartDashboard.putNumber("SuperStructure/Target Pitch", getTargetPitch().getDegrees()); + + SmartDashboard.putNumber("SuperStructure/Current RPM", getShooterRPM()); + SmartDashboard.putNumber("SuperStructure/Current Yaw", getTurretAngle().getDegrees()); + SmartDashboard.putNumber("SuperStructure/Current Pitch", getHoodAngle().getDegrees()); + + SmartDashboard.putNumber("SuperStructure/Hood Angle Error (Deg)", getTargetPitch().getDegrees() - getHoodAngle().getDegrees()); + + SmartDashboard.putBoolean("SuperStructure/Shooter At Tolerance?", isShooterAtTolerance()); + SmartDashboard.putBoolean("SuperStructure/Hood At Tolerance?", isHoodAtTolerance()); + SmartDashboard.putBoolean("SuperStructure/Turret At Tolerant?", 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/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java index 3a663574..02a204f5 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java @@ -46,9 +46,8 @@ public void setDriverInput(Gamepad gamepad) { public enum TurretState { IDLE, ZERO, - SHOOTING, - FERRYING, - HUB, + SHOOT, + FERRY, LEFT_CORNER, RIGHT_CORNER, TESTING; @@ -58,9 +57,8 @@ public Rotation2d getTargetAngle() { return switch (getState()) { case IDLE -> getAngle(); case ZERO -> Rotation2d.kZero; - case SHOOTING -> getScoringAngle(); - case FERRYING -> getFerryAngle(); - case HUB -> Settings.Turret.HUB; + case SHOOT -> getScoringAngle(); + case FERRY -> getFerryAngle(); case LEFT_CORNER -> Settings.Turret.LEFT_CORNER; case RIGHT_CORNER -> Settings.Turret.RIGHT_CORNER; case TESTING -> driverInputToAngle(); From ea7a80afedd2d858461be74247c247466ce4f21b Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Wed, 4 Mar 2026 18:19:37 -0500 Subject: [PATCH 065/150] CLEAN: cleanup of doom and despair Co-authored-by: Xuan Hao Cen Co-authored-by: Lucas O Co-authored-by: hc-45 --- .../robot/commands/auton/poaching/BottomOneCyclePoach.java | 2 +- .../robot/commands/auton/poaching/BottomTwoCyclePoach.java | 2 +- .../robot/commands/auton/poaching/TopOneCyclePoach.java | 2 +- .../robot/commands/auton/poaching/TopTwoCyclePoach.java | 2 +- .../stuypulse/robot/commands/auton/regular/BottomTwoCycle.java | 2 +- .../com/stuypulse/robot/commands/auton/regular/DepotAuton.java | 2 +- .../com/stuypulse/robot/commands/auton/regular/EightFuel.java | 2 +- .../com/stuypulse/robot/commands/auton/regular/TopTwoCycle.java | 2 +- .../java/com/stuypulse/robot/commands/auton/test/BoxTest.java | 2 +- 9 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomOneCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomOneCyclePoach.java index c55bc2e0..36d8ecbf 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomOneCyclePoach.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomOneCyclePoach.java @@ -4,11 +4,11 @@ import com.stuypulse.robot.commands.climberhopper.ClimberDown; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; import com.stuypulse.robot.commands.intake.IntakeDeploy; 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.superstructure.HoodedShooterInterpolation; import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; import com.stuypulse.robot.subsystems.spindexer.Spindexer; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomTwoCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomTwoCyclePoach.java index 9b38668a..6730361d 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomTwoCyclePoach.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomTwoCyclePoach.java @@ -4,11 +4,11 @@ import com.stuypulse.robot.commands.climberhopper.ClimberDown; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; import com.stuypulse.robot.commands.intake.IntakeDeploy; 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.superstructure.HoodedShooterInterpolation; import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; import com.stuypulse.robot.subsystems.spindexer.Spindexer; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopOneCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopOneCyclePoach.java index 2454b2e8..1a228719 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopOneCyclePoach.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopOneCyclePoach.java @@ -4,11 +4,11 @@ import com.stuypulse.robot.commands.climberhopper.ClimberDown; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; import com.stuypulse.robot.commands.intake.IntakeDeploy; 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.superstructure.HoodedShooterInterpolation; import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; import com.stuypulse.robot.subsystems.spindexer.Spindexer; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopTwoCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopTwoCyclePoach.java index 6d028a2f..06b33c69 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopTwoCyclePoach.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopTwoCyclePoach.java @@ -4,11 +4,11 @@ import com.stuypulse.robot.commands.climberhopper.ClimberDown; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; import com.stuypulse.robot.commands.intake.IntakeDeploy; 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.superstructure.HoodedShooterInterpolation; import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; import com.stuypulse.robot.subsystems.spindexer.Spindexer; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/BottomTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/BottomTwoCycle.java index 124205a0..d56e64b6 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/BottomTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/BottomTwoCycle.java @@ -4,11 +4,11 @@ import com.stuypulse.robot.commands.climberhopper.ClimberDown; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; import com.stuypulse.robot.commands.intake.IntakeDeploy; 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.superstructure.HoodedShooterInterpolation; import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; import com.stuypulse.robot.subsystems.spindexer.Spindexer; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java index f776459a..47208efa 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java @@ -4,11 +4,11 @@ import com.stuypulse.robot.commands.climberhopper.ClimberDown; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; import com.stuypulse.robot.commands.intake.IntakeDeploy; 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.superstructure.HoodedShooterInterpolation; import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; import com.stuypulse.robot.subsystems.spindexer.Spindexer; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java index f9cca46d..1b27d6af 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java @@ -2,8 +2,8 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterKB; import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.superstructure.HoodedShooterKB; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/TopTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/TopTwoCycle.java index 118ba420..08e45ad6 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/TopTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/TopTwoCycle.java @@ -4,11 +4,11 @@ import com.stuypulse.robot.commands.climberhopper.ClimberDown; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; import com.stuypulse.robot.commands.intake.IntakeDeploy; 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.superstructure.HoodedShooterInterpolation; import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; import com.stuypulse.robot.subsystems.spindexer.Spindexer; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java b/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java index faac48cf..b0b2b1c1 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java @@ -1,7 +1,7 @@ package com.stuypulse.robot.commands.auton.test; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterKB; +import com.stuypulse.robot.commands.superstructure.HoodedShooterKB; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj.DriverStation; From 52ba31c24ccd67e10e4a5669f33dacf4971d3bf9 Mon Sep 17 00:00:00 2001 From: Lucas Date: Wed, 4 Mar 2026 18:45:50 -0500 Subject: [PATCH 066/150] REFACTOR: All imports and class names now use Superstructure instead of HoodedShooter --- .../com/stuypulse/robot/RobotContainer.java | 28 +++++++++--------- .../auton/poaching/BottomOneCyclePoach.java | 6 ++-- .../auton/poaching/BottomTwoCyclePoach.java | 8 ++--- .../auton/poaching/TopOneCyclePoach.java | 6 ++-- .../auton/poaching/TopTwoCyclePoach.java | 8 ++--- .../auton/regular/BottomTwoCycle.java | 8 ++--- .../commands/auton/regular/DepotAuton.java | 6 ++-- .../commands/auton/regular/EightFuel.java | 8 ++--- .../commands/auton/regular/TopTwoCycle.java | 12 ++++---- .../robot/commands/auton/test/BoxTest.java | 4 +-- .../commands/handoff/HandoffReverse.java | 2 +- .../robot/commands/handoff/HandoffRun.java | 2 +- ...HandoffState.java => HandoffSetState.java} | 4 +-- .../robot/commands/handoff/HandoffStop.java | 2 +- .../{hoodedshooter => hood}/HoodAnalog.java | 4 +-- .../ZeroHoodAtLowerHardstop.java | 4 +-- .../commands/spindexer/SpindexerDynamic.java | 2 +- .../commands/spindexer/SpindexerReverse.java | 2 +- .../commands/spindexer/SpindexerRun.java | 2 +- ...dexerState.java => SpindexerSetState.java} | 4 +-- .../commands/spindexer/SpindexerStop.java | 2 +- .../commands/superstructure/HoodAnalog.java | 29 ------------------- .../superstructure/HoodedShooterShoot.java | 14 --------- ...terFerry.java => SuperstructureFerry.java} | 7 +++-- ....java => SuperstructureInterpolation.java} | 6 ++-- ...edShooterKB.java => SuperstructureKB.java} | 6 ++-- .../SuperstructureLeftCorner.java | 2 +- ...everse.java => SuperstructureReverse.java} | 6 ++-- ...er.java => SuperstructureRightCorner.java} | 6 ++-- .../SuperstructureSetState.java | 12 ++++---- .../superstructure/SuperstructureShoot.java | 2 +- ...ooterStow.java => SuperstructureStow.java} | 6 ++-- .../robot/commands/turret/TurretHub.java | 14 --------- 33 files changed, 89 insertions(+), 145 deletions(-) rename src/main/java/com/stuypulse/robot/commands/handoff/{SetHandoffState.java => HandoffSetState.java} (89%) rename src/main/java/com/stuypulse/robot/commands/{hoodedshooter => hood}/HoodAnalog.java (84%) rename src/main/java/com/stuypulse/robot/commands/{superstructure => hood}/ZeroHoodAtLowerHardstop.java (76%) rename src/main/java/com/stuypulse/robot/commands/spindexer/{SetSpindexerState.java => SpindexerSetState.java} (88%) delete mode 100644 src/main/java/com/stuypulse/robot/commands/superstructure/HoodAnalog.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterShoot.java rename src/main/java/com/stuypulse/robot/commands/superstructure/{HoodedShooterFerry.java => SuperstructureFerry.java} (62%) rename src/main/java/com/stuypulse/robot/commands/superstructure/{HoodedShooterInterpolation.java => SuperstructureInterpolation.java} (68%) rename src/main/java/com/stuypulse/robot/commands/superstructure/{HoodedShooterKB.java => SuperstructureKB.java} (70%) rename src/main/java/com/stuypulse/robot/commands/superstructure/{HoodedShooterReverse.java => SuperstructureReverse.java} (69%) rename src/main/java/com/stuypulse/robot/commands/superstructure/{HoodedShooterRightCorner.java => SuperstructureRightCorner.java} (68%) rename src/main/java/com/stuypulse/robot/commands/superstructure/{HoodedShooterStow.java => SuperstructureStow.java} (70%) delete mode 100644 src/main/java/com/stuypulse/robot/commands/turret/TurretHub.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 0bf04425..37822084 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -20,6 +20,7 @@ import com.stuypulse.robot.commands.climberhopper.ClimberOverrideUp; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.hood.ZeroHoodAtLowerHardstop; import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.intake.IntakeRunRollers; import com.stuypulse.robot.commands.intake.IntakeStopRollers; @@ -28,12 +29,11 @@ import com.stuypulse.robot.commands.intake.ZeroPivotStowed; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.superstructure.HoodedShooterFerry; -import com.stuypulse.robot.commands.superstructure.HoodedShooterInterpolation; -import com.stuypulse.robot.commands.superstructure.HoodedShooterKB; -import com.stuypulse.robot.commands.superstructure.HoodedShooterShoot; -import com.stuypulse.robot.commands.superstructure.HoodedShooterStow; -import com.stuypulse.robot.commands.superstructure.ZeroHoodAtLowerHardstop; +import com.stuypulse.robot.commands.superstructure.SuperstructureFerry; +import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureKB; +import com.stuypulse.robot.commands.superstructure.SuperstructureShoot; +import com.stuypulse.robot.commands.superstructure.SuperstructureStow; import com.stuypulse.robot.commands.swerve.SwerveDriveAlignTurretToHub; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; @@ -137,7 +137,7 @@ private void configureDefaultCommands() { private void configureButtonBindings() { // Scoring Routine driver.getTopButton() - .whileTrue(new HoodedShooterShoot().onlyIf(() -> !superstructure.isHoodUnderTrench()) + .whileTrue(new SuperstructureShoot().onlyIf(() -> !superstructure.isHoodUnderTrench()) .alongWith(new SwerveDriveAlignTurretToHub()) // .alongWith(new TurretShoot()) .andThen(new WaitUntilCommand(superstructure::atTolerance)) @@ -145,7 +145,7 @@ private void configureButtonBindings() { .alongWith(new WaitUntilCommand(handoff::atTolerance)) .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) .onFalse(new SpindexerStop() - .alongWith(new HoodedShooterStow()) + .alongWith(new SuperstructureStow()) .alongWith(new HandoffStop())); // Intake Deploy @@ -157,14 +157,14 @@ private void configureButtonBindings() { .onTrue(new IntakeStow()); driver.getLeftButton() - .whileTrue(new HoodedShooterShoot().onlyIf(() -> !superstructure.isHoodUnderTrench())) - .onFalse(new HoodedShooterStow()); + .whileTrue(new SuperstructureShoot().onlyIf(() -> !superstructure.isHoodUnderTrench())) + .onFalse(new SuperstructureStow()); driver.getRightButton() - .whileTrue(new HoodedShooterFerry().onlyIf( + .whileTrue(new SuperstructureFerry().onlyIf( () -> CommandSwerveDrivetrain.getInstance().getPose().getX() > Field.getHubPose().getX())) - .onFalse(new HoodedShooterStow()); + .onFalse(new SuperstructureStow()); // Reset Heading driver.getDPadUp() @@ -174,7 +174,7 @@ private void configureButtonBindings() { // // Ferry Routine using Interpolation Settings // driver.getBottomButton() - // .onTrue(new HoodedShooterFerry() + // .onTrue(new SuperstructureFerry() // .alongWith(new TurretFerry()) // .alongWith(new WaitUntilCommand(() -> hoodedShooter.bothAtTolerance())) // .andThen(new HandoffRun().onlyIf(() -> hoodedShooter.bothAtTolerance()) @@ -182,7 +182,7 @@ private void configureButtonBindings() { // .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && hoodedShooter.bothAtTolerance()))) // ) // .onFalse(new SpindexerStop() - // .alongWith(new HoodedShooterStow()) + // .alongWith(new SuperstructureStow()) // .alongWith(new HandoffStop())); //-------------------------------------------------------------------------------------------------------------------------\\ diff --git a/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomOneCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomOneCyclePoach.java index 36d8ecbf..fe954dff 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomOneCyclePoach.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomOneCyclePoach.java @@ -8,9 +8,9 @@ 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.superstructure.HoodedShooterInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -35,7 +35,7 @@ public BottomOneCyclePoach(PathPlannerPath... paths) { new IntakeStow() ), new ParallelCommandGroup( - new WaitUntilCommand(() -> HoodedShooter.getInstance().bothAtTolerance()), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new SwerveClimbAlign() ), new SpindexerRun().alongWith( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomTwoCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomTwoCyclePoach.java index 6730361d..999e216a 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomTwoCyclePoach.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomTwoCyclePoach.java @@ -8,9 +8,9 @@ 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.superstructure.HoodedShooterInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -34,7 +34,7 @@ public BottomTwoCyclePoach(PathPlannerPath... paths) { CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( new IntakeStow() ), - new WaitUntilCommand(() -> HoodedShooter.getInstance().bothAtTolerance()), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new SpindexerRun().alongWith( new HandoffRun() ).withTimeout(5.0), @@ -53,7 +53,7 @@ public BottomTwoCyclePoach(PathPlannerPath... paths) { new IntakeStow() ), new ParallelCommandGroup( - new WaitUntilCommand(() -> HoodedShooter.getInstance().bothAtTolerance()), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new SwerveClimbAlign() ), new SpindexerRun().alongWith( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopOneCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopOneCyclePoach.java index 1a228719..25da5f00 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopOneCyclePoach.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopOneCyclePoach.java @@ -8,9 +8,9 @@ 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.superstructure.HoodedShooterInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -35,7 +35,7 @@ public TopOneCyclePoach(PathPlannerPath... paths) { new IntakeStow() ), new ParallelCommandGroup( - new WaitUntilCommand(() -> HoodedShooter.getInstance().bothAtTolerance()), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new SwerveClimbAlign() ), new SpindexerRun().alongWith( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopTwoCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopTwoCyclePoach.java index 06b33c69..c4fec762 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopTwoCyclePoach.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopTwoCyclePoach.java @@ -8,9 +8,9 @@ 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.superstructure.HoodedShooterInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -34,7 +34,7 @@ public TopTwoCyclePoach(PathPlannerPath... paths) { CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( new IntakeStow() ), - new WaitUntilCommand(() -> HoodedShooter.getInstance().bothAtTolerance()), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new SpindexerRun().alongWith( new HandoffRun() ).withTimeout(5.0), @@ -53,7 +53,7 @@ public TopTwoCyclePoach(PathPlannerPath... paths) { new IntakeStow() ), new ParallelCommandGroup( - new WaitUntilCommand(() -> HoodedShooter.getInstance().bothAtTolerance()), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new SwerveClimbAlign() ), new SpindexerRun().alongWith( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/BottomTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/BottomTwoCycle.java index d56e64b6..01b024c2 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/BottomTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/BottomTwoCycle.java @@ -8,9 +8,9 @@ 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.superstructure.HoodedShooterInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -34,7 +34,7 @@ public BottomTwoCycle(PathPlannerPath... paths) { CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( new IntakeStow() ), - new WaitUntilCommand(() -> HoodedShooter.getInstance().bothAtTolerance()), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new SpindexerRun().alongWith( new HandoffRun() ).withTimeout(5.0), @@ -53,7 +53,7 @@ public BottomTwoCycle(PathPlannerPath... paths) { new IntakeStow() ), new ParallelCommandGroup( - new WaitUntilCommand(() -> HoodedShooter.getInstance().bothAtTolerance()), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new SwerveClimbAlign() ), new SpindexerRun().alongWith( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java index 47208efa..aafdc78a 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java @@ -8,9 +8,9 @@ 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.superstructure.HoodedShooterInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -34,7 +34,7 @@ public DepotAuton(PathPlannerPath... paths) { CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]) ), - new WaitUntilCommand(() -> HoodedShooter.getInstance().bothAtTolerance()), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new SpindexerRun().alongWith( new HandoffRun() ).until(() -> DriverStation.getMatchTime() < 2).andThen( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java index 1b27d6af..bef51934 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java @@ -3,8 +3,8 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.spindexer.SpindexerRun; -import com.stuypulse.robot.commands.superstructure.HoodedShooterKB; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; +import com.stuypulse.robot.commands.superstructure.SuperstructureKB; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -16,8 +16,8 @@ public EightFuel(PathPlannerPath... paths) { addCommands( - new HoodedShooterKB().until( - () -> HoodedShooter.getInstance().bothAtTolerance() + new SuperstructureKB().until( + () -> Superstructure.getInstance().atTolerance() ), new SpindexerRun().alongWith( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/TopTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/TopTwoCycle.java index 08e45ad6..1b4842fc 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/TopTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/TopTwoCycle.java @@ -8,9 +8,9 @@ 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.superstructure.HoodedShooterInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -35,8 +35,8 @@ public TopTwoCycle(PathPlannerPath... paths) { new IntakeStow() ), new ParallelCommandGroup( - new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()), - new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()) + new WaitUntilCommand(() -> Superstructure.getInstance().isShooterAtTolerance()), + new WaitUntilCommand(() -> Superstructure.getInstance().isHoodAtTolerance()) ), new SpindexerRun().alongWith( new HandoffRun() @@ -56,8 +56,8 @@ public TopTwoCycle(PathPlannerPath... paths) { new IntakeStow() ), new ParallelCommandGroup( - new WaitUntilCommand(() -> HoodedShooter.getInstance().isShooterAtTolerance()), - new WaitUntilCommand(() -> HoodedShooter.getInstance().isHoodAtTolerance()), + new WaitUntilCommand(() -> Superstructure.getInstance().isShooterAtTolerance()), + new WaitUntilCommand(() -> Superstructure.getInstance().isHoodAtTolerance()), new SwerveClimbAlign() ), new SpindexerRun().alongWith( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java b/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java index b0b2b1c1..3d0d292e 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java @@ -1,7 +1,7 @@ package com.stuypulse.robot.commands.auton.test; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.superstructure.HoodedShooterKB; +import com.stuypulse.robot.commands.superstructure.SuperstructureKB; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj.DriverStation; @@ -15,7 +15,7 @@ public BoxTest(PathPlannerPath... paths) { addCommands( - new HoodedShooterKB().until(() -> DriverStation.getMatchTime() < 18).andThen( + new SuperstructureKB().until(() -> DriverStation.getMatchTime() < 18).andThen( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0])), CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), diff --git a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffReverse.java b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffReverse.java index 1176cae4..663b21c5 100644 --- a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffReverse.java +++ b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffReverse.java @@ -7,7 +7,7 @@ import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; -public class HandoffReverse extends SetHandoffState { +public class HandoffReverse extends HandoffSetState { public HandoffReverse() { super(HandoffState.REVERSE); } diff --git a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffRun.java b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffRun.java index dd166bdf..ebd37507 100644 --- a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffRun.java +++ b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffRun.java @@ -6,7 +6,7 @@ package com.stuypulse.robot.commands.handoff; import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; -public class HandoffRun extends SetHandoffState { +public class HandoffRun extends HandoffSetState { public HandoffRun() { super(HandoffState.FORWARD); } diff --git a/src/main/java/com/stuypulse/robot/commands/handoff/SetHandoffState.java b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffSetState.java similarity index 89% rename from src/main/java/com/stuypulse/robot/commands/handoff/SetHandoffState.java rename to src/main/java/com/stuypulse/robot/commands/handoff/HandoffSetState.java index d2e5787c..72885181 100644 --- a/src/main/java/com/stuypulse/robot/commands/handoff/SetHandoffState.java +++ b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffSetState.java @@ -10,11 +10,11 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; -public class SetHandoffState extends InstantCommand{ +public class HandoffSetState extends InstantCommand{ private final Handoff handoff; private HandoffState state; - public SetHandoffState(HandoffState state) { + public HandoffSetState(HandoffState state) { this.handoff = Handoff.getInstance(); this.state = state; addRequirements(handoff); diff --git a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffStop.java b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffStop.java index 964b89ae..738159c0 100644 --- a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffStop.java +++ b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffStop.java @@ -6,7 +6,7 @@ package com.stuypulse.robot.commands.handoff; import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; -public class HandoffStop extends SetHandoffState { +public class HandoffStop extends HandoffSetState { public HandoffStop() { super(HandoffState.STOP); } diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodAnalog.java b/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java similarity index 84% rename from src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodAnalog.java rename to src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java index dab637ea..12f3d325 100644 --- a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodAnalog.java +++ b/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java @@ -1,6 +1,6 @@ -package com.stuypulse.robot.commands.superstructure; +package com.stuypulse.robot.commands.hood; -import com.stuypulse.robot.subsystems.hoodedshooter.hood.Hood; +import com.stuypulse.robot.subsystems.hood.Hood; import com.stuypulse.stuylib.input.Gamepad; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/ZeroHoodAtLowerHardstop.java b/src/main/java/com/stuypulse/robot/commands/hood/ZeroHoodAtLowerHardstop.java similarity index 76% rename from src/main/java/com/stuypulse/robot/commands/superstructure/ZeroHoodAtLowerHardstop.java rename to src/main/java/com/stuypulse/robot/commands/hood/ZeroHoodAtLowerHardstop.java index da38ffac..defe431d 100644 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/ZeroHoodAtLowerHardstop.java +++ b/src/main/java/com/stuypulse/robot/commands/hood/ZeroHoodAtLowerHardstop.java @@ -1,6 +1,6 @@ -package com.stuypulse.robot.commands.superstructure; +package com.stuypulse.robot.commands.hood; -import com.stuypulse.robot.subsystems.hoodedshooter.hood.Hood; +import com.stuypulse.robot.subsystems.hood.Hood; import edu.wpi.first.wpilibj2.command.InstantCommand; diff --git a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDynamic.java b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDynamic.java index c25a1dfd..c17ab7e8 100644 --- a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDynamic.java +++ b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDynamic.java @@ -7,7 +7,7 @@ import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; -public class SpindexerDynamic extends SetSpindexerState { +public class SpindexerDynamic extends SpindexerSetState { public SpindexerDynamic() { super(SpindexerState.DYNAMIC); } diff --git a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerReverse.java b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerReverse.java index 77795d39..721e92ec 100644 --- a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerReverse.java +++ b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerReverse.java @@ -6,7 +6,7 @@ package com.stuypulse.robot.commands.spindexer; import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; -public class SpindexerReverse extends SetSpindexerState{ +public class SpindexerReverse extends SpindexerSetState{ public SpindexerReverse() { super(SpindexerState.REVERSE); } diff --git a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerRun.java b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerRun.java index 79fc4c8c..a46f2db4 100644 --- a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerRun.java +++ b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerRun.java @@ -7,7 +7,7 @@ import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; -public class SpindexerRun extends SetSpindexerState{ +public class SpindexerRun extends SpindexerSetState{ public SpindexerRun(){ super(SpindexerState.FORWARD); } diff --git a/src/main/java/com/stuypulse/robot/commands/spindexer/SetSpindexerState.java b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerSetState.java similarity index 88% rename from src/main/java/com/stuypulse/robot/commands/spindexer/SetSpindexerState.java rename to src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerSetState.java index ec4dde5b..415a4de1 100644 --- a/src/main/java/com/stuypulse/robot/commands/spindexer/SetSpindexerState.java +++ b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerSetState.java @@ -10,11 +10,11 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; -public class SetSpindexerState extends InstantCommand { +public class SpindexerSetState extends InstantCommand { private final Spindexer spindexer; private SpindexerState state; - public SetSpindexerState(SpindexerState state) { + public SpindexerSetState(SpindexerState state) { this.spindexer = Spindexer.getInstance(); this.state = state; addRequirements(spindexer); diff --git a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerStop.java b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerStop.java index cf39063f..362be749 100644 --- a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerStop.java +++ b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerStop.java @@ -7,7 +7,7 @@ import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; -public class SpindexerStop extends SetSpindexerState { +public class SpindexerStop extends SpindexerSetState { public SpindexerStop() { super(SpindexerState.STOP); } diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/HoodAnalog.java b/src/main/java/com/stuypulse/robot/commands/superstructure/HoodAnalog.java deleted file mode 100644 index dab637ea..00000000 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/HoodAnalog.java +++ /dev/null @@ -1,29 +0,0 @@ -package com.stuypulse.robot.commands.superstructure; - -import com.stuypulse.robot.subsystems.hoodedshooter.hood.Hood; -import com.stuypulse.stuylib.input.Gamepad; - -import edu.wpi.first.wpilibj2.command.Command; - -public class HoodAnalog extends Command{ - private final Hood hood; - private final Gamepad driver; - - public HoodAnalog(Gamepad driver) { - this.driver = driver; - hood = Hood.getInstance(); - - addRequirements(hood); - } - - @Override - public void initialize() { - hood.setState(Hood.HoodState.ANALOG); - } - - @Override - public void execute() { - hood.hoodAnalogToInput(driver); - //SmartDashboard.putNumber("HoodedShooter/Hood/Analog", hood.hoodAnalogToOutput().getDegrees()); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterShoot.java b/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterShoot.java deleted file mode 100644 index 79d87ce7..00000000 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterShoot.java +++ /dev/null @@ -1,14 +0,0 @@ -/************************ 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.commands.superstructure; - -import com.stuypulse.robot.subsystems.Superstructure.Superstructure.SuperstructureState; - -public class SuperstructureShoot extends SuperstructureSetState { - public SuperstructureShoot() { - super(SuperstructureState.SHOOT); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterFerry.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureFerry.java similarity index 62% rename from src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterFerry.java rename to src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureFerry.java index 37f9acc5..ef06d38b 100644 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureFerry.java @@ -5,10 +5,11 @@ /***************************************************************/ package com.stuypulse.robot.commands.superstructure; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -public class HoodedShooterFerry extends HoodedShooterSetState { - public HoodedShooterFerry(){ +public class SuperstructureFerry extends SuperstructureSetState { + public SuperstructureFerry(){ super(SuperstructureState.FERRY); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterInterpolation.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureInterpolation.java similarity index 68% rename from src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterInterpolation.java rename to src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureInterpolation.java index dbf694f0..d9f7dffe 100644 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterInterpolation.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureInterpolation.java @@ -5,10 +5,10 @@ /***************************************************************/ package com.stuypulse.robot.commands.superstructure; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -public class HoodedShooterInterpolation extends HoodedShooterSetState { - public HoodedShooterInterpolation() { +public class SuperstructureInterpolation extends SuperstructureSetState { + public SuperstructureInterpolation() { super(SuperstructureState.INTERPOLATION); } } diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterKB.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureKB.java similarity index 70% rename from src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterKB.java rename to src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureKB.java index ef352e6c..fe5ad13c 100644 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterKB.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureKB.java @@ -5,10 +5,10 @@ /***************************************************************/ package com.stuypulse.robot.commands.superstructure; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -public class HoodedShooterKB extends HoodedShooterSetState { - public HoodedShooterKB() { +public class SuperstructureKB extends SuperstructureSetState { + public SuperstructureKB() { super(SuperstructureState.KB); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureLeftCorner.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureLeftCorner.java index ba823f11..6fb36bde 100644 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureLeftCorner.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureLeftCorner.java @@ -5,7 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.commands.superstructure; -import com.stuypulse.robot.subsystems.superstructure.SuperstructureState; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; public class SuperstructureLeftCorner extends SuperstructureSetState { public SuperstructureLeftCorner() { diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterReverse.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureReverse.java similarity index 69% rename from src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterReverse.java rename to src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureReverse.java index 6c6eb9b6..b64d15a4 100644 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterReverse.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureReverse.java @@ -5,10 +5,10 @@ /***************************************************************/ package com.stuypulse.robot.commands.superstructure; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -public class HoodedShooterReverse extends HoodedShooterSetState { - public HoodedShooterReverse() { +public class SuperstructureReverse extends SuperstructureSetState { + public SuperstructureReverse() { super(SuperstructureState.REVERSE); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterRightCorner.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureRightCorner.java similarity index 68% rename from src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterRightCorner.java rename to src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureRightCorner.java index d3395896..a305a7fa 100644 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterRightCorner.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureRightCorner.java @@ -5,10 +5,10 @@ /***************************************************************/ package com.stuypulse.robot.commands.superstructure; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -public class HoodedShooterRightCorner extends HoodedShooterSetState { - public HoodedShooterRightCorner() { +public class SuperstructureRightCorner extends SuperstructureSetState { + public SuperstructureRightCorner() { super(SuperstructureState.RIGHT_CORNER); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSetState.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSetState.java index 5694f94c..35d44533 100644 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSetState.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSetState.java @@ -5,24 +5,24 @@ /***************************************************************/ package com.stuypulse.robot.commands.superstructure; -import com.stuypulse.robot.subsystems.Superstructure.Superstructure; -import com.stuypulse.robot.subsystems.Superstructure.Superstructure.SuperstructureState; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; import edu.wpi.first.wpilibj2.command.InstantCommand; public class SuperstructureSetState extends InstantCommand { - private final Superstructure Superstructure; + private final Superstructure superstructure; private final SuperstructureState state; public SuperstructureSetState(SuperstructureState state) { - Superstructure = Superstructure.getInstance(); + superstructure = Superstructure.getInstance(); this.state = state; - addRequirements(Superstructure); + addRequirements(superstructure); } @Override public void execute() { - Superstructure.setState(state); + superstructure.setState(state); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureShoot.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureShoot.java index 91add107..6afd299e 100644 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureShoot.java @@ -5,7 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.commands.superstructure; -import com.stuypulse.robot.subsystems.superstructure.SuperstructureState; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; public class SuperstructureShoot extends SuperstructureSetState { public SuperstructureShoot() { diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterStow.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureStow.java similarity index 70% rename from src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterStow.java rename to src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureStow.java index ea6ab0af..5838c414 100644 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/HoodedShooterStow.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureStow.java @@ -5,10 +5,10 @@ /***************************************************************/ package com.stuypulse.robot.commands.superstructure; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -public class HoodedShooterStow extends HoodedShooterSetState{ - public HoodedShooterStow(){ +public class SuperstructureStow extends SuperstructureSetState{ + public SuperstructureStow(){ super(SuperstructureState.STOW); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretHub.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretHub.java deleted file mode 100644 index 18a09bc2..00000000 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretHub.java +++ /dev/null @@ -1,14 +0,0 @@ -/************************ 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.commands.turret; - -import com.stuypulse.robot.subsystems.turret.Turret.TurretState; - -public class TurretHub extends TurretSetState { - public TurretHub() { - super(TurretState.HUB); - } -} \ No newline at end of file From 0f715258badaf8bce298ad2ba8d8a45cab56af3d Mon Sep 17 00:00:00 2001 From: Lucas Date: Wed, 4 Mar 2026 18:50:45 -0500 Subject: [PATCH 067/150] REFACTOR: Move hood, shooter, and turret subsystems into superstructure --- .../java/com/stuypulse/robot/RobotContainer.java | 6 +++--- .../stuypulse/robot/commands/hood/HoodAnalog.java | 2 +- .../robot/commands/hood/ZeroHoodAtLowerHardstop.java | 2 +- .../commands/swerve/SwerveDriveAlignTurretToHub.java | 2 +- .../robot/commands/turret/TurretAnalog.java | 5 ++--- .../robot/commands/turret/TurretDefaultCommand.java | 4 ++-- .../stuypulse/robot/commands/turret/TurretFerry.java | 2 +- .../stuypulse/robot/commands/turret/TurretIdle.java | 2 +- .../robot/commands/turret/TurretLeftCorner.java | 2 +- .../robot/commands/turret/TurretRightCorner.java | 2 +- .../stuypulse/robot/commands/turret/TurretSeed.java | 2 +- .../robot/commands/turret/TurretSetState.java | 4 ++-- .../stuypulse/robot/commands/turret/TurretShoot.java | 2 +- .../stuypulse/robot/commands/turret/ZeroTurret.java | 2 +- .../subsystems/superstructure/Superstructure.java | 12 ++++++------ .../subsystems/{ => superstructure}/hood/Hood.java | 2 +- .../{ => superstructure}/hood/HoodImpl.java | 2 +- .../{ => superstructure}/shooter/Shooter.java | 2 +- .../{ => superstructure}/shooter/ShooterImpl.java | 2 +- .../{ => superstructure}/shooter/ShooterSim.java | 2 +- .../{ => superstructure}/turret/Turret.java | 2 +- .../{ => superstructure}/turret/TurretImpl.java | 2 +- .../{ => superstructure}/turret/TurretSim.java | 2 +- .../subsystems/swerve/CommandSwerveDrivetrain.java | 2 +- 24 files changed, 34 insertions(+), 35 deletions(-) rename src/main/java/com/stuypulse/robot/subsystems/{ => superstructure}/hood/Hood.java (98%) rename src/main/java/com/stuypulse/robot/subsystems/{ => superstructure}/hood/HoodImpl.java (99%) rename src/main/java/com/stuypulse/robot/subsystems/{ => superstructure}/shooter/Shooter.java (97%) rename src/main/java/com/stuypulse/robot/subsystems/{ => superstructure}/shooter/ShooterImpl.java (99%) rename src/main/java/com/stuypulse/robot/subsystems/{ => superstructure}/shooter/ShooterSim.java (98%) rename src/main/java/com/stuypulse/robot/subsystems/{ => superstructure}/turret/Turret.java (98%) rename src/main/java/com/stuypulse/robot/subsystems/{ => superstructure}/turret/TurretImpl.java (99%) rename src/main/java/com/stuypulse/robot/subsystems/{ => superstructure}/turret/TurretSim.java (98%) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 37822084..9fd3321c 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -47,12 +47,12 @@ import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.hood.Hood; -import com.stuypulse.robot.subsystems.shooter.Shooter; +import com.stuypulse.robot.subsystems.superstructure.hood.Hood; +import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret; import com.stuypulse.robot.subsystems.intake.Intake; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.subsystems.turret.Turret; import com.stuypulse.robot.subsystems.vision.LimelightVision; import com.stuypulse.robot.util.PathUtil.AutonConfig; import com.stuypulse.stuylib.input.Gamepad; diff --git a/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java b/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java index 12f3d325..035c2748 100644 --- a/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java +++ b/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java @@ -1,6 +1,6 @@ package com.stuypulse.robot.commands.hood; -import com.stuypulse.robot.subsystems.hood.Hood; +import com.stuypulse.robot.subsystems.superstructure.hood.Hood; import com.stuypulse.stuylib.input.Gamepad; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/com/stuypulse/robot/commands/hood/ZeroHoodAtLowerHardstop.java b/src/main/java/com/stuypulse/robot/commands/hood/ZeroHoodAtLowerHardstop.java index defe431d..ef4fab1a 100644 --- a/src/main/java/com/stuypulse/robot/commands/hood/ZeroHoodAtLowerHardstop.java +++ b/src/main/java/com/stuypulse/robot/commands/hood/ZeroHoodAtLowerHardstop.java @@ -1,6 +1,6 @@ package com.stuypulse.robot.commands.hood; -import com.stuypulse.robot.subsystems.hood.Hood; +import com.stuypulse.robot.subsystems.superstructure.hood.Hood; import edu.wpi.first.wpilibj2.command.InstantCommand; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java index dd0e50fb..1abd0986 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java @@ -9,8 +9,8 @@ import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Gains.Swerve.Alignment; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.subsystems.turret.Turret; import com.stuypulse.stuylib.control.angle.AngleController; import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; import com.stuypulse.stuylib.math.Angle; diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretAnalog.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretAnalog.java index 7f63376b..62569756 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretAnalog.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretAnalog.java @@ -5,11 +5,10 @@ /***************************************************************/ package com.stuypulse.robot.commands.turret; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; import com.stuypulse.stuylib.input.Gamepad; -import com.stuypulse.robot.subsystems.turret.Turret; -import com.stuypulse.robot.subsystems.turret.Turret.TurretState; - import edu.wpi.first.wpilibj2.command.Command; public class TurretAnalog extends Command { diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java index 8f7c5886..11901db9 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java @@ -1,9 +1,9 @@ package com.stuypulse.robot.commands.turret; import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.subsystems.turret.Turret; -import com.stuypulse.robot.subsystems.turret.Turret.TurretState; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretFerry.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretFerry.java index 8fb76a39..2381eadb 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretFerry.java @@ -5,7 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.commands.turret; -import com.stuypulse.robot.subsystems.turret.Turret.TurretState; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; public class TurretFerry extends TurretSetState { public TurretFerry() { diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretIdle.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretIdle.java index f0997013..37039cb2 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretIdle.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretIdle.java @@ -5,7 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.commands.turret; -import com.stuypulse.robot.subsystems.turret.Turret.TurretState; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; public class TurretIdle extends TurretSetState { public TurretIdle() { diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretLeftCorner.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretLeftCorner.java index 20297313..0fb3fb67 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretLeftCorner.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretLeftCorner.java @@ -5,7 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.commands.turret; -import com.stuypulse.robot.subsystems.turret.Turret.TurretState; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; public class TurretLeftCorner extends TurretSetState { public TurretLeftCorner() { diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretRightCorner.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretRightCorner.java index 98b07bf3..5786fd60 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretRightCorner.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretRightCorner.java @@ -5,7 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.commands.turret; -import com.stuypulse.robot.subsystems.turret.Turret.TurretState; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; public class TurretRightCorner extends TurretSetState { public TurretRightCorner() { diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretSeed.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretSeed.java index 8833a4a2..0bcb7a57 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretSeed.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretSeed.java @@ -5,7 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.commands.turret; -import com.stuypulse.robot.subsystems.turret.Turret; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret; import edu.wpi.first.wpilibj2.command.InstantCommand; diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretSetState.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretSetState.java index 12197f7c..62bf7ddf 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretSetState.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretSetState.java @@ -5,8 +5,8 @@ /***************************************************************/ package com.stuypulse.robot.commands.turret; -import com.stuypulse.robot.subsystems.turret.Turret; -import com.stuypulse.robot.subsystems.turret.Turret.TurretState; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; import edu.wpi.first.wpilibj2.command.Command; // import edu.wpi.first.wpilibj2.command.InstantCommand; diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretShoot.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretShoot.java index e210b4a2..04ace480 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretShoot.java @@ -5,7 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.commands.turret; -import com.stuypulse.robot.subsystems.turret.Turret.TurretState; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; public class TurretShoot extends TurretSetState { public TurretShoot() { diff --git a/src/main/java/com/stuypulse/robot/commands/turret/ZeroTurret.java b/src/main/java/com/stuypulse/robot/commands/turret/ZeroTurret.java index 8afbe74e..cea47dca 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/ZeroTurret.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/ZeroTurret.java @@ -5,7 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.commands.turret; -import com.stuypulse.robot.subsystems.turret.Turret; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret; import edu.wpi.first.wpilibj2.command.InstantCommand; diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index 0ee83a95..ef3d3699 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -6,12 +6,12 @@ package com.stuypulse.robot.subsystems.superstructure; import com.stuypulse.robot.Robot; -import com.stuypulse.robot.subsystems.shooter.Shooter; -import com.stuypulse.robot.subsystems.shooter.Shooter.ShooterState; -import com.stuypulse.robot.subsystems.hood.Hood; -import com.stuypulse.robot.subsystems.hood.Hood.HoodState; -import com.stuypulse.robot.subsystems.turret.Turret; -import com.stuypulse.robot.subsystems.turret.Turret.TurretState; +import com.stuypulse.robot.subsystems.superstructure.hood.Hood; +import com.stuypulse.robot.subsystems.superstructure.hood.Hood.HoodState; +import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter; +import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter.ShooterState; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; diff --git a/src/main/java/com/stuypulse/robot/subsystems/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java similarity index 98% rename from src/main/java/com/stuypulse/robot/subsystems/hood/Hood.java rename to src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java index c0e08922..a65fc344 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java @@ -3,7 +3,7 @@ /* 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.subsystems.hood; +package com.stuypulse.robot.subsystems.superstructure.hood; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; diff --git a/src/main/java/com/stuypulse/robot/subsystems/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java similarity index 99% rename from src/main/java/com/stuypulse/robot/subsystems/hood/HoodImpl.java rename to src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index 2e9b14d8..b00cee37 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -3,7 +3,7 @@ /* 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.subsystems.hood; +package com.stuypulse.robot.subsystems.superstructure.hood; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Gains; diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java similarity index 97% rename from src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java rename to src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index 9622bcab..8a850a01 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java @@ -3,7 +3,7 @@ /* 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.subsystems.shooter; +package com.stuypulse.robot.subsystems.superstructure.shooter; import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java similarity index 99% rename from src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java rename to src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java index b5155f82..be6d6373 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -3,7 +3,7 @@ /* 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.subsystems.shooter; +package com.stuypulse.robot.subsystems.superstructure.shooter; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Gains; diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java similarity index 98% rename from src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java rename to src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java index 2e506a85..4814dbb0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java @@ -3,7 +3,7 @@ /* 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.subsystems.shooter; +package com.stuypulse.robot.subsystems.superstructure.shooter; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Settings; diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java similarity index 98% rename from src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java rename to src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java index 02a204f5..ec69fe92 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java @@ -3,7 +3,7 @@ /* 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.subsystems.turret; +package com.stuypulse.robot.subsystems.superstructure.turret; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.math.Vector2D; diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java similarity index 99% rename from src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java rename to src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index f30364cb..7e2ff40b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -3,7 +3,7 @@ /* 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.subsystems.turret; +package com.stuypulse.robot.subsystems.superstructure.turret; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Gains; diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java similarity index 98% rename from src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java rename to src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java index b6348f06..12bc7159 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java @@ -3,7 +3,7 @@ /* 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.subsystems.turret; +package com.stuypulse.robot.subsystems.superstructure.turret; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Settings; 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 568fa8fc..68293793 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -21,8 +21,8 @@ import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Gains; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret; import com.stuypulse.robot.subsystems.swerve.TunerConstants.TunerSwerveDrivetrain; -import com.stuypulse.robot.subsystems.turret.Turret; import com.stuypulse.stuylib.math.Angle; import com.stuypulse.stuylib.math.Vector2D; From 9fbad78434c37a52c810c3127b41c41487cb390d Mon Sep 17 00:00:00 2001 From: Lucas Date: Wed, 4 Mar 2026 19:06:11 -0500 Subject: [PATCH 068/150] REFACTOR: Rename all utils and constants from HoodedShooter to Superstructure --- .../com/stuypulse/robot/constants/Gains.java | 45 +++++---- .../com/stuypulse/robot/constants/Ports.java | 14 +-- .../stuypulse/robot/constants/Settings.java | 92 +++++++++---------- .../subsystems/superstructure/hood/Hood.java | 28 +++--- .../superstructure/hood/HoodImpl.java | 38 ++++---- .../superstructure/shooter/Shooter.java | 20 ++-- .../superstructure/shooter/ShooterImpl.java | 48 +++++----- .../superstructure/turret/Turret.java | 6 +- .../superstructure/turret/TurretImpl.java | 40 ++++---- .../HoodAngleCalculator.java | 10 +- .../ShotCalculator.java | 8 +- .../util/turret/TurretAngleCalculator.java | 8 +- 12 files changed, 178 insertions(+), 179 deletions(-) rename src/main/java/com/stuypulse/robot/util/{hoodedshooter => superstructure}/HoodAngleCalculator.java (94%) rename src/main/java/com/stuypulse/robot/util/{hoodedshooter => superstructure}/ShotCalculator.java (95%) diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 0f117934..fa35b768 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -29,7 +29,7 @@ public interface ClimberHopper { // double kA = 0.0; } - public interface HoodedShooter { + public interface Superstructure { public interface Shooter { SmartNumber kP = new SmartNumber("HoodedShooter/Shooter/Gains/kP", 0.45); SmartNumber kI = new SmartNumber("HoodedShooter/Shooter/Gains/kI", 0.0); @@ -58,6 +58,27 @@ public interface Hood { double kA = 0.0; } + public interface Turret { + public interface slot0 { + double kP = 1300.0; + double kI = 0.0; + double kD = 140.0; + + double kS = 0.23; // FOUND ON 2/25 PD 8 + double kV = 0.0; + double kA = 0.0; + } + + public interface slot1 { + double kP = 0.0; + double kI = 0.0; + double kD = 0.0; + + double kS = 0.0; + double kV = 0.0; + double kA = 0.0; + } + } } public interface Spindexer { @@ -94,28 +115,6 @@ public interface Handoff { double kA = 0.00284; } - public interface Turret { - public interface slot0 { - double kP = 1300.0; - double kI = 0.0; - double kD = 140.0; - - double kS = 0.23; // FOUND ON 2/25 PD 8 - double kV = 0.0; - double kA = 0.0; - } - - public interface slot1 { - double kP = 0.0; - double kI = 0.0; - double kD = 0.0; - - double kS = 0.0; - double kV = 0.0; - double kA = 0.0; - } - } - public interface Swerve { public interface Drive { double kP = 0.3838; diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 45c4722f..0874d3e3 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -24,7 +24,7 @@ public interface Handoff { int HANDOFF = 43; } - public interface HoodedShooter { + public interface Superstructure { public interface Hood { int MOTOR = 45; int THROUGHBORE_ENCODER = 44; @@ -34,6 +34,12 @@ public interface Shooter { int MOTOR_LEAD = 47; int MOTOR_FOLLOW = 46; } + + public interface Turret { + int MOTOR = 40; + int ENCODER17T = 42; + int ENCODER18T = 41; + } } public interface Intake { @@ -46,10 +52,4 @@ public interface Spindexer { int SPINDEXER_LEAD_MOTOR = 30; int SPINDEXER_FOLLOW_MOTOR = 31; } - - public interface Turret { - int MOTOR = 40; - int ENCODER17T = 42; - int ENCODER18T = 41; - } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index aaaf0711..d452a778 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -122,7 +122,7 @@ public interface Constants { } } - public interface HoodedShooter { + public interface Superstructure { public final double SHOOTER_TOLERANCE_RPM = 100.0; public final Rotation2d HOOD_TOLERANCE = Rotation2d.fromDegrees(0.5); @@ -208,7 +208,52 @@ public interface Angles { public final Rotation2d KB_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 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 Rotation2d TOLERANCE = Rotation2d.fromDegrees(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); + + double RESOLUTION_OF_ABSOLUTE_ENCODER = 0.1; + double WRAP_DEBOUNCE = 0.5; + Rotation2d MAX_THEORETICAL_ROTATION = Rotation2d.fromDegrees(612); + Rotation2d MIN_THEORETICAL_ROTATION = Rotation2d.fromDegrees(-612); + + public interface Constants { + public final double RANGE = 210.0; + + public final double SLOT_SWITCHING_THRESHOLD_ROT = .5; + + public final Transform2d TURRET_OFFSET = new Transform2d(Units.inchesToMeters(-4.0), Units.inchesToMeters(8.0), Rotation2d.kZero); + public final double TURRET_HEIGHT = Units.inchesToMeters(0.0); + + public final double GEAR_RATIO_MOTOR_TO_MECH = (60.0 / 9.0) * (95.0 / 12.0); //1425.0 / 36.0; + + public interface BigGear { + public final int TEETH = 95; + } + + 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 ShootOnTheFly { @@ -262,51 +307,6 @@ public interface Constraints { } } - 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 Rotation2d TOLERANCE = Rotation2d.fromDegrees(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); - - double RESOLUTION_OF_ABSOLUTE_ENCODER = 0.1; - double WRAP_DEBOUNCE = 0.5; - Rotation2d MAX_THEORETICAL_ROTATION = Rotation2d.fromDegrees(612); - Rotation2d MIN_THEORETICAL_ROTATION = Rotation2d.fromDegrees(-612); - - public interface Constants { - public final double RANGE = 210.0; - - public final double SLOT_SWITCHING_THRESHOLD_ROT = .5; - - public final Transform2d TURRET_OFFSET = new Transform2d(Units.inchesToMeters(-4.0), Units.inchesToMeters(8.0), Rotation2d.kZero); - public final double TURRET_HEIGHT = Units.inchesToMeters(0.0); - - public final double GEAR_RATIO_MOTOR_TO_MECH = (60.0 / 9.0) * (95.0 / 12.0); //1425.0 / 36.0; - - public interface BigGear { - public final int TEETH = 95; - } - - 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); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java index a65fc344..65f464c9 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java @@ -8,7 +8,7 @@ import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.util.hoodedshooter.HoodAngleCalculator; +import com.stuypulse.robot.util.superstructure.HoodAngleCalculator; import com.stuypulse.stuylib.input.Gamepad; import edu.wpi.first.math.geometry.Pose2d; @@ -58,12 +58,12 @@ public void setState(HoodState state){ public Rotation2d getTargetAngle() { return switch(state) { - case STOW -> Settings.HoodedShooter.Hood.Angles.MIN_ANGLE; + case STOW -> Settings.Superstructure.Hood.Angles.MIN_ANGLE; case FERRY -> Rotation2d.fromDegrees(30); - case SHOOT -> Rotation2d.fromDegrees(Settings.HoodedShooter.Hood.Angles.SHOOT_ANGLE.get()); - case KB -> Settings.HoodedShooter.Hood.Angles.KB_ANGLE; - case LEFT_CORNER -> Settings.HoodedShooter.Hood.Angles.LEFT_CORNER_ANGLE; - case RIGHT_CORNER -> Settings.HoodedShooter.Hood.Angles.RIGHT_CORNER_ANGLE; + case SHOOT -> Rotation2d.fromDegrees(Settings.Superstructure.Hood.Angles.SHOOT_ANGLE.get()); + case KB -> Settings.Superstructure.Hood.Angles.KB_ANGLE; + case LEFT_CORNER -> Settings.Superstructure.Hood.Angles.LEFT_CORNER_ANGLE; + case RIGHT_CORNER -> Settings.Superstructure.Hood.Angles.RIGHT_CORNER_ANGLE; case INTERPOLATION -> HoodAngleCalculator.interpolateHoodAngle().get(); case ANALOG -> hoodAnalogToOutput(); case IDLE -> getAngle(); @@ -72,14 +72,14 @@ public Rotation2d getTargetAngle() { public boolean atTolerance() { double error = getAngle().minus(getTargetAngle()).getRotations(); - return Math.abs(error) < Settings.HoodedShooter.HOOD_TOLERANCE.getRotations(); + return Math.abs(error) < Settings.Superstructure.HOOD_TOLERANCE.getRotations(); } public abstract Rotation2d getAngle(); public void hoodAnalogToInput(Gamepad gamepad) { - double hoodMin = Settings.HoodedShooter.Hood.Angles.MIN_ANGLE.getDegrees(); - double hoodMax = Settings.HoodedShooter.Hood.Angles.MAX_ANGLE.getDegrees(); + double hoodMin = Settings.Superstructure.Hood.Angles.MIN_ANGLE.getDegrees(); + double hoodMax = Settings.Superstructure.Hood.Angles.MAX_ANGLE.getDegrees(); this.driverInput = Rotation2d.fromDegrees(hoodMin + (gamepad.getLeftX() + 1.0) * ((hoodMax - hoodMin) / 2)); } @@ -111,14 +111,14 @@ public boolean isHoodUnderTrench() { @Override public void periodic() { - SmartDashboard.putString("HoodedShooter/Hood/State", state.name()); + SmartDashboard.putString("Superstructure/Hood/State", state.name()); SmartDashboard.putString("States/Hood", state.name()); - SmartDashboard.putNumber("HoodedShooter/Hood/Target Angle", getTargetAngle().getDegrees()); - SmartDashboard.putNumber("HoodedShooter/Hood/Current Angle", getAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/Hood/Target Angle", getTargetAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/Hood/Current Angle", getAngle().getDegrees()); - //SmartDashboard.putNumber("HoodedShooter/Hood/Analog Target Angle", hoodAnalogToOutput().getDegrees()); + //SmartDashboard.putNumber("Superstructure/Hood/Analog Target Angle", hoodAnalogToOutput().getDegrees()); - SmartDashboard.putBoolean("HoodedShooter/Hood/Under Trench", isHoodUnderTrench()); + SmartDashboard.putBoolean("Superstructure/Hood/Under Trench", isHoodUnderTrench()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index b00cee37..e572e1c5 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -50,24 +50,24 @@ public HoodImpl() { .withStatorCurrentLimitEnabled(false) .withRampRate(0.25) - .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) + .withPIDConstants(Gains.Superstructure.Hood.kP, Gains.Superstructure.Hood.kI, Gains.Superstructure.Hood.kD, 0) + .withFFConstants(Gains.Superstructure.Hood.kS, Gains.Superstructure.Hood.kV, Gains.Superstructure.Hood.kA, 0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 0) - .withSensorToMechanismRatio(Settings.HoodedShooter.Hood.GEAR_RATIO) + .withSensorToMechanismRatio(Settings.Superstructure.Hood.GEAR_RATIO) .withSoftLimits( true, true, - Settings.HoodedShooter.Hood.FORWARD_SOFT_LIMIT.getRotations(), - Settings.HoodedShooter.Hood.REVERSE_SOFT_LIMIT.getRotations()); + Settings.Superstructure.Hood.FORWARD_SOFT_LIMIT.getRotations(), + Settings.Superstructure.Hood.REVERSE_SOFT_LIMIT.getRotations()); hoodEncoderConfig = new Motors.CANCoderConfig() .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) .withAbsoluteSensorDiscontinuityPoint(1.0) - .withMagnetOffset(Settings.HoodedShooter.Hood.ENCODER_OFFSET.getRotations()); + .withMagnetOffset(Settings.Superstructure.Hood.ENCODER_OFFSET.getRotations()); - hoodMotor = new TalonFX(Ports.HoodedShooter.Hood.MOTOR, Ports.RIO); - hoodEncoder = new CANcoder(Ports.HoodedShooter.Hood.THROUGHBORE_ENCODER, Ports.RIO); + hoodMotor = new TalonFX(Ports.Superstructure.Hood.MOTOR, Ports.RIO); + hoodEncoder = new CANcoder(Ports.Superstructure.Hood.THROUGHBORE_ENCODER, Ports.RIO); hoodConfig.configure(hoodMotor); hoodEncoderConfig.configure(hoodEncoder); @@ -77,8 +77,8 @@ public HoodImpl() { voltageOverride = Optional.empty(); - isStalling = BStream.create(() -> hoodMotor.getSupplyCurrent().getValueAsDouble() > Settings.HoodedShooter.Hood.STALL_CURRENT_LIMIT) //TODO: update value in Settings after testing - .filtered(new BDebounce.Both(Settings.HoodedShooter.Hood.STALL_DEBOUNCE)); + isStalling = BStream.create(() -> hoodMotor.getSupplyCurrent().getValueAsDouble() > Settings.Superstructure.Hood.STALL_CURRENT_LIMIT) //TODO: update value in Settings after testing + .filtered(new BDebounce.Both(Settings.Superstructure.Hood.STALL_DEBOUNCE)); } @Override @@ -95,7 +95,7 @@ public void zeroHoodEncoderAtLowerHardstop() { double currentOffset = hoodEncoderConfig.getConfiguration().MagnetSensor.MagnetOffset; double positionWithCurrentOffset = hoodEncoder.getPosition().getValueAsDouble(); - double newOffset = -((positionWithCurrentOffset - currentOffset) - Settings.HoodedShooter.Hood.Angles.MIN_ANGLE.getRotations()); + double newOffset = -((positionWithCurrentOffset - currentOffset) - Settings.Superstructure.Hood.Angles.MIN_ANGLE.getRotations()); hoodEncoderConfig.withMagnetOffset(newOffset); @@ -118,7 +118,7 @@ public void seedHood() { } private double getAbsoluteHoodAngleDeg() { - return Settings.HoodedShooter.Hood.Angles.MIN_ANGLE.getDegrees() + hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.HoodedShooter.Hood.ENCODER_TO_MECH; + return Settings.Superstructure.Hood.Angles.MIN_ANGLE.getDegrees() + hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.Superstructure.Hood.ENCODER_TO_MECH; } @Override @@ -144,16 +144,16 @@ public void periodic() { } if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("HoodedShooter/Hood/Correct Hood Angle (deg)", getAbsoluteHoodAngleDeg()); + SmartDashboard.putNumber("Superstructure/Hood/Correct Hood Angle (deg)", getAbsoluteHoodAngleDeg()); - SmartDashboard.putNumber("HoodedShooter/Hood/Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("HoodedShooter/Hood/Supply Current", hoodMotor.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("HoodedShooter/Hood/Stator Current", hoodMotor.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Hood/Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Hood/Supply Current", hoodMotor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Hood/Stator Current", hoodMotor.getStatorCurrent().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("Superstructure/Hood/Closed Loop Error (deg)", hoodMotor.getClosedLoopError().getValueAsDouble() * 360.0); + SmartDashboard.putBoolean("Superstructure/Hood/Has Used Absolute Encoder", hasUsedAbsoluteEncoder); - SmartDashboard.putNumber("HoodedShooter/Hood/Raw Motor Encoder Value", hoodMotor.getPosition().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Hood/Raw Motor Encoder Value", hoodMotor.getPosition().getValueAsDouble()); SmartDashboard.putNumber("InterpolationTesting/Hood Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index 8a850a01..f7b079e8 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java @@ -7,7 +7,7 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.util.hoodedshooter.HoodAngleCalculator; +import com.stuypulse.robot.util.superstructure.HoodAngleCalculator; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -58,21 +58,21 @@ public double getTargetRPM() { case STOP -> 0; case SHOOT -> getShootRPM(); case FERRY -> HoodAngleCalculator.interpolateFerryingRPM().get(); - case REVERSE -> Settings.HoodedShooter.Shooter.RPMs.REVERSE; - case KB -> Settings.HoodedShooter.Shooter.RPMs.KB_RPM; - case LEFT_CORNER -> Settings.HoodedShooter.Shooter.RPMs.LEFT_CORNER_RPM; - case RIGHT_CORNER -> Settings.HoodedShooter.Shooter.RPMs.RIGHT_CORNER_RPM; + case REVERSE -> Settings.Superstructure.Shooter.RPMs.REVERSE; + case KB -> Settings.Superstructure.Shooter.RPMs.KB_RPM; + case LEFT_CORNER -> Settings.Superstructure.Shooter.RPMs.LEFT_CORNER_RPM; + case RIGHT_CORNER -> Settings.Superstructure.Shooter.RPMs.RIGHT_CORNER_RPM; case INTERPOLATION -> HoodAngleCalculator.interpolateShooterRPM().get(); }; } public double getShootRPM() { - return Settings.HoodedShooter.Shooter.RPMs.SHOOT_RPM.get(); // Adjustable RPM on Glass + return Settings.Superstructure.Shooter.RPMs.SHOOT_RPM.get(); // Adjustable RPM on Glass } public boolean atTolerance() { double diff = Math.abs(getTargetRPM() - getRPM()); - return diff < Settings.HoodedShooter.SHOOTER_TOLERANCE_RPM; + return diff < Settings.Superstructure.SHOOTER_TOLERANCE_RPM; } public abstract double getRPM(); @@ -81,11 +81,11 @@ public boolean atTolerance() { @Override public void periodic() { - SmartDashboard.putString("HoodedShooter/Shooter/State", state.name()); + SmartDashboard.putString("Superstructure/Shooter/State", state.name()); SmartDashboard.putString("States/Shooter", state.name()); - SmartDashboard.putNumber("HoodedShooter/Shooter/Current RPM", getRPM()); - SmartDashboard.putNumber("HoodedShooter/Shooter/Target RPM", getTargetRPM()); + SmartDashboard.putNumber("Superstructure/Shooter/Current RPM", getRPM()); + SmartDashboard.putNumber("Superstructure/Shooter/Target RPM", getTargetRPM()); SmartDashboard.putNumber("InterpolationTesting/Shooter Interpolated Target Shoot RPM", HoodAngleCalculator.interpolateShooterRPM().get()); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java index be6d6373..0584ef22 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -42,19 +42,19 @@ public ShooterImpl() { .withSupplyCurrentLimitEnabled(false) .withStatorCurrentLimitEnabled(false) - .withPIDConstants(Gains.HoodedShooter.Shooter.kP.get(), Gains.HoodedShooter.Shooter.kI.get(), Gains.HoodedShooter.Shooter.kD.get(), 0) - .withFFConstants(Gains.HoodedShooter.Shooter.kS.get(), Gains.HoodedShooter.Shooter.kV.get(), Gains.HoodedShooter.Shooter.kA.get(), 0) + .withPIDConstants(Gains.Superstructure.Shooter.kP.get(), Gains.Superstructure.Shooter.kI.get(), Gains.Superstructure.Shooter.kD.get(), 0) + .withFFConstants(Gains.Superstructure.Shooter.kS.get(), Gains.Superstructure.Shooter.kV.get(), Gains.Superstructure.Shooter.kA.get(), 0) - .withSensorToMechanismRatio(Settings.HoodedShooter.Shooter.GEAR_RATIO); + .withSensorToMechanismRatio(Settings.Superstructure.Shooter.GEAR_RATIO); - shooterLeader = new TalonFX(Ports.HoodedShooter.Shooter.MOTOR_LEAD, Ports.RIO); - shooterFollower = new TalonFX(Ports.HoodedShooter.Shooter.MOTOR_FOLLOW, Ports.RIO); + shooterLeader = new TalonFX(Ports.Superstructure.Shooter.MOTOR_LEAD, Ports.RIO); + shooterFollower = new TalonFX(Ports.Superstructure.Shooter.MOTOR_FOLLOW, Ports.RIO); shooterConfig.configure(shooterLeader); shooterConfig.configure(shooterFollower); shooterController = new VelocityVoltage(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE).withEnableFOC(true); - follower = new Follower(Ports.HoodedShooter.Shooter.MOTOR_LEAD, MotorAlignmentValue.Opposed); + follower = new Follower(Ports.Superstructure.Shooter.MOTOR_LEAD, MotorAlignmentValue.Opposed); shooterFollower.setControl(follower); @@ -81,23 +81,23 @@ public void periodic() { shooterConfig.updateGainsConfig( shooterLeader, 0, - Gains.HoodedShooter.Shooter.kP, - Gains.HoodedShooter.Shooter.kI, - Gains.HoodedShooter.Shooter.kD, - Gains.HoodedShooter.Shooter.kS, - Gains.HoodedShooter.Shooter.kV, - Gains.HoodedShooter.Shooter.kA + Gains.Superstructure.Shooter.kP, + Gains.Superstructure.Shooter.kI, + Gains.Superstructure.Shooter.kD, + Gains.Superstructure.Shooter.kS, + Gains.Superstructure.Shooter.kV, + Gains.Superstructure.Shooter.kA ); shooterConfig.updateGainsConfig( shooterFollower, 0, - Gains.HoodedShooter.Shooter.kP, - Gains.HoodedShooter.Shooter.kI, - Gains.HoodedShooter.Shooter.kD, - Gains.HoodedShooter.Shooter.kS, - Gains.HoodedShooter.Shooter.kV, - Gains.HoodedShooter.Shooter.kA + Gains.Superstructure.Shooter.kP, + Gains.Superstructure.Shooter.kI, + Gains.Superstructure.Shooter.kD, + Gains.Superstructure.Shooter.kS, + Gains.Superstructure.Shooter.kV, + Gains.Superstructure.Shooter.kA ); if (EnabledSubsystems.SHOOTER.get()) { @@ -117,14 +117,14 @@ public void periodic() { } if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("HoodedShooter/Shooter/Leader Current (amps)", shooterLeader.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("HoodedShooter/Shooter/Follower Supply Current (amps)", shooterFollower.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("HoodedShooter/Shooter/Follower Stator Current", shooterFollower.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Shooter/Leader Current (amps)", shooterLeader.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Shooter/Follower Supply Current (amps)", shooterFollower.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Shooter/Follower Stator Current", shooterFollower.getStatorCurrent().getValueAsDouble()); - SmartDashboard.putNumber("HoodedShooter/Shooter/Leader Voltage", shooterLeader.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("HoodedShooter/Shooter/Follower Voltage", shooterFollower.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Shooter/Leader Voltage", shooterLeader.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Shooter/Follower Voltage", shooterFollower.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("HoodedShooter/Shooter/Follower RPM", getFollowerRPM()); + SmartDashboard.putNumber("Superstructure/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()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java index ec69fe92..12f68540 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java @@ -59,8 +59,8 @@ public Rotation2d getTargetAngle() { case ZERO -> Rotation2d.kZero; case SHOOT -> getScoringAngle(); case FERRY -> getFerryAngle(); - case LEFT_CORNER -> Settings.Turret.LEFT_CORNER; - case RIGHT_CORNER -> Settings.Turret.RIGHT_CORNER; + case LEFT_CORNER -> Settings.Superstructure.Turret.LEFT_CORNER; + case RIGHT_CORNER -> Settings.Superstructure.Turret.RIGHT_CORNER; case TESTING -> driverInputToAngle(); }; } @@ -72,7 +72,7 @@ public Rotation2d driverInputToAngle() { public boolean atTargetAngle() { double error = getAngle().minus(getTargetAngle()).getRotations(); - return Math.abs(error) < Settings.Turret.TOLERANCE.getRotations(); + return Math.abs(error) < Settings.Superstructure.Turret.TOLERANCE.getRotations(); } public Rotation2d getScoringAngle() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index 7e2ff40b..44bfb77a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -10,7 +10,7 @@ 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.constants.Settings.Superstructure.Turret; import com.stuypulse.robot.util.SysId; import com.stuypulse.robot.util.turret.TurretAngleCalculator; import edu.wpi.first.math.geometry.Rotation2d; @@ -49,20 +49,20 @@ public TurretImpl() { .withRampRate(0.25) .withVoltageLimits(6, -6) //TODO: VERIFY MAX VOLTAGE - .withPIDConstants(Gains.Turret.slot0.kP, Gains.Turret.slot0.kI, Gains.Turret.slot0.kD, 0) - .withFFConstants(Gains.Turret.slot0.kS, Gains.Turret.slot0.kV, Gains.Turret.slot0.kA, 0) + .withPIDConstants(Gains.Superstructure.Turret.slot0.kP, Gains.Superstructure.Turret.slot0.kI, Gains.Superstructure.Turret.slot0.kD, 0) + .withFFConstants(Gains.Superstructure.Turret.slot0.kS, Gains.Superstructure.Turret.slot0.kV, Gains.Superstructure.Turret.slot0.kA, 0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 0) - .withPIDConstants(Gains.Turret.slot1.kP, Gains.Turret.slot1.kI, Gains.Turret.slot1.kD, 1) - .withFFConstants(Gains.Turret.slot1.kS, Gains.Turret.slot1.kV, Gains.Turret.slot1.kA, 1) + .withPIDConstants(Gains.Superstructure.Turret.slot1.kP, Gains.Superstructure.Turret.slot1.kI, Gains.Superstructure.Turret.slot1.kD, 1) + .withFFConstants(Gains.Superstructure.Turret.slot1.kS, Gains.Superstructure.Turret.slot1.kV, Gains.Superstructure.Turret.slot1.kA, 1) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 1) - .withSensorToMechanismRatio(Settings.Turret.Constants.GEAR_RATIO_MOTOR_TO_MECH) + .withSensorToMechanismRatio(Settings.Superstructure.Turret.Constants.GEAR_RATIO_MOTOR_TO_MECH) .withSoftLimits( false, false, - Settings.Turret.Constants.SoftwareLimit.FORWARD_MAX_ROTATIONS, - Settings.Turret.Constants.SoftwareLimit.BACKWARDS_MAX_ROTATIONS); + Settings.Superstructure.Turret.Constants.SoftwareLimit.FORWARD_MAX_ROTATIONS, + Settings.Superstructure.Turret.Constants.SoftwareLimit.BACKWARDS_MAX_ROTATIONS); encoder17tConfig = new Motors.CANCoderConfig() .withSensorDirection(SensorDirectionValue.Clockwise_Positive) @@ -72,9 +72,9 @@ public TurretImpl() { .withSensorDirection(SensorDirectionValue.Clockwise_Positive) .withAbsoluteSensorDiscontinuityPoint(1.0); - motor = new TalonFX(Ports.Turret.MOTOR, Ports.RIO); - encoder17t = new CANcoder(Ports.Turret.ENCODER17T, Ports.RIO); - encoder18t = new CANcoder(Ports.Turret.ENCODER18T, Ports.RIO); + motor = new TalonFX(Ports.Superstructure.Turret.MOTOR, Ports.RIO); + encoder17t = new CANcoder(Ports.Superstructure.Turret.ENCODER17T, Ports.RIO); + encoder18t = new CANcoder(Ports.Superstructure.Turret.ENCODER18T, Ports.RIO); turretConfig.configure(motor); encoder17tConfig.configure(encoder17t); @@ -130,7 +130,7 @@ public Rotation2d getAngle() { @Override public boolean atTargetAngle() { double error = getAngle().minus(getTargetAngle()).getRotations() + 0.5; - return Math.abs(error) < Settings.Turret.TOLERANCE.getRotations(); + return Math.abs(error) < Settings.Superstructure.Turret.TOLERANCE.getRotations(); } private double getDelta(double target, double current) { @@ -168,16 +168,16 @@ public void periodic() { } if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("Turret/Relative Encoder Position (Rot)", motor.getPosition().getValueAsDouble() * 360.0); - SmartDashboard.putNumber("Turret/Closed Loop Error (deg)", motor.getClosedLoopError().getValueAsDouble() * 360.0); + SmartDashboard.putNumber("Superstructure/Turret/Relative Encoder Position (Rot)", motor.getPosition().getValueAsDouble() * 360.0); + SmartDashboard.putNumber("Superstructure/Turret/Closed Loop Error (deg)", motor.getClosedLoopError().getValueAsDouble() * 360.0); - SmartDashboard.putNumber("Turret/Encoder18t Abs Position (Rot)", encoder18t.getAbsolutePosition().getValueAsDouble()); - SmartDashboard.putNumber("Turret/Encoder17t Abs Position (Rot)", encoder17t.getAbsolutePosition().getValueAsDouble()); - SmartDashboard.putNumber("Turret/Vector Space Position (Deg)", getVectorSpaceAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/Turret/Encoder18t Abs Position (Rot)", encoder18t.getAbsolutePosition().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Turret/Encoder17t Abs Position (Rot)", encoder17t.getAbsolutePosition().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Turret/Vector Space Position (Deg)", getVectorSpaceAngle().getDegrees()); - SmartDashboard.putNumber("Turret/Voltage", motor.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("Turret/Stator Current", motor.getStatorCurrent().getValueAsDouble()); - SmartDashboard.putNumber("Turret/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Turret/Voltage", motor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Turret/Stator Current", motor.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Turret/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Current Draws/Turret (amps)", motor.getSupplyCurrent().getValueAsDouble()); } diff --git a/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/HoodAngleCalculator.java similarity index 94% rename from src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java rename to src/main/java/com/stuypulse/robot/util/superstructure/HoodAngleCalculator.java index 0abb933f..6e28ebec 100644 --- a/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/HoodAngleCalculator.java @@ -3,15 +3,15 @@ /* 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; +package com.stuypulse.robot.util.superstructure; 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.constants.Settings.Superstructure.AngleInterpolation; +import com.stuypulse.robot.constants.Settings.Superstructure.FerryRPMInterpolation; +import com.stuypulse.robot.constants.Settings.Superstructure.RPMInterpolation; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.util.hoodedshooter.ShotCalculator.SOTMSolution; +import com.stuypulse.robot.util.superstructure.ShotCalculator.SOTMSolution; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/src/main/java/com/stuypulse/robot/util/hoodedshooter/ShotCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/ShotCalculator.java similarity index 95% rename from src/main/java/com/stuypulse/robot/util/hoodedshooter/ShotCalculator.java rename to src/main/java/com/stuypulse/robot/util/superstructure/ShotCalculator.java index 0d5930cd..043e0b68 100644 --- a/src/main/java/com/stuypulse/robot/util/hoodedshooter/ShotCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/ShotCalculator.java @@ -3,12 +3,12 @@ /* 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; +package com.stuypulse.robot.util.superstructure; 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 com.stuypulse.robot.constants.Settings.Superstructure.AngleInterpolation; +import com.stuypulse.robot.constants.Settings.Superstructure.RPMInterpolation; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -53,7 +53,7 @@ public static StationarySolution solveInterpolation(Pose2d turretPose, Pose2d ta double targetRPM = distanceRPMInterpolator.get(distanceMeters); // Physics-based TOF - double launchSpeed = 0.5 * targetRPM * (2 * Math.PI / 60.0) * Settings.HoodedShooter.Shooter.FLYWHEEL_RADIUS; + double launchSpeed = 0.5 * targetRPM * (2 * Math.PI / 60.0) * Settings.Superstructure.Shooter.FLYWHEEL_RADIUS; Rotation2d launchAngle = Rotation2d.kCCW_Pi_2.minus(targetHoodAngle); double v_x = launchSpeed * Math.cos(launchAngle.getRadians()); 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 263ae98e..ed1dd0a6 100644 --- a/src/main/java/com/stuypulse/robot/util/turret/TurretAngleCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/turret/TurretAngleCalculator.java @@ -11,9 +11,9 @@ 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 double MAX_ANGLE_DEGREES = Settings.Superstructure.Turret.MAX_THEORETICAL_ROTATION.getDegrees(); + private static final double MIN_ANGLE_DEGREES = Settings.Superstructure.Turret.MIN_THEORETICAL_ROTATION.getDegrees(); + private static final double RESOLUTION = Settings.Superstructure.Turret.RESOLUTION_OF_ABSOLUTE_ENCODER; private static final int NUM_POINTS = (int) ((MAX_ANGLE_DEGREES - MIN_ANGLE_DEGREES) / RESOLUTION); private static int leastDistanceIndex = 0; @@ -23,7 +23,7 @@ public class TurretAngleCalculator { private static double[] generateEncoderValues(int teeth) { double[] values = new double[NUM_POINTS]; - double gearRatio = 1.0 * Settings.Turret.Constants.BigGear.TEETH / teeth; + double gearRatio = 1.0 * Settings.Superstructure.Turret.Constants.BigGear.TEETH / teeth; int i = 0; for (double angle = MIN_ANGLE_DEGREES; angle < MAX_ANGLE_DEGREES; angle += RESOLUTION) { From 026d295566d37aca033cbbf6b4c5779bf672fbed Mon Sep 17 00:00:00 2001 From: Lucas Date: Wed, 4 Mar 2026 19:08:34 -0500 Subject: [PATCH 069/150] REFACTOR: Forgot to rename something --- .../robot/subsystems/swerve/CommandSwerveDrivetrain.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 68293793..05c155c9 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -449,7 +449,7 @@ public double[] getWheelDrivePositionsRadians(){ public void periodic() { Pose2d pose = getPose(); turretPose = new Pose2d( - pose.getTranslation().plus(Settings.Turret.Constants.TURRET_OFFSET.getTranslation().rotateBy(pose.getRotation())), + pose.getTranslation().plus(Settings.Superstructure.Turret.Constants.TURRET_OFFSET.getTranslation().rotateBy(pose.getRotation())), pose.getRotation().plus(Turret.getInstance().getAngle()) ); From 970738b0a81a143bacd8b67a08ab8288ee2e60cd Mon Sep 17 00:00:00 2001 From: Lucas Date: Wed, 4 Mar 2026 19:12:17 -0500 Subject: [PATCH 070/150] REFACTOR: Rename all SmartDashboard stuff from HoodedShooter to Superstructure --- .../com/stuypulse/robot/RobotContainer.java | 46 +++++++++---------- .../robot/commands/hood/HoodAnalog.java | 2 +- .../com/stuypulse/robot/constants/Gains.java | 12 ++--- .../stuypulse/robot/constants/Settings.java | 10 ++-- .../superstructure/shooter/ShooterSim.java | 6 +-- .../superstructure/HoodAngleCalculator.java | 14 +++--- 6 files changed, 45 insertions(+), 45 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 9fd3321c..bdddb7af 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -176,10 +176,10 @@ private void configureButtonBindings() { // driver.getBottomButton() // .onTrue(new SuperstructureFerry() // .alongWith(new TurretFerry()) - // .alongWith(new WaitUntilCommand(() -> hoodedShooter.bothAtTolerance())) - // .andThen(new HandoffRun().onlyIf(() -> hoodedShooter.bothAtTolerance()) + // .alongWith(new WaitUntilCommand(() -> superstructure.bothAtTolerance())) + // .andThen(new HandoffRun().onlyIf(() -> superstructure.bothAtTolerance()) // .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) - // .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && hoodedShooter.bothAtTolerance()))) + // .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && superstructure.bothAtTolerance()))) // ) // .onFalse(new SpindexerStop() // .alongWith(new SuperstructureStow()) @@ -199,14 +199,14 @@ private void configureButtonBindings() { driver.getLeftButton() .whileTrue( new SwerveXMode().alongWith( - new HoodedShooterLeftCorner().alongWith( + new SuperstructureLeftCorner().alongWith( new TurretLeftCorner()).alongWith( - new WaitUntilCommand(() -> hoodedShooter.isHoodAtTolerance())).alongWith( - new WaitUntilCommand(() -> hoodedShooter.isShooterAtTolerance())).alongWith( + new WaitUntilCommand(() -> superstructure.isHoodAtTolerance())).alongWith( + new WaitUntilCommand(() -> superstructure.isShooterAtTolerance())).alongWith( new WaitUntilCommand(() -> turret.atTargetAngle())).andThen( new SpindexerRun().alongWith(new HandoffRun())))) .onFalse( - new HoodedShooterStow().alongWith( + new SuperstructureStow().alongWith( new SpindexerRun().alongWith( new HandoffStop())) ); @@ -215,14 +215,14 @@ private void configureButtonBindings() { driver.getRightButton() .whileTrue( new SwerveXMode().alongWith( - new HoodedShooterRightCorner().alongWith( + new SuperstructureRightCorner().alongWith( new TurretRightCorner()).alongWith( - new WaitUntilCommand(() -> hoodedShooter.isHoodAtTolerance())).alongWith( - new WaitUntilCommand(() -> hoodedShooter.isShooterAtTolerance())).alongWith( + new WaitUntilCommand(() -> superstructure.isHoodAtTolerance())).alongWith( + new WaitUntilCommand(() -> superstructure.isShooterAtTolerance())).alongWith( new WaitUntilCommand(() -> turret.atTargetAngle())).andThen( new SpindexerRun().alongWith(new HandoffRun())))) .onFalse( - new HoodedShooterStow().alongWith( + new SuperstructureStow().alongWith( new SpindexerRun().alongWith( new HandoffStop())) ); @@ -231,14 +231,14 @@ private void configureButtonBindings() { driver.getBottomButton() .whileTrue( new SwerveXMode().alongWith( - new HoodedShooterShoot().alongWith( + new SuperstructureShoot().alongWith( new TurretShoot()).alongWith( - new WaitUntilCommand(() -> hoodedShooter.isHoodAtTolerance())).alongWith( - new WaitUntilCommand(() -> hoodedShooter.isShooterAtTolerance())).alongWith( + new WaitUntilCommand(() -> Superstructure.isHoodAtTolerance())).alongWith( + new WaitUntilCommand(() -> Superstructure.isShooterAtTolerance())).alongWith( new WaitUntilCommand(() -> turret.atTargetAngle())).andThen( new SpindexerRun().alongWith(new HandoffRun())))) .onFalse( - new HoodedShooterStow().alongWith( + new SuperstructureStow().alongWith( new SpindexerRun().alongWith( new HandoffStop())) ); @@ -270,14 +270,14 @@ private void configureButtonBindings() { driver.getDPadLeft() .whileTrue( new SwerveXMode().alongWith( - new HoodedShooterFerry().alongWith( + new SuperstructureFerry().alongWith( new TurretFerry()).alongWith( - new WaitUntilCommand(() -> hoodedShooter.isHoodAtTolerance())).alongWith( - new WaitUntilCommand(() -> hoodedShooter.isShooterAtTolerance())).alongWith( + new WaitUntilCommand(() -> superstructure.isHoodAtTolerance())).alongWith( + new WaitUntilCommand(() -> superstructure.isShooterAtTolerance())).alongWith( new WaitUntilCommand(() -> turret.atTargetAngle())).andThen( new SpindexerRun().alongWith(new HandoffRun())))) .onFalse( - new HoodedShooterStow().alongWith( + new SuperstructureStow().alongWith( new SpindexerRun().alongWith( new HandoffStop())) ); @@ -286,14 +286,14 @@ private void configureButtonBindings() { driver.getDPadRight() .whileTrue( new SwerveXMode().alongWith( - new HoodedShooterShoot().alongWith( + new SuperstructureShoot().alongWith( new TurretShoot()).alongWith( - new WaitUntilCommand(() -> hoodedShooter.isHoodAtTolerance())).alongWith( - new WaitUntilCommand(() -> hoodedShooter.isShooterAtTolerance())).alongWith( + new WaitUntilCommand(() -> superstructure.isHoodAtTolerance())).alongWith( + new WaitUntilCommand(() -> superstructure.isShooterAtTolerance())).alongWith( new WaitUntilCommand(() -> turret.atTargetAngle())).andThen( new SpindexerRun().alongWith(new HandoffRun())))) .onFalse( - new HoodedShooterStow().alongWith( + new SuperstructureStow().alongWith( new SpindexerRun().alongWith( new HandoffStop())) ); diff --git a/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java b/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java index 035c2748..d0bcf8b2 100644 --- a/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java +++ b/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java @@ -24,6 +24,6 @@ public void initialize() { @Override public void execute() { hood.hoodAnalogToInput(driver); - //SmartDashboard.putNumber("HoodedShooter/Hood/Analog", hood.hoodAnalogToOutput().getDegrees()); + //SmartDashboard.putNumber("Superstructure/Hood/Analog", hood.hoodAnalogToOutput().getDegrees()); } } diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index fa35b768..8e20894e 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -31,13 +31,13 @@ public interface ClimberHopper { public interface Superstructure { public interface Shooter { - SmartNumber kP = new SmartNumber("HoodedShooter/Shooter/Gains/kP", 0.45); - SmartNumber kI = new SmartNumber("HoodedShooter/Shooter/Gains/kI", 0.0); - SmartNumber kD = new SmartNumber("HoodedShooter/Shooter/Gains/kD", 0.0); + SmartNumber kP = new SmartNumber("Superstructure/Shooter/Gains/kP", 0.45); + SmartNumber kI = new SmartNumber("Superstructure/Shooter/Gains/kI", 0.0); + SmartNumber kD = new SmartNumber("Superstructure/Shooter/Gains/kD", 0.0); - SmartNumber kS = new SmartNumber("HoodedShooter/Shooter/Gains/kS", 0.0); - SmartNumber kV = new SmartNumber("HoodedShooter/Shooter/Gains/kV", 0.123); - SmartNumber kA = new SmartNumber("HoodedShooter/Shooter/Gains/kA", 0.0); + SmartNumber kS = new SmartNumber("Superstructure/Shooter/Gains/kS", 0.0); + SmartNumber kV = new SmartNumber("Superstructure/Shooter/Gains/kV", 0.123); + SmartNumber kA = new SmartNumber("Superstructure/Shooter/Gains/kA", 0.0); // double kP = 0.45; // double kI = 0.0; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index d452a778..060c1ffa 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -163,8 +163,8 @@ public interface Shooter { public final double FLYWHEEL_RADIUS = Units.inchesToMeters(3.965 / 2); public interface RPMs { - public final SmartNumber SHOOT_RPM = new SmartNumber("HoodedShooter/Shoot State Target RPM", 3500.0); - public final SmartNumber FERRY_RPM = new SmartNumber("HoodedShooter/Ferry State Target RPM", 2000.0); + public final SmartNumber SHOOT_RPM = new SmartNumber("Superstructure/Shoot State Target RPM", 3500.0); + public final SmartNumber FERRY_RPM = new SmartNumber("Superstructure/Ferry State Target RPM", 2000.0); public final double REVERSE = 0.0; public final double KB_RPM = 0.0; public final double LEFT_CORNER_RPM = 0.0; @@ -198,8 +198,8 @@ public interface Hood { public final double STALL_DEBOUNCE = 0.5; 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 SmartNumber SHOOT_ANGLE = new SmartNumber("Superstructure/Shoot State Target Angle (deg)", 15.0); + public final SmartNumber FERRY_ANGLE = new SmartNumber("Superstructure/Ferry State Target Angle (deg)", 20.0); public final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(7.0); public final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(40.0); @@ -259,7 +259,7 @@ public interface SoftwareLimit { 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("Superstructure/SOTM/Update Delay", 0.00); } public interface Swerve { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java index 4814dbb0..1be77039 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java @@ -75,14 +75,14 @@ public void periodic() { if (EnabledSubsystems.SHOOTER.get()) { if (voltageOverride.isPresent()) { sim.setInput(voltageOverride.get()); - SmartDashboard.putNumber("HoodedShooter/Shooter/Input Voltage", voltageOverride.get()); + SmartDashboard.putNumber("Superstructure/Shooter/Input Voltage", voltageOverride.get()); } else { - SmartDashboard.putNumber("HoodedShooter/Shooter/Input Voltage", controller.getU(0)); + SmartDashboard.putNumber("Superstructure/Shooter/Input Voltage", controller.getU(0)); sim.setInput(controller.getU(0)); } } else { sim.setInput(0); - SmartDashboard.putNumber("HoodedShooter/Shooter/Input Voltage", 0.0); + SmartDashboard.putNumber("Superstructure/Shooter/Input Voltage", 0.0); } sim.update(Settings.DT); diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/HoodAngleCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/HoodAngleCalculator.java index 6e28ebec..864e6f25 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/HoodAngleCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/HoodAngleCalculator.java @@ -68,7 +68,7 @@ public static Supplier interpolateHoodAngle() { Rotation2d targetAngle = Rotation2d.fromRadians(distanceAngleInterpolator.get(distanceMeters)); - SmartDashboard.putNumber("HoodedShooter/Interpolated Target Angle", targetAngle.getDegrees()); + SmartDashboard.putNumber("Superstructure/Interpolated Target Angle", targetAngle.getDegrees()); return targetAngle; }; @@ -83,7 +83,7 @@ public static Supplier interpolateShooterRPM() { double targetRPM = ShotCalculator.solveInterpolation(turretPose, hubPose).targetRPM(); - SmartDashboard.putNumber("HoodedShooter/Interpolated RPM", targetRPM); + SmartDashboard.putNumber("Superstructure/Interpolated RPM", targetRPM); return targetRPM; }; @@ -100,7 +100,7 @@ public static Supplier interpolateFerryingRPM() { double targetRPM = ferryingDistanceRPMInterpolator.get(distanceMeters); - SmartDashboard.putNumber("HoodedShooter/Interpolated Ferrying RPM", targetRPM); + SmartDashboard.putNumber("Superstructure/Interpolated Ferrying RPM", targetRPM); return targetRPM; }; @@ -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("Superstructure/SOTM/Calculated Turret Angle", sol.targetTurretAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/SOTM/Calculated Hood Angle", sol.targetHoodAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/SOTM/Calculated Flight time", sol.flightTime()); + SmartDashboard.putNumber("Superstructure/SOTM/Turret Dist to Virtual Pose", futureTurretPose.getTranslation().getDistance(sol.virtualPose().getTranslation())); } public static Supplier calculateHoodAngleSOTM() { From 66787d9f26c9bf3701eec327d73e5885fde215e5 Mon Sep 17 00:00:00 2001 From: Lucas Date: Wed, 4 Mar 2026 19:23:48 -0500 Subject: [PATCH 071/150] FIX: Remove bad import --- .vscode/settings.json | 3 ++- .../robot/subsystems/superstructure/turret/TurretImpl.java | 3 +-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 565b0808..25918d97 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -26,5 +26,6 @@ }, ], "java.test.defaultConfig": "WPIlibUnitTests", - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable", + "java.debug.settings.onBuildFailureProceed": true } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index 44bfb77a..8f26814f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -10,7 +10,6 @@ import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Settings.Superstructure.Turret; import com.stuypulse.robot.util.SysId; import com.stuypulse.robot.util.turret.TurretAngleCalculator; import edu.wpi.first.math.geometry.Rotation2d; @@ -139,7 +138,7 @@ private double getDelta(double target, double current) { if (delta > 180.0) delta -= 360; else if (delta < -180) delta += 360; - if (Math.abs(current + delta) < Constants.RANGE) return delta; + if (Math.abs(current + delta) < Settings.Superstructure.Turret.Constants.RANGE) return delta; return delta < 0 ? delta + 360 : delta - 360; } From 026c0f0c65357036b6aec07245d9fb1937ca9dd3 Mon Sep 17 00:00:00 2001 From: Brian Date: Wed, 4 Mar 2026 22:52:29 -0500 Subject: [PATCH 072/150] FEAT: Port over alpha bot SOTM --- .../com/stuypulse/robot/RobotContainer.java | 18 +- .../superstructure/SuperstructureSOTM.java | 14 ++ .../com/stuypulse/robot/constants/Field.java | 1 + .../stuypulse/robot/constants/Settings.java | 25 ++- .../superstructure/Superstructure.java | 10 +- .../subsystems/superstructure/hood/Hood.java | 7 +- .../superstructure/shooter/Shooter.java | 13 +- .../superstructure/turret/Turret.java | 4 + .../superstructure/HoodAngleCalculator.java | 160 ------------------ .../InterpolationCalculator.java | 101 +++++++++++ .../SOTMSolutionCalculator.java | 125 ++++++++++++++ .../util/superstructure/ShotCalculator.java | 128 +++++--------- 12 files changed, 342 insertions(+), 264 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTM.java delete mode 100644 src/main/java/com/stuypulse/robot/util/superstructure/HoodAngleCalculator.java create mode 100644 src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java create mode 100644 src/main/java/com/stuypulse/robot/util/superstructure/SOTMSolutionCalculator.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index bdddb7af..6d34795e 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -32,6 +32,7 @@ import com.stuypulse.robot.commands.superstructure.SuperstructureFerry; import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureKB; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; import com.stuypulse.robot.commands.superstructure.SuperstructureShoot; import com.stuypulse.robot.commands.superstructure.SuperstructureStow; import com.stuypulse.robot.commands.swerve.SwerveDriveAlignTurretToHub; @@ -161,10 +162,21 @@ private void configureButtonBindings() { .onFalse(new SuperstructureStow()); + // driver.getRightButton() + // .whileTrue(new SuperstructureFerry().onlyIf( + // () -> CommandSwerveDrivetrain.getInstance().getPose().getX() > Field.getHubPose().getX())) + // .onFalse(new SuperstructureStow()); + + // SOTM driver.getRightButton() - .whileTrue(new SuperstructureFerry().onlyIf( - () -> CommandSwerveDrivetrain.getInstance().getPose().getX() > Field.getHubPose().getX())) - .onFalse(new SuperstructureStow()); + .whileTrue(new SuperstructureSOTM().onlyIf(() -> !superstructure.isHoodUnderTrench()) + .andThen(new WaitUntilCommand(superstructure::atTolerance)) + .andThen(new HandoffRun().onlyIf(superstructure::atTolerance) + .alongWith(new WaitUntilCommand(handoff::atTolerance)) + .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) + .onFalse(new SpindexerStop() + .alongWith(new SuperstructureStow()) + .alongWith(new HandoffStop())); // Reset Heading driver.getDPadUp() diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTM.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTM.java new file mode 100644 index 00000000..786c27cf --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTM.java @@ -0,0 +1,14 @@ +/************************ 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.commands.superstructure; + +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; + +public class SuperstructureSOTM extends SuperstructureSetState { + public SuperstructureSOTM() { + super(SuperstructureState.SOTM); + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 7804a00d..fcfe352e 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -36,6 +36,7 @@ public interface Field { // Alliance relative hub center coordinates public static final Pose2d hubCenter = new Pose2d(Units.inchesToMeters(182.11), WIDTH / 2.0, new Rotation2d()); public static final Pose3d hubCenter3d = new Pose3d(hubCenter.getX(), hubCenter.getY(), Units.inchesToMeters(72), Rotation3d.kZero); + public static final double HUB_RADIUS = Units.inchesToMeters(41.7 / 2); public static Pose2d getHubPose() { return hubCenter; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 060c1ffa..109b8102 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -152,6 +152,19 @@ public interface RPMInterpolation { }; } + public interface TOFInterpolation{ + public final double[][] distanceTOFInterpolationValues = { // COLLECT THESE + // {1.22, 0.0}, + {1.30, 1.01}, // seconds + // {1.43, 0.0}, + // {2.15, 0.0}, + {2.864967, 1.1}, + // {3.65, 0.0}, + {4.43, 1.234}, + {5.32, 1.267} + }; + } + public interface FerryRPMInterpolation { public final double[][] distanceRPMInterpolationValues = { {3.79, 3450.0} @@ -255,12 +268,14 @@ public interface SoftwareLimit { } } } + + public interface SOTM { + public final int MAX_ITERATIONS = 5; + public final double TIME_TOLERANCE = 0.01; + public final SmartNumber UPDATE_DELAY = new SmartNumber("Superstructure/SOTM/Update Delay", 0.00); + } } - public interface ShootOnTheFly { - public final int MAX_ITERATIONS = 5; - public final double TIME_TOLERANCE = 0.01; - public final SmartNumber UPDATE_DELAY = new SmartNumber("Superstructure/SOTM/Update Delay", 0.00); - } + public interface Swerve { public final double MODULE_VELOCITY_DEADBAND_M_PER_S = 0.1; diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index ef3d3699..1cb11144 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -12,6 +12,7 @@ import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter.ShooterState; import com.stuypulse.robot.subsystems.superstructure.turret.Turret; import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; +import com.stuypulse.robot.util.superstructure.SOTMSolutionCalculator; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -50,11 +51,12 @@ public enum SuperstructureState { KB(HoodState.KB, ShooterState.KB, TurretState.SHOOT), LEFT_CORNER(HoodState.LEFT_CORNER, ShooterState.LEFT_CORNER, TurretState.SHOOT), RIGHT_CORNER(HoodState.RIGHT_CORNER, ShooterState.RIGHT_CORNER, TurretState.SHOOT), - INTERPOLATION(HoodState.INTERPOLATION, ShooterState.INTERPOLATION, TurretState.SHOOT); + INTERPOLATION(HoodState.INTERPOLATION, ShooterState.INTERPOLATION, TurretState.SHOOT), + SOTM(HoodState.SOTM, ShooterState.SOTM, TurretState.SOTM); private HoodState hoodState; private ShooterState shooterState; - private TurretState turretState; + private TurretState turretState; private SuperstructureState(HoodState hoodState, ShooterState shooterState, TurretState TurretState) { this.hoodState = hoodState; @@ -132,6 +134,10 @@ public Rotation2d getTurretAngle(){ @Override public void periodic() { + if (getState() == SuperstructureState.SOTM) { + SOTMSolutionCalculator.updateSOTMSolution(); + } + SmartDashboard.putString("SuperStructure/State", state.name()); SmartDashboard.putString("States/SuperStructure", state.name()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java index 65f464c9..188a25b6 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java @@ -8,7 +8,8 @@ import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.util.superstructure.HoodAngleCalculator; +import com.stuypulse.robot.util.superstructure.SOTMSolutionCalculator; +import com.stuypulse.robot.util.superstructure.InterpolationCalculator; import com.stuypulse.stuylib.input.Gamepad; import edu.wpi.first.math.geometry.Pose2d; @@ -40,6 +41,7 @@ public enum HoodState { LEFT_CORNER, RIGHT_CORNER, INTERPOLATION, + SOTM, ANALOG, IDLE; } @@ -64,7 +66,8 @@ public Rotation2d getTargetAngle() { case KB -> Settings.Superstructure.Hood.Angles.KB_ANGLE; case LEFT_CORNER -> Settings.Superstructure.Hood.Angles.LEFT_CORNER_ANGLE; case RIGHT_CORNER -> Settings.Superstructure.Hood.Angles.RIGHT_CORNER_ANGLE; - case INTERPOLATION -> HoodAngleCalculator.interpolateHoodAngle().get(); + case INTERPOLATION -> InterpolationCalculator.interpolateShotInfo().targetHoodAngle(); + case SOTM -> SOTMSolutionCalculator.calculateHoodAngleSOTM().get(); case ANALOG -> hoodAnalogToOutput(); case IDLE -> getAngle(); }; diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index f7b079e8..75545920 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java @@ -7,7 +7,8 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.util.superstructure.HoodAngleCalculator; +import com.stuypulse.robot.util.superstructure.SOTMSolutionCalculator; +import com.stuypulse.robot.util.superstructure.InterpolationCalculator; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -38,7 +39,8 @@ public enum ShooterState { KB, LEFT_CORNER, RIGHT_CORNER, - INTERPOLATION; + INTERPOLATION, + SOTM; } public Shooter() { @@ -57,12 +59,13 @@ public double getTargetRPM() { return switch(state) { case STOP -> 0; case SHOOT -> getShootRPM(); - case FERRY -> HoodAngleCalculator.interpolateFerryingRPM().get(); + case FERRY -> InterpolationCalculator.interpolateFerryingRPM().get(); case REVERSE -> Settings.Superstructure.Shooter.RPMs.REVERSE; case KB -> Settings.Superstructure.Shooter.RPMs.KB_RPM; case LEFT_CORNER -> Settings.Superstructure.Shooter.RPMs.LEFT_CORNER_RPM; case RIGHT_CORNER -> Settings.Superstructure.Shooter.RPMs.RIGHT_CORNER_RPM; - case INTERPOLATION -> HoodAngleCalculator.interpolateShooterRPM().get(); + case INTERPOLATION -> InterpolationCalculator.interpolateShotInfo().targetRPM(); + case SOTM -> SOTMSolutionCalculator.calculateShooterRPMSOTM().get(); }; } @@ -87,6 +90,6 @@ public void periodic() { SmartDashboard.putNumber("Superstructure/Shooter/Current RPM", getRPM()); SmartDashboard.putNumber("Superstructure/Shooter/Target RPM", getTargetRPM()); - SmartDashboard.putNumber("InterpolationTesting/Shooter Interpolated Target Shoot RPM", HoodAngleCalculator.interpolateShooterRPM().get()); + SmartDashboard.putNumber("InterpolationTesting/Shooter Interpolated Target Shoot RPM", InterpolationCalculator.interpolateShotInfo().targetRPM()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java index 12f68540..3c517dac 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java @@ -10,9 +10,11 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.commands.hood.HoodAnalog; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.util.superstructure.SOTMSolutionCalculator; import com.stuypulse.robot.util.turret.TurretVisualizer; import edu.wpi.first.math.geometry.Pose2d; @@ -47,6 +49,7 @@ public enum TurretState { IDLE, ZERO, SHOOT, + SOTM, FERRY, LEFT_CORNER, RIGHT_CORNER, @@ -58,6 +61,7 @@ public Rotation2d getTargetAngle() { case IDLE -> getAngle(); case ZERO -> Rotation2d.kZero; case SHOOT -> getScoringAngle(); + case SOTM -> SOTMSolutionCalculator.calculateTurretAngleSOTM().get(); case FERRY -> getFerryAngle(); case LEFT_CORNER -> Settings.Superstructure.Turret.LEFT_CORNER; case RIGHT_CORNER -> Settings.Superstructure.Turret.RIGHT_CORNER; diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/HoodAngleCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/HoodAngleCalculator.java deleted file mode 100644 index 864e6f25..00000000 --- a/src/main/java/com/stuypulse/robot/util/superstructure/HoodAngleCalculator.java +++ /dev/null @@ -1,160 +0,0 @@ -/************************ 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.superstructure; -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Settings.Superstructure.AngleInterpolation; -import com.stuypulse.robot.constants.Settings.Superstructure.FerryRPMInterpolation; -import com.stuypulse.robot.constants.Settings.Superstructure.RPMInterpolation; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.util.superstructure.ShotCalculator.SOTMSolution; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -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; - - static { - distanceAngleInterpolator = new InterpolatingDoubleTreeMap(); - for (double[] pair : AngleInterpolation.distanceAngleInterpolationValues) { - distanceAngleInterpolator.put(pair[0], pair[1]); - } - } - - static { - distanceRPMInterpolator = new InterpolatingDoubleTreeMap(); - for (double[] pair : RPMInterpolation.distanceRPMInterpolationValues) { - distanceRPMInterpolator.put(pair[0], pair[1]); - } - } - - 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.getTurretPose().getTranslation(); - - double distanceMeters = hubPose.getDistance(currentPose); - - Rotation2d targetAngle = Rotation2d.fromRadians(distanceAngleInterpolator.get(distanceMeters)); - - SmartDashboard.putNumber("Superstructure/Interpolated Target Angle", targetAngle.getDegrees()); - - return targetAngle; - }; - } - - public static Supplier interpolateShooterRPM() { - return () -> { - CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); - - Pose2d hubPose = Field.getHubPose(); - Pose2d turretPose = swerve.getTurretPose(); - - double targetRPM = ShotCalculator.solveInterpolation(turretPose, hubPose).targetRPM(); - - SmartDashboard.putNumber("Superstructure/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("Superstructure/Interpolated Ferrying RPM", targetRPM); - - return targetRPM; - }; - } - - 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("Superstructure/SOTM/Calculated Turret Angle", sol.targetTurretAngle().getDegrees()); - SmartDashboard.putNumber("Superstructure/SOTM/Calculated Hood Angle", sol.targetHoodAngle().getDegrees()); - SmartDashboard.putNumber("Superstructure/SOTM/Calculated Flight time", sol.flightTime()); - SmartDashboard.putNumber("Superstructure/SOTM/Turret Dist to Virtual Pose", futureTurretPose.getTranslation().getDistance(sol.virtualPose().getTranslation())); - } - - public static Supplier calculateHoodAngleSOTM() { - return () -> sol.targetHoodAngle(); - } - - public static Supplier calculateTurretAngleSOTM() { - return () -> sol.targetTurretAngle(); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java new file mode 100644 index 00000000..244607e3 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java @@ -0,0 +1,101 @@ +package com.stuypulse.robot.util.superstructure; + +import java.util.function.Supplier; + +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings.Superstructure.AngleInterpolation; +import com.stuypulse.robot.constants.Settings.Superstructure.FerryRPMInterpolation; +import com.stuypulse.robot.constants.Settings.Superstructure.RPMInterpolation; +import com.stuypulse.robot.constants.Settings.Superstructure.TOFInterpolation; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class InterpolationCalculator { + + public static InterpolatingDoubleTreeMap distanceAngleInterpolator; + public static InterpolatingDoubleTreeMap distanceRPMInterpolator; + public static InterpolatingDoubleTreeMap distanceTOFInterpolator; + + public static InterpolatingDoubleTreeMap ferryingDistanceRPMInterpolator; + + static { + distanceAngleInterpolator = new InterpolatingDoubleTreeMap(); + for (double[] pair : AngleInterpolation.distanceAngleInterpolationValues) { + distanceAngleInterpolator.put(pair[0], pair[1]); + } + + distanceRPMInterpolator = new InterpolatingDoubleTreeMap(); + for (double[] pair : RPMInterpolation.distanceRPMInterpolationValues) { + distanceRPMInterpolator.put(pair[0], pair[1]); + } + + distanceTOFInterpolator = new InterpolatingDoubleTreeMap(); + for (double[] pair : TOFInterpolation.distanceTOFInterpolationValues) { + distanceTOFInterpolator.put(pair[0], pair[1]); + } + + ferryingDistanceRPMInterpolator = new InterpolatingDoubleTreeMap(); + for(double[] pair: FerryRPMInterpolation.distanceRPMInterpolationValues) { + ferryingDistanceRPMInterpolator.put(pair[0], pair[1]); + } + } + + + public record InterpolatedShotInfo( + Rotation2d targetHoodAngle, + double targetRPM, + double flightTimeSeconds) { + } + + public static InterpolatedShotInfo interpolateShotInfo(){ + return interpolateShotInfo(Field.getHubPose()); + } + + public static InterpolatedShotInfo interpolateShotInfo(Pose2d targetPose) { + CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); + + Translation2d hubPose = targetPose.getTranslation(); + Translation2d turretPose = swerve.getTurretPose().getTranslation(); + + double distanceMeters = turretPose.getDistance(hubPose); + + Rotation2d targetAngle = Rotation2d.fromRadians(distanceAngleInterpolator.get(distanceMeters)); + double targetRPM = distanceRPMInterpolator.get(distanceMeters); + double flightTime = distanceTOFInterpolator.get(distanceMeters); + + + SmartDashboard.putNumber("HoodedShooter/Interpolated Target Angle", targetAngle.getDegrees()); + SmartDashboard.putNumber("HoodedShooter/Interpolated RPM", targetRPM); + SmartDashboard.putNumber("HoodedShooter/Interpolated TOF", flightTime); + + return new InterpolatedShotInfo( + targetAngle, + targetRPM, + flightTime + ); + } + + 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; + }; + } + + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMSolutionCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMSolutionCalculator.java new file mode 100644 index 00000000..b1cf6164 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMSolutionCalculator.java @@ -0,0 +1,125 @@ +/************************ 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.superstructure; + +import java.util.function.Supplier; + +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.superstructure.hood.Hood; +import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret; +import com.stuypulse.robot.util.superstructure.ShotCalculator.SOTMSolution; +import com.stuypulse.stuylib.math.Vector2D; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + + +public class SOTMSolutionCalculator { + + public static SOTMSolution sol; + + private static FieldObject2d hubPose2d; + private static FieldObject2d virtualHubPose2d; + private static FieldObject2d futureTurretPose2d; + + + static { + sol = new SOTMSolution( + Hood.getInstance().getAngle(), + Turret.getInstance().getAngle(), + Shooter.getInstance().getRPM(), + Field.getHubPose(), + 0.0 + ); + + hubPose2d = Field.FIELD2D.getObject("hubPose"); + virtualHubPose2d = Field.FIELD2D.getObject("virtualHubPose"); + futureTurretPose2d = Field.FIELD2D.getObject("futureTurretPose"); + } + + + 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.Superstructure.SOTM.UPDATE_DELAY.doubleValue(), + robotRelativeSpeeds.vyMetersPerSecond * Settings.Superstructure.SOTM.UPDATE_DELAY.doubleValue(), + 0 + ) + ); + + Vector2D oppositeDirection = new Vector2D(new Translation2d( + -robotRelativeSpeeds.vxMetersPerSecond, + -robotRelativeSpeeds.vyMetersPerSecond + )); + + if (!oppositeDirection.equals(Vector2D.kOrigin)) { + oppositeDirection = oppositeDirection.normalize(); + } + + hubPose = hubPose.exp( + new Twist2d( + oppositeDirection.x * Field.HUB_RADIUS, + oppositeDirection.y * Field.HUB_RADIUS, + 0 + ) + ); + + + SOTMSolution solution = ShotCalculator.solveShootOnTheMove( + futureTurretPose, + robotPose, + hubPose, + fieldRelativeSpeeds, + Settings.Superstructure.SOTM.MAX_ITERATIONS, + Settings.Superstructure.SOTM.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("Superstructure/SOTM/calculated turret angle", sol.targetTurretAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/SOTM/calculated hood angle", sol.targetHoodAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/SOTM/calculated flight time", sol.flightTime()); + SmartDashboard.putNumber("Superstructure/SOTM/turret dist to virtual pose", futureTurretPose.getTranslation().getDistance(sol.virtualPose().getTranslation())); + } + + public static Supplier calculateHoodAngleSOTM() { + return () -> sol.targetHoodAngle(); + } + + public static Supplier calculateTurretAngleSOTM() { + return () -> sol.targetTurretAngle(); + } + + public static Supplier calculateShooterRPMSOTM() { + return () -> sol.targetShooterRPM(); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/ShotCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/ShotCalculator.java index 043e0b68..519451b9 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/ShotCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/ShotCalculator.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. */ @@ -6,69 +6,22 @@ package com.stuypulse.robot.util.superstructure; import com.stuypulse.robot.Robot; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Settings.Superstructure.AngleInterpolation; -import com.stuypulse.robot.constants.Settings.Superstructure.RPMInterpolation; +import com.stuypulse.robot.util.superstructure.InterpolationCalculator.InterpolatedShotInfo; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public final class ShotCalculator { public static final double g = 9.81; - public static InterpolatingDoubleTreeMap distanceAngleInterpolator; - public static InterpolatingDoubleTreeMap distanceRPMInterpolator; - - static { - distanceAngleInterpolator = new InterpolatingDoubleTreeMap(); - for (double[] pair : AngleInterpolation.distanceAngleInterpolationValues) { - distanceAngleInterpolator.put(pair[0], pair[1]); - } - } - - static { - distanceRPMInterpolator = new InterpolatingDoubleTreeMap(); - for (double[] pair : RPMInterpolation.distanceRPMInterpolationValues) { - distanceRPMInterpolator.put(pair[0], pair[1]); - } - } - - public record StationarySolution( - Rotation2d targetHoodAngle, - double targetRPM, - double flightTimeSeconds) { - } - - public static StationarySolution solveInterpolation(Pose2d turretPose, Pose2d targetPose) { - - double distanceMeters = turretPose.getTranslation().getDistance(targetPose.getTranslation()); - - // Interpolated Angle - Rotation2d targetHoodAngle = Rotation2d.fromRadians(distanceAngleInterpolator.get(distanceMeters)); - - // Interpolated RPM - double targetRPM = distanceRPMInterpolator.get(distanceMeters); - - // Physics-based TOF - double launchSpeed = 0.5 * targetRPM * (2 * Math.PI / 60.0) * Settings.Superstructure.Shooter.FLYWHEEL_RADIUS; - Rotation2d launchAngle = Rotation2d.kCCW_Pi_2.minus(targetHoodAngle); - - double v_x = launchSpeed * Math.cos(launchAngle.getRadians()); - double flightTime = distanceMeters / v_x; - - return new StationarySolution( - targetHoodAngle, - targetRPM, - flightTime - ); - } public record SOTMSolution( Rotation2d targetHoodAngle, Rotation2d targetTurretAngle, + double targetShooterRPM, Pose2d virtualPose, double flightTime) { } @@ -82,37 +35,36 @@ public static SOTMSolution solveShootOnTheMove( double timeTolerance) { /* - * 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 - ); + 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. + */ + + + InterpolatedShotInfo sol = InterpolationCalculator.interpolateShotInfo(); + + double t_guess = sol.flightTimeSeconds(); Pose2d virtualPose = targetPose; @@ -120,6 +72,8 @@ public static SOTMSolution solveShootOnTheMove( for (int i = 0; i < maxIterations; i++) { + SmartDashboard.putNumber("Superstructure/SOTM/iteration #", i); + double dx = fieldRelativeSpeeds.vxMetersPerSecond * t_guess; double dy = fieldRelativeSpeeds.vyMetersPerSecond * t_guess; @@ -128,10 +82,8 @@ public static SOTMSolution solveShootOnTheMove( targetPose.getY() - dy, targetPose.getRotation()); - StationarySolution newSol = solveInterpolation( - turretPose, - virtualPose - ); + + InterpolatedShotInfo newSol = InterpolationCalculator.interpolateShotInfo(virtualPose); if (Math.abs(newSol.flightTimeSeconds() - t_guess) < timeTolerance) { break; @@ -140,6 +92,7 @@ public static SOTMSolution solveShootOnTheMove( t_guess = newSol.flightTimeSeconds(); sol = newSol; + } Translation2d virtualTranslation = virtualPose.getTranslation(); @@ -148,8 +101,8 @@ public static SOTMSolution solveShootOnTheMove( double yaw = Math.atan2( virtualTranslation.getY() - turretTranslation.getY(), virtualTranslation.getX() - turretTranslation.getX() - ); - + ); + Rotation2d targetTurretAngle = Robot.isReal() ? Rotation2d.fromRadians(-yaw).plus(robotPose.getRotation()) : Rotation2d.fromRadians(yaw).minus(robotPose.getRotation()); @@ -157,6 +110,7 @@ public static SOTMSolution solveShootOnTheMove( return new SOTMSolution( sol.targetHoodAngle(), targetTurretAngle, + sol.targetRPM(), virtualPose, sol.flightTimeSeconds() ); From 7c367727103b29e0f0683f54c31f476a27016622 Mon Sep 17 00:00:00 2001 From: Xuan Hao Date: Thu, 5 Mar 2026 00:07:51 -0500 Subject: [PATCH 073/150] REFACTOR: Unified all logging for the super structure CLEAN: Removed unnecessary imports for super structure files REFACTOR: Moved trench check method to super structure, turret angle calc method to util REFACTOR: Combined the SOTM Calculator files, and moved turret util to be part of super structure --- .../robot/commands/hood/HoodAnalog.java | 2 +- .../superstructure/SuperstructureFerry.java | 1 - .../swerve/SwerveDriveAlignTurretToHub.java | 3 +- .../com/stuypulse/robot/constants/Gains.java | 12 +- .../stuypulse/robot/constants/Settings.java | 12 +- .../superstructure/Superstructure.java | 23 +- .../subsystems/superstructure/hood/Hood.java | 32 +-- .../superstructure/hood/HoodImpl.java | 17 +- .../superstructure/shooter/Shooter.java | 10 +- .../superstructure/shooter/ShooterImpl.java | 12 +- .../superstructure/shooter/ShooterSim.java | 6 +- .../superstructure/turret/Turret.java | 47 ++-- .../superstructure/turret/TurretImpl.java | 19 +- .../superstructure/turret/TurretSim.java | 10 +- .../swerve/CommandSwerveDrivetrain.java | 2 +- .../InterpolationCalculator.java | 8 +- .../util/superstructure/SOTMCalculator.java | 219 ++++++++++++++++++ .../SOTMSolutionCalculator.java | 125 ---------- .../util/superstructure/ShotCalculator.java | 118 ---------- .../TurretAngleCalculator.java | 38 ++- .../TurretVisualizer.java | 2 +- 21 files changed, 356 insertions(+), 362 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java delete mode 100644 src/main/java/com/stuypulse/robot/util/superstructure/SOTMSolutionCalculator.java delete mode 100644 src/main/java/com/stuypulse/robot/util/superstructure/ShotCalculator.java rename src/main/java/com/stuypulse/robot/util/{turret => superstructure}/TurretAngleCalculator.java (60%) rename src/main/java/com/stuypulse/robot/util/{turret => superstructure}/TurretVisualizer.java (97%) diff --git a/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java b/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java index d0bcf8b2..73744cbb 100644 --- a/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java +++ b/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java @@ -24,6 +24,6 @@ public void initialize() { @Override public void execute() { hood.hoodAnalogToInput(driver); - //SmartDashboard.putNumber("Superstructure/Hood/Analog", hood.hoodAnalogToOutput().getDegrees()); + //SmartDashboard.putNumber("SuperStructure/Hood/Analog", hood.hoodAnalogToOutput().getDegrees()); } } diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureFerry.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureFerry.java index ef06d38b..bf05fdcd 100644 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureFerry.java @@ -5,7 +5,6 @@ /***************************************************************/ package com.stuypulse.robot.commands.superstructure; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; public class SuperstructureFerry extends SuperstructureSetState { diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java index 1abd0986..49c6758d 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java @@ -11,6 +11,7 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.superstructure.turret.Turret; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.util.superstructure.TurretAngleCalculator; import com.stuypulse.stuylib.control.angle.AngleController; import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; import com.stuypulse.stuylib.math.Angle; @@ -67,6 +68,6 @@ public void execute() { Angle.fromRotation2d(robot.getRotation())))); SmartDashboard.putNumber("Swerve/Angle Error", angleController.getError().toDegrees()); - SmartDashboard.putNumber("Swerve/Target Angle Hub Deg", turret.getPointAtTargetAngle(Field.getHubPose()).getDegrees()); + SmartDashboard.putNumber("Swerve/Target Angle Hub Deg", TurretAngleCalculator.getPointAtTargetAngle(Field.getHubPose().getTranslation(), swerve.getTurretPose().getTranslation()).getDegrees()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 8e20894e..1999bcdf 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -31,13 +31,13 @@ public interface ClimberHopper { public interface Superstructure { public interface Shooter { - SmartNumber kP = new SmartNumber("Superstructure/Shooter/Gains/kP", 0.45); - SmartNumber kI = new SmartNumber("Superstructure/Shooter/Gains/kI", 0.0); - SmartNumber kD = new SmartNumber("Superstructure/Shooter/Gains/kD", 0.0); + SmartNumber kP = new SmartNumber("SuperStructure/Shooter/Gains/kP", 0.45); + SmartNumber kI = new SmartNumber("SuperStructure/Shooter/Gains/kI", 0.0); + SmartNumber kD = new SmartNumber("SuperStructure/Shooter/Gains/kD", 0.0); - SmartNumber kS = new SmartNumber("Superstructure/Shooter/Gains/kS", 0.0); - SmartNumber kV = new SmartNumber("Superstructure/Shooter/Gains/kV", 0.123); - SmartNumber kA = new SmartNumber("Superstructure/Shooter/Gains/kA", 0.0); + SmartNumber kS = new SmartNumber("SuperStructure/Shooter/Gains/kS", 0.0); + SmartNumber kV = new SmartNumber("SuperStructure/Shooter/Gains/kV", 0.123); + SmartNumber kA = new SmartNumber("SuperStructure/Shooter/Gains/kA", 0.0); // double kP = 0.45; // double kI = 0.0; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 109b8102..fa93cdc1 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -176,8 +176,8 @@ public interface Shooter { public final double FLYWHEEL_RADIUS = Units.inchesToMeters(3.965 / 2); public interface RPMs { - public final SmartNumber SHOOT_RPM = new SmartNumber("Superstructure/Shoot State Target RPM", 3500.0); - public final SmartNumber FERRY_RPM = new SmartNumber("Superstructure/Ferry State Target RPM", 2000.0); + public final SmartNumber SHOOT_RPM = new SmartNumber("SuperStructure/Shoot State Target RPM", 3500.0); + public final SmartNumber FERRY_RPM = new SmartNumber("SuperStructure/Ferry State Target RPM", 2000.0); public final double REVERSE = 0.0; public final double KB_RPM = 0.0; public final double LEFT_CORNER_RPM = 0.0; @@ -211,8 +211,8 @@ public interface Hood { public final double STALL_DEBOUNCE = 0.5; public interface Angles { - public final SmartNumber SHOOT_ANGLE = new SmartNumber("Superstructure/Shoot State Target Angle (deg)", 15.0); - public final SmartNumber FERRY_ANGLE = new SmartNumber("Superstructure/Ferry State Target Angle (deg)", 20.0); + public final SmartNumber SHOOT_ANGLE = new SmartNumber("SuperStructure/Shoot State Target Angle (deg)", 15.0); + public final SmartNumber FERRY_ANGLE = new SmartNumber("SuperStructure/Ferry State Target Angle (deg)", 20.0); public final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(7.0); public final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(40.0); @@ -272,7 +272,7 @@ public interface SoftwareLimit { public interface SOTM { public final int MAX_ITERATIONS = 5; public final double TIME_TOLERANCE = 0.01; - public final SmartNumber UPDATE_DELAY = new SmartNumber("Superstructure/SOTM/Update Delay", 0.00); + public final SmartNumber UPDATE_DELAY = new SmartNumber("SuperStructure/SOTM/Update Delay", 0.00); } } @@ -308,7 +308,7 @@ public interface Tolerances { } public interface Constraints { - public final double MAX_VELOCITY_M_PER_S = 4.3; + public final double MAX_VELOCITY_M_PER_S = 1.0; // 4.3p 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); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index 1cb11144..3b5fc9da 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -5,15 +5,17 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure; -import com.stuypulse.robot.Robot; +import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.subsystems.superstructure.hood.Hood; import com.stuypulse.robot.subsystems.superstructure.hood.Hood.HoodState; import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter; import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter.ShooterState; import com.stuypulse.robot.subsystems.superstructure.turret.Turret; import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; -import com.stuypulse.robot.util.superstructure.SOTMSolutionCalculator; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.util.superstructure.SOTMCalculator; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -93,7 +95,19 @@ public boolean atTolerance() { } public boolean isHoodUnderTrench() { - return hood.isHoodUnderTrench(); + Pose2d pose = CommandSwerveDrivetrain.getInstance().getTurretPose(); + + 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 isCloseToAllianceSideTrenchX = Math.abs(pose.getX() - Field.NearRightTrench.rightEdge.getX()) < Field.trenchHoodTolerance; + + boolean isCloseToNeutralSideTrenchX = Math.abs(pose.getX() - Field.FarRightTrench.rightEdge.getX()) < Field.trenchHoodTolerance; + + boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) && (isCloseToAllianceSideTrenchX || isCloseToNeutralSideTrenchX); + + return isUnderTrench; } public boolean isShooterAtTolerance() { @@ -135,7 +149,7 @@ public Rotation2d getTurretAngle(){ @Override public void periodic() { if (getState() == SuperstructureState.SOTM) { - SOTMSolutionCalculator.updateSOTMSolution(); + SOTMCalculator.updateSOTMSolution(); } SmartDashboard.putString("SuperStructure/State", state.name()); @@ -155,6 +169,7 @@ public void periodic() { SmartDashboard.putBoolean("SuperStructure/Hood At Tolerance?", isHoodAtTolerance()); SmartDashboard.putBoolean("SuperStructure/Turret At Tolerant?", isHoodAtTolerance()); + SmartDashboard.putBoolean("SuperStructure/Hood/Under Trench", isHoodUnderTrench()); SmartDashboard.putNumber("InterpolationTesting/Hood Angle", getHoodAngle().getDegrees()); SmartDashboard.putNumber("InterpolationTesting/Shooter RPM", getShooterRPM()); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java index 188a25b6..3ceb123c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java @@ -5,14 +5,11 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure.hood; -import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.util.superstructure.SOTMSolutionCalculator; +import com.stuypulse.robot.util.superstructure.SOTMCalculator; import com.stuypulse.robot.util.superstructure.InterpolationCalculator; import com.stuypulse.stuylib.input.Gamepad; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -67,7 +64,7 @@ public Rotation2d getTargetAngle() { case LEFT_CORNER -> Settings.Superstructure.Hood.Angles.LEFT_CORNER_ANGLE; case RIGHT_CORNER -> Settings.Superstructure.Hood.Angles.RIGHT_CORNER_ANGLE; case INTERPOLATION -> InterpolationCalculator.interpolateShotInfo().targetHoodAngle(); - case SOTM -> SOTMSolutionCalculator.calculateHoodAngleSOTM().get(); + case SOTM -> SOTMCalculator.calculateHoodAngleSOTM(); case ANALOG -> hoodAnalogToOutput(); case IDLE -> getAngle(); }; @@ -90,22 +87,6 @@ public void hoodAnalogToInput(Gamepad gamepad) { public Rotation2d hoodAnalogToOutput() { return this.driverInput; } - - public boolean isHoodUnderTrench() { - Pose2d pose = CommandSwerveDrivetrain.getInstance().getTurretPose(); - - 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 isCloseToAllianceSideTrenchX = Math.abs(pose.getX() - Field.NearRightTrench.rightEdge.getX()) < Field.trenchHoodTolerance; - - boolean isCloseToNeutralSideTrenchX = Math.abs(pose.getX() - Field.FarRightTrench.rightEdge.getX()) < Field.trenchHoodTolerance; - - boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) && (isCloseToAllianceSideTrenchX || isCloseToNeutralSideTrenchX); - - return isUnderTrench; - } public abstract boolean isStalling(); public abstract SysIdRoutine getHoodSysIdRoutine(); @@ -114,14 +95,13 @@ public boolean isHoodUnderTrench() { @Override public void periodic() { - SmartDashboard.putString("Superstructure/Hood/State", state.name()); + SmartDashboard.putString("SuperStructure/Hood/State", state.name()); SmartDashboard.putString("States/Hood", state.name()); - SmartDashboard.putNumber("Superstructure/Hood/Target Angle", getTargetAngle().getDegrees()); - SmartDashboard.putNumber("Superstructure/Hood/Current Angle", getAngle().getDegrees()); + SmartDashboard.putNumber("SuperStructure/Hood/Target Angle", getTargetAngle().getDegrees()); + SmartDashboard.putNumber("SuperStructure/Hood/Current Angle", getAngle().getDegrees()); - //SmartDashboard.putNumber("Superstructure/Hood/Analog Target Angle", hoodAnalogToOutput().getDegrees()); + //SmartDashboard.putNumber("SuperStructure/Hood/Analog Target Angle", hoodAnalogToOutput().getDegrees()); - SmartDashboard.putBoolean("Superstructure/Hood/Under Trench", isHoodUnderTrench()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index e572e1c5..00681629 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -10,6 +10,7 @@ import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.util.SysId; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; @@ -134,7 +135,7 @@ public void periodic() { if (voltageOverride.isPresent()) { hoodMotor.setVoltage(voltageOverride.get()); } else { - if (isHoodUnderTrench()) { + if (Superstructure.getInstance().isHoodUnderTrench()) { setState(HoodState.STOW); } hoodMotor.setControl(controller.withPosition(getTargetAngle().getRotations())); @@ -144,16 +145,16 @@ public void periodic() { } if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("Superstructure/Hood/Correct Hood Angle (deg)", getAbsoluteHoodAngleDeg()); + SmartDashboard.putNumber("SuperStructure/Hood/Correct Hood Angle (deg)", getAbsoluteHoodAngleDeg()); - SmartDashboard.putNumber("Superstructure/Hood/Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Hood/Supply Current", hoodMotor.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Hood/Stator Current", hoodMotor.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("SuperStructure/Hood/Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("SuperStructure/Hood/Supply Current", hoodMotor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("SuperStructure/Hood/Stator Current", hoodMotor.getStatorCurrent().getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Hood/Closed Loop Error (deg)", hoodMotor.getClosedLoopError().getValueAsDouble() * 360.0); - SmartDashboard.putBoolean("Superstructure/Hood/Has Used Absolute Encoder", hasUsedAbsoluteEncoder); + SmartDashboard.putNumber("SuperStructure/Hood/Closed Loop Error (deg)", hoodMotor.getClosedLoopError().getValueAsDouble() * 360.0); + SmartDashboard.putBoolean("SuperStructure/Hood/Has Used Absolute Encoder", hasUsedAbsoluteEncoder); - SmartDashboard.putNumber("Superstructure/Hood/Raw Motor Encoder Value", hoodMotor.getPosition().getValueAsDouble()); + SmartDashboard.putNumber("SuperStructure/Hood/Raw Motor Encoder Value", hoodMotor.getPosition().getValueAsDouble()); SmartDashboard.putNumber("InterpolationTesting/Hood Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index 75545920..8dcfd382 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java @@ -7,7 +7,7 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.util.superstructure.SOTMSolutionCalculator; +import com.stuypulse.robot.util.superstructure.SOTMCalculator; import com.stuypulse.robot.util.superstructure.InterpolationCalculator; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -65,7 +65,7 @@ public double getTargetRPM() { case LEFT_CORNER -> Settings.Superstructure.Shooter.RPMs.LEFT_CORNER_RPM; case RIGHT_CORNER -> Settings.Superstructure.Shooter.RPMs.RIGHT_CORNER_RPM; case INTERPOLATION -> InterpolationCalculator.interpolateShotInfo().targetRPM(); - case SOTM -> SOTMSolutionCalculator.calculateShooterRPMSOTM().get(); + case SOTM -> SOTMCalculator.calculateShooterRPMSOTM(); }; } @@ -84,11 +84,11 @@ public boolean atTolerance() { @Override public void periodic() { - SmartDashboard.putString("Superstructure/Shooter/State", state.name()); + SmartDashboard.putString("SuperStructure/Shooter/State", state.name()); SmartDashboard.putString("States/Shooter", state.name()); - SmartDashboard.putNumber("Superstructure/Shooter/Current RPM", getRPM()); - SmartDashboard.putNumber("Superstructure/Shooter/Target RPM", getTargetRPM()); + SmartDashboard.putNumber("SuperStructure/Shooter/Current RPM", getRPM()); + SmartDashboard.putNumber("SuperStructure/Shooter/Target RPM", getTargetRPM()); SmartDashboard.putNumber("InterpolationTesting/Shooter Interpolated Target Shoot RPM", InterpolationCalculator.interpolateShotInfo().targetRPM()); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java index 0584ef22..56a6d509 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -117,14 +117,14 @@ public void periodic() { } if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("Superstructure/Shooter/Leader Current (amps)", shooterLeader.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Shooter/Follower Supply Current (amps)", shooterFollower.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Shooter/Follower Stator Current", shooterFollower.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("SuperStructure/Shooter/Leader Current (amps)", shooterLeader.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("SuperStructure/Shooter/Follower Supply Current (amps)", shooterFollower.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("SuperStructure/Shooter/Follower Stator Current", shooterFollower.getStatorCurrent().getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Shooter/Leader Voltage", shooterLeader.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Shooter/Follower Voltage", shooterFollower.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("SuperStructure/Shooter/Leader Voltage", shooterLeader.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("SuperStructure/Shooter/Follower Voltage", shooterFollower.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Shooter/Follower RPM", getFollowerRPM()); + SmartDashboard.putNumber("SuperStructure/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()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java index 1be77039..7ab82028 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java @@ -75,14 +75,14 @@ public void periodic() { if (EnabledSubsystems.SHOOTER.get()) { if (voltageOverride.isPresent()) { sim.setInput(voltageOverride.get()); - SmartDashboard.putNumber("Superstructure/Shooter/Input Voltage", voltageOverride.get()); + SmartDashboard.putNumber("SuperStructure/Shooter/Input Voltage", voltageOverride.get()); } else { - SmartDashboard.putNumber("Superstructure/Shooter/Input Voltage", controller.getU(0)); + SmartDashboard.putNumber("SuperStructure/Shooter/Input Voltage", controller.getU(0)); sim.setInput(controller.getU(0)); } } else { sim.setInput(0); - SmartDashboard.putNumber("Superstructure/Shooter/Input Voltage", 0.0); + SmartDashboard.putNumber("SuperStructure/Shooter/Input Voltage", 0.0); } sim.update(Settings.DT); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java index 3c517dac..56c30f57 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java @@ -10,15 +10,16 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; -import com.stuypulse.robot.commands.hood.HoodAnalog; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.util.superstructure.SOTMSolutionCalculator; -import com.stuypulse.robot.util.turret.TurretVisualizer; +import com.stuypulse.robot.util.superstructure.SOTMCalculator; +import com.stuypulse.robot.util.superstructure.TurretAngleCalculator; +import com.stuypulse.robot.util.superstructure.TurretVisualizer; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -61,7 +62,7 @@ public Rotation2d getTargetAngle() { case IDLE -> getAngle(); case ZERO -> Rotation2d.kZero; case SHOOT -> getScoringAngle(); - case SOTM -> SOTMSolutionCalculator.calculateTurretAngleSOTM().get(); + case SOTM -> SOTMCalculator.calculateTurretAngleSOTM(); case FERRY -> getFerryAngle(); case LEFT_CORNER -> Settings.Superstructure.Turret.LEFT_CORNER; case RIGHT_CORNER -> Settings.Superstructure.Turret.RIGHT_CORNER; @@ -70,7 +71,7 @@ public Rotation2d getTargetAngle() { } public Rotation2d driverInputToAngle() { - SmartDashboard.putNumber("Turret/Driver Input", driverInput.x); + SmartDashboard.putNumber("SuperStructure/Turret/Driver Input", driverInput.x); return Rotation2d.fromDegrees(driverInput.x * 180); } @@ -80,12 +81,17 @@ public boolean atTargetAngle() { } public Rotation2d getScoringAngle() { - return getPointAtTargetAngle(Field.getHubPose()); + Translation2d target = Field.getHubPose().getTranslation(); + Translation2d turret = CommandSwerveDrivetrain.getInstance().getTurretPose().getTranslation(); + return TurretAngleCalculator.getPointAtTargetAngle(target, turret); } public Rotation2d getFerryAngle() { Pose2d robot = CommandSwerveDrivetrain.getInstance().getPose(); - return getPointAtTargetAngle(Field.getFerryZonePose(robot.getTranslation())); + Translation2d target = Field.getFerryZonePose(robot.getTranslation()).getTranslation(); + Translation2d turret = CommandSwerveDrivetrain.getInstance().getTurretPose().getTranslation(); + + return TurretAngleCalculator.getPointAtTargetAngle(target, turret); } public abstract Rotation2d getAngle(); @@ -105,10 +111,10 @@ public TurretState getState() { @Override public void periodic() { - SmartDashboard.putString("Turret/State", state.name()); + SmartDashboard.putString("SuperStructure/Turret/State", state.name()); SmartDashboard.putString("States/Turret", state.name()); - SmartDashboard.putNumber("Turret/Target Angle", getTargetAngle().getDegrees()); - SmartDashboard.putBoolean("Turret/At Target Angle?", atTargetAngle()); + SmartDashboard.putNumber("SuperStructure/Turret/Target Angle", getTargetAngle().getDegrees()); + SmartDashboard.putBoolean("SuperStructure/Turret/At Target Angle?", atTargetAngle()); if (Settings.DEBUG_MODE) { if (EnabledSubsystems.TURRET.get()) { @@ -119,25 +125,4 @@ public void periodic() { } } } - - public Rotation2d getPointAtTargetAngle(Pose2d targetPose) { - Pose2d robotPose = CommandSwerveDrivetrain.getInstance().getPose(); - Pose2d turretPose = CommandSwerveDrivetrain.getInstance().getTurretPose(); - - Vector2D turret = new Vector2D(turretPose.getTranslation()); - Vector2D target = new Vector2D(targetPose.getTranslation()); - - Vector2D turretToTarget = target.sub(turret); - Vector2D zeroVector = new Vector2D(robotPose.getRotation().getCos(), robotPose.getRotation().getSin()); - - // https://www.youtube.com/watch?v=_VuZZ9_58Wg - double crossProduct = zeroVector.x * turretToTarget.y - zeroVector.y * turretToTarget.x; - double dotProduct = zeroVector.dot(turretToTarget); - - Rotation2d targetAngle = (Robot.isReal() ? - Rotation2d.fromRadians(-Math.atan2(crossProduct, dotProduct)) : - Rotation2d.fromRadians(Math.atan2(crossProduct, dotProduct))); - - return targetAngle; - } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index 8f26814f..bde15c15 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -11,7 +11,8 @@ import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.SysId; -import com.stuypulse.robot.util.turret.TurretAngleCalculator; +import com.stuypulse.robot.util.superstructure.TurretAngleCalculator; + import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -167,16 +168,16 @@ public void periodic() { } if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("Superstructure/Turret/Relative Encoder Position (Rot)", motor.getPosition().getValueAsDouble() * 360.0); - SmartDashboard.putNumber("Superstructure/Turret/Closed Loop Error (deg)", motor.getClosedLoopError().getValueAsDouble() * 360.0); + SmartDashboard.putNumber("SuperStructure/Turret/Relative Encoder Position (Rot)", motor.getPosition().getValueAsDouble() * 360.0); + SmartDashboard.putNumber("SuperStructure/Turret/Closed Loop Error (deg)", motor.getClosedLoopError().getValueAsDouble() * 360.0); - SmartDashboard.putNumber("Superstructure/Turret/Encoder18t Abs Position (Rot)", encoder18t.getAbsolutePosition().getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Turret/Encoder17t Abs Position (Rot)", encoder17t.getAbsolutePosition().getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Turret/Vector Space Position (Deg)", getVectorSpaceAngle().getDegrees()); + SmartDashboard.putNumber("SuperStructure/Turret/Encoder18t Abs Position (Rot)", encoder18t.getAbsolutePosition().getValueAsDouble()); + SmartDashboard.putNumber("SuperStructure/Turret/Encoder17t Abs Position (Rot)", encoder17t.getAbsolutePosition().getValueAsDouble()); + SmartDashboard.putNumber("SuperStructure/Turret/Vector Space Position (Deg)", getVectorSpaceAngle().getDegrees()); - SmartDashboard.putNumber("Superstructure/Turret/Voltage", motor.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Turret/Stator Current", motor.getStatorCurrent().getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Turret/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("SuperStructure/Turret/Voltage", motor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("SuperStructure/Turret/Stator Current", motor.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("SuperStructure/Turret/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Current Draws/Turret (amps)", motor.getSupplyCurrent().getValueAsDouble()); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java index 12bc7159..1d8af158 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java @@ -104,13 +104,13 @@ public void periodic() { goal = new TrapezoidProfile.State(getTargetAngle().getRadians(), 0.0); setpoint = profile.calculate(Settings.DT, setpoint, goal); - SmartDashboard.putNumber("Turret/Constraints/Max Vel (deg per s)", Units.radiansToDegrees(maxAngularVelRadiansPerSecond)); - SmartDashboard.putNumber("Turret/Constraints/Max Accel (deg per s per s)", Units.radiansToDegrees(maxAngularAccelRadiansPerSecondSquared)); + SmartDashboard.putNumber("SuperStructure/Turret/Constraints/Max Vel (deg per s)", Units.radiansToDegrees(maxAngularVelRadiansPerSecond)); + SmartDashboard.putNumber("SuperStructure/Turret/Constraints/Max Accel (deg per s per s)", Units.radiansToDegrees(maxAngularAccelRadiansPerSecondSquared)); - SmartDashboard.putNumber("Turret/Motion Profile Setpoint (deg)", Units.radiansToDegrees(setpoint.position)); - SmartDashboard.putNumber("Turret/Error: abs(turret - target) (deg)", Math.abs(getAngle().minus(getTargetAngle()).getDegrees())); + SmartDashboard.putNumber("SuperStructure/Turret/Motion Profile Setpoint (deg)", Units.radiansToDegrees(setpoint.position)); + SmartDashboard.putNumber("SuperStructure/Turret/Error: abs(turret - target) (deg)", Math.abs(getAngle().minus(getTargetAngle()).getDegrees())); - SmartDashboard.putNumber("Turret/Current Angle (deg)", sim.getOutput(0)); + SmartDashboard.putNumber("SuperStructure/Turret/Current Angle (deg)", sim.getOutput(0)); controller.setNextR(VecBuilder.fill(setpoint.position, 0.0)); controller.correct(VecBuilder.fill(sim.getOutput(0), sim.getOutput(1))); 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 05c155c9..3d5d356b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -455,7 +455,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("SuperStructure/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()); diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java index 244607e3..8f4485dd 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java @@ -69,9 +69,9 @@ public static InterpolatedShotInfo interpolateShotInfo(Pose2d targetPose) { double flightTime = distanceTOFInterpolator.get(distanceMeters); - SmartDashboard.putNumber("HoodedShooter/Interpolated Target Angle", targetAngle.getDegrees()); - SmartDashboard.putNumber("HoodedShooter/Interpolated RPM", targetRPM); - SmartDashboard.putNumber("HoodedShooter/Interpolated TOF", flightTime); + SmartDashboard.putNumber("SuperStructure/Interpolated Target Angle", targetAngle.getDegrees()); + SmartDashboard.putNumber("SuperStructure/Interpolated RPM", targetRPM); + SmartDashboard.putNumber("SuperStructure/Interpolated TOF", flightTime); return new InterpolatedShotInfo( targetAngle, @@ -91,7 +91,7 @@ public static Supplier interpolateFerryingRPM() { double targetRPM = ferryingDistanceRPMInterpolator.get(distanceMeters); - SmartDashboard.putNumber("HoodedShooter/Interpolated Ferrying RPM", targetRPM); + SmartDashboard.putNumber("SuperStructure/Interpolated Ferrying RPM", targetRPM); return targetRPM; }; diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java new file mode 100644 index 00000000..3c18e88b --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java @@ -0,0 +1,219 @@ +package com.stuypulse.robot.util.superstructure; + +import java.util.function.Supplier; + +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.superstructure.hood.Hood; +import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret; +import com.stuypulse.robot.util.superstructure.InterpolationCalculator.InterpolatedShotInfo; +import com.stuypulse.stuylib.math.Vector2D; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + + +public class SOTMCalculator { + public static final double g = 9.81; + + public static SOTMSolution sol; + + private static FieldObject2d hubPose2d; + private static FieldObject2d virtualHubPose2d; + private static FieldObject2d futureTurretPose2d; + + public record SOTMSolution( + Rotation2d targetHoodAngle, + Rotation2d targetTurretAngle, + double targetShooterRPM, + Pose2d virtualPose, + double flightTime) { + } + + static { + sol = new SOTMSolution( + Hood.getInstance().getAngle(), + Turret.getInstance().getAngle(), + Shooter.getInstance().getRPM(), + Field.getHubPose(), + 0.0 + ); + + hubPose2d = Field.FIELD2D.getObject("hubPose"); + virtualHubPose2d = Field.FIELD2D.getObject("virtualHubPose"); + futureTurretPose2d = Field.FIELD2D.getObject("futureTurretPose"); + } + + + public static SOTMSolution solveShootOnTheMove( + Pose2d turretPose, + Pose2d robotPose, + Pose2d targetPose, + ChassisSpeeds fieldRelativeSpeeds, + int maxIterations, + double timeTolerance) { + + /* + 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. + */ + + + InterpolatedShotInfo sol = InterpolationCalculator.interpolateShotInfo(); + + + double t_guess = sol.flightTimeSeconds(); + + Pose2d virtualPose = targetPose; + + + for (int i = 0; i < maxIterations; i++) { + + SmartDashboard.putNumber("SuperStructure/SOTM/iteration #", i); + + double dx = fieldRelativeSpeeds.vxMetersPerSecond * t_guess; + double dy = fieldRelativeSpeeds.vyMetersPerSecond * t_guess; + + virtualPose = new Pose2d( + targetPose.getX() - dx, + targetPose.getY() - dy, + targetPose.getRotation()); + + + InterpolatedShotInfo newSol = InterpolationCalculator.interpolateShotInfo(virtualPose); + + if (Math.abs(newSol.flightTimeSeconds() - t_guess) < timeTolerance) { + break; + } + + t_guess = newSol.flightTimeSeconds(); + + sol = newSol; + + } + + Translation2d virtualTranslation = virtualPose.getTranslation(); + Translation2d turretTranslation = turretPose.getTranslation(); + + // double yaw = Math.atan2( + // virtualTranslation.getY() - turretTranslation.getY(), + // virtualTranslation.getX() - turretTranslation.getX() + // ); + + // Rotation2d targetTurretAngle = Robot.isReal() ? + // Rotation2d.fromRadians(-yaw).plus(robotPose.getRotation()) : + // Rotation2d.fromRadians(yaw).minus(robotPose.getRotation()); + + return new SOTMSolution( + sol.targetHoodAngle(), + TurretAngleCalculator.getPointAtTargetAngle(virtualTranslation, turretTranslation), + sol.targetRPM(), + virtualPose, + sol.flightTimeSeconds() + ); + } + + + 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.Superstructure.SOTM.UPDATE_DELAY.doubleValue(), + robotRelativeSpeeds.vyMetersPerSecond * Settings.Superstructure.SOTM.UPDATE_DELAY.doubleValue(), + 0 + ) + ); + + Vector2D oppositeDirection = new Vector2D(new Translation2d( + -robotRelativeSpeeds.vxMetersPerSecond, + -robotRelativeSpeeds.vyMetersPerSecond + )); + + if (!oppositeDirection.equals(Vector2D.kOrigin)) { + oppositeDirection = oppositeDirection.normalize(); + } + + hubPose = hubPose.exp( + new Twist2d( + oppositeDirection.x * Field.HUB_RADIUS, + oppositeDirection.y * Field.HUB_RADIUS, + 0 + ) + ); + + + SOTMSolution solution = solveShootOnTheMove( + futureTurretPose, + robotPose, + hubPose, + fieldRelativeSpeeds, + Settings.Superstructure.SOTM.MAX_ITERATIONS, + Settings.Superstructure.SOTM.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("SuperStructure/SOTM/calculated turret angle", sol.targetTurretAngle().getDegrees()); + SmartDashboard.putNumber("SuperStructure/SOTM/calculated hood angle", sol.targetHoodAngle().getDegrees()); + SmartDashboard.putNumber("SuperStructure/SOTM/calculated flight time", sol.flightTime()); + SmartDashboard.putNumber("SuperStructure/SOTM/turret dist to virtual pose", futureTurretPose.getTranslation().getDistance(sol.virtualPose().getTranslation())); + } + + public static Rotation2d calculateHoodAngleSOTM() { + return sol.targetHoodAngle(); + } + + public static Rotation2d calculateTurretAngleSOTM() { + return sol.targetTurretAngle(); + } + + public static Double calculateShooterRPMSOTM() { + return sol.targetShooterRPM(); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMSolutionCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMSolutionCalculator.java deleted file mode 100644 index b1cf6164..00000000 --- a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMSolutionCalculator.java +++ /dev/null @@ -1,125 +0,0 @@ -/************************ 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.superstructure; - -import java.util.function.Supplier; - -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.superstructure.hood.Hood; -import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.subsystems.superstructure.turret.Turret; -import com.stuypulse.robot.util.superstructure.ShotCalculator.SOTMSolution; -import com.stuypulse.stuylib.math.Vector2D; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - - -public class SOTMSolutionCalculator { - - public static SOTMSolution sol; - - private static FieldObject2d hubPose2d; - private static FieldObject2d virtualHubPose2d; - private static FieldObject2d futureTurretPose2d; - - - static { - sol = new SOTMSolution( - Hood.getInstance().getAngle(), - Turret.getInstance().getAngle(), - Shooter.getInstance().getRPM(), - Field.getHubPose(), - 0.0 - ); - - hubPose2d = Field.FIELD2D.getObject("hubPose"); - virtualHubPose2d = Field.FIELD2D.getObject("virtualHubPose"); - futureTurretPose2d = Field.FIELD2D.getObject("futureTurretPose"); - } - - - 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.Superstructure.SOTM.UPDATE_DELAY.doubleValue(), - robotRelativeSpeeds.vyMetersPerSecond * Settings.Superstructure.SOTM.UPDATE_DELAY.doubleValue(), - 0 - ) - ); - - Vector2D oppositeDirection = new Vector2D(new Translation2d( - -robotRelativeSpeeds.vxMetersPerSecond, - -robotRelativeSpeeds.vyMetersPerSecond - )); - - if (!oppositeDirection.equals(Vector2D.kOrigin)) { - oppositeDirection = oppositeDirection.normalize(); - } - - hubPose = hubPose.exp( - new Twist2d( - oppositeDirection.x * Field.HUB_RADIUS, - oppositeDirection.y * Field.HUB_RADIUS, - 0 - ) - ); - - - SOTMSolution solution = ShotCalculator.solveShootOnTheMove( - futureTurretPose, - robotPose, - hubPose, - fieldRelativeSpeeds, - Settings.Superstructure.SOTM.MAX_ITERATIONS, - Settings.Superstructure.SOTM.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("Superstructure/SOTM/calculated turret angle", sol.targetTurretAngle().getDegrees()); - SmartDashboard.putNumber("Superstructure/SOTM/calculated hood angle", sol.targetHoodAngle().getDegrees()); - SmartDashboard.putNumber("Superstructure/SOTM/calculated flight time", sol.flightTime()); - SmartDashboard.putNumber("Superstructure/SOTM/turret dist to virtual pose", futureTurretPose.getTranslation().getDistance(sol.virtualPose().getTranslation())); - } - - public static Supplier calculateHoodAngleSOTM() { - return () -> sol.targetHoodAngle(); - } - - public static Supplier calculateTurretAngleSOTM() { - return () -> sol.targetTurretAngle(); - } - - public static Supplier calculateShooterRPMSOTM() { - return () -> sol.targetShooterRPM(); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/ShotCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/ShotCalculator.java deleted file mode 100644 index 519451b9..00000000 --- a/src/main/java/com/stuypulse/robot/util/superstructure/ShotCalculator.java +++ /dev/null @@ -1,118 +0,0 @@ -/************************ 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.superstructure; - -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.util.superstructure.InterpolationCalculator.InterpolatedShotInfo; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -public final class ShotCalculator { - public static final double g = 9.81; - - - public record SOTMSolution( - Rotation2d targetHoodAngle, - Rotation2d targetTurretAngle, - double targetShooterRPM, - Pose2d virtualPose, - double flightTime) { - } - - public static SOTMSolution solveShootOnTheMove( - Pose2d turretPose, - Pose2d robotPose, - Pose2d targetPose, - ChassisSpeeds fieldRelativeSpeeds, - int maxIterations, - double timeTolerance) { - - /* - 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. - */ - - - InterpolatedShotInfo sol = InterpolationCalculator.interpolateShotInfo(); - - - double t_guess = sol.flightTimeSeconds(); - - Pose2d virtualPose = targetPose; - - - for (int i = 0; i < maxIterations; i++) { - - SmartDashboard.putNumber("Superstructure/SOTM/iteration #", i); - - double dx = fieldRelativeSpeeds.vxMetersPerSecond * t_guess; - double dy = fieldRelativeSpeeds.vyMetersPerSecond * t_guess; - - virtualPose = new Pose2d( - targetPose.getX() - dx, - targetPose.getY() - dy, - targetPose.getRotation()); - - - InterpolatedShotInfo newSol = InterpolationCalculator.interpolateShotInfo(virtualPose); - - if (Math.abs(newSol.flightTimeSeconds() - t_guess) < timeTolerance) { - break; - } - - t_guess = newSol.flightTimeSeconds(); - - sol = newSol; - - } - - Translation2d virtualTranslation = virtualPose.getTranslation(); - Translation2d turretTranslation = turretPose.getTranslation(); - - double yaw = Math.atan2( - virtualTranslation.getY() - turretTranslation.getY(), - virtualTranslation.getX() - turretTranslation.getX() - ); - - Rotation2d targetTurretAngle = Robot.isReal() ? - Rotation2d.fromRadians(-yaw).plus(robotPose.getRotation()) : - Rotation2d.fromRadians(yaw).minus(robotPose.getRotation()); - - return new SOTMSolution( - sol.targetHoodAngle(), - targetTurretAngle, - sol.targetRPM(), - virtualPose, - sol.flightTimeSeconds() - ); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/turret/TurretAngleCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/TurretAngleCalculator.java similarity index 60% rename from src/main/java/com/stuypulse/robot/util/turret/TurretAngleCalculator.java rename to src/main/java/com/stuypulse/robot/util/superstructure/TurretAngleCalculator.java index ed1dd0a6..a9d1dab3 100644 --- a/src/main/java/com/stuypulse/robot/util/turret/TurretAngleCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/TurretAngleCalculator.java @@ -3,11 +3,15 @@ /* 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; +package com.stuypulse.robot.util.superstructure; +import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; public class TurretAngleCalculator { @@ -61,4 +65,36 @@ public static Rotation2d getAbsoluteAngle(double encoder17TValue, double encoder public static int lowestDistanceIndex() { return leastDistanceIndex; } + + public static Rotation2d getPointAtTargetAngle(Translation2d targetTranslation, Translation2d turretTranslation) { + + // Vector2D turret = new Vector2D(turretPose.getTranslation()); + // Vector2D target = new Vector2D(targetPose.getTranslation()); + + // Vector2D turretToTarget = target.sub(turret); + // Vector2D zeroVector = new Vector2D(robotPose.getRotation().getCos(), robotPose.getRotation().getSin()); + + // // https://www.youtube.com/watch?v=_VuZZ9_58Wg + // double crossProduct = zeroVector.x * turretToTarget.y - zeroVector.y * turretToTarget.x; + // double dotProduct = zeroVector.dot(turretToTarget); + + // Rotation2d targetAngle = (Robot.isReal() ? + // Rotation2d.fromRadians(-Math.atan2(crossProduct, dotProduct)) : + // Rotation2d.fromRadians(Math.atan2(crossProduct, dotProduct))); + + CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); + + Pose2d robotPose = swerve.getPose(); + + double yaw = Math.atan2( + targetTranslation.getY() - turretTranslation.getY(), + targetTranslation.getX() - turretTranslation.getX() + ); + + Rotation2d targetAngle = Robot.isReal() ? + Rotation2d.fromRadians(-yaw).plus(robotPose.getRotation()) : + Rotation2d.fromRadians(yaw).minus(robotPose.getRotation()); + + return targetAngle; + } } diff --git a/src/main/java/com/stuypulse/robot/util/turret/TurretVisualizer.java b/src/main/java/com/stuypulse/robot/util/superstructure/TurretVisualizer.java similarity index 97% rename from src/main/java/com/stuypulse/robot/util/turret/TurretVisualizer.java rename to src/main/java/com/stuypulse/robot/util/superstructure/TurretVisualizer.java index 6271e0d6..b9a80170 100644 --- a/src/main/java/com/stuypulse/robot/util/turret/TurretVisualizer.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/TurretVisualizer.java @@ -3,7 +3,7 @@ /* 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; +package com.stuypulse.robot.util.superstructure; import edu.wpi.first.math.geometry.Rotation2d; From f4250c30d901f8fd0de3693ac053b7060861d0c4 Mon Sep 17 00:00:00 2001 From: Brian Date: Thu, 5 Mar 2026 01:06:56 -0500 Subject: [PATCH 074/150] FIX: make sotm actually interpolate on the lookahead turret pose --- .../subsystems/swerve/CommandSwerveDrivetrain.java | 4 ++-- .../superstructure/InterpolationCalculator.java | 14 ++++++++------ .../robot/util/superstructure/SOTMCalculator.java | 8 ++------ 3 files changed, 12 insertions(+), 14 deletions(-) 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 3d5d356b..8d195139 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -50,7 +50,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem { private final static CommandSwerveDrivetrain instance; - // private FieldObject2d turret2d = Field.FIELD2D.getObject("Turret 2D"); + private FieldObject2d turret2d = Field.FIELD2D.getObject("Turret 2D"); private Pose2d turretPose = new Pose2d(); static { @@ -453,7 +453,7 @@ public void periodic() { pose.getRotation().plus(Turret.getInstance().getAngle()) ); - // turret2d.setPose(Robot.isBlue() ? turretPose : Field.transformToOppositeAlliance(turretPose)); + turret2d.setPose(Robot.isBlue() ? turretPose : Field.transformToOppositeAlliance(turretPose)); SmartDashboard.putNumber("SuperStructure/Turret/Dist From Hub", turretPose.getTranslation().getDistance(Field.hubCenter.getTranslation())); SmartDashboard.putNumber("InterpolationTesting/Turret Dist From Hub", turretPose.getTranslation().getDistance(Field.hubCenter.getTranslation())); diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java index 8f4485dd..2a8eb7dd 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java @@ -44,7 +44,8 @@ public class InterpolationCalculator { ferryingDistanceRPMInterpolator.put(pair[0], pair[1]); } } - + + public record InterpolatedShotInfo( Rotation2d targetHoodAngle, @@ -53,16 +54,17 @@ public record InterpolatedShotInfo( } public static InterpolatedShotInfo interpolateShotInfo(){ - return interpolateShotInfo(Field.getHubPose()); + CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); + + return interpolateShotInfo(swerve.getTurretPose(), Field.getHubPose()); } - public static InterpolatedShotInfo interpolateShotInfo(Pose2d targetPose) { - CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); + public static InterpolatedShotInfo interpolateShotInfo(Pose2d turretPose, Pose2d targetPose) { Translation2d hubPose = targetPose.getTranslation(); - Translation2d turretPose = swerve.getTurretPose().getTranslation(); + Translation2d currentPose = turretPose.getTranslation(); - double distanceMeters = turretPose.getDistance(hubPose); + double distanceMeters = currentPose.getDistance(hubPose); Rotation2d targetAngle = Rotation2d.fromRadians(distanceAngleInterpolator.get(distanceMeters)); double targetRPM = distanceRPMInterpolator.get(distanceMeters); diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java index 3c18e88b..c21279b5 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java @@ -1,7 +1,5 @@ package com.stuypulse.robot.util.superstructure; -import java.util.function.Supplier; - import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; @@ -55,7 +53,6 @@ public record SOTMSolution( public static SOTMSolution solveShootOnTheMove( Pose2d turretPose, - Pose2d robotPose, Pose2d targetPose, ChassisSpeeds fieldRelativeSpeeds, int maxIterations, @@ -89,7 +86,7 @@ but looking at the second equation, to get the flight time we need to solveBalli */ - InterpolatedShotInfo sol = InterpolationCalculator.interpolateShotInfo(); + InterpolatedShotInfo sol = InterpolationCalculator.interpolateShotInfo(turretPose, targetPose); double t_guess = sol.flightTimeSeconds(); @@ -110,7 +107,7 @@ but looking at the second equation, to get the flight time we need to solveBalli targetPose.getRotation()); - InterpolatedShotInfo newSol = InterpolationCalculator.interpolateShotInfo(virtualPose); + InterpolatedShotInfo newSol = InterpolationCalculator.interpolateShotInfo(turretPose, virtualPose); if (Math.abs(newSol.flightTimeSeconds() - t_guess) < timeTolerance) { break; @@ -185,7 +182,6 @@ public static void updateSOTMSolution() { SOTMSolution solution = solveShootOnTheMove( futureTurretPose, - robotPose, hubPose, fieldRelativeSpeeds, Settings.Superstructure.SOTM.MAX_ITERATIONS, From 848fdbc940d8b1ffed110d9c076444ab26fb8fc2 Mon Sep 17 00:00:00 2001 From: Xuan Hao Date: Thu, 5 Mar 2026 01:33:35 -0500 Subject: [PATCH 075/150] REFACTOR: fix typo in logging for superstructure REFACTOR: use normal return values instead of suppliers for readability --- .../robot/commands/hood/HoodAnalog.java | 2 +- .../com/stuypulse/robot/constants/Gains.java | 12 +++--- .../stuypulse/robot/constants/Settings.java | 10 ++--- .../superstructure/Superstructure.java | 24 +++++------ .../subsystems/superstructure/hood/Hood.java | 8 ++-- .../superstructure/hood/HoodImpl.java | 14 +++---- .../superstructure/shooter/Shooter.java | 8 ++-- .../superstructure/shooter/ShooterImpl.java | 12 +++--- .../superstructure/shooter/ShooterSim.java | 6 +-- .../superstructure/turret/Turret.java | 8 ++-- .../superstructure/turret/TurretImpl.java | 16 +++---- .../superstructure/turret/TurretSim.java | 10 ++--- .../swerve/CommandSwerveDrivetrain.java | 2 +- .../InterpolationCalculator.java | 42 +++++++++---------- .../util/superstructure/SOTMCalculator.java | 12 +++--- 15 files changed, 92 insertions(+), 94 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java b/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java index 73744cbb..d0bcf8b2 100644 --- a/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java +++ b/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java @@ -24,6 +24,6 @@ public void initialize() { @Override public void execute() { hood.hoodAnalogToInput(driver); - //SmartDashboard.putNumber("SuperStructure/Hood/Analog", hood.hoodAnalogToOutput().getDegrees()); + //SmartDashboard.putNumber("Superstructure/Hood/Analog", hood.hoodAnalogToOutput().getDegrees()); } } diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 1999bcdf..8e20894e 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -31,13 +31,13 @@ public interface ClimberHopper { public interface Superstructure { public interface Shooter { - SmartNumber kP = new SmartNumber("SuperStructure/Shooter/Gains/kP", 0.45); - SmartNumber kI = new SmartNumber("SuperStructure/Shooter/Gains/kI", 0.0); - SmartNumber kD = new SmartNumber("SuperStructure/Shooter/Gains/kD", 0.0); + SmartNumber kP = new SmartNumber("Superstructure/Shooter/Gains/kP", 0.45); + SmartNumber kI = new SmartNumber("Superstructure/Shooter/Gains/kI", 0.0); + SmartNumber kD = new SmartNumber("Superstructure/Shooter/Gains/kD", 0.0); - SmartNumber kS = new SmartNumber("SuperStructure/Shooter/Gains/kS", 0.0); - SmartNumber kV = new SmartNumber("SuperStructure/Shooter/Gains/kV", 0.123); - SmartNumber kA = new SmartNumber("SuperStructure/Shooter/Gains/kA", 0.0); + SmartNumber kS = new SmartNumber("Superstructure/Shooter/Gains/kS", 0.0); + SmartNumber kV = new SmartNumber("Superstructure/Shooter/Gains/kV", 0.123); + SmartNumber kA = new SmartNumber("Superstructure/Shooter/Gains/kA", 0.0); // double kP = 0.45; // double kI = 0.0; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index fa93cdc1..4ff64aa5 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -176,8 +176,8 @@ public interface Shooter { public final double FLYWHEEL_RADIUS = Units.inchesToMeters(3.965 / 2); public interface RPMs { - public final SmartNumber SHOOT_RPM = new SmartNumber("SuperStructure/Shoot State Target RPM", 3500.0); - public final SmartNumber FERRY_RPM = new SmartNumber("SuperStructure/Ferry State Target RPM", 2000.0); + public final SmartNumber SHOOT_RPM = new SmartNumber("Superstructure/Shoot State Target RPM", 3500.0); + public final SmartNumber FERRY_RPM = new SmartNumber("Superstructure/Ferry State Target RPM", 2000.0); public final double REVERSE = 0.0; public final double KB_RPM = 0.0; public final double LEFT_CORNER_RPM = 0.0; @@ -211,8 +211,8 @@ public interface Hood { public final double STALL_DEBOUNCE = 0.5; public interface Angles { - public final SmartNumber SHOOT_ANGLE = new SmartNumber("SuperStructure/Shoot State Target Angle (deg)", 15.0); - public final SmartNumber FERRY_ANGLE = new SmartNumber("SuperStructure/Ferry State Target Angle (deg)", 20.0); + public final SmartNumber SHOOT_ANGLE = new SmartNumber("Superstructure/Shoot State Target Angle (deg)", 15.0); + public final SmartNumber FERRY_ANGLE = new SmartNumber("Superstructure/Ferry State Target Angle (deg)", 20.0); public final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(7.0); public final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(40.0); @@ -272,7 +272,7 @@ public interface SoftwareLimit { public interface SOTM { public final int MAX_ITERATIONS = 5; public final double TIME_TOLERANCE = 0.01; - public final SmartNumber UPDATE_DELAY = new SmartNumber("SuperStructure/SOTM/Update Delay", 0.00); + public final SmartNumber UPDATE_DELAY = new SmartNumber("Superstructure/SOTM/Update Delay", 0.00); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index 3b5fc9da..ede1dd5e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -152,24 +152,24 @@ public void periodic() { SOTMCalculator.updateSOTMSolution(); } - SmartDashboard.putString("SuperStructure/State", state.name()); + SmartDashboard.putString("Superstructure/State", state.name()); SmartDashboard.putString("States/SuperStructure", state.name()); - SmartDashboard.putNumber("SuperStructure/Target RPM", getTargetRPM()); - SmartDashboard.putNumber("SuperStructure/Target Yaw", getTargetYaw().getDegrees()); - SmartDashboard.putNumber("SuperStructure/Target Pitch", getTargetPitch().getDegrees()); + SmartDashboard.putNumber("Superstructure/Target RPM", getTargetRPM()); + SmartDashboard.putNumber("Superstructure/Target Yaw", getTargetYaw().getDegrees()); + SmartDashboard.putNumber("Superstructure/Target Pitch", getTargetPitch().getDegrees()); - SmartDashboard.putNumber("SuperStructure/Current RPM", getShooterRPM()); - SmartDashboard.putNumber("SuperStructure/Current Yaw", getTurretAngle().getDegrees()); - SmartDashboard.putNumber("SuperStructure/Current Pitch", getHoodAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/Current RPM", getShooterRPM()); + SmartDashboard.putNumber("Superstructure/Current Yaw", getTurretAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/Current Pitch", getHoodAngle().getDegrees()); - SmartDashboard.putNumber("SuperStructure/Hood Angle Error (Deg)", getTargetPitch().getDegrees() - getHoodAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/Hood Angle Error (Deg)", getTargetPitch().getDegrees() - getHoodAngle().getDegrees()); - SmartDashboard.putBoolean("SuperStructure/Shooter At Tolerance?", isShooterAtTolerance()); - SmartDashboard.putBoolean("SuperStructure/Hood At Tolerance?", isHoodAtTolerance()); - SmartDashboard.putBoolean("SuperStructure/Turret At Tolerant?", isHoodAtTolerance()); + SmartDashboard.putBoolean("Superstructure/Shooter At Tolerance?", isShooterAtTolerance()); + SmartDashboard.putBoolean("Superstructure/Hood At Tolerance?", isHoodAtTolerance()); + SmartDashboard.putBoolean("Superstructure/Turret At Tolerant?", isHoodAtTolerance()); - SmartDashboard.putBoolean("SuperStructure/Hood/Under Trench", isHoodUnderTrench()); + SmartDashboard.putBoolean("Superstructure/Hood/Under Trench", isHoodUnderTrench()); SmartDashboard.putNumber("InterpolationTesting/Hood Angle", getHoodAngle().getDegrees()); SmartDashboard.putNumber("InterpolationTesting/Shooter RPM", getShooterRPM()); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java index 3ceb123c..7ed5db47 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java @@ -95,13 +95,13 @@ public Rotation2d hoodAnalogToOutput() { @Override public void periodic() { - SmartDashboard.putString("SuperStructure/Hood/State", state.name()); + SmartDashboard.putString("Superstructure/Hood/State", state.name()); SmartDashboard.putString("States/Hood", state.name()); - SmartDashboard.putNumber("SuperStructure/Hood/Target Angle", getTargetAngle().getDegrees()); - SmartDashboard.putNumber("SuperStructure/Hood/Current Angle", getAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/Hood/Target Angle", getTargetAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/Hood/Current Angle", getAngle().getDegrees()); - //SmartDashboard.putNumber("SuperStructure/Hood/Analog Target Angle", hoodAnalogToOutput().getDegrees()); + //SmartDashboard.putNumber("Superstructure/Hood/Analog Target Angle", hoodAnalogToOutput().getDegrees()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index 00681629..ddf67b72 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -145,16 +145,16 @@ public void periodic() { } if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("SuperStructure/Hood/Correct Hood Angle (deg)", getAbsoluteHoodAngleDeg()); + SmartDashboard.putNumber("Superstructure/Hood/Correct Hood Angle (deg)", getAbsoluteHoodAngleDeg()); - SmartDashboard.putNumber("SuperStructure/Hood/Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("SuperStructure/Hood/Supply Current", hoodMotor.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("SuperStructure/Hood/Stator Current", hoodMotor.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Hood/Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Hood/Supply Current", hoodMotor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Hood/Stator Current", hoodMotor.getStatorCurrent().getValueAsDouble()); - SmartDashboard.putNumber("SuperStructure/Hood/Closed Loop Error (deg)", hoodMotor.getClosedLoopError().getValueAsDouble() * 360.0); - SmartDashboard.putBoolean("SuperStructure/Hood/Has Used Absolute Encoder", hasUsedAbsoluteEncoder); + SmartDashboard.putNumber("Superstructure/Hood/Closed Loop Error (deg)", hoodMotor.getClosedLoopError().getValueAsDouble() * 360.0); + SmartDashboard.putBoolean("Superstructure/Hood/Has Used Absolute Encoder", hasUsedAbsoluteEncoder); - SmartDashboard.putNumber("SuperStructure/Hood/Raw Motor Encoder Value", hoodMotor.getPosition().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Hood/Raw Motor Encoder Value", hoodMotor.getPosition().getValueAsDouble()); SmartDashboard.putNumber("InterpolationTesting/Hood Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index 8dcfd382..129f36d0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java @@ -59,7 +59,7 @@ public double getTargetRPM() { return switch(state) { case STOP -> 0; case SHOOT -> getShootRPM(); - case FERRY -> InterpolationCalculator.interpolateFerryingRPM().get(); + case FERRY -> InterpolationCalculator.interpolateFerryingRPM(); case REVERSE -> Settings.Superstructure.Shooter.RPMs.REVERSE; case KB -> Settings.Superstructure.Shooter.RPMs.KB_RPM; case LEFT_CORNER -> Settings.Superstructure.Shooter.RPMs.LEFT_CORNER_RPM; @@ -84,11 +84,11 @@ public boolean atTolerance() { @Override public void periodic() { - SmartDashboard.putString("SuperStructure/Shooter/State", state.name()); + SmartDashboard.putString("Superstructure/Shooter/State", state.name()); SmartDashboard.putString("States/Shooter", state.name()); - SmartDashboard.putNumber("SuperStructure/Shooter/Current RPM", getRPM()); - SmartDashboard.putNumber("SuperStructure/Shooter/Target RPM", getTargetRPM()); + SmartDashboard.putNumber("Superstructure/Shooter/Current RPM", getRPM()); + SmartDashboard.putNumber("Superstructure/Shooter/Target RPM", getTargetRPM()); SmartDashboard.putNumber("InterpolationTesting/Shooter Interpolated Target Shoot RPM", InterpolationCalculator.interpolateShotInfo().targetRPM()); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java index 56a6d509..0584ef22 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -117,14 +117,14 @@ public void periodic() { } if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("SuperStructure/Shooter/Leader Current (amps)", shooterLeader.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("SuperStructure/Shooter/Follower Supply Current (amps)", shooterFollower.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("SuperStructure/Shooter/Follower Stator Current", shooterFollower.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Shooter/Leader Current (amps)", shooterLeader.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Shooter/Follower Supply Current (amps)", shooterFollower.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Shooter/Follower Stator Current", shooterFollower.getStatorCurrent().getValueAsDouble()); - SmartDashboard.putNumber("SuperStructure/Shooter/Leader Voltage", shooterLeader.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("SuperStructure/Shooter/Follower Voltage", shooterFollower.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Shooter/Leader Voltage", shooterLeader.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Shooter/Follower Voltage", shooterFollower.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("SuperStructure/Shooter/Follower RPM", getFollowerRPM()); + SmartDashboard.putNumber("Superstructure/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()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java index 7ab82028..1be77039 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java @@ -75,14 +75,14 @@ public void periodic() { if (EnabledSubsystems.SHOOTER.get()) { if (voltageOverride.isPresent()) { sim.setInput(voltageOverride.get()); - SmartDashboard.putNumber("SuperStructure/Shooter/Input Voltage", voltageOverride.get()); + SmartDashboard.putNumber("Superstructure/Shooter/Input Voltage", voltageOverride.get()); } else { - SmartDashboard.putNumber("SuperStructure/Shooter/Input Voltage", controller.getU(0)); + SmartDashboard.putNumber("Superstructure/Shooter/Input Voltage", controller.getU(0)); sim.setInput(controller.getU(0)); } } else { sim.setInput(0); - SmartDashboard.putNumber("SuperStructure/Shooter/Input Voltage", 0.0); + SmartDashboard.putNumber("Superstructure/Shooter/Input Voltage", 0.0); } sim.update(Settings.DT); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java index 56c30f57..e3865139 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java @@ -71,7 +71,7 @@ public Rotation2d getTargetAngle() { } public Rotation2d driverInputToAngle() { - SmartDashboard.putNumber("SuperStructure/Turret/Driver Input", driverInput.x); + SmartDashboard.putNumber("Superstructure/Turret/Driver Input", driverInput.x); return Rotation2d.fromDegrees(driverInput.x * 180); } @@ -111,10 +111,10 @@ public TurretState getState() { @Override public void periodic() { - SmartDashboard.putString("SuperStructure/Turret/State", state.name()); + SmartDashboard.putString("Superstructure/Turret/State", state.name()); SmartDashboard.putString("States/Turret", state.name()); - SmartDashboard.putNumber("SuperStructure/Turret/Target Angle", getTargetAngle().getDegrees()); - SmartDashboard.putBoolean("SuperStructure/Turret/At Target Angle?", atTargetAngle()); + SmartDashboard.putNumber("Superstructure/Turret/Target Angle", getTargetAngle().getDegrees()); + SmartDashboard.putBoolean("Superstructure/Turret/At Target Angle?", atTargetAngle()); if (Settings.DEBUG_MODE) { if (EnabledSubsystems.TURRET.get()) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index bde15c15..700d1734 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -168,16 +168,16 @@ public void periodic() { } if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("SuperStructure/Turret/Relative Encoder Position (Rot)", motor.getPosition().getValueAsDouble() * 360.0); - SmartDashboard.putNumber("SuperStructure/Turret/Closed Loop Error (deg)", motor.getClosedLoopError().getValueAsDouble() * 360.0); + SmartDashboard.putNumber("Superstructure/Turret/Relative Encoder Position (Rot)", motor.getPosition().getValueAsDouble() * 360.0); + SmartDashboard.putNumber("Superstructure/Turret/Closed Loop Error (deg)", motor.getClosedLoopError().getValueAsDouble() * 360.0); - SmartDashboard.putNumber("SuperStructure/Turret/Encoder18t Abs Position (Rot)", encoder18t.getAbsolutePosition().getValueAsDouble()); - SmartDashboard.putNumber("SuperStructure/Turret/Encoder17t Abs Position (Rot)", encoder17t.getAbsolutePosition().getValueAsDouble()); - SmartDashboard.putNumber("SuperStructure/Turret/Vector Space Position (Deg)", getVectorSpaceAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/Turret/Encoder18t Abs Position (Rot)", encoder18t.getAbsolutePosition().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Turret/Encoder17t Abs Position (Rot)", encoder17t.getAbsolutePosition().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Turret/Vector Space Position (Deg)", getVectorSpaceAngle().getDegrees()); - SmartDashboard.putNumber("SuperStructure/Turret/Voltage", motor.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("SuperStructure/Turret/Stator Current", motor.getStatorCurrent().getValueAsDouble()); - SmartDashboard.putNumber("SuperStructure/Turret/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Turret/Voltage", motor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Turret/Stator Current", motor.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Turret/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Current Draws/Turret (amps)", motor.getSupplyCurrent().getValueAsDouble()); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java index 1d8af158..cfba49e6 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java @@ -104,13 +104,13 @@ public void periodic() { goal = new TrapezoidProfile.State(getTargetAngle().getRadians(), 0.0); setpoint = profile.calculate(Settings.DT, setpoint, goal); - SmartDashboard.putNumber("SuperStructure/Turret/Constraints/Max Vel (deg per s)", Units.radiansToDegrees(maxAngularVelRadiansPerSecond)); - SmartDashboard.putNumber("SuperStructure/Turret/Constraints/Max Accel (deg per s per s)", Units.radiansToDegrees(maxAngularAccelRadiansPerSecondSquared)); + SmartDashboard.putNumber("Superstructure/Turret/Constraints/Max Vel (deg per s)", Units.radiansToDegrees(maxAngularVelRadiansPerSecond)); + SmartDashboard.putNumber("Superstructure/Turret/Constraints/Max Accel (deg per s per s)", Units.radiansToDegrees(maxAngularAccelRadiansPerSecondSquared)); - SmartDashboard.putNumber("SuperStructure/Turret/Motion Profile Setpoint (deg)", Units.radiansToDegrees(setpoint.position)); - SmartDashboard.putNumber("SuperStructure/Turret/Error: abs(turret - target) (deg)", Math.abs(getAngle().minus(getTargetAngle()).getDegrees())); + SmartDashboard.putNumber("Superstructure/Turret/Motion Profile Setpoint (deg)", Units.radiansToDegrees(setpoint.position)); + SmartDashboard.putNumber("Superstructure/Turret/Error: abs(turret - target) (deg)", Math.abs(getAngle().minus(getTargetAngle()).getDegrees())); - SmartDashboard.putNumber("SuperStructure/Turret/Current Angle (deg)", sim.getOutput(0)); + SmartDashboard.putNumber("Superstructure/Turret/Current Angle (deg)", sim.getOutput(0)); controller.setNextR(VecBuilder.fill(setpoint.position, 0.0)); controller.correct(VecBuilder.fill(sim.getOutput(0), sim.getOutput(1))); 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 8d195139..73a58457 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -455,7 +455,7 @@ public void periodic() { turret2d.setPose(Robot.isBlue() ? turretPose : Field.transformToOppositeAlliance(turretPose)); - SmartDashboard.putNumber("SuperStructure/Turret/Dist From Hub", turretPose.getTranslation().getDistance(Field.hubCenter.getTranslation())); + SmartDashboard.putNumber("Superstructure/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()); diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java index 2a8eb7dd..e4f0381a 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java @@ -3,6 +3,7 @@ import java.util.function.Supplier; import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Superstructure.AngleInterpolation; import com.stuypulse.robot.constants.Settings.Superstructure.FerryRPMInterpolation; import com.stuypulse.robot.constants.Settings.Superstructure.RPMInterpolation; @@ -23,6 +24,13 @@ public class InterpolationCalculator { public static InterpolatingDoubleTreeMap ferryingDistanceRPMInterpolator; + public record InterpolatedShotInfo( + Rotation2d targetHoodAngle, + double targetRPM, + double flightTimeSeconds) { + } + + static { distanceAngleInterpolator = new InterpolatingDoubleTreeMap(); for (double[] pair : AngleInterpolation.distanceAngleInterpolationValues) { @@ -44,15 +52,7 @@ public class InterpolationCalculator { ferryingDistanceRPMInterpolator.put(pair[0], pair[1]); } } - - - public record InterpolatedShotInfo( - Rotation2d targetHoodAngle, - double targetRPM, - double flightTimeSeconds) { - } - public static InterpolatedShotInfo interpolateShotInfo(){ CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); @@ -71,9 +71,9 @@ public static InterpolatedShotInfo interpolateShotInfo(Pose2d turretPose, Pose2d double flightTime = distanceTOFInterpolator.get(distanceMeters); - SmartDashboard.putNumber("SuperStructure/Interpolated Target Angle", targetAngle.getDegrees()); - SmartDashboard.putNumber("SuperStructure/Interpolated RPM", targetRPM); - SmartDashboard.putNumber("SuperStructure/Interpolated TOF", flightTime); + SmartDashboard.putNumber("Superstructure/Interpolated Target Angle", targetAngle.getDegrees()); + SmartDashboard.putNumber("Superstructure/Interpolated RPM", targetRPM); + SmartDashboard.putNumber("Superstructure/Interpolated TOF", flightTime); return new InterpolatedShotInfo( targetAngle, @@ -82,21 +82,19 @@ public static InterpolatedShotInfo interpolateShotInfo(Pose2d turretPose, Pose2d ); } - public static Supplier interpolateFerryingRPM() { - return () -> { - CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); + public static double interpolateFerryingRPM() { + CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); - Translation2d currentPose = swerve.getTurretPose().getTranslation(); - Translation2d cornerPose = Field.getFerryZonePose(currentPose).getTranslation(); + Translation2d currentPose = swerve.getTurretPose().getTranslation(); + Translation2d cornerPose = Field.getFerryZonePose(currentPose).getTranslation(); - double distanceMeters = cornerPose.getDistance(currentPose); + double distanceMeters = cornerPose.getDistance(currentPose); - double targetRPM = ferryingDistanceRPMInterpolator.get(distanceMeters); + double targetRPM = ferryingDistanceRPMInterpolator.get(distanceMeters); - SmartDashboard.putNumber("SuperStructure/Interpolated Ferrying RPM", targetRPM); - - return targetRPM; - }; + SmartDashboard.putNumber("Superstructure/Interpolated Ferrying RPM", targetRPM); + + return targetRPM; } diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java index c21279b5..501c17b2 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java @@ -96,7 +96,7 @@ but looking at the second equation, to get the flight time we need to solveBalli for (int i = 0; i < maxIterations; i++) { - SmartDashboard.putNumber("SuperStructure/SOTM/iteration #", i); + SmartDashboard.putNumber("Superstructure/SOTM/iteration #", i); double dx = fieldRelativeSpeeds.vxMetersPerSecond * t_guess; double dy = fieldRelativeSpeeds.vyMetersPerSecond * t_guess; @@ -195,10 +195,10 @@ public static void updateSOTMSolution() { futureTurretPose2d.setPose((Robot.isBlue() ? futureTurretPose : Field.transformToOppositeAlliance(futureTurretPose))); - SmartDashboard.putNumber("SuperStructure/SOTM/calculated turret angle", sol.targetTurretAngle().getDegrees()); - SmartDashboard.putNumber("SuperStructure/SOTM/calculated hood angle", sol.targetHoodAngle().getDegrees()); - SmartDashboard.putNumber("SuperStructure/SOTM/calculated flight time", sol.flightTime()); - SmartDashboard.putNumber("SuperStructure/SOTM/turret dist to virtual pose", futureTurretPose.getTranslation().getDistance(sol.virtualPose().getTranslation())); + SmartDashboard.putNumber("Superstructure/SOTM/calculated turret angle", sol.targetTurretAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/SOTM/calculated hood angle", sol.targetHoodAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/SOTM/calculated flight time", sol.flightTime()); + SmartDashboard.putNumber("Superstructure/SOTM/turret dist to virtual pose", futureTurretPose.getTranslation().getDistance(sol.virtualPose().getTranslation())); } public static Rotation2d calculateHoodAngleSOTM() { @@ -209,7 +209,7 @@ public static Rotation2d calculateTurretAngleSOTM() { return sol.targetTurretAngle(); } - public static Double calculateShooterRPMSOTM() { + public static double calculateShooterRPMSOTM() { return sol.targetShooterRPM(); } } \ No newline at end of file From 3aefa202e4fd22d0e2c5f47c27b00310c789dcf4 Mon Sep 17 00:00:00 2001 From: Xuan Hao Date: Thu, 5 Mar 2026 01:50:11 -0500 Subject: [PATCH 076/150] REFACTOR: collapse the constants interface in settings into their respective subsystems REFACTOR: rename redundant variable names such as shoot_rpm stored in the rpm interface --- .../stuypulse/robot/constants/Settings.java | 112 +++++++++--------- .../climberhopper/ClimberHopperImpl.java | 6 +- .../climberhopper/ClimberHopperSim.java | 12 +- .../subsystems/spindexer/SpindexerImpl.java | 8 +- .../subsystems/superstructure/hood/Hood.java | 14 +-- .../superstructure/hood/HoodImpl.java | 4 +- .../superstructure/shooter/Shooter.java | 10 +- .../superstructure/turret/TurretImpl.java | 8 +- .../swerve/CommandSwerveDrivetrain.java | 2 +- .../superstructure/TurretAngleCalculator.java | 2 +- 10 files changed, 88 insertions(+), 90 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 4ff64aa5..72a2f808 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -37,24 +37,24 @@ public interface HubDATA { } public interface ClimberHopper { - public interface Constants { - public final double GEAR_RATIO = 45.0; + /* CONSTANTS */ + public final double GEAR_RATIO = 45.0; - public final double MIN_HEIGHT_METERS = 0.0; - public final double MAX_HEIGHT_METERS = 1.0; + public final double MIN_HEIGHT_METERS = 0.0; + public final double MAX_HEIGHT_METERS = 1.0; - public final double MASS_KG = 1.0; + public final double MASS_KG = 1.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; + 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; - public final double DRUM_RADIUS_METERS = ((MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (NUM_ROTATIONS_TO_REACH_TOP / GEAR_RATIO)) / 2.0 / Math.PI; - } + public final double DRUM_RADIUS_METERS = ((MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (NUM_ROTATIONS_TO_REACH_TOP / GEAR_RATIO)) / 2.0 / Math.PI; + /* CONSTANTS */ - public final double CLIMBER_UP_HEIGHT_METERS = Constants.MAX_HEIGHT_METERS; + public final double CLIMBER_UP_HEIGHT_METERS = 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_DOWN_HEIGHT_METERS = MIN_HEIGHT_METERS; public final double HOPPER_UP_HEIGHT_METERS = 0.5; public final double STALL = 10.0; @@ -117,9 +117,8 @@ public interface Spindexer { double RPM_TOLERANCE = 400.0; - public interface Constants { - double GEAR_RATIO = 8.0 / 1.0; - } + /* CONSTANTS */ + double GEAR_RATIO = 8.0 / 1.0; } public interface Superstructure { @@ -175,13 +174,13 @@ public interface Shooter { public final double GEAR_RATIO = 1.0; public final double FLYWHEEL_RADIUS = Units.inchesToMeters(3.965 / 2); - public interface RPMs { - public final SmartNumber SHOOT_RPM = new SmartNumber("Superstructure/Shoot State Target RPM", 3500.0); - public final SmartNumber FERRY_RPM = new SmartNumber("Superstructure/Ferry State Target RPM", 2000.0); + public interface RPM { + public final SmartNumber SHOOT = new SmartNumber("Superstructure/Shoot State Target RPM", 3500.0); + public final SmartNumber FERRY = new SmartNumber("Superstructure/Ferry State Target RPM", 2000.0); public final double REVERSE = 0.0; - public final double KB_RPM = 0.0; - public final double LEFT_CORNER_RPM = 0.0; - public final double RIGHT_CORNER_RPM = 0.0; + public final double KB = 0.0; + public final double LEFT_CORNER = 0.0; + public final double RIGHT_CORNER = 0.0; } } @@ -211,16 +210,16 @@ public interface Hood { public final double STALL_DEBOUNCE = 0.5; public interface Angles { - public final SmartNumber SHOOT_ANGLE = new SmartNumber("Superstructure/Shoot State Target Angle (deg)", 15.0); - public final SmartNumber FERRY_ANGLE = new SmartNumber("Superstructure/Ferry State Target Angle (deg)", 20.0); + public final SmartNumber SHOOT = new SmartNumber("Superstructure/Shoot State Target Angle (deg)", 15.0); + public final SmartNumber FERRY = new SmartNumber("Superstructure/Ferry State Target Angle (deg)", 20.0); - public final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(7.0); - public final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(40.0); + public final Rotation2d MIN = Rotation2d.fromDegrees(7.0); + public final Rotation2d MAX = Rotation2d.fromDegrees(40.0); - public final Rotation2d UNDER_TRENCH_ANGLE = Rotation2d.fromDegrees(11.0); - public final Rotation2d KB_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 final Rotation2d UNDER_TRENCH = Rotation2d.fromDegrees(11.0); + public final Rotation2d KB = Rotation2d.fromDegrees(12.0); + public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(10.0); + public final Rotation2d RIGHT_CORNER = Rotation2d.fromDegrees(10.0); } } @@ -238,34 +237,33 @@ public interface Turret { Rotation2d MAX_THEORETICAL_ROTATION = Rotation2d.fromDegrees(612); Rotation2d MIN_THEORETICAL_ROTATION = Rotation2d.fromDegrees(-612); - public interface Constants { - public final double RANGE = 210.0; - - public final double SLOT_SWITCHING_THRESHOLD_ROT = .5; - - public final Transform2d TURRET_OFFSET = new Transform2d(Units.inchesToMeters(-4.0), Units.inchesToMeters(8.0), Rotation2d.kZero); - public final double TURRET_HEIGHT = Units.inchesToMeters(0.0); - - public final double GEAR_RATIO_MOTOR_TO_MECH = (60.0 / 9.0) * (95.0 / 12.0); //1425.0 / 36.0; - - public interface BigGear { - public final int TEETH = 95; - } - - 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; - } + /* CONSTANTS */ + public final double RANGE = 210.0; + + public final double SLOT_SWITCHING_THRESHOLD_ROT = .5; + + public final Transform2d TURRET_OFFSET = new Transform2d(Units.inchesToMeters(-4.0), Units.inchesToMeters(8.0), Rotation2d.kZero); + public final double TURRET_HEIGHT = Units.inchesToMeters(0.0); + + public final double GEAR_RATIO_MOTOR_TO_MECH = (60.0 / 9.0) * (95.0 / 12.0); //1425.0 / 36.0; + + public interface BigGear { + public final int TEETH = 95; + } + + 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; } } 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 b4368e53..f601a9c0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java @@ -45,7 +45,7 @@ public ClimberHopperImpl() { .withSoftLimits( false, false, - Settings.ClimberHopper.ROTATIONS_AT_BOTTOM + Settings.ClimberHopper.Constants.NUM_ROTATIONS_TO_REACH_TOP, + Settings.ClimberHopper.ROTATIONS_AT_BOTTOM + Settings.ClimberHopper.NUM_ROTATIONS_TO_REACH_TOP, Settings.ClimberHopper.ROTATIONS_AT_BOTTOM); motor = new TalonFX(Ports.ClimberHopper.CLIMBER_HOPPER, Ports.CANIVORE); @@ -68,7 +68,7 @@ public boolean getStalling() { @Override public double getCurrentHeight() { - return this.motor.getPosition().getValueAsDouble() * Settings.ClimberHopper.Constants.POSITION_CONVERSION_FACTOR; + return this.motor.getPosition().getValueAsDouble() * Settings.ClimberHopper.POSITION_CONVERSION_FACTOR; } private boolean isWithinTolerance(double toleranceMeters) { @@ -81,7 +81,7 @@ public boolean atTargetHeight() { } public void resetPostionUpper() { - motor.setPosition(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM + Settings.ClimberHopper.Constants.NUM_ROTATIONS_TO_REACH_TOP); + motor.setPosition(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM + Settings.ClimberHopper.NUM_ROTATIONS_TO_REACH_TOP); } @Override 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 9ae11e3e..18b1eae9 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java @@ -28,13 +28,13 @@ public ClimberHopperSim() { sim = new ElevatorSim( DCMotor.getKrakenX60(1), - Settings.ClimberHopper.Constants.GEAR_RATIO, - Settings.ClimberHopper.Constants.MASS_KG, - Settings.ClimberHopper.Constants.DRUM_RADIUS_METERS, - Settings.ClimberHopper.Constants.MIN_HEIGHT_METERS, - Settings.ClimberHopper.Constants.MAX_HEIGHT_METERS, + Settings.ClimberHopper.GEAR_RATIO, + Settings.ClimberHopper.MASS_KG, + Settings.ClimberHopper.DRUM_RADIUS_METERS, + Settings.ClimberHopper.MIN_HEIGHT_METERS, + Settings.ClimberHopper.MAX_HEIGHT_METERS, false, - Settings.ClimberHopper.Constants.MIN_HEIGHT_METERS + Settings.ClimberHopper.MIN_HEIGHT_METERS ); voltageOverride = Optional.empty(); 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 1922d2e3..8ddf9e2a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -47,7 +47,7 @@ public SpindexerImpl() { .withPIDConstants(Gains.Spindexer.kP, Gains.Spindexer.kI, Gains.Spindexer.kD, 0) .withFFConstants(Gains.Spindexer.kS, Gains.Spindexer.kV, Gains.Spindexer.kA, 0) - .withSensorToMechanismRatio(Settings.Spindexer.Constants.GEAR_RATIO); + .withSensorToMechanismRatio(Settings.Spindexer.GEAR_RATIO); spindexerFollowerConfig = new Motors.TalonFXConfig() .withInvertedValue(InvertedValue.Clockwise_Positive) @@ -60,7 +60,7 @@ public SpindexerImpl() { .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); + .withSensorToMechanismRatio(Settings.Spindexer.GEAR_RATIO); leadMotor = new TalonFX(Ports.Spindexer.SPINDEXER_LEAD_MOTOR, Ports.CANIVORE); followerMotor = new TalonFX(Ports.Spindexer.SPINDEXER_FOLLOW_MOTOR, Ports.CANIVORE); @@ -75,11 +75,11 @@ public SpindexerImpl() { } private double getCurrentLeadMotorRPM() { - return leadMotor.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE * Settings.Spindexer.Constants.GEAR_RATIO; + return leadMotor.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE * Settings.Spindexer.GEAR_RATIO; } private double getCurrentFollowerMotorRPM() { - return followerMotor.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE * Settings.Spindexer.Constants.GEAR_RATIO; + return followerMotor.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE * Settings.Spindexer.GEAR_RATIO; } private boolean atTolerance() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java index 7ed5db47..05235273 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java @@ -57,12 +57,12 @@ public void setState(HoodState state){ public Rotation2d getTargetAngle() { return switch(state) { - case STOW -> Settings.Superstructure.Hood.Angles.MIN_ANGLE; + case STOW -> Settings.Superstructure.Hood.Angles.MIN; case FERRY -> Rotation2d.fromDegrees(30); - case SHOOT -> Rotation2d.fromDegrees(Settings.Superstructure.Hood.Angles.SHOOT_ANGLE.get()); - case KB -> Settings.Superstructure.Hood.Angles.KB_ANGLE; - case LEFT_CORNER -> Settings.Superstructure.Hood.Angles.LEFT_CORNER_ANGLE; - case RIGHT_CORNER -> Settings.Superstructure.Hood.Angles.RIGHT_CORNER_ANGLE; + case SHOOT -> Rotation2d.fromDegrees(Settings.Superstructure.Hood.Angles.SHOOT.get()); + case KB -> Settings.Superstructure.Hood.Angles.KB; + case LEFT_CORNER -> Settings.Superstructure.Hood.Angles.LEFT_CORNER; + case RIGHT_CORNER -> Settings.Superstructure.Hood.Angles.RIGHT_CORNER; case INTERPOLATION -> InterpolationCalculator.interpolateShotInfo().targetHoodAngle(); case SOTM -> SOTMCalculator.calculateHoodAngleSOTM(); case ANALOG -> hoodAnalogToOutput(); @@ -78,8 +78,8 @@ public boolean atTolerance() { public abstract Rotation2d getAngle(); public void hoodAnalogToInput(Gamepad gamepad) { - double hoodMin = Settings.Superstructure.Hood.Angles.MIN_ANGLE.getDegrees(); - double hoodMax = Settings.Superstructure.Hood.Angles.MAX_ANGLE.getDegrees(); + double hoodMin = Settings.Superstructure.Hood.Angles.MIN.getDegrees(); + double hoodMax = Settings.Superstructure.Hood.Angles.MAX.getDegrees(); this.driverInput = Rotation2d.fromDegrees(hoodMin + (gamepad.getLeftX() + 1.0) * ((hoodMax - hoodMin) / 2)); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index ddf67b72..adf3e0f7 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -96,7 +96,7 @@ public void zeroHoodEncoderAtLowerHardstop() { double currentOffset = hoodEncoderConfig.getConfiguration().MagnetSensor.MagnetOffset; double positionWithCurrentOffset = hoodEncoder.getPosition().getValueAsDouble(); - double newOffset = -((positionWithCurrentOffset - currentOffset) - Settings.Superstructure.Hood.Angles.MIN_ANGLE.getRotations()); + double newOffset = -((positionWithCurrentOffset - currentOffset) - Settings.Superstructure.Hood.Angles.MIN.getRotations()); hoodEncoderConfig.withMagnetOffset(newOffset); @@ -119,7 +119,7 @@ public void seedHood() { } private double getAbsoluteHoodAngleDeg() { - return Settings.Superstructure.Hood.Angles.MIN_ANGLE.getDegrees() + hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.Superstructure.Hood.ENCODER_TO_MECH; + return Settings.Superstructure.Hood.Angles.MIN.getDegrees() + hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.Superstructure.Hood.ENCODER_TO_MECH; } @Override diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index 129f36d0..ff6be1fc 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java @@ -60,17 +60,17 @@ public double getTargetRPM() { case STOP -> 0; case SHOOT -> getShootRPM(); case FERRY -> InterpolationCalculator.interpolateFerryingRPM(); - case REVERSE -> Settings.Superstructure.Shooter.RPMs.REVERSE; - case KB -> Settings.Superstructure.Shooter.RPMs.KB_RPM; - case LEFT_CORNER -> Settings.Superstructure.Shooter.RPMs.LEFT_CORNER_RPM; - case RIGHT_CORNER -> Settings.Superstructure.Shooter.RPMs.RIGHT_CORNER_RPM; + case REVERSE -> Settings.Superstructure.Shooter.RPM.REVERSE; + case KB -> Settings.Superstructure.Shooter.RPM.KB; + case LEFT_CORNER -> Settings.Superstructure.Shooter.RPM.LEFT_CORNER; + case RIGHT_CORNER -> Settings.Superstructure.Shooter.RPM.RIGHT_CORNER; case INTERPOLATION -> InterpolationCalculator.interpolateShotInfo().targetRPM(); case SOTM -> SOTMCalculator.calculateShooterRPMSOTM(); }; } public double getShootRPM() { - return Settings.Superstructure.Shooter.RPMs.SHOOT_RPM.get(); // Adjustable RPM on Glass + return Settings.Superstructure.Shooter.RPM.SHOOT.get(); // Adjustable RPM on Glass } public boolean atTolerance() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index 700d1734..4b736146 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -57,12 +57,12 @@ public TurretImpl() { .withFFConstants(Gains.Superstructure.Turret.slot1.kS, Gains.Superstructure.Turret.slot1.kV, Gains.Superstructure.Turret.slot1.kA, 1) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 1) - .withSensorToMechanismRatio(Settings.Superstructure.Turret.Constants.GEAR_RATIO_MOTOR_TO_MECH) + .withSensorToMechanismRatio(Settings.Superstructure.Turret.GEAR_RATIO_MOTOR_TO_MECH) .withSoftLimits( false, false, - Settings.Superstructure.Turret.Constants.SoftwareLimit.FORWARD_MAX_ROTATIONS, - Settings.Superstructure.Turret.Constants.SoftwareLimit.BACKWARDS_MAX_ROTATIONS); + Settings.Superstructure.Turret.SoftwareLimit.FORWARD_MAX_ROTATIONS, + Settings.Superstructure.Turret.SoftwareLimit.BACKWARDS_MAX_ROTATIONS); encoder17tConfig = new Motors.CANCoderConfig() .withSensorDirection(SensorDirectionValue.Clockwise_Positive) @@ -139,7 +139,7 @@ private double getDelta(double target, double current) { if (delta > 180.0) delta -= 360; else if (delta < -180) delta += 360; - if (Math.abs(current + delta) < Settings.Superstructure.Turret.Constants.RANGE) return delta; + if (Math.abs(current + delta) < Settings.Superstructure.Turret.RANGE) return delta; return delta < 0 ? delta + 360 : delta - 360; } 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 73a58457..8812b506 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -449,7 +449,7 @@ public double[] getWheelDrivePositionsRadians(){ public void periodic() { Pose2d pose = getPose(); turretPose = new Pose2d( - pose.getTranslation().plus(Settings.Superstructure.Turret.Constants.TURRET_OFFSET.getTranslation().rotateBy(pose.getRotation())), + pose.getTranslation().plus(Settings.Superstructure.Turret.TURRET_OFFSET.getTranslation().rotateBy(pose.getRotation())), pose.getRotation().plus(Turret.getInstance().getAngle()) ); diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/TurretAngleCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/TurretAngleCalculator.java index a9d1dab3..5454983f 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/TurretAngleCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/TurretAngleCalculator.java @@ -27,7 +27,7 @@ public class TurretAngleCalculator { private static double[] generateEncoderValues(int teeth) { double[] values = new double[NUM_POINTS]; - double gearRatio = 1.0 * Settings.Superstructure.Turret.Constants.BigGear.TEETH / teeth; + double gearRatio = 1.0 * Settings.Superstructure.Turret.BigGear.TEETH / teeth; int i = 0; for (double angle = MIN_ANGLE_DEGREES; angle < MAX_ANGLE_DEGREES; angle += RESOLUTION) { From 832a3023d1006dd61a266f5150e5fae54aefe6af Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Thu, 5 Mar 2026 15:00:51 -0500 Subject: [PATCH 077/150] FIX: swerve align to hub should cross wheels in end(), fix max velocity --- .../swerve/SwerveDriveAlignTurretToHub.java | 19 +++++++---- .../pidToPose/SwerveDrivePIDToPose.java | 6 ++-- .../stuypulse/robot/constants/Settings.java | 34 ++++++++----------- 3 files changed, 30 insertions(+), 29 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java index 49c6758d..7c8ff025 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java @@ -6,6 +6,8 @@ package com.stuypulse.robot.commands.swerve; +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.ctre.phoenix6.swerve.SwerveRequest.SwerveDriveBrake; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Gains.Swerve.Alignment; import com.stuypulse.robot.constants.Settings; @@ -36,7 +38,7 @@ public SwerveDriveAlignTurretToHub() { turret = Turret.getInstance(); angleController = new AnglePIDController(Alignment.THETA.kP, Alignment.THETA.kI, Alignment.THETA.kD) - .setSetpointFilter(new AMotionProfile(Settings.Swerve.Alignment.Constraints.DEFAULT_MAX_ANGULAR_VELOCITY, Settings.Swerve.Alignment.Constraints.DEFAULT_MAX_ANGULAR_ACCELERATION)); + .setSetpointFilter(new AMotionProfile(Settings.Swerve.Constraints.MAX_ANGULAR_VEL_RAD_PER_S, Settings.Swerve.Constraints.MAX_ANGULAR_ACCEL_RAD_PER_S_SQUARED)); addRequirements(swerve); } @@ -45,11 +47,6 @@ protected double getAngleError() { return angleController.getError().getRotation2d().getDegrees(); } - @Override - public boolean isFinished() { - return angleController.isDoneDegrees(Settings.Swerve.Alignment.Tolerances.THETA_TOLERANCE_DEG); - } - public Rotation2d getTargetAngle() { Translation2d robot = swerve.getPose().getTranslation(); Translation2d hub = Field.getHubPose().getTranslation(); @@ -70,4 +67,14 @@ public void execute() { SmartDashboard.putNumber("Swerve/Angle Error", angleController.getError().toDegrees()); SmartDashboard.putNumber("Swerve/Target Angle Hub Deg", TurretAngleCalculator.getPointAtTargetAngle(Field.getHubPose().getTranslation(), swerve.getTurretPose().getTranslation()).getDegrees()); } + + @Override + public boolean isFinished() { + return angleController.isDoneDegrees(Settings.Swerve.Alignment.Tolerances.THETA_TOLERANCE_DEG); + } + + @Override + public void end(boolean interrupted) { + swerve.setControl(new SwerveRequest.SwerveDriveBrake()); + } } \ No newline at end of file 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 b68b37f7..401726f5 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,10 +70,10 @@ 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.DEFAULT_MAX_ANGULAR_VELOCITY, Settings.Swerve.Alignment.Constraints.DEFAULT_MAX_ANGULAR_ACCELERATION))); + .setSetpointFilter(new AMotionProfile(Settings.Swerve.Constraints.MAX_ANGULAR_VEL_RAD_PER_S, Settings.Swerve.Constraints.MAX_ANGULAR_ACCEL_RAD_PER_S_SQUARED))); - maxVelocity = Settings.Swerve.Alignment.Constraints.DEFAULT_MAX_VELOCITY; - maxAcceleration = Settings.Swerve.Alignment.Constraints.DEFAULT_MAX_ACCELERATION; + maxVelocity = Settings.Swerve.Constraints.MAX_VELOCITY_M_PER_S; + maxAcceleration = Settings.Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED; isMotionProfiled = true; translationSetpoint = getNewTranslationSetpointGenerator(); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 72a2f808..48c554c7 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -279,13 +279,21 @@ public interface Swerve { public final double MODULE_VELOCITY_DEADBAND_M_PER_S = 0.1; public final double ROTATIONAL_DEADBAND_RAD_PER_S = 0.1; + public interface Constraints { + public final double MAX_VELOCITY_M_PER_S = 4.93; // 4.3p + public final double MAX_ACCEL_M_PER_S_SQUARED = 15.0; + public final double MAX_ANGULAR_VEL_RAD_PER_S = Units.degreesToRadians(600.0); + public final double MAX_ANGULAR_ACCEL_RAD_PER_S_SQUARED = Units.degreesToRadians(900.0); + + 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_SQUARED); + } + public interface Alignment { - public interface Constraints { - public final double DEFAULT_MAX_VELOCITY = 4.93; - public final double DEFAULT_MAX_ACCELERATION = 15.0; - public final double DEFAULT_MAX_ANGULAR_VELOCITY = Units.degreesToRadians(600.0); - public final double DEFAULT_MAX_ANGULAR_ACCELERATION = Units.degreesToRadians(900.0); - } public interface Targets {} @@ -304,20 +312,6 @@ public interface Tolerances { public final double ALIGNMENT_DEBOUNCE = 0.15; } } - - public interface Constraints { - public final double MAX_VELOCITY_M_PER_S = 1.0; // 4.3p - 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 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 Vision { From 25462a7e76ce935f9f14d3dea4abdc09ec0290cc Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Thu, 5 Mar 2026 16:06:04 -0500 Subject: [PATCH 078/150] FEAT + CLEAN + FIX: add HoodSim and VisualizerHood, cleanup logging across the big 3, fix under the trench stowing logic for hood --- simgui.json | 14 +- .../com/stuypulse/robot/RobotContainer.java | 37 +++--- .../superstructure/SuperstructureFerry.java | 2 +- .../superstructure/SuperstructureStow.java | 4 +- .../stuypulse/robot/constants/Settings.java | 10 +- .../superstructure/Superstructure.java | 40 +----- .../subsystems/superstructure/hood/Hood.java | 33 ++++- .../superstructure/hood/HoodImpl.java | 25 +++- .../superstructure/hood/HoodSim.java | 125 ++++++++++++++++++ .../superstructure/shooter/Shooter.java | 2 - .../superstructure/turret/Turret.java | 11 +- .../superstructure/turret/TurretImpl.java | 2 +- .../InterpolationCalculator.java | 9 +- .../util/superstructure/VisualizerHood.java | 73 ++++++++++ ...tVisualizer.java => VisualizerTurret.java} | 10 +- 15 files changed, 300 insertions(+), 97 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java create mode 100644 src/main/java/com/stuypulse/robot/util/superstructure/VisualizerHood.java rename src/main/java/com/stuypulse/robot/util/superstructure/{TurretVisualizer.java => VisualizerTurret.java} (89%) diff --git a/simgui.json b/simgui.json index 849c61c5..1acb9155 100644 --- a/simgui.json +++ b/simgui.json @@ -22,6 +22,7 @@ "/SmartDashboard/Robot/Zero Pivot Encoder at Upper Limit (Stowed)": "Command", "/SmartDashboard/Robot/Zero Turret Encoders": "Command", "/SmartDashboard/Visualizers/ClimberHopper": "Mechanism2d", + "/SmartDashboard/Visualizers/Hood": "Mechanism2d", "/SmartDashboard/Visualizers/Turret": "Mechanism2d" }, "windows": { @@ -41,6 +42,11 @@ "visible": true } }, + "/SmartDashboard/Visualizers/Hood": { + "window": { + "visible": true + } + }, "/SmartDashboard/Visualizers/Turret": { "window": { "visible": true @@ -57,19 +63,13 @@ }, "transitory": { "SmartDashboard": { - "Enabled Subsystems": { - "open": true - }, "HoodedShooter": { "Hood": { "open": true }, "open": true }, - "States": { - "open": true - }, - "Swerve": { + "Superstructure": { "open": true }, "Turret": { diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 6d34795e..44347880 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -157,26 +157,27 @@ private void configureButtonBindings() { driver.getLeftTriggerButton() .onTrue(new IntakeStow()); - driver.getLeftButton() - .whileTrue(new SuperstructureShoot().onlyIf(() -> !superstructure.isHoodUnderTrench())) - .onFalse(new SuperstructureStow()); - - - // driver.getRightButton() - // .whileTrue(new SuperstructureFerry().onlyIf( - // () -> CommandSwerveDrivetrain.getInstance().getPose().getX() > Field.getHubPose().getX())) - // .onFalse(new SuperstructureStow()); + driver.getRightButton() + .whileTrue(new SuperstructureFerry().onlyIf(() -> !superstructure.isHoodUnderTrench()) + // .alongWith(new TurretShoot()) + .andThen(new WaitUntilCommand(superstructure::atTolerance)) + .andThen(new HandoffRun().onlyIf(superstructure::atTolerance) + .alongWith(new WaitUntilCommand(handoff::atTolerance)) + .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) + .onFalse(new SpindexerStop() + .alongWith(new SuperstructureStow()) + .alongWith(new HandoffStop())); // SOTM - driver.getRightButton() - .whileTrue(new SuperstructureSOTM().onlyIf(() -> !superstructure.isHoodUnderTrench()) - .andThen(new WaitUntilCommand(superstructure::atTolerance)) - .andThen(new HandoffRun().onlyIf(superstructure::atTolerance) - .alongWith(new WaitUntilCommand(handoff::atTolerance)) - .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) - .onFalse(new SpindexerStop() - .alongWith(new SuperstructureStow()) - .alongWith(new HandoffStop())); + // driver.getRightButton() + // .whileTrue(new SuperstructureSOTM().onlyIf(() -> !superstructure.isHoodUnderTrench()) + // .andThen(new WaitUntilCommand(superstructure::atTolerance)) + // .andThen(new HandoffRun().onlyIf(superstructure::atTolerance) + // .alongWith(new WaitUntilCommand(handoff::atTolerance)) + // .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) + // .onFalse(new SpindexerStop() + // .alongWith(new SuperstructureStow()) + // .alongWith(new HandoffStop())); // Reset Heading driver.getDPadUp() diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureFerry.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureFerry.java index bf05fdcd..755b9642 100644 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureFerry.java @@ -8,7 +8,7 @@ import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; public class SuperstructureFerry extends SuperstructureSetState { - public SuperstructureFerry(){ + public SuperstructureFerry() { super(SuperstructureState.FERRY); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureStow.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureStow.java index 5838c414..0fbac6ea 100644 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureStow.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureStow.java @@ -7,8 +7,8 @@ import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -public class SuperstructureStow extends SuperstructureSetState{ - public SuperstructureStow(){ +public class SuperstructureStow extends SuperstructureSetState { + public SuperstructureStow() { super(SuperstructureState.STOW); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 48c554c7..fe247734 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -175,8 +175,8 @@ public interface Shooter { public final double FLYWHEEL_RADIUS = Units.inchesToMeters(3.965 / 2); public interface RPM { - public final SmartNumber SHOOT = new SmartNumber("Superstructure/Shoot State Target RPM", 3500.0); - public final SmartNumber FERRY = new SmartNumber("Superstructure/Ferry State Target RPM", 2000.0); + public final SmartNumber SHOOT = new SmartNumber("InterpolationTesting/Shoot State Target RPM", 3500.0); + public final SmartNumber FERRY = new SmartNumber("InterpolationTesting/Ferry State Target RPM", 2000.0); public final double REVERSE = 0.0; public final double KB = 0.0; public final double LEFT_CORNER = 0.0; @@ -210,13 +210,13 @@ public interface Hood { public final double STALL_DEBOUNCE = 0.5; public interface Angles { - public final SmartNumber SHOOT = new SmartNumber("Superstructure/Shoot State Target Angle (deg)", 15.0); - public final SmartNumber FERRY = new SmartNumber("Superstructure/Ferry State Target Angle (deg)", 20.0); + public final SmartNumber SHOOT = new SmartNumber("InterpolationTesting/Shoot State Target Angle (deg)", 15.0); + public final SmartNumber FERRY = new SmartNumber("InterpolationTesting/Ferry State Target Angle (deg)", 20.0); public final Rotation2d MIN = Rotation2d.fromDegrees(7.0); public final Rotation2d MAX = Rotation2d.fromDegrees(40.0); - public final Rotation2d UNDER_TRENCH = Rotation2d.fromDegrees(11.0); + public final Rotation2d STOW = Rotation2d.fromDegrees(11.0); public final Rotation2d KB = Rotation2d.fromDegrees(12.0); public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(10.0); public final Rotation2d RIGHT_CORNER = Rotation2d.fromDegrees(10.0); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index ede1dd5e..e56f17ca 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -95,19 +95,7 @@ public boolean atTolerance() { } public boolean isHoodUnderTrench() { - Pose2d pose = CommandSwerveDrivetrain.getInstance().getTurretPose(); - - 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 isCloseToAllianceSideTrenchX = Math.abs(pose.getX() - Field.NearRightTrench.rightEdge.getX()) < Field.trenchHoodTolerance; - - boolean isCloseToNeutralSideTrenchX = Math.abs(pose.getX() - Field.FarRightTrench.rightEdge.getX()) < Field.trenchHoodTolerance; - - boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) && (isCloseToAllianceSideTrenchX || isCloseToNeutralSideTrenchX); - - return isUnderTrench; + return hood.isUnderTrench(); } public boolean isShooterAtTolerance() { @@ -119,21 +107,13 @@ public boolean isHoodAtTolerance() { } public boolean isTurretAtTolerance(){ - return turret.atTargetAngle(); + return turret.atTolerance(); } public double getTargetRPM() { return shooter.getTargetRPM(); } - public Rotation2d getTargetYaw() { - return hood.getTargetAngle(); - } - - public Rotation2d getTargetPitch(){ - return turret.getTargetAngle(); - } - public double getShooterRPM() { return shooter.getRPM(); } @@ -155,22 +135,12 @@ public void periodic() { SmartDashboard.putString("Superstructure/State", state.name()); SmartDashboard.putString("States/SuperStructure", state.name()); - SmartDashboard.putNumber("Superstructure/Target RPM", getTargetRPM()); - SmartDashboard.putNumber("Superstructure/Target Yaw", getTargetYaw().getDegrees()); - SmartDashboard.putNumber("Superstructure/Target Pitch", getTargetPitch().getDegrees()); - - SmartDashboard.putNumber("Superstructure/Current RPM", getShooterRPM()); - SmartDashboard.putNumber("Superstructure/Current Yaw", getTurretAngle().getDegrees()); - SmartDashboard.putNumber("Superstructure/Current Pitch", getHoodAngle().getDegrees()); - - SmartDashboard.putNumber("Superstructure/Hood Angle Error (Deg)", getTargetPitch().getDegrees() - getHoodAngle().getDegrees()); - SmartDashboard.putBoolean("Superstructure/Shooter At Tolerance?", isShooterAtTolerance()); SmartDashboard.putBoolean("Superstructure/Hood At Tolerance?", isHoodAtTolerance()); - SmartDashboard.putBoolean("Superstructure/Turret At Tolerant?", isHoodAtTolerance()); + SmartDashboard.putBoolean("Superstructure/Turret At Tolerance?", isTurretAtTolerance()); SmartDashboard.putBoolean("Superstructure/Hood/Under Trench", isHoodUnderTrench()); - SmartDashboard.putNumber("InterpolationTesting/Hood Angle", getHoodAngle().getDegrees()); - SmartDashboard.putNumber("InterpolationTesting/Shooter RPM", getShooterRPM()); + SmartDashboard.putNumber("InterpolationTesting/Current Hood Angle", getHoodAngle().getDegrees()); + SmartDashboard.putNumber("InterpolationTesting/Current Shooter RPM", getShooterRPM()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java index 05235273..e60e188c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java @@ -5,8 +5,11 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure.hood; +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.superstructure.SOTMCalculator; +import com.stuypulse.robot.util.superstructure.VisualizerHood; import com.stuypulse.robot.util.superstructure.InterpolationCalculator; import com.stuypulse.stuylib.input.Gamepad; @@ -23,7 +26,11 @@ public abstract class Hood extends SubsystemBase{ private Rotation2d driverInput; static { - instance = new HoodImpl(); + if (Robot.isReal()) { + instance = new HoodImpl(); + } else { + instance = new HoodSim(); + } } public static Hood getInstance(){ @@ -56,8 +63,12 @@ public void setState(HoodState state){ } public Rotation2d getTargetAngle() { + if (isUnderTrench()) { + return Settings.Superstructure.Hood.Angles.STOW; + } + return switch(state) { - case STOW -> Settings.Superstructure.Hood.Angles.MIN; + case STOW -> Settings.Superstructure.Hood.Angles.STOW; case FERRY -> Rotation2d.fromDegrees(30); case SHOOT -> Rotation2d.fromDegrees(Settings.Superstructure.Hood.Angles.SHOOT.get()); case KB -> Settings.Superstructure.Hood.Angles.KB; @@ -72,7 +83,11 @@ public Rotation2d getTargetAngle() { public boolean atTolerance() { double error = getAngle().minus(getTargetAngle()).getRotations(); - return Math.abs(error) < Settings.Superstructure.HOOD_TOLERANCE.getRotations(); + if (Robot.isReal()) { + return Math.abs(error) < Settings.Superstructure.HOOD_TOLERANCE.getRotations(); + } else { + return Math.abs(error) < Settings.Superstructure.HOOD_TOLERANCE.getRotations() + (0.5 / 360.0); + } } public abstract Rotation2d getAngle(); @@ -88,8 +103,11 @@ public Rotation2d hoodAnalogToOutput() { return this.driverInput; } + public abstract boolean isUnderTrench(); public abstract boolean isStalling(); + public abstract SysIdRoutine getHoodSysIdRoutine(); + public abstract void zeroHoodEncoderAtLowerHardstop(); public abstract void seedHood(); @@ -101,7 +119,12 @@ public void periodic() { SmartDashboard.putNumber("Superstructure/Hood/Target Angle", getTargetAngle().getDegrees()); SmartDashboard.putNumber("Superstructure/Hood/Current Angle", getAngle().getDegrees()); - //SmartDashboard.putNumber("Superstructure/Hood/Analog Target Angle", hoodAnalogToOutput().getDegrees()); - + if (Settings.DEBUG_MODE) { + if (EnabledSubsystems.HOOD.get()) { + VisualizerHood.getInstance().update(getAngle(), atTolerance()); + } else { + // VisualizerHood.getInstance().update(new Rotation2d(), false); + } + } } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index adf3e0f7..242d81c5 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -6,15 +6,18 @@ package com.stuypulse.robot.subsystems.superstructure.hood; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Gains; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.SysId; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -107,6 +110,23 @@ public void zeroHoodEncoderAtLowerHardstop() { public boolean isStalling() { return isStalling.getAsBoolean(); } + + @Override + public boolean isUnderTrench() { + Pose2d pose = CommandSwerveDrivetrain.getInstance().getTurretPose(); + + 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 isCloseToAllianceSideTrenchX = Math.abs(pose.getX() - Field.NearRightTrench.rightEdge.getX()) < Field.trenchHoodTolerance; + + boolean isCloseToNeutralSideTrenchX = Math.abs(pose.getX() - Field.FarRightTrench.rightEdge.getX()) < Field.trenchHoodTolerance; + + boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) && (isCloseToAllianceSideTrenchX || isCloseToNeutralSideTrenchX); + + return isUnderTrench; + } /* * Example: @@ -135,9 +155,6 @@ public void periodic() { if (voltageOverride.isPresent()) { hoodMotor.setVoltage(voltageOverride.get()); } else { - if (Superstructure.getInstance().isHoodUnderTrench()) { - setState(HoodState.STOW); - } hoodMotor.setControl(controller.withPosition(getTargetAngle().getRotations())); } } else { @@ -156,8 +173,6 @@ public void periodic() { SmartDashboard.putNumber("Superstructure/Hood/Raw Motor Encoder Value", hoodMotor.getPosition().getValueAsDouble()); - SmartDashboard.putNumber("InterpolationTesting/Hood Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("Current Draws/Hood (amps)", hoodMotor.getSupplyCurrent().getValueAsDouble()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java new file mode 100644 index 00000000..91f4a9d9 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java @@ -0,0 +1,125 @@ +/************************ 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.subsystems.superstructure.hood; + +import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.SysId; +import com.stuypulse.robot.util.superstructure.VisualizerHood; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +import java.util.Optional; + +public class HoodSim extends Hood { + + // Simulates hood motion as a linear elevator, then converts to angle + private final ElevatorSim sim; + + private Optional voltageOverride; + + // Arc length constants — tune to match your hood geometry + private static final double HOOD_ARM_LENGTH_METERS = 0.3; + + private static final double MIN_HEIGHT = HOOD_ARM_LENGTH_METERS * + Math.sin(Settings.Superstructure.Hood.Angles.MIN.getRadians()); + private static final double MAX_HEIGHT = HOOD_ARM_LENGTH_METERS * + Math.sin(Settings.Superstructure.Hood.Angles.MAX.getRadians()); + + public HoodSim() { + sim = new ElevatorSim( + DCMotor.getKrakenX60(1), + 1.0, // gear ratio + 1.0, // carriage mass kg + 0.01, // drum radius m + MIN_HEIGHT, + MAX_HEIGHT, + true, + MIN_HEIGHT, + 0.001, 0.001 // measurementStdDevs as plain double vararg + ); + + voltageOverride = Optional.empty(); + } + + // Convert linear elevator height back to hood angle + @Override + public Rotation2d getAngle() { + double height = sim.getPositionMeters(); + double clampedHeight = Math.max(MIN_HEIGHT, Math.min(MAX_HEIGHT, height)); + double angleRad = Math.asin(clampedHeight / HOOD_ARM_LENGTH_METERS); + return Rotation2d.fromRadians(angleRad); + } + + @Override + public boolean isUnderTrench() { + return false; // No drivetrain in sim context; always false + } + + @Override + public boolean isStalling() { + return false; + } + + @Override + public void zeroHoodEncoderAtLowerHardstop() { + // No-op in sim + } + + @Override + public void seedHood() { + // No-op in sim — ElevatorSim starts at MIN_HEIGHT + } + + private void setVoltageOverride(Optional volts) { + this.voltageOverride = volts; + } + + @Override + public SysIdRoutine getHoodSysIdRoutine() { + return SysId.getRoutine( + 0.45, + 2, + "Hood", + voltage -> setVoltageOverride(Optional.of(voltage)), + () -> sim.getPositionMeters(), + () -> sim.getVelocityMetersPerSecond(), + () -> voltageOverride.orElse(0.0), + getInstance() + ); + } + + @Override + public void periodic() { + super.periodic(); + + // Drive sim toward target angle via a simple proportional voltage + double targetHeight = HOOD_ARM_LENGTH_METERS * + Math.sin(getTargetAngle().getRadians()); + double error = targetHeight - sim.getPositionMeters(); + double voltage = Math.max(-12.0, Math.min(12.0, error * 50.0)); + + if (EnabledSubsystems.HOOD.get()) { + sim.setInputVoltage(voltageOverride.orElse(voltage)); + } else { + sim.setInputVoltage(0.0); + } + + sim.update(Settings.DT); + + VisualizerHood.getInstance().update(getAngle(), atTolerance()); + + if (Settings.DEBUG_MODE) { + SmartDashboard.putNumber("Superstructure/Hood/Sim Height (m)", sim.getPositionMeters()); + } + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index ff6be1fc..f6f3ad65 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java @@ -89,7 +89,5 @@ public void periodic() { SmartDashboard.putNumber("Superstructure/Shooter/Current RPM", getRPM()); SmartDashboard.putNumber("Superstructure/Shooter/Target RPM", getTargetRPM()); - - SmartDashboard.putNumber("InterpolationTesting/Shooter Interpolated Target Shoot RPM", InterpolationCalculator.interpolateShotInfo().targetRPM()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java index e3865139..5f3b2f15 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java @@ -15,7 +15,7 @@ import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.superstructure.SOTMCalculator; import com.stuypulse.robot.util.superstructure.TurretAngleCalculator; -import com.stuypulse.robot.util.superstructure.TurretVisualizer; +import com.stuypulse.robot.util.superstructure.VisualizerTurret; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -75,7 +75,7 @@ public Rotation2d driverInputToAngle() { return Rotation2d.fromDegrees(driverInput.x * 180); } - public boolean atTargetAngle() { + public boolean atTolerance() { double error = getAngle().minus(getTargetAngle()).getRotations(); return Math.abs(error) < Settings.Superstructure.Turret.TOLERANCE.getRotations(); } @@ -113,15 +113,16 @@ public TurretState getState() { public void periodic() { SmartDashboard.putString("Superstructure/Turret/State", state.name()); SmartDashboard.putString("States/Turret", state.name()); + SmartDashboard.putNumber("Superstructure/Turret/Target Angle", getTargetAngle().getDegrees()); - SmartDashboard.putBoolean("Superstructure/Turret/At Target Angle?", atTargetAngle()); + SmartDashboard.putNumber("Superstructure/Turret/Current Angle", getAngle().getDegrees()); if (Settings.DEBUG_MODE) { if (EnabledSubsystems.TURRET.get()) { - TurretVisualizer.getInstance().updateTurretAngle(getAngle().plus((Robot.isBlue() ? Rotation2d.kZero : Rotation2d.k180deg)), atTargetAngle()); + VisualizerTurret.getInstance().updateTurretAngle(getAngle().plus((Robot.isBlue() ? Rotation2d.kZero : Rotation2d.k180deg)), atTolerance()); } else { - TurretVisualizer.getInstance().updateTurretAngle(new Rotation2d(), false); + VisualizerTurret.getInstance().updateTurretAngle(new Rotation2d(), false); } } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index 4b736146..3c50246e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -128,7 +128,7 @@ public Rotation2d getAngle() { } @Override - public boolean atTargetAngle() { + public boolean atTolerance() { double error = getAngle().minus(getTargetAngle()).getRotations() + 0.5; return Math.abs(error) < Settings.Superstructure.Turret.TOLERANCE.getRotations(); } diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java index e4f0381a..b22866ed 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java @@ -1,9 +1,6 @@ package com.stuypulse.robot.util.superstructure; -import java.util.function.Supplier; - import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Superstructure.AngleInterpolation; import com.stuypulse.robot.constants.Settings.Superstructure.FerryRPMInterpolation; import com.stuypulse.robot.constants.Settings.Superstructure.RPMInterpolation; @@ -71,9 +68,9 @@ public static InterpolatedShotInfo interpolateShotInfo(Pose2d turretPose, Pose2d double flightTime = distanceTOFInterpolator.get(distanceMeters); - SmartDashboard.putNumber("Superstructure/Interpolated Target Angle", targetAngle.getDegrees()); - SmartDashboard.putNumber("Superstructure/Interpolated RPM", targetRPM); - SmartDashboard.putNumber("Superstructure/Interpolated TOF", flightTime); + SmartDashboard.putNumber("InterpolationTesting/Interpolated Target Angle", targetAngle.getDegrees()); + SmartDashboard.putNumber("InterpolationTesting/Interpolated RPM", targetRPM); + SmartDashboard.putNumber("InterpolationTesting/Interpolated TOF", flightTime); return new InterpolatedShotInfo( targetAngle, diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/VisualizerHood.java b/src/main/java/com/stuypulse/robot/util/superstructure/VisualizerHood.java new file mode 100644 index 00000000..bf785425 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/superstructure/VisualizerHood.java @@ -0,0 +1,73 @@ +/************************ 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.superstructure; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +public class VisualizerHood { + + private static final VisualizerHood instance; + + static { + instance = new VisualizerHood(); + } + + public static VisualizerHood getInstance() { + return instance; + } + + private final Mechanism2d canvas; + private final double width, height; + + private final MechanismRoot2d hoodPivot; + + private final MechanismLigament2d hoodArm; + + private final MechanismLigament2d hoodTip; + + private VisualizerHood() { + width = Units.inchesToMeters(30); + height = Units.inchesToMeters(30); + + canvas = new Mechanism2d(width, height); + + hoodPivot = canvas.getRoot("Hood Pivot", width / 2, height * 0.2); + + hoodArm = new MechanismLigament2d( + "Hood Arm", + Units.inchesToMeters(12), + 0.0, + 6, + new Color8Bit(Color.kOrange) + ); + + hoodTip = new MechanismLigament2d( + "Hood Tip", + Units.inchesToMeters(3), + -90.0, + 4, + new Color8Bit(Color.kOrangeRed) + ); + + hoodPivot.append(hoodArm); + hoodArm.append(hoodTip); + } + + public void update(Rotation2d hoodAngle, boolean atTarget) { + hoodArm.setAngle(hoodAngle); + hoodArm.setColor(atTarget ? new Color8Bit(Color.kGreen) : new Color8Bit(Color.kRed)); + hoodTip.setColor(atTarget ? new Color8Bit(Color.kGreen) : new Color8Bit(Color.kRed)); + + SmartDashboard.putData("Visualizers/Hood", canvas); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/TurretVisualizer.java b/src/main/java/com/stuypulse/robot/util/superstructure/VisualizerTurret.java similarity index 89% rename from src/main/java/com/stuypulse/robot/util/superstructure/TurretVisualizer.java rename to src/main/java/com/stuypulse/robot/util/superstructure/VisualizerTurret.java index b9a80170..6ebde597 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/TurretVisualizer.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/VisualizerTurret.java @@ -15,14 +15,14 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; -public class TurretVisualizer { - public static TurretVisualizer instance; +public class VisualizerTurret { + public static VisualizerTurret instance; static { - instance = new TurretVisualizer(); + instance = new VisualizerTurret(); } - public static TurretVisualizer getInstance() { + public static VisualizerTurret getInstance() { return instance; } @@ -32,7 +32,7 @@ public static TurretVisualizer getInstance() { private final MechanismRoot2d turretPivot; private final MechanismLigament2d turret; - private TurretVisualizer() { + private VisualizerTurret() { width = Units.inchesToMeters(20); height = Units.inchesToMeters(20); From f60173ca027f5bbad39cf154b4e40547ed6196d1 Mon Sep 17 00:00:00 2001 From: Material-Energy Date: Thu, 5 Mar 2026 18:25:23 -0500 Subject: [PATCH 079/150] FEAT: Added a default command for the superstructure Co-authored-by: Lucas O Co-authored-by: Brian Zheng --- .../com/stuypulse/robot/RobotContainer.java | 42 ++++++++-------- .../SuperstructureDefaultCommand.java | 47 ++++++++++++++++++ .../com/stuypulse/robot/constants/Field.java | 8 ++- .../superstructure/Superstructure.java | 5 -- .../subsystems/superstructure/hood/Hood.java | 8 +-- .../superstructure/hood/HoodImpl.java | 17 ------- .../superstructure/hood/HoodSim.java | 5 -- .../swerve/CommandSwerveDrivetrain.java | 49 +++++++++++++++++++ 8 files changed, 129 insertions(+), 52 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureDefaultCommand.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 44347880..e69d0970 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -29,6 +29,7 @@ import com.stuypulse.robot.commands.intake.ZeroPivotStowed; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureDefaultCommand; import com.stuypulse.robot.commands.superstructure.SuperstructureFerry; import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureKB; @@ -127,6 +128,7 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); + superstructure.setDefaultCommand(new SuperstructureDefaultCommand()); // climberHopper.setDefaultCommand(new ClimberHopperDefaultCommand()); // turret.setDefaultCommand(new TurretDefaultCommand()); } @@ -138,7 +140,7 @@ private void configureDefaultCommands() { private void configureButtonBindings() { // Scoring Routine driver.getTopButton() - .whileTrue(new SuperstructureShoot().onlyIf(() -> !superstructure.isHoodUnderTrench()) + .whileTrue(new SuperstructureShoot().onlyIf(() -> !swerve.isUnderTrench()) .alongWith(new SwerveDriveAlignTurretToHub()) // .alongWith(new TurretShoot()) .andThen(new WaitUntilCommand(superstructure::atTolerance)) @@ -157,27 +159,27 @@ private void configureButtonBindings() { driver.getLeftTriggerButton() .onTrue(new IntakeStow()); - driver.getRightButton() - .whileTrue(new SuperstructureFerry().onlyIf(() -> !superstructure.isHoodUnderTrench()) - // .alongWith(new TurretShoot()) - .andThen(new WaitUntilCommand(superstructure::atTolerance)) - .andThen(new HandoffRun().onlyIf(superstructure::atTolerance) - .alongWith(new WaitUntilCommand(handoff::atTolerance)) - .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) - .onFalse(new SpindexerStop() - .alongWith(new SuperstructureStow()) - .alongWith(new HandoffStop())); + // driver.getRightButton() + // .whileTrue(new SuperstructureFerry().onlyIf(() -> !swerve.isUnderTrench()) + // // .alongWith(new TurretShoot()) + // .andThen(new WaitUntilCommand(superstructure::atTolerance)) + // .andThen(new HandoffRun().onlyIf(superstructure::atTolerance) + // .alongWith(new WaitUntilCommand(handoff::atTolerance)) + // .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) + // .onFalse(new SpindexerStop() + // .alongWith(new SuperstructureStow()) + // .alongWith(new HandoffStop())); // SOTM - // driver.getRightButton() - // .whileTrue(new SuperstructureSOTM().onlyIf(() -> !superstructure.isHoodUnderTrench()) - // .andThen(new WaitUntilCommand(superstructure::atTolerance)) - // .andThen(new HandoffRun().onlyIf(superstructure::atTolerance) - // .alongWith(new WaitUntilCommand(handoff::atTolerance)) - // .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) - // .onFalse(new SpindexerStop() - // .alongWith(new SuperstructureStow()) - // .alongWith(new HandoffStop())); + driver.getRightButton() + .whileTrue(new SuperstructureSOTM().onlyIf(() -> !swerve.isUnderTrench()) + .andThen(new WaitUntilCommand(superstructure::atTolerance)) + .andThen(new HandoffRun().onlyIf(superstructure::atTolerance) + .alongWith(new WaitUntilCommand(handoff::atTolerance)) + .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) + .onFalse(new SpindexerStop() + .alongWith(new SuperstructureStow()) + .alongWith(new HandoffStop())); // Reset Heading driver.getDPadUp() diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureDefaultCommand.java new file mode 100644 index 00000000..754f78d2 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureDefaultCommand.java @@ -0,0 +1,47 @@ +package com.stuypulse.robot.commands.superstructure; + +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class SuperstructureDefaultCommand extends Command { + private final CommandSwerveDrivetrain swerve; + private final Superstructure superstructure; + + public SuperstructureDefaultCommand() { + swerve = CommandSwerveDrivetrain.getInstance(); + superstructure = Superstructure.getInstance(); + + addRequirements(superstructure); + } + + @Override + public void execute() { + // TODO: check if stop Handoff only is possible or both Handoff & Spindexer is needed + + // + if (swerve.isUnderTrench()) { + new SuperstructureStow(); + new SpindexerStop().alongWith(new HandoffStop()); + } + if (swerve.isInOpponentZone()){ + new SpindexerStop().alongWith(new HandoffStop()) + .andThen(new WaitUntilCommand(superstructure::atTolerance)) + .andThen(new HandoffRun()).alongWith(new SpindexerRun()); + } + + // Prevent shooting from hard to aim places + if (swerve.isBehindTower() || swerve.isBehindHub()) { + new SpindexerStop().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 fcfe352e..4d7e113a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -35,15 +35,21 @@ public interface Field { // Alliance relative hub center coordinates public static final Pose2d hubCenter = new Pose2d(Units.inchesToMeters(182.11), WIDTH / 2.0, new Rotation2d()); - public static final Pose3d hubCenter3d = new Pose3d(hubCenter.getX(), hubCenter.getY(), Units.inchesToMeters(72), Rotation3d.kZero); + public static final Pose2d hubFarRightCorner = new Pose2d(Units.inchesToMeters(205.6), WIDTH / 2.0 - Units.inchesToMeters(47 / 2.0), Rotation2d.kZero); + public static final Pose2d hubFarLeftCorner = new Pose2d(Units.inchesToMeters(205.6), WIDTH / 2.0 + Units.inchesToMeters(47 / 2.0), Rotation2d.kZero); + public static final double HUB_RADIUS = Units.inchesToMeters(41.7 / 2); + public static final double OPPONENT_ZONE_X = LENGTH - Units.inchesToMeters(158.6); + public static Pose2d getHubPose() { return hubCenter; } // Alliance relative tower center coordinates public final Pose2d towerCenter = new Pose2d(Units.inchesToMeters(42.0), Units.inchesToMeters(147.47), new Rotation2d()); + public final Pose2d towerFarRight = new Pose2d(Units.inchesToMeters(42.0), Units.inchesToMeters(147.47-23.5), new Rotation2d()); + public final Pose2d towerFarLeft = new Pose2d(Units.inchesToMeters(42.0), Units.inchesToMeters(147.47+23.5), new Rotation2d()); public final double barDisplacement = Units.inchesToMeters(11.38); public final double DISTANCE_TO_RUNGS = Units.inchesToMeters(20); // placeholder value, how far away in terms of y-cord from the rung diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index e56f17ca..4c940663 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -94,10 +94,6 @@ public boolean atTolerance() { return isShooterAtTolerance() && isHoodAtTolerance() && isTurretAtTolerance(); } - public boolean isHoodUnderTrench() { - return hood.isUnderTrench(); - } - public boolean isShooterAtTolerance() { return shooter.atTolerance(); } @@ -139,7 +135,6 @@ public void periodic() { SmartDashboard.putBoolean("Superstructure/Hood At Tolerance?", isHoodAtTolerance()); SmartDashboard.putBoolean("Superstructure/Turret At Tolerance?", isTurretAtTolerance()); - SmartDashboard.putBoolean("Superstructure/Hood/Under Trench", isHoodUnderTrench()); SmartDashboard.putNumber("InterpolationTesting/Current Hood Angle", getHoodAngle().getDegrees()); SmartDashboard.putNumber("InterpolationTesting/Current Shooter RPM", getShooterRPM()); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java index e60e188c..8fbede33 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java @@ -63,9 +63,10 @@ public void setState(HoodState state){ } public Rotation2d getTargetAngle() { - if (isUnderTrench()) { - return Settings.Superstructure.Hood.Angles.STOW; - } + // if (isUnderTrench()) { + // return Settings.Superstructure.Hood.Angles.STOW; + // } + // don't think we need this when we have superstructure default command. return switch(state) { case STOW -> Settings.Superstructure.Hood.Angles.STOW; @@ -103,7 +104,6 @@ public Rotation2d hoodAnalogToOutput() { return this.driverInput; } - public abstract boolean isUnderTrench(); public abstract boolean isStalling(); public abstract SysIdRoutine getHoodSysIdRoutine(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index 242d81c5..276db80d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -110,23 +110,6 @@ public void zeroHoodEncoderAtLowerHardstop() { public boolean isStalling() { return isStalling.getAsBoolean(); } - - @Override - public boolean isUnderTrench() { - Pose2d pose = CommandSwerveDrivetrain.getInstance().getTurretPose(); - - 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 isCloseToAllianceSideTrenchX = Math.abs(pose.getX() - Field.NearRightTrench.rightEdge.getX()) < Field.trenchHoodTolerance; - - boolean isCloseToNeutralSideTrenchX = Math.abs(pose.getX() - Field.FarRightTrench.rightEdge.getX()) < Field.trenchHoodTolerance; - - boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) && (isCloseToAllianceSideTrenchX || isCloseToNeutralSideTrenchX); - - return isUnderTrench; - } /* * Example: diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java index 91f4a9d9..562a059a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java @@ -60,11 +60,6 @@ public Rotation2d getAngle() { return Rotation2d.fromRadians(angleRad); } - @Override - public boolean isUnderTrench() { - return false; // No drivetrain in sim context; always false - } - @Override public boolean isStalling() { return false; 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 8812b506..16224996 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -29,11 +29,14 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; + import static edu.wpi.first.units.Units.Second; import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.wpilibj.Notifier; @@ -53,6 +56,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private FieldObject2d turret2d = Field.FIELD2D.getObject("Turret 2D"); private Pose2d turretPose = new Pose2d(); + + + static { instance = TunerConstants.createDrivetrain(); } @@ -445,6 +451,43 @@ public double[] getWheelDrivePositionsRadians(){ return positions; } + public boolean isUnderTrench() { + Pose2d pose = getTurretPose(); + + 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 isCloseToAllianceSideTrenchX = Math.abs(pose.getX() - Field.NearRightTrench.rightEdge.getX()) < Field.trenchHoodTolerance; + + boolean isCloseToNeutralSideTrenchX = Math.abs(pose.getX() - Field.FarRightTrench.rightEdge.getX()) < Field.trenchHoodTolerance; + + boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) && (isCloseToAllianceSideTrenchX || isCloseToNeutralSideTrenchX); + + return isUnderTrench; + } + + + public boolean isInOpponentZone(){ + Translation2d turretPose = getTurretPose().getTranslation(); + return turretPose.getX() > Field.OPPONENT_ZONE_X; + } + + public boolean isBehindTower() { + boolean withinTowerX = Field.towerFarRight.getX() < getTurretPose().getTranslation().getX() && getTurretPose().getTranslation().getX() < Field.towerFarLeft.getX(); + boolean withinTowerY = getPose().getTranslation().getY() < Field.towerCenter.getY(); + return withinTowerX && withinTowerY; + } + + // Stop ferrying when in rectangle behind hub (on neutral zone + public boolean isBehindHub() { + Translation2d turretPose = getTurretPose().getTranslation(); + boolean behindHubX = Field.hubFarLeftCorner.getX() < turretPose.getX() && turretPose.getX() < Field.hubFarLeftCorner.getX() + Units.inchesToMeters(283); + boolean withinHubY = Field.hubFarRightCorner.getY() < getTurretPose().getY() && getTurretPose().getY() < Field.hubFarLeftCorner.getY(); + + return behindHubX && withinHubY; + } + @Override public void periodic() { Pose2d pose = getPose(); @@ -455,6 +498,12 @@ public void periodic() { turret2d.setPose(Robot.isBlue() ? turretPose : Field.transformToOppositeAlliance(turretPose)); + SmartDashboard.putBoolean("FieldPositions/isBehindTower", isBehindTower()); + SmartDashboard.putBoolean("FieldPositions/isUnderTrench", isUnderTrench()); + SmartDashboard.putBoolean("FieldPositions/isBehindHub", isBehindHub()); + SmartDashboard.putBoolean("FieldPositions/isInOpponentZone", isInOpponentZone()); + + SmartDashboard.putNumber("Superstructure/Turret/Dist From Hub", turretPose.getTranslation().getDistance(Field.hubCenter.getTranslation())); SmartDashboard.putNumber("InterpolationTesting/Turret Dist From Hub", turretPose.getTranslation().getDistance(Field.hubCenter.getTranslation())); From 16313b18d987cbb4efb7ef4bfd1648d9be262f30 Mon Sep 17 00:00:00 2001 From: Lucas Date: Thu, 5 Mar 2026 20:39:53 -0500 Subject: [PATCH 080/150] FEAT: Stop Handoff and Spindexer when behind tower or hub with ConditionalCommands Co-authored-by: simon --- simgui.json | 5 ++- .../com/stuypulse/robot/RobotContainer.java | 10 +++--- .../handoff/HandoffConditionalCommand.java | 11 +++++++ .../SpindexerConditionalCommand.java | 15 +++++++++ .../SuperstructureDefaultCommand.java | 32 +++++++++++-------- .../swerve/CommandSwerveDrivetrain.java | 5 ++- 6 files changed, 57 insertions(+), 21 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/handoff/HandoffConditionalCommand.java create mode 100644 src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java diff --git a/simgui.json b/simgui.json index 1acb9155..84fb2ea6 100644 --- a/simgui.json +++ b/simgui.json @@ -63,13 +63,16 @@ }, "transitory": { "SmartDashboard": { + "FieldPositions": { + "open": true + }, "HoodedShooter": { "Hood": { "open": true }, "open": true }, - "Superstructure": { + "States": { "open": true }, "Turret": { diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index e69d0970..fea8fd9f 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -18,6 +18,7 @@ import com.stuypulse.robot.commands.climberhopper.ClimberOverrideDown; import com.stuypulse.robot.commands.climberhopper.ClimberOverrideStop; import com.stuypulse.robot.commands.climberhopper.ClimberOverrideUp; +import com.stuypulse.robot.commands.handoff.HandoffConditionalCommand; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.hood.ZeroHoodAtLowerHardstop; @@ -27,6 +28,7 @@ import com.stuypulse.robot.commands.intake.IntakeStow; import com.stuypulse.robot.commands.intake.ZeroPivotDeployed; import com.stuypulse.robot.commands.intake.ZeroPivotStowed; +import com.stuypulse.robot.commands.spindexer.SpindexerConditionalCommand; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureDefaultCommand; @@ -144,9 +146,9 @@ private void configureButtonBindings() { .alongWith(new SwerveDriveAlignTurretToHub()) // .alongWith(new TurretShoot()) .andThen(new WaitUntilCommand(superstructure::atTolerance)) - .andThen(new HandoffRun().onlyIf(superstructure::atTolerance) + .andThen(new HandoffConditionalCommand().onlyIf(superstructure::atTolerance) .alongWith(new WaitUntilCommand(handoff::atTolerance)) - .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) + .andThen(new SpindexerConditionalCommand().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) .onFalse(new SpindexerStop() .alongWith(new SuperstructureStow()) .alongWith(new HandoffStop())); @@ -174,9 +176,9 @@ private void configureButtonBindings() { driver.getRightButton() .whileTrue(new SuperstructureSOTM().onlyIf(() -> !swerve.isUnderTrench()) .andThen(new WaitUntilCommand(superstructure::atTolerance)) - .andThen(new HandoffRun().onlyIf(superstructure::atTolerance) + .andThen(new HandoffConditionalCommand().onlyIf(superstructure::atTolerance) .alongWith(new WaitUntilCommand(handoff::atTolerance)) - .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) + .andThen(new SpindexerConditionalCommand().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) .onFalse(new SpindexerStop() .alongWith(new SuperstructureStow()) .alongWith(new HandoffStop())); diff --git a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffConditionalCommand.java b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffConditionalCommand.java new file mode 100644 index 00000000..efc95db8 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffConditionalCommand.java @@ -0,0 +1,11 @@ +package com.stuypulse.robot.commands.handoff; + +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.ConditionalCommand; + +public class HandoffConditionalCommand extends ConditionalCommand{ + public HandoffConditionalCommand() { + super(new HandoffStop(), new HandoffRun(), () -> (CommandSwerveDrivetrain.getInstance().isBehindTower() || CommandSwerveDrivetrain.getInstance().isBehindHub())); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java new file mode 100644 index 00000000..b52cd230 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java @@ -0,0 +1,15 @@ +/************************ 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.commands.spindexer; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.ConditionalCommand; + +public class SpindexerConditionalCommand extends ConditionalCommand { + public SpindexerConditionalCommand() { + super(new SpindexerStop(), new SpindexerRun(), () -> (CommandSwerveDrivetrain.getInstance().isBehindTower() || CommandSwerveDrivetrain.getInstance().isBehindHub())); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureDefaultCommand.java index 754f78d2..2716ec4c 100644 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureDefaultCommand.java @@ -11,11 +11,13 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class SuperstructureDefaultCommand extends Command { private final CommandSwerveDrivetrain swerve; private final Superstructure superstructure; + // private boolean trench = false; public SuperstructureDefaultCommand() { swerve = CommandSwerveDrivetrain.getInstance(); @@ -26,22 +28,26 @@ public SuperstructureDefaultCommand() { @Override public void execute() { - // TODO: check if stop Handoff only is possible or both Handoff & Spindexer is needed - - // + if (swerve.isUnderTrench()) { new SuperstructureStow(); new SpindexerStop().alongWith(new HandoffStop()); } - if (swerve.isInOpponentZone()){ - new SpindexerStop().alongWith(new HandoffStop()) - .andThen(new WaitUntilCommand(superstructure::atTolerance)) - .andThen(new HandoffRun()).alongWith(new SpindexerRun()); - } - - // Prevent shooting from hard to aim places - if (swerve.isBehindTower() || swerve.isBehindHub()) { - new SpindexerStop().alongWith(new HandoffStop()); - } + + // SIMON's WORK + // if (swerve.isUnderTrench() && trench == false) { + // CommandScheduler.getInstance().schedule(new SuperstructureStow()); + // CommandScheduler.getInstance().schedule(new SpindexerStop().alongWith(new HandoffStop())); + // trench = true; + // } else { + // trench = false; + // } + + // TODO: WHAT REASON FOR THIS? + // if (swerve.isInOpponentZone()) { + // new SpindexerStop().alongWith(new HandoffStop()) + // .andThen(new WaitUntilCommand(superstructure::atTolerance)) + // .andThen(new HandoffRun()).alongWith(new SpindexerRun()); + // } } } 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 16224996..b99857b0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -531,8 +531,7 @@ public void periodic() { Field.FIELD2D.getRobotObject().setPose(Robot.isBlue() ? pose : Field.transformToOppositeAlliance(pose)); - if (Settings.DEBUG_MODE) { - } + if (Settings.DEBUG_MODE) {} SmartDashboard.putNumber("Swerve/Velocity Robot Relative X (m per s)", getChassisSpeeds().vxMetersPerSecond); SmartDashboard.putNumber("Swerve/Velocity Robot Relative Y (m per s)", getChassisSpeeds().vyMetersPerSecond); @@ -544,5 +543,5 @@ public void periodic() { SmartDashboard.putNumber("Swerve/Angular Velocity (rad per s)", getChassisSpeeds().omegaRadiansPerSecond); SmartDashboard.putNumber("Swerve/Distance From Hub (meters)", Field.hubCenter.getTranslation().getDistance(getPose().getTranslation())); - } + } } \ No newline at end of file From 66bc6241aa6aece2788b88bad231777657f3b11b Mon Sep 17 00:00:00 2001 From: Lucas Date: Thu, 5 Mar 2026 21:05:47 -0500 Subject: [PATCH 081/150] REFACTOR: clarify names and move behind hub x tolerance to Field constants --- .../climbAlign/SwerveClimbAlignBot.java | 2 +- .../climbAlign/SwerveClimbAlignTop.java | 2 +- .../com/stuypulse/robot/constants/Field.java | 8 ++++--- .../swerve/CommandSwerveDrivetrain.java | 24 +++++++++---------- 4 files changed, 19 insertions(+), 17 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlignBot.java b/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlignBot.java index f07cde7f..e7c23408 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlignBot.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlignBot.java @@ -13,7 +13,7 @@ public class SwerveClimbAlignBot extends SwerveDrivePIDToPose{ public SwerveClimbAlignBot(){ - super(new Pose2d(Field.towerCenter.getX(), Field.towerCenter.getY() - Field.barDisplacement - Field.DISTANCE_TO_RUNGS, new Rotation2d(0))); + super(new Pose2d(Field.towerFarCenter.getX(), Field.towerFarCenter.getY() - Field.barDisplacement - Field.DISTANCE_TO_RUNGS, new Rotation2d(0))); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlignTop.java b/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlignTop.java index 7a5e38e0..afb9c3db 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlignTop.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlignTop.java @@ -15,7 +15,7 @@ public class SwerveClimbAlignTop extends SwerveDrivePIDToPose{ public SwerveClimbAlignTop(){ - super(new Pose2d(Field.towerCenter.getX(), Field.towerCenter.getY() + Field.barDisplacement + Field.DISTANCE_TO_RUNGS, new Rotation2d(Units.degreesToRadians(180)))); + super(new Pose2d(Field.towerFarCenter.getX(), Field.towerFarCenter.getY() + Field.barDisplacement + Field.DISTANCE_TO_RUNGS, new Rotation2d(Units.degreesToRadians(180)))); } @Override diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 4d7e113a..acf6dd95 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -30,7 +30,7 @@ public interface Field { double WIDTH = Units.inchesToMeters(317.000); double LENGTH = Units.inchesToMeters(651.200); - public static final double trenchXTolerance = Units.inchesToMeters(50); + public static final double trenchHopperTolerance = Units.inchesToMeters(50); public static final double trenchHoodTolerance = Units.inchesToMeters(15); // Alliance relative hub center coordinates @@ -42,12 +42,14 @@ public interface Field { public static final double OPPONENT_ZONE_X = LENGTH - Units.inchesToMeters(158.6); + public static final double hubTolerance = Units.inchesToMeters(283); + public static Pose2d getHubPose() { return hubCenter; } // Alliance relative tower center coordinates - public final Pose2d towerCenter = new Pose2d(Units.inchesToMeters(42.0), Units.inchesToMeters(147.47), new Rotation2d()); + public final Pose2d towerFarCenter = new Pose2d(Units.inchesToMeters(42.0), Units.inchesToMeters(147.47), new Rotation2d()); public final Pose2d towerFarRight = new Pose2d(Units.inchesToMeters(42.0), Units.inchesToMeters(147.47-23.5), new Rotation2d()); public final Pose2d towerFarLeft = new Pose2d(Units.inchesToMeters(42.0), Units.inchesToMeters(147.47+23.5), new Rotation2d()); public final double barDisplacement = Units.inchesToMeters(11.38); @@ -55,7 +57,7 @@ public static Pose2d getHubPose() { public final double DISTANCE_TO_RUNGS = Units.inchesToMeters(20); // placeholder value, how far away in terms of y-cord from the rung public static boolean closerToTop(){ - return (CommandSwerveDrivetrain.getInstance().getPose().getY() >= Field.towerCenter.getY()); + return (CommandSwerveDrivetrain.getInstance().getPose().getY() >= Field.towerFarCenter.getY()); } // 1.0 meters from driverstation wall and field wall 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 b99857b0..11d7161a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -452,15 +452,15 @@ public double[] getWheelDrivePositionsRadians(){ } public boolean isUnderTrench() { - Pose2d pose = getTurretPose(); + Translation2d turretTranslation = getTurretPose().getTranslation(); - boolean isBetweenRightTrenchesY = Field.NearRightTrench.rightEdge.getY() < pose.getY() && Field.NearRightTrench.leftEdge.getY() > pose.getY(); + boolean isBetweenRightTrenchesY = Field.NearRightTrench.rightEdge.getY() < turretTranslation.getY() && Field.NearRightTrench.leftEdge.getY() > turretTranslation.getY(); - boolean isBetweenLeftTrenchesY = Field.NearLeftTrench.rightEdge.getY() < pose.getY() && Field.NearLeftTrench.leftEdge.getY() > pose.getY(); + boolean isBetweenLeftTrenchesY = Field.NearLeftTrench.rightEdge.getY() < turretTranslation.getY() && Field.NearLeftTrench.leftEdge.getY() > turretTranslation.getY(); - boolean isCloseToAllianceSideTrenchX = Math.abs(pose.getX() - Field.NearRightTrench.rightEdge.getX()) < Field.trenchHoodTolerance; + boolean isCloseToAllianceSideTrenchX = Math.abs(turretTranslation.getX() - Field.NearRightTrench.rightEdge.getX()) < Field.trenchHoodTolerance; - boolean isCloseToNeutralSideTrenchX = Math.abs(pose.getX() - Field.FarRightTrench.rightEdge.getX()) < Field.trenchHoodTolerance; + boolean isCloseToNeutralSideTrenchX = Math.abs(turretTranslation.getX() - Field.FarRightTrench.rightEdge.getX()) < Field.trenchHoodTolerance; boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) && (isCloseToAllianceSideTrenchX || isCloseToNeutralSideTrenchX); @@ -469,20 +469,20 @@ public boolean isUnderTrench() { public boolean isInOpponentZone(){ - Translation2d turretPose = getTurretPose().getTranslation(); - return turretPose.getX() > Field.OPPONENT_ZONE_X; + Translation2d turretTranslation = getTurretPose().getTranslation(); + return turretTranslation.getX() > Field.OPPONENT_ZONE_X; } public boolean isBehindTower() { - boolean withinTowerX = Field.towerFarRight.getX() < getTurretPose().getTranslation().getX() && getTurretPose().getTranslation().getX() < Field.towerFarLeft.getX(); - boolean withinTowerY = getPose().getTranslation().getY() < Field.towerCenter.getY(); + boolean withinTowerX = getPose().getTranslation().getX() < Field.towerFarCenter.getX(); + boolean withinTowerY = Field.towerFarRight.getY() < getTurretPose().getTranslation().getY() && getTurretPose().getTranslation().getY() < Field.towerFarLeft.getY(); return withinTowerX && withinTowerY; } - // Stop ferrying when in rectangle behind hub (on neutral zone + // Stop ferrying when in rectangle behind hub (in neutral zone) public boolean isBehindHub() { - Translation2d turretPose = getTurretPose().getTranslation(); - boolean behindHubX = Field.hubFarLeftCorner.getX() < turretPose.getX() && turretPose.getX() < Field.hubFarLeftCorner.getX() + Units.inchesToMeters(283); + Translation2d turretTranslation = getTurretPose().getTranslation(); + boolean behindHubX = Field.hubFarLeftCorner.getX() < turretTranslation.getX() && turretTranslation.getX() < Field.hubFarLeftCorner.getX() + Field.hubTolerance; boolean withinHubY = Field.hubFarRightCorner.getY() < getTurretPose().getY() && getTurretPose().getY() < Field.hubFarLeftCorner.getY(); return behindHubX && withinHubY; From 5e321525f0b59f498b2339bf8dcb4b92fd591c50 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 5 Mar 2026 21:06:14 -0500 Subject: [PATCH 082/150] feat: wheel radius characterization fixes + working interpolation + more offsets for the limelight though not confirmed --- src/main/java/com/stuypulse/robot/Robot.java | 3 + .../com/stuypulse/robot/RobotContainer.java | 143 +++++++++--------- ...va => ZeroHoodEncoderAtUpperHardstop.java} | 6 +- .../SwerveWheelRadiusCharacterization.java | 4 +- .../stuypulse/robot/constants/Cameras.java | 8 +- .../stuypulse/robot/constants/Settings.java | 114 ++++++++++---- .../robot/subsystems/intake/IntakeImpl.java | 2 - .../subsystems/spindexer/SpindexerImpl.java | 5 +- .../superstructure/Superstructure.java | 4 +- .../subsystems/superstructure/hood/Hood.java | 2 +- .../superstructure/hood/HoodImpl.java | 6 +- .../superstructure/hood/HoodSim.java | 2 +- .../superstructure/shooter/ShooterImpl.java | 2 - .../superstructure/turret/Turret.java | 2 + .../superstructure/turret/TurretImpl.java | 2 +- 15 files changed, 177 insertions(+), 128 deletions(-) rename src/main/java/com/stuypulse/robot/commands/hood/{ZeroHoodAtLowerHardstop.java => ZeroHoodEncoderAtUpperHardstop.java} (68%) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index dc9f28aa..c31e43c7 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -52,6 +52,9 @@ public void robotInit() { public void robotPeriodic() { CommandScheduler.getInstance().run(); // SmartDashboard.putNumber("Robot/Voltage of Robot", powerDistribution.getVoltage()); + if (!Robot.isReal()) { + SmartDashboard.putData(CommandScheduler.getInstance()); + } SmartDashboard.putNumber("Robot/Match Time", DriverStation.getMatchTime()); diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 44347880..11910cbb 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -14,13 +14,15 @@ import com.stuypulse.robot.commands.auton.regular.DepotAuton; import com.stuypulse.robot.commands.auton.regular.EightFuel; import com.stuypulse.robot.commands.auton.regular.TopTwoCycle; +import com.stuypulse.robot.commands.climberhopper.ClimberDown; // import com.stuypulse.robot.commands.auton.test.BoxTest; import com.stuypulse.robot.commands.climberhopper.ClimberOverrideDown; import com.stuypulse.robot.commands.climberhopper.ClimberOverrideStop; import com.stuypulse.robot.commands.climberhopper.ClimberOverrideUp; +import com.stuypulse.robot.commands.climberhopper.ClimberUp; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.hood.ZeroHoodAtLowerHardstop; +import com.stuypulse.robot.commands.hood.ZeroHoodEncoderAtUpperHardstop; import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.intake.IntakeRunRollers; import com.stuypulse.robot.commands.intake.IntakeStopRollers; @@ -32,6 +34,8 @@ import com.stuypulse.robot.commands.superstructure.SuperstructureFerry; import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureKB; +import com.stuypulse.robot.commands.superstructure.SuperstructureLeftCorner; +import com.stuypulse.robot.commands.superstructure.SuperstructureRightCorner; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; import com.stuypulse.robot.commands.superstructure.SuperstructureShoot; import com.stuypulse.robot.commands.superstructure.SuperstructureStow; @@ -39,6 +43,8 @@ import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveWheelRadiusCharacterization; +import com.stuypulse.robot.commands.swerve.SwerveXMode; +import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.commands.turret.TurretDefaultCommand; import com.stuypulse.robot.commands.turret.TurretShoot; import com.stuypulse.robot.commands.turret.ZeroTurret; @@ -106,14 +112,14 @@ public RobotContainer() { swerve.configureAutoBuilder(); configureDefaultCommands(); configureButtonBindings(); - //configureAutons(); + configureAutons(); configureSysids(); SmartDashboard.putData("Field", Field.FIELD2D); SmartDashboard.putData("Robot/Zero Pivot Encoder at Lower Limit (Deployed)", new ZeroPivotDeployed().ignoringDisable(true)); SmartDashboard.putData("Robot/Zero Pivot Encoder at Upper Limit (Stowed)", new ZeroPivotStowed().ignoringDisable(true)); SmartDashboard.putData("Robot/Zero Turret Encoders", new ZeroTurret().ignoringDisable(true)); - SmartDashboard.putData("Robot/Zero Hood Encoder", new ZeroHoodAtLowerHardstop().ignoringDisable(true)); + SmartDashboard.putData("Robot/Zero Hood Encoder", new ZeroHoodEncoderAtUpperHardstop().ignoringDisable(true)); SmartDashboard.putData("Robot/Override Up", new ClimberOverrideUp()); SmartDashboard.putData("Robot/Override Down", new ClimberOverrideDown()); @@ -138,9 +144,22 @@ private void configureDefaultCommands() { private void configureButtonBindings() { // Scoring Routine driver.getTopButton() + .whileTrue(new SuperstructureInterpolation().onlyIf(() -> !superstructure.isHoodUnderTrench()) + // .alongWith(new SwerveDriveAlignTurretToHub()) + // .alongWith(new TurretShoot()) + .andThen(new WaitUntilCommand(superstructure::atTolerance)) + .andThen(new HandoffRun().onlyIf(superstructure::atTolerance) + .alongWith(new WaitUntilCommand(handoff::atTolerance)) + .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) + .onFalse(new SpindexerStop() + .alongWith(new SuperstructureStow()) + .alongWith(new HandoffStop())); + + // Scoring Routine + driver.getBottomButton() .whileTrue(new SuperstructureShoot().onlyIf(() -> !superstructure.isHoodUnderTrench()) - .alongWith(new SwerveDriveAlignTurretToHub()) - // .alongWith(new TurretShoot()) + // .alongWith(new SwerveDriveAlignTurretToHub()) + // .alongWith(new TurretShoot()) .andThen(new WaitUntilCommand(superstructure::atTolerance)) .andThen(new HandoffRun().onlyIf(superstructure::atTolerance) .alongWith(new WaitUntilCommand(handoff::atTolerance)) @@ -153,6 +172,9 @@ private void configureButtonBindings() { driver.getRightTriggerButton() .onTrue(new IntakeDeploy()); + // driver.getRightBumper() + // .whileTrue(new SwerveWheelRadiusCharacterization()); + // Intake Stow driver.getLeftTriggerButton() .onTrue(new IntakeStow()); @@ -183,40 +205,26 @@ private void configureButtonBindings() { driver.getDPadUp() .onTrue(new SwerveResetHeading()) .onTrue(new ResetLimelightIMU()) - .onFalse(new SetIMUMode(0)); - - // // Ferry Routine using Interpolation Settings - // driver.getBottomButton() - // .onTrue(new SuperstructureFerry() - // .alongWith(new TurretFerry()) - // .alongWith(new WaitUntilCommand(() -> superstructure.bothAtTolerance())) - // .andThen(new HandoffRun().onlyIf(() -> superstructure.bothAtTolerance()) - // .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) - // .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && superstructure.bothAtTolerance()))) - // ) - // .onFalse(new SpindexerStop() - // .alongWith(new SuperstructureStow()) - // .alongWith(new HandoffStop())); - -//-------------------------------------------------------------------------------------------------------------------------\\ -//-------------------------------------------------------------------------------------------------------------------------\\ -//-------------------------------------------------------------------------------------------------------------------------\\ -//-------------------------------------------------------------------------------------------------------------------------\\ -//-------------------------------------------------------------------------------------------------------------------------\\ - /* + .onFalse(new SetIMUMode(0)); + // Climb Align driver.getTopButton() - .whileTrue(new SwerveClimbAlign().alongWith(new ClimberUp())); + .whileTrue(new SwerveClimbAlign().alongWith(new ClimberUp()).andThen(new SwerveXMode())); + + // Climber Up + driver.getLeftTriggerButton() + .onTrue(new ClimberUp()); + + // Climber Down + driver.getRightTriggerButton() + .onTrue(new ClimberDown()); // Left Corner Shoot driver.getLeftButton() .whileTrue( new SwerveXMode().alongWith( new SuperstructureLeftCorner().alongWith( - new TurretLeftCorner()).alongWith( - new WaitUntilCommand(() -> superstructure.isHoodAtTolerance())).alongWith( - new WaitUntilCommand(() -> superstructure.isShooterAtTolerance())).alongWith( - new WaitUntilCommand(() -> turret.atTargetAngle())).andThen( + new WaitUntilCommand(() -> superstructure.atTolerance())).andThen( new SpindexerRun().alongWith(new HandoffRun())))) .onFalse( new SuperstructureStow().alongWith( @@ -229,11 +237,8 @@ private void configureButtonBindings() { .whileTrue( new SwerveXMode().alongWith( new SuperstructureRightCorner().alongWith( - new TurretRightCorner()).alongWith( - new WaitUntilCommand(() -> superstructure.isHoodAtTolerance())).alongWith( - new WaitUntilCommand(() -> superstructure.isShooterAtTolerance())).alongWith( - new WaitUntilCommand(() -> turret.atTargetAngle())).andThen( - new SpindexerRun().alongWith(new HandoffRun())))) + new WaitUntilCommand(() -> superstructure.atTolerance()).andThen( + new SpindexerRun().alongWith(new HandoffRun()))))) .onFalse( new SuperstructureStow().alongWith( new SpindexerRun().alongWith( @@ -241,44 +246,39 @@ private void configureButtonBindings() { ); // Hub Shoot - driver.getBottomButton() - .whileTrue( - new SwerveXMode().alongWith( - new SuperstructureShoot().alongWith( - new TurretShoot()).alongWith( - new WaitUntilCommand(() -> Superstructure.isHoodAtTolerance())).alongWith( - new WaitUntilCommand(() -> Superstructure.isShooterAtTolerance())).alongWith( - new WaitUntilCommand(() -> turret.atTargetAngle())).andThen( - new SpindexerRun().alongWith(new HandoffRun())))) - .onFalse( - new SuperstructureStow().alongWith( - new SpindexerRun().alongWith( - new HandoffStop())) - ); - - // Intake Up and Off - driver.getLeftTriggerButton() - .onTrue(new IntakeStow()); - - // Intake Down and On - driver.getRightTriggerButton() - .onTrue(new IntakeDeploy()); - - // Climb Down Placeholder - driver.getLeftBumper() - .onTrue(new BuzzController(driver).alongWith(new ClimberDown())) - .onFalse(new HopperDown()); + // driver.getBottomButton() + // .whileTrue( + // new SwerveXMode().alongWith( + // new SuperstructureKB().alongWith( + // new WaitUntilCommand(() -> superstructure.atTolerance())).andThen( + // new SpindexerRun().alongWith(new HandoffRun())))) + // .onFalse( + // new SuperstructureStow().alongWith( + // new SpindexerRun().alongWith( + // new HandoffStop())) + // ); - // Climb Up Placeholder - driver.getRightBumper() - .onTrue(new BuzzController(driver)) - .whileTrue(new ClimberUp()) - .onFalse(new HopperDown()); - // Reset Heading - driver.getDPadUp() - .onTrue(new SwerveResetHeading()); + // // Ferry Routine using Interpolation Settings + // driver.getBottomButton() + // .onTrue(new SuperstructureFerry() + // .alongWith(new TurretFerry()) + // .alongWith(new WaitUntilCommand(() -> superstructure.bothAtTolerance())) + // .andThen(new HandoffRun().onlyIf(() -> superstructure.bothAtTolerance()) + // .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) + // .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && superstructure.bothAtTolerance()))) + // ) + // .onFalse(new SpindexerStop() + // .alongWith(new SuperstructureStow()) + // .alongWith(new HandoffStop())); +//-------------------------------------------------------------------------------------------------------------------------\\ +//-------------------------------------------------------------------------------------------------------------------------\\ +//-------------------------------------------------------------------------------------------------------------------------\\ +//-------------------------------------------------------------------------------------------------------------------------\\ +//-------------------------------------------------------------------------------------------------------------------------\\ + /* + // Ferry In Place driver.getDPadLeft() .whileTrue( @@ -321,6 +321,7 @@ public void configureAutons() { autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); + autonChooser.addOption("Wheel Radius", new SwerveWheelRadiusCharacterization()); // TESTS // AutonConfig BOX_TEST = new AutonConfig("Box Test", BoxTest::new, // "Box 1", "Box 2", "Box 3", "Box 4"); diff --git a/src/main/java/com/stuypulse/robot/commands/hood/ZeroHoodAtLowerHardstop.java b/src/main/java/com/stuypulse/robot/commands/hood/ZeroHoodEncoderAtUpperHardstop.java similarity index 68% rename from src/main/java/com/stuypulse/robot/commands/hood/ZeroHoodAtLowerHardstop.java rename to src/main/java/com/stuypulse/robot/commands/hood/ZeroHoodEncoderAtUpperHardstop.java index ef4fab1a..926739e9 100644 --- a/src/main/java/com/stuypulse/robot/commands/hood/ZeroHoodAtLowerHardstop.java +++ b/src/main/java/com/stuypulse/robot/commands/hood/ZeroHoodEncoderAtUpperHardstop.java @@ -4,10 +4,10 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; -public class ZeroHoodAtLowerHardstop extends InstantCommand{ +public class ZeroHoodEncoderAtUpperHardstop extends InstantCommand{ private final Hood hood; - public ZeroHoodAtLowerHardstop() { + public ZeroHoodEncoderAtUpperHardstop() { hood = Hood.getInstance(); addRequirements(hood); @@ -15,7 +15,7 @@ public ZeroHoodAtLowerHardstop() { @Override public void initialize() { - hood.zeroHoodEncoderAtLowerHardstop(); + hood.zeroHoodEncoderAtUpperHardstop(); hood.seedHood(); } } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java index 2c68d4a1..2dcb8b16 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java @@ -17,8 +17,8 @@ public class SwerveWheelRadiusCharacterization extends Command { * - Allow the robot to rotate 3-5 times when running the routine */ - private static final double HALF_TRACK_WIDTH_INCHES = 9.0 / 2; - private static final double HALF_TRACK_LENGTH_INCHES = 13.0 / 2; + private static final double HALF_TRACK_WIDTH_INCHES = 9.0 ; + private static final double HALF_TRACK_LENGTH_INCHES = 13.0 ; private static final double DRIVE_RADIUS_INCHES = Math.sqrt(HALF_TRACK_WIDTH_INCHES * HALF_TRACK_WIDTH_INCHES + HALF_TRACK_LENGTH_INCHES * HALF_TRACK_LENGTH_INCHES); private static final double DRIVE_RADIUS_METERS = Units.inchesToMeters(DRIVE_RADIUS_INCHES); diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 4f71fbac..dcb95981 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -25,10 +25,10 @@ public interface Cameras { // new Pose3d(Units.inchesToMeters(2.490), Units.inchesToMeters(14.8620), Units.inchesToMeters(5.920157), // new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15.0), Units.degreesToRadians(251.))), // RobotContainer.EnabledSubsystems.LIMELIGHT), - // new Camera("limelight-module", - // new Pose3d(Units.inchesToMeters(-12.963), Units.inchesToMeters(-11.0845), Units.inchesToMeters(7.743595), - // new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(31), Units.degreesToRadians(180))), - // RobotContainer.EnabledSubsystems.LIMELIGHT) + new Camera("limelight-module", //11.0845u + new Pose3d(Units.inchesToMeters(-10.768823), Units.inchesToMeters(12.717519), Units.inchesToMeters(8.331714), + new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(31.0), Units.degreesToRadians(185.0))), + RobotContainer.EnabledSubsystems.LIMELIGHT)// 31 // 26.965 //THIS ROTATION IS NOT EXACTLY 180 DEGREES... }; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index fe247734..475e7657 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -97,7 +97,7 @@ public interface Intake { Rotation2d PIVOT_MAX_VEL_STOW = Rotation2d.fromDegrees(360.0); Rotation2d PIVOT_MAX_ACCEL_STOW = Rotation2d.fromDegrees(600.0); - Rotation2d THRESHHOLD_TO_START_ROLLERS = Rotation2d.fromDegrees(60.0); + Rotation2d THRESHHOLD_TO_START_ROLLERS = Rotation2d.fromDegrees(10.0); Rotation2d ARBITRARY_VOLTAGE_THRESHOLD = Rotation2d.fromDegrees(15.0); @@ -125,45 +125,92 @@ public interface Superstructure { public final double SHOOTER_TOLERANCE_RPM = 100.0; public final Rotation2d HOOD_TOLERANCE = Rotation2d.fromDegrees(0.5); - public interface AngleInterpolation { - public final double[][] distanceAngleInterpolationValues = { +public interface AngleInterpolation { + double[][] distanceAngleInterpolationValues = { {1.22, Units.degreesToRadians(20)}, //AGAINST THE HUB - {1.30, Units.degreesToRadians(16.5)}, - {1.43, Units.degreesToRadians(21.0)}, - {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)} + {1.43, Units.degreesToRadians(21.0)}, //meters, radians + {2.15, Units.degreesToRadians(23.23)}, //KEVIN-APPROVED + {2.864967, Units.degreesToRadians(27)}, //KEVIN-APPROVED + {3.65, Units.degreesToRadians(28.0)}, //KEVIN-APPROVED + {4.43, Units.degreesToRadians(33.5)}, //KEVIN-APPROVED + {5.66, Units.degreesToRadians(39)} //KEVIN-APPROVED }; } - - public interface RPMInterpolation { - public final double[][] distanceRPMInterpolationValues = { - {1.22, 2850.0}, //AGAINST THE HUB - {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 RPMInterpolation{ + double[][] distanceRPMInterpolationValues = { + {1.22, 2950.0}, //KEVIN-APPROVED + {1.43, 3000.0}, // meters, RPM + {2.15, 3050.0}, //KEVIN-APPROVED + {2.864967, 3150}, //KEVIN-APPROVED + {3.65, 3400.0}, //KEVIN-APPROVED + {4.43, 3600.0}, //KEVIN-APPROVED + {5.66, 3900.0} //KEVIN-APPROVED }; } public interface TOFInterpolation{ - public final double[][] distanceTOFInterpolationValues = { // COLLECT THESE - // {1.22, 0.0}, - {1.30, 1.01}, // seconds - // {1.43, 0.0}, - // {2.15, 0.0}, - {2.864967, 1.1}, - // {3.65, 0.0}, - {4.43, 1.234}, - {5.32, 1.267} + // double[][] distanceTOFInterpolationValues = { + // {1.30, 0.0}, // seconds + // {1.43, 0.0}, + // {2.15, 0.0}, + // {2.864967, 0.0}, + // {3.65, 0.0}, + // {4.43, 0.0}, + // {5.32, 0.0} + // }; + + double[][] distanceTOFInterpolationValues = { + {1.30, 1.0}, // seconds + {1.43, 1.0}, + {2.15, 1.0}, + {2.864967, 1.0}, + {3.65, 1.0}, + {4.43, 1.0}, + {5.32, 1.0} }; } + // public interface AngleInterpolation { + // public final double[][] distanceAngleInterpolationValues = { + // {1.22, Units.degreesToRadians(20)}, //AGAINST THE HUB + // {1.43, Units.degreesToRadians(21.0)}, + // {2.15, Units.degreesToRadians(23.23)}, + + // //NEW + // {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 { + // public final double[][] distanceRPMInterpolationValues = { + // {1.22, 2850.0}, //AGAINST THE HUB + // {1.43, 3000.0}, + // {2.15, 3050.0}, + + // //NEW + // {2.864967, 3215.271125}, + // {3.65, 3400.0}, + // {4.43, 3650.0}, + // {5.32, 3950.0} + // }; + // } + + // public interface TOFInterpolation{ + // public final double[][] distanceTOFInterpolationValues = { // COLLECT THESE + // // {1.22, 0.0}, + // {1.30, 1.01}, // seconds + // // {1.43, 0.0}, + // // {2.15, 0.0}, + // {2.864967, 1.1}, + // // {3.65, 0.0}, + // {4.43, 1.234}, + // {5.32, 1.267} + // }; + // } + public interface FerryRPMInterpolation { public final double[][] distanceRPMInterpolationValues = { {3.79, 3450.0} @@ -204,7 +251,8 @@ public interface Hood { public final Rotation2d ENCODER_OFFSET = Rotation2d.fromRotations(-0.043); public final Rotation2d FORWARD_SOFT_LIMIT = Rotation2d.fromDegrees(39.0); - public final Rotation2d REVERSE_SOFT_LIMIT = Rotation2d.fromDegrees(8.0); + public final Rotation2d REVERSE_SOFT_LIMIT = Rotation2d.fromDegrees(20.0); + public final Rotation2d MIN_FROM_HORIZON = Rotation2d.fromDegrees(7.0); public final double STALL_CURRENT_LIMIT = 20.0; public final double STALL_DEBOUNCE = 0.5; @@ -213,7 +261,7 @@ public interface Angles { public final SmartNumber SHOOT = new SmartNumber("InterpolationTesting/Shoot State Target Angle (deg)", 15.0); public final SmartNumber FERRY = new SmartNumber("InterpolationTesting/Ferry State Target Angle (deg)", 20.0); - public final Rotation2d MIN = Rotation2d.fromDegrees(7.0); + public final Rotation2d MIN = Rotation2d.fromDegrees(20.0); public final Rotation2d MAX = Rotation2d.fromDegrees(40.0); public final Rotation2d STOW = Rotation2d.fromDegrees(11.0); @@ -228,7 +276,7 @@ public interface Turret { public final Rotation2d MAX_ACCEL = new Rotation2d(Units.degreesToRadians(600.0)); public final Rotation2d TOLERANCE = Rotation2d.fromDegrees(2.0); - public final Rotation2d HUB = Rotation2d.fromDegrees(0.0); + public final Rotation2d KB = Rotation2d.fromDegrees(0.0); public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(0.0); public final Rotation2d RIGHT_CORNER = Rotation2d.fromDegrees(0.0); 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 8f716c50..38b72a2d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -171,13 +171,11 @@ public void periodic() { rollerFollower.setControl(follower); } else { rollerLeader.stopMotor(); - rollerFollower.stopMotor(); } } } else { pivot.stopMotor(); rollerLeader.stopMotor(); - rollerFollower.stopMotor(); } if (Settings.DEBUG_MODE) { 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 8ddf9e2a..519e44e6 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -71,6 +71,8 @@ public SpindexerImpl() { controller = new VelocityVoltage(getTargetRPM()).withEnableFOC(true); follower = new Follower(Ports.Spindexer.SPINDEXER_LEAD_MOTOR, MotorAlignmentValue.Aligned); + followerMotor.setControl(follower); + voltageOverride = Optional.empty(); } @@ -97,15 +99,12 @@ public void periodic() { } else { if (atTolerance() && getState() == SpindexerState.STOP) { leadMotor.stopMotor(); - followerMotor.stopMotor(); } else { leadMotor.setControl(controller.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE)); - followerMotor.setControl(follower); } } } else { leadMotor.stopMotor(); - followerMotor.stopMotor(); } if (Settings.DEBUG_MODE) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index e56f17ca..d4cf7cf5 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -50,7 +50,7 @@ public enum SuperstructureState { SHOOT(HoodState.SHOOT, ShooterState.SHOOT, TurretState.SHOOT), FERRY(HoodState.FERRY, ShooterState.FERRY, TurretState.FERRY), REVERSE(HoodState.SHOOT, ShooterState.REVERSE, TurretState.SHOOT), - KB(HoodState.KB, ShooterState.KB, TurretState.SHOOT), + KB(HoodState.KB, ShooterState.KB, TurretState.KB), LEFT_CORNER(HoodState.LEFT_CORNER, ShooterState.LEFT_CORNER, TurretState.SHOOT), RIGHT_CORNER(HoodState.RIGHT_CORNER, ShooterState.RIGHT_CORNER, TurretState.SHOOT), INTERPOLATION(HoodState.INTERPOLATION, ShooterState.INTERPOLATION, TurretState.SHOOT), @@ -91,7 +91,7 @@ public SuperstructureState getState(){ } public boolean atTolerance() { - return isShooterAtTolerance() && isHoodAtTolerance() && isTurretAtTolerance(); + return isShooterAtTolerance() && isHoodAtTolerance();// && isTurretAtTolerance(); } public boolean isHoodUnderTrench() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java index e60e188c..1402e7ee 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java @@ -108,7 +108,7 @@ public Rotation2d hoodAnalogToOutput() { public abstract SysIdRoutine getHoodSysIdRoutine(); - public abstract void zeroHoodEncoderAtLowerHardstop(); + public abstract void zeroHoodEncoderAtUpperHardstop(); public abstract void seedHood(); @Override diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index 242d81c5..7581b474 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -93,13 +93,13 @@ public Rotation2d getAngle() { //TODO: Implementation as of 3/3 but not yet tested // Should ONLY be called at the lower hardstop! @Override - public void zeroHoodEncoderAtLowerHardstop() { + public void zeroHoodEncoderAtUpperHardstop() { hoodEncoder.getConfigurator().refresh(hoodEncoderConfig.getConfiguration().MagnetSensor); double currentOffset = hoodEncoderConfig.getConfiguration().MagnetSensor.MagnetOffset; double positionWithCurrentOffset = hoodEncoder.getPosition().getValueAsDouble(); - double newOffset = -((positionWithCurrentOffset - currentOffset) - Settings.Superstructure.Hood.Angles.MIN.getRotations()); + double newOffset = -((positionWithCurrentOffset - currentOffset) - Settings.Superstructure.Hood.Angles.MAX.getRotations()); hoodEncoderConfig.withMagnetOffset(newOffset); @@ -139,7 +139,7 @@ public void seedHood() { } private double getAbsoluteHoodAngleDeg() { - return Settings.Superstructure.Hood.Angles.MIN.getDegrees() + hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.Superstructure.Hood.ENCODER_TO_MECH; + return Settings.Superstructure.Hood.MIN_FROM_HORIZON.getDegrees() + hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.Superstructure.Hood.ENCODER_TO_MECH; } @Override diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java index 91f4a9d9..7ff1b54f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java @@ -71,7 +71,7 @@ public boolean isStalling() { } @Override - public void zeroHoodEncoderAtLowerHardstop() { + public void zeroHoodEncoderAtUpperHardstop() { // No-op in sim } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java index 0584ef22..bb3c7626 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -103,7 +103,6 @@ public void periodic() { if (EnabledSubsystems.SHOOTER.get()) { if (getState() == ShooterState.STOP) { shooterLeader.stopMotor(); - shooterFollower.stopMotor(); } else if (voltageOverride.isPresent()) { shooterLeader.setVoltage(voltageOverride.get()); shooterFollower.setControl(follower); @@ -113,7 +112,6 @@ public void periodic() { } } else { shooterLeader.stopMotor(); - shooterFollower.stopMotor(); } if (Settings.DEBUG_MODE) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java index 5f3b2f15..13f4f79e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java @@ -54,6 +54,7 @@ public enum TurretState { FERRY, LEFT_CORNER, RIGHT_CORNER, + KB, TESTING; } @@ -66,6 +67,7 @@ public Rotation2d getTargetAngle() { case FERRY -> getFerryAngle(); case LEFT_CORNER -> Settings.Superstructure.Turret.LEFT_CORNER; case RIGHT_CORNER -> Settings.Superstructure.Turret.RIGHT_CORNER; + case KB -> Settings.Superstructure.Turret.KB; case TESTING -> driverInputToAngle(); }; } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index 3c50246e..d5b40d0a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -173,7 +173,7 @@ public void periodic() { SmartDashboard.putNumber("Superstructure/Turret/Encoder18t Abs Position (Rot)", encoder18t.getAbsolutePosition().getValueAsDouble()); SmartDashboard.putNumber("Superstructure/Turret/Encoder17t Abs Position (Rot)", encoder17t.getAbsolutePosition().getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Turret/Vector Space Position (Deg)", getVectorSpaceAngle().getDegrees()); + // SmartDashboard.putNumber("Superstructure/Turret/Vector Space Position (Deg)", getVectorSpaceAngle().getDegrees()); SmartDashboard.putNumber("Superstructure/Turret/Voltage", motor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Superstructure/Turret/Stator Current", motor.getStatorCurrent().getValueAsDouble()); From ba8433f4c3abe7331f455d8850ad3148cdab6ed0 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 5 Mar 2026 21:55:51 -0500 Subject: [PATCH 083/150] feat: turret offsets + intake test --- src/main/java/com/stuypulse/robot/RobotContainer.java | 4 ++-- src/main/java/com/stuypulse/robot/constants/Settings.java | 4 +++- .../robot/subsystems/superstructure/Superstructure.java | 4 ++++ .../robot/subsystems/superstructure/turret/TurretImpl.java | 4 +++- 4 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index b54e3ba9..3362bbd5 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -146,7 +146,7 @@ private void configureDefaultCommands() { private void configureButtonBindings() { // Scoring Routine driver.getTopButton() - .whileTrue(new SuperstructureInterpolation().onlyIf(() -> !superstructure.isHoodUnderTrench()) + .whileTrue(new SuperstructureInterpolation()//.onlyIf(() -> !superstructure.isHoodUnderTrench()) // .alongWith(new SwerveDriveAlignTurretToHub()) // .alongWith(new TurretShoot()) .andThen(new WaitUntilCommand(superstructure::atTolerance)) @@ -159,7 +159,7 @@ private void configureButtonBindings() { // Scoring Routine driver.getBottomButton() - .whileTrue(new SuperstructureShoot().onlyIf(() -> !superstructure.isHoodUnderTrench()) + .whileTrue(new SuperstructureShoot())//.onlyIf(() -> !superstructure.isHoodUnderTrench())) // .alongWith(new SwerveDriveAlignTurretToHub()) // .alongWith(new TurretShoot()) .whileTrue(new SuperstructureShoot().onlyIf(() -> !swerve.isUnderTrench()) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 475e7657..20ac28b8 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -86,7 +86,7 @@ public interface Intake { Rotation2d PIVOT_STOW_ANGLE = Rotation2d.fromDegrees(90.0); Rotation2d PIVOT_DEPLOY_ANGLE = Rotation2d.fromDegrees(0.0); - Rotation2d PIVOT_ANGLE_TOLERANCE = Rotation2d.fromDegrees(3.0); + Rotation2d PIVOT_ANGLE_TOLERANCE = Rotation2d.fromDegrees(15.0); Rotation2d PIVOT_MAX_ANGLE = Rotation2d.fromDegrees(90.0); Rotation2d PIVOT_MIN_ANGLE = Rotation2d.fromDegrees(0.0); @@ -287,6 +287,8 @@ public interface Turret { /* CONSTANTS */ public final double RANGE = 210.0; + public final double RANGE_LOWER = -45; + public final double RANGE_UPPER = 390; public final double SLOT_SWITCHING_THRESHOLD_ROT = .5; diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index 81c1ff82..ba096d78 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -94,6 +94,10 @@ public boolean atTolerance() { return isShooterAtTolerance() && isHoodAtTolerance();// && isTurretAtTolerance(); } + // public boolean isHoodUnderTrench() { + // return hood.isUnderTrench(); + // } + public boolean isShooterAtTolerance() { return shooter.atTolerance(); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index d5b40d0a..fc432214 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -139,7 +139,9 @@ private double getDelta(double target, double current) { if (delta > 180.0) delta -= 360; else if (delta < -180) delta += 360; - if (Math.abs(current + delta) < Settings.Superstructure.Turret.RANGE) return delta; + // if (Math.abs(current + delta) < Settings.Superstructure.Turret.RANGE) return delta; + if (current + delta < Settings.Superstructure.Turret.RANGE_LOWER) return delta + 360; + if (current + delta > Settings.Superstructure.Turret.RANGE_UPPER) return delta - 360; return delta < 0 ? delta + 360 : delta - 360; } From 2cdd070432ece4c21ce7841bce0d1fb05882e26f Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Thu, 5 Mar 2026 21:56:15 -0500 Subject: [PATCH 084/150] fix: fix last commit w/ unsaved files --- .../java/com/stuypulse/robot/constants/Settings.java | 9 ++++----- .../stuypulse/robot/subsystems/intake/IntakeImpl.java | 5 +++-- .../subsystems/superstructure/turret/TurretImpl.java | 4 ++-- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 20ac28b8..22e1adbd 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -286,9 +286,8 @@ public interface Turret { Rotation2d MIN_THEORETICAL_ROTATION = Rotation2d.fromDegrees(-612); /* CONSTANTS */ - public final double RANGE = 210.0; - public final double RANGE_LOWER = -45; - public final double RANGE_UPPER = 390; + public final double RANGE_LEFT = -45; + public final double RANGE_RIGHT = 390; public final double SLOT_SWITCHING_THRESHOLD_ROT = .5; @@ -303,12 +302,12 @@ public interface BigGear { public interface Encoder17t { public final int TEETH = 17; - public final Rotation2d OFFSET = new Rotation2d(); + public final Rotation2d OFFSET = new Rotation2d(); //-0.86962890625 } public interface Encoder18t { public final int TEETH = 18; - public final Rotation2d OFFSET = new Rotation2d(); + public final Rotation2d OFFSET = new Rotation2d(); //-0.700927734375 } public interface SoftwareLimit { 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 38b72a2d..8d1fb367 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -71,7 +71,7 @@ public IntakeImpl() { rollerConfig = new Motors.TalonFXConfig() .withInvertedValue(InvertedValue.CounterClockwise_Positive) - .withNeutralMode(NeutralModeValue.Coast) + .withNeutralMode(NeutralModeValue.Brake) .withSupplyCurrentLimitAmps(45) .withStatorCurrentLimitEnabled(false) @@ -168,14 +168,15 @@ public void periodic() { // ROLLERS if (getPivotAngle().getDegrees() <= Settings.Intake.THRESHHOLD_TO_START_ROLLERS.getDegrees()) { rollerLeader.setControl(rollerController.withOutput(getRollerState().getTargetDutyCycle())); - rollerFollower.setControl(follower); } else { rollerLeader.stopMotor(); } + rollerFollower.setControl(follower); } } else { pivot.stopMotor(); rollerLeader.stopMotor(); + rollerFollower.stopMotor(); } if (Settings.DEBUG_MODE) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index fc432214..8db19809 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -140,8 +140,8 @@ private double getDelta(double target, double current) { else if (delta < -180) delta += 360; // if (Math.abs(current + delta) < Settings.Superstructure.Turret.RANGE) return delta; - if (current + delta < Settings.Superstructure.Turret.RANGE_LOWER) return delta + 360; - if (current + delta > Settings.Superstructure.Turret.RANGE_UPPER) return delta - 360; + if (current + delta < Settings.Superstructure.Turret.RANGE_LEFT) return delta + 360; + if (current + delta > Settings.Superstructure.Turret.RANGE_RIGHT) return delta - 360; return delta < 0 ? delta + 360 : delta - 360; } From 666ae93d34fba4ba08ddc73034808aa1be6508b0 Mon Sep 17 00:00:00 2001 From: Lucas Date: Thu, 5 Mar 2026 21:56:24 -0500 Subject: [PATCH 085/150] TEST: need to test --- .../robot/commands/handoff/HandoffConditionalCommand.java | 5 ++++- .../commands/spindexer/SpindexerConditionalCommand.java | 3 +++ .../robot/subsystems/superstructure/Superstructure.java | 1 + 3 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffConditionalCommand.java b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffConditionalCommand.java index efc95db8..5b79f3a2 100644 --- a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffConditionalCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffConditionalCommand.java @@ -2,10 +2,13 @@ import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.ConditionalCommand; -public class HandoffConditionalCommand extends ConditionalCommand{ +public class HandoffConditionalCommand extends ConditionalCommand { public HandoffConditionalCommand() { super(new HandoffStop(), new HandoffRun(), () -> (CommandSwerveDrivetrain.getInstance().isBehindTower() || CommandSwerveDrivetrain.getInstance().isBehindHub())); + // DEBUG + SmartDashboard.putBoolean("FieldPositions/Should Handoff Stop", (CommandSwerveDrivetrain.getInstance().isBehindTower() || CommandSwerveDrivetrain.getInstance().isBehindHub())); } } diff --git a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java index b52cd230..c1cd6ddf 100644 --- a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java @@ -6,10 +6,13 @@ package com.stuypulse.robot.commands.spindexer; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.ConditionalCommand; public class SpindexerConditionalCommand extends ConditionalCommand { public SpindexerConditionalCommand() { super(new SpindexerStop(), new SpindexerRun(), () -> (CommandSwerveDrivetrain.getInstance().isBehindTower() || CommandSwerveDrivetrain.getInstance().isBehindHub())); + // DEBUG + SmartDashboard.putBoolean("FieldPositions/Should Spindexer Stop", (CommandSwerveDrivetrain.getInstance().isBehindTower() || CommandSwerveDrivetrain.getInstance().isBehindHub())); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index 4c940663..f3cc1e7d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -134,6 +134,7 @@ public void periodic() { SmartDashboard.putBoolean("Superstructure/Shooter At Tolerance?", isShooterAtTolerance()); SmartDashboard.putBoolean("Superstructure/Hood At Tolerance?", isHoodAtTolerance()); SmartDashboard.putBoolean("Superstructure/Turret At Tolerance?", isTurretAtTolerance()); + SmartDashboard.putBoolean("Superstructure/Everything At Tolerance?", atTolerance()); SmartDashboard.putNumber("InterpolationTesting/Current Hood Angle", getHoodAngle().getDegrees()); SmartDashboard.putNumber("InterpolationTesting/Current Shooter RPM", getShooterRPM()); From 794724265f92bd9d3375edba5a56e5b52366c91a Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Fri, 6 Mar 2026 00:34:45 -0500 Subject: [PATCH 086/150] FIX: tunerconstants drivetrain dimensions were wrong --- .../SwerveWheelRadiusCharacterization.java | 10 ++-------- .../robot/subsystems/swerve/TunerConstants.java | 16 ++++++++-------- 2 files changed, 10 insertions(+), 16 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java index 2dcb8b16..504e8603 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java @@ -2,7 +2,6 @@ import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; @@ -17,18 +16,16 @@ public class SwerveWheelRadiusCharacterization extends Command { * - Allow the robot to rotate 3-5 times when running the routine */ - private static final double HALF_TRACK_WIDTH_INCHES = 9.0 ; - private static final double HALF_TRACK_LENGTH_INCHES = 13.0 ; + private static final double HALF_TRACK_WIDTH_INCHES = 9.0; + private static final double HALF_TRACK_LENGTH_INCHES = 13.0; private static final double DRIVE_RADIUS_INCHES = Math.sqrt(HALF_TRACK_WIDTH_INCHES * HALF_TRACK_WIDTH_INCHES + HALF_TRACK_LENGTH_INCHES * HALF_TRACK_LENGTH_INCHES); private static final double DRIVE_RADIUS_METERS = Units.inchesToMeters(DRIVE_RADIUS_INCHES); private static final double TARGET_ROTATIONAL_RATE = 1.5; // rad/s about center of rotation private static final double RAMP_TIME = 0.75; // seconds to reach target - // private static final double RAMP_RATE = 0.05; private final Timer timer = new Timer(); - // private final SlewRateLimiter limiter = new SlewRateLimiter(RAMP_RATE); private final CommandSwerveDrivetrain swerve; @@ -62,9 +59,6 @@ public void execute() { commandedRate = TARGET_ROTATIONAL_RATE; // Steady state omega } - // Go back to WPILib SlewRateLimiter if manual ramping doesn't work well - // double speed = limiter.calculate(TARGET_ROTATIONAL_RATE); - swerve.setControl(swerve.getFieldCentricSwerveRequest() .withVelocityX(0.0) .withVelocityY(0.0) diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java index ce81aad8..a51f6c1c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java @@ -141,8 +141,8 @@ public class TunerConstants { private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; - private static final Distance kFrontLeftXPos = Inches.of(4.5); - private static final Distance kFrontLeftYPos = Inches.of(6.5); + private static final Distance kFrontLeftXPos = Inches.of(9.0); + private static final Distance kFrontLeftYPos = Inches.of(13.0); // Front Right private static final int kFrontRightDriveMotorId = 13; @@ -152,8 +152,8 @@ public class TunerConstants { private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; - private static final Distance kFrontRightXPos = Inches.of(4.5); - private static final Distance kFrontRightYPos = Inches.of(-6.5); + private static final Distance kFrontRightXPos = Inches.of(9.0); + private static final Distance kFrontRightYPos = Inches.of(-13.0); // Back Left private static final int kBackLeftDriveMotorId = 16; @@ -163,8 +163,8 @@ public class TunerConstants { private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; - private static final Distance kBackLeftXPos = Inches.of(-4.5); - private static final Distance kBackLeftYPos = Inches.of(6.5); + private static final Distance kBackLeftXPos = Inches.of(-9.0); + private static final Distance kBackLeftYPos = Inches.of(13.0); // Back Right private static final int kBackRightDriveMotorId = 11; @@ -174,8 +174,8 @@ public class TunerConstants { private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; - private static final Distance kBackRightXPos = Inches.of(-4.5); - private static final Distance kBackRightYPos = Inches.of(-6.5); + private static final Distance kBackRightXPos = Inches.of(-9.0); + private static final Distance kBackRightYPos = Inches.of(-13.0); public static final SwerveModuleConstants FrontLeft = From 11dbd8ee598a2e521637dcbf8dfa9f335bdfa4a1 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 6 Mar 2026 12:08:12 -0500 Subject: [PATCH 087/150] FEAT: get climber up setpoint --- src/main/java/com/stuypulse/robot/RobotContainer.java | 6 +++--- src/main/java/com/stuypulse/robot/constants/Cameras.java | 8 ++++---- src/main/java/com/stuypulse/robot/constants/Settings.java | 4 ++-- .../robot/subsystems/climberhopper/ClimberHopperImpl.java | 5 +++-- 4 files changed, 12 insertions(+), 11 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 3362bbd5..34ce8899 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -217,11 +217,11 @@ private void configureButtonBindings() { .whileTrue(new SwerveClimbAlign().alongWith(new ClimberUp()).andThen(new SwerveXMode())); // Climber Up - driver.getLeftTriggerButton() - .onTrue(new ClimberUp()); + driver.getRightBumper() + .whileTrue(new ClimberUp()); // Climber Down - driver.getRightTriggerButton() + driver.getLeftBumper() .onTrue(new ClimberDown()); // Left Corner Shoot diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index dcb95981..5c5083f2 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -21,10 +21,10 @@ public interface Cameras { new Pose3d(-0.233, 0.375959, 0.20368, new Rotation3d(Units.degreesToRadians(180), Units.degreesToRadians(31.08), Units.degreesToRadians(-80))), RobotContainer.EnabledSubsystems.LIMELIGHT), - // new Camera("limelight-shooter", - // new Pose3d(Units.inchesToMeters(2.490), Units.inchesToMeters(14.8620), Units.inchesToMeters(5.920157), - // new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15.0), Units.degreesToRadians(251.))), - // RobotContainer.EnabledSubsystems.LIMELIGHT), + new Camera("limelight-shooter", + new Pose3d(Units.inchesToMeters(2.490), Units.inchesToMeters(14.8620), Units.inchesToMeters(5.920157), + new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15.0), Units.degreesToRadians(251.0))), + RobotContainer.EnabledSubsystems.LIMELIGHT), new Camera("limelight-module", //11.0845u new Pose3d(Units.inchesToMeters(-10.768823), Units.inchesToMeters(12.717519), Units.inchesToMeters(8.331714), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(31.0), Units.degreesToRadians(185.0))), diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 22e1adbd..8dcf08c6 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -41,7 +41,7 @@ public interface ClimberHopper { public final double GEAR_RATIO = 45.0; public final double MIN_HEIGHT_METERS = 0.0; - public final double MAX_HEIGHT_METERS = 1.0; + public final double MAX_HEIGHT_METERS = 2.884; public final double MASS_KG = 1.0; @@ -69,7 +69,7 @@ public interface ClimberHopper { public final double RAMP_RATE = 50.0; - public final double MOTOR_VOLTAGE = 12; + public final double MOTOR_VOLTAGE = 1; } public interface Handoff { 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 f601a9c0..5dfc1546 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java @@ -91,8 +91,8 @@ public void periodic() { if (voltageOverride.isPresent()) { voltage = voltageOverride.get(); } else { - if (getState() == ClimberHopperState.CLIMBER_DOWN) voltage = -Settings.ClimberHopper.MOTOR_VOLTAGE; - else if (getState() == ClimberHopperState.CLIMBER_UP) voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; + if (getState() == ClimberHopperState.CLIMBER_DOWN && !atTargetHeight()) voltage = -Settings.ClimberHopper.MOTOR_VOLTAGE; + else if (getState() == ClimberHopperState.CLIMBER_UP && !atTargetHeight()) voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; else voltage = 0; } @@ -106,6 +106,7 @@ public void periodic() { SmartDashboard.putBoolean("ClimberHopper/Stalling", getStalling()); SmartDashboard.putNumber("ClimberHopper/Current Height", getCurrentHeight()); + SmartDashboard.putBoolean("Climber/At Target Height?", atTargetHeight()); SmartDashboard.putNumber("ClimberHopper/Voltage", voltage); SmartDashboard.putNumber("ClimberHopper/Applied Voltage", motor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("ClimberHopper/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); From 4e454bf2601932de01b270e0c2b18a0235c80876 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 6 Mar 2026 14:51:58 -0500 Subject: [PATCH 088/150] ADD: Climb rotation constants --- .../stuypulse/robot/constants/Settings.java | 24 +++++++++++++------ .../climberhopper/ClimberHopper.java | 10 ++++++-- .../climberhopper/ClimberHopperImpl.java | 19 +++++++++++---- .../climberhopper/ClimberHopperSim.java | 5 ++++ 4 files changed, 44 insertions(+), 14 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 8dcf08c6..cfc5d445 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -41,21 +41,30 @@ public interface ClimberHopper { public final double GEAR_RATIO = 45.0; public final double MIN_HEIGHT_METERS = 0.0; + // public final double MIN_ROTATIONS = -0.1; public final double MAX_HEIGHT_METERS = 2.884; + // public final double MAX_ROTATIONS = 20; + public final double MASS_KG = 1.0; - public final double NUM_ROTATIONS_TO_REACH_TOP = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (0.480 / 13.0); + // public final double NUM_ROTATIONS_TO_REACH_TOP = MAX_ROTATIONS - MIN_ROTATIONS; + public final double NUM_ROTATIONS_TO_REACH_TOP = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (0.480 / 13.0); // TODO: verify this 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; + public final double DRUM_RADIUS_METERS = ((MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (NUM_ROTATIONS_TO_REACH_TOP / GEAR_RATIO)) / 2.0 / Math.PI; /* CONSTANTS */ public final double CLIMBER_UP_HEIGHT_METERS = MAX_HEIGHT_METERS; - public final double CLIMBER_DOWN_HEIGHT_METERS = 0.2; + // public final double CLIMBER_UP_ROTATIONS = MAX_ROTATIONS; // TODO: FIND + public final double CLIMBER_DOWN_HEIGHT_METERS = MIN_HEIGHT_METERS; + // public final double CLIMBER_DOWN_ROTATIONS = MIN_ROTATIONS; public final double HOPPER_DOWN_HEIGHT_METERS = MIN_HEIGHT_METERS; - public final double HOPPER_UP_HEIGHT_METERS = 0.5; + // public final double HOPPER_DOWN_ROTATIONS = MIN_ROTATIONS; + public final double HOPPER_UP_HEIGHT_METERS = MAX_HEIGHT_METERS; + // public final double HOPPER_UP_ROTATIONS = MAX_ROTATIONS; public final double STALL = 10.0; @@ -65,11 +74,12 @@ public interface ClimberHopper { public final double GYRO_TOLERANCE = 0.0; - public final double HEIGHT_TOLERANCE_METERS = 0.02; + public final double HEIGHT_TOLERANCE_METERS = 0.1; + // public final double TOLERANCE_ROTATIONS = 0.1; public final double RAMP_RATE = 50.0; - public final double MOTOR_VOLTAGE = 1; + public final double MOTOR_VOLTAGE = 1.0; } public interface Handoff { @@ -302,12 +312,12 @@ public interface BigGear { public interface Encoder17t { public final int TEETH = 17; - public final Rotation2d OFFSET = new Rotation2d(); //-0.86962890625 + public final Rotation2d OFFSET = Rotation2d.fromRotations(-0.279541015625);//(-0.86962890625); //0.6787109375 } public interface Encoder18t { public final int TEETH = 18; - public final Rotation2d OFFSET = new Rotation2d(); //-0.700927734375 + public final Rotation2d OFFSET = Rotation2d.fromRotations(-0.42822265625);//(-0.700927734375); //0.53564453125 } public interface SoftwareLimit { 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 63160b9b..97fb6487 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java @@ -29,10 +29,15 @@ public static ClimberHopper getInstance() { } public enum ClimberHopperState { + // CLIMBER_UP(Settings.ClimberHopper.CLIMBER_UP_ROTATIONS), + // CLIMBER_DOWN(Settings.ClimberHopper.CLIMBER_DOWN_ROTATIONS), + // // HOPPER_UP(Settings.ClimberHopper.HOPPER_UP_ROTATIONS), + // HOPPER_DOWN(Settings.ClimberHopper.HOPPER_DOWN_ROTATIONS), 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_DOWN(Settings.ClimberHopper.HOPPER_DOWN_HEIGHT_METERS); + // HOPPER_UP(Settings.ClimberHopper.HOPPER_UP_ROTATIONS), + HOPPER_DOWN(Settings.ClimberHopper.HOPPER_UP_HEIGHT_METERS), + STOP(0.0); private double targetHeight; @@ -62,6 +67,7 @@ public void setState(ClimberHopperState state) { public abstract boolean getStalling(); public abstract double getCurrentHeight(); + public abstract double getCurrentRotations(); public abstract boolean atTargetHeight(); /** 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 5dfc1546..5b072d62 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java @@ -42,7 +42,7 @@ public ClimberHopperImpl() { .withSupplyCurrentLimitAmps(50.0) .withStatorCurrentLimitEnabled(false) .withRampRate(Settings.ClimberHopper.RAMP_RATE) - + .withSensorToMechanismRatio(Settings.ClimberHopper.GEAR_RATIO) .withSoftLimits( false, false, Settings.ClimberHopper.ROTATIONS_AT_BOTTOM + Settings.ClimberHopper.NUM_ROTATIONS_TO_REACH_TOP, @@ -71,6 +71,11 @@ public double getCurrentHeight() { return this.motor.getPosition().getValueAsDouble() * Settings.ClimberHopper.POSITION_CONVERSION_FACTOR; } + @Override + public double getCurrentRotations() { + return motor.getPosition().getValueAsDouble(); + } + private boolean isWithinTolerance(double toleranceMeters) { return Math.abs(getState().getTargetHeight() - getCurrentHeight()) < toleranceMeters; } @@ -90,10 +95,14 @@ public void periodic() { if (voltageOverride.isPresent()) { voltage = voltageOverride.get(); - } else { - if (getState() == ClimberHopperState.CLIMBER_DOWN && !atTargetHeight()) voltage = -Settings.ClimberHopper.MOTOR_VOLTAGE; - else if (getState() == ClimberHopperState.CLIMBER_UP && !atTargetHeight()) voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; - else voltage = 0; + } else { + if (!atTargetHeight()) { + if (getState() == ClimberHopperState.CLIMBER_DOWN) voltage = -Settings.ClimberHopper.MOTOR_VOLTAGE; + else if (getState() == ClimberHopperState.CLIMBER_UP) voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; + else if (getState() == ClimberHopperState.HOPPER_DOWN) voltage = -Settings.ClimberHopper.MOTOR_VOLTAGE; + } else { + voltage = 0; + } } if (EnabledSubsystems.CLIMBER_HOPPER.get()) { 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 18b1eae9..cd39369a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java @@ -49,6 +49,11 @@ public double getCurrentHeight() { return sim.getPositionMeters(); } + @Override + public double getCurrentRotations() { + return sim.getPositionMeters() / Settings.ClimberHopper.POSITION_CONVERSION_FACTOR; + } + private boolean isWithinTolerance(double toleranceMeters) { return Math.abs(getState().getTargetHeight() - getCurrentHeight()) < toleranceMeters; } From 52e8ac6bffe672474e525a245a09b1e29c43b9ff Mon Sep 17 00:00:00 2001 From: Lucas Date: Fri, 6 Mar 2026 16:08:11 -0500 Subject: [PATCH 089/150] FIX+FEAT: ClimberHopper only buttons and swich set state from InstantCommand to Command --- .../com/stuypulse/robot/RobotContainer.java | 27 ++++++++++++++----- .../climberhopper/ClimberHopperSetState.java | 4 +-- .../stuypulse/robot/constants/Settings.java | 2 +- 3 files changed, 23 insertions(+), 10 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index a1a6a822..26612fb8 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -21,6 +21,7 @@ import com.stuypulse.robot.commands.climberhopper.ClimberOverrideUp; import com.stuypulse.robot.commands.handoff.HandoffConditionalCommand; import com.stuypulse.robot.commands.climberhopper.ClimberUp; +import com.stuypulse.robot.commands.climberhopper.HopperDown; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.hood.ZeroHoodEncoderAtUpperHardstop; @@ -55,6 +56,7 @@ import com.stuypulse.robot.commands.vision.SetIMUMode; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.superstructure.hood.Hood; @@ -218,13 +220,24 @@ private void configureButtonBindings() { driver.getTopButton() .whileTrue(new SwerveClimbAlign().alongWith(new ClimberUp()).andThen(new SwerveXMode())); - // Climber Up - driver.getRightBumper() - .whileTrue(new ClimberUp()); - - // Climber Down - driver.getLeftBumper() - .onTrue(new ClimberDown()); + if (Settings.DEBUG_MODE) { + // Climber Up + driver.getRightBumper() + .whileTrue(new ClimberUp()); + + // Climber Down + driver.getLeftBumper() + .onTrue(new ClimberDown()); + } else { + // Climber Up + driver.getRightBumper() + .whileTrue(new ClimberUp()) + .onFalse(new HopperDown()); + + // Climber Down + driver.getLeftBumper() + .onTrue(new ClimberDown()); + } // Left Corner Shoot driver.getLeftButton() 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 85d8d6cd..6849ef97 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.InstantCommand; +import edu.wpi.first.wpilibj2.command.Command; -public class ClimberHopperSetState extends InstantCommand { +public class ClimberHopperSetState extends Command { private final ClimberHopper climberHopper = ClimberHopper.getInstance(); private final ClimberHopperState state; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 8dcf08c6..53911ba2 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -41,7 +41,7 @@ public interface ClimberHopper { public final double GEAR_RATIO = 45.0; public final double MIN_HEIGHT_METERS = 0.0; - public final double MAX_HEIGHT_METERS = 2.884; + public final double MAX_HEIGHT_METERS = 0.5; public final double MASS_KG = 1.0; From d80a7f68cadbc76c530117d2d02966b8bf69d611 Mon Sep 17 00:00:00 2001 From: szhu70 Date: Fri, 6 Mar 2026 17:10:39 -0500 Subject: [PATCH 090/150] TEST: atTolerance was our culprit for non responsive spindexer and handoff, might just be broken in sim, default command is to be fixed, and change SOTM to be toggle --- simgui.json | 9 +-------- src/main/java/com/stuypulse/robot/RobotContainer.java | 10 +++++----- .../spindexer/SpindexerConditionalCommand.java | 2 +- .../superstructure/SuperstructureDefaultCommand.java | 11 +++++------ 4 files changed, 12 insertions(+), 20 deletions(-) diff --git a/simgui.json b/simgui.json index 84fb2ea6..a6cbfa70 100644 --- a/simgui.json +++ b/simgui.json @@ -21,16 +21,12 @@ "/SmartDashboard/Robot/Zero Pivot Encoder at Lower Limit (Deployed)": "Command", "/SmartDashboard/Robot/Zero Pivot Encoder at Upper Limit (Stowed)": "Command", "/SmartDashboard/Robot/Zero Turret Encoders": "Command", + "/SmartDashboard/Scheduler": "Scheduler", "/SmartDashboard/Visualizers/ClimberHopper": "Mechanism2d", "/SmartDashboard/Visualizers/Hood": "Mechanism2d", "/SmartDashboard/Visualizers/Turret": "Mechanism2d" }, "windows": { - "/SmartDashboard/Autonomous": { - "window": { - "visible": true - } - }, "/SmartDashboard/Field": { "bottom": 1914, "height": 8.069275856018066, @@ -63,9 +59,6 @@ }, "transitory": { "SmartDashboard": { - "FieldPositions": { - "open": true - }, "HoodedShooter": { "Hood": { "open": true diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index a1a6a822..00b307ad 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -136,7 +136,7 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); - superstructure.setDefaultCommand(new SuperstructureDefaultCommand()); + // superstructure.setDefaultCommand(new SuperstructureDefaultCommand()); // climberHopper.setDefaultCommand(new ClimberHopperDefaultCommand()); // turret.setDefaultCommand(new TurretDefaultCommand()); } @@ -161,16 +161,16 @@ private void configureButtonBindings() { // Scoring Routine driver.getBottomButton() - .whileTrue(new SuperstructureShoot())//.onlyIf(() -> !superstructure.isHoodUnderTrench())) + //.whileTrue(new SuperstructureShoot())//.onlyIf(() -> !superstructure.isHoodUnderTrench())) // .alongWith(new SwerveDriveAlignTurretToHub()) // .alongWith(new TurretShoot()) .whileTrue(new SuperstructureShoot().onlyIf(() -> !swerve.isUnderTrench()) .alongWith(new SwerveDriveAlignTurretToHub()) // .alongWith(new TurretShoot()) - .andThen(new WaitUntilCommand(superstructure::atTolerance)) + //.andThen(new WaitUntilCommand(superstructure::atTolerance)) .andThen(new HandoffConditionalCommand().onlyIf(superstructure::atTolerance) - .alongWith(new WaitUntilCommand(handoff::atTolerance)) - .andThen(new SpindexerConditionalCommand().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) + //.alongWith(new WaitUntilCommand(handoff::atTolerance)) + .andThen(new SpindexerConditionalCommand())))//.onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) .onFalse(new SpindexerStop() .alongWith(new SuperstructureStow()) .alongWith(new HandoffStop())); diff --git a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java index c1cd6ddf..ea022156 100644 --- a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java @@ -11,7 +11,7 @@ public class SpindexerConditionalCommand extends ConditionalCommand { public SpindexerConditionalCommand() { - super(new SpindexerStop(), new SpindexerRun(), () -> (CommandSwerveDrivetrain.getInstance().isBehindTower() || CommandSwerveDrivetrain.getInstance().isBehindHub())); + super(new SpindexerStop(), new SpindexerRun() ,() -> (CommandSwerveDrivetrain.getInstance().isBehindTower() || CommandSwerveDrivetrain.getInstance().isBehindHub())); // DEBUG SmartDashboard.putBoolean("FieldPositions/Should Spindexer Stop", (CommandSwerveDrivetrain.getInstance().isBehindTower() || CommandSwerveDrivetrain.getInstance().isBehindHub())); } diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureDefaultCommand.java index 2716ec4c..476f20af 100644 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureDefaultCommand.java @@ -17,7 +17,7 @@ public class SuperstructureDefaultCommand extends Command { private final CommandSwerveDrivetrain swerve; private final Superstructure superstructure; - // private boolean trench = false; + private boolean trench = false; public SuperstructureDefaultCommand() { swerve = CommandSwerveDrivetrain.getInstance(); @@ -29,12 +29,11 @@ public SuperstructureDefaultCommand() { @Override public void execute() { - if (swerve.isUnderTrench()) { - new SuperstructureStow(); - new SpindexerStop().alongWith(new HandoffStop()); - } + // if (swerve.isUnderTrench()) { + // new SuperstructureStow(); + // new SpindexerStop().alongWith(new HandoffStop()); + // } - // SIMON's WORK // if (swerve.isUnderTrench() && trench == false) { // CommandScheduler.getInstance().schedule(new SuperstructureStow()); // CommandScheduler.getInstance().schedule(new SpindexerStop().alongWith(new HandoffStop())); From b394971de7eee6368e19a507adc2e1d0f721f019 Mon Sep 17 00:00:00 2001 From: Lucas Date: Fri, 6 Mar 2026 17:16:10 -0500 Subject: [PATCH 091/150] FEAT: SOTM toggle ConditionalCommand --- .../java/com/stuypulse/robot/RobotContainer.java | 11 ++++++++++- .../SuperstructureSOTMConditional.java | 16 ++++++++++++++++ 2 files changed, 26 insertions(+), 1 deletion(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTMConditional.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 26612fb8..91ca9db8 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -41,6 +41,7 @@ import com.stuypulse.robot.commands.superstructure.SuperstructureLeftCorner; import com.stuypulse.robot.commands.superstructure.SuperstructureRightCorner; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTMConditional; import com.stuypulse.robot.commands.superstructure.SuperstructureShoot; import com.stuypulse.robot.commands.superstructure.SuperstructureStow; import com.stuypulse.robot.commands.swerve.SwerveDriveAlignTurretToHub; @@ -161,7 +162,7 @@ private void configureButtonBindings() { .alongWith(new SuperstructureStow()) .alongWith(new HandoffStop())); - // Scoring Routine + // Scoring Routine driver.getBottomButton() .whileTrue(new SuperstructureShoot())//.onlyIf(() -> !superstructure.isHoodUnderTrench())) // .alongWith(new SwerveDriveAlignTurretToHub()) @@ -210,6 +211,14 @@ private void configureButtonBindings() { .alongWith(new SuperstructureStow()) .alongWith(new HandoffStop())); + // SOTM Toggle + driver.getRightButton() + .onTrue(new SuperstructureSOTMConditional().onlyIf(() -> !swerve.isUnderTrench()) + .andThen(new WaitUntilCommand(superstructure::atTolerance)) + .andThen(new HandoffConditionalCommand().onlyIf(superstructure::atTolerance) + .alongWith(new WaitUntilCommand(handoff::atTolerance)) + .andThen(new SpindexerConditionalCommand().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))); + // Reset Heading driver.getDPadUp() .onTrue(new SwerveResetHeading()) diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTMConditional.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTMConditional.java new file mode 100644 index 00000000..3e604a86 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTMConditional.java @@ -0,0 +1,16 @@ +package com.stuypulse.robot.commands.superstructure; + +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; + +public class SuperstructureSOTMConditional extends ConditionalCommand{ + public SuperstructureSOTMConditional() { + super(new SuperstructureStow(), new SuperstructureSOTM(), () -> (Superstructure.getInstance().getState() == SuperstructureState.SOTM)); + } +} From ec7b04923a71490a71ddabd5c7ea7aa783210940 Mon Sep 17 00:00:00 2001 From: szhu70 Date: Fri, 6 Mar 2026 17:51:20 -0500 Subject: [PATCH 092/150] FEAT: SOTM toggle and untoggle, default command. AtTolerance is disabled because of sim issues. Co-authored-by: Lucas O --- simgui-ds.json | 5 +- .../com/stuypulse/robot/RobotContainer.java | 54 +++++++++---------- .../SuperstructureDefaultCommand.java | 25 +++------ .../SuperstructureSOTMConditional.java | 2 +- 4 files changed, 37 insertions(+), 49 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 69b1a3cb..c186fdd9 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -18,12 +18,13 @@ } ], "axisCount": 3, - "buttonCount": 4, + "buttonCount": 5, "buttonKeys": [ 90, 88, 67, - 86 + 86, + 66 ], "povConfig": [ { diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index e5c18af4..9a348a90 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -139,7 +139,7 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); - // superstructure.setDefaultCommand(new SuperstructureDefaultCommand()); + superstructure.setDefaultCommand(new SuperstructureDefaultCommand()); // climberHopper.setDefaultCommand(new ClimberHopperDefaultCommand()); // turret.setDefaultCommand(new TurretDefaultCommand()); } @@ -200,24 +200,24 @@ private void configureButtonBindings() { // .alongWith(new SuperstructureStow()) // .alongWith(new HandoffStop())); - // SOTM - driver.getRightButton() - .whileTrue(new SuperstructureSOTM().onlyIf(() -> !swerve.isUnderTrench()) - .andThen(new WaitUntilCommand(superstructure::atTolerance)) - .andThen(new HandoffConditionalCommand().onlyIf(superstructure::atTolerance) - .alongWith(new WaitUntilCommand(handoff::atTolerance)) - .andThen(new SpindexerConditionalCommand().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) - .onFalse(new SpindexerStop() - .alongWith(new SuperstructureStow()) - .alongWith(new HandoffStop())); + // // SOTM + // driver.getRightButton() + // .whileTrue(new SuperstructureSOTM().onlyIf(() -> !swerve.isUnderTrench()) + // .andThen(new WaitUntilCommand(superstructure::atTolerance)) + // .andThen(new HandoffConditionalCommand().onlyIf(superstructure::atTolerance) + // .alongWith(new WaitUntilCommand(handoff::atTolerance)) + // .andThen(new SpindexerConditionalCommand().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) + // .onFalse(new SpindexerStop() + // .alongWith(new SuperstructureStow()) + // .alongWith(new HandoffStop())); // SOTM Toggle driver.getRightButton() - .onTrue(new SuperstructureSOTMConditional().onlyIf(() -> !swerve.isUnderTrench()) - .andThen(new WaitUntilCommand(superstructure::atTolerance)) - .andThen(new HandoffConditionalCommand().onlyIf(superstructure::atTolerance) - .alongWith(new WaitUntilCommand(handoff::atTolerance)) - .andThen(new SpindexerConditionalCommand().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))); + .onTrue(new SuperstructureSOTMConditional().onlyIf(() -> !swerve.isUnderTrench())); + // .andThen(new WaitUntilCommand(superstructure::atTolerance)) + // .andThen(new HandoffConditionalCommand().onlyIf(superstructure::atTolerance) + // .alongWith(new WaitUntilCommand(handoff::atTolerance)) + // .andThen(new SpindexerConditionalCommand().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))); // Reset Heading driver.getDPadUp() @@ -262,17 +262,17 @@ private void configureButtonBindings() { ); // Right Corner Shoot - driver.getRightButton() - .whileTrue( - new SwerveXMode().alongWith( - new SuperstructureRightCorner().alongWith( - new WaitUntilCommand(() -> superstructure.atTolerance()).andThen( - new SpindexerRun().alongWith(new HandoffRun()))))) - .onFalse( - new SuperstructureStow().alongWith( - new SpindexerRun().alongWith( - new HandoffStop())) - ); + // driver.getRightButton() + // .whileTrue( + // new SwerveXMode().alongWith( + // new SuperstructureRightCorner().alongWith( + // new WaitUntilCommand(() -> superstructure.atTolerance()).andThen( + // new SpindexerRun().alongWith(new HandoffRun()))))) + // .onFalse( + // new SuperstructureStow().alongWith( + // new SpindexerRun().alongWith( + // new HandoffStop())) + // ); // Hub Shoot // driver.getBottomButton() diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureDefaultCommand.java index 476f20af..150b8ade 100644 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureDefaultCommand.java @@ -28,25 +28,12 @@ public SuperstructureDefaultCommand() { @Override public void execute() { + if (swerve.isUnderTrench() && trench == false) { + CommandScheduler.getInstance().schedule(new SuperstructureStow()); + trench = true; + } else { + trench = false; + } - // if (swerve.isUnderTrench()) { - // new SuperstructureStow(); - // new SpindexerStop().alongWith(new HandoffStop()); - // } - - // if (swerve.isUnderTrench() && trench == false) { - // CommandScheduler.getInstance().schedule(new SuperstructureStow()); - // CommandScheduler.getInstance().schedule(new SpindexerStop().alongWith(new HandoffStop())); - // trench = true; - // } else { - // trench = false; - // } - - // TODO: WHAT REASON FOR THIS? - // if (swerve.isInOpponentZone()) { - // new SpindexerStop().alongWith(new HandoffStop()) - // .andThen(new WaitUntilCommand(superstructure::atTolerance)) - // .andThen(new HandoffRun()).alongWith(new SpindexerRun()); - // } } } diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTMConditional.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTMConditional.java index 3e604a86..62d90518 100644 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTMConditional.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTMConditional.java @@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.ConditionalCommand; -public class SuperstructureSOTMConditional extends ConditionalCommand{ +public class SuperstructureSOTMConditional extends ConditionalCommand { public SuperstructureSOTMConditional() { super(new SuperstructureStow(), new SuperstructureSOTM(), () -> (Superstructure.getInstance().getState() == SuperstructureState.SOTM)); } From 34f1e8d1d3d62b40db3491b6c188eb0364dcee97 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 6 Mar 2026 20:29:32 -0500 Subject: [PATCH 093/150] SCORING WORKS!! --- simgui-ds.json | 3 +- simgui.json | 15 ++- .../com/stuypulse/robot/RobotContainer.java | 123 ++++++++++-------- .../commands/intake/ZeroPivotDeployed.java | 2 + .../commands/intake/ZeroPivotStowed.java | 4 +- .../SuperstructureSetState.java | 9 +- .../robot/commands/turret/TurretSetState.java | 4 +- .../stuypulse/robot/constants/Cameras.java | 18 +-- .../com/stuypulse/robot/constants/Gains.java | 22 ++-- .../stuypulse/robot/constants/Settings.java | 24 ++-- .../robot/subsystems/intake/IntakeImpl.java | 103 ++++++++------- .../subsystems/superstructure/hood/Hood.java | 8 +- .../superstructure/turret/TurretImpl.java | 42 ++++-- .../subsystems/vision/LimelightVision.java | 30 ++--- 14 files changed, 241 insertions(+), 166 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 69b1a3cb..c4b7efd3 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -91,7 +91,8 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0" + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ] } diff --git a/simgui.json b/simgui.json index 84fb2ea6..df359ee1 100644 --- a/simgui.json +++ b/simgui.json @@ -21,6 +21,7 @@ "/SmartDashboard/Robot/Zero Pivot Encoder at Lower Limit (Deployed)": "Command", "/SmartDashboard/Robot/Zero Pivot Encoder at Upper Limit (Stowed)": "Command", "/SmartDashboard/Robot/Zero Turret Encoders": "Command", + "/SmartDashboard/Scheduler": "Scheduler", "/SmartDashboard/Visualizers/ClimberHopper": "Mechanism2d", "/SmartDashboard/Visualizers/Hood": "Mechanism2d", "/SmartDashboard/Visualizers/Turret": "Mechanism2d" @@ -63,6 +64,9 @@ }, "transitory": { "SmartDashboard": { + "Enabled Subsystems": { + "open": true + }, "FieldPositions": { "open": true }, @@ -72,13 +76,22 @@ }, "open": true }, - "States": { + "Superstructure": { + "Hood": { + "open": true + }, + "Turret": { + "open": true + }, "open": true }, "Turret": { "open": true }, "open": true + }, + "limelight-right": { + "open": true } } }, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index a1a6a822..92fb7a97 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -49,6 +49,7 @@ import com.stuypulse.robot.commands.swerve.SwerveXMode; import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.commands.turret.TurretDefaultCommand; +import com.stuypulse.robot.commands.turret.TurretIdle; import com.stuypulse.robot.commands.turret.TurretShoot; import com.stuypulse.robot.commands.turret.ZeroTurret; import com.stuypulse.robot.commands.vision.ResetLimelightIMU; @@ -56,6 +57,7 @@ import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.subsystems.handoff.Handoff; +import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.superstructure.hood.Hood; import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter; @@ -80,14 +82,17 @@ public class RobotContainer { public interface EnabledSubsystems { SmartBoolean SWERVE = new SmartBoolean("Enabled Subsystems/Swerve Is Enabled", true); - SmartBoolean TURRET = new SmartBoolean("Enabled Subsystems/Turret Is Enabled", false); + SmartBoolean TURRET = new SmartBoolean("Enabled Subsystems/Turret Is Enabled", true); SmartBoolean HANDOFF = new SmartBoolean("Enabled Subsystems/Handoff Is Enabled", true); SmartBoolean INTAKE = new SmartBoolean("Enabled Subsystems/Intake Is Enabled", true); SmartBoolean SPINDEXER = new SmartBoolean("Enabled Subsystems/Spindexer Is Enabled", true); SmartBoolean CLIMBER_HOPPER = new SmartBoolean("Enabled Subsystems/Climber-Hopper Is Enabled", false); SmartBoolean HOOD = new SmartBoolean("Enabled Subsystems/Hood Is Enabled", true); SmartBoolean SHOOTER = new SmartBoolean("Enabled Subsystems/Shooter Is Enabled", true); - SmartBoolean LIMELIGHT = new SmartBoolean("Enabled Subsystems/Limelight Is Enabled", true); + + SmartBoolean BACK_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Back Limelight Is Enabled", false); + SmartBoolean LEFT_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Left Limelight Is Enabled", false); + SmartBoolean RIGHT_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Right Limelight Is Enabled", true); } // Gamepads @@ -136,7 +141,7 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); - superstructure.setDefaultCommand(new SuperstructureDefaultCommand()); + // superstructure.setDefaultCommand(new SuperstructureDefaultCommand()); // climberHopper.setDefaultCommand(new ClimberHopperDefaultCommand()); // turret.setDefaultCommand(new TurretDefaultCommand()); } @@ -152,28 +157,34 @@ private void configureButtonBindings() { // .alongWith(new SwerveDriveAlignTurretToHub()) // .alongWith(new TurretShoot()) .andThen(new WaitUntilCommand(superstructure::atTolerance)) - .andThen(new HandoffRun().onlyIf(superstructure::atTolerance) - .alongWith(new WaitUntilCommand(handoff::atTolerance)) - .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) + .andThen(new HandoffRun()) + // .alongWith(new WaitUntilCommand(handoff::atTolerance)) + .andThen(new WaitUntilCommand(handoff::atTolerance)) + .andThen(new SpindexerRun())) .onFalse(new SpindexerStop() .alongWith(new SuperstructureStow()) .alongWith(new HandoffStop())); - // Scoring Routine - driver.getBottomButton() - .whileTrue(new SuperstructureShoot())//.onlyIf(() -> !superstructure.isHoodUnderTrench())) - // .alongWith(new SwerveDriveAlignTurretToHub()) - // .alongWith(new TurretShoot()) - .whileTrue(new SuperstructureShoot().onlyIf(() -> !swerve.isUnderTrench()) - .alongWith(new SwerveDriveAlignTurretToHub()) - // .alongWith(new TurretShoot()) - .andThen(new WaitUntilCommand(superstructure::atTolerance)) - .andThen(new HandoffConditionalCommand().onlyIf(superstructure::atTolerance) - .alongWith(new WaitUntilCommand(handoff::atTolerance)) - .andThen(new SpindexerConditionalCommand().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) - .onFalse(new SpindexerStop() - .alongWith(new SuperstructureStow()) - .alongWith(new HandoffStop())); + // Test Turret + // driver.getBottomButton() + // .whileTrue(new TurretShoot()) + // .onFalse(new TurretIdle()); + + // Scoring Routine + // driver.getBottomButton() + // .whileTrue(new SuperstructureShoot())//.onlyIf(() -> !superstructure.isHoodUnderTrench())) + // // .alongWith(new SwerveDriveAlignTurretToHub()) + // // .alongWith(new TurretShoot()) + // .whileTrue(new SuperstructureShoot().onlyIf(() -> !swerve.isUnderTrench()) + // .alongWith(new SwerveDriveAlignTurretToHub()) + // // .alongWith(new TurretShoot()) + // .andThen(new WaitUntilCommand(superstructure::atTolerance)) + // .andThen(new HandoffConditionalCommand().onlyIf(superstructure::atTolerance) + // .alongWith(new WaitUntilCommand(handoff::atTolerance)) + // .andThen(new SpindexerConditionalCommand().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) + // .onFalse(new SpindexerStop() + // .alongWith(new SuperstructureStow()) + // .alongWith(new HandoffStop())); // Intake Deploy driver.getRightTriggerButton() @@ -198,15 +209,15 @@ private void configureButtonBindings() { // .alongWith(new HandoffStop())); // SOTM - driver.getRightButton() - .whileTrue(new SuperstructureSOTM().onlyIf(() -> !swerve.isUnderTrench()) - .andThen(new WaitUntilCommand(superstructure::atTolerance)) - .andThen(new HandoffConditionalCommand().onlyIf(superstructure::atTolerance) - .alongWith(new WaitUntilCommand(handoff::atTolerance)) - .andThen(new SpindexerConditionalCommand().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) - .onFalse(new SpindexerStop() - .alongWith(new SuperstructureStow()) - .alongWith(new HandoffStop())); + // driver.getRightButton() + // .whileTrue(new SuperstructureSOTM().onlyIf(() -> !swerve.isUnderTrench()) + // .andThen(new WaitUntilCommand(superstructure::atTolerance)) + // .andThen(new HandoffConditionalCommand().onlyIf(superstructure::atTolerance) + // .alongWith(new WaitUntilCommand(handoff::atTolerance)) + // .andThen(new SpindexerConditionalCommand().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) + // .onFalse(new SpindexerStop() + // .alongWith(new SuperstructureStow()) + // .alongWith(new HandoffStop())); // Reset Heading driver.getDPadUp() @@ -215,8 +226,8 @@ private void configureButtonBindings() { .onFalse(new SetIMUMode(0)); // Climb Align - driver.getTopButton() - .whileTrue(new SwerveClimbAlign().alongWith(new ClimberUp()).andThen(new SwerveXMode())); + // driver.getTopButton() + // .whileTrue(new SwerveClimbAlign().alongWith(new ClimberUp()).andThen(new SwerveXMode())); // Climber Up driver.getRightBumper() @@ -226,31 +237,31 @@ private void configureButtonBindings() { driver.getLeftBumper() .onTrue(new ClimberDown()); - // Left Corner Shoot - driver.getLeftButton() - .whileTrue( - new SwerveXMode().alongWith( - new SuperstructureLeftCorner().alongWith( - new WaitUntilCommand(() -> superstructure.atTolerance())).andThen( - new SpindexerRun().alongWith(new HandoffRun())))) - .onFalse( - new SuperstructureStow().alongWith( - new SpindexerRun().alongWith( - new HandoffStop())) - ); + // // Left Corner Shoot + // driver.getLeftButton() + // .whileTrue( + // new SwerveXMode().alongWith( + // new SuperstructureLeftCorner().alongWith( + // new WaitUntilCommand(() -> superstructure.atTolerance())).andThen( + // new SpindexerRun().alongWith(new HandoffRun())))) + // .onFalse( + // new SuperstructureStow().alongWith( + // new SpindexerRun().alongWith( + // new HandoffStop())) + // ); - // Right Corner Shoot - driver.getRightButton() - .whileTrue( - new SwerveXMode().alongWith( - new SuperstructureRightCorner().alongWith( - new WaitUntilCommand(() -> superstructure.atTolerance()).andThen( - new SpindexerRun().alongWith(new HandoffRun()))))) - .onFalse( - new SuperstructureStow().alongWith( - new SpindexerRun().alongWith( - new HandoffStop())) - ); + // // Right Corner Shoot + // driver.getRightButton() + // .whileTrue( + // new SwerveXMode().alongWith( + // new SuperstructureRightCorner().alongWith( + // new WaitUntilCommand(() -> superstructure.atTolerance()).andThen( + // new SpindexerRun().alongWith(new HandoffRun()))))) + // .onFalse( + // new SuperstructureStow().alongWith( + // new SpindexerRun().alongWith( + // new HandoffStop())) + // ); // Hub Shoot // driver.getBottomButton() diff --git a/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotDeployed.java b/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotDeployed.java index 40015c04..e0902543 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotDeployed.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotDeployed.java @@ -1,6 +1,7 @@ package com.stuypulse.robot.commands.intake; import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.intake.Intake.PivotState; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -14,5 +15,6 @@ public ZeroPivotDeployed() { @Override public void initialize() { intake.zeroPivotDeployed(); + intake.setPivotState(PivotState.DEPLOY); } } diff --git a/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotStowed.java b/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotStowed.java index 71e2c559..d39698d4 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotStowed.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotStowed.java @@ -1,6 +1,7 @@ package com.stuypulse.robot.commands.intake; import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.intake.Intake.PivotState; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -13,6 +14,7 @@ public ZeroPivotStowed() { @Override public void initialize() { - intake.zeroPivotDeployed(); + intake.zeroPivotStowed(); + intake.setPivotState(PivotState.STOW); } } diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSetState.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSetState.java index 35d44533..96e68c3f 100644 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSetState.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSetState.java @@ -8,9 +8,9 @@ import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Command; -public class SuperstructureSetState extends InstantCommand { +public class SuperstructureSetState extends Command { private final Superstructure superstructure; private final SuperstructureState state; @@ -25,4 +25,9 @@ public SuperstructureSetState(SuperstructureState state) { public void execute() { superstructure.setState(state); } + + @Override + public boolean isFinished() { + return superstructure.getState() == state; + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretSetState.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretSetState.java index 62bf7ddf..09bbb1b1 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretSetState.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretSetState.java @@ -9,9 +9,9 @@ import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; import edu.wpi.first.wpilibj2.command.Command; -// import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; -public class TurretSetState extends Command { +public class TurretSetState extends InstantCommand { private final Turret turret; private final TurretState state; diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 5c5083f2..0bc1fa5d 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -17,18 +17,20 @@ public interface Cameras { public Camera[] LimelightCameras = { - new Camera("limelight-climber", + new Camera("limelight-right", new Pose3d(-0.233, 0.375959, 0.20368, + // new Pose3d(0.0,0.0,0.0, new Rotation3d(Units.degreesToRadians(180), Units.degreesToRadians(31.08), Units.degreesToRadians(-80))), - RobotContainer.EnabledSubsystems.LIMELIGHT), - new Camera("limelight-shooter", - new Pose3d(Units.inchesToMeters(2.490), Units.inchesToMeters(14.8620), Units.inchesToMeters(5.920157), + RobotContainer.EnabledSubsystems.RIGHT_LIMELIGHT), + new Camera("limelight-left", + new Pose3d(Units.inchesToMeters(2.490), Units.inchesToMeters(14.8620), Units.inchesToMeters(7.920157), + // new Pose3d(0.0,0.0,0.0, new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15.0), Units.degreesToRadians(251.0))), - RobotContainer.EnabledSubsystems.LIMELIGHT), - new Camera("limelight-module", //11.0845u - new Pose3d(Units.inchesToMeters(-10.768823), Units.inchesToMeters(12.717519), Units.inchesToMeters(8.331714), + RobotContainer.EnabledSubsystems.LEFT_LIMELIGHT), + new Camera("limelight-back", //11.0845u + new Pose3d(Units.inchesToMeters(-10.768823), Units.inchesToMeters(-12.717519), Units.inchesToMeters(8.331714), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(31.0), Units.degreesToRadians(185.0))), - RobotContainer.EnabledSubsystems.LIMELIGHT)// 31 // 26.965 + RobotContainer.EnabledSubsystems.BACK_LIMELIGHT)// 31 // 26.965 //THIS ROTATION IS NOT EXACTLY 180 DEGREES... }; diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 8e20894e..69fcf42c 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -60,23 +60,23 @@ public interface Hood { public interface Turret { public interface slot0 { - double kP = 1300.0; + double kP = 200.0; double kI = 0.0; - double kD = 140.0; - - double kS = 0.23; // FOUND ON 2/25 PD 8 + double kD = 0.0; + + double kS = 0.4775; double kV = 0.0; double kA = 0.0; } public interface slot1 { - double kP = 0.0; - double kI = 0.0; - double kD = 0.0; - - double kS = 0.0; - double kV = 0.0; - double kA = 0.0; + SmartNumber kP = new SmartNumber("Superstructure/Turret/Gains/kP", 20.0); + SmartNumber kI = new SmartNumber("Superstructure/Turret/Gains/kI", 0.0); + SmartNumber kD = new SmartNumber("Superstructure/Turret/Gains/kD", 0.0); + + SmartNumber kS = new SmartNumber("Superstructure/Turret/Gains/kS", 0.4775); + SmartNumber kV = new SmartNumber("Superstructure/Turret/Gains/kV", 0.0); + SmartNumber kA = new SmartNumber("Superstructure/Turret/Gains/kA", 0.0); } } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index cfc5d445..77400d8b 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -49,7 +49,8 @@ public interface ClimberHopper { public final double MASS_KG = 1.0; // public final double NUM_ROTATIONS_TO_REACH_TOP = MAX_ROTATIONS - MIN_ROTATIONS; - public final double NUM_ROTATIONS_TO_REACH_TOP = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (0.480 / 13.0); // TODO: verify this + // public final double NUM_ROTATIONS_TO_REACH_TOP = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (0.480 / 13.0); // TODO: verify this + public final double NUM_ROTATIONS_TO_REACH_TOP = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / Units.inchesToMeters(2 * Math.PI * 0.75); // TODO: verify this 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; @@ -96,7 +97,7 @@ public interface Intake { Rotation2d PIVOT_STOW_ANGLE = Rotation2d.fromDegrees(90.0); Rotation2d PIVOT_DEPLOY_ANGLE = Rotation2d.fromDegrees(0.0); - Rotation2d PIVOT_ANGLE_TOLERANCE = Rotation2d.fromDegrees(15.0); + Rotation2d PIVOT_ANGLE_TOLERANCE = Rotation2d.fromDegrees(5.0); Rotation2d PIVOT_MAX_ANGLE = Rotation2d.fromDegrees(90.0); Rotation2d PIVOT_MIN_ANGLE = Rotation2d.fromDegrees(0.0); @@ -107,7 +108,7 @@ public interface Intake { Rotation2d PIVOT_MAX_VEL_STOW = Rotation2d.fromDegrees(360.0); Rotation2d PIVOT_MAX_ACCEL_STOW = Rotation2d.fromDegrees(600.0); - Rotation2d THRESHHOLD_TO_START_ROLLERS = Rotation2d.fromDegrees(10.0); + Rotation2d THRESHHOLD_TO_START_ROLLERS = Rotation2d.fromDegrees(5.0); Rotation2d ARBITRARY_VOLTAGE_THRESHOLD = Rotation2d.fromDegrees(15.0); @@ -116,6 +117,7 @@ public interface Intake { double GEAR_RATIO = 37.93; double debugVoltage = 0; //TODO: set value + SmartNumber debugDutyCycle = new SmartNumber("Intake/Debug Duty Cycle", 0.1); double STALL_CURRENT_LIMIT = 0; //TODO: set value double STALL_DEBOUNCE = 1.0; //TODO: VERIFY } @@ -284,7 +286,7 @@ public interface Angles { 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 Rotation2d TOLERANCE = Rotation2d.fromDegrees(2.0); + public final Rotation2d TOLERANCE = Rotation2d.fromDegrees(5.0); // TODO: reduce to 2 degrees public final Rotation2d KB = Rotation2d.fromDegrees(0.0); public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(0.0); @@ -296,10 +298,12 @@ public interface Turret { Rotation2d MIN_THEORETICAL_ROTATION = Rotation2d.fromDegrees(-612); /* CONSTANTS */ - public final double RANGE_LEFT = -45; - public final double RANGE_RIGHT = 390; + public final double RANGE_LEFT = -360.0; // -120 + public final double RANGE_RIGHT = 85.0; //390; // 410 + + // 85, -360 - public final double SLOT_SWITCHING_THRESHOLD_ROT = .5; + public final Rotation2d GAIN_SWITCHING_THRESHOLD = Rotation2d.fromDegrees(30); public final Transform2d TURRET_OFFSET = new Transform2d(Units.inchesToMeters(-4.0), Units.inchesToMeters(8.0), Rotation2d.kZero); public final double TURRET_HEIGHT = Units.inchesToMeters(0.0); @@ -312,12 +316,12 @@ public interface BigGear { public interface Encoder17t { public final int TEETH = 17; - public final Rotation2d OFFSET = Rotation2d.fromRotations(-0.279541015625);//(-0.86962890625); //0.6787109375 + public final Rotation2d OFFSET = Rotation2d.fromRotations(-0.716);//(-0.28125);//.fromRotations(-0.279541015625);//(-0.86962890625); //0.6787109375 } public interface Encoder18t { public final int TEETH = 18; - public final Rotation2d OFFSET = Rotation2d.fromRotations(-0.42822265625);//(-0.700927734375); //0.53564453125 + public final Rotation2d OFFSET = Rotation2d.fromRotations(-0.559);//(-0.442);//(0.58203125);//.fromRotations(-0.42822265625);//(-0.700927734375); //0.53564453125 } public interface SoftwareLimit { @@ -341,7 +345,7 @@ public interface Swerve { public interface Constraints { public final double MAX_VELOCITY_M_PER_S = 4.93; // 4.3p public final double MAX_ACCEL_M_PER_S_SQUARED = 15.0; - public final double MAX_ANGULAR_VEL_RAD_PER_S = Units.degreesToRadians(600.0); + public final double MAX_ANGULAR_VEL_RAD_PER_S = Units.degreesToRadians(400.0); public final double MAX_ANGULAR_ACCEL_RAD_PER_S_SQUARED = Units.degreesToRadians(900.0); public final PathConstraints DEFAULT_CONSTRAINTS = 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 8d1fb367..b85b1ee3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -54,28 +54,30 @@ public class IntakeImpl extends Intake { public IntakeImpl() { pivotConfig = new Motors.TalonFXConfig() - .withInvertedValue(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake) - - .withSupplyCurrentLimitAmps(60) - .withStatorCurrentLimitEnabled(false) - .withRampRate(0.25) - - .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) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign, 0) - .withGravityType(GravityTypeValue.Arm_Cosine) - .withMotionProfile(Settings.Intake.PIVOT_MAX_VEL_STOW.getRotations(), Settings.Intake.PIVOT_MAX_ACCEL_STOW.getRotations()) - - .withSensorToMechanismRatio(Settings.Intake.GEAR_RATIO); + .withInvertedValue(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) + + .withSupplyCurrentLimitAmps(60) + .withStatorCurrentLimitEnabled(false) + .withRampRate(0.25) + + .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) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign, 0) + .withGravityType(GravityTypeValue.Arm_Cosine) + .withMotionProfile(Settings.Intake.PIVOT_MAX_VEL_STOW.getRotations(), + Settings.Intake.PIVOT_MAX_ACCEL_STOW.getRotations()) + + .withSensorToMechanismRatio(Settings.Intake.GEAR_RATIO); rollerConfig = new Motors.TalonFXConfig() - .withInvertedValue(InvertedValue.CounterClockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake) + .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) - .withSupplyCurrentLimitAmps(45) - .withStatorCurrentLimitEnabled(false) - .withRampRate(0.50); + .withSupplyCurrentLimitAmps(45) + .withStatorCurrentLimitEnabled(false) + .withRampRate(0.50); pivot = new TalonFX(Ports.Intake.PIVOT, Ports.RIO); pivotConfig.configure(pivot); @@ -100,12 +102,12 @@ public IntakeImpl() { pivot.setPosition(Settings.Intake.PIVOT_MAX_ANGLE.getRotations()); pivotStalling = BStream.create( - () -> Math.abs(pivot.getSupplyCurrent().getValueAsDouble()) > Settings.Intake.STALL_CURRENT_LIMIT) - .filtered(new BDebounce.Both(Settings.Intake.STALL_DEBOUNCE)); + () -> Math.abs(pivot.getSupplyCurrent().getValueAsDouble()) > Settings.Intake.STALL_CURRENT_LIMIT) + .filtered(new BDebounce.Both(Settings.Intake.STALL_DEBOUNCE)); } - @Override - public boolean pivotStalling() { + @Override + public boolean pivotStalling() { return pivotStalling.get(); } @@ -126,7 +128,7 @@ private void setMotionProfileConstraints(Rotation2d velLimit, Rotation2d accelLi pivotConfig.withMotionProfile(velLimit.getRotations(), accelLimit.getRotations()); pivotConfig.configure(pivot); } - + @Override public void setPivotState(PivotState pivotState) { super.setPivotState(pivotState); @@ -148,7 +150,7 @@ public void zeroPivotStowed() { @Override public void zeroPivotDeployed() { pivot.setPosition(Settings.Intake.PIVOT_MIN_ANGLE.getRotations()); - } + } @Override public void periodic() { @@ -159,14 +161,16 @@ public void periodic() { pivot.setVoltage(pivotVoltageOverride.get()); } else { // PIVOT - if (getPivotState() == PivotState.DEPLOY && getPivotAngle().getDegrees() <= Settings.Intake.ARBITRARY_VOLTAGE_THRESHOLD.getDegrees()) { - pivot.setControl(new VoltageOut(-Settings.Intake.PUSHDOWN_VOLTAGE)); // applying 3 volts + if (getPivotState() == PivotState.DEPLOY + && getPivotAngle().getDegrees() <= Settings.Intake.ARBITRARY_VOLTAGE_THRESHOLD.getDegrees()) { + pivot.setControl(new VoltageOut(-Settings.Intake.PUSHDOWN_VOLTAGE)); // applying 3 volts } else { pivot.setControl(new PositionVoltage(getPivotState().getTargetAngle().getRotations())); } // ROLLERS - if (getPivotAngle().getDegrees() <= Settings.Intake.THRESHHOLD_TO_START_ROLLERS.getDegrees()) { + if (getPivotState() == PivotState.DEPLOY + && getPivotAngle().getDegrees() <= Settings.Intake.THRESHHOLD_TO_START_ROLLERS.getDegrees()) { rollerLeader.setControl(rollerController.withOutput(getRollerState().getTargetDutyCycle())); } else { rollerLeader.stopMotor(); @@ -192,24 +196,33 @@ public void periodic() { SmartDashboard.putNumber("Intake/Pivot Max Velocity Limit (deg/s)", velLimit.get()); SmartDashboard.putNumber("Intake/Pivot Max Accel Limit (deg/s^2)", accelLimit.get()); - SmartDashboard.putNumber("Intake/Pivot Angle Error (deg)", Math.abs(getPivotState().getTargetAngle().getDegrees() - getPivotAngle().getDegrees())); + SmartDashboard.putNumber("Intake/Pivot Angle Error (deg)", + Math.abs(getPivotState().getTargetAngle().getDegrees() - getPivotAngle().getDegrees())); - SmartDashboard.putNumber("Intake/Pivot Closed Loop Error (deg)", pivot.getClosedLoopError().getValueAsDouble() * 360.0); + SmartDashboard.putNumber("Intake/Pivot Closed Loop Error (deg)", + pivot.getClosedLoopError().getValueAsDouble() * 360.0); // ROLLERS - SmartDashboard.putNumber("Intake/Roller Leader Voltage (volts)", rollerLeader.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("Intake/Roller Follower Voltage (volts)", rollerFollower.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Roller Leader Voltage (volts)", + rollerLeader.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Roller Follower Voltage (volts)", + rollerFollower.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("Intake/Roller Leader Current (amps)", rollerLeader.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("Intake/Roller Follower Current (amps)", rollerFollower.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Roller Leader Current (amps)", + rollerLeader.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Roller Follower Current (amps)", + rollerFollower.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Intake/Pivot Temperature (C)", pivot.getDeviceTemp().getValueAsDouble()); SmartDashboard.putNumber("Intake/Leader Temperature (C)", rollerLeader.getDeviceTemp().getValueAsDouble()); - SmartDashboard.putNumber("Intake/Follower Temperature (C)", rollerFollower.getDeviceTemp().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Follower Temperature (C)", + rollerFollower.getDeviceTemp().getValueAsDouble()); SmartDashboard.putNumber("Current Draws/Intake Pivot (amps)", pivot.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("Current Draws/Intake Roller Leader (amps)", rollerLeader.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("Current Draws/Intake Roller Follower (amps)", rollerFollower.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Current Draws/Intake Roller Leader (amps)", + rollerLeader.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Current Draws/Intake Roller Follower (amps)", + rollerFollower.getSupplyCurrent().getValueAsDouble()); } } @@ -221,13 +234,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()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java index 98254462..852a66b7 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java @@ -8,6 +8,7 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.superstructure.SOTMCalculator; import com.stuypulse.robot.util.superstructure.VisualizerHood; import com.stuypulse.robot.util.superstructure.InterpolationCalculator; @@ -63,10 +64,9 @@ public void setState(HoodState state){ } public Rotation2d getTargetAngle() { - // if (isUnderTrench()) { - // return Settings.Superstructure.Hood.Angles.STOW; - // } - // don't think we need this when we have superstructure default command. + if (CommandSwerveDrivetrain.getInstance().isUnderTrench()) { + return Settings.Superstructure.Hood.Angles.STOW; + } return switch(state) { case STOW -> Settings.Superstructure.Hood.Angles.STOW; diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index 8db19809..6a4069c9 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -47,14 +47,14 @@ public TurretImpl() { .withSupplyCurrentLimitAmps(80) .withStatorCurrentLimitEnabled(false) .withRampRate(0.25) - .withVoltageLimits(6, -6) //TODO: VERIFY MAX VOLTAGE + // .withVoltageLimits(6, -6) //TODO: VERIFY MAX VOLTAGE .withPIDConstants(Gains.Superstructure.Turret.slot0.kP, Gains.Superstructure.Turret.slot0.kI, Gains.Superstructure.Turret.slot0.kD, 0) .withFFConstants(Gains.Superstructure.Turret.slot0.kS, Gains.Superstructure.Turret.slot0.kV, Gains.Superstructure.Turret.slot0.kA, 0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 0) - .withPIDConstants(Gains.Superstructure.Turret.slot1.kP, Gains.Superstructure.Turret.slot1.kI, Gains.Superstructure.Turret.slot1.kD, 1) - .withFFConstants(Gains.Superstructure.Turret.slot1.kS, Gains.Superstructure.Turret.slot1.kV, Gains.Superstructure.Turret.slot1.kA, 1) + .withPIDConstants(Gains.Superstructure.Turret.slot1.kP.get(), Gains.Superstructure.Turret.slot1.kI.get(), Gains.Superstructure.Turret.slot1.kD.get(), 1) + .withFFConstants(Gains.Superstructure.Turret.slot1.kS.get(), Gains.Superstructure.Turret.slot1.kV.get(), Gains.Superstructure.Turret.slot1.kA.get(), 1) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 1) .withSensorToMechanismRatio(Settings.Superstructure.Turret.GEAR_RATIO_MOTOR_TO_MECH) @@ -65,11 +65,13 @@ public TurretImpl() { Settings.Superstructure.Turret.SoftwareLimit.BACKWARDS_MAX_ROTATIONS); encoder17tConfig = new Motors.CANCoderConfig() - .withSensorDirection(SensorDirectionValue.Clockwise_Positive) + .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) + .withMagnetOffset(Settings.Superstructure.Turret.Encoder17t.OFFSET.getRotations()) .withAbsoluteSensorDiscontinuityPoint(1.0); encoder18tConfig = new Motors.CANCoderConfig() - .withSensorDirection(SensorDirectionValue.Clockwise_Positive) + .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) + .withMagnetOffset(Settings.Superstructure.Turret.Encoder18t.OFFSET.getRotations()) .withAbsoluteSensorDiscontinuityPoint(1.0); motor = new TalonFX(Ports.Superstructure.Turret.MOTOR, Ports.RIO); @@ -118,8 +120,8 @@ public void zeroEncoders() { } public void seedTurret() { - motor.setPosition(0); //TODO: SEED USING CRT INSTEAD OF TS, TS IS TEMP - // motor.setPosition(getVectorSpaceAngle().getRotations()); + // motor.setPosition(0); //TODO: SEED USING CRT INSTEAD OF TS, TS IS TEMP + motor.setPosition(getVectorSpaceAngle().getRotations()); } @Override @@ -129,7 +131,7 @@ public Rotation2d getAngle() { @Override public boolean atTolerance() { - double error = getAngle().minus(getTargetAngle()).getRotations() + 0.5; + double error = getAngle().minus(getTargetAngle()).getRotations(); return Math.abs(error) < Settings.Superstructure.Turret.TOLERANCE.getRotations(); } @@ -143,13 +145,22 @@ private double getDelta(double target, double current) { if (current + delta < Settings.Superstructure.Turret.RANGE_LEFT) return delta + 360; if (current + delta > Settings.Superstructure.Turret.RANGE_RIGHT) return delta - 360; - return delta < 0 ? delta + 360 : delta - 360; + // return delta < 0 ? delta + 360 : delta - 360; + return delta; } @Override public void periodic() { super.periodic(); + turretConfig.updateGainsConfig(motor, 1, + Gains.Superstructure.Turret.slot1.kP, + Gains.Superstructure.Turret.slot1.kI, + Gains.Superstructure.Turret.slot1.kD, + Gains.Superstructure.Turret.slot1.kS, + Gains.Superstructure.Turret.slot1.kV, + Gains.Superstructure.Turret.slot1.kA); + if (!hasUsedAbsoluteEncoder) { seedTurret(); hasUsedAbsoluteEncoder = true; @@ -159,17 +170,26 @@ public void periodic() { double currentAngle = getAngle().getDegrees(); double actualTargetDeg = currentAngle + getDelta(getTargetAngle().getDegrees(), currentAngle); + boolean isWrapping = Math.abs(actualTargetDeg - currentAngle) > + Settings.Superstructure.Turret.GAIN_SWITCHING_THRESHOLD.getDegrees(); + int slot = 0; + + if(isWrapping) { + slot = 1; + } + if (EnabledSubsystems.TURRET.get()) { if (voltageOverride.isPresent()) { motor.setVoltage(voltageOverride.get()); } else { - motor.setControl(controller.withPosition(actualTargetDeg / 360.0).withSlot(0)); + motor.setControl(controller.withPosition(actualTargetDeg / 360.0).withSlot(slot)); } } else { motor.stopMotor(); } if (Settings.DEBUG_MODE) { + SmartDashboard.putNumber("Superstructure/Turret/Relative Encoder Position (Rot)", motor.getPosition().getValueAsDouble() * 360.0); SmartDashboard.putNumber("Superstructure/Turret/Closed Loop Error (deg)", motor.getClosedLoopError().getValueAsDouble() * 360.0); @@ -181,6 +201,8 @@ public void periodic() { SmartDashboard.putNumber("Superstructure/Turret/Stator Current", motor.getStatorCurrent().getValueAsDouble()); SmartDashboard.putNumber("Superstructure/Turret/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Turret/Wrapped Target Angle (deg)", actualTargetDeg); + SmartDashboard.putNumber("Current Draws/Turret (amps)", motor.getSupplyCurrent().getValueAsDouble()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 57d04170..8bd27e30 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -34,7 +34,7 @@ public static LimelightVision getInstance() { private String[] names; private SmartBoolean enabled; - private SmartBoolean[] camerasEnabled; + // private SmartBoolean[] camerasEnabled; private MegaTagMode megaTagMode; public enum MegaTagMode { @@ -58,13 +58,13 @@ public LimelightVision() { ); } - camerasEnabled = new SmartBoolean[Cameras.LimelightCameras.length]; + // camerasEnabled = new SmartBoolean[Cameras.LimelightCameras.length]; - for (int i = 0; i < camerasEnabled.length; i++) { - camerasEnabled[i] = new SmartBoolean("Vision/" + names[i] + " Is Enabled", true); - LimelightHelpers.SetIMUMode(names[i], Settings.Vision.INTERNAL_EXTERNAL_ASSIST_INDEX); - SmartDashboard.putBoolean("Vision/" + names[i] + " Has Data", false); - } + // for (int i = 0; i < camerasEnabled.length; i++) { + // camerasEnabled[i] = new SmartBoolean("Vision/" + names[i] + " Is Enabled", true); + // LimelightHelpers.SetIMUMode(names[i], Settings.Vision.INTERNAL_EXTERNAL_ASSIST_INDEX); + // SmartDashboard.putBoolean("Vision/" + names[i] + " Has Data", false); + // } enabled = new SmartBoolean("Vision/Is Enabled", true); megaTagMode = MegaTagMode.MEGATAG1; @@ -84,13 +84,13 @@ public void disable() { enabled.set(false); } - public void setCameraEnabled(String name, boolean enabled) { - for (int i = 0; i < names.length; i++) { - if (names[i].equals(name)) { - camerasEnabled[i].set(enabled); - } - } - } + // public void setCameraEnabled(String name, boolean enabled) { + // for (int i = 0; i < names.length; i++) { + // if (names[i].equals(name)) { + // camerasEnabled[i].set(enabled); + // } + // } + // } public void setMegaTagMode(MegaTagMode mode) { this.megaTagMode = mode; @@ -130,7 +130,7 @@ public IMUData[] getIMUData() { public void periodic() { if (enabled.get()) { for (int i = 0; i < names.length; i++) { - if (camerasEnabled[i].get()) { + if (Cameras.LimelightCameras[i].isEnabled()) { String limelightName = names[i]; // Seed robot heading (used by MT2) From c2f1a1c2fed4162bac2409aa3475d0c8147ef513 Mon Sep 17 00:00:00 2001 From: Brian Zheng Date: Fri, 6 Mar 2026 20:38:27 -0500 Subject: [PATCH 094/150] FIX: Add fixes from alphabot --- .../java/com/stuypulse/robot/constants/Settings.java | 6 +++--- .../robot/util/superstructure/SOTMCalculator.java | 12 ++++++------ 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index ed0f8039..408d10c1 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -331,9 +331,9 @@ public interface SoftwareLimit { } public interface SOTM { - public final int MAX_ITERATIONS = 5; - public final double TIME_TOLERANCE = 0.01; - public final SmartNumber UPDATE_DELAY = new SmartNumber("Superstructure/SOTM/Update Delay", 0.00); + public final int MAX_ITERATIONS = 10; + double TIME_TOLERANCE = 1e-5; + SmartNumber UPDATE_DELAY = new SmartNumber("Superstructure/SOTM/update delay", 0.23); } } diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java index 501c17b2..efb9423b 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java @@ -162,13 +162,13 @@ public static void updateSOTMSolution() { ) ); - Vector2D oppositeDirection = new Vector2D(new Translation2d( - -robotRelativeSpeeds.vxMetersPerSecond, - -robotRelativeSpeeds.vyMetersPerSecond - )); + Vector2D oppositeDirection = Vector2D.kOrigin; - if (!oppositeDirection.equals(Vector2D.kOrigin)) { - oppositeDirection = oppositeDirection.normalize(); + if (Math.abs(robotRelativeSpeeds.vxMetersPerSecond) > Settings.Swerve.MODULE_VELOCITY_DEADBAND_M_PER_S && Math.abs(robotRelativeSpeeds.vyMetersPerSecond) > Settings.Swerve.MODULE_VELOCITY_DEADBAND_M_PER_S) { + oppositeDirection = new Vector2D(new Translation2d( + -robotRelativeSpeeds.vxMetersPerSecond, + -robotRelativeSpeeds.vyMetersPerSecond + )); } hubPose = hubPose.exp( From fcfcea86c6deb741605404d052d1cd6fe69f0fef Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Fri, 6 Mar 2026 21:51:39 -0500 Subject: [PATCH 095/150] Add: intake stop rollers --- .../java/com/stuypulse/robot/RobotContainer.java | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 034a4db7..e1bf8627 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -227,18 +227,21 @@ private void configureButtonBindings() { .onTrue(new SwerveResetHeading()) .onTrue(new ResetLimelightIMU()) .onFalse(new SetIMUMode(0)); + + driver.getLeftBumper() + .onTrue(new IntakeStopRollers()); // Climb Align // driver.getTopButton() // .whileTrue(new SwerveClimbAlign().alongWith(new ClimberUp()).andThen(new SwerveXMode())); // Climber Up - driver.getRightBumper() - .whileTrue(new ClimberUp()); + // driver.getRightBumper() + // .whileTrue(new ClimberUp()); - // Climber Down - driver.getLeftBumper() - .onTrue(new ClimberDown()); + // // Climber Down + // driver.getLeftBumper() + // .onTrue(new ClimberDown()); // // Left Corner Shoot // driver.getLeftButton() From 937c185cbcf0cf604a158bb27b49d2cbe6fa93bc Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Sat, 7 Mar 2026 01:29:33 -0500 Subject: [PATCH 096/150] ADD: Path sweeping and updated button bindings --- .../paths/Bottom NZ To Tower Right (P).path | 46 +++-- .../paths/Bottom Trench To NZ.path | 2 +- .../paths/Top NZ To Tower Left (P).path | 46 +++-- .../paths/Top Trench To NZ (P).path | 2 +- .../com/stuypulse/robot/RobotContainer.java | 167 ++++++++---------- 5 files changed, 136 insertions(+), 127 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Bottom NZ To Tower Right (P).path b/src/main/deploy/pathplanner/paths/Bottom NZ To Tower Right (P).path index f14aad0f..9a12b6a7 100644 --- a/src/main/deploy/pathplanner/paths/Bottom NZ To Tower Right (P).path +++ b/src/main/deploy/pathplanner/paths/Bottom NZ To Tower Right (P).path @@ -8,24 +8,40 @@ }, "prevControl": null, "nextControl": { - "x": 7.333950177935943, - "y": 0.41453143534993897 + "x": 6.15043890865955, + "y": 3.179644128113879 }, "isLocked": false, "linkedName": "NZ Bottom Center (P)" }, { "anchor": { - "x": 4.579596678529064, - "y": 0.43604982206405685 + "x": 6.236512455516015, + "y": 2.1360023724792403 }, "prevControl": { - "x": 6.882064056939502, - "y": 0.4468090154211143 + "x": 6.258030842230132, + "y": 3.0074970344009486 }, "nextControl": { - "x": 2.9648389542439793, - "y": 0.4285042252216036 + "x": 6.200408475809454, + "y": 0.6737911943635129 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.60111506524318, + "y": 0.6727520759193351 + }, + "prevControl": { + "x": 5.97829181494662, + "y": 0.6297153024911029 + }, + "nextControl": { + "x": 3.4004720478367103, + "y": 0.7102721702132873 }, "isLocked": false, "linkedName": null @@ -36,8 +52,8 @@ "y": 2.5448517200474496 }, "prevControl": { - "x": 2.1695373665480435, - "y": 0.4037722419928822 + "x": 2.8258481613285884, + "y": 0.6835112692763936 }, "nextControl": null, "isLocked": false, @@ -46,12 +62,12 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.0510046367851622, - "rotationDegrees": 90.0 + "waypointRelativePos": 1.0896445131375578, + "rotationDegrees": -90.0 }, { - "waypointRelativePos": 1.3261205564142173, - "rotationDegrees": 90.0 + "waypointRelativePos": 2.2812982998454565, + "rotationDegrees": -90.0 } ], "constraintZones": [], @@ -72,7 +88,7 @@ "reversed": false, "folder": "To Tower", "idealStartingState": { - "velocity": 0, + "velocity": 3.5, "rotation": 90.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Bottom Trench To NZ.path b/src/main/deploy/pathplanner/paths/Bottom Trench To NZ.path index 2c3fbfe8..2a0ecf99 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Bottom Trench To NZ.path @@ -55,7 +55,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 0, + "velocity": 3.5, "rotation": 90.0 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Top NZ To Tower Left (P).path b/src/main/deploy/pathplanner/paths/Top NZ To Tower Left (P).path index e490fd44..8e42b997 100644 --- a/src/main/deploy/pathplanner/paths/Top NZ To Tower Left (P).path +++ b/src/main/deploy/pathplanner/paths/Top NZ To Tower Left (P).path @@ -8,24 +8,40 @@ }, "prevControl": null, "nextControl": { - "x": 7.097247924080665, - "y": 7.763060498220641 + "x": 6.236512455516015, + "y": 4.739727164887308 }, "isLocked": false, "linkedName": "NZ Top Center (P)" }, { "anchor": { - "x": 4.558078291814947, - "y": 7.655468564650059 + "x": 6.2042348754448415, + "y": 5.761850533807828 }, "prevControl": { - "x": 6.73143534994069, - "y": 7.666227758007118 + "x": 6.247271648873072, + "y": 4.9871886120996445 }, "nextControl": { - "x": 2.6534508870348223, - "y": 7.646039716111543 + "x": 6.138366113055437, + "y": 6.947488256817166 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.536559905100831, + "y": 7.397247924080664 + }, + "prevControl": { + "x": 6.26879003558719, + "y": 7.440284697508897 + }, + "nextControl": { + "x": 2.6324967222244107, + "y": 7.349942006493671 }, "isLocked": false, "linkedName": null @@ -36,8 +52,8 @@ "y": 4.933392645314354 }, "prevControl": { - "x": 2.3524436536180313, - "y": 7.3434519572953745 + "x": 2.6321826809015425, + "y": 6.8485290628707 }, "nextControl": null, "isLocked": false, @@ -46,8 +62,12 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.15, - "rotationDegrees": -90.0 + "waypointRelativePos": 1.2612055641421953, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.2581143740340126, + "rotationDegrees": 90.0 } ], "constraintZones": [], @@ -68,7 +88,7 @@ "reversed": false, "folder": "To Tower", "idealStartingState": { - "velocity": 0, + "velocity": 3.5, "rotation": -90.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Top Trench To NZ (P).path b/src/main/deploy/pathplanner/paths/Top Trench To NZ (P).path index b294751f..2e438fa2 100644 --- a/src/main/deploy/pathplanner/paths/Top Trench To NZ (P).path +++ b/src/main/deploy/pathplanner/paths/Top Trench To NZ (P).path @@ -55,7 +55,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 0, + "velocity": 3.5, "rotation": -90.0 }, "reversed": false, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index e1bf8627..1c59328a 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -230,111 +230,84 @@ private void configureButtonBindings() { driver.getLeftBumper() .onTrue(new IntakeStopRollers()); - - // Climb Align - // driver.getTopButton() - // .whileTrue(new SwerveClimbAlign().alongWith(new ClimberUp()).andThen(new SwerveXMode())); - // Climber Up - // driver.getRightBumper() - // .whileTrue(new ClimberUp()); +//--------------------------------------------------------------------------------------------------------------------------\\ - // // Climber Down - // driver.getLeftBumper() - // .onTrue(new ClimberDown()); + // // Scoring In Place + // driver.getTopButton() + // .whileTrue(new SwerveXMode()) + // .whileTrue(new SuperstructureInterpolation().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + // .andThen(new SpindexerRun()).alongWith(new HandoffRun())) + // .onFalse(new SuperstructureInterpolation().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); - // // Left Corner Shoot + // // Manual Left Corner Scoring // driver.getLeftButton() - // .whileTrue( - // new SwerveXMode().alongWith( - // new SuperstructureLeftCorner().alongWith( - // new WaitUntilCommand(() -> superstructure.atTolerance())).andThen( - // new SpindexerRun().alongWith(new HandoffRun())))) - // .onFalse( - // new SuperstructureStow().alongWith( - // new SpindexerRun().alongWith( - // new HandoffStop())) - // ); - - // // Right Corner Shoot + // .whileTrue(new SwerveXMode()) + // .whileTrue(new SuperstructureLeftCorner().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + // .andThen(new SpindexerRun()).alongWith(new HandoffRun())) + // .onFalse(new SuperstructureInterpolation().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); + + // // Manual Right Corner Scoring // driver.getRightButton() - // .whileTrue( - // new SwerveXMode().alongWith( - // new SuperstructureRightCorner().alongWith( - // new WaitUntilCommand(() -> superstructure.atTolerance()).andThen( - // new SpindexerRun().alongWith(new HandoffRun()))))) - // .onFalse( - // new SuperstructureStow().alongWith( - // new SpindexerRun().alongWith( - // new HandoffStop())) - // ); - - // Hub Shoot - // driver.getBottomButton() - // .whileTrue( - // new SwerveXMode().alongWith( - // new SuperstructureKB().alongWith( - // new WaitUntilCommand(() -> superstructure.atTolerance())).andThen( - // new SpindexerRun().alongWith(new HandoffRun())))) - // .onFalse( - // new SuperstructureStow().alongWith( - // new SpindexerRun().alongWith( - // new HandoffStop())) - // ); - - - // // Ferry Routine using Interpolation Settings + // .whileTrue(new SwerveXMode()) + // .whileTrue(new SuperstructureRightCorner().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + // .andThen(new SpindexerRun()).alongWith(new HandoffRun())) + // .onFalse(new SuperstructureInterpolation().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); + + // // Manual KB Distance Scoring // driver.getBottomButton() - // .onTrue(new SuperstructureFerry() - // .alongWith(new TurretFerry()) - // .alongWith(new WaitUntilCommand(() -> superstructure.bothAtTolerance())) - // .andThen(new HandoffRun().onlyIf(() -> superstructure.bothAtTolerance()) - // .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) - // .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && superstructure.bothAtTolerance()))) - // ) - // .onFalse(new SpindexerStop() - // .alongWith(new SuperstructureStow()) - // .alongWith(new HandoffStop())); + // .whileTrue(new SwerveXMode()) + // .whileTrue(new SuperstructureKB().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + // .andThen(new SpindexerRun()).alongWith(new HandoffRun())) + // .onFalse(new SuperstructureInterpolation().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); + + // // Reset Heading + // driver.getDPadUp() + // .onTrue(new SwerveResetHeading()) + // .onTrue(new ResetLimelightIMU()) + // .onFalse(new SetIMUMode(0)); + + // // Ferrying In Place + // driver.getDPadRight() + // .whileTrue(new SwerveXMode()) + // .whileTrue(new SuperstructureFerry().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + // .andThen(new SpindexerRun()).alongWith(new HandoffRun())) + // .onFalse(new SuperstructureFerry().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); + + // /** + // // Ferrying SOTM + // driver.getLeftMenuButton() + // .onTrue(new SuperstructureSOTM().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + // .andThen(new SpindexerRun()).alongWith(new HandoffRun())) + // .onFalse(new SuperstructureSOTM().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); + // **/ + + // // Scoring SOTM + // driver.getRightMenuButton() + // .onTrue(new SuperstructureSOTM().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + // .andThen(new SpindexerRun()).alongWith(new HandoffRun())) + // .onFalse(new SuperstructureSOTM().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); + + // // Swerve X Wheels + // driver.getLeftBumper() + // .whileTrue(new SwerveXMode()); -//-------------------------------------------------------------------------------------------------------------------------\\ -//-------------------------------------------------------------------------------------------------------------------------\\ -//-------------------------------------------------------------------------------------------------------------------------\\ -//-------------------------------------------------------------------------------------------------------------------------\\ -//-------------------------------------------------------------------------------------------------------------------------\\ - /* + // /** + // // Climb + // driver.getRightBumper() + // .whileTrue(new ClimberUp().alongWith(new IntakeDeploy()) + // .andThen(new SwerveClimbAlign())) + // .onFalse(new ClimberDown()); + // **/ + + // // Stow Intake + // driver.getLeftTriggerButton() + // .onTrue(new IntakeStow()); + + // // Deploy Intake + // driver.getRightTriggerButton() + // .onTrue(new IntakeDeploy()); - // Ferry In Place - driver.getDPadLeft() - .whileTrue( - new SwerveXMode().alongWith( - new SuperstructureFerry().alongWith( - new TurretFerry()).alongWith( - new WaitUntilCommand(() -> superstructure.isHoodAtTolerance())).alongWith( - new WaitUntilCommand(() -> superstructure.isShooterAtTolerance())).alongWith( - new WaitUntilCommand(() -> turret.atTargetAngle())).andThen( - new SpindexerRun().alongWith(new HandoffRun())))) - .onFalse( - new SuperstructureStow().alongWith( - new SpindexerRun().alongWith( - new HandoffStop())) - ); - - // Score In Place - driver.getDPadRight() - .whileTrue( - new SwerveXMode().alongWith( - new SuperstructureShoot().alongWith( - new TurretShoot()).alongWith( - new WaitUntilCommand(() -> superstructure.isHoodAtTolerance())).alongWith( - new WaitUntilCommand(() -> superstructure.isShooterAtTolerance())).alongWith( - new WaitUntilCommand(() -> turret.atTargetAngle())).andThen( - new SpindexerRun().alongWith(new HandoffRun())))) - .onFalse( - new SuperstructureStow().alongWith( - new SpindexerRun().alongWith( - new HandoffStop())) - ); - */ } /**************/ From 2a164997527e779a89871e2ac46ca0b369867c80 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 7 Mar 2026 11:47:29 -0500 Subject: [PATCH 097/150] FIX: robotcontainer for jophy --- .../com/stuypulse/robot/RobotContainer.java | 92 +++++++++++-------- 1 file changed, 56 insertions(+), 36 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 1c59328a..61afefd9 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -26,6 +26,7 @@ import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.hood.ZeroHoodEncoderAtUpperHardstop; import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.intake.IntakeDigestion; import com.stuypulse.robot.commands.intake.IntakeRunRollers; import com.stuypulse.robot.commands.intake.IntakeStopRollers; import com.stuypulse.robot.commands.intake.IntakeStow; @@ -59,9 +60,11 @@ import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Gains.Intake.Pivot; import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; import com.stuypulse.robot.subsystems.superstructure.hood.Hood; import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter; import com.stuypulse.robot.subsystems.superstructure.turret.Turret; @@ -77,6 +80,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; @@ -93,8 +98,8 @@ public interface EnabledSubsystems { SmartBoolean HOOD = new SmartBoolean("Enabled Subsystems/Hood Is Enabled", true); SmartBoolean SHOOTER = new SmartBoolean("Enabled Subsystems/Shooter Is Enabled", true); - SmartBoolean BACK_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Back Limelight Is Enabled", false); - SmartBoolean LEFT_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Left Limelight Is Enabled", false); + SmartBoolean BACK_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Back Limelight Is Enabled", true); + SmartBoolean LEFT_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Left Limelight Is Enabled", true); SmartBoolean RIGHT_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Right Limelight Is Enabled", true); } @@ -156,18 +161,61 @@ private void configureDefaultCommands() { private void configureButtonBindings() { // Scoring Routine driver.getTopButton() - .whileTrue(new SuperstructureInterpolation()//.onlyIf(() -> !superstructure.isHoodUnderTrench()) - // .alongWith(new SwerveDriveAlignTurretToHub()) - // .alongWith(new TurretShoot()) + .whileTrue(new SuperstructureInterpolation() .andThen(new WaitUntilCommand(superstructure::atTolerance)) - .andThen(new HandoffRun()) - // .alongWith(new WaitUntilCommand(handoff::atTolerance)) + .andThen(new HandoffRun()) .andThen(new WaitUntilCommand(handoff::atTolerance)) - .andThen(new SpindexerRun())) + .andThen(new SpindexerRun())) .onFalse(new SpindexerStop() .alongWith(new SuperstructureStow()) .alongWith(new HandoffStop())); + // Intake Stow + driver.getLeftTriggerButton() + .onTrue(new IntakeStow()); + + // Intake Deploy + driver.getRightTriggerButton() + .onTrue(new IntakeDeploy()); + + // Reset Heading + driver.getDPadUp() + .onTrue(new SwerveResetHeading()) + .onTrue(new ResetLimelightIMU()) + .onFalse(new SetIMUMode(0)); + + // Stop Rollers + driver.getLeftBumper() + .onTrue(new IntakeStopRollers()); + + // SOTM + // driver.getRightMenuButton() + // .whileTrue(new SuperstructureSOTM().onlyIf(() -> !swerve.isUnderTrench()) + // .andThen(new WaitUntilCommand(superstructure::atTolerance)) + // .andThen(new HandoffConditionalCommand().onlyIf(superstructure::atTolerance) + // .alongWith(new WaitUntilCommand(handoff::atTolerance)) + // .andThen(new SpindexerConditionalCommand().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) + // .onFalse(new SpindexerStop() + // .alongWith(new SuperstructureStow()) + // .alongWith(new HandoffStop())); + + // Scoring SOTM + // driver.getRightMenuButton() + // .onTrue(new ConditionalCommand( + // new ParallelCommandGroup( + // new SuperstructureInterpolation(), + // new SpindexerStop(), + // new HandoffStop() + // ), + // new SuperstructureSOTM().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + // .andThen(new SpindexerRun()).alongWith(new HandoffRun()), + // () -> superstructure.getState() == SuperstructureState.SOTM + // )); + + // driver.getDPadDown() + // .whileTrue(new IntakeDigestion()) + // .onFalse(new IntakeDeploy()); + // Test Turret // driver.getBottomButton() // .whileTrue(new TurretShoot()) @@ -189,17 +237,9 @@ private void configureButtonBindings() { // .alongWith(new SuperstructureStow()) // .alongWith(new HandoffStop())); - // Intake Deploy - driver.getRightTriggerButton() - .onTrue(new IntakeDeploy()); - // driver.getRightBumper() // .whileTrue(new SwerveWheelRadiusCharacterization()); - // Intake Stow - driver.getLeftTriggerButton() - .onTrue(new IntakeStow()); - // driver.getRightButton() // .whileTrue(new SuperstructureFerry().onlyIf(() -> !swerve.isUnderTrench()) // // .alongWith(new TurretShoot()) @@ -210,26 +250,6 @@ private void configureButtonBindings() { // .onFalse(new SpindexerStop() // .alongWith(new SuperstructureStow()) // .alongWith(new HandoffStop())); - - // SOTM - // driver.getRightButton() - // .whileTrue(new SuperstructureSOTM().onlyIf(() -> !swerve.isUnderTrench()) - // .andThen(new WaitUntilCommand(superstructure::atTolerance)) - // .andThen(new HandoffConditionalCommand().onlyIf(superstructure::atTolerance) - // .alongWith(new WaitUntilCommand(handoff::atTolerance)) - // .andThen(new SpindexerConditionalCommand().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) - // .onFalse(new SpindexerStop() - // .alongWith(new SuperstructureStow()) - // .alongWith(new HandoffStop())); - - // Reset Heading - driver.getDPadUp() - .onTrue(new SwerveResetHeading()) - .onTrue(new ResetLimelightIMU()) - .onFalse(new SetIMUMode(0)); - - driver.getLeftBumper() - .onTrue(new IntakeStopRollers()); //--------------------------------------------------------------------------------------------------------------------------\\ From 62f6bfd430aaace909d21bf5ebccd676e0e0bd64 Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Sat, 7 Mar 2026 11:56:46 -0500 Subject: [PATCH 098/150] ADD: Final autos, controls --- simgui-ds.json | 5 + simgui.json | 24 +++- .../pathplanner/autos/Bottom 1 Cycle (P).auto | 25 ---- .../pathplanner/autos/Bottom 2 Cycle (P).auto | 37 ------ .../pathplanner/autos/Bottom 2 Cycle.auto | 37 ------ .../deploy/pathplanner/autos/Depot Auto.auto | 2 +- .../pathplanner/autos/Top 1 Cycle (P).auto | 25 ---- .../pathplanner/autos/Top 2 Cycle (P).auto | 37 ------ .../deploy/pathplanner/autos/Top 2 Cycle.auto | 37 ------ .../paths/Bottom NZ To Score (P).path | 54 --------- .../pathplanner/paths/Bottom NZ To Score.path | 54 --------- .../paths/Bottom NZ To Tower Right (P).path | 95 --------------- .../paths/Bottom NZ To Tower Right.path | 79 ------------ .../paths/Bottom Score To NZ (P).path | 68 ----------- .../pathplanner/paths/Bottom Score To NZ.path | 68 ----------- .../Bottom Trench Score To Tower Right.path | 59 --------- .../paths/Bottom Trench To NZ (P).path | 68 ----------- .../paths/Bottom Trench To NZ.path | 68 ----------- src/main/deploy/pathplanner/paths/Box 3.path | 2 +- .../paths/Depot To Tower Left.path | 27 +++-- .../pathplanner/paths/Top Bump To Depot.path | 54 --------- .../paths/Top NZ To Score (P).path | 54 --------- .../pathplanner/paths/Top NZ To Score.path | 54 --------- .../paths/Top NZ To Tower Left (P).path | 95 --------------- .../paths/Top NZ To Tower Left.path | 75 ------------ .../paths/Top Score To NZ (P).path | 68 ----------- .../pathplanner/paths/Top Score To NZ.path | 68 ----------- .../paths/Top Trench Score To Tower Left.path | 54 --------- .../paths/Top Trench To NZ (P).path | 68 ----------- .../pathplanner/paths/Top Trench To NZ.path | 68 ----------- src/main/deploy/pathplanner/settings.json | 27 +++-- .../com/stuypulse/robot/RobotContainer.java | 114 +++++++++++------- .../auton/poaching/BottomOneCyclePoach.java | 55 --------- .../auton/poaching/BottomTwoCyclePoach.java | 74 ------------ .../auton/poaching/TopOneCyclePoach.java | 55 --------- .../auton/poaching/TopTwoCyclePoach.java | 74 ------------ .../auton/regular/BottomTwoCycle.java | 74 ------------ .../commands/auton/regular/DepotAuton.java | 16 ++- .../commands/auton/regular/EightFuel.java | 2 - .../commands/auton/regular/TopTwoCycle.java | 78 ------------ .../robot/commands/auton/test/BoxTest.java | 2 - .../robot/commands/swerve/SwerveXMode.java | 16 +-- 42 files changed, 144 insertions(+), 1972 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/Bottom 1 Cycle (P).auto delete mode 100644 src/main/deploy/pathplanner/autos/Bottom 2 Cycle (P).auto delete mode 100644 src/main/deploy/pathplanner/autos/Bottom 2 Cycle.auto delete mode 100644 src/main/deploy/pathplanner/autos/Top 1 Cycle (P).auto delete mode 100644 src/main/deploy/pathplanner/autos/Top 2 Cycle (P).auto delete mode 100644 src/main/deploy/pathplanner/autos/Top 2 Cycle.auto delete mode 100644 src/main/deploy/pathplanner/paths/Bottom NZ To Score (P).path delete mode 100644 src/main/deploy/pathplanner/paths/Bottom NZ To Score.path delete mode 100644 src/main/deploy/pathplanner/paths/Bottom NZ To Tower Right (P).path delete mode 100644 src/main/deploy/pathplanner/paths/Bottom NZ To Tower Right.path delete mode 100644 src/main/deploy/pathplanner/paths/Bottom Score To NZ (P).path delete mode 100644 src/main/deploy/pathplanner/paths/Bottom Score To NZ.path delete mode 100644 src/main/deploy/pathplanner/paths/Bottom Trench Score To Tower Right.path delete mode 100644 src/main/deploy/pathplanner/paths/Bottom Trench To NZ (P).path delete mode 100644 src/main/deploy/pathplanner/paths/Bottom Trench To NZ.path delete mode 100644 src/main/deploy/pathplanner/paths/Top Bump To Depot.path delete mode 100644 src/main/deploy/pathplanner/paths/Top NZ To Score (P).path delete mode 100644 src/main/deploy/pathplanner/paths/Top NZ To Score.path delete mode 100644 src/main/deploy/pathplanner/paths/Top NZ To Tower Left (P).path delete mode 100644 src/main/deploy/pathplanner/paths/Top NZ To Tower Left.path delete mode 100644 src/main/deploy/pathplanner/paths/Top Score To NZ (P).path delete mode 100644 src/main/deploy/pathplanner/paths/Top Score To NZ.path delete mode 100644 src/main/deploy/pathplanner/paths/Top Trench Score To Tower Left.path delete mode 100644 src/main/deploy/pathplanner/paths/Top Trench To NZ (P).path delete mode 100644 src/main/deploy/pathplanner/paths/Top Trench To NZ.path delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomOneCyclePoach.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomTwoCyclePoach.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/poaching/TopOneCyclePoach.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/poaching/TopTwoCyclePoach.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/BottomTwoCycle.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/TopTwoCycle.java diff --git a/simgui-ds.json b/simgui-ds.json index 20138e7d..089f8616 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "System Joysticks": { + "window": { + "enabled": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/simgui.json b/simgui.json index 489fde89..7bb2b9eb 100644 --- a/simgui.json +++ b/simgui.json @@ -22,12 +22,16 @@ "/SmartDashboard/Robot/Zero Pivot Encoder at Upper Limit (Stowed)": "Command", "/SmartDashboard/Robot/Zero Turret Encoders": "Command", "/SmartDashboard/Scheduler": "Scheduler", - "/SmartDashboard/Scheduler": "Scheduler", "/SmartDashboard/Visualizers/ClimberHopper": "Mechanism2d", "/SmartDashboard/Visualizers/Hood": "Mechanism2d", "/SmartDashboard/Visualizers/Turret": "Mechanism2d" }, "windows": { + "/SmartDashboard/Autonomous": { + "window": { + "visible": true + } + }, "/SmartDashboard/Field": { "bottom": 1914, "height": 8.069275856018066, @@ -72,6 +76,9 @@ }, "open": true }, + "States": { + "open": true + }, "Superstructure": { "Hood": { "open": true @@ -81,6 +88,21 @@ }, "open": true }, + "Swerve": { + "Modules": { + "Module 0": { + "open": true + }, + "Module 1": { + "open": true + }, + "Module 2": { + "open": true + }, + "open": true + }, + "open": true + }, "Turret": { "open": true }, diff --git a/src/main/deploy/pathplanner/autos/Bottom 1 Cycle (P).auto b/src/main/deploy/pathplanner/autos/Bottom 1 Cycle (P).auto deleted file mode 100644 index 129a618e..00000000 --- a/src/main/deploy/pathplanner/autos/Bottom 1 Cycle (P).auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Bottom Trench To NZ (P)" - } - }, - { - "type": "path", - "data": { - "pathName": "Bottom NZ To Tower Right (P)" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Bottom 2 Cycle (P).auto b/src/main/deploy/pathplanner/autos/Bottom 2 Cycle (P).auto deleted file mode 100644 index cbe54ab7..00000000 --- a/src/main/deploy/pathplanner/autos/Bottom 2 Cycle (P).auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Bottom Trench To NZ (P)" - } - }, - { - "type": "path", - "data": { - "pathName": "Bottom NZ To Score (P)" - } - }, - { - "type": "path", - "data": { - "pathName": "Bottom Score To NZ" - } - }, - { - "type": "path", - "data": { - "pathName": "Bottom NZ To Tower Right" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Bottom 2 Cycle.auto b/src/main/deploy/pathplanner/autos/Bottom 2 Cycle.auto deleted file mode 100644 index c748089e..00000000 --- a/src/main/deploy/pathplanner/autos/Bottom 2 Cycle.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Bottom Trench To NZ" - } - }, - { - "type": "path", - "data": { - "pathName": "Bottom NZ To Score" - } - }, - { - "type": "path", - "data": { - "pathName": "Bottom Score To NZ" - } - }, - { - "type": "path", - "data": { - "pathName": "Bottom NZ To Tower Right" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Depot Auto.auto b/src/main/deploy/pathplanner/autos/Depot Auto.auto index 8e5b974e..03a0af76 100644 --- a/src/main/deploy/pathplanner/autos/Depot Auto.auto +++ b/src/main/deploy/pathplanner/autos/Depot Auto.auto @@ -7,7 +7,7 @@ { "type": "path", "data": { - "pathName": "Top Bump To Depot" + "pathName": "Left Bump To Depot" } }, { diff --git a/src/main/deploy/pathplanner/autos/Top 1 Cycle (P).auto b/src/main/deploy/pathplanner/autos/Top 1 Cycle (P).auto deleted file mode 100644 index be7edc36..00000000 --- a/src/main/deploy/pathplanner/autos/Top 1 Cycle (P).auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Top Trench To NZ (P)" - } - }, - { - "type": "path", - "data": { - "pathName": "Top NZ To Tower Left (P)" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Top 2 Cycle (P).auto b/src/main/deploy/pathplanner/autos/Top 2 Cycle (P).auto deleted file mode 100644 index c96a26b2..00000000 --- a/src/main/deploy/pathplanner/autos/Top 2 Cycle (P).auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Top Trench To NZ (P)" - } - }, - { - "type": "path", - "data": { - "pathName": "Top NZ To Score (P)" - } - }, - { - "type": "path", - "data": { - "pathName": "Top Score To NZ" - } - }, - { - "type": "path", - "data": { - "pathName": "Top NZ To Tower Left" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Top 2 Cycle.auto b/src/main/deploy/pathplanner/autos/Top 2 Cycle.auto deleted file mode 100644 index 667286b6..00000000 --- a/src/main/deploy/pathplanner/autos/Top 2 Cycle.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Top Trench To NZ" - } - }, - { - "type": "path", - "data": { - "pathName": "Top NZ To Score" - } - }, - { - "type": "path", - "data": { - "pathName": "Top Score To NZ" - } - }, - { - "type": "path", - "data": { - "pathName": "Top NZ To Tower Left" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom NZ To Score (P).path b/src/main/deploy/pathplanner/paths/Bottom NZ To Score (P).path deleted file mode 100644 index 9f8790a0..00000000 --- a/src/main/deploy/pathplanner/paths/Bottom NZ To Score (P).path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 8.28075919335706, - "y": 3.6638078291814944 - }, - "prevControl": null, - "nextControl": { - "x": 7.0757295373665485, - "y": 0.1778291814946611 - }, - "isLocked": false, - "linkedName": "NZ Bottom Center (P)" - }, - { - "anchor": { - "x": 4.370362595419848, - "y": 0.4282538167938924 - }, - "prevControl": { - "x": 6.871304863582444, - "y": 0.3392170818505338 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Bottom Trench Score" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": "To Score", - "idealStartingState": { - "velocity": 0, - "rotation": 90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom NZ To Score.path b/src/main/deploy/pathplanner/paths/Bottom NZ To Score.path deleted file mode 100644 index c7e4179c..00000000 --- a/src/main/deploy/pathplanner/paths/Bottom NZ To Score.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.8073546856465015, - "y": 3.696085409252669 - }, - "prevControl": null, - "nextControl": { - "x": 6.763712930011864, - "y": 0.32845788849347635 - }, - "isLocked": false, - "linkedName": "NZ Bottom Center" - }, - { - "anchor": { - "x": 4.370362595419848, - "y": 0.4282538167938924 - }, - "prevControl": { - "x": 7.274169847328244, - "y": 0.47009541984732894 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Bottom Trench Score" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": "To Score", - "idealStartingState": { - "velocity": 0, - "rotation": 90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom NZ To Tower Right (P).path b/src/main/deploy/pathplanner/paths/Bottom NZ To Tower Right (P).path deleted file mode 100644 index 9a12b6a7..00000000 --- a/src/main/deploy/pathplanner/paths/Bottom NZ To Tower Right (P).path +++ /dev/null @@ -1,95 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 8.28075919335706, - "y": 3.6638078291814944 - }, - "prevControl": null, - "nextControl": { - "x": 6.15043890865955, - "y": 3.179644128113879 - }, - "isLocked": false, - "linkedName": "NZ Bottom Center (P)" - }, - { - "anchor": { - "x": 6.236512455516015, - "y": 2.1360023724792403 - }, - "prevControl": { - "x": 6.258030842230132, - "y": 3.0074970344009486 - }, - "nextControl": { - "x": 6.200408475809454, - "y": 0.6737911943635129 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.60111506524318, - "y": 0.6727520759193351 - }, - "prevControl": { - "x": 5.97829181494662, - "y": 0.6297153024911029 - }, - "nextControl": { - "x": 3.4004720478367103, - "y": 0.7102721702132873 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.1258956109134053, - "y": 2.5448517200474496 - }, - "prevControl": { - "x": 2.8258481613285884, - "y": 0.6835112692763936 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Tower Right 1" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.0896445131375578, - "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 2.2812982998454565, - "rotationDegrees": -90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "To Tower", - "idealStartingState": { - "velocity": 3.5, - "rotation": 90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom NZ To Tower Right.path b/src/main/deploy/pathplanner/paths/Bottom NZ To Tower Right.path deleted file mode 100644 index 793b2f81..00000000 --- a/src/main/deploy/pathplanner/paths/Bottom NZ To Tower Right.path +++ /dev/null @@ -1,79 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.8073546856465015, - "y": 3.696085409252669 - }, - "prevControl": null, - "nextControl": { - "x": 6.5485290628707, - "y": 0.4683274021352313 - }, - "isLocked": false, - "linkedName": "NZ Bottom Center" - }, - { - "anchor": { - "x": 4.579596678529064, - "y": 0.43604982206405685 - }, - "prevControl": { - "x": 6.558452312157588, - "y": 0.5020116765183412 - }, - "nextControl": { - "x": 2.965717674970344, - "y": 0.382253855278766 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.1258956109134053, - "y": 2.5448517200474496 - }, - "prevControl": { - "x": 2.1695373665480435, - "y": 0.4037722419928822 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Tower Right 1" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.0510046367851622, - "rotationDegrees": 90.0 - }, - { - "waypointRelativePos": 1.3261205564142173, - "rotationDegrees": 90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "To Tower", - "idealStartingState": { - "velocity": 0, - "rotation": 90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Score To NZ (P).path b/src/main/deploy/pathplanner/paths/Bottom Score To NZ (P).path deleted file mode 100644 index 5bcab21d..00000000 --- a/src/main/deploy/pathplanner/paths/Bottom Score To NZ (P).path +++ /dev/null @@ -1,68 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.370362595419848, - "y": 0.4282538167938924 - }, - "prevControl": null, - "nextControl": { - "x": 8.807959667852908, - "y": 0.4898457888493476 - }, - "isLocked": false, - "linkedName": "Bottom Trench Score" - }, - { - "anchor": { - "x": 8.28075919335706, - "y": 3.6638078291814944 - }, - "prevControl": { - "x": 8.22218094908225, - "y": 0.7767372184944703 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "NZ Bottom Center (P)" - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.7228003784295178, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.5, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": "To NZ", - "idealStartingState": { - "velocity": 0, - "rotation": 90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Score To NZ.path b/src/main/deploy/pathplanner/paths/Bottom Score To NZ.path deleted file mode 100644 index 66693c25..00000000 --- a/src/main/deploy/pathplanner/paths/Bottom Score To NZ.path +++ /dev/null @@ -1,68 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.370362595419848, - "y": 0.4282538167938924 - }, - "prevControl": null, - "nextControl": { - "x": 8.248481613285884, - "y": 0.4468090154211143 - }, - "isLocked": false, - "linkedName": "Bottom Trench Score" - }, - { - "anchor": { - "x": 7.8073546856465015, - "y": 3.696085409252669 - }, - "prevControl": { - "x": 7.748776441371692, - "y": 0.8090147985656451 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "NZ Bottom Center" - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.7228003784295178, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.5, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": "To NZ", - "idealStartingState": { - "velocity": 0, - "rotation": 90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Trench Score To Tower Right.path b/src/main/deploy/pathplanner/paths/Bottom Trench Score To Tower Right.path deleted file mode 100644 index f31aaaac..00000000 --- a/src/main/deploy/pathplanner/paths/Bottom Trench Score To Tower Right.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.370362595419848, - "y": 0.4282538167938924 - }, - "prevControl": null, - "nextControl": { - "x": 1.3195610913404519, - "y": 0.6404744958481603 - }, - "isLocked": false, - "linkedName": "Bottom Trench Score" - }, - { - "anchor": { - "x": 1.1258956109134053, - "y": 2.5448517200474496 - }, - "prevControl": { - "x": 1.534744958481614, - "y": 1.3183036773428227 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Tower Right 1" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.21947449768161098, - "rotationDegrees": 90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "To Tower", - "idealStartingState": { - "velocity": 0, - "rotation": 90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Trench To NZ (P).path b/src/main/deploy/pathplanner/paths/Bottom Trench To NZ (P).path deleted file mode 100644 index fd178db9..00000000 --- a/src/main/deploy/pathplanner/paths/Bottom Trench To NZ (P).path +++ /dev/null @@ -1,68 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.370362595419848, - "y": 0.411517175572518 - }, - "prevControl": null, - "nextControl": { - "x": 8.689608540925267, - "y": 0.47908659549228905 - }, - "isLocked": false, - "linkedName": "Bottom Trench Auto Start" - }, - { - "anchor": { - "x": 8.28075919335706, - "y": 3.6638078291814944 - }, - "prevControl": { - "x": 8.205444839857652, - "y": 0.3714946619217079 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "NZ Bottom Center (P)" - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.6830652790917692, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.5, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": "To NZ", - "idealStartingState": { - "velocity": 0, - "rotation": 90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Trench To NZ.path b/src/main/deploy/pathplanner/paths/Bottom Trench To NZ.path deleted file mode 100644 index 2a0ecf99..00000000 --- a/src/main/deploy/pathplanner/paths/Bottom Trench To NZ.path +++ /dev/null @@ -1,68 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.370362595419848, - "y": 0.411517175572518 - }, - "prevControl": null, - "nextControl": { - "x": 7.968742586002373, - "y": 0.4468090154211143 - }, - "isLocked": false, - "linkedName": "Bottom Trench Auto Start" - }, - { - "anchor": { - "x": 7.8073546856465015, - "y": 3.696085409252669 - }, - "prevControl": { - "x": 7.936465005931199, - "y": 0.7157888493475686 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "NZ Bottom Center" - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.6830652790917692, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.5, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 3.5, - "rotation": 90.0 - }, - "reversed": false, - "folder": "To NZ", - "idealStartingState": { - "velocity": 0, - "rotation": 90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Box 3.path b/src/main/deploy/pathplanner/paths/Box 3.path index 53675b86..5d16b2e8 100644 --- a/src/main/deploy/pathplanner/paths/Box 3.path +++ b/src/main/deploy/pathplanner/paths/Box 3.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 1.77619604200323, - "y": 0.9977414835927592 + "y": 0.9977414835927593 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/Depot To Tower Left.path b/src/main/deploy/pathplanner/paths/Depot To Tower Left.path index 1e1adf3a..9e8ba71d 100644 --- a/src/main/deploy/pathplanner/paths/Depot To Tower Left.path +++ b/src/main/deploy/pathplanner/paths/Depot To Tower Left.path @@ -3,32 +3,37 @@ "waypoints": [ { "anchor": { - "x": 0.6632502965545264, - "y": 5.968082061068702 + "x": 0.7278054567022547, + "y": 5.9339976275207595 }, "prevControl": null, "nextControl": { - "x": 2.4169988137603804, - "y": 6.020071174377224 + "x": 3.869489916963227, + "y": 6.149181494661922 }, "isLocked": false, "linkedName": "Depot Collect" }, { "anchor": { - "x": 1.1043772241992889, - "y": 4.933392645314354 + "x": 1.2227283511269282, + "y": 4.922633451957295 }, "prevControl": { - "x": 2.051186239620404, - "y": 5.030225385527876 + "x": 3.643546856465006, + "y": 4.83655990510083 }, "nextControl": null, "isLocked": false, "linkedName": "Tower Left 1" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.06336939721792927, + "rotationDegrees": -90.0 + } + ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -42,13 +47,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 90.0 }, "reversed": false, "folder": "Misc", "idealStartingState": { "velocity": 0, - "rotation": 180.0 + "rotation": -90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Bump To Depot.path b/src/main/deploy/pathplanner/paths/Top Bump To Depot.path deleted file mode 100644 index dfe32110..00000000 --- a/src/main/deploy/pathplanner/paths/Top Bump To Depot.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.7296204033160913, - "y": 5.968082061068702 - }, - "prevControl": null, - "nextControl": { - "x": 3.9920262726131046, - "y": 5.956602493249752 - }, - "isLocked": false, - "linkedName": "Top Bump Start" - }, - { - "anchor": { - "x": 0.6632502965545264, - "y": 5.968082061068702 - }, - "prevControl": { - "x": 1.613412361420885, - "y": 5.968117899160344 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Depot Collect" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "Misc", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top NZ To Score (P).path b/src/main/deploy/pathplanner/paths/Top NZ To Score (P).path deleted file mode 100644 index cd31ab00..00000000 --- a/src/main/deploy/pathplanner/paths/Top NZ To Score (P).path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 8.259240806642943, - "y": 4.43846975088968 - }, - "prevControl": null, - "nextControl": { - "x": 6.849786476868328, - "y": 7.956725978647688 - }, - "isLocked": false, - "linkedName": "NZ Top Center (P)" - }, - { - "anchor": { - "x": 3.5921087786259545, - "y": 7.63337786259542 - }, - "prevControl": { - "x": 6.623843416370107, - "y": 7.655468564650059 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Top Trench Score" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0 - }, - "reversed": false, - "folder": "To Score", - "idealStartingState": { - "velocity": 0, - "rotation": -90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top NZ To Score.path b/src/main/deploy/pathplanner/paths/Top NZ To Score.path deleted file mode 100644 index fac618a4..00000000 --- a/src/main/deploy/pathplanner/paths/Top NZ To Score.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.785836298932384, - "y": 4.4169513641755636 - }, - "prevControl": null, - "nextControl": { - "x": 6.882064056939502, - "y": 7.924448398576512 - }, - "isLocked": false, - "linkedName": "NZ Top Center" - }, - { - "anchor": { - "x": 3.5921087786259545, - "y": 7.63337786259542 - }, - "prevControl": { - "x": 6.623843416370107, - "y": 7.655468564650059 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Top Trench Score" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0 - }, - "reversed": false, - "folder": "To Score", - "idealStartingState": { - "velocity": 0, - "rotation": -90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top NZ To Tower Left (P).path b/src/main/deploy/pathplanner/paths/Top NZ To Tower Left (P).path deleted file mode 100644 index 8e42b997..00000000 --- a/src/main/deploy/pathplanner/paths/Top NZ To Tower Left (P).path +++ /dev/null @@ -1,95 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 8.259240806642943, - "y": 4.43846975088968 - }, - "prevControl": null, - "nextControl": { - "x": 6.236512455516015, - "y": 4.739727164887308 - }, - "isLocked": false, - "linkedName": "NZ Top Center (P)" - }, - { - "anchor": { - "x": 6.2042348754448415, - "y": 5.761850533807828 - }, - "prevControl": { - "x": 6.247271648873072, - "y": 4.9871886120996445 - }, - "nextControl": { - "x": 6.138366113055437, - "y": 6.947488256817166 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.536559905100831, - "y": 7.397247924080664 - }, - "prevControl": { - "x": 6.26879003558719, - "y": 7.440284697508897 - }, - "nextControl": { - "x": 2.6324967222244107, - "y": 7.349942006493671 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.1043772241992889, - "y": 4.933392645314354 - }, - "prevControl": { - "x": 2.6321826809015425, - "y": 6.8485290628707 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Tower Left 1" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.2612055641421953, - "rotationDegrees": 90.0 - }, - { - "waypointRelativePos": 2.2581143740340126, - "rotationDegrees": 90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "To Tower", - "idealStartingState": { - "velocity": 3.5, - "rotation": -90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top NZ To Tower Left.path b/src/main/deploy/pathplanner/paths/Top NZ To Tower Left.path deleted file mode 100644 index ff5882dd..00000000 --- a/src/main/deploy/pathplanner/paths/Top NZ To Tower Left.path +++ /dev/null @@ -1,75 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.785836298932384, - "y": 4.4169513641755636 - }, - "prevControl": null, - "nextControl": { - "x": 7.333950177935943, - "y": 7.859893238434164 - }, - "isLocked": false, - "linkedName": "NZ Top Center" - }, - { - "anchor": { - "x": 4.558078291814947, - "y": 7.655468564650059 - }, - "prevControl": { - "x": 5.980083146752088, - "y": 7.631366787447734 - }, - "nextControl": { - "x": 2.653701067615659, - "y": 7.687746144721235 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.1043772241992889, - "y": 4.933392645314354 - }, - "prevControl": { - "x": 2.5783867141162524, - "y": 6.9668801897983395 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Tower Left 1" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.15, - "rotationDegrees": -90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "To Tower", - "idealStartingState": { - "velocity": 0, - "rotation": -90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Score To NZ (P).path b/src/main/deploy/pathplanner/paths/Top Score To NZ (P).path deleted file mode 100644 index 89e1f1cf..00000000 --- a/src/main/deploy/pathplanner/paths/Top Score To NZ (P).path +++ /dev/null @@ -1,68 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.5921087786259545, - "y": 7.63337786259542 - }, - "prevControl": null, - "nextControl": { - "x": 8.50670225385528, - "y": 7.515599051008303 - }, - "isLocked": false, - "linkedName": "Top Trench Score" - }, - { - "anchor": { - "x": 8.259240806642943, - "y": 4.43846975088968 - }, - "prevControl": { - "x": 8.366832740213523, - "y": 7.6985053380782915 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "NZ Top Center (P)" - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.7587511825922443, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.5, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0 - }, - "reversed": false, - "folder": "To NZ", - "idealStartingState": { - "velocity": 0, - "rotation": -90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Score To NZ.path b/src/main/deploy/pathplanner/paths/Top Score To NZ.path deleted file mode 100644 index 31cf5bb4..00000000 --- a/src/main/deploy/pathplanner/paths/Top Score To NZ.path +++ /dev/null @@ -1,68 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.5921087786259545, - "y": 7.63337786259542 - }, - "prevControl": null, - "nextControl": { - "x": 8.32379596678529, - "y": 7.569395017793594 - }, - "isLocked": false, - "linkedName": "Top Trench Score" - }, - { - "anchor": { - "x": 7.785836298932384, - "y": 4.4169513641755636 - }, - "prevControl": { - "x": 7.763125401827354, - "y": 7.670407928336639 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "NZ Top Center" - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.7587511825922443, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.5, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0 - }, - "reversed": false, - "folder": "To NZ", - "idealStartingState": { - "velocity": 0, - "rotation": -90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Trench Score To Tower Left.path b/src/main/deploy/pathplanner/paths/Top Trench Score To Tower Left.path deleted file mode 100644 index d5821799..00000000 --- a/src/main/deploy/pathplanner/paths/Top Trench Score To Tower Left.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.5921087786259545, - "y": 7.63337786259542 - }, - "prevControl": null, - "nextControl": { - "x": 1.4326293622109532, - "y": 6.100108466886093 - }, - "isLocked": false, - "linkedName": "Top Trench Score" - }, - { - "anchor": { - "x": 1.1043772241992889, - "y": 4.933392645314354 - }, - "prevControl": { - "x": 1.0438041131203828, - "y": 5.495921921420095 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Tower Left 1" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "To Tower", - "idealStartingState": { - "velocity": 0, - "rotation": -90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Trench To NZ (P).path b/src/main/deploy/pathplanner/paths/Top Trench To NZ (P).path deleted file mode 100644 index 2e438fa2..00000000 --- a/src/main/deploy/pathplanner/paths/Top Trench To NZ (P).path +++ /dev/null @@ -1,68 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.030877817319099, - "y": 7.633950177935944 - }, - "prevControl": null, - "nextControl": { - "x": 8.67884934756821, - "y": 7.590913404507711 - }, - "isLocked": false, - "linkedName": "Top Trench Auto Start" - }, - { - "anchor": { - "x": 8.259240806642943, - "y": 4.43846975088968 - }, - "prevControl": { - "x": 8.302277580071175, - "y": 7.8060972716488735 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "NZ Top Center (P)" - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.7152317880794704, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.5, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 3.5, - "rotation": -90.0 - }, - "reversed": false, - "folder": "To NZ", - "idealStartingState": { - "velocity": 0, - "rotation": -90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Trench To NZ.path b/src/main/deploy/pathplanner/paths/Top Trench To NZ.path deleted file mode 100644 index b4045124..00000000 --- a/src/main/deploy/pathplanner/paths/Top Trench To NZ.path +++ /dev/null @@ -1,68 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.030877817319099, - "y": 7.633950177935944 - }, - "prevControl": null, - "nextControl": { - "x": 8.226963226571769, - "y": 7.590913404507711 - }, - "isLocked": false, - "linkedName": "Top Trench Auto Start" - }, - { - "anchor": { - "x": 7.785836298932384, - "y": 4.4169513641755636 - }, - "prevControl": { - "x": 7.882669039145908, - "y": 7.730782918149465 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "NZ Top Center" - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.7152317880794704, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.5, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0 - }, - "reversed": false, - "folder": "To NZ", - "idealStartingState": { - "velocity": 0, - "rotation": -90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 0fb0623e..65131c44 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.84, - "robotLength": 0.66, + "robotWidth": 0.762, + "robotLength": 0.981, "holonomicMode": true, "pathFolders": [ "Misc", @@ -17,7 +17,7 @@ "defaultMaxAngVel": 400.0, "defaultMaxAngAccel": 900.0, "defaultNominalVoltage": 12.0, - "robotMass": 49.4, + "robotMass": 64.86, "robotMOI": 6.883, "robotTrackwidth": 0.546, "driveWheelRadius": 0.051, @@ -26,18 +26,19 @@ "driveMotorType": "krakenX60FOC", "driveCurrentLimit": 120.0, "wheelCOF": 1.542, - "flModuleX": 0.2, - "flModuleY": 0.12, - "frModuleX": 0.2, - "frModuleY": -0.3, - "blModuleX": -0.2, - "blModuleY": 0.12, - "brModuleX": -0.2, - "brModuleY": -0.3, + "flModuleX": 0.33, + "flModuleY": 0.229, + "frModuleX": 0.33, + "frModuleY": -0.229, + "blModuleX": -0.33, + "blModuleY": 0.229, + "brModuleX": -0.33, + "brModuleY": -0.229, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [ - "{\"name\":\"Intake\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.475,\"y\":0.0},\"size\":{\"width\":0.84,\"length\":0.29},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":true}}", - "{\"name\":\"Turret\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":-0.1,\"y\":0.2},\"size\":{\"width\":0.3,\"length\":0.35},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}" + "{\"name\":\"Intake\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.0,\"y\":-0.525},\"size\":{\"width\":0.3048,\"length\":0.98},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":true}}", + "{\"name\":\"Turret\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.25,\"y\":0.175},\"size\":{\"width\":0.3,\"length\":0.35},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}", + "{\"name\":\"Climber\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":-0.44,\"y\":0.1},\"size\":{\"width\":0.2,\"length\":0.1},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":true}}" ] } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 61afefd9..1ba352c8 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -6,14 +6,16 @@ package com.stuypulse.robot; import com.stuypulse.robot.commands.auton.DoNothingAuton; -import com.stuypulse.robot.commands.auton.poaching.BottomOneCyclePoach; -import com.stuypulse.robot.commands.auton.poaching.BottomTwoCyclePoach; -import com.stuypulse.robot.commands.auton.poaching.TopOneCyclePoach; -import com.stuypulse.robot.commands.auton.poaching.TopTwoCyclePoach; -import com.stuypulse.robot.commands.auton.regular.BottomTwoCycle; +import com.stuypulse.robot.commands.auton.poaching.RightTwoCyclePoach; +import com.stuypulse.robot.commands.auton.poaching.LeftOneCyclePoach; +import com.stuypulse.robot.commands.auton.poaching.LeftTwoCyclePoach; +import com.stuypulse.robot.commands.auton.poaching.RightOneCyclePoach; +import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; import com.stuypulse.robot.commands.auton.regular.DepotAuton; import com.stuypulse.robot.commands.auton.regular.EightFuel; -import com.stuypulse.robot.commands.auton.regular.TopTwoCycle; +import com.stuypulse.robot.commands.auton.regular.LeftOneCycle; +import com.stuypulse.robot.commands.auton.regular.LeftTwoCycle; +import com.stuypulse.robot.commands.auton.regular.RightOneCycle; import com.stuypulse.robot.commands.climberhopper.ClimberDown; // import com.stuypulse.robot.commands.auton.test.BoxTest; import com.stuypulse.robot.commands.climberhopper.ClimberOverrideDown; @@ -26,7 +28,7 @@ import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.hood.ZeroHoodEncoderAtUpperHardstop; import com.stuypulse.robot.commands.intake.IntakeDeploy; -import com.stuypulse.robot.commands.intake.IntakeDigestion; +// import com.stuypulse.robot.commands.intake.IntakeDigestion; import com.stuypulse.robot.commands.intake.IntakeRunRollers; import com.stuypulse.robot.commands.intake.IntakeStopRollers; import com.stuypulse.robot.commands.intake.IntakeStow; @@ -226,13 +228,11 @@ private void configureButtonBindings() { // .whileTrue(new SuperstructureShoot())//.onlyIf(() -> !superstructure.isHoodUnderTrench())) // // .alongWith(new SwerveDriveAlignTurretToHub()) // // .alongWith(new TurretShoot()) - // .whileTrue(new SuperstructureShoot().onlyIf(() -> !swerve.isUnderTrench()) - // .alongWith(new SwerveDriveAlignTurretToHub()) - // // .alongWith(new TurretShoot()) // .andThen(new WaitUntilCommand(superstructure::atTolerance)) - // .andThen(new HandoffConditionalCommand().onlyIf(superstructure::atTolerance) - // .alongWith(new WaitUntilCommand(handoff::atTolerance)) - // .andThen(new SpindexerConditionalCommand().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) + // .andThen(new HandoffRun()) + // // .alongWith(new WaitUntilCommand(handoff::atTolerance)) + // .andThen(new WaitUntilCommand(handoff::atTolerance)) + // .andThen(new SpindexerRun())) // .onFalse(new SpindexerStop() // .alongWith(new SuperstructureStow()) // .alongWith(new HandoffStop())); @@ -297,16 +297,30 @@ private void configureButtonBindings() { // /** // // Ferrying SOTM // driver.getLeftMenuButton() - // .onTrue(new SuperstructureSOTM().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) - // .andThen(new SpindexerRun()).alongWith(new HandoffRun())) - // .onFalse(new SuperstructureSOTM().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); + // .onTrue(new ConditionalCommand( + // new ParallelCommandGroup( + // new SuperstructureFerry(), + // new SpindexerStop(), + // new HandoffStop() + // ), + // new SuperstructureSOTM().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + // .andThen(new SpindexerRun()).alongWith(new HandoffRun()), + // () -> superstructure.getState() == SuperstructureState.SOTM + // )); // **/ // // Scoring SOTM // driver.getRightMenuButton() - // .onTrue(new SuperstructureSOTM().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) - // .andThen(new SpindexerRun()).alongWith(new HandoffRun())) - // .onFalse(new SuperstructureSOTM().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); + // .onTrue(new ConditionalCommand( + // new ParallelCommandGroup( + // new SuperstructureInterpolation(), + // new SpindexerStop(), + // new HandoffStop() + // ), + // new SuperstructureSOTM().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + // .andThen(new SpindexerRun()).alongWith(new HandoffRun()), + // () -> superstructure.getState() == SuperstructureState.SOTM + // )); // // Swerve X Wheels // driver.getLeftBumper() @@ -339,46 +353,54 @@ public void configureAutons() { autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); autonChooser.addOption("Wheel Radius", new SwerveWheelRadiusCharacterization()); - // TESTS + // // TESTS // AutonConfig BOX_TEST = new AutonConfig("Box Test", BoxTest::new, // "Box 1", "Box 2", "Box 3", "Box 4"); // BOX_TEST.register(autonChooser); - // BASE - AutonConfig EIGHT_FUEL = new AutonConfig("Eight Fuel", EightFuel::new, - ""); - EIGHT_FUEL.register(autonChooser); + // // BASE + // AutonConfig EIGHT_FUEL = new AutonConfig("Eight Fuel", EightFuel::new, + // ""); + // EIGHT_FUEL.register(autonChooser); - // DEPOT - AutonConfig DEPOT_AUTON = new AutonConfig("Depot Auton", DepotAuton::new, - "Top Bump To Depot", "Depot To Tower Left"); - DEPOT_AUTON.register(autonChooser); + // // DEPOT + // AutonConfig DEPOT_AUTON = new AutonConfig("Depot Auton", DepotAuton::new, + // "Left Bump To Depot", "Depot To Tower Left"); + // DEPOT_AUTON.register(autonChooser); // ONE CYCLES - AutonConfig TOP_ONE_CYCLE_POACH = new AutonConfig("Top One Cycle (Poach)", TopOneCyclePoach::new, - "Top Trench To NZ (P)", "Top NZ To Tower Left (P)"); - TOP_ONE_CYCLE_POACH.register(autonChooser); + AutonConfig LEFT_ONE_CYCLE = new AutonConfig("Left One Cycle", LeftOneCycle::new, + "Left Trench To NZ", "Left NZ To Tower Left"); + LEFT_ONE_CYCLE.register(autonChooser); + + AutonConfig RIGHT_ONE_CYCLE = new AutonConfig("Right One Cycle", RightOneCycle::new, + "Right Trench To NZ", "Right NZ To Tower Right"); + RIGHT_ONE_CYCLE.register(autonChooser); + + AutonConfig LEFT_ONE_CYCLE_POACH = new AutonConfig("Left One Cycle (P)", LeftOneCyclePoach::new, + "Left Trench To NZ (P)", "Left NZ To Tower Left (P)"); + LEFT_ONE_CYCLE_POACH.register(autonChooser); - AutonConfig BOTTOM_ONE_CYCLE_POACH = new AutonConfig("Bottom One Cycle (Poach)", BottomOneCyclePoach::new, - "Bottom Trench To NZ (P)", "Bottom NZ To Tower Right (P)"); - BOTTOM_ONE_CYCLE_POACH.register(autonChooser); + AutonConfig RIGHT_ONE_CYCLE_POACH = new AutonConfig("Right One Cycle (Poach)", RightOneCyclePoach::new, + "Right Trench To NZ (P)", "Right NZ To Tower Right (P)"); + RIGHT_ONE_CYCLE_POACH.register(autonChooser); // TWO CYCLES - AutonConfig TOP_TWO_CYCLE = new AutonConfig("Top Two Cycle", TopTwoCycle::new, - "Top Trench To NZ", "Top NZ To Score", "Top Score To NZ", "Top NZ To Tower Left"); - TOP_TWO_CYCLE.register(autonChooser); + // AutonConfig LEFT_TWO_CYCLE = new AutonConfig("Left Two Cycle", LeftTwoCycle::new, + // "Left Trench To NZ", "Left NZ To Score", "Left Score To NZ", "Left NZ To Tower Left"); + // LEFT_TWO_CYCLE.register(autonChooser); - AutonConfig BOTTOM_TWO_CYCLE = new AutonConfig("Bottom Two Cycle", BottomTwoCycle::new, - "Bottom Trench To NZ", "Bottom NZ To Score", "Bottom Score To NZ", "Bottom NZ To Tower Right"); - BOTTOM_TWO_CYCLE.register(autonChooser); + // AutonConfig RIGHT_TWO_CYCLE = new AutonConfig("Right Two Cycle", RightTwoCycle::new, + // "Right Trench To NZ", "Right NZ To Score", "Right Score To NZ", "Right NZ To Tower Right"); + // RIGHT_TWO_CYCLE.register(autonChooser); - AutonConfig TOP_TWO_CYCLE_POACH = new AutonConfig("Top Two Cycle (Poach)", TopTwoCyclePoach::new, - "Top Trench To NZ (P)", "Top NZ To Score (P)", "Top Score To NZ", "Top NZ To Tower Left"); - TOP_TWO_CYCLE_POACH.register(autonChooser); + // AutonConfig LEFT_TWO_CYCLE_POACH = new AutonConfig("Left Two Cycle (Poach)", LeftTwoCyclePoach::new, + // "Left Trench To NZ (P)", "Left NZ To Score (P)", "Left Score To NZ", "Left NZ To Tower Left"); + // LEFT_TWO_CYCLE_POACH.register(autonChooser); - AutonConfig BOTTOM_TWO_CYCLE_POACH = new AutonConfig("Bottom Two Cycle (Poach)", BottomTwoCyclePoach::new, - "Bottom Trench To NZ (P)", "Bottom NZ To Score (P)", "Bottom Score To NZ", "Bottom NZ To Tower Right"); - BOTTOM_TWO_CYCLE_POACH.register(autonChooser); + // AutonConfig RIGHT_TWO_CYCLE_POACH = new AutonConfig("Right Two Cycle (Poach)", RightTwoCyclePoach::new, + // "Right Trench To NZ (P)", "Right NZ To Score (P)", "Right Score To NZ", "Right NZ To Tower Right"); + // RIGHT_TWO_CYCLE_POACH.register(autonChooser); SmartDashboard.putData("Autonomous", autonChooser); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomOneCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomOneCyclePoach.java deleted file mode 100644 index fe954dff..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomOneCyclePoach.java +++ /dev/null @@ -1,55 +0,0 @@ -package com.stuypulse.robot.commands.auton.poaching; - -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.climberhopper.ClimberDown; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -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.superstructure.SuperstructureInterpolation; -import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.spindexer.Spindexer; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class BottomOneCyclePoach extends SequentialCommandGroup { - - public BottomOneCyclePoach(PathPlannerPath... paths) { - - addCommands( - - // NZ Trip 1 - new IntakeDeploy().alongWith( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) - ), - - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new IntakeStow() - ), - new ParallelCommandGroup( - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new SwerveClimbAlign() - ), - new SpindexerRun().alongWith( - new HandoffRun() - ).until(() -> DriverStation.getMatchTime() < 2).andThen( - new ParallelCommandGroup( - new HandoffStop(), - new SpindexerStop(), - new ClimberDown() - ) - ) - - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomTwoCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomTwoCyclePoach.java deleted file mode 100644 index 999e216a..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/poaching/BottomTwoCyclePoach.java +++ /dev/null @@ -1,74 +0,0 @@ -package com.stuypulse.robot.commands.auton.poaching; - -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.climberhopper.ClimberDown; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -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.superstructure.SuperstructureInterpolation; -import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.spindexer.Spindexer; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class BottomTwoCyclePoach extends SequentialCommandGroup { - - public BottomTwoCyclePoach(PathPlannerPath... paths) { - - addCommands( - - // NZ Trip 1 - new IntakeDeploy().alongWith( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) - ), - - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new IntakeStow() - ), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new SpindexerRun().alongWith( - new HandoffRun() - ).withTimeout(5.0), - - // NZ Trip 2 - new IntakeDeploy().alongWith( - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new HandoffStop(), - new SpindexerStop() - ) - ), - - // Trip 2 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).alongWith( - new IntakeStow() - ), - new ParallelCommandGroup( - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new SwerveClimbAlign() - ), - new SpindexerRun().alongWith( - new HandoffRun() - ).until(() -> DriverStation.getMatchTime() < 2).andThen( - new ParallelCommandGroup( - new HandoffStop(), - new SpindexerStop(), - new ClimberDown() - ) - ) - - - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopOneCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopOneCyclePoach.java deleted file mode 100644 index 25da5f00..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopOneCyclePoach.java +++ /dev/null @@ -1,55 +0,0 @@ -package com.stuypulse.robot.commands.auton.poaching; - -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.climberhopper.ClimberDown; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -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.superstructure.SuperstructureInterpolation; -import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.spindexer.Spindexer; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class TopOneCyclePoach extends SequentialCommandGroup { - - public TopOneCyclePoach(PathPlannerPath... paths) { - - addCommands( - - // NZ Trip 1 - new IntakeDeploy().alongWith( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) - ), - - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new IntakeStow() - ), - new ParallelCommandGroup( - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new SwerveClimbAlign() - ), - new SpindexerRun().alongWith( - new HandoffRun() - ).until(() -> DriverStation.getMatchTime() < 2).andThen( - new ParallelCommandGroup( - new HandoffStop(), - new SpindexerStop(), - new ClimberDown() - ) - ) - - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopTwoCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopTwoCyclePoach.java deleted file mode 100644 index c4fec762..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/poaching/TopTwoCyclePoach.java +++ /dev/null @@ -1,74 +0,0 @@ -package com.stuypulse.robot.commands.auton.poaching; - -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.climberhopper.ClimberDown; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -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.superstructure.SuperstructureInterpolation; -import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.spindexer.Spindexer; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class TopTwoCyclePoach extends SequentialCommandGroup { - - public TopTwoCyclePoach(PathPlannerPath... paths) { - - addCommands( - - // NZ Trip 1 - new IntakeDeploy().alongWith( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) - ), - - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new IntakeStow() - ), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new SpindexerRun().alongWith( - new HandoffRun() - ).withTimeout(5.0), - - // NZ Trip 2 - new IntakeDeploy().alongWith( - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new HandoffStop(), - new SpindexerStop() - ) - ), - - // Trip 2 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).alongWith( - new IntakeStow() - ), - new ParallelCommandGroup( - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new SwerveClimbAlign() - ), - new SpindexerRun().alongWith( - new HandoffRun() - ).until(() -> DriverStation.getMatchTime() < 2).andThen( - new ParallelCommandGroup( - new HandoffStop(), - new SpindexerStop(), - new ClimberDown() - ) - ) - - - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/BottomTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/BottomTwoCycle.java deleted file mode 100644 index 01b024c2..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/BottomTwoCycle.java +++ /dev/null @@ -1,74 +0,0 @@ -package com.stuypulse.robot.commands.auton.regular; - -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.climberhopper.ClimberDown; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -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.superstructure.SuperstructureInterpolation; -import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.spindexer.Spindexer; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class BottomTwoCycle extends SequentialCommandGroup { - - public BottomTwoCycle(PathPlannerPath... paths) { - - addCommands( - - // NZ Trip 1 - new IntakeDeploy().alongWith( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) - ), - - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new IntakeStow() - ), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new SpindexerRun().alongWith( - new HandoffRun() - ).withTimeout(5.0), - - // NZ Trip 2 - new IntakeDeploy().alongWith( - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new HandoffStop(), - new SpindexerStop() - ) - ), - - // Trip 2 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).alongWith( - new IntakeStow() - ), - new ParallelCommandGroup( - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new SwerveClimbAlign() - ), - new SpindexerRun().alongWith( - new HandoffRun() - ).until(() -> DriverStation.getMatchTime() < 2).andThen( - new ParallelCommandGroup( - new HandoffStop(), - new SpindexerStop(), - new ClimberDown() - ) - ) - - - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java index aafdc78a..abf66b34 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java @@ -8,10 +8,7 @@ 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.superstructure.SuperstructureInterpolation; -import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj.DriverStation; @@ -37,13 +34,14 @@ public DepotAuton(PathPlannerPath... paths) { new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new SpindexerRun().alongWith( new HandoffRun() - ).until(() -> DriverStation.getMatchTime() < 2).andThen( - new ParallelCommandGroup( - new HandoffStop(), - new SpindexerStop(), - new ClimberDown() - ) ) + // .until(() -> DriverStation.getMatchTime() < 2).andThen( + // new ParallelCommandGroup( + // new HandoffStop(), + // new SpindexerStop(), + // new ClimberDown() + // ) + // ) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java index bef51934..61a78621 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java @@ -6,9 +6,7 @@ import com.stuypulse.robot.commands.superstructure.SuperstructureKB; import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class EightFuel extends SequentialCommandGroup { diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/TopTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/TopTwoCycle.java deleted file mode 100644 index 1b4842fc..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/TopTwoCycle.java +++ /dev/null @@ -1,78 +0,0 @@ -package com.stuypulse.robot.commands.auton.regular; - -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.climberhopper.ClimberDown; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -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.superstructure.SuperstructureInterpolation; -import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.spindexer.Spindexer; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class TopTwoCycle extends SequentialCommandGroup { - - public TopTwoCycle(PathPlannerPath... paths) { - - addCommands( - - // NZ Trip 1 - new IntakeDeploy().alongWith( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) - ), - - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new IntakeStow() - ), - new ParallelCommandGroup( - new WaitUntilCommand(() -> Superstructure.getInstance().isShooterAtTolerance()), - new WaitUntilCommand(() -> Superstructure.getInstance().isHoodAtTolerance()) - ), - new SpindexerRun().alongWith( - new HandoffRun() - ).withTimeout(5.0), - - // NZ Trip 2 - new IntakeDeploy().alongWith( - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new HandoffStop(), - new SpindexerStop() - ) - ), - - // Trip 2 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).alongWith( - new IntakeStow() - ), - new ParallelCommandGroup( - new WaitUntilCommand(() -> Superstructure.getInstance().isShooterAtTolerance()), - new WaitUntilCommand(() -> Superstructure.getInstance().isHoodAtTolerance()), - new SwerveClimbAlign() - ), - new SpindexerRun().alongWith( - new HandoffRun() - ).until(() -> DriverStation.getMatchTime() < 2).andThen( - new ParallelCommandGroup( - new HandoffStop(), - new SpindexerStop(), - new ClimberDown() - ) - ) - - - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java b/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java index 3d0d292e..3b46a9b7 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java @@ -6,8 +6,6 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class BoxTest extends SequentialCommandGroup { diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveXMode.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveXMode.java index 9422a6b4..f299b20e 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveXMode.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveXMode.java @@ -1,18 +1,20 @@ -/************************ 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.commands.swerve; +import com.ctre.phoenix6.swerve.SwerveRequest; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj2.command.Command; -import com.ctre.phoenix6.swerve.SwerveRequest; - public class SwerveXMode extends Command { + private CommandSwerveDrivetrain swerve; + public SwerveXMode() { + swerve = CommandSwerveDrivetrain.getInstance(); + addRequirements(swerve); + } + + @Override + public void initialize() { SwerveRequest request = new SwerveRequest.SwerveDriveBrake(); CommandSwerveDrivetrain.getInstance().setControl(request); } From 85abfdd8e60e20ab0076922d5eb92a810d5a0faa Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Sat, 7 Mar 2026 11:56:49 -0500 Subject: [PATCH 099/150] FIX: Path tweaks --- .../pathplanner/autos/Left 1 Cycle (P).auto | 25 +++++ .../pathplanner/autos/Left 1 Cycle.auto | 25 +++++ .../pathplanner/autos/Left 2 Cycle (P).auto | 37 ++++++++ .../pathplanner/autos/Left 2 Cycle.auto | 37 ++++++++ .../pathplanner/autos/Right 1 Cycle (P).auto | 25 +++++ .../pathplanner/autos/Right 1 Cycle.auto | 25 +++++ .../pathplanner/autos/Right 2 Cycle (P).auto | 37 ++++++++ .../pathplanner/autos/Right 2 Cycle.auto | 37 ++++++++ .../pathplanner/paths/Left Bump To Depot.path | 54 +++++++++++ .../paths/Left NZ To Score (P).path | 70 ++++++++++++++ .../pathplanner/paths/Left NZ To Score.path | 70 ++++++++++++++ .../paths/Left NZ To Tower Left (P).path | 95 +++++++++++++++++++ .../paths/Left NZ To Tower Left.path | 95 +++++++++++++++++++ .../paths/Left Score To NZ (P).path | 68 +++++++++++++ .../pathplanner/paths/Left Score To NZ.path | 68 +++++++++++++ .../Left Trench Score To Tower Left.path | 54 +++++++++++ .../paths/Left Trench To NZ (P).path | 68 +++++++++++++ .../pathplanner/paths/Left Trench To NZ.path | 68 +++++++++++++ .../paths/Right NZ To Score (P).path | 70 ++++++++++++++ .../pathplanner/paths/Right NZ To Score.path | 70 ++++++++++++++ .../paths/Right NZ To Tower Right (P).path | 95 +++++++++++++++++++ .../paths/Right NZ To Tower Right.path | 95 +++++++++++++++++++ .../paths/Right Score To NZ (P).path | 68 +++++++++++++ .../pathplanner/paths/Right Score To NZ.path | 68 +++++++++++++ .../Right Trench Score To Tower Right.path | 59 ++++++++++++ .../paths/Right Trench To NZ (P).path | 68 +++++++++++++ .../pathplanner/paths/Right Trench To NZ.path | 68 +++++++++++++ .../auton/poaching/LeftOneCyclePoach.java | 54 +++++++++++ .../auton/poaching/LeftTwoCyclePoach.java | 72 ++++++++++++++ .../auton/poaching/RightOneCyclePoach.java | 54 +++++++++++ .../auton/poaching/RightTwoCyclePoach.java | 73 ++++++++++++++ .../commands/auton/regular/LeftOneCycle.java | 51 ++++++++++ .../commands/auton/regular/LeftTwoCycle.java | 75 +++++++++++++++ .../commands/auton/regular/RightOneCycle.java | 53 +++++++++++ .../commands/auton/regular/RightTwoCycle.java | 72 ++++++++++++++ 35 files changed, 2123 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/Left 1 Cycle (P).auto create mode 100644 src/main/deploy/pathplanner/autos/Left 1 Cycle.auto create mode 100644 src/main/deploy/pathplanner/autos/Left 2 Cycle (P).auto create mode 100644 src/main/deploy/pathplanner/autos/Left 2 Cycle.auto create mode 100644 src/main/deploy/pathplanner/autos/Right 1 Cycle (P).auto create mode 100644 src/main/deploy/pathplanner/autos/Right 1 Cycle.auto create mode 100644 src/main/deploy/pathplanner/autos/Right 2 Cycle (P).auto create mode 100644 src/main/deploy/pathplanner/autos/Right 2 Cycle.auto create mode 100644 src/main/deploy/pathplanner/paths/Left Bump To Depot.path create mode 100644 src/main/deploy/pathplanner/paths/Left NZ To Score (P).path create mode 100644 src/main/deploy/pathplanner/paths/Left NZ To Score.path create mode 100644 src/main/deploy/pathplanner/paths/Left NZ To Tower Left (P).path create mode 100644 src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path create mode 100644 src/main/deploy/pathplanner/paths/Left Score To NZ (P).path create mode 100644 src/main/deploy/pathplanner/paths/Left Score To NZ.path create mode 100644 src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path create mode 100644 src/main/deploy/pathplanner/paths/Left Trench To NZ (P).path create mode 100644 src/main/deploy/pathplanner/paths/Left Trench To NZ.path create mode 100644 src/main/deploy/pathplanner/paths/Right NZ To Score (P).path create mode 100644 src/main/deploy/pathplanner/paths/Right NZ To Score.path create mode 100644 src/main/deploy/pathplanner/paths/Right NZ To Tower Right (P).path create mode 100644 src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path create mode 100644 src/main/deploy/pathplanner/paths/Right Score To NZ (P).path create mode 100644 src/main/deploy/pathplanner/paths/Right Score To NZ.path create mode 100644 src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path create mode 100644 src/main/deploy/pathplanner/paths/Right Trench To NZ (P).path create mode 100644 src/main/deploy/pathplanner/paths/Right Trench To NZ.path create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/poaching/LeftOneCyclePoach.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/poaching/LeftTwoCyclePoach.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/poaching/RightOneCyclePoach.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/poaching/RightTwoCyclePoach.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java diff --git a/src/main/deploy/pathplanner/autos/Left 1 Cycle (P).auto b/src/main/deploy/pathplanner/autos/Left 1 Cycle (P).auto new file mode 100644 index 00000000..aeb62f24 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 1 Cycle (P).auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Trench To NZ (P)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left NZ To Tower Left (P)" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 1 Cycle.auto b/src/main/deploy/pathplanner/autos/Left 1 Cycle.auto new file mode 100644 index 00000000..95448c32 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 1 Cycle.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Trench To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Left NZ To Tower Left" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 2 Cycle (P).auto b/src/main/deploy/pathplanner/autos/Left 2 Cycle (P).auto new file mode 100644 index 00000000..e6b9622e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 2 Cycle (P).auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Trench To NZ (P)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left NZ To Score (P)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Left NZ To Tower Left" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 2 Cycle.auto b/src/main/deploy/pathplanner/autos/Left 2 Cycle.auto new file mode 100644 index 00000000..a1ede983 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 2 Cycle.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Trench To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Left NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Left NZ To Tower Left" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 1 Cycle (P).auto b/src/main/deploy/pathplanner/autos/Right 1 Cycle (P).auto new file mode 100644 index 00000000..5a6c524b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 1 Cycle (P).auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Trench To NZ (P)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right NZ To Tower Right (P)" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 1 Cycle.auto b/src/main/deploy/pathplanner/autos/Right 1 Cycle.auto new file mode 100644 index 00000000..4e27649a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 1 Cycle.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Trench To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Right NZ To Tower Right" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 2 Cycle (P).auto b/src/main/deploy/pathplanner/autos/Right 2 Cycle (P).auto new file mode 100644 index 00000000..ffa28f3d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 2 Cycle (P).auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Trench To NZ (P)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right NZ To Score (P)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Right NZ To Tower Right" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 2 Cycle.auto b/src/main/deploy/pathplanner/autos/Right 2 Cycle.auto new file mode 100644 index 00000000..fdbfe73e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 2 Cycle.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Trench To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Right NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Right NZ To Tower Right" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Bump To Depot.path b/src/main/deploy/pathplanner/paths/Left Bump To Depot.path new file mode 100644 index 00000000..65b1e461 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Bump To Depot.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.589750889679717, + "y": 5.9339976275207595 + }, + "prevControl": null, + "nextControl": { + "x": 3.8521567589767303, + "y": 5.92251805970181 + }, + "isLocked": false, + "linkedName": "Top Bump Start" + }, + { + "anchor": { + "x": 0.7278054567022547, + "y": 5.9339976275207595 + }, + "prevControl": { + "x": 1.6779675215686134, + "y": 5.934033465612401 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Depot Collect" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Misc", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score (P).path b/src/main/deploy/pathplanner/paths/Left NZ To Score (P).path new file mode 100644 index 00000000..64401b1a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score (P).path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.259240806642943, + "y": 4.43846975088968 + }, + "prevControl": null, + "nextControl": { + "x": 6.44093712930012, + "y": 4.459988137603796 + }, + "isLocked": false, + "linkedName": "NZ Top Center (P)" + }, + { + "anchor": { + "x": 6.182716488730724, + "y": 6.1814590747330955 + }, + "prevControl": { + "x": 6.258030842230132, + "y": 4.546061684460261 + }, + "nextControl": { + "x": 6.1110029711424225, + "y": 7.73866688522192 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.9555634638196917, + "y": 7.623190984578885 + }, + "prevControl": { + "x": 6.505492289442469, + "y": 7.784578884934756 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Top Trench Score" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 3.5, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path new file mode 100644 index 00000000..eb31f5b8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.785836298932384, + "y": 4.4169513641755636 + }, + "prevControl": null, + "nextControl": { + "x": 6.15043890865955, + "y": 4.459988137603796 + }, + "isLocked": false, + "linkedName": "NZ Top Center" + }, + { + "anchor": { + "x": 6.118161328588376, + "y": 5.98779359430605 + }, + "prevControl": { + "x": 6.140771545709218, + "y": 4.691474479377742 + }, + "nextControl": { + "x": 6.085883748517201, + "y": 7.8383748517200456 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.9555634638196917, + "y": 7.623190984578885 + }, + "prevControl": { + "x": 6.363264886854474, + "y": 7.666800073347641 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Top Trench Score" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 3.5, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Tower Left (P).path b/src/main/deploy/pathplanner/paths/Left NZ To Tower Left (P).path new file mode 100644 index 00000000..14a50ef2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left NZ To Tower Left (P).path @@ -0,0 +1,95 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.259240806642943, + "y": 4.43846975088968 + }, + "prevControl": null, + "nextControl": { + "x": 6.236512455516015, + "y": 4.739727164887308 + }, + "isLocked": false, + "linkedName": "NZ Top Center (P)" + }, + { + "anchor": { + "x": 6.2042348754448415, + "y": 5.761850533807828 + }, + "prevControl": { + "x": 6.1734013254892925, + "y": 4.986606992068286 + }, + "nextControl": { + "x": 6.279549228944247, + "y": 7.655468564650059 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.579596678529064, + "y": 7.666227758007118 + }, + "prevControl": { + "x": 6.310689800101284, + "y": 7.590135752663284 + }, + "nextControl": { + "x": 2.621423487544485, + "y": 7.7523013048635825 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2227283511269282, + "y": 4.922633451957295 + }, + "prevControl": { + "x": 2.2018149466192183, + "y": 7.268137603795967 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Tower Left 1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.2612055641421953, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 2.2581143740340126, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "To Tower", + "idealStartingState": { + "velocity": 3.5, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path b/src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path new file mode 100644 index 00000000..74b4be88 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path @@ -0,0 +1,95 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.785836298932384, + "y": 4.4169513641755636 + }, + "prevControl": null, + "nextControl": { + "x": 6.139679715302492, + "y": 4.38467378410439 + }, + "isLocked": false, + "linkedName": "NZ Top Center" + }, + { + "anchor": { + "x": 6.2903084223013055, + "y": 6.256773428232503 + }, + "prevControl": { + "x": 6.258030842230131, + "y": 5.492870699881376 + }, + "nextControl": { + "x": 6.344786942972187, + "y": 7.546098417443335 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.6118742586002375, + "y": 7.666227758007118 + }, + "prevControl": { + "x": 6.3441043890865965, + "y": 7.687746144721235 + }, + "nextControl": { + "x": 2.707370456345143, + "y": 7.642569325680967 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2227283511269282, + "y": 4.922633451957295 + }, + "prevControl": { + "x": 2.5461091340450777, + "y": 7.343451957295374 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Tower Left 1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.1406491499227187, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 2.24884080370944, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "To Tower", + "idealStartingState": { + "velocity": 3.5, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ (P).path b/src/main/deploy/pathplanner/paths/Left Score To NZ (P).path new file mode 100644 index 00000000..86a368cd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ (P).path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.9555634638196917, + "y": 7.623190984578885 + }, + "prevControl": null, + "nextControl": { + "x": 8.872514827995255, + "y": 7.687746144721235 + }, + "isLocked": false, + "linkedName": "Top Trench Score" + }, + { + "anchor": { + "x": 8.259240806642943, + "y": 4.43846975088968 + }, + "prevControl": { + "x": 8.366832740213523, + "y": 7.6985053380782915 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "NZ Top Center (P)" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7587511825922443, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.5, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ.path b/src/main/deploy/pathplanner/paths/Left Score To NZ.path new file mode 100644 index 00000000..b32e225a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.9555634638196917, + "y": 7.623190984578885 + }, + "prevControl": null, + "nextControl": { + "x": 8.313036773428234, + "y": 7.6447093712930005 + }, + "isLocked": false, + "linkedName": "Top Trench Score" + }, + { + "anchor": { + "x": 7.785836298932384, + "y": 4.4169513641755636 + }, + "prevControl": { + "x": 7.763125401827354, + "y": 7.670407928336639 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "NZ Top Center" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7587511825922443, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.5, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path b/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path new file mode 100644 index 00000000..c5fb88b4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.9555634638196917, + "y": 7.623190984578885 + }, + "prevControl": null, + "nextControl": { + "x": 1.965112692763939, + "y": 7.171304863582444 + }, + "isLocked": false, + "linkedName": "Top Trench Score" + }, + { + "anchor": { + "x": 1.2227283511269282, + "y": 4.922633451957295 + }, + "prevControl": { + "x": 1.5777817319098464, + "y": 6.04158956109134 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Tower Left 1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "To Tower", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ (P).path b/src/main/deploy/pathplanner/paths/Left Trench To NZ (P).path new file mode 100644 index 00000000..ab2909ee --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ (P).path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.407449584816133, + "y": 7.633950177935944 + }, + "prevControl": null, + "nextControl": { + "x": 9.055421115065243, + "y": 7.590913404507711 + }, + "isLocked": false, + "linkedName": "Top Trench Auto Start" + }, + { + "anchor": { + "x": 8.259240806642943, + "y": 4.43846975088968 + }, + "prevControl": { + "x": 8.302277580071175, + "y": 7.8060972716488735 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "NZ Top Center (P)" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7152317880794704, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.5, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 3.5, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path new file mode 100644 index 00000000..01628d6a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.407449584816133, + "y": 7.633950177935944 + }, + "prevControl": null, + "nextControl": { + "x": 8.366832740213523, + "y": 7.612431791221827 + }, + "isLocked": false, + "linkedName": "Top Trench Auto Start" + }, + { + "anchor": { + "x": 7.785836298932384, + "y": 4.4169513641755636 + }, + "prevControl": { + "x": 7.882669039145908, + "y": 7.730782918149465 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "NZ Top Center" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7152317880794704, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.5, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 3.5, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score (P).path b/src/main/deploy/pathplanner/paths/Right NZ To Score (P).path new file mode 100644 index 00000000..2aa0eea3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score (P).path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.28075919335706, + "y": 3.6638078291814944 + }, + "prevControl": null, + "nextControl": { + "x": 6.064365361803085, + "y": 3.663807829181495 + }, + "isLocked": false, + "linkedName": "NZ Bottom Center (P)" + }, + { + "anchor": { + "x": 6.1289205219454335, + "y": 2.1037247924080655 + }, + "prevControl": { + "x": 6.13374753053443, + "y": 2.900181209592518 + }, + "nextControl": { + "x": 6.118161328588376, + "y": 0.3284578884934761 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.375172004744959, + "y": 0.4037722419928821 + }, + "prevControl": { + "x": 6.434987345268173, + "y": 0.3470130871206981 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Bottom Trench Score" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 3.5, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path new file mode 100644 index 00000000..1d2f2010 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.8073546856465015, + "y": 3.696085409252669 + }, + "prevControl": null, + "nextControl": { + "x": 6.483973902728352, + "y": 3.566975088967972 + }, + "isLocked": false, + "linkedName": "NZ Bottom Center" + }, + { + "anchor": { + "x": 6.387141162514829, + "y": 2.049928825622776 + }, + "prevControl": { + "x": 6.3502895773017505, + "y": 3.3950116859001485 + }, + "nextControl": { + "x": 6.430177935943061, + "y": 0.47908659549228894 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.375172004744959, + "y": 0.4037722419928821 + }, + "prevControl": { + "x": 6.3979003558718865, + "y": 0.5006049822064051 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Bottom Trench Score" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 3.5, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Tower Right (P).path b/src/main/deploy/pathplanner/paths/Right NZ To Tower Right (P).path new file mode 100644 index 00000000..22d21cf6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right NZ To Tower Right (P).path @@ -0,0 +1,95 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.28075919335706, + "y": 3.6638078291814944 + }, + "prevControl": null, + "nextControl": { + "x": 6.15043890865955, + "y": 3.179644128113879 + }, + "isLocked": false, + "linkedName": "NZ Bottom Center (P)" + }, + { + "anchor": { + "x": 6.236512455516015, + "y": 2.1360023724792403 + }, + "prevControl": { + "x": 6.258030842230132, + "y": 3.0074970344009486 + }, + "nextControl": { + "x": 6.200408475809454, + "y": 0.6737911943635129 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.60111506524318, + "y": 0.6727520759193351 + }, + "prevControl": { + "x": 5.97829181494662, + "y": 0.6297153024911029 + }, + "nextControl": { + "x": 3.4004720478367103, + "y": 0.7102721702132873 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.9645077105575333, + "y": 2.5448517200474496 + }, + "prevControl": { + "x": 2.6644602609727146, + "y": 0.6835112692763936 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Tower Right 1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0896445131375578, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.2812982998454565, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "To Tower", + "idealStartingState": { + "velocity": 3.5, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path b/src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path new file mode 100644 index 00000000..7adb21a7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path @@ -0,0 +1,95 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.8073546856465015, + "y": 3.696085409252669 + }, + "prevControl": null, + "nextControl": { + "x": 5.892218268090155, + "y": 3.7176037959667845 + }, + "isLocked": false, + "linkedName": "NZ Bottom Center" + }, + { + "anchor": { + "x": 6.2903084223013055, + "y": 1.6625978647686828 + }, + "prevControl": { + "x": 6.335836337853352, + "y": 2.588332147660291 + }, + "nextControl": { + "x": 6.225753262158957, + "y": 0.3499762752075912 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.579596678529064, + "y": 0.4683274021352315 + }, + "prevControl": { + "x": 6.161198102016608, + "y": 0.4252906287069993 + }, + "nextControl": { + "x": 2.899578804528647, + "y": 0.5140421742168749 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.9645077105575333, + "y": 2.5448517200474496 + }, + "prevControl": { + "x": 2.24485172004745, + "y": 0.5113641755634628 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Tower Right 1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.3400309119010836, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.3261205564142173, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "To Tower", + "idealStartingState": { + "velocity": 3.5, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (P).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (P).path new file mode 100644 index 00000000..8d3a5e2e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ (P).path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.375172004744959, + "y": 0.4037722419928821 + }, + "prevControl": null, + "nextControl": { + "x": 8.81276907717802, + "y": 0.4653642140483373 + }, + "isLocked": false, + "linkedName": "Bottom Trench Score" + }, + { + "anchor": { + "x": 8.28075919335706, + "y": 3.6638078291814944 + }, + "prevControl": { + "x": 8.22218094908225, + "y": 0.7767372184944703 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "NZ Bottom Center (P)" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7228003784295178, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.5, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ.path b/src/main/deploy/pathplanner/paths/Right Score To NZ.path new file mode 100644 index 00000000..5ff2c841 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.375172004744959, + "y": 0.4037722419928821 + }, + "prevControl": null, + "nextControl": { + "x": 8.253291022610997, + "y": 0.42232744062010397 + }, + "isLocked": false, + "linkedName": "Bottom Trench Score" + }, + { + "anchor": { + "x": 7.8073546856465015, + "y": 3.696085409252669 + }, + "prevControl": { + "x": 7.936465005931199, + "y": 0.7911032028469753 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "NZ Bottom Center" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7228003784295178, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.5, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 3.5, + "rotation": 180.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path b/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path new file mode 100644 index 00000000..8754b973 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.375172004744959, + "y": 0.4037722419928821 + }, + "prevControl": null, + "nextControl": { + "x": 1.3243705006655637, + "y": 0.61599292104715 + }, + "isLocked": false, + "linkedName": "Bottom Trench Score" + }, + { + "anchor": { + "x": 0.9645077105575333, + "y": 2.5448517200474496 + }, + "prevControl": { + "x": 1.373357058125742, + "y": 1.3183036773428227 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Tower Right 1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.17310664605873255, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "To Tower", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ (P).path b/src/main/deploy/pathplanner/paths/Right Trench To NZ (P).path new file mode 100644 index 00000000..ccfb3e7a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ (P).path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.370362595419848, + "y": 0.411517175572518 + }, + "prevControl": null, + "nextControl": { + "x": 8.689608540925267, + "y": 0.47908659549228905 + }, + "isLocked": false, + "linkedName": "Bottom Trench Auto Start" + }, + { + "anchor": { + "x": 8.28075919335706, + "y": 3.6638078291814944 + }, + "prevControl": { + "x": 8.205444839857652, + "y": 0.37149466192170744 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "NZ Bottom Center (P)" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6830652790917692, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.5, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path new file mode 100644 index 00000000..98a1a93b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.370362595419848, + "y": 0.411517175572518 + }, + "prevControl": null, + "nextControl": { + "x": 7.968742586002373, + "y": 0.4468090154211143 + }, + "isLocked": false, + "linkedName": "Bottom Trench Auto Start" + }, + { + "anchor": { + "x": 7.8073546856465015, + "y": 3.696085409252669 + }, + "prevControl": { + "x": 7.936465005931199, + "y": 0.7157888493475686 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "NZ Bottom Center" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6830652790917692, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.5, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 3.5, + "rotation": 180.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/poaching/LeftOneCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/LeftOneCyclePoach.java new file mode 100644 index 00000000..4d6527af --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/poaching/LeftOneCyclePoach.java @@ -0,0 +1,54 @@ +package com.stuypulse.robot.commands.auton.poaching; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.climberhopper.ClimberDown; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +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.climbAlign.SwerveClimbAlign; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class LeftOneCyclePoach extends SequentialCommandGroup { + + public LeftOneCyclePoach(PathPlannerPath... paths) { + + addCommands( + + // NZ Trip 1 + new IntakeDeploy().alongWith( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new IntakeStow() + ), + new ParallelCommandGroup( + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) + // new SwerveClimbAlign() + ), + new SpindexerRun().alongWith( + new HandoffRun() + ) + // .until(() -> DriverStation.getMatchTime() < 2).andThen( + // new ParallelCommandGroup( + // new HandoffStop(), + // new SpindexerStop(), + // new ClimberDown() + // ) + // ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/poaching/LeftTwoCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/LeftTwoCyclePoach.java new file mode 100644 index 00000000..2d534b1b --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/poaching/LeftTwoCyclePoach.java @@ -0,0 +1,72 @@ +package com.stuypulse.robot.commands.auton.poaching; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.climberhopper.ClimberDown; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +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.climbAlign.SwerveClimbAlign; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class LeftTwoCyclePoach extends SequentialCommandGroup { + + public LeftTwoCyclePoach(PathPlannerPath... paths) { + + addCommands( + + // NZ Trip 1 + new IntakeDeploy().alongWith( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new IntakeStow() + ), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new SpindexerRun().alongWith( + new HandoffRun() + ).withTimeout(5.0), + + // NZ Trip 2 + new IntakeDeploy().alongWith( + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ) + ), + + // Trip 2 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).alongWith( + new IntakeStow() + ), + new ParallelCommandGroup( + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) + // new SwerveClimbAlign() + ), + new SpindexerRun().alongWith( + new HandoffRun() + ) + // .until(() -> DriverStation.getMatchTime() < 2).andThen( + // new ParallelCommandGroup( + // new HandoffStop(), + // new SpindexerStop(), + // new ClimberDown() + // ) + // ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/poaching/RightOneCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/RightOneCyclePoach.java new file mode 100644 index 00000000..b3eea775 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/poaching/RightOneCyclePoach.java @@ -0,0 +1,54 @@ +package com.stuypulse.robot.commands.auton.poaching; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.climberhopper.ClimberDown; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +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.climbAlign.SwerveClimbAlign; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class RightOneCyclePoach extends SequentialCommandGroup { + + public RightOneCyclePoach(PathPlannerPath... paths) { + + addCommands( + + // NZ Trip 1 + new IntakeDeploy().alongWith( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new IntakeStow() + ), + new ParallelCommandGroup( + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) + // new SwerveClimbAlign() + ), + new SpindexerRun().alongWith( + new HandoffRun() + ) + // .until(() -> DriverStation.getMatchTime() < 2).andThen( + // new ParallelCommandGroup( + // new HandoffStop(), + // new SpindexerStop(), + // new ClimberDown() + // ) + // ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/poaching/RightTwoCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/RightTwoCyclePoach.java new file mode 100644 index 00000000..6c9f0a78 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/poaching/RightTwoCyclePoach.java @@ -0,0 +1,73 @@ +package com.stuypulse.robot.commands.auton.poaching; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.climberhopper.ClimberDown; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +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.climbAlign.SwerveClimbAlign; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class RightTwoCyclePoach extends SequentialCommandGroup { + + public RightTwoCyclePoach(PathPlannerPath... paths) { + + addCommands( + + // NZ Trip 1 + new IntakeDeploy().alongWith( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new IntakeStow() + ), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new SpindexerRun().alongWith( + new HandoffRun() + ).withTimeout(5.0), + + // NZ Trip 2 + new IntakeDeploy().alongWith( + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ) + ), + + // Trip 2 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).alongWith( + new IntakeStow() + ), + new ParallelCommandGroup( + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) + // new SwerveClimbAlign() + ), + new SpindexerRun().alongWith( + new HandoffRun() + ) + // .until(() -> DriverStation.getMatchTime() < 2).andThen( + // new ParallelCommandGroup( + // new HandoffStop(), + // new SpindexerStop(), + // new ClimberDown() + // ) + // ) + + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java new file mode 100644 index 00000000..5f4239b3 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java @@ -0,0 +1,51 @@ +package com.stuypulse.robot.commands.auton.regular; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.climberhopper.ClimberDown; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +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.climbAlign.SwerveClimbAlign; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class LeftOneCycle extends SequentialCommandGroup { + + public LeftOneCycle(PathPlannerPath... paths) { + + addCommands( + + // NZ Trip 1 + new IntakeDeploy().alongWith( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new IntakeStow() + ), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new SpindexerRun().alongWith( + new HandoffRun() + ).withTimeout(5.0) + // .until(() -> DriverStation.getMatchTime() < 2).andThen( + // new ParallelCommandGroup( + // new HandoffStop(), + // new SpindexerStop(), + // new ClimberDown() + // ) + // ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java new file mode 100644 index 00000000..99b75683 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java @@ -0,0 +1,75 @@ +package com.stuypulse.robot.commands.auton.regular; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.climberhopper.ClimberDown; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +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.climbAlign.SwerveClimbAlign; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class LeftTwoCycle extends SequentialCommandGroup { + + public LeftTwoCycle(PathPlannerPath... paths) { + + addCommands( + + // NZ Trip 1 + new IntakeDeploy().alongWith( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new IntakeStow() + ), + new ParallelCommandGroup( + new WaitUntilCommand(() -> Superstructure.getInstance().isShooterAtTolerance()), + new WaitUntilCommand(() -> Superstructure.getInstance().isHoodAtTolerance()) + ), + new SpindexerRun().alongWith( + new HandoffRun() + ).withTimeout(5.0), + + // NZ Trip 2 + new IntakeDeploy().alongWith( + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ) + ), + + // Trip 2 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).alongWith( + new IntakeStow() + ), + new ParallelCommandGroup( + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) + // new SwerveClimbAlign() + ), + new SpindexerRun().alongWith( + new HandoffRun() + ) + // .until(() -> DriverStation.getMatchTime() < 2).andThen( + // new ParallelCommandGroup( + // new HandoffStop(), + // new SpindexerStop(), + // new ClimberDown() + // ) + // ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java new file mode 100644 index 00000000..cecea925 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java @@ -0,0 +1,53 @@ +package com.stuypulse.robot.commands.auton.regular; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.climberhopper.ClimberDown; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +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.climbAlign.SwerveClimbAlign; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class RightOneCycle extends SequentialCommandGroup { + + public RightOneCycle(PathPlannerPath... paths) { + + addCommands( + + // NZ Trip 1 + new IntakeDeploy().alongWith( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new IntakeStow() + ), + new ParallelCommandGroup( + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) + // new SwerveClimbAlign() + ), + new SpindexerRun().alongWith( + new HandoffRun() + ) + // .until(() -> DriverStation.getMatchTime() < 2).andThen( + // new ParallelCommandGroup( + // new HandoffStop(), + // new SpindexerStop(), + // new ClimberDown() + // ) + // ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java new file mode 100644 index 00000000..b3f835a5 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java @@ -0,0 +1,72 @@ +package com.stuypulse.robot.commands.auton.regular; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.climberhopper.ClimberDown; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +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.climbAlign.SwerveClimbAlign; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class RightTwoCycle extends SequentialCommandGroup { + + public RightTwoCycle(PathPlannerPath... paths) { + + addCommands( + + // NZ Trip 1 + new IntakeDeploy().alongWith( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new IntakeStow() + ), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new SpindexerRun().alongWith( + new HandoffRun() + ).withTimeout(5.0), + + // NZ Trip 2 + new IntakeDeploy().alongWith( + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ) + ), + + // Trip 2 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).alongWith( + new IntakeStow() + ), + new ParallelCommandGroup( + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) + // new SwerveClimbAlign() + ), + new SpindexerRun().alongWith( + new HandoffRun() + ) + // .until(() -> DriverStation.getMatchTime() < 2).andThen( + // new ParallelCommandGroup( + // new HandoffStop(), + // new SpindexerStop(), + // new ClimberDown() + // ) + // ) + + ); + + } + +} From 3ce3b6245851615aa7fb45497335bf0f322e09dc Mon Sep 17 00:00:00 2001 From: Brian Zheng Date: Sat, 7 Mar 2026 13:35:36 -0500 Subject: [PATCH 100/150] FEAT: 4 TOF values --- .../stuypulse/robot/constants/Settings.java | 23 ++++++------------- 1 file changed, 7 insertions(+), 16 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 408d10c1..4748c8ae 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -161,24 +161,15 @@ public interface RPMInterpolation{ } public interface TOFInterpolation{ - // double[][] distanceTOFInterpolationValues = { - // {1.30, 0.0}, // seconds - // {1.43, 0.0}, - // {2.15, 0.0}, - // {2.864967, 0.0}, - // {3.65, 0.0}, - // {4.43, 0.0}, - // {5.32, 0.0} - // }; double[][] distanceTOFInterpolationValues = { - {1.30, 1.0}, // seconds - {1.43, 1.0}, - {2.15, 1.0}, - {2.864967, 1.0}, - {3.65, 1.0}, - {4.43, 1.0}, - {5.32, 1.0} + {1.22, 1.115}, // seconds + // {1.43,}, + // {2.15, }, + {2.864967, 1.205}, + {3.65, 1.32}, + // {4.43, }, + {5.66, 1.29} }; } From 82d1b6551fc3bafdde2a3815fa9e0689a6f2d3d7 Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Sat, 7 Mar 2026 13:41:42 -0500 Subject: [PATCH 101/150] ADD: Elastic format --- elastic-layout.json | 905 ++++++++++++++++++ simgui.json | 6 + src/main/java/com/stuypulse/robot/Robot.java | 16 +- .../com/stuypulse/robot/RobotContainer.java | 7 + 4 files changed, 926 insertions(+), 8 deletions(-) create mode 100644 elastic-layout.json diff --git a/elastic-layout.json b/elastic-layout.json new file mode 100644 index 00000000..81632983 --- /dev/null +++ b/elastic-layout.json @@ -0,0 +1,905 @@ +{ + "version": 1.0, + "grid_size": 70, + "tabs": [ + { + "name": "Autonomous", + "grid_layout": { + "layouts": [ + { + "title": "Enabled Subsystems", + "x": 1120.0, + "y": 0.0, + "width": 280.0, + "height": 420.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "Intake", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Intake Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Climber", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Climber-Hopper Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Handoff", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Handoff Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Hood", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Hood Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Spindexer", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Spindexer Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Swerve", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Swerve Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Shooter", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Shooter Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Turret", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Turret Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Back Limelight", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Back Limelight Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Left Limelight", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Left Limelight Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Right Limelight", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Right Limelight Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + } + ] + } + ], + "containers": [ + { + "title": "Field", + "x": 0.0, + "y": 0.0, + "width": 840.0, + "height": 420.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Field", + "period": 0.06, + "field_game": "Rebuilt (No Fuel)", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295, + "show_robot_outside_widget": true + } + }, + { + "title": "Match Time", + "x": 0.0, + "y": 420.0, + "width": 280.0, + "height": 140.0, + "type": "Match Time", + "properties": { + "topic": "/SmartDashboard/Match Time", + "period": 0.06, + "data_type": "double", + "time_display_mode": "Minutes and Seconds", + "red_start_time": 15, + "yellow_start_time": 30 + } + }, + { + "title": "Handoff At Tol?", + "x": 980.0, + "y": 0.0, + "width": 140.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Handoff/At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Handoff State", + "x": 840.0, + "y": 0.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Handoff/State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Pivot At Tol?", + "x": 980.0, + "y": 70.0, + "width": 140.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Intake/Pivot At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Pivot State", + "x": 840.0, + "y": 70.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Intake/Pivot State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Hood State", + "x": 840.0, + "y": 140.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Superstructure/Hood/State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Shooter State", + "x": 840.0, + "y": 210.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Superstructure/Shooter/State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Turret State", + "x": 840.0, + "y": 280.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Superstructure/Turret/State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Hood At Tol?", + "x": 980.0, + "y": 140.0, + "width": 140.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Superstructure/Hood At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Shooter At Tol?", + "x": 980.0, + "y": 210.0, + "width": 140.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Superstructure/Shooter At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Turret At Tol?", + "x": 980.0, + "y": 280.0, + "width": 140.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Superstructure/Turret At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Shift", + "x": 0.0, + "y": 560.0, + "width": 280.0, + "height": 140.0, + "type": "Large Text Display", + "properties": { + "topic": "/SmartDashboard/FMSUtil/Field State", + "period": 0.06, + "data_type": "string" + } + }, + { + "title": "Is Active Shift?", + "x": 280.0, + "y": 560.0, + "width": 140.0, + "height": 140.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/FMSUtil/Is Active Shift?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Time Left In Shift", + "x": 280.0, + "y": 420.0, + "width": 280.0, + "height": 140.0, + "type": "Match Time", + "properties": { + "topic": "/SmartDashboard/FMSUtil/Time Left In Shift", + "period": 0.06, + "data_type": "double", + "time_display_mode": "Minutes and Seconds", + "red_start_time": 20, + "yellow_start_time": 30 + } + }, + { + "title": "Superstructure At Tol?", + "x": 840.0, + "y": 350.0, + "width": 280.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Superstructure/Everything At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + } + ] + } + }, + { + "name": "Teleoperated", + "grid_layout": { + "layouts": [ + { + "title": "Enabled Subsystems", + "x": 1120.0, + "y": 0.0, + "width": 280.0, + "height": 420.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "Intake", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Intake Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Climber", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Climber-Hopper Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Handoff", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Handoff Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Hood", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Hood Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Spindexer", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Spindexer Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Swerve", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Swerve Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Shooter", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Shooter Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Turret", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Turret Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Back Limelight", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Back Limelight Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Left Limelight", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Left Limelight Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Right Limelight", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Right Limelight Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + } + ] + } + ], + "containers": [ + { + "title": "Field", + "x": 0.0, + "y": 0.0, + "width": 840.0, + "height": 420.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Field", + "period": 0.06, + "field_game": "Rebuilt (No Fuel)", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295, + "show_robot_outside_widget": true + } + }, + { + "title": "Match Time", + "x": 0.0, + "y": 420.0, + "width": 280.0, + "height": 140.0, + "type": "Match Time", + "properties": { + "topic": "/SmartDashboard/Match Time", + "period": 0.06, + "data_type": "double", + "time_display_mode": "Minutes and Seconds", + "red_start_time": 15, + "yellow_start_time": 30 + } + }, + { + "title": "Handoff At Tol?", + "x": 980.0, + "y": 0.0, + "width": 140.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Handoff/At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Handoff State", + "x": 840.0, + "y": 0.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Handoff/State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Pivot At Tol?", + "x": 980.0, + "y": 70.0, + "width": 140.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Intake/Pivot At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Pivot State", + "x": 840.0, + "y": 70.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Intake/Pivot State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Hood State", + "x": 840.0, + "y": 140.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Superstructure/Hood/State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Shooter State", + "x": 840.0, + "y": 210.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Superstructure/Shooter/State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Turret State", + "x": 840.0, + "y": 280.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Superstructure/Turret/State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Hood At Tol?", + "x": 980.0, + "y": 140.0, + "width": 140.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Superstructure/Hood At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Shooter At Tol?", + "x": 980.0, + "y": 210.0, + "width": 140.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Superstructure/Shooter At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Turret At Tol?", + "x": 980.0, + "y": 280.0, + "width": 140.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Superstructure/Turret At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Shift", + "x": 0.0, + "y": 560.0, + "width": 280.0, + "height": 140.0, + "type": "Large Text Display", + "properties": { + "topic": "/SmartDashboard/FMSUtil/Field State", + "period": 0.06, + "data_type": "string" + } + }, + { + "title": "Is Active Shift?", + "x": 280.0, + "y": 560.0, + "width": 140.0, + "height": 140.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/FMSUtil/Is Active Shift?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Time Left In Shift", + "x": 280.0, + "y": 420.0, + "width": 280.0, + "height": 140.0, + "type": "Match Time", + "properties": { + "topic": "/SmartDashboard/FMSUtil/Time Left In Shift", + "period": 0.06, + "data_type": "double", + "time_display_mode": "Minutes and Seconds", + "red_start_time": 20, + "yellow_start_time": 30 + } + }, + { + "title": "Won Auto?", + "x": 420.0, + "y": 560.0, + "width": 140.0, + "height": 140.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/FMSUtil/No Auto Winner Data", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Handoff Reverse", + "x": 840.0, + "y": 420.0, + "width": 280.0, + "height": 280.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Reverse Handoff", + "period": 0.06, + "show_type": false, + "maximize_button_space": true + } + }, + { + "title": "Intake Reverse", + "x": 560.0, + "y": 420.0, + "width": 280.0, + "height": 280.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Intake Reverse", + "period": 0.06, + "show_type": false, + "maximize_button_space": true + } + }, + { + "title": "Spindexer Reverse", + "x": 1120.0, + "y": 420.0, + "width": 280.0, + "height": 280.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Spindexer Reverse", + "period": 0.06, + "show_type": false, + "maximize_button_space": true + } + }, + { + "title": "Superstructure At Tol?", + "x": 840.0, + "y": 350.0, + "width": 280.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Superstructure/Everything At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + } + ] + } + } + ] +} \ No newline at end of file diff --git a/simgui.json b/simgui.json index 7bb2b9eb..743d992a 100644 --- a/simgui.json +++ b/simgui.json @@ -11,6 +11,8 @@ "/FMSInfo": "FMSInfo", "/SmartDashboard/Autonomous": "String Chooser", "/SmartDashboard/Field": "Field2d", + "/SmartDashboard/Handoff Reverse": "Command", + "/SmartDashboard/Intake Reverse": "Command", "/SmartDashboard/PathPlanner": "Alerts", "/SmartDashboard/Robot/Override Down": "Command", "/SmartDashboard/Robot/Override Stop": "Command", @@ -22,6 +24,7 @@ "/SmartDashboard/Robot/Zero Pivot Encoder at Upper Limit (Stowed)": "Command", "/SmartDashboard/Robot/Zero Turret Encoders": "Command", "/SmartDashboard/Scheduler": "Scheduler", + "/SmartDashboard/Spindexer Reverse": "Command", "/SmartDashboard/Visualizers/ClimberHopper": "Mechanism2d", "/SmartDashboard/Visualizers/Hood": "Mechanism2d", "/SmartDashboard/Visualizers/Turret": "Mechanism2d" @@ -67,6 +70,9 @@ "Enabled Subsystems": { "open": true }, + "FMSUtil": { + "open": true + }, "FieldPositions": { "open": true }, diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index c31e43c7..fabb51b2 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -27,7 +27,7 @@ public class Robot extends TimedRobot { private static Alliance alliance; private PowerDistribution powerDistribution; - // private FMSUtil fmsUtil; + private FMSUtil fms; public static boolean isBlue() { return alliance == Alliance.Blue; @@ -41,7 +41,7 @@ public static boolean isBlue() { public void robotInit() { robot = new RobotContainer(); powerDistribution = new PowerDistribution(); - // fmsUtil = new FMSUtil(false); + fms = new FMSUtil(false); DataLogManager.start(); SignalLogger.start(); @@ -62,9 +62,9 @@ public void robotPeriodic() { alliance = DriverStation.getAlliance().get(); } - // SmartDashboard.putNumber("FMSUtil/Time Left In Shift", fmsUtil.getTimeLeftInShift()); - // SmartDashboard.putBoolean("FMSUtil/Is Active Shift?", fmsUtil.isActiveShift()); - // SmartDashboard.putString("FMSUtil/Field State", fmsUtil.getCurrentFieldState().toString()); + SmartDashboard.putNumber("FMSUtil/Time Left In Shift", fms.getTimeLeftInShift()); + SmartDashboard.putBoolean("FMSUtil/Is Active Shift?", fms.isActiveShift()); + SmartDashboard.putString("FMSUtil/Field State", fms.getCurrentFieldState().toString()); } /*********************/ @@ -85,7 +85,7 @@ public void disabledPeriodic() {} @Override public void autonomousInit() { - // fmsUtil.restartTimer(true); + fms.restartTimer(true); CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); @@ -108,7 +108,7 @@ public void autonomousExit() {} @Override public void teleopInit() { - // fmsUtil.restartTimer(false); + fms.restartTimer(false); CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); @@ -116,7 +116,7 @@ public void teleopInit() { auto.cancel(); } - // SmartDashboard.putBoolean("FMSUtil/Won Auto?", fmsUtil.didWinAuto()); + SmartDashboard.putBoolean("FMSUtil/Won Auto?", fms.didWinAuto()); } @Override diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 1ba352c8..48871b44 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -22,6 +22,7 @@ import com.stuypulse.robot.commands.climberhopper.ClimberOverrideStop; import com.stuypulse.robot.commands.climberhopper.ClimberOverrideUp; import com.stuypulse.robot.commands.handoff.HandoffConditionalCommand; +import com.stuypulse.robot.commands.handoff.HandoffReverse; import com.stuypulse.robot.commands.climberhopper.ClimberUp; import com.stuypulse.robot.commands.climberhopper.HopperDown; import com.stuypulse.robot.commands.handoff.HandoffRun; @@ -30,11 +31,13 @@ import com.stuypulse.robot.commands.intake.IntakeDeploy; // import com.stuypulse.robot.commands.intake.IntakeDigestion; import com.stuypulse.robot.commands.intake.IntakeRunRollers; +import com.stuypulse.robot.commands.intake.IntakeSetState; import com.stuypulse.robot.commands.intake.IntakeStopRollers; import com.stuypulse.robot.commands.intake.IntakeStow; import com.stuypulse.robot.commands.intake.ZeroPivotDeployed; import com.stuypulse.robot.commands.intake.ZeroPivotStowed; import com.stuypulse.robot.commands.spindexer.SpindexerConditionalCommand; +import com.stuypulse.robot.commands.spindexer.SpindexerReverse; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureDefaultCommand; @@ -71,6 +74,7 @@ import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter; import com.stuypulse.robot.subsystems.superstructure.turret.Turret; import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.intake.Intake.RollerState; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.subsystems.vision.LimelightVision; @@ -142,6 +146,9 @@ public RobotContainer() { SmartDashboard.putData("Robot/Override Up", new ClimberOverrideUp()); SmartDashboard.putData("Robot/Override Down", new ClimberOverrideDown()); SmartDashboard.putData("Robot/Override Stop", new ClimberOverrideStop()); + SmartDashboard.putData("Handoff Reverse", new HandoffReverse()); + SmartDashboard.putData("Intake Reverse", new IntakeSetState(RollerState.OUTTAKE)); + SmartDashboard.putData("Spindexer Reverse", new SpindexerReverse()); } From fdfefbf346deb42391833768ce4099b1ad49bd7b Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Sat, 7 Mar 2026 15:06:29 -0500 Subject: [PATCH 102/150] ADD: added rotation to sweep fuel in auto --- .../paths/Left NZ To Score (P).path | 33 +++++++++----- .../pathplanner/paths/Left NZ To Score.path | 33 +++++++++----- .../paths/Left NZ To Tower Left (P).path | 26 ++++++----- .../paths/Left NZ To Tower Left.path | 34 +++++++------- .../paths/Left Score To NZ (P).path | 39 +++++++--------- .../pathplanner/paths/Left Score To NZ.path | 37 +++++++--------- .../Left Trench Score To Tower Left.path | 19 +++++--- .../paths/Left Trench To NZ (P).path | 20 ++------- .../pathplanner/paths/Left Trench To NZ.path | 20 ++------- .../paths/Right NZ To Score (P).path | 35 +++++++++------ .../pathplanner/paths/Right NZ To Score.path | 25 +++++++---- .../paths/Right NZ To Tower Right (P).path | 38 +++++++++------- .../paths/Right NZ To Tower Right.path | 34 +++++++------- .../paths/Right Score To NZ (P).path | 35 +++++++-------- .../pathplanner/paths/Right Score To NZ.path | 39 +++++++--------- .../Right Trench Score To Tower Right.path | 16 +++---- .../paths/Right Trench To NZ (P).path | 20 ++------- .../pathplanner/paths/Right Trench To NZ.path | 20 ++------- .../com/stuypulse/robot/RobotContainer.java | 44 +++++++++---------- .../commands/auton/regular/LeftOneCycle.java | 9 ++-- .../commands/auton/regular/RightOneCycle.java | 4 +- 21 files changed, 280 insertions(+), 300 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score (P).path b/src/main/deploy/pathplanner/paths/Left NZ To Score (P).path index 64401b1a..a4498a1c 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score (P).path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score (P).path @@ -20,31 +20,40 @@ "y": 6.1814590747330955 }, "prevControl": { - "x": 6.258030842230132, - "y": 4.546061684460261 + "x": 6.167817715485877, + "y": 4.542594017799959 }, "nextControl": { - "x": 6.1110029711424225, - "y": 7.73866688522192 + "x": 6.193475682087783, + "y": 7.36497034400949 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.9555634638196917, - "y": 7.623190984578885 + "x": 4.42896797153025, + "y": 7.3434519572953745 }, "prevControl": { - "x": 6.505492289442469, - "y": 7.784578884934756 + "x": 6.301067615658363, + "y": 7.408007117437722 }, "nextControl": null, "isLocked": false, "linkedName": "Top Trench Score" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.18547140649150043, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.6491499227202471, + "rotationDegrees": 180.0 + } + ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -54,16 +63,16 @@ "maxAngularVelocity": 400.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "reversed": false, "folder": "To Score", "idealStartingState": { - "velocity": 3.5, + "velocity": 5.75, "rotation": 0.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index eb31f5b8..99029798 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -20,31 +20,40 @@ "y": 5.98779359430605 }, "prevControl": { - "x": 6.140771545709218, - "y": 4.691474479377742 + "x": 6.070175108111238, + "y": 4.692165641423325 }, "nextControl": { - "x": 6.085883748517201, - "y": 7.8383748517200456 + "x": 6.171957295373667, + "y": 7.440284697508897 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.9555634638196917, - "y": 7.623190984578885 + "x": 4.42896797153025, + "y": 7.3434519572953745 }, "prevControl": { - "x": 6.363264886854474, - "y": 7.666800073347641 + "x": 6.4516963226571775, + "y": 7.429525504151838 }, "nextControl": null, "isLocked": false, "linkedName": "Top Trench Score" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.36166924265842487, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.856259659969088, + "rotationDegrees": 180.0 + } + ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -54,16 +63,16 @@ "maxAngularVelocity": 400.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "reversed": false, "folder": "To Score", "idealStartingState": { - "velocity": 3.5, + "velocity": 5.75, "rotation": 0.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Tower Left (P).path b/src/main/deploy/pathplanner/paths/Left NZ To Tower Left (P).path index 14a50ef2..c6b36dc9 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Tower Left (P).path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Tower Left (P).path @@ -32,16 +32,16 @@ }, { "anchor": { - "x": 4.579596678529064, - "y": 7.666227758007118 + "x": 4.644151838671412, + "y": 7.397247924080664 }, "prevControl": { - "x": 6.310689800101284, - "y": 7.590135752663284 + "x": 6.3752449602436325, + "y": 7.32115591873683 }, "nextControl": { - "x": 2.621423487544485, - "y": 7.7523013048635825 + "x": 2.6859786476868335, + "y": 7.4833214709371285 }, "isLocked": false, "linkedName": null @@ -62,12 +62,16 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.2612055641421953, - "rotationDegrees": 0.0 + "waypointRelativePos": 0.17398119122256722, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.3714733542319881, + "rotationDegrees": 180.0 }, { "waypointRelativePos": 2.2581143740340126, - "rotationDegrees": 0.0 + "rotationDegrees": 180.0 } ], "constraintZones": [], @@ -79,7 +83,7 @@ "maxAngularVelocity": 400.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { "velocity": 0, @@ -88,7 +92,7 @@ "reversed": false, "folder": "To Tower", "idealStartingState": { - "velocity": 3.5, + "velocity": 5.75, "rotation": 0.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path b/src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path index 74b4be88..084f1ff7 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path @@ -20,28 +20,28 @@ "y": 6.256773428232503 }, "prevControl": { - "x": 6.258030842230131, - "y": 5.492870699881376 + "x": 6.253236331698933, + "y": 5.493088361823635 }, "nextControl": { - "x": 6.344786942972187, - "y": 7.546098417443335 + "x": 6.3441043890865965, + "y": 7.36497034400949 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.6118742586002375, - "y": 7.666227758007118 + "x": 4.676429418742587, + "y": 7.408007117437722 }, "prevControl": { - "x": 6.3441043890865965, - "y": 7.687746144721235 + "x": 6.279549228944247, + "y": 7.397247924080664 }, "nextControl": { - "x": 2.707370456345143, - "y": 7.642569325680967 + "x": 2.7718215695294193, + "y": 7.420789720452577 }, "isLocked": false, "linkedName": null @@ -62,12 +62,16 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.1406491499227187, - "rotationDegrees": 0.0 + "waypointRelativePos": 0.27742946708464067, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.5266457680250779, + "rotationDegrees": 180.0 }, { "waypointRelativePos": 2.24884080370944, - "rotationDegrees": 0.0 + "rotationDegrees": 180.0 } ], "constraintZones": [], @@ -79,7 +83,7 @@ "maxAngularVelocity": 400.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { "velocity": 0, @@ -88,7 +92,7 @@ "reversed": false, "folder": "To Tower", "idealStartingState": { - "velocity": 3.5, + "velocity": 5.75, "rotation": 0.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ (P).path b/src/main/deploy/pathplanner/paths/Left Score To NZ (P).path index 86a368cd..f9b2ab6f 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ (P).path +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ (P).path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.9555634638196917, - "y": 7.623190984578885 + "x": 4.42896797153025, + "y": 7.3434519572953745 }, "prevControl": null, "nextControl": { - "x": 8.872514827995255, - "y": 7.687746144721235 + "x": 9.02314353499407, + "y": 7.41876631079478 }, "isLocked": false, "linkedName": "Top Trench Score" @@ -20,30 +20,25 @@ "y": 4.43846975088968 }, "prevControl": { - "x": 8.366832740213523, - "y": 7.6985053380782915 + "x": 8.259240806642943, + "y": 7.590913404507711 }, "nextControl": null, "isLocked": false, "linkedName": "NZ Top Center (P)" } ], - "rotationTargets": [], - "constraintZones": [ + "rotationTargets": [ + { + "waypointRelativePos": 0.30407523510971435, + "rotationDegrees": 180.0 + }, { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.7587511825922443, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.5, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - } + "waypointRelativePos": 0.47335423197492277, + "rotationDegrees": 0.0 } ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -52,17 +47,17 @@ "maxAngularVelocity": 400.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { - "velocity": 0, + "velocity": 5.75, "rotation": 0.0 }, "reversed": false, "folder": "To NZ", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ.path b/src/main/deploy/pathplanner/paths/Left Score To NZ.path index b32e225a..16c1dba0 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.9555634638196917, - "y": 7.623190984578885 + "x": 4.42896797153025, + "y": 7.3434519572953745 }, "prevControl": null, "nextControl": { - "x": 8.313036773428234, - "y": 7.6447093712930005 + "x": 8.571257413997628, + "y": 7.408007117437722 }, "isLocked": false, "linkedName": "Top Trench Score" @@ -28,22 +28,17 @@ "linkedName": "NZ Top Center" } ], - "rotationTargets": [], - "constraintZones": [ + "rotationTargets": [ { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.7587511825922443, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.5, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - } + "waypointRelativePos": 0.32288401253918425, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 } ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -52,17 +47,17 @@ "maxAngularVelocity": 400.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { - "velocity": 0, + "velocity": 5.75, "rotation": 0.0 }, "reversed": false, "folder": "To NZ", "idealStartingState": { - "velocity": 0, - "rotation": 0.0 + "velocity": 5.75, + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path b/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path index c5fb88b4..1517e1fa 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path +++ b/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.9555634638196917, - "y": 7.623190984578885 + "x": 4.42896797153025, + "y": 7.3434519572953745 }, "prevControl": null, "nextControl": { - "x": 1.965112692763939, - "y": 7.171304863582444 + "x": 2.051186239620404, + "y": 7.408007117437722 }, "isLocked": false, "linkedName": "Top Trench Score" @@ -28,7 +28,12 @@ "linkedName": "Tower Left 1" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.2742946708463985, + "rotationDegrees": 180.0 + } + ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -38,7 +43,7 @@ "maxAngularVelocity": 400.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { "velocity": 0.0, @@ -48,7 +53,7 @@ "folder": "To Tower", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ (P).path b/src/main/deploy/pathplanner/paths/Left Trench To NZ (P).path index ab2909ee..703b8ca7 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ (P).path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ (P).path @@ -29,21 +29,7 @@ } ], "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.7152317880794704, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.5, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -52,10 +38,10 @@ "maxAngularVelocity": 400.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { - "velocity": 3.5, + "velocity": 5.75, "rotation": 0.0 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path index 01628d6a..bae5956b 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -29,21 +29,7 @@ } ], "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.7152317880794704, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.5, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -52,10 +38,10 @@ "maxAngularVelocity": 400.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { - "velocity": 3.5, + "velocity": 5.75, "rotation": 0.0 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score (P).path b/src/main/deploy/pathplanner/paths/Right NZ To Score (P).path index 2aa0eea3..897066ba 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score (P).path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score (P).path @@ -20,31 +20,40 @@ "y": 2.1037247924080655 }, "prevControl": { - "x": 6.13374753053443, - "y": 2.900181209592518 + "x": 6.161624603842337, + "y": 2.899524118566044 }, "nextControl": { - "x": 6.118161328588376, - "y": 0.3284578884934761 + "x": 6.064365361803085, + "y": 0.5328825622775799 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.375172004744959, - "y": 0.4037722419928821 + "x": 4.063155397390274, + "y": 0.6942704626334513 }, "prevControl": { - "x": 6.434987345268173, - "y": 0.3470130871206981 + "x": 6.5915658362989324, + "y": 0.6727520759193351 }, "nextControl": null, "isLocked": false, "linkedName": "Bottom Trench Score" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.16692426584235037, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + } + ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -54,16 +63,16 @@ "maxAngularVelocity": 400.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { - "velocity": 0, - "rotation": 180.0 + "velocity": 0.0, + "rotation": 0.0 }, "reversed": false, "folder": "To Score", "idealStartingState": { - "velocity": 3.5, + "velocity": 5.75, "rotation": 180.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index 1d2f2010..0868d7f8 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -32,19 +32,28 @@ }, { "anchor": { - "x": 4.375172004744959, - "y": 0.4037722419928821 + "x": 4.063155397390274, + "y": 0.6942704626334513 }, "prevControl": { - "x": 6.3979003558718865, - "y": 0.5006049822064051 + "x": 6.085883748517201, + "y": 0.7911032028469743 }, "nextControl": null, "isLocked": false, "linkedName": "Bottom Trench Score" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.15455950540958613, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.7789799072642942, + "rotationDegrees": 0.0 + } + ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -54,16 +63,16 @@ "maxAngularVelocity": 400.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "reversed": false, "folder": "To Score", "idealStartingState": { - "velocity": 3.5, + "velocity": 5.75, "rotation": 180.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Tower Right (P).path b/src/main/deploy/pathplanner/paths/Right NZ To Tower Right (P).path index 22d21cf6..4c4562e0 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Tower Right (P).path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Tower Right (P).path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.15043890865955, - "y": 3.179644128113879 + "x": 6.107402135231317, + "y": 3.45938315539739 }, "isLocked": false, "linkedName": "NZ Bottom Center (P)" @@ -20,28 +20,28 @@ "y": 2.1360023724792403 }, "prevControl": { - "x": 6.258030842230132, - "y": 3.0074970344009486 + "x": 6.21230622918883, + "y": 3.007426520257926 }, "nextControl": { - "x": 6.200408475809454, - "y": 0.6737911943635129 + "x": 6.26879003558719, + "y": 0.974009489916964 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.60111506524318, - "y": 0.6727520759193351 + "x": 4.375172004744959, + "y": 0.7050296559905087 }, "prevControl": { - "x": 5.97829181494662, - "y": 0.6297153024911029 + "x": 6.408659549228946, + "y": 0.7265480427046247 }, "nextControl": { - "x": 3.4004720478367103, - "y": 0.7102721702132873 + "x": 3.174010129462057, + "y": 0.6923189483155574 }, "isLocked": false, "linkedName": null @@ -62,12 +62,16 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.0896445131375578, - "rotationDegrees": 180.0 + "waypointRelativePos": 0.15987460815047247, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.7006269592476471, + "rotationDegrees": 0.0 }, { "waypointRelativePos": 2.2812982998454565, - "rotationDegrees": 180.0 + "rotationDegrees": 0.0 } ], "constraintZones": [], @@ -79,7 +83,7 @@ "maxAngularVelocity": 400.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { "velocity": 0, @@ -88,7 +92,7 @@ "reversed": false, "folder": "To Tower", "idealStartingState": { - "velocity": 3.5, + "velocity": 5.75, "rotation": 180.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path b/src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path index 7adb21a7..783e81e1 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path @@ -20,28 +20,28 @@ "y": 1.6625978647686828 }, "prevControl": { - "x": 6.335836337853352, - "y": 2.588332147660291 + "x": 6.190929893964972, + "y": 2.584107854796496 }, "nextControl": { - "x": 6.225753262158957, - "y": 0.3499762752075912 + "x": 6.408659549228946, + "y": 0.5651601423487547 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.579596678529064, - "y": 0.4683274021352315 + "x": 4.558078291814947, + "y": 0.6835112692763936 }, "prevControl": { - "x": 6.161198102016608, - "y": 0.4252906287069993 + "x": 6.171957295373667, + "y": 0.6727520759193362 }, "nextControl": { - "x": 2.899578804528647, - "y": 0.5140421742168749 + "x": 2.877475909519994, + "y": 0.694715285158359 }, "isLocked": false, "linkedName": null @@ -62,12 +62,16 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.3400309119010836, - "rotationDegrees": 180.0 + "waypointRelativePos": 0.1551724137931077, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.5548589341692779, + "rotationDegrees": 0.0 }, { "waypointRelativePos": 2.3261205564142173, - "rotationDegrees": 180.0 + "rotationDegrees": 0.0 } ], "constraintZones": [], @@ -79,7 +83,7 @@ "maxAngularVelocity": 400.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { "velocity": 0, @@ -88,7 +92,7 @@ "reversed": false, "folder": "To Tower", "idealStartingState": { - "velocity": 3.5, + "velocity": 5.75, "rotation": 180.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (P).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (P).path index 8d3a5e2e..8c025b28 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ (P).path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ (P).path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.375172004744959, - "y": 0.4037722419928821 + "x": 4.063155397390274, + "y": 0.6942704626334513 }, "prevControl": null, "nextControl": { - "x": 8.81276907717802, - "y": 0.4653642140483373 + "x": 8.500752469823334, + "y": 0.7558624346889065 }, "isLocked": false, "linkedName": "Bottom Trench Score" @@ -28,22 +28,17 @@ "linkedName": "NZ Bottom Center (P)" } ], - "rotationTargets": [], - "constraintZones": [ + "rotationTargets": [ { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.7228003784295178, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.5, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - } + "waypointRelativePos": 0.3025078369905977, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.4263322884012563, + "rotationDegrees": 180.0 } ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -52,17 +47,17 @@ "maxAngularVelocity": 400.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { - "velocity": 0, + "velocity": 5.75, "rotation": 180.0 }, "reversed": false, "folder": "To NZ", "idealStartingState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ.path b/src/main/deploy/pathplanner/paths/Right Score To NZ.path index 5ff2c841..5b1ed49a 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.375172004744959, - "y": 0.4037722419928821 + "x": 4.063155397390274, + "y": 0.6942704626334513 }, "prevControl": null, "nextControl": { - "x": 8.253291022610997, - "y": 0.42232744062010397 + "x": 8.646571767497036, + "y": 0.8448991696322663 }, "isLocked": false, "linkedName": "Bottom Trench Score" @@ -20,30 +20,25 @@ "y": 3.696085409252669 }, "prevControl": { - "x": 7.936465005931199, - "y": 0.7911032028469753 + "x": 7.7535587188612105, + "y": 0.7265480427046254 }, "nextControl": null, "isLocked": false, "linkedName": "NZ Bottom Center" } ], - "rotationTargets": [], - "constraintZones": [ + "rotationTargets": [ { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.7228003784295178, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.5, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - } + "waypointRelativePos": 0.3291536050156759, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.38871473354232106, + "rotationDegrees": 180.0 } ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -52,17 +47,17 @@ "maxAngularVelocity": 400.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { - "velocity": 3.5, + "velocity": 5.75, "rotation": 180.0 }, "reversed": false, "folder": "To NZ", "idealStartingState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path b/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path index 8754b973..22344310 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path +++ b/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.375172004744959, - "y": 0.4037722419928821 + "x": 4.063155397390274, + "y": 0.6942704626334513 }, "prevControl": null, "nextControl": { - "x": 1.3243705006655637, - "y": 0.61599292104715 + "x": 1.0123538933108782, + "y": 0.9064911416877192 }, "isLocked": false, "linkedName": "Bottom Trench Score" @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.17310664605873255, - "rotationDegrees": 180.0 + "rotationDegrees": 0.0 } ], "constraintZones": [], @@ -43,7 +43,7 @@ "maxAngularVelocity": 400.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { "velocity": 0, @@ -52,8 +52,8 @@ "reversed": false, "folder": "To Tower", "idealStartingState": { - "velocity": 0, - "rotation": 180.0 + "velocity": 0.0, + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ (P).path b/src/main/deploy/pathplanner/paths/Right Trench To NZ (P).path index ccfb3e7a..4f561017 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ (P).path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ (P).path @@ -29,21 +29,7 @@ } ], "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.6830652790917692, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.5, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -52,10 +38,10 @@ "maxAngularVelocity": 400.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { - "velocity": 0, + "velocity": 5.75, "rotation": 180.0 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index 98a1a93b..890b02c5 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -29,21 +29,7 @@ } ], "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.6830652790917692, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.5, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -52,10 +38,10 @@ "maxAngularVelocity": 400.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { - "velocity": 3.5, + "velocity": 5.75, "rotation": 180.0 }, "reversed": false, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 48871b44..19761700 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -359,7 +359,7 @@ public void configureAutons() { autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); - autonChooser.addOption("Wheel Radius", new SwerveWheelRadiusCharacterization()); + // autonChooser.addOption("Wheel Radius", new SwerveWheelRadiusCharacterization()); // // TESTS // AutonConfig BOX_TEST = new AutonConfig("Box Test", BoxTest::new, // "Box 1", "Box 2", "Box 3", "Box 4"); @@ -384,7 +384,7 @@ public void configureAutons() { "Right Trench To NZ", "Right NZ To Tower Right"); RIGHT_ONE_CYCLE.register(autonChooser); - AutonConfig LEFT_ONE_CYCLE_POACH = new AutonConfig("Left One Cycle (P)", LeftOneCyclePoach::new, + AutonConfig LEFT_ONE_CYCLE_POACH = new AutonConfig("Left One Cycle (Poach)", LeftOneCyclePoach::new, "Left Trench To NZ (P)", "Left NZ To Tower Left (P)"); LEFT_ONE_CYCLE_POACH.register(autonChooser); @@ -415,15 +415,15 @@ public void configureAutons() { public void configureSysids() { - autonChooser.addOption("SysID Module Translation Dynamic Forwards", swerve.sysIdDynamic(Direction.kForward)); - autonChooser.addOption("SysID Module Translation Dynamic Backwards", swerve.sysIdDynamic(Direction.kReverse)); - autonChooser.addOption("SysID Module Translation Quasi Forwards", swerve.sysIdQuasistatic(Direction.kForward)); - autonChooser.addOption("SysID Module Translation Quasi Backwards", swerve.sysIdQuasistatic(Direction.kReverse)); + // autonChooser.addOption("SysID Module Translation Dynamic Forwards", swerve.sysIdDynamic(Direction.kForward)); + // autonChooser.addOption("SysID Module Translation Dynamic Backwards", swerve.sysIdDynamic(Direction.kReverse)); + // autonChooser.addOption("SysID Module Translation Quasi Forwards", swerve.sysIdQuasistatic(Direction.kForward)); + // autonChooser.addOption("SysID Module Translation Quasi Backwards", swerve.sysIdQuasistatic(Direction.kReverse)); - autonChooser.addOption("SysID Rotation Translation Dynamic Forwards", swerve.sysidRotationDynamic(Direction.kForward)); - autonChooser.addOption("SysID Rotation Translation Dynamic Backwards", swerve.sysidRotationDynamic(Direction.kReverse)); - autonChooser.addOption("SysID Rotation Translation Quasi Forwards", swerve.sysidRotationQuasiStatic(Direction.kForward)); - autonChooser.addOption("SysID Rotation Translation Quasi Backwards", swerve.sysidRotationQuasiStatic(Direction.kReverse)); + // autonChooser.addOption("SysID Rotation Translation Dynamic Forwards", swerve.sysidRotationDynamic(Direction.kForward)); + // autonChooser.addOption("SysID Rotation Translation Dynamic Backwards", swerve.sysidRotationDynamic(Direction.kReverse)); + // autonChooser.addOption("SysID Rotation Translation Quasi Forwards", swerve.sysidRotationQuasiStatic(Direction.kForward)); + // autonChooser.addOption("SysID Rotation Translation Quasi Backwards", swerve.sysidRotationQuasiStatic(Direction.kReverse)); // autonChooser.addOption("SysID Turret Dynamic Forwards", turret.getSysIdRoutine().dynamic(Direction.kForward)); @@ -454,20 +454,20 @@ public void configureSysids() { // 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 Forwards", 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 spindexerSysId = spindexer.getSysIdRoutine(); + // autonChooser.addOption("SysID Spindexer Dynamic Forwards", 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)); - // Wheel Radius Characterization - autonChooser.addOption("Wheel Characterization", new SwerveWheelRadiusCharacterization()); + // // Wheel Radius Characterization + // autonChooser.addOption("Wheel Characterization", new SwerveWheelRadiusCharacterization()); - SysIdRoutine handoffSysId = handoff.getSysIdRoutine(); - autonChooser.addOption("SysID Handoff Dynamic Forward", handoffSysId.dynamic(Direction.kForward)); - autonChooser.addOption("SysID Handoff Dynamic Backwards", handoffSysId.dynamic(Direction.kReverse)); - autonChooser.addOption("SysID Handoff Quasi Forwards", handoffSysId.quasistatic(Direction.kForward)); - autonChooser.addOption("SysID Handoff Quasi Backwards", handoffSysId.quasistatic(Direction.kReverse)); + // SysIdRoutine handoffSysId = handoff.getSysIdRoutine(); + // autonChooser.addOption("SysID Handoff Dynamic Forward", handoffSysId.dynamic(Direction.kForward)); + // autonChooser.addOption("SysID Handoff Dynamic Backwards", handoffSysId.dynamic(Direction.kReverse)); + // autonChooser.addOption("SysID Handoff Quasi Forwards", handoffSysId.quasistatic(Direction.kForward)); + // autonChooser.addOption("SysID Handoff Quasi Backwards", handoffSysId.quasistatic(Direction.kReverse)); } public Command getAutonomousCommand() { diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java index 5f4239b3..08d1bbe9 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java @@ -29,13 +29,14 @@ public LeftOneCycle(PathPlannerPath... paths) { ), // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new IntakeStow() + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + new ParallelCommandGroup( + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) + // new SwerveClimbAlign() ), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new SpindexerRun().alongWith( new HandoffRun() - ).withTimeout(5.0) + ) // .until(() -> DriverStation.getMatchTime() < 2).andThen( // new ParallelCommandGroup( // new HandoffStop(), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java index cecea925..ad5c783e 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java @@ -28,9 +28,7 @@ public RightOneCycle(PathPlannerPath... paths) { ), // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new IntakeStow() - ), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), new ParallelCommandGroup( new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) // new SwerveClimbAlign() From 24d1ef42310240a200ab079e911576e4bb3c2a57 Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Sat, 7 Mar 2026 16:56:04 -0500 Subject: [PATCH 103/150] FIX: Uppdated intake widget --- simgui.json | 3 +++ .../deploy/pathplanner/paths/Left NZ To Score (P).path | 8 ++++---- src/main/deploy/pathplanner/paths/Left NZ To Score.path | 8 ++++---- .../deploy/pathplanner/paths/Left Score To NZ (P).path | 8 ++++---- src/main/deploy/pathplanner/paths/Left Score To NZ.path | 8 ++++---- .../paths/Left Trench Score To Tower Left.path | 8 ++++---- .../deploy/pathplanner/paths/Right NZ To Score (P).path | 8 ++++---- src/main/deploy/pathplanner/paths/Right NZ To Score.path | 8 ++++---- .../deploy/pathplanner/paths/Right Score To NZ (P).path | 8 ++++---- src/main/deploy/pathplanner/paths/Right Score To NZ.path | 8 ++++---- .../paths/Right Trench Score To Tower Right.path | 8 ++++---- src/main/deploy/pathplanner/settings.json | 2 +- 12 files changed, 44 insertions(+), 41 deletions(-) diff --git a/simgui.json b/simgui.json index 743d992a..c5d4adca 100644 --- a/simgui.json +++ b/simgui.json @@ -105,6 +105,9 @@ "Module 2": { "open": true }, + "Module 3": { + "open": true + }, "open": true }, "open": true diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score (P).path b/src/main/deploy/pathplanner/paths/Left NZ To Score (P).path index a4498a1c..c2a10be7 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score (P).path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score (P).path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 4.42896797153025, - "y": 7.3434519572953745 + "x": 4.375172004744959, + "y": 7.472562277580071 }, "prevControl": { - "x": 6.301067615658363, - "y": 7.408007117437722 + "x": 6.247271648873072, + "y": 7.537117437722419 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 99029798..333bf6db 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 4.42896797153025, - "y": 7.3434519572953745 + "x": 4.375172004744959, + "y": 7.472562277580071 }, "prevControl": { - "x": 6.4516963226571775, - "y": 7.429525504151838 + "x": 6.3979003558718865, + "y": 7.558635824436535 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ (P).path b/src/main/deploy/pathplanner/paths/Left Score To NZ (P).path index f9b2ab6f..69339aef 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ (P).path +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ (P).path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.42896797153025, - "y": 7.3434519572953745 + "x": 4.375172004744959, + "y": 7.472562277580071 }, "prevControl": null, "nextControl": { - "x": 9.02314353499407, - "y": 7.41876631079478 + "x": 8.969347568208779, + "y": 7.547876631079476 }, "isLocked": false, "linkedName": "Top Trench Score" diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ.path b/src/main/deploy/pathplanner/paths/Left Score To NZ.path index 16c1dba0..2d07f618 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.42896797153025, - "y": 7.3434519572953745 + "x": 4.375172004744959, + "y": 7.472562277580071 }, "prevControl": null, "nextControl": { - "x": 8.571257413997628, - "y": 7.408007117437722 + "x": 8.517461447212337, + "y": 7.537117437722419 }, "isLocked": false, "linkedName": "Top Trench Score" diff --git a/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path b/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path index 1517e1fa..d854b0cf 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path +++ b/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.42896797153025, - "y": 7.3434519572953745 + "x": 4.375172004744959, + "y": 7.472562277580071 }, "prevControl": null, "nextControl": { - "x": 2.051186239620404, - "y": 7.408007117437722 + "x": 1.997390272835113, + "y": 7.537117437722419 }, "isLocked": false, "linkedName": "Top Trench Score" diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score (P).path b/src/main/deploy/pathplanner/paths/Right NZ To Score (P).path index 897066ba..6c248072 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score (P).path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score (P).path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 4.063155397390274, - "y": 0.6942704626334513 + "x": 4.041637010676157, + "y": 0.6297153024911029 }, "prevControl": { - "x": 6.5915658362989324, - "y": 0.6727520759193351 + "x": 6.570047449584816, + "y": 0.6081969157769866 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index 0868d7f8..1a298b93 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 4.063155397390274, - "y": 0.6942704626334513 + "x": 4.041637010676157, + "y": 0.6297153024911029 }, "prevControl": { - "x": 6.085883748517201, - "y": 0.7911032028469743 + "x": 6.064365361803085, + "y": 0.7265480427046259 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (P).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (P).path index 8c025b28..aef6e723 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ (P).path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ (P).path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.063155397390274, - "y": 0.6942704626334513 + "x": 4.041637010676157, + "y": 0.6297153024911029 }, "prevControl": null, "nextControl": { - "x": 8.500752469823334, - "y": 0.7558624346889065 + "x": 8.479234083109219, + "y": 0.691307274546558 }, "isLocked": false, "linkedName": "Bottom Trench Score" diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ.path b/src/main/deploy/pathplanner/paths/Right Score To NZ.path index 5b1ed49a..29a4254b 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.063155397390274, - "y": 0.6942704626334513 + "x": 4.041637010676157, + "y": 0.6297153024911029 }, "prevControl": null, "nextControl": { - "x": 8.646571767497036, - "y": 0.8448991696322663 + "x": 8.62505338078292, + "y": 0.7803440094899179 }, "isLocked": false, "linkedName": "Bottom Trench Score" diff --git a/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path b/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path index 22344310..0759e52d 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path +++ b/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.063155397390274, - "y": 0.6942704626334513 + "x": 4.041637010676157, + "y": 0.6297153024911029 }, "prevControl": null, "nextControl": { - "x": 1.0123538933108782, - "y": 0.9064911416877192 + "x": 0.990835506596762, + "y": 0.8419359815453707 }, "isLocked": false, "linkedName": "Bottom Trench Score" diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 65131c44..6b970ef4 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -37,7 +37,7 @@ "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [ - "{\"name\":\"Intake\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.0,\"y\":-0.525},\"size\":{\"width\":0.3048,\"length\":0.98},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":true}}", + "{\"name\":\"Intake\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.0,\"y\":-0.475},\"size\":{\"width\":0.19,\"length\":0.98},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":true}}", "{\"name\":\"Turret\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.25,\"y\":0.175},\"size\":{\"width\":0.3,\"length\":0.35},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}", "{\"name\":\"Climber\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":-0.44,\"y\":0.1},\"size\":{\"width\":0.2,\"length\":0.1},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":true}}" ] From 76178b2cdcbf55d7ddf026707f46ebb69142a6da Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 7 Mar 2026 17:13:41 -0500 Subject: [PATCH 104/150] FEAT: alex told me to push and flipped me off so i pushed --- simgui-ds.json | 6 +++ simgui.json | 16 +++++--- src/main/java/com/stuypulse/robot/Robot.java | 14 +++++-- .../com/stuypulse/robot/RobotContainer.java | 39 +++++++++++++------ .../commands/intake/IntakeDigestion.java | 29 ++++++++++++++ .../commands/intake/IntakeDigestionDown.java | 9 +++++ .../commands/intake/IntakeDigestionup.java | 9 +++++ .../SuperstructureSOTMConditional.java | 2 +- .../commands/swerve/SwerveResetHeading.java | 5 +++ .../robot/commands/vision/SetIMUMode.java | 5 +++ .../robot/commands/vision/SetMegaTagMode.java | 6 +++ .../stuypulse/robot/constants/Cameras.java | 7 ++-- .../robot/constants/DriverConstants.java | 4 +- .../com/stuypulse/robot/constants/Field.java | 2 +- .../com/stuypulse/robot/constants/Gains.java | 2 +- .../com/stuypulse/robot/constants/Motors.java | 8 ++++ .../stuypulse/robot/constants/Settings.java | 21 +++++++--- .../robot/subsystems/intake/Intake.java | 4 +- .../robot/subsystems/intake/IntakeImpl.java | 12 ++++-- .../superstructure/Superstructure.java | 2 +- .../superstructure/shooter/ShooterImpl.java | 8 +++- .../superstructure/turret/Turret.java | 2 +- .../subsystems/vision/LimelightVision.java | 20 ++++++---- 23 files changed, 183 insertions(+), 49 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestion.java create mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestionDown.java create mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestionup.java diff --git a/simgui-ds.json b/simgui-ds.json index 20138e7d..b408e686 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,10 @@ { + "System Joysticks": { + "window": { + "enabled": false + } + }, + "disable": true, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/simgui.json b/simgui.json index 489fde89..d402b29b 100644 --- a/simgui.json +++ b/simgui.json @@ -22,7 +22,6 @@ "/SmartDashboard/Robot/Zero Pivot Encoder at Upper Limit (Stowed)": "Command", "/SmartDashboard/Robot/Zero Turret Encoders": "Command", "/SmartDashboard/Scheduler": "Scheduler", - "/SmartDashboard/Scheduler": "Scheduler", "/SmartDashboard/Visualizers/ClimberHopper": "Mechanism2d", "/SmartDashboard/Visualizers/Hood": "Mechanism2d", "/SmartDashboard/Visualizers/Turret": "Mechanism2d" @@ -39,6 +38,11 @@ "visible": true } }, + "/SmartDashboard/Scheduler": { + "window": { + "visible": true + } + }, "/SmartDashboard/Visualizers/Hood": { "window": { "visible": true @@ -72,13 +76,15 @@ }, "open": true }, + "Intake": { + "open": true + }, "Superstructure": { "Hood": { "open": true - }, - "Turret": { - "open": true - }, + } + }, + "Swerve": { "open": true }, "Turret": { diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index c31e43c7..dcdc2534 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -5,7 +5,9 @@ /***************************************************************/ package com.stuypulse.robot; +import com.stuypulse.robot.commands.vision.SetIMUMode; import com.stuypulse.robot.commands.vision.SetMegaTagMode; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.vision.LimelightVision; import com.stuypulse.robot.util.FMSUtil; @@ -25,7 +27,7 @@ public class Robot extends TimedRobot { private RobotContainer robot; private Command auto; private static Alliance alliance; - private PowerDistribution powerDistribution; + // private PowerDistribution powerDistribution; // private FMSUtil fmsUtil; @@ -40,12 +42,12 @@ public static boolean isBlue() { @Override public void robotInit() { robot = new RobotContainer(); - powerDistribution = new PowerDistribution(); + // powerDistribution = new PowerDistribution(); // fmsUtil = new FMSUtil(false); DataLogManager.start(); SignalLogger.start(); - + CommandScheduler.getInstance().schedule(new SetIMUMode(Settings.Vision.RESET_IMU_INDEX)); } @Override @@ -77,7 +79,9 @@ public void disabledInit() { } @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + CommandScheduler.getInstance().schedule(new SetIMUMode(Settings.Vision.RESET_IMU_INDEX)); + } /***********************/ /*** AUTONOMOUS MODE ***/ @@ -88,6 +92,7 @@ public void autonomousInit() { // fmsUtil.restartTimer(true); CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); + CommandScheduler.getInstance().schedule(new SetIMUMode(Settings.Vision.INTERNAL_EXTERNAL_ASSIST_INDEX)); auto = robot.getAutonomousCommand(); @@ -111,6 +116,7 @@ public void teleopInit() { // fmsUtil.restartTimer(false); CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); + CommandScheduler.getInstance().schedule(new SetIMUMode(Settings.Vision.INTERNAL_EXTERNAL_ASSIST_INDEX)); if (auto != null) { auto.cancel(); diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 61afefd9..80f0983c 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -99,7 +99,7 @@ public interface EnabledSubsystems { SmartBoolean SHOOTER = new SmartBoolean("Enabled Subsystems/Shooter Is Enabled", true); SmartBoolean BACK_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Back Limelight Is Enabled", true); - SmartBoolean LEFT_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Left Limelight Is Enabled", true); + SmartBoolean LEFT_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Left Limelight Is Enabled", false); SmartBoolean RIGHT_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Right Limelight Is Enabled", true); } @@ -161,6 +161,21 @@ private void configureDefaultCommands() { private void configureButtonBindings() { // Scoring Routine driver.getTopButton() + .whileTrue(new SwerveXMode()) + .onTrue(new IntakeRunRollers()) + .whileTrue(new SuperstructureShoot() + .andThen(new WaitUntilCommand(superstructure::atTolerance)) + .andThen(new HandoffRun()) + .andThen(new WaitUntilCommand(handoff::atTolerance)) + .andThen(new SpindexerRun())) + .onFalse(new SpindexerStop() + .alongWith(new SuperstructureStow()) + .alongWith(new HandoffStop())); + + // Interpolation-based Scoring + driver.getBottomButton() + .whileTrue(new SwerveXMode()) + .onTrue(new IntakeRunRollers()) .whileTrue(new SuperstructureInterpolation() .andThen(new WaitUntilCommand(superstructure::atTolerance)) .andThen(new HandoffRun()) @@ -189,15 +204,16 @@ private void configureButtonBindings() { .onTrue(new IntakeStopRollers()); // SOTM - // driver.getRightMenuButton() - // .whileTrue(new SuperstructureSOTM().onlyIf(() -> !swerve.isUnderTrench()) - // .andThen(new WaitUntilCommand(superstructure::atTolerance)) - // .andThen(new HandoffConditionalCommand().onlyIf(superstructure::atTolerance) - // .alongWith(new WaitUntilCommand(handoff::atTolerance)) - // .andThen(new SpindexerConditionalCommand().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) - // .onFalse(new SpindexerStop() - // .alongWith(new SuperstructureStow()) - // .alongWith(new HandoffStop())); + // should be right menu + driver.getRightButton() + .whileTrue(new SuperstructureSOTM().onlyIf(() -> !swerve.isUnderTrench()) + .andThen(new WaitUntilCommand(superstructure::atTolerance)) + .andThen(new HandoffConditionalCommand().onlyIf(superstructure::atTolerance) + .alongWith(new WaitUntilCommand(handoff::atTolerance)) + .andThen(new SpindexerConditionalCommand().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) + .onFalse(new SpindexerStop() + .alongWith(new SuperstructureStow()) + .alongWith(new HandoffStop())); // Scoring SOTM // driver.getRightMenuButton() @@ -212,7 +228,8 @@ private void configureButtonBindings() { // () -> superstructure.getState() == SuperstructureState.SOTM // )); - // driver.getDPadDown() + // digestion by moving the pivot + // driver.getDPadDown() // .whileTrue(new IntakeDigestion()) // .onFalse(new IntakeDeploy()); diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestion.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestion.java new file mode 100644 index 00000000..c2e89853 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestion.java @@ -0,0 +1,29 @@ +package com.stuypulse.robot.commands.intake; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.intake.Intake.PivotState; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import com.stuypulse.robot.commands.intake.IntakeDigestionup; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RepeatCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class IntakeDigestion extends RepeatCommand { + public IntakeDigestion() { + super( + new SequentialCommandGroup( + new IntakeDigestionup(), + new WaitUntilCommand(() -> Intake.getInstance().pivotAtTolerance()).withTimeout(0.4), + new WaitCommand(1.0), + new IntakeDigestionDown(), + new WaitUntilCommand(() -> Intake.getInstance().pivotAtTolerance()).withTimeout(0.4), + new WaitCommand(0.5) + ) + ); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestionDown.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestionDown.java new file mode 100644 index 00000000..66cbd528 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestionDown.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.intake; + +import com.stuypulse.robot.subsystems.intake.Intake.PivotState; + +public class IntakeDigestionDown extends IntakeSetState{ + public IntakeDigestionDown() { + super(PivotState.DIGESTION_DOWN); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestionup.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestionup.java new file mode 100644 index 00000000..e6fbd71f --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestionup.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.intake; + +import com.stuypulse.robot.subsystems.intake.Intake.PivotState; + +public class IntakeDigestionup extends IntakeSetState{ + public IntakeDigestionup(){ + super(PivotState.DIGESTION_UP); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTMConditional.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTMConditional.java index 62d90518..cf694fae 100644 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTMConditional.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTMConditional.java @@ -11,6 +11,6 @@ public class SuperstructureSOTMConditional extends ConditionalCommand { public SuperstructureSOTMConditional() { - super(new SuperstructureStow(), new SuperstructureSOTM(), () -> (Superstructure.getInstance().getState() == SuperstructureState.SOTM)); + super(new SuperstructureInterpolation(), new SuperstructureSOTM(), () -> (Superstructure.getInstance().getState() == SuperstructureState.SOTM)); } } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetHeading.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetHeading.java index 72a623d3..b2f10daa 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetHeading.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetHeading.java @@ -14,4 +14,9 @@ public class SwerveResetHeading extends InstantCommand { public SwerveResetHeading() { super(() -> CommandSwerveDrivetrain.getInstance().resetRotation(Rotation2d.kZero)); } + + // @Override + // public void initialize() { + // CommandSwerveDrivetrain.getInstance().getPigeon2().reset(); + // } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/vision/SetIMUMode.java b/src/main/java/com/stuypulse/robot/commands/vision/SetIMUMode.java index c979e0b6..80ec87bd 100644 --- a/src/main/java/com/stuypulse/robot/commands/vision/SetIMUMode.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/SetIMUMode.java @@ -23,4 +23,9 @@ public void initialize() { super.initialize(); vision.setIMUMode(index); } + + @Override + public boolean runsWhenDisabled() { + return true; + } } diff --git a/src/main/java/com/stuypulse/robot/commands/vision/SetMegaTagMode.java b/src/main/java/com/stuypulse/robot/commands/vision/SetMegaTagMode.java index 4e5d8412..9cec1728 100644 --- a/src/main/java/com/stuypulse/robot/commands/vision/SetMegaTagMode.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/SetMegaTagMode.java @@ -23,4 +23,10 @@ public void initialize() { super.initialize(); LimelightVision.getInstance().setMegaTagMode(megaTagMode); } + + + @Override + public boolean runsWhenDisabled() { + return true; + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 0bc1fa5d..c5b2b48d 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -12,20 +12,21 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Unit; /** This interface stores information about each camera. */ public interface Cameras { public Camera[] LimelightCameras = { - new Camera("limelight-right", - new Pose3d(-0.233, 0.375959, 0.20368, + new Camera("limelight-right", //.381, .2325, .2069592 + new Pose3d(Units.inchesToMeters(-14.962), Units.inchesToMeters(9.164), Units.inchesToMeters(8.118), //TODO: check this offset // new Pose3d(0.0,0.0,0.0, new Rotation3d(Units.degreesToRadians(180), Units.degreesToRadians(31.08), Units.degreesToRadians(-80))), RobotContainer.EnabledSubsystems.RIGHT_LIMELIGHT), new Camera("limelight-left", new Pose3d(Units.inchesToMeters(2.490), Units.inchesToMeters(14.8620), Units.inchesToMeters(7.920157), // new Pose3d(0.0,0.0,0.0, - new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15.0), Units.degreesToRadians(251.0))), + new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15.0), Units.degreesToRadians(289.0))), RobotContainer.EnabledSubsystems.LEFT_LIMELIGHT), new Camera("limelight-back", //11.0845u new Pose3d(Units.inchesToMeters(-10.768823), Units.inchesToMeters(-12.717519), Units.inchesToMeters(8.331714), diff --git a/src/main/java/com/stuypulse/robot/constants/DriverConstants.java b/src/main/java/com/stuypulse/robot/constants/DriverConstants.java index e2c9886d..603e25c0 100644 --- a/src/main/java/com/stuypulse/robot/constants/DriverConstants.java +++ b/src/main/java/com/stuypulse/robot/constants/DriverConstants.java @@ -12,8 +12,8 @@ public interface Driver { double BUZZ_INTENSITY = 1.0; public interface Drive { - double DEADBAND = 0.05; - double RC = 0.05; + double DEADBAND = 0.05; //TODO: tune to philip's request + double RC = 0.05; int POWER = 2; } public interface Turn { diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index acf6dd95..6f4beceb 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -31,7 +31,7 @@ public interface Field { double LENGTH = Units.inchesToMeters(651.200); public static final double trenchHopperTolerance = Units.inchesToMeters(50); - public static final double trenchHoodTolerance = Units.inchesToMeters(15); + public static final double trenchHoodTolerance = Units.inchesToMeters(10); // Alliance relative hub center coordinates public static final Pose2d hubCenter = new Pose2d(Units.inchesToMeters(182.11), WIDTH / 2.0, new Rotation2d()); diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 69fcf42c..7020d4ae 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -36,7 +36,7 @@ public interface Shooter { SmartNumber kD = new SmartNumber("Superstructure/Shooter/Gains/kD", 0.0); SmartNumber kS = new SmartNumber("Superstructure/Shooter/Gains/kS", 0.0); - SmartNumber kV = new SmartNumber("Superstructure/Shooter/Gains/kV", 0.123); + SmartNumber kV = new SmartNumber("Superstructure/Shooter/Gains/kV", 0.123);//0.11650); SmartNumber kA = new SmartNumber("Superstructure/Shooter/Gains/kA", 0.0); // double kP = 0.45; diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index e5cc39ab..941fab59 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -299,6 +299,14 @@ public TalonFXConfig withNeutralMode(NeutralModeValue neutralMode) { return this; } + public TalonFXConfig withVelocityTimeFilter(double filterInSeconds) { + feedbackConfigs.withVelocityFilterTimeConstant(filterInSeconds); + + configuration.withFeedback(feedbackConfigs); + + return this; + } + // RAMP RATE CONFIGS public TalonFXConfig withRampRate(double rampRate) { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 4748c8ae..3c43340a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -97,10 +97,13 @@ public interface Intake { Rotation2d PIVOT_STOW_ANGLE = Rotation2d.fromDegrees(90.0); Rotation2d PIVOT_DEPLOY_ANGLE = Rotation2d.fromDegrees(0.0); + Rotation2d DIGESTION_UP_ANGLE = Rotation2d.fromDegrees(70); // TODO: verify + Rotation2d DIGESTION_DOWN_ANGLE = Rotation2d.fromDegrees(20); + Rotation2d PIVOT_ANGLE_TOLERANCE = Rotation2d.fromDegrees(5.0); - Rotation2d PIVOT_MAX_ANGLE = Rotation2d.fromDegrees(90.0); - Rotation2d PIVOT_MIN_ANGLE = Rotation2d.fromDegrees(0.0); + Rotation2d PIVOT_MAX_ANGLE = Rotation2d.fromDegrees(88.0); + Rotation2d PIVOT_MIN_ANGLE = Rotation2d.fromDegrees(-10.0); Rotation2d PIVOT_MAX_VEL_DEPLOY = Rotation2d.fromDegrees(720.0); Rotation2d PIVOT_MAX_ACCEL_DEPLOY = Rotation2d.fromDegrees(1440.0); @@ -108,7 +111,7 @@ public interface Intake { Rotation2d PIVOT_MAX_VEL_STOW = Rotation2d.fromDegrees(360.0); Rotation2d PIVOT_MAX_ACCEL_STOW = Rotation2d.fromDegrees(600.0); - Rotation2d THRESHHOLD_TO_START_ROLLERS = Rotation2d.fromDegrees(5.0); + Rotation2d THRESHOLD_TO_START_ROLLERS = Rotation2d.fromDegrees(10.0); Rotation2d ARBITRARY_VOLTAGE_THRESHOLD = Rotation2d.fromDegrees(15.0); @@ -120,6 +123,8 @@ public interface Intake { SmartNumber debugDutyCycle = new SmartNumber("Intake/Debug Duty Cycle", 0.1); double STALL_CURRENT_LIMIT = 0; //TODO: set value double STALL_DEBOUNCE = 1.0; //TODO: VERIFY + double DIGESTION_DEBOUNCE = 0.2; //TODO: verify + double CURRENT_LIMIT = 45.0; } public interface Spindexer { @@ -137,12 +142,13 @@ public interface Superstructure { public final double SHOOTER_TOLERANCE_RPM = 100.0; public final Rotation2d HOOD_TOLERANCE = Rotation2d.fromDegrees(0.5); -public interface AngleInterpolation { + public interface AngleInterpolation { double[][] distanceAngleInterpolationValues = { {1.22, Units.degreesToRadians(20)}, //AGAINST THE HUB {1.43, Units.degreesToRadians(21.0)}, //meters, radians {2.15, Units.degreesToRadians(23.23)}, //KEVIN-APPROVED {2.864967, Units.degreesToRadians(27)}, //KEVIN-APPROVED + // {3.38, Units.degreesToRadians(37.5)}, {3.65, Units.degreesToRadians(28.0)}, //KEVIN-APPROVED {4.43, Units.degreesToRadians(33.5)}, //KEVIN-APPROVED {5.66, Units.degreesToRadians(39)} //KEVIN-APPROVED @@ -154,6 +160,7 @@ public interface RPMInterpolation{ {1.43, 3000.0}, // meters, RPM {2.15, 3050.0}, //KEVIN-APPROVED {2.864967, 3150}, //KEVIN-APPROVED + //{3.38, 3200} {3.65, 3400.0}, //KEVIN-APPROVED {4.43, 3600.0}, //KEVIN-APPROVED {5.66, 3900.0} //KEVIN-APPROVED @@ -277,7 +284,7 @@ public interface Angles { 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 Rotation2d TOLERANCE = Rotation2d.fromDegrees(5.0); // TODO: reduce to 2 degrees + public final Rotation2d TOLERANCE = Rotation2d.fromDegrees(2.0); public final Rotation2d KB = Rotation2d.fromDegrees(0.0); public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(0.0); @@ -334,7 +341,9 @@ public interface Swerve { public final double ROTATIONAL_DEADBAND_RAD_PER_S = 0.1; public interface Constraints { - public final double MAX_VELOCITY_M_PER_S = 4.93; // 4.3p + public final double MAX_VELOCITY_M_PER_S = 4.93; + public final double MAX_VELOCITY_SOTM_M_PER_S = 1.00; + 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_SQUARED = Units.degreesToRadians(900.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 20db72de..14623cbb 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -35,7 +35,9 @@ protected Intake() { public enum PivotState { DEPLOY(Settings.Intake.PIVOT_DEPLOY_ANGLE), - STOW(Settings.Intake.PIVOT_STOW_ANGLE); + STOW(Settings.Intake.PIVOT_STOW_ANGLE), + DIGESTION_UP(Settings.Intake.DIGESTION_UP_ANGLE), + DIGESTION_DOWN(Settings.Intake.DIGESTION_DOWN_ANGLE); private final Rotation2d targetAngle; 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 b85b1ee3..8a5cbdee 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -23,6 +23,7 @@ import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Gains.Intake.Pivot; import com.stuypulse.robot.util.SettableNumber; import com.stuypulse.robot.util.SysId; import com.stuypulse.stuylib.streams.booleans.BStream; @@ -75,7 +76,7 @@ public IntakeImpl() { .withInvertedValue(InvertedValue.CounterClockwise_Positive) .withNeutralMode(NeutralModeValue.Brake) - .withSupplyCurrentLimitAmps(45) + .withSupplyCurrentLimitAmps(Settings.Intake.CURRENT_LIMIT) .withStatorCurrentLimitEnabled(false) .withRampRate(0.50); @@ -155,22 +156,25 @@ public void zeroPivotDeployed() { @Override public void periodic() { super.periodic(); + PivotState state = getPivotState(); if (EnabledSubsystems.INTAKE.get()) { if (pivotVoltageOverride.isPresent()) { pivot.setVoltage(pivotVoltageOverride.get()); } else { // PIVOT - if (getPivotState() == PivotState.DEPLOY + if (state == PivotState.DEPLOY && getPivotAngle().getDegrees() <= Settings.Intake.ARBITRARY_VOLTAGE_THRESHOLD.getDegrees()) { pivot.setControl(new VoltageOut(-Settings.Intake.PUSHDOWN_VOLTAGE)); // applying 3 volts + } else if (state == PivotState.DIGESTION_DOWN || state == PivotState.DIGESTION_UP) { + pivot.setControl(new MotionMagicVoltage(state.getTargetAngle().getRotations())); //TODO: verify motion profile works } else { - pivot.setControl(new PositionVoltage(getPivotState().getTargetAngle().getRotations())); + pivot.setControl(new PositionVoltage(state.getTargetAngle().getRotations())); } // ROLLERS if (getPivotState() == PivotState.DEPLOY - && getPivotAngle().getDegrees() <= Settings.Intake.THRESHHOLD_TO_START_ROLLERS.getDegrees()) { + && getPivotAngle().getDegrees() <= Settings.Intake.THRESHOLD_TO_START_ROLLERS.getDegrees()) { rollerLeader.setControl(rollerController.withOutput(getRollerState().getTargetDutyCycle())); } else { rollerLeader.stopMotor(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index f7c4a18e..2cd7307d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -91,7 +91,7 @@ public SuperstructureState getState(){ } public boolean atTolerance() { - return isShooterAtTolerance() && isHoodAtTolerance();// && isTurretAtTolerance(); + return isShooterAtTolerance() && isHoodAtTolerance() && isTurretAtTolerance(); } // public boolean isHoodUnderTrench() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java index bb3c7626..14293ca6 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -15,8 +15,10 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.MotorAlignmentValue; @@ -45,10 +47,14 @@ public ShooterImpl() { .withPIDConstants(Gains.Superstructure.Shooter.kP.get(), Gains.Superstructure.Shooter.kI.get(), Gains.Superstructure.Shooter.kD.get(), 0) .withFFConstants(Gains.Superstructure.Shooter.kS.get(), Gains.Superstructure.Shooter.kV.get(), Gains.Superstructure.Shooter.kA.get(), 0) - .withSensorToMechanismRatio(Settings.Superstructure.Shooter.GEAR_RATIO); + .withSensorToMechanismRatio(Settings.Superstructure.Shooter.GEAR_RATIO) + .withVelocityTimeFilter(0.1); shooterLeader = new TalonFX(Ports.Superstructure.Shooter.MOTOR_LEAD, Ports.RIO); + shooterLeader.getVelocity().setUpdateFrequency(1000.0); + shooterFollower = new TalonFX(Ports.Superstructure.Shooter.MOTOR_FOLLOW, Ports.RIO); + shooterFollower.getVelocity().setUpdateFrequency(1000.0); shooterConfig.configure(shooterLeader); shooterConfig.configure(shooterFollower); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java index 13f4f79e..8a69198c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java @@ -39,7 +39,7 @@ public static Turret getInstance() { public Turret() { driverInput = Vector2D.kOrigin; - state = TurretState.IDLE; + state = TurretState.SHOOT; } public void setDriverInput(Gamepad gamepad) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 8bd27e30..9f67fba1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -162,8 +162,12 @@ public void periodic() { if (poseEstimate != null && poseEstimate.tagCount > 0) { Pose2d robotPose = poseEstimate.pose; double timestamp = poseEstimate.timestampSeconds; - - CommandSwerveDrivetrain.getInstance().addVisionMeasurement(robotPose, timestamp, Settings.Vision.MT1_STDEVS); + + if (megaTagMode == MegaTagMode.MEGATAG1) { + CommandSwerveDrivetrain.getInstance().addVisionMeasurement(robotPose, timestamp, Settings.Vision.MT1_STDEVS); + } else { + CommandSwerveDrivetrain.getInstance().addVisionMeasurement(robotPose, timestamp, Settings.Vision.MT2_STDEVS); + } SmartDashboard.putNumber("Vision/Pose X Component", robotPose.getX()); SmartDashboard.putNumber("Vision/Pose Y Component", robotPose.getY()); @@ -174,13 +178,15 @@ public void periodic() { SmartDashboard.putBoolean("Vision/" + names[i] + " Has Data", false); } - SmartDashboard.putString("Vision/MegaTag Mode", megaTagMode.toString()); + + } + String limelightName = names[i]; + SmartDashboard.putString("Vision/MegaTag Mode", megaTagMode.toString()); // this yaw is seems to be the robot yaw passed into the LL - SmartDashboard.putNumber("Vision/Limelight Robot Yaw", LimelightHelpers.getIMUData(limelightName).robotYaw); + SmartDashboard.putNumber("Vision/Limelight Robot Yaw " + limelightName, LimelightHelpers.getIMUData(limelightName).robotYaw); // this is just the yaw of the internal imu - SmartDashboard.putNumber("Vision/Limelight Yaw", LimelightHelpers.getIMUData(limelightName).Yaw); - - } + SmartDashboard.putNumber("Vision/Limelight Yaw " + limelightName, LimelightHelpers.getIMUData(limelightName).Yaw); + SmartDashboard.putNumber("Vision/Limelight Robot Yaw Passed in", (CommandSwerveDrivetrain.getInstance().getPose().getRotation().getDegrees() + (Robot.isBlue() ? 0 : 180)) % 360); } } } From dda4be36586cd560346cc13c866fb66c46680ed1 Mon Sep 17 00:00:00 2001 From: Alex Wang Date: Sat, 7 Mar 2026 17:34:39 -0500 Subject: [PATCH 105/150] feat: add logging for the limelight poses to advantagescope --- .../subsystems/vision/LimelightVision.java | 21 ++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 9f67fba1..c9f1cadc 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -17,6 +17,8 @@ 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.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -36,6 +38,8 @@ public static LimelightVision getInstance() { private SmartBoolean enabled; // private SmartBoolean[] camerasEnabled; private MegaTagMode megaTagMode; + private Pose2d[] arrayOfLimelightPoses; + private final StructArrayPublisher arrayPublisher; public enum MegaTagMode { MEGATAG1, @@ -43,7 +47,12 @@ public enum MegaTagMode { } public LimelightVision() { + arrayPublisher = NetworkTableInstance.getDefault() + .getStructArrayTopic("MyPoseArray", Pose2d.struct).publish(); + + arrayOfLimelightPoses = new Pose2d[Cameras.LimelightCameras.length]; names = new String[Cameras.LimelightCameras.length]; + for (int i = 0; i < Cameras.LimelightCameras.length; i++) { names[i] = Cameras.LimelightCameras[i].getName(); Pose3d robotRelativePose = Cameras.LimelightCameras[i].getLocation(); @@ -56,6 +65,8 @@ public LimelightVision() { Rotation2d.fromRadians(robotRelativePose.getRotation().getY()).getDegrees(), Rotation2d.fromRadians(robotRelativePose.getRotation().getZ()).getDegrees() ); + + arrayOfLimelightPoses[i] = new Pose2d(); } // camerasEnabled = new SmartBoolean[Cameras.LimelightCameras.length]; @@ -168,6 +179,8 @@ public void periodic() { } else { CommandSwerveDrivetrain.getInstance().addVisionMeasurement(robotPose, timestamp, Settings.Vision.MT2_STDEVS); } + + arrayOfLimelightPoses[i] = robotPose; SmartDashboard.putNumber("Vision/Pose X Component", robotPose.getX()); SmartDashboard.putNumber("Vision/Pose Y Component", robotPose.getY()); @@ -178,7 +191,12 @@ public void periodic() { SmartDashboard.putBoolean("Vision/" + names[i] + " Has Data", false); } - + SmartDashboard.putString("Vision/MegaTag Mode", megaTagMode.toString()); + // this yaw is seems to be the robot yaw passed into the LL + SmartDashboard.putNumber("Vision/Limelight Robot Yaw", LimelightHelpers.getIMUData(limelightName).robotYaw); + // this is just the yaw of the internal imu + SmartDashboard.putNumber("Vision/Limelight Yaw", LimelightHelpers.getIMUData(limelightName).Yaw); + } String limelightName = names[i]; SmartDashboard.putString("Vision/MegaTag Mode", megaTagMode.toString()); @@ -188,6 +206,7 @@ public void periodic() { SmartDashboard.putNumber("Vision/Limelight Yaw " + limelightName, LimelightHelpers.getIMUData(limelightName).Yaw); SmartDashboard.putNumber("Vision/Limelight Robot Yaw Passed in", (CommandSwerveDrivetrain.getInstance().getPose().getRotation().getDegrees() + (Robot.isBlue() ? 0 : 180)) % 360); } + arrayPublisher.set(arrayOfLimelightPoses); } } } \ No newline at end of file From 5c2229e0897618f8235fbcc3bc9e0b9edbd67fd0 Mon Sep 17 00:00:00 2001 From: DanTheMan95 <81121522+Danx3mer@users.noreply.github.com> Date: Sat, 7 Mar 2026 17:36:02 -0500 Subject: [PATCH 106/150] feat: add ferry interpolation starting values --- .../stuypulse/robot/constants/Settings.java | 26 ++++++++++++++----- .../InterpolationCalculator.java | 2 +- 2 files changed, 20 insertions(+), 8 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 3c43340a..e52a967a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -154,6 +154,7 @@ public interface AngleInterpolation { {5.66, Units.degreesToRadians(39)} //KEVIN-APPROVED }; } + public interface RPMInterpolation{ double[][] distanceRPMInterpolationValues = { {1.22, 2950.0}, //KEVIN-APPROVED @@ -168,7 +169,6 @@ public interface RPMInterpolation{ } public interface TOFInterpolation{ - double[][] distanceTOFInterpolationValues = { {1.22, 1.115}, // seconds // {1.43,}, @@ -180,6 +180,24 @@ public interface TOFInterpolation{ }; } + public interface FerryRPMInterpolation { + double[][] ferryDistanceRPMInterpolation = { + {5.16, 3800.0}, + {6.94, 4050.0}, + {7.87, 4300.0}, + {9.77, 4550.0} + }; + } + + public interface FerryTOFInterpolation { + double [][] FerryTOFInterpolationInterpolation = { + // {5.16, }, + // {6.94, }, + // {7.87, }, + // {9.77, }, + }; + } + // public interface AngleInterpolation { // public final double[][] distanceAngleInterpolationValues = { // {1.22, Units.degreesToRadians(20)}, //AGAINST THE HUB @@ -221,12 +239,6 @@ public interface TOFInterpolation{ // }; // } - 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); diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java index b22866ed..b3960495 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java @@ -45,7 +45,7 @@ public record InterpolatedShotInfo( } ferryingDistanceRPMInterpolator = new InterpolatingDoubleTreeMap(); - for(double[] pair: FerryRPMInterpolation.distanceRPMInterpolationValues) { + for(double[] pair: FerryRPMInterpolation.ferryDistanceRPMInterpolation) { ferryingDistanceRPMInterpolator.put(pair[0], pair[1]); } } From 143676e22da4e5063b4b6e7b0e9304aef3697230 Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Sat, 7 Mar 2026 18:44:42 -0500 Subject: [PATCH 107/150] FIX: better auto sweeping and logic --- .../pathplanner/autos/Left 1 Cycle (P).auto | 25 ---- .../pathplanner/autos/Left 1 Cycle.auto | 2 +- ...Cycle (P).auto => Left 2 Cycle Climb.auto} | 12 +- .../pathplanner/autos/Left 2 Cycle.auto | 8 +- .../pathplanner/autos/Right 1 Cycle (P).auto | 4 +- .../pathplanner/autos/Right 2 Cycle (P).auto | 37 ----- ... 1 Cycle.auto => Right 2 Cycle Climb.auto} | 8 +- .../pathplanner/autos/Right 2 Cycle.auto | 8 +- .../paths/Depot To Tower Left.path | 4 +- .../pathplanner/paths/Left Bump To Depot.path | 2 +- .../paths/Left NZ To Score (P).path | 79 ----------- .../pathplanner/paths/Left NZ To Score.path | 51 ++----- .../paths/Left NZ To Tower Left (P).path | 99 ------------- .../paths/Left NZ To Tower Left.path | 60 +++----- .../paths/Left Score Score Tower.path | 131 ++++++++++++++++++ .../paths/Left Score To NZ (P).path | 63 --------- .../pathplanner/paths/Left Score To NZ.path | 63 --------- .../paths/Left Score To Score.path | 111 +++++++++++++++ .../Left Trench Score To Tower Left.path | 18 +-- .../paths/Left Trench To NZ (P).path | 54 -------- .../pathplanner/paths/Left Trench To NZ.path | 24 ++-- .../paths/Right NZ To Score (P).path | 79 ----------- .../pathplanner/paths/Right NZ To Score.path | 51 ++----- .../paths/Right NZ To Tower Right (P).path | 99 ------------- .../paths/Right NZ To Tower Right.path | 58 +++----- .../paths/Right Score Score Tower.path | 131 ++++++++++++++++++ .../paths/Right Score To NZ (P).path | 63 --------- .../pathplanner/paths/Right Score To NZ.path | 63 --------- .../paths/Right Score To Score.path | 111 +++++++++++++++ .../Right Trench Score To Tower Right.path | 20 +-- .../paths/Right Trench To NZ (P).path | 54 -------- .../pathplanner/paths/Right Trench To NZ.path | 24 ++-- src/main/deploy/pathplanner/settings.json | 26 ++-- .../com/stuypulse/robot/RobotContainer.java | 57 +++----- .../auton/poaching/LeftOneCyclePoach.java | 54 -------- .../auton/poaching/LeftTwoCyclePoach.java | 72 ---------- .../auton/poaching/RightOneCyclePoach.java | 54 -------- .../auton/poaching/RightTwoCyclePoach.java | 73 ---------- .../commands/auton/regular/EightFuel.java | 12 +- .../commands/auton/regular/LeftOneCycle.java | 2 + .../commands/auton/regular/LeftTwoCycle.java | 16 +-- .../commands/auton/regular/RightOneCycle.java | 2 + .../commands/auton/regular/RightTwoCycle.java | 16 +-- .../robot/commands/auton/test/BoxTest.java | 3 +- 44 files changed, 660 insertions(+), 1343 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/Left 1 Cycle (P).auto rename src/main/deploy/pathplanner/autos/{Left 2 Cycle (P).auto => Left 2 Cycle Climb.auto} (61%) delete mode 100644 src/main/deploy/pathplanner/autos/Right 2 Cycle (P).auto rename src/main/deploy/pathplanner/autos/{Right 1 Cycle.auto => Right 2 Cycle Climb.auto} (68%) delete mode 100644 src/main/deploy/pathplanner/paths/Left NZ To Score (P).path delete mode 100644 src/main/deploy/pathplanner/paths/Left NZ To Tower Left (P).path create mode 100644 src/main/deploy/pathplanner/paths/Left Score Score Tower.path delete mode 100644 src/main/deploy/pathplanner/paths/Left Score To NZ (P).path delete mode 100644 src/main/deploy/pathplanner/paths/Left Score To NZ.path create mode 100644 src/main/deploy/pathplanner/paths/Left Score To Score.path delete mode 100644 src/main/deploy/pathplanner/paths/Left Trench To NZ (P).path delete mode 100644 src/main/deploy/pathplanner/paths/Right NZ To Score (P).path delete mode 100644 src/main/deploy/pathplanner/paths/Right NZ To Tower Right (P).path create mode 100644 src/main/deploy/pathplanner/paths/Right Score Score Tower.path delete mode 100644 src/main/deploy/pathplanner/paths/Right Score To NZ (P).path delete mode 100644 src/main/deploy/pathplanner/paths/Right Score To NZ.path create mode 100644 src/main/deploy/pathplanner/paths/Right Score To Score.path delete mode 100644 src/main/deploy/pathplanner/paths/Right Trench To NZ (P).path delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/poaching/LeftOneCyclePoach.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/poaching/LeftTwoCyclePoach.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/poaching/RightOneCyclePoach.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/poaching/RightTwoCyclePoach.java diff --git a/src/main/deploy/pathplanner/autos/Left 1 Cycle (P).auto b/src/main/deploy/pathplanner/autos/Left 1 Cycle (P).auto deleted file mode 100644 index aeb62f24..00000000 --- a/src/main/deploy/pathplanner/autos/Left 1 Cycle (P).auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Left Trench To NZ (P)" - } - }, - { - "type": "path", - "data": { - "pathName": "Left NZ To Tower Left (P)" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 1 Cycle.auto b/src/main/deploy/pathplanner/autos/Left 1 Cycle.auto index 95448c32..689d4d90 100644 --- a/src/main/deploy/pathplanner/autos/Left 1 Cycle.auto +++ b/src/main/deploy/pathplanner/autos/Left 1 Cycle.auto @@ -13,7 +13,7 @@ { "type": "path", "data": { - "pathName": "Left NZ To Tower Left" + "pathName": "Left NZ To Score" } } ] diff --git a/src/main/deploy/pathplanner/autos/Left 2 Cycle (P).auto b/src/main/deploy/pathplanner/autos/Left 2 Cycle Climb.auto similarity index 61% rename from src/main/deploy/pathplanner/autos/Left 2 Cycle (P).auto rename to src/main/deploy/pathplanner/autos/Left 2 Cycle Climb.auto index e6b9622e..e437961e 100644 --- a/src/main/deploy/pathplanner/autos/Left 2 Cycle (P).auto +++ b/src/main/deploy/pathplanner/autos/Left 2 Cycle Climb.auto @@ -7,25 +7,19 @@ { "type": "path", "data": { - "pathName": "Left Trench To NZ (P)" + "pathName": "Left Trench To NZ" } }, { "type": "path", "data": { - "pathName": "Left NZ To Score (P)" + "pathName": "Left NZ To Score" } }, { "type": "path", "data": { - "pathName": "Left Score To NZ" - } - }, - { - "type": "path", - "data": { - "pathName": "Left NZ To Tower Left" + "pathName": "Left Score Score Tower" } } ] diff --git a/src/main/deploy/pathplanner/autos/Left 2 Cycle.auto b/src/main/deploy/pathplanner/autos/Left 2 Cycle.auto index a1ede983..b666c1c2 100644 --- a/src/main/deploy/pathplanner/autos/Left 2 Cycle.auto +++ b/src/main/deploy/pathplanner/autos/Left 2 Cycle.auto @@ -19,13 +19,7 @@ { "type": "path", "data": { - "pathName": "Left Score To NZ" - } - }, - { - "type": "path", - "data": { - "pathName": "Left NZ To Tower Left" + "pathName": "Left Score To Score" } } ] diff --git a/src/main/deploy/pathplanner/autos/Right 1 Cycle (P).auto b/src/main/deploy/pathplanner/autos/Right 1 Cycle (P).auto index 5a6c524b..0592ec02 100644 --- a/src/main/deploy/pathplanner/autos/Right 1 Cycle (P).auto +++ b/src/main/deploy/pathplanner/autos/Right 1 Cycle (P).auto @@ -7,13 +7,13 @@ { "type": "path", "data": { - "pathName": "Right Trench To NZ (P)" + "pathName": "Right Trench To NZ" } }, { "type": "path", "data": { - "pathName": "Right NZ To Tower Right (P)" + "pathName": "Right NZ To Score" } } ] diff --git a/src/main/deploy/pathplanner/autos/Right 2 Cycle (P).auto b/src/main/deploy/pathplanner/autos/Right 2 Cycle (P).auto deleted file mode 100644 index ffa28f3d..00000000 --- a/src/main/deploy/pathplanner/autos/Right 2 Cycle (P).auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Right Trench To NZ (P)" - } - }, - { - "type": "path", - "data": { - "pathName": "Right NZ To Score (P)" - } - }, - { - "type": "path", - "data": { - "pathName": "Right Score To NZ" - } - }, - { - "type": "path", - "data": { - "pathName": "Right NZ To Tower Right" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 1 Cycle.auto b/src/main/deploy/pathplanner/autos/Right 2 Cycle Climb.auto similarity index 68% rename from src/main/deploy/pathplanner/autos/Right 1 Cycle.auto rename to src/main/deploy/pathplanner/autos/Right 2 Cycle Climb.auto index 4e27649a..aef61bda 100644 --- a/src/main/deploy/pathplanner/autos/Right 1 Cycle.auto +++ b/src/main/deploy/pathplanner/autos/Right 2 Cycle Climb.auto @@ -13,7 +13,13 @@ { "type": "path", "data": { - "pathName": "Right NZ To Tower Right" + "pathName": "Right NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score Score Tower" } } ] diff --git a/src/main/deploy/pathplanner/autos/Right 2 Cycle.auto b/src/main/deploy/pathplanner/autos/Right 2 Cycle.auto index fdbfe73e..1a967da4 100644 --- a/src/main/deploy/pathplanner/autos/Right 2 Cycle.auto +++ b/src/main/deploy/pathplanner/autos/Right 2 Cycle.auto @@ -19,13 +19,7 @@ { "type": "path", "data": { - "pathName": "Right Score To NZ" - } - }, - { - "type": "path", - "data": { - "pathName": "Right NZ To Tower Right" + "pathName": "Right Score To Score" } } ] diff --git a/src/main/deploy/pathplanner/paths/Depot To Tower Left.path b/src/main/deploy/pathplanner/paths/Depot To Tower Left.path index 9e8ba71d..afda93a2 100644 --- a/src/main/deploy/pathplanner/paths/Depot To Tower Left.path +++ b/src/main/deploy/pathplanner/paths/Depot To Tower Left.path @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "Tower Left 1" + "linkedName": "Tower Left" } ], "rotationTargets": [ @@ -47,7 +47,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 90.0 + "rotation": 0.0 }, "reversed": false, "folder": "Misc", diff --git a/src/main/deploy/pathplanner/paths/Left Bump To Depot.path b/src/main/deploy/pathplanner/paths/Left Bump To Depot.path index 65b1e461..9ed4afa7 100644 --- a/src/main/deploy/pathplanner/paths/Left Bump To Depot.path +++ b/src/main/deploy/pathplanner/paths/Left Bump To Depot.path @@ -12,7 +12,7 @@ "y": 5.92251805970181 }, "isLocked": false, - "linkedName": "Top Bump Start" + "linkedName": "Left Bump Start" }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score (P).path b/src/main/deploy/pathplanner/paths/Left NZ To Score (P).path deleted file mode 100644 index c2a10be7..00000000 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score (P).path +++ /dev/null @@ -1,79 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 8.259240806642943, - "y": 4.43846975088968 - }, - "prevControl": null, - "nextControl": { - "x": 6.44093712930012, - "y": 4.459988137603796 - }, - "isLocked": false, - "linkedName": "NZ Top Center (P)" - }, - { - "anchor": { - "x": 6.182716488730724, - "y": 6.1814590747330955 - }, - "prevControl": { - "x": 6.167817715485877, - "y": 4.542594017799959 - }, - "nextControl": { - "x": 6.193475682087783, - "y": 7.36497034400949 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.375172004744959, - "y": 7.472562277580071 - }, - "prevControl": { - "x": 6.247271648873072, - "y": 7.537117437722419 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Top Trench Score" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.18547140649150043, - "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 0.6491499227202471, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": true - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "To Score", - "idealStartingState": { - "velocity": 5.75, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 333bf6db..3aa7f9af 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -3,57 +3,32 @@ "waypoints": [ { "anchor": { - "x": 7.785836298932384, - "y": 4.4169513641755636 + "x": 8.259240806642943, + "y": 4.43846975088968 }, "prevControl": null, "nextControl": { - "x": 6.15043890865955, - "y": 4.459988137603796 + "x": 6.376381969157771, + "y": 7.612431791221827 }, "isLocked": false, - "linkedName": "NZ Top Center" + "linkedName": "NZ Left Center" }, { "anchor": { - "x": 6.118161328588376, - "y": 5.98779359430605 + "x": 4.020118623962041, + "y": 7.590913404507711 }, "prevControl": { - "x": 6.070175108111238, - "y": 4.692165641423325 - }, - "nextControl": { - "x": 6.171957295373667, - "y": 7.440284697508897 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.375172004744959, - "y": 7.472562277580071 - }, - "prevControl": { - "x": 6.3979003558718865, - "y": 7.558635824436535 + "x": 6.462455516014234, + "y": 7.6985053380782915 }, "nextControl": null, "isLocked": false, - "linkedName": "Top Trench Score" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.36166924265842487, - "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 0.856259659969088, - "rotationDegrees": 180.0 + "linkedName": "Left Trench Score" } ], + "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -67,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": -90.0 }, "reversed": false, "folder": "To Score", "idealStartingState": { "velocity": 5.75, - "rotation": 0.0 + "rotation": -90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Tower Left (P).path b/src/main/deploy/pathplanner/paths/Left NZ To Tower Left (P).path deleted file mode 100644 index c6b36dc9..00000000 --- a/src/main/deploy/pathplanner/paths/Left NZ To Tower Left (P).path +++ /dev/null @@ -1,99 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 8.259240806642943, - "y": 4.43846975088968 - }, - "prevControl": null, - "nextControl": { - "x": 6.236512455516015, - "y": 4.739727164887308 - }, - "isLocked": false, - "linkedName": "NZ Top Center (P)" - }, - { - "anchor": { - "x": 6.2042348754448415, - "y": 5.761850533807828 - }, - "prevControl": { - "x": 6.1734013254892925, - "y": 4.986606992068286 - }, - "nextControl": { - "x": 6.279549228944247, - "y": 7.655468564650059 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.644151838671412, - "y": 7.397247924080664 - }, - "prevControl": { - "x": 6.3752449602436325, - "y": 7.32115591873683 - }, - "nextControl": { - "x": 2.6859786476868335, - "y": 7.4833214709371285 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.2227283511269282, - "y": 4.922633451957295 - }, - "prevControl": { - "x": 2.2018149466192183, - "y": 7.268137603795967 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Tower Left 1" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.17398119122256722, - "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 0.3714733542319881, - "rotationDegrees": 180.0 - }, - { - "waypointRelativePos": 2.2581143740340126, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": true - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": "To Tower", - "idealStartingState": { - "velocity": 5.75, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path b/src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path index 084f1ff7..7627554b 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path @@ -3,45 +3,29 @@ "waypoints": [ { "anchor": { - "x": 7.785836298932384, - "y": 4.4169513641755636 + "x": 8.259240806642943, + "y": 4.43846975088968 }, "prevControl": null, "nextControl": { - "x": 6.139679715302492, - "y": 4.38467378410439 + "x": 6.161198102016607, + "y": 7.515599051008303 }, "isLocked": false, - "linkedName": "NZ Top Center" + "linkedName": "NZ Left Center" }, { "anchor": { - "x": 6.2903084223013055, - "y": 6.256773428232503 + "x": 4.633392645314355, + "y": 7.655468564650059 }, "prevControl": { - "x": 6.253236331698933, - "y": 5.493088361823635 + "x": 5.92449584816133, + "y": 7.655468564650059 }, "nextControl": { - "x": 6.3441043890865965, - "y": 7.36497034400949 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.676429418742587, - "y": 7.408007117437722 - }, - "prevControl": { - "x": 6.279549228944247, - "y": 7.397247924080664 - }, - "nextControl": { - "x": 2.7718215695294193, - "y": 7.420789720452577 + "x": 2.6733286409106265, + "y": 7.655468564650059 }, "isLocked": false, "linkedName": null @@ -52,26 +36,18 @@ "y": 4.922633451957295 }, "prevControl": { - "x": 2.5461091340450777, - "y": 7.343451957295374 + "x": 2.2018149466192183, + "y": 7.268137603795967 }, "nextControl": null, "isLocked": false, - "linkedName": "Tower Left 1" + "linkedName": "Tower Left" } ], "rotationTargets": [ { - "waypointRelativePos": 0.27742946708464067, - "rotationDegrees": 90.0 - }, - { - "waypointRelativePos": 0.5266457680250779, - "rotationDegrees": 180.0 - }, - { - "waypointRelativePos": 2.24884080370944, - "rotationDegrees": 180.0 + "waypointRelativePos": 1.2581143740340126, + "rotationDegrees": -90.0 } ], "constraintZones": [], @@ -87,13 +63,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 90.0 + "rotation": 0.0 }, "reversed": false, "folder": "To Tower", "idealStartingState": { "velocity": 5.75, - "rotation": 0.0 + "rotation": -90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Score Score Tower.path b/src/main/deploy/pathplanner/paths/Left Score Score Tower.path new file mode 100644 index 00000000..ce5994d6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Score Score Tower.path @@ -0,0 +1,131 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.020118623962041, + "y": 7.590913404507711 + }, + "prevControl": null, + "nextControl": { + "x": 6.376381969162431, + "y": 7.5909134045129925 + }, + "isLocked": false, + "linkedName": "Left Trench Score" + }, + { + "anchor": { + "x": 5.924495848161329, + "y": 6.3858837485172 + }, + "prevControl": { + "x": 5.981334656466404, + "y": 7.488556629635666 + }, + "nextControl": { + "x": 5.870699881376039, + "y": 5.3422419928825615 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.483973902728352, + "y": 4.4707473309608545 + }, + "prevControl": { + "x": 5.84048778435412, + "y": 4.1030409776021175 + }, + "nextControl": { + "x": 7.312431791226488, + "y": 4.944151838676692 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.73143534994069, + "y": 7.408007117437722 + }, + "prevControl": { + "x": 7.011174377219542, + "y": 7.41876631079478 + }, + "nextControl": { + "x": 4.795823194932401, + "y": 7.333560496090013 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.558078291814947, + "y": 7.408007117437722 + }, + "prevControl": { + "x": 5.354258600237249, + "y": 7.472562277580071 + }, + "nextControl": { + "x": 3.6566879283896982, + "y": 7.3349214122951345 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2227283511269282, + "y": 4.922633451957295 + }, + "prevControl": { + "x": 1.7714472123368927, + "y": 7.031435349940688 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Tower Left" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.4623824451410663, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.2238010657193716, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 4.440497335701599, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 5.75, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Tower", + "idealStartingState": { + "velocity": 5.75, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ (P).path b/src/main/deploy/pathplanner/paths/Left Score To NZ (P).path deleted file mode 100644 index 69339aef..00000000 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ (P).path +++ /dev/null @@ -1,63 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.375172004744959, - "y": 7.472562277580071 - }, - "prevControl": null, - "nextControl": { - "x": 8.969347568208779, - "y": 7.547876631079476 - }, - "isLocked": false, - "linkedName": "Top Trench Score" - }, - { - "anchor": { - "x": 8.259240806642943, - "y": 4.43846975088968 - }, - "prevControl": { - "x": 8.259240806642943, - "y": 7.590913404507711 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "NZ Top Center (P)" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.30407523510971435, - "rotationDegrees": 180.0 - }, - { - "waypointRelativePos": 0.47335423197492277, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": true - }, - "goalEndState": { - "velocity": 5.75, - "rotation": 0.0 - }, - "reversed": false, - "folder": "To NZ", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ.path b/src/main/deploy/pathplanner/paths/Left Score To NZ.path deleted file mode 100644 index 2d07f618..00000000 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ.path +++ /dev/null @@ -1,63 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.375172004744959, - "y": 7.472562277580071 - }, - "prevControl": null, - "nextControl": { - "x": 8.517461447212337, - "y": 7.537117437722419 - }, - "isLocked": false, - "linkedName": "Top Trench Score" - }, - { - "anchor": { - "x": 7.785836298932384, - "y": 4.4169513641755636 - }, - "prevControl": { - "x": 7.763125401827354, - "y": 7.670407928336639 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "NZ Top Center" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.32288401253918425, - "rotationDegrees": 180.0 - }, - { - "waypointRelativePos": 0.5, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": true - }, - "goalEndState": { - "velocity": 5.75, - "rotation": 0.0 - }, - "reversed": false, - "folder": "To NZ", - "idealStartingState": { - "velocity": 5.75, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path new file mode 100644 index 00000000..673e46b3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -0,0 +1,111 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.020118623962041, + "y": 7.590913404507711 + }, + "prevControl": null, + "nextControl": { + "x": 6.376381969162431, + "y": 7.5909134045129925 + }, + "isLocked": false, + "linkedName": "Left Trench Score" + }, + { + "anchor": { + "x": 5.924495848161329, + "y": 6.3858837485172 + }, + "prevControl": { + "x": 5.981334656466404, + "y": 7.488556629635666 + }, + "nextControl": { + "x": 5.870699881376039, + "y": 5.3422419928825615 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.483973902728352, + "y": 4.4707473309608545 + }, + "prevControl": { + "x": 5.84048778435412, + "y": 4.1030409776021175 + }, + "nextControl": { + "x": 7.312431791226488, + "y": 4.944151838676692 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.83902728351593, + "y": 7.408007117437722 + }, + "prevControl": { + "x": 7.215599051012967, + "y": 7.386488730728886 + }, + "nextControl": { + "x": 4.90513878751039, + "y": 7.518515031468064 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.472004744958482, + "y": 7.408007117437722 + }, + "prevControl": { + "x": 5.913736654808932, + "y": 7.42952550415712 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Center NZ (2)" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.4623824451410663, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.2238010657193716, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 5.75, + "rotation": 90.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 5.75, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path b/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path index d854b0cf..b3a53574 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path +++ b/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 4.375172004744959, - "y": 7.472562277580071 + "x": 4.020118623962041, + "y": 7.590913404507711 }, "prevControl": null, "nextControl": { - "x": 1.997390272835113, - "y": 7.537117437722419 + "x": 1.642336892052195, + "y": 7.655468564650059 }, "isLocked": false, - "linkedName": "Top Trench Score" + "linkedName": "Left Trench Score" }, { "anchor": { @@ -25,13 +25,13 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "Tower Left 1" + "linkedName": "Tower Left" } ], "rotationTargets": [ { "waypointRelativePos": 0.2742946708463985, - "rotationDegrees": 180.0 + "rotationDegrees": -90.0 } ], "constraintZones": [], @@ -47,13 +47,13 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": 90.0 + "rotation": 0.0 }, "reversed": false, "folder": "To Tower", "idealStartingState": { "velocity": 0, - "rotation": 180.0 + "rotation": -90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ (P).path b/src/main/deploy/pathplanner/paths/Left Trench To NZ (P).path deleted file mode 100644 index 703b8ca7..00000000 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ (P).path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.407449584816133, - "y": 7.633950177935944 - }, - "prevControl": null, - "nextControl": { - "x": 9.055421115065243, - "y": 7.590913404507711 - }, - "isLocked": false, - "linkedName": "Top Trench Auto Start" - }, - { - "anchor": { - "x": 8.259240806642943, - "y": 4.43846975088968 - }, - "prevControl": { - "x": 8.302277580071175, - "y": 7.8060972716488735 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "NZ Top Center (P)" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": true - }, - "goalEndState": { - "velocity": 5.75, - "rotation": 0.0 - }, - "reversed": false, - "folder": "To NZ", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path index bae5956b..c8d423ab 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 4.407449584816133, - "y": 7.633950177935944 + "x": 3.998600237247925, + "y": 7.526358244365362 }, "prevControl": null, "nextControl": { - "x": 8.366832740213523, - "y": 7.612431791221827 + "x": 8.646571767497036, + "y": 7.483321470937129 }, "isLocked": false, - "linkedName": "Top Trench Auto Start" + "linkedName": "Left Trench Start" }, { "anchor": { - "x": 7.785836298932384, - "y": 4.4169513641755636 + "x": 8.259240806642943, + "y": 4.43846975088968 }, "prevControl": { - "x": 7.882669039145908, - "y": 7.730782918149465 + "x": 8.302277580071175, + "y": 7.8060972716488735 }, "nextControl": null, "isLocked": false, - "linkedName": "NZ Top Center" + "linkedName": "NZ Left Center" } ], "rotationTargets": [], @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 5.75, - "rotation": 0.0 + "rotation": -90.0 }, "reversed": false, "folder": "To NZ", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": -90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score (P).path b/src/main/deploy/pathplanner/paths/Right NZ To Score (P).path deleted file mode 100644 index 6c248072..00000000 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score (P).path +++ /dev/null @@ -1,79 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 8.28075919335706, - "y": 3.6638078291814944 - }, - "prevControl": null, - "nextControl": { - "x": 6.064365361803085, - "y": 3.663807829181495 - }, - "isLocked": false, - "linkedName": "NZ Bottom Center (P)" - }, - { - "anchor": { - "x": 6.1289205219454335, - "y": 2.1037247924080655 - }, - "prevControl": { - "x": 6.161624603842337, - "y": 2.899524118566044 - }, - "nextControl": { - "x": 6.064365361803085, - "y": 0.5328825622775799 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.041637010676157, - "y": 0.6297153024911029 - }, - "prevControl": { - "x": 6.570047449584816, - "y": 0.6081969157769866 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Bottom Trench Score" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.16692426584235037, - "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 0.5, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": true - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "To Score", - "idealStartingState": { - "velocity": 5.75, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index 1a298b93..aabe421e 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -3,57 +3,32 @@ "waypoints": [ { "anchor": { - "x": 7.8073546856465015, - "y": 3.696085409252669 + "x": 8.28075919335706, + "y": 3.6638078291814944 }, "prevControl": null, "nextControl": { - "x": 6.483973902728352, - "y": 3.566975088967972 - }, - "isLocked": false, - "linkedName": "NZ Bottom Center" - }, - { - "anchor": { - "x": 6.387141162514829, - "y": 2.049928825622776 - }, - "prevControl": { - "x": 6.3502895773017505, - "y": 3.3950116859001485 - }, - "nextControl": { - "x": 6.430177935943061, - "y": 0.47908659549228894 + "x": 6.634602609727167, + "y": 0.5221233689205218 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.041637010676157, - "y": 0.6297153024911029 + "x": 4.461245551601425, + "y": 0.5006049822064051 }, "prevControl": { - "x": 6.064365361803085, - "y": 0.7265480427046259 + "x": 6.656120996441283, + "y": 0.4683274021352317 }, "nextControl": null, "isLocked": false, - "linkedName": "Bottom Trench Score" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.15455950540958613, - "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 0.7789799072642942, - "rotationDegrees": 0.0 + "linkedName": "Right Trench Score" } ], + "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -66,14 +41,14 @@ "unlimited": true }, "goalEndState": { - "velocity": 0, - "rotation": 0.0 + "velocity": 0.0, + "rotation": 90.0 }, "reversed": false, "folder": "To Score", "idealStartingState": { "velocity": 5.75, - "rotation": 180.0 + "rotation": 90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Tower Right (P).path b/src/main/deploy/pathplanner/paths/Right NZ To Tower Right (P).path deleted file mode 100644 index 4c4562e0..00000000 --- a/src/main/deploy/pathplanner/paths/Right NZ To Tower Right (P).path +++ /dev/null @@ -1,99 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 8.28075919335706, - "y": 3.6638078291814944 - }, - "prevControl": null, - "nextControl": { - "x": 6.107402135231317, - "y": 3.45938315539739 - }, - "isLocked": false, - "linkedName": "NZ Bottom Center (P)" - }, - { - "anchor": { - "x": 6.236512455516015, - "y": 2.1360023724792403 - }, - "prevControl": { - "x": 6.21230622918883, - "y": 3.007426520257926 - }, - "nextControl": { - "x": 6.26879003558719, - "y": 0.974009489916964 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.375172004744959, - "y": 0.7050296559905087 - }, - "prevControl": { - "x": 6.408659549228946, - "y": 0.7265480427046247 - }, - "nextControl": { - "x": 3.174010129462057, - "y": 0.6923189483155574 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.9645077105575333, - "y": 2.5448517200474496 - }, - "prevControl": { - "x": 2.6644602609727146, - "y": 0.6835112692763936 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Tower Right 1" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.15987460815047247, - "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 0.7006269592476471, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 2.2812982998454565, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": true - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0 - }, - "reversed": false, - "folder": "To Tower", - "idealStartingState": { - "velocity": 5.75, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path b/src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path index 783e81e1..433b2592 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path @@ -3,45 +3,29 @@ "waypoints": [ { "anchor": { - "x": 7.8073546856465015, - "y": 3.696085409252669 + "x": 8.28075919335706, + "y": 3.6638078291814944 }, "prevControl": null, "nextControl": { - "x": 5.892218268090155, - "y": 3.7176037959667845 - }, - "isLocked": false, - "linkedName": "NZ Bottom Center" - }, - { - "anchor": { - "x": 6.2903084223013055, - "y": 1.6625978647686828 - }, - "prevControl": { - "x": 6.190929893964972, - "y": 2.584107854796496 - }, - "nextControl": { - "x": 6.408659549228946, - "y": 0.5651601423487547 + "x": 6.053606168446025, + "y": 0.4898457888493475 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.558078291814947, - "y": 0.6835112692763936 + "x": 4.3644128113879015, + "y": 0.4252906287069993 }, "prevControl": { - "x": 6.171957295373667, - "y": 0.6727520759193362 + "x": 6.397900355871888, + "y": 0.4468090154211153 }, "nextControl": { - "x": 2.877475909519994, - "y": 0.694715285158359 + "x": 3.1632509361049994, + "y": 0.412579921032048 }, "isLocked": false, "linkedName": null @@ -52,26 +36,18 @@ "y": 2.5448517200474496 }, "prevControl": { - "x": 2.24485172004745, - "y": 0.5113641755634628 + "x": 2.6644602609727146, + "y": 0.6835112692763936 }, "nextControl": null, "isLocked": false, - "linkedName": "Tower Right 1" + "linkedName": "Tower Right" } ], "rotationTargets": [ { - "waypointRelativePos": 0.1551724137931077, - "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 0.5548589341692779, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 2.3261205564142173, - "rotationDegrees": 0.0 + "waypointRelativePos": 1.5156739811912283, + "rotationDegrees": 90.0 } ], "constraintZones": [], @@ -87,13 +63,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -90.0 + "rotation": 180.0 }, "reversed": false, "folder": "To Tower", "idealStartingState": { "velocity": 5.75, - "rotation": 180.0 + "rotation": 90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Score Score Tower.path b/src/main/deploy/pathplanner/paths/Right Score Score Tower.path new file mode 100644 index 00000000..6ad8008c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Score Score Tower.path @@ -0,0 +1,131 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.42896797153025, + "y": 0.5006049822064051 + }, + "prevControl": null, + "nextControl": { + "x": 6.656120996441281, + "y": 0.4145314353499395 + }, + "isLocked": false, + "linkedName": "Right Trench Start" + }, + { + "anchor": { + "x": 5.9890510083036785, + "y": 3.4271055753262156 + }, + "prevControl": { + "x": 5.809794588396062, + "y": 0.6785071367427231 + }, + "nextControl": { + "x": 6.053606168446026, + "y": 4.4169513641755636 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.946619217081851, + "y": 3.6315302491103196 + }, + "prevControl": { + "x": 6.83902728351127, + "y": 4.1479715302491105 + }, + "nextControl": { + "x": 7.053981008712923, + "y": 3.116193649281173 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.763712930011864, + "y": 0.6512336892052202 + }, + "prevControl": { + "x": 7.013067438601092, + "y": 0.6691872138236445 + }, + "nextControl": { + "x": 5.418813760379598, + "y": 0.5544009489916971 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.6118742586002375, + "y": 0.6512336892052202 + }, + "prevControl": { + "x": 5.85994068801898, + "y": 0.6942704626334513 + }, + "nextControl": { + "x": 3.3189082801011907, + "y": 0.6066486554638754 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.9645077105575333, + "y": 2.5448517200474496 + }, + "prevControl": { + "x": 1.4917081850533813, + "y": 0.9417319098457893 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Tower Right" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9449378330372995, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.2, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 4.360568383658999, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 5.75, + "rotation": 180.0 + }, + "reversed": false, + "folder": "To Tower", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (P).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (P).path deleted file mode 100644 index aef6e723..00000000 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ (P).path +++ /dev/null @@ -1,63 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.041637010676157, - "y": 0.6297153024911029 - }, - "prevControl": null, - "nextControl": { - "x": 8.479234083109219, - "y": 0.691307274546558 - }, - "isLocked": false, - "linkedName": "Bottom Trench Score" - }, - { - "anchor": { - "x": 8.28075919335706, - "y": 3.6638078291814944 - }, - "prevControl": { - "x": 8.22218094908225, - "y": 0.7767372184944703 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "NZ Bottom Center (P)" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.3025078369905977, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 0.4263322884012563, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": true - }, - "goalEndState": { - "velocity": 5.75, - "rotation": 180.0 - }, - "reversed": false, - "folder": "To NZ", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ.path b/src/main/deploy/pathplanner/paths/Right Score To NZ.path deleted file mode 100644 index 29a4254b..00000000 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ.path +++ /dev/null @@ -1,63 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.041637010676157, - "y": 0.6297153024911029 - }, - "prevControl": null, - "nextControl": { - "x": 8.62505338078292, - "y": 0.7803440094899179 - }, - "isLocked": false, - "linkedName": "Bottom Trench Score" - }, - { - "anchor": { - "x": 7.8073546856465015, - "y": 3.696085409252669 - }, - "prevControl": { - "x": 7.7535587188612105, - "y": 0.7265480427046254 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "NZ Bottom Center" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.3291536050156759, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 0.38871473354232106, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": true - }, - "goalEndState": { - "velocity": 5.75, - "rotation": 180.0 - }, - "reversed": false, - "folder": "To NZ", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path new file mode 100644 index 00000000..9ff8fb78 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -0,0 +1,111 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.42896797153025, + "y": 0.5006049822064051 + }, + "prevControl": null, + "nextControl": { + "x": 6.656120996441281, + "y": 0.4145314353499395 + }, + "isLocked": false, + "linkedName": "Right Trench Start" + }, + { + "anchor": { + "x": 5.9890510083036785, + "y": 3.4271055753262156 + }, + "prevControl": { + "x": 6.010569395017794, + "y": 0.6727520759193353 + }, + "nextControl": { + "x": 5.981149519717918, + "y": 4.438496114303611 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.946619217081851, + "y": 3.6315302491103196 + }, + "prevControl": { + "x": 6.903582443653619, + "y": 4.104934756820878 + }, + "nextControl": { + "x": 6.994277354658343, + "y": 3.107290735768909 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.1295255041518395, + "y": 1.415136417556346 + }, + "prevControl": { + "x": 7.118173588922177, + "y": 1.6648785526089203 + }, + "nextControl": { + "x": 7.172562277580072, + "y": 0.4683274021352315 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.052396204033215, + "y": 0.6512336892052202 + }, + "prevControl": { + "x": 6.171957295373667, + "y": 0.6512336892052202 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Center NZ (2)" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9449378330372995, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.2, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 6.75, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 5.75, + "rotation": -90.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path b/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path index 0759e52d..25a9b4e4 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path +++ b/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 4.041637010676157, - "y": 0.6297153024911029 + "x": 4.461245551601425, + "y": 0.5006049822064051 }, "prevControl": null, "nextControl": { - "x": 0.990835506596762, - "y": 0.8419359815453707 + "x": 1.4104440475220295, + "y": 0.712825661260673 }, "isLocked": false, - "linkedName": "Bottom Trench Score" + "linkedName": "Right Trench Score" }, { "anchor": { @@ -25,13 +25,13 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "Tower Right 1" + "linkedName": "Tower Right" } ], "rotationTargets": [ { - "waypointRelativePos": 0.17310664605873255, - "rotationDegrees": 0.0 + "waypointRelativePos": 0.41536050156739895, + "rotationDegrees": 90.0 } ], "constraintZones": [], @@ -47,13 +47,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -90.0 + "rotation": 180.0 }, "reversed": false, "folder": "To Tower", "idealStartingState": { "velocity": 0.0, - "rotation": 0.0 + "rotation": 90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ (P).path b/src/main/deploy/pathplanner/paths/Right Trench To NZ (P).path deleted file mode 100644 index 4f561017..00000000 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ (P).path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.370362595419848, - "y": 0.411517175572518 - }, - "prevControl": null, - "nextControl": { - "x": 8.689608540925267, - "y": 0.47908659549228905 - }, - "isLocked": false, - "linkedName": "Bottom Trench Auto Start" - }, - { - "anchor": { - "x": 8.28075919335706, - "y": 3.6638078291814944 - }, - "prevControl": { - "x": 8.205444839857652, - "y": 0.37149466192170744 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "NZ Bottom Center (P)" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, - "unlimited": true - }, - "goalEndState": { - "velocity": 5.75, - "rotation": 180.0 - }, - "reversed": false, - "folder": "To NZ", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index 890b02c5..84afa546 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 4.370362595419848, - "y": 0.411517175572518 + "x": 4.42896797153025, + "y": 0.5006049822064051 }, "prevControl": null, "nextControl": { - "x": 7.968742586002373, - "y": 0.4468090154211143 + "x": 8.74821391703567, + "y": 0.5681744021261762 }, "isLocked": false, - "linkedName": "Bottom Trench Auto Start" + "linkedName": "Right Trench Start" }, { "anchor": { - "x": 7.8073546856465015, - "y": 3.696085409252669 + "x": 8.28075919335706, + "y": 3.6638078291814944 }, "prevControl": { - "x": 7.936465005931199, - "y": 0.7157888493475686 + "x": 8.205444839857652, + "y": 0.37149466192170744 }, "nextControl": null, "isLocked": false, - "linkedName": "NZ Bottom Center" + "linkedName": null } ], "rotationTargets": [], @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 5.75, - "rotation": 180.0 + "rotation": 90.0 }, "reversed": false, "folder": "To NZ", "idealStartingState": { "velocity": 0, - "rotation": 180.0 + "rotation": 90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 6b970ef4..818357d9 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.762, - "robotLength": 0.981, + "robotWidth": 0.981, + "robotLength": 0.762, "holonomicMode": true, "pathFolders": [ "Misc", @@ -26,19 +26,19 @@ "driveMotorType": "krakenX60FOC", "driveCurrentLimit": 120.0, "wheelCOF": 1.542, - "flModuleX": 0.33, - "flModuleY": 0.229, - "frModuleX": 0.33, - "frModuleY": -0.229, - "blModuleX": -0.33, - "blModuleY": 0.229, - "brModuleX": -0.33, - "brModuleY": -0.229, + "flModuleX": 0.229, + "flModuleY": 0.33, + "frModuleX": 0.229, + "frModuleY": -0.33, + "blModuleX": -0.229, + "blModuleY": 0.33, + "brModuleX": -0.229, + "brModuleY": -0.33, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [ - "{\"name\":\"Intake\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.0,\"y\":-0.475},\"size\":{\"width\":0.19,\"length\":0.98},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":true}}", - "{\"name\":\"Turret\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.25,\"y\":0.175},\"size\":{\"width\":0.3,\"length\":0.35},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}", - "{\"name\":\"Climber\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":-0.44,\"y\":0.1},\"size\":{\"width\":0.2,\"length\":0.1},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":true}}" + "{\"name\":\"Intake\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.475,\"y\":0.0},\"size\":{\"width\":0.98,\"length\":0.19},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":true}}", + "{\"name\":\"Turret\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":-0.15,\"y\":0.275},\"size\":{\"width\":0.3,\"length\":0.35},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}", + "{\"name\":\"Climber\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":-0.1,\"y\":-0.45},\"size\":{\"width\":0.1,\"length\":0.2},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":true}}" ] } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 19761700..e90b5e52 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -6,18 +6,14 @@ package com.stuypulse.robot; import com.stuypulse.robot.commands.auton.DoNothingAuton; -import com.stuypulse.robot.commands.auton.poaching.RightTwoCyclePoach; -import com.stuypulse.robot.commands.auton.poaching.LeftOneCyclePoach; -import com.stuypulse.robot.commands.auton.poaching.LeftTwoCyclePoach; -import com.stuypulse.robot.commands.auton.poaching.RightOneCyclePoach; import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; +// import com.stuypulse.robot.commands.auton.test.BoxTest; import com.stuypulse.robot.commands.auton.regular.DepotAuton; import com.stuypulse.robot.commands.auton.regular.EightFuel; import com.stuypulse.robot.commands.auton.regular.LeftOneCycle; import com.stuypulse.robot.commands.auton.regular.LeftTwoCycle; import com.stuypulse.robot.commands.auton.regular.RightOneCycle; import com.stuypulse.robot.commands.climberhopper.ClimberDown; -// import com.stuypulse.robot.commands.auton.test.BoxTest; import com.stuypulse.robot.commands.climberhopper.ClimberOverrideDown; import com.stuypulse.robot.commands.climberhopper.ClimberOverrideStop; import com.stuypulse.robot.commands.climberhopper.ClimberOverrideUp; @@ -359,55 +355,38 @@ public void configureAutons() { autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); - // autonChooser.addOption("Wheel Radius", new SwerveWheelRadiusCharacterization()); - // // TESTS + // TESTS // AutonConfig BOX_TEST = new AutonConfig("Box Test", BoxTest::new, // "Box 1", "Box 2", "Box 3", "Box 4"); // BOX_TEST.register(autonChooser); - // // BASE - // AutonConfig EIGHT_FUEL = new AutonConfig("Eight Fuel", EightFuel::new, - // ""); - // EIGHT_FUEL.register(autonChooser); + // BASE + AutonConfig EIGHT_FUEL = new AutonConfig("Eight Fuel", EightFuel::new, + ""); + EIGHT_FUEL.register(autonChooser); - // // DEPOT - // AutonConfig DEPOT_AUTON = new AutonConfig("Depot Auton", DepotAuton::new, - // "Left Bump To Depot", "Depot To Tower Left"); - // DEPOT_AUTON.register(autonChooser); + // DEPOT + AutonConfig DEPOT_AUTON = new AutonConfig("Depot Auton", DepotAuton::new, + "Left Bump To Depot", "Depot To Tower Left"); + DEPOT_AUTON.register(autonChooser); // ONE CYCLES AutonConfig LEFT_ONE_CYCLE = new AutonConfig("Left One Cycle", LeftOneCycle::new, - "Left Trench To NZ", "Left NZ To Tower Left"); + "Left Trench To NZ", "Left NZ To Score"); LEFT_ONE_CYCLE.register(autonChooser); AutonConfig RIGHT_ONE_CYCLE = new AutonConfig("Right One Cycle", RightOneCycle::new, - "Right Trench To NZ", "Right NZ To Tower Right"); + "Right Trench To NZ", "Right NZ To Score"); RIGHT_ONE_CYCLE.register(autonChooser); - AutonConfig LEFT_ONE_CYCLE_POACH = new AutonConfig("Left One Cycle (Poach)", LeftOneCyclePoach::new, - "Left Trench To NZ (P)", "Left NZ To Tower Left (P)"); - LEFT_ONE_CYCLE_POACH.register(autonChooser); - - AutonConfig RIGHT_ONE_CYCLE_POACH = new AutonConfig("Right One Cycle (Poach)", RightOneCyclePoach::new, - "Right Trench To NZ (P)", "Right NZ To Tower Right (P)"); - RIGHT_ONE_CYCLE_POACH.register(autonChooser); - // TWO CYCLES - // AutonConfig LEFT_TWO_CYCLE = new AutonConfig("Left Two Cycle", LeftTwoCycle::new, - // "Left Trench To NZ", "Left NZ To Score", "Left Score To NZ", "Left NZ To Tower Left"); - // LEFT_TWO_CYCLE.register(autonChooser); - - // AutonConfig RIGHT_TWO_CYCLE = new AutonConfig("Right Two Cycle", RightTwoCycle::new, - // "Right Trench To NZ", "Right NZ To Score", "Right Score To NZ", "Right NZ To Tower Right"); - // RIGHT_TWO_CYCLE.register(autonChooser); - - // AutonConfig LEFT_TWO_CYCLE_POACH = new AutonConfig("Left Two Cycle (Poach)", LeftTwoCyclePoach::new, - // "Left Trench To NZ (P)", "Left NZ To Score (P)", "Left Score To NZ", "Left NZ To Tower Left"); - // LEFT_TWO_CYCLE_POACH.register(autonChooser); + AutonConfig LEFT_TWO_CYCLE = new AutonConfig("Left Two Cycle", LeftTwoCycle::new, + "Left Trench To NZ", "Left NZ To Score", "Left Score To Score"); + LEFT_TWO_CYCLE.register(autonChooser); - // AutonConfig RIGHT_TWO_CYCLE_POACH = new AutonConfig("Right Two Cycle (Poach)", RightTwoCyclePoach::new, - // "Right Trench To NZ (P)", "Right NZ To Score (P)", "Right Score To NZ", "Right NZ To Tower Right"); - // RIGHT_TWO_CYCLE_POACH.register(autonChooser); + AutonConfig RIGHT_TWO_CYCLE = new AutonConfig("Right Two Cycle", RightTwoCycle::new, + "Right Trench To NZ", "Right NZ To Score", "Right Score To Score"); + RIGHT_TWO_CYCLE.register(autonChooser); SmartDashboard.putData("Autonomous", autonChooser); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/poaching/LeftOneCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/LeftOneCyclePoach.java deleted file mode 100644 index 4d6527af..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/poaching/LeftOneCyclePoach.java +++ /dev/null @@ -1,54 +0,0 @@ -package com.stuypulse.robot.commands.auton.poaching; - -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.climberhopper.ClimberDown; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -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.climbAlign.SwerveClimbAlign; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class LeftOneCyclePoach extends SequentialCommandGroup { - - public LeftOneCyclePoach(PathPlannerPath... paths) { - - addCommands( - - // NZ Trip 1 - new IntakeDeploy().alongWith( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) - ), - - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new IntakeStow() - ), - new ParallelCommandGroup( - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) - // new SwerveClimbAlign() - ), - new SpindexerRun().alongWith( - new HandoffRun() - ) - // .until(() -> DriverStation.getMatchTime() < 2).andThen( - // new ParallelCommandGroup( - // new HandoffStop(), - // new SpindexerStop(), - // new ClimberDown() - // ) - // ) - - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/poaching/LeftTwoCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/LeftTwoCyclePoach.java deleted file mode 100644 index 2d534b1b..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/poaching/LeftTwoCyclePoach.java +++ /dev/null @@ -1,72 +0,0 @@ -package com.stuypulse.robot.commands.auton.poaching; - -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.climberhopper.ClimberDown; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -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.climbAlign.SwerveClimbAlign; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class LeftTwoCyclePoach extends SequentialCommandGroup { - - public LeftTwoCyclePoach(PathPlannerPath... paths) { - - addCommands( - - // NZ Trip 1 - new IntakeDeploy().alongWith( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) - ), - - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new IntakeStow() - ), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new SpindexerRun().alongWith( - new HandoffRun() - ).withTimeout(5.0), - - // NZ Trip 2 - new IntakeDeploy().alongWith( - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new HandoffStop(), - new SpindexerStop() - ) - ), - - // Trip 2 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).alongWith( - new IntakeStow() - ), - new ParallelCommandGroup( - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) - // new SwerveClimbAlign() - ), - new SpindexerRun().alongWith( - new HandoffRun() - ) - // .until(() -> DriverStation.getMatchTime() < 2).andThen( - // new ParallelCommandGroup( - // new HandoffStop(), - // new SpindexerStop(), - // new ClimberDown() - // ) - // ) - - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/poaching/RightOneCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/RightOneCyclePoach.java deleted file mode 100644 index b3eea775..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/poaching/RightOneCyclePoach.java +++ /dev/null @@ -1,54 +0,0 @@ -package com.stuypulse.robot.commands.auton.poaching; - -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.climberhopper.ClimberDown; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -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.climbAlign.SwerveClimbAlign; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class RightOneCyclePoach extends SequentialCommandGroup { - - public RightOneCyclePoach(PathPlannerPath... paths) { - - addCommands( - - // NZ Trip 1 - new IntakeDeploy().alongWith( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) - ), - - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new IntakeStow() - ), - new ParallelCommandGroup( - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) - // new SwerveClimbAlign() - ), - new SpindexerRun().alongWith( - new HandoffRun() - ) - // .until(() -> DriverStation.getMatchTime() < 2).andThen( - // new ParallelCommandGroup( - // new HandoffStop(), - // new SpindexerStop(), - // new ClimberDown() - // ) - // ) - - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/poaching/RightTwoCyclePoach.java b/src/main/java/com/stuypulse/robot/commands/auton/poaching/RightTwoCyclePoach.java deleted file mode 100644 index 6c9f0a78..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/poaching/RightTwoCyclePoach.java +++ /dev/null @@ -1,73 +0,0 @@ -package com.stuypulse.robot.commands.auton.poaching; - -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.climberhopper.ClimberDown; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -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.climbAlign.SwerveClimbAlign; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class RightTwoCyclePoach extends SequentialCommandGroup { - - public RightTwoCyclePoach(PathPlannerPath... paths) { - - addCommands( - - // NZ Trip 1 - new IntakeDeploy().alongWith( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) - ), - - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new IntakeStow() - ), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new SpindexerRun().alongWith( - new HandoffRun() - ).withTimeout(5.0), - - // NZ Trip 2 - new IntakeDeploy().alongWith( - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new HandoffStop(), - new SpindexerStop() - ) - ), - - // Trip 2 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).alongWith( - new IntakeStow() - ), - new ParallelCommandGroup( - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) - // new SwerveClimbAlign() - ), - new SpindexerRun().alongWith( - new HandoffRun() - ) - // .until(() -> DriverStation.getMatchTime() < 2).andThen( - // new ParallelCommandGroup( - // new HandoffStop(), - // new SpindexerStop(), - // new ClimberDown() - // ) - // ) - - - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java index 61a78621..423ce0ff 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java @@ -4,9 +4,11 @@ import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.superstructure.SuperstructureKB; +import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class EightFuel extends SequentialCommandGroup { @@ -14,12 +16,10 @@ public EightFuel(PathPlannerPath... paths) { addCommands( - new SuperstructureKB().until( - () -> Superstructure.getInstance().atTolerance() - ), - - new SpindexerRun().alongWith( - new HandoffRun() + new SuperstructureKB().alongWith( + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()).andThen( + new SpindexerRun().alongWith(new HandoffRun()) + ) ) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java index 08d1bbe9..cf6ba0f0 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java @@ -8,6 +8,7 @@ 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.superstructure.SuperstructureInterpolation; import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -27,6 +28,7 @@ public LeftOneCycle(PathPlannerPath... paths) { new IntakeDeploy().alongWith( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) ), + new SuperstructureInterpolation(), // Trip 1 To Score CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java index 99b75683..516bbf08 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java @@ -8,6 +8,7 @@ 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.superstructure.SuperstructureInterpolation; import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -27,6 +28,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { new IntakeDeploy().alongWith( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) ), + new SuperstructureInterpolation(), // Trip 1 To Score CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( @@ -41,18 +43,12 @@ public LeftTwoCycle(PathPlannerPath... paths) { ).withTimeout(5.0), // NZ Trip 2 - new IntakeDeploy().alongWith( - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new HandoffStop(), - new SpindexerStop() - ) + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() ), - // Trip 2 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).alongWith( - new IntakeStow() - ), new ParallelCommandGroup( new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) // new SwerveClimbAlign() diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java index ad5c783e..dddfad45 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java @@ -8,6 +8,7 @@ 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.superstructure.SuperstructureInterpolation; import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -26,6 +27,7 @@ public RightOneCycle(PathPlannerPath... paths) { new IntakeDeploy().alongWith( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) ), + new SuperstructureInterpolation(), // Trip 1 To Score CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java index b3f835a5..bbb56974 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java @@ -8,6 +8,7 @@ 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.superstructure.SuperstructureInterpolation; import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -27,6 +28,7 @@ public RightTwoCycle(PathPlannerPath... paths) { new IntakeDeploy().alongWith( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) ), + new SuperstructureInterpolation(), // Trip 1 To Score CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( @@ -38,18 +40,12 @@ public RightTwoCycle(PathPlannerPath... paths) { ).withTimeout(5.0), // NZ Trip 2 - new IntakeDeploy().alongWith( - new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new HandoffStop(), - new SpindexerStop() - ) + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() ), - // Trip 2 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]).alongWith( - new IntakeStow() - ), new ParallelCommandGroup( new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) // new SwerveClimbAlign() diff --git a/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java b/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java index 3b46a9b7..08160941 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java @@ -13,8 +13,7 @@ public BoxTest(PathPlannerPath... paths) { addCommands( - new SuperstructureKB().until(() -> DriverStation.getMatchTime() < 18).andThen( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0])), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]) From 2ea8336a462b980c598828f550df0268d8b0a510 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 7 Mar 2026 20:56:40 -0500 Subject: [PATCH 108/150] PROGRESS --- simgui-ds.json | 6 -- simgui.json | 16 ++--- src/main/java/com/stuypulse/robot/Robot.java | 7 +- .../com/stuypulse/robot/RobotContainer.java | 63 ++++++++++------- .../robot/commands/intake/IntakeOuttake.java | 14 ++++ .../superstructure/SuperstructureFOTM.java | 14 ++++ .../commands/swerve/SwerveDriveDrive.java | 27 ++++++-- .../robot/constants/DriverConstants.java | 2 +- .../com/stuypulse/robot/constants/Field.java | 6 +- .../com/stuypulse/robot/constants/Gains.java | 16 ++++- .../stuypulse/robot/constants/Settings.java | 38 +++++----- .../robot/subsystems/intake/IntakeImpl.java | 12 ++-- .../subsystems/spindexer/SpindexerImpl.java | 9 ++- .../superstructure/Superstructure.java | 8 ++- .../subsystems/superstructure/hood/Hood.java | 2 + .../superstructure/shooter/Shooter.java | 4 +- .../superstructure/shooter/ShooterImpl.java | 7 +- .../superstructure/turret/Turret.java | 4 ++ .../superstructure/turret/TurretImpl.java | 46 +++++++------ .../superstructure/turret/TurretSim.java | 6 ++ .../swerve/CommandSwerveDrivetrain.java | 8 +-- .../util/superstructure/SOTMCalculator.java | 69 +++++++++++++++---- 22 files changed, 257 insertions(+), 127 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeOuttake.java create mode 100644 src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureFOTM.java diff --git a/simgui-ds.json b/simgui-ds.json index b408e686..20138e7d 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,10 +1,4 @@ { - "System Joysticks": { - "window": { - "enabled": false - } - }, - "disable": true, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/simgui.json b/simgui.json index d402b29b..8f995dc1 100644 --- a/simgui.json +++ b/simgui.json @@ -42,16 +42,6 @@ "window": { "visible": true } - }, - "/SmartDashboard/Visualizers/Hood": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Visualizers/Turret": { - "window": { - "visible": true - } } } }, @@ -79,10 +69,14 @@ "Intake": { "open": true }, + "States": { + "open": true + }, "Superstructure": { "Hood": { "open": true - } + }, + "open": true }, "Swerve": { "open": true diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index dcdc2534..41663b43 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -105,7 +105,9 @@ public void autonomousInit() { public void autonomousPeriodic() {} @Override - public void autonomousExit() {} + public void autonomousExit( + + ) {} /*******************/ /*** TELEOP MODE ***/ @@ -116,7 +118,8 @@ public void teleopInit() { // fmsUtil.restartTimer(false); CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); - CommandScheduler.getInstance().schedule(new SetIMUMode(Settings.Vision.INTERNAL_EXTERNAL_ASSIST_INDEX)); + // CommandScheduler.getInstance().schedule(new SetIMUMode(Settings.Vision.INTERNAL_EXTERNAL_ASSIST_INDEX)); + CommandScheduler.getInstance().schedule(new SetIMUMode(0)); if (auto != null) { auto.cancel(); diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 80f0983c..44ae59b9 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -27,6 +27,7 @@ import com.stuypulse.robot.commands.hood.ZeroHoodEncoderAtUpperHardstop; import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.intake.IntakeDigestion; +import com.stuypulse.robot.commands.intake.IntakeOuttake; import com.stuypulse.robot.commands.intake.IntakeRunRollers; import com.stuypulse.robot.commands.intake.IntakeStopRollers; import com.stuypulse.robot.commands.intake.IntakeStow; @@ -36,6 +37,7 @@ import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureDefaultCommand; +import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; import com.stuypulse.robot.commands.superstructure.SuperstructureFerry; import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureKB; @@ -160,7 +162,7 @@ private void configureDefaultCommands() { private void configureButtonBindings() { // Scoring Routine - driver.getTopButton() + driver.getBottomButton() .whileTrue(new SwerveXMode()) .onTrue(new IntakeRunRollers()) .whileTrue(new SuperstructureShoot() @@ -173,7 +175,7 @@ private void configureButtonBindings() { .alongWith(new HandoffStop())); // Interpolation-based Scoring - driver.getBottomButton() + driver.getTopButton() .whileTrue(new SwerveXMode()) .onTrue(new IntakeRunRollers()) .whileTrue(new SuperstructureInterpolation() @@ -191,8 +193,9 @@ private void configureButtonBindings() { // Intake Deploy driver.getRightTriggerButton() - .onTrue(new IntakeDeploy()); - + .onTrue(new IntakeDeploy()); + // .onTrue(new SuperstructureShoot()); // TODO: put shooter in shoot --> turn off sotm + // Reset Heading driver.getDPadUp() .onTrue(new SwerveResetHeading()) @@ -203,29 +206,45 @@ private void configureButtonBindings() { driver.getLeftBumper() .onTrue(new IntakeStopRollers()); - // SOTM - // should be right menu - driver.getRightButton() - .whileTrue(new SuperstructureSOTM().onlyIf(() -> !swerve.isUnderTrench()) - .andThen(new WaitUntilCommand(superstructure::atTolerance)) - .andThen(new HandoffConditionalCommand().onlyIf(superstructure::atTolerance) - .alongWith(new WaitUntilCommand(handoff::atTolerance)) - .andThen(new SpindexerConditionalCommand().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) - .onFalse(new SpindexerStop() - .alongWith(new SuperstructureStow()) - .alongWith(new HandoffStop())); + // driver.getRightBumper() + // .whileTrue(new IntakeOuttake()) + // .onFalse(new IntakeRunRollers()); - // Scoring SOTM - // driver.getRightMenuButton() + // SOTM + driver.getRightMenuButton() + .onTrue(new IntakeRunRollers()) + .onTrue(new ConditionalCommand( + new ParallelCommandGroup( + new SuperstructureInterpolation(), + new SpindexerStop(), + new HandoffStop() + ), + new SuperstructureSOTM().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + .andThen(new SpindexerRun()).alongWith(new HandoffRun()), + () -> superstructure.getState() == SuperstructureState.SOTM + )); + + // Ferrying In Place + driver.getDPadRight() + .whileTrue(new SwerveXMode()) + .whileTrue(new SuperstructureFerry().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + .andThen(new HandoffRun()) + .andThen(new WaitUntilCommand(handoff::atTolerance)) + .alongWith(new SpindexerRun())) + .onFalse(new SuperstructureFerry().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); + + // FOTM + // driver.getDPadRight() + // .onTrue(new IntakeRunRollers()) // .onTrue(new ConditionalCommand( // new ParallelCommandGroup( // new SuperstructureInterpolation(), // new SpindexerStop(), // new HandoffStop() // ), - // new SuperstructureSOTM().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + // new SuperstructureFOTM().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) // .andThen(new SpindexerRun()).alongWith(new HandoffRun()), - // () -> superstructure.getState() == SuperstructureState.SOTM + // () -> superstructure.getState() == SuperstructureState.FOTM // )); // digestion by moving the pivot @@ -304,12 +323,6 @@ private void configureButtonBindings() { // .onTrue(new ResetLimelightIMU()) // .onFalse(new SetIMUMode(0)); - // // Ferrying In Place - // driver.getDPadRight() - // .whileTrue(new SwerveXMode()) - // .whileTrue(new SuperstructureFerry().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) - // .andThen(new SpindexerRun()).alongWith(new HandoffRun())) - // .onFalse(new SuperstructureFerry().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); // /** // // Ferrying SOTM diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeOuttake.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeOuttake.java new file mode 100644 index 00000000..a72c138f --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeOuttake.java @@ -0,0 +1,14 @@ +/************************ 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.commands.intake; + +import com.stuypulse.robot.subsystems.intake.Intake.RollerState; + +public class IntakeOuttake extends IntakeSetState { + public IntakeOuttake() { + super(RollerState.OUTTAKE); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureFOTM.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureFOTM.java new file mode 100644 index 00000000..abace6a7 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureFOTM.java @@ -0,0 +1,14 @@ +/************************ 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.commands.superstructure; + +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; + +public class SuperstructureFOTM extends SuperstructureSetState { + public SuperstructureFOTM() { + super(SuperstructureState.FOTM); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java index 33fd1e47..fadd8c7f 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java @@ -17,22 +17,31 @@ import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public class SwerveDriveDrive extends Command { private final CommandSwerveDrivetrain swerve; + private final Superstructure superstructure; private final Gamepad driver; + private double maxVelocity; + private final VStream speed; private final IStream turn; public SwerveDriveDrive(Gamepad driver) { swerve = CommandSwerveDrivetrain.getInstance(); + superstructure = Superstructure.getInstance(); speed = VStream.create(this::getDriverInputAsVelocity) .filtered( @@ -63,10 +72,20 @@ private Vector2D getDriverInputAsVelocity() { @Override public void execute() { + Vector2D speedVector = speed.get(); + double angularVel = turn.get(); + + if (speedVector.magnitude() > 0.05 && superstructure.getState() == SuperstructureState.SOTM) { + speedVector = speedVector.normalize().mul(Settings.Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S); + // angularVel = (angularVel / Swerve.Constraints.MAX_ANGULAR_VEL_RAD_PER_S) * Settings.Swerve.Constraints.MAX_ANGULAR_VEL_SOTM_RAD_PER_S; + } + + SmartDashboard.putNumber("Swerve/SOTM velocity x", speedVector.x * Settings.Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S); + SmartDashboard.putNumber("Swerve/SOTM velocity y", speedVector.y * Settings.Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S); + swerve.setControl(swerve.getFieldCentricSwerveRequest() - .withVelocityX(speed.get().x) - .withVelocityY(speed.get().y) - .withRotationalRate(-turn.getAsDouble()) - ); + .withVelocityX(speedVector.x) + .withVelocityY(speedVector.y) + .withRotationalRate(-angularVel)); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/DriverConstants.java b/src/main/java/com/stuypulse/robot/constants/DriverConstants.java index 603e25c0..ccdc7681 100644 --- a/src/main/java/com/stuypulse/robot/constants/DriverConstants.java +++ b/src/main/java/com/stuypulse/robot/constants/DriverConstants.java @@ -14,7 +14,7 @@ public interface Driver { public interface Drive { double DEADBAND = 0.05; //TODO: tune to philip's request double RC = 0.05; - int POWER = 2; + int POWER = 3; } public interface Turn { double DEADBAND = 0.05; diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 6f4beceb..8f12e456 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -31,7 +31,7 @@ public interface Field { double LENGTH = Units.inchesToMeters(651.200); public static final double trenchHopperTolerance = Units.inchesToMeters(50); - public static final double trenchHoodTolerance = Units.inchesToMeters(10); + public static final double trenchHoodTolerance = Units.inchesToMeters(20); // Alliance relative hub center coordinates public static final Pose2d hubCenter = new Pose2d(Units.inchesToMeters(182.11), WIDTH / 2.0, new Rotation2d()); @@ -61,8 +61,8 @@ 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 leftFerryZone = new Pose2d(Units.inchesToMeters(31.5), Units.inchesToMeters(WIDTH - 34.5), new Rotation2d()); + public final Pose2d rightFerryZone = new Pose2d(Units.inchesToMeters(20.75), Units.inchesToMeters(76), 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) { diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 7020d4ae..14895ee7 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -31,14 +31,24 @@ public interface ClimberHopper { public interface Superstructure { public interface Shooter { - SmartNumber kP = new SmartNumber("Superstructure/Shooter/Gains/kP", 0.45); + + SmartNumber kP = new SmartNumber("Superstructure/Shooter/Gains/kP", 10.0); SmartNumber kI = new SmartNumber("Superstructure/Shooter/Gains/kI", 0.0); SmartNumber kD = new SmartNumber("Superstructure/Shooter/Gains/kD", 0.0); - SmartNumber kS = new SmartNumber("Superstructure/Shooter/Gains/kS", 0.0); - SmartNumber kV = new SmartNumber("Superstructure/Shooter/Gains/kV", 0.123);//0.11650); + SmartNumber kS = new SmartNumber("Superstructure/Shooter/Gains/kS", 10.32); + SmartNumber kV = new SmartNumber("Superstructure/Shooter/Gains/kV", 0.0835); SmartNumber kA = new SmartNumber("Superstructure/Shooter/Gains/kA", 0.0); + // BELOW ARE VELOCITY PID AND VOLTAGEBASED FF GAINS + // SmartNumber kP = new SmartNumber("Superstructure/Shooter/Gains/kP", 0.45); + // SmartNumber kI = new SmartNumber("Superstructure/Shooter/Gains/kI", 0.0); + // SmartNumber kD = new SmartNumber("Superstructure/Shooter/Gains/kD", 0.0); + + // SmartNumber kS = new SmartNumber("Superstructure/Shooter/Gains/kS", 0.0); + // SmartNumber kV = new SmartNumber("Superstructure/Shooter/Gains/kV", 0.123);//0.11650); + // SmartNumber kA = new SmartNumber("Superstructure/Shooter/Gains/kA", 0.0); + // double kP = 0.45; // double kI = 0.0; // double kD = 0.0; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index e52a967a..1f78fe59 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -144,39 +144,31 @@ public interface Superstructure { public interface AngleInterpolation { double[][] distanceAngleInterpolationValues = { - {1.22, Units.degreesToRadians(20)}, //AGAINST THE HUB - {1.43, Units.degreesToRadians(21.0)}, //meters, radians - {2.15, Units.degreesToRadians(23.23)}, //KEVIN-APPROVED - {2.864967, Units.degreesToRadians(27)}, //KEVIN-APPROVED - // {3.38, Units.degreesToRadians(37.5)}, - {3.65, Units.degreesToRadians(28.0)}, //KEVIN-APPROVED - {4.43, Units.degreesToRadians(33.5)}, //KEVIN-APPROVED + {1.22, Units.degreesToRadians(22.5)}, //BLAY-APPROVED (ALMOST AGAINST HUB), LOCKED IN + {2.15, Units.degreesToRadians(27)}, //BLAY-APPROVED + {3.38, Units.degreesToRadians(37)}, //BLAY-APPROVED + {4.43, Units.degreesToRadians(39)}, //BLAY-APPROVED {5.66, Units.degreesToRadians(39)} //KEVIN-APPROVED }; } public interface RPMInterpolation{ double[][] distanceRPMInterpolationValues = { - {1.22, 2950.0}, //KEVIN-APPROVED - {1.43, 3000.0}, // meters, RPM - {2.15, 3050.0}, //KEVIN-APPROVED - {2.864967, 3150}, //KEVIN-APPROVED - //{3.38, 3200} - {3.65, 3400.0}, //KEVIN-APPROVED - {4.43, 3600.0}, //KEVIN-APPROVED + {1.22, 2700.0}, //BLAY-APPROVED, LOCKED IN + {2.15, 2900.0}, //BLAY-APPROVED + {3.38, 3200}, //BLAY-APPROVED + {4.43, 3550.0}, //BLAY-APPROVED {5.66, 3900.0} //KEVIN-APPROVED }; } public interface TOFInterpolation{ double[][] distanceTOFInterpolationValues = { - {1.22, 1.115}, // seconds - // {1.43,}, + {1.22, 0.965}, // seconds // {2.15, }, - {2.864967, 1.205}, - {3.65, 1.32}, - // {4.43, }, - {5.66, 1.29} + {3.38, 1.32}, + {4.43, 1.125}, + {5.66, 1.29} }; } @@ -286,7 +278,7 @@ public interface Angles { public final Rotation2d MIN = Rotation2d.fromDegrees(20.0); public final Rotation2d MAX = Rotation2d.fromDegrees(40.0); - public final Rotation2d STOW = Rotation2d.fromDegrees(11.0); + public final Rotation2d STOW = Rotation2d.fromDegrees(21.0); public final Rotation2d KB = Rotation2d.fromDegrees(12.0); public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(10.0); public final Rotation2d RIGHT_CORNER = Rotation2d.fromDegrees(10.0); @@ -357,7 +349,9 @@ public interface Constraints { public final double MAX_VELOCITY_SOTM_M_PER_S = 1.00; 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_VEL_RAD_PER_S = Units.degreesToRadians(300.0); + public final double MAX_ANGULAR_VEL_SOTM_RAD_PER_S = Units.degreesToRadians(75.0); + public final double MAX_ANGULAR_ACCEL_RAD_PER_S_SQUARED = Units.degreesToRadians(900.0); public final PathConstraints DEFAULT_CONSTRAINTS = 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 8a5cbdee..da21a016 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -156,24 +156,24 @@ public void zeroPivotDeployed() { @Override public void periodic() { super.periodic(); - PivotState state = getPivotState(); + PivotState pivotState = getPivotState(); if (EnabledSubsystems.INTAKE.get()) { if (pivotVoltageOverride.isPresent()) { pivot.setVoltage(pivotVoltageOverride.get()); } else { // PIVOT - if (state == PivotState.DEPLOY + if (pivotState == PivotState.DEPLOY && getPivotAngle().getDegrees() <= Settings.Intake.ARBITRARY_VOLTAGE_THRESHOLD.getDegrees()) { pivot.setControl(new VoltageOut(-Settings.Intake.PUSHDOWN_VOLTAGE)); // applying 3 volts - } else if (state == PivotState.DIGESTION_DOWN || state == PivotState.DIGESTION_UP) { - pivot.setControl(new MotionMagicVoltage(state.getTargetAngle().getRotations())); //TODO: verify motion profile works + } else if (pivotState == PivotState.DIGESTION_DOWN || pivotState == PivotState.DIGESTION_UP) { + pivot.setControl(new MotionMagicVoltage(pivotState.getTargetAngle().getRotations())); //TODO: verify motion profile works } else { - pivot.setControl(new PositionVoltage(state.getTargetAngle().getRotations())); + pivot.setControl(new PositionVoltage(pivotState.getTargetAngle().getRotations())); } // ROLLERS - if (getPivotState() == PivotState.DEPLOY + if (pivotState == PivotState.DEPLOY && getPivotAngle().getDegrees() <= Settings.Intake.THRESHOLD_TO_START_ROLLERS.getDegrees()) { rollerLeader.setControl(rollerController.withOutput(getRollerState().getTargetDutyCycle())); } 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 519e44e6..c38134b6 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -40,7 +40,7 @@ public SpindexerImpl() { .withInvertedValue(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Brake) - .withSupplyCurrentLimitAmps(80) + .withSupplyCurrentLimitAmps(45) .withStatorCurrentLimitEnabled(false) .withRampRate(0.25) @@ -53,7 +53,7 @@ public SpindexerImpl() { .withInvertedValue(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Brake) - .withSupplyCurrentLimitAmps(80) + .withSupplyCurrentLimitAmps(45) .withStatorCurrentLimitEnabled(false) .withRampRate(0.25) @@ -97,6 +97,11 @@ public void periodic() { if (voltageOverride.isPresent()) { leadMotor.setVoltage(voltageOverride.get()); } else { + /** + * THE FOLLOWING CODE IS NOT A MISTAKE + * + * WHEN STOPPING THE + */ if (atTolerance() && getState() == SpindexerState.STOP) { leadMotor.stopMotor(); } else { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index 2cd7307d..f1ddf68e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -49,6 +49,7 @@ public enum SuperstructureState { STOW(HoodState.STOW, ShooterState.SHOOT, TurretState.SHOOT), SHOOT(HoodState.SHOOT, ShooterState.SHOOT, TurretState.SHOOT), FERRY(HoodState.FERRY, ShooterState.FERRY, TurretState.FERRY), + FOTM(HoodState.FOTM, ShooterState.FOTM, TurretState.FOTM), REVERSE(HoodState.SHOOT, ShooterState.REVERSE, TurretState.SHOOT), KB(HoodState.KB, ShooterState.KB, TurretState.KB), LEFT_CORNER(HoodState.LEFT_CORNER, ShooterState.LEFT_CORNER, TurretState.SHOOT), @@ -126,9 +127,14 @@ public Rotation2d getTurretAngle(){ return turret.getAngle(); } + public boolean isWrapping() { + return turret.isWrapping(); + } + @Override public void periodic() { - if (getState() == SuperstructureState.SOTM) { + SuperstructureState state = getState(); + if (state == SuperstructureState.SOTM || state == SuperstructureState.FOTM) { SOTMCalculator.updateSOTMSolution(); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java index 852a66b7..9784fb75 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java @@ -47,6 +47,7 @@ public enum HoodState { RIGHT_CORNER, INTERPOLATION, SOTM, + FOTM, ANALOG, IDLE; } @@ -77,6 +78,7 @@ public Rotation2d getTargetAngle() { case RIGHT_CORNER -> Settings.Superstructure.Hood.Angles.RIGHT_CORNER; case INTERPOLATION -> InterpolationCalculator.interpolateShotInfo().targetHoodAngle(); case SOTM -> SOTMCalculator.calculateHoodAngleSOTM(); + case FOTM -> SOTMCalculator.calculateHoodAngleFOTM(); case ANALOG -> hoodAnalogToOutput(); case IDLE -> getAngle(); }; diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index f6f3ad65..2fe70245 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java @@ -40,7 +40,8 @@ public enum ShooterState { LEFT_CORNER, RIGHT_CORNER, INTERPOLATION, - SOTM; + SOTM, + FOTM; } public Shooter() { @@ -66,6 +67,7 @@ public double getTargetRPM() { case RIGHT_CORNER -> Settings.Superstructure.Shooter.RPM.RIGHT_CORNER; case INTERPOLATION -> InterpolationCalculator.interpolateShotInfo().targetRPM(); case SOTM -> SOTMCalculator.calculateShooterRPMSOTM(); + case FOTM -> SOTMCalculator.calculateShooterRPMFOTM(); }; } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java index 14293ca6..c7e04f3a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -17,6 +17,7 @@ import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; @@ -31,7 +32,8 @@ public class ShooterImpl extends Shooter { private final TalonFX shooterLeader; private final TalonFX shooterFollower; - private final VelocityVoltage shooterController; + // private final VelocityVoltage shooterController; + private final VelocityTorqueCurrentFOC shooterController; private final Follower follower; private Optional voltageOverride; @@ -59,7 +61,8 @@ public ShooterImpl() { shooterConfig.configure(shooterLeader); shooterConfig.configure(shooterFollower); - shooterController = new VelocityVoltage(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE).withEnableFOC(true); + // shooterController = new VelocityVoltage(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE).withEnableFOC(true); + shooterController = new VelocityTorqueCurrentFOC(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE); follower = new Follower(Ports.Superstructure.Shooter.MOTOR_LEAD, MotorAlignmentValue.Opposed); shooterFollower.setControl(follower); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java index 8a69198c..4c38d54c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java @@ -51,6 +51,7 @@ public enum TurretState { ZERO, SHOOT, SOTM, + FOTM, FERRY, LEFT_CORNER, RIGHT_CORNER, @@ -64,6 +65,7 @@ public Rotation2d getTargetAngle() { case ZERO -> Rotation2d.kZero; case SHOOT -> getScoringAngle(); case SOTM -> SOTMCalculator.calculateTurretAngleSOTM(); + case FOTM -> SOTMCalculator.calculateTurretAngleFOTM(); case FERRY -> getFerryAngle(); case LEFT_CORNER -> Settings.Superstructure.Turret.LEFT_CORNER; case RIGHT_CORNER -> Settings.Superstructure.Turret.RIGHT_CORNER; @@ -102,6 +104,8 @@ public Rotation2d getFerryAngle() { public abstract void seedTurret(); public abstract void zeroEncoders(); + + public abstract boolean isWrapping(); public void setState(TurretState state) { this.state = state; diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index 6a4069c9..be16a1a6 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -31,7 +31,7 @@ public class TurretImpl extends Turret { private final Motors.CANCoderConfig encoder17tConfig; private final Motors.CANCoderConfig encoder18tConfig; - private final TalonFX motor; + private final TalonFX turretMotor; private final CANcoder encoder17t; private final CANcoder encoder18t; @@ -39,6 +39,8 @@ public class TurretImpl extends Turret { private Optional voltageOverride; private final PositionVoltage controller; + private boolean isWrapping; + public TurretImpl() { turretConfig = new Motors.TalonFXConfig() .withInvertedValue(InvertedValue.Clockwise_Positive) @@ -74,11 +76,11 @@ public TurretImpl() { .withMagnetOffset(Settings.Superstructure.Turret.Encoder18t.OFFSET.getRotations()) .withAbsoluteSensorDiscontinuityPoint(1.0); - motor = new TalonFX(Ports.Superstructure.Turret.MOTOR, Ports.RIO); + turretMotor = new TalonFX(Ports.Superstructure.Turret.MOTOR, Ports.RIO); encoder17t = new CANcoder(Ports.Superstructure.Turret.ENCODER17T, Ports.RIO); encoder18t = new CANcoder(Ports.Superstructure.Turret.ENCODER18T, Ports.RIO); - turretConfig.configure(motor); + turretConfig.configure(turretMotor); encoder17tConfig.configure(encoder17t); encoder18tConfig.configure(encoder18t); @@ -121,12 +123,16 @@ public void zeroEncoders() { public void seedTurret() { // motor.setPosition(0); //TODO: SEED USING CRT INSTEAD OF TS, TS IS TEMP - motor.setPosition(getVectorSpaceAngle().getRotations()); + turretMotor.setPosition(getVectorSpaceAngle().getRotations()); + } + + public boolean isWrapping() { + return isWrapping; } @Override public Rotation2d getAngle() { - return Rotation2d.fromRotations(motor.getPosition().getValueAsDouble()); + return Rotation2d.fromRotations(turretMotor.getPosition().getValueAsDouble()); } @Override @@ -153,7 +159,7 @@ private double getDelta(double target, double current) { public void periodic() { super.periodic(); - turretConfig.updateGainsConfig(motor, 1, + turretConfig.updateGainsConfig(turretMotor, 1, Gains.Superstructure.Turret.slot1.kP, Gains.Superstructure.Turret.slot1.kI, Gains.Superstructure.Turret.slot1.kD, @@ -170,8 +176,8 @@ public void periodic() { double currentAngle = getAngle().getDegrees(); double actualTargetDeg = currentAngle + getDelta(getTargetAngle().getDegrees(), currentAngle); - boolean isWrapping = Math.abs(actualTargetDeg - currentAngle) > - Settings.Superstructure.Turret.GAIN_SWITCHING_THRESHOLD.getDegrees(); + isWrapping = Math.abs(actualTargetDeg - currentAngle) > + Settings.Superstructure.Turret.GAIN_SWITCHING_THRESHOLD.getDegrees(); int slot = 0; if(isWrapping) { @@ -180,30 +186,30 @@ public void periodic() { if (EnabledSubsystems.TURRET.get()) { if (voltageOverride.isPresent()) { - motor.setVoltage(voltageOverride.get()); + turretMotor.setVoltage(voltageOverride.get()); } else { - motor.setControl(controller.withPosition(actualTargetDeg / 360.0).withSlot(slot)); + turretMotor.setControl(controller.withPosition(actualTargetDeg / 360.0).withSlot(slot)); } } else { - motor.stopMotor(); + turretMotor.stopMotor(); } if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("Superstructure/Turret/Relative Encoder Position (Rot)", motor.getPosition().getValueAsDouble() * 360.0); - SmartDashboard.putNumber("Superstructure/Turret/Closed Loop Error (deg)", motor.getClosedLoopError().getValueAsDouble() * 360.0); + SmartDashboard.putNumber("Superstructure/Turret/Relative Encoder Position (Rot)", turretMotor.getPosition().getValueAsDouble() * 360.0); + SmartDashboard.putNumber("Superstructure/Turret/Closed Loop Error (deg)", turretMotor.getClosedLoopError().getValueAsDouble() * 360.0); SmartDashboard.putNumber("Superstructure/Turret/Encoder18t Abs Position (Rot)", encoder18t.getAbsolutePosition().getValueAsDouble()); SmartDashboard.putNumber("Superstructure/Turret/Encoder17t Abs Position (Rot)", encoder17t.getAbsolutePosition().getValueAsDouble()); // SmartDashboard.putNumber("Superstructure/Turret/Vector Space Position (Deg)", getVectorSpaceAngle().getDegrees()); - SmartDashboard.putNumber("Superstructure/Turret/Voltage", motor.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Turret/Stator Current", motor.getStatorCurrent().getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Turret/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Turret/Voltage", turretMotor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Turret/Stator Current", turretMotor.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Turret/Supply Current", turretMotor.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Superstructure/Turret/Wrapped Target Angle (deg)", actualTargetDeg); - SmartDashboard.putNumber("Current Draws/Turret (amps)", motor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Current Draws/Turret (amps)", turretMotor.getSupplyCurrent().getValueAsDouble()); } } @@ -218,9 +224,9 @@ public SysIdRoutine getSysIdRoutine() { 6, "Turret", voltage -> setVoltageOverride(Optional.of(voltage)), - () -> this.motor.getPosition().getValueAsDouble(), - () -> this.motor.getVelocity().getValueAsDouble(), - () -> this.motor.getMotorVoltage().getValueAsDouble(), + () -> this.turretMotor.getPosition().getValueAsDouble(), + () -> this.turretMotor.getVelocity().getValueAsDouble(), + () -> this.turretMotor.getMotorVoltage().getValueAsDouble(), getInstance()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java index cfba49e6..ec68b67e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java @@ -142,4 +142,10 @@ public SysIdRoutine getSysIdRoutine() { () -> sim.getInput(0), getInstance()); } + + @Override + public boolean isWrapping() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'isWrapping'"); + } } \ No newline at end of file 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 11d7161a..fbef970c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -532,15 +532,15 @@ public void periodic() { if (Settings.DEBUG_MODE) {} - - SmartDashboard.putNumber("Swerve/Velocity Robot Relative X (m per s)", getChassisSpeeds().vxMetersPerSecond); - SmartDashboard.putNumber("Swerve/Velocity Robot Relative Y (m per s)", getChassisSpeeds().vyMetersPerSecond); + ChassisSpeeds chassisSpeeds = getChassisSpeeds(); + SmartDashboard.putNumber("Swerve/Velocity Robot Relative X (m per s)", chassisSpeeds.vxMetersPerSecond); + SmartDashboard.putNumber("Swerve/Velocity Robot Relative Y (m per s)", chassisSpeeds.vyMetersPerSecond); SmartDashboard.putNumber("Swerve/Velocity Field Relative X (m per s)", getFieldRelativeSpeeds().x); SmartDashboard.putNumber("Swerve/Field Relative Rotation", pose.getRotation().getDegrees()); SmartDashboard.putNumber("Swerve/Velocity Field Relative Y (m per s)", getFieldRelativeSpeeds().y); - SmartDashboard.putNumber("Swerve/Angular Velocity (rad per s)", getChassisSpeeds().omegaRadiansPerSecond); + SmartDashboard.putNumber("Swerve/Angular Velocity (rad per s)", chassisSpeeds.omegaRadiansPerSecond); SmartDashboard.putNumber("Swerve/Distance From Hub (meters)", Field.hubCenter.getTranslation().getDistance(getPose().getTranslation())); } diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java index efb9423b..f8d60b80 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java @@ -22,12 +22,16 @@ public class SOTMCalculator { public static final double g = 9.81; - public static SOTMSolution sol; + public static SOTMSolution hubSol; + public static SOTMSolution ferrySol; private static FieldObject2d hubPose2d; private static FieldObject2d virtualHubPose2d; private static FieldObject2d futureTurretPose2d; + private static FieldObject2d ferryPose2d; + private static FieldObject2d virtualFerryPose2d; + public record SOTMSolution( Rotation2d targetHoodAngle, Rotation2d targetTurretAngle, @@ -37,7 +41,7 @@ public record SOTMSolution( } static { - sol = new SOTMSolution( + hubSol = new SOTMSolution( Hood.getInstance().getAngle(), Turret.getInstance().getAngle(), Shooter.getInstance().getRPM(), @@ -45,6 +49,14 @@ public record SOTMSolution( 0.0 ); + ferrySol = new SOTMSolution( + Hood.getInstance().getAngle(), + Turret.getInstance().getAngle(), + Shooter.getInstance().getRPM(), + Field.rightFerryZone, + 0.0 + ); + hubPose2d = Field.FIELD2D.getObject("hubPose"); virtualHubPose2d = Field.FIELD2D.getObject("virtualHubPose"); futureTurretPose2d = Field.FIELD2D.getObject("futureTurretPose"); @@ -140,13 +152,13 @@ but looking at the second equation, to get the flight time we need to solveBalli ); } - public static void updateSOTMSolution() { CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); Pose2d robotPose = swerve.getPose(); Pose2d hubPose = Field.getHubPose(); + Pose2d ferryPose = Field.getFerryZonePose(robotPose.getTranslation()); ChassisSpeeds robotRelativeSpeeds = swerve.getChassisSpeeds(); ChassisSpeeds fieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds( @@ -180,7 +192,7 @@ public static void updateSOTMSolution() { ); - SOTMSolution solution = solveShootOnTheMove( + SOTMSolution hubSolution = solveShootOnTheMove( futureTurretPose, hubPose, fieldRelativeSpeeds, @@ -188,28 +200,57 @@ public static void updateSOTMSolution() { Settings.Superstructure.SOTM.TIME_TOLERANCE ); - sol = solution; + SOTMSolution ferrySolution = solveShootOnTheMove( + futureTurretPose, + ferryPose, + fieldRelativeSpeeds, + Settings.Superstructure.SOTM.MAX_ITERATIONS, + Settings.Superstructure.SOTM.TIME_TOLERANCE + ); + + hubSol = hubSolution; + ferrySol = ferrySolution; - hubPose2d.setPose(Robot.isBlue() ? hubPose : Field.transformToOppositeAlliance(hubPose)); - virtualHubPose2d.setPose((Robot.isBlue() ? sol.virtualPose() : Field.transformToOppositeAlliance(sol.virtualPose()))); + ferryPose2d.setPose(Robot.isBlue() ? ferryPose : Field.transformToOppositeAlliance(hubPose)); + virtualFerryPose2d.setPose((Robot.isBlue() ? ferrySol.virtualPose() : Field.transformToOppositeAlliance(ferrySol.virtualPose()))); + + // hubPose2d.setPose(Robot.isBlue() ? hubPose : Field.transformToOppositeAlliance(hubPose)); + // virtualHubPose2d.setPose((Robot.isBlue() ? hubSol.virtualPose() : Field.transformToOppositeAlliance(hubSol.virtualPose()))); futureTurretPose2d.setPose((Robot.isBlue() ? futureTurretPose : Field.transformToOppositeAlliance(futureTurretPose))); - SmartDashboard.putNumber("Superstructure/SOTM/calculated turret angle", sol.targetTurretAngle().getDegrees()); - SmartDashboard.putNumber("Superstructure/SOTM/calculated hood angle", sol.targetHoodAngle().getDegrees()); - SmartDashboard.putNumber("Superstructure/SOTM/calculated flight time", sol.flightTime()); - SmartDashboard.putNumber("Superstructure/SOTM/turret dist to virtual pose", futureTurretPose.getTranslation().getDistance(sol.virtualPose().getTranslation())); + SmartDashboard.putNumber("Superstructure/SOTM/calculated turret angle", hubSol.targetTurretAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/SOTM/calculated hood angle", hubSol.targetHoodAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/SOTM/calculated flight time", hubSol.flightTime()); + SmartDashboard.putNumber("Superstructure/SOTM/turret dist to virtual pose", futureTurretPose.getTranslation().getDistance(hubSol.virtualPose().getTranslation())); + + SmartDashboard.putNumber("Superstructure/FOTM/calculated turret angle", ferrySol.targetTurretAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/FOTM/calculated hood angle", ferrySol.targetHoodAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/FOTM/calculated flight time", ferrySol.flightTime()); + SmartDashboard.putNumber("Superstructure/FOTM/turret dist to ferry pose", futureTurretPose.getTranslation().getDistance(ferrySol.virtualPose().getTranslation())); } public static Rotation2d calculateHoodAngleSOTM() { - return sol.targetHoodAngle(); + return hubSol.targetHoodAngle(); } public static Rotation2d calculateTurretAngleSOTM() { - return sol.targetTurretAngle(); + return hubSol.targetTurretAngle(); } public static double calculateShooterRPMSOTM() { - return sol.targetShooterRPM(); + return hubSol.targetShooterRPM(); + } + + public static Rotation2d calculateHoodAngleFOTM() { + return ferrySol.targetHoodAngle(); + } + + public static Rotation2d calculateTurretAngleFOTM() { + return ferrySol.targetTurretAngle(); + } + + public static double calculateShooterRPMFOTM() { + return ferrySol.targetShooterRPM(); } } \ No newline at end of file From 1069d6ba187a751c4a3f841d342a0679b6df55aa Mon Sep 17 00:00:00 2001 From: Brian Zheng Date: Sat, 7 Mar 2026 21:07:00 -0500 Subject: [PATCH 109/150] FIX: fotm --- .../stuypulse/robot/util/superstructure/SOTMCalculator.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java index f8d60b80..5f8babb8 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java @@ -59,6 +59,10 @@ public record SOTMSolution( hubPose2d = Field.FIELD2D.getObject("hubPose"); virtualHubPose2d = Field.FIELD2D.getObject("virtualHubPose"); + + ferryPose2d = Field.FIELD2D.getObject("ferryPose"); + virtualFerryPose2d = Field.FIELD2D.getObject("virtualFerryPose"); + futureTurretPose2d = Field.FIELD2D.getObject("futureTurretPose"); } @@ -211,7 +215,7 @@ public static void updateSOTMSolution() { hubSol = hubSolution; ferrySol = ferrySolution; - ferryPose2d.setPose(Robot.isBlue() ? ferryPose : Field.transformToOppositeAlliance(hubPose)); + ferryPose2d.setPose(Robot.isBlue() ? ferryPose : Field.transformToOppositeAlliance(ferryPose)); virtualFerryPose2d.setPose((Robot.isBlue() ? ferrySol.virtualPose() : Field.transformToOppositeAlliance(ferrySol.virtualPose()))); // hubPose2d.setPose(Robot.isBlue() ? hubPose : Field.transformToOppositeAlliance(hubPose)); From 2c422a7f2d1c3c89250a25f23960d46bc7e9d6c4 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Sat, 7 Mar 2026 22:17:36 -0500 Subject: [PATCH 110/150] MORE CHANGES --- .../com/stuypulse/robot/RobotContainer.java | 6 ++--- .../stuypulse/robot/constants/Cameras.java | 2 +- .../com/stuypulse/robot/constants/Gains.java | 24 +++++++++---------- .../stuypulse/robot/constants/Settings.java | 12 +++++----- .../subsystems/spindexer/SpindexerImpl.java | 4 +--- .../superstructure/shooter/ShooterImpl.java | 8 +++---- .../superstructure/turret/TurretImpl.java | 2 +- .../swerve/CommandSwerveDrivetrain.java | 1 + 8 files changed, 29 insertions(+), 30 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 44ae59b9..570f4558 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -101,7 +101,7 @@ public interface EnabledSubsystems { SmartBoolean SHOOTER = new SmartBoolean("Enabled Subsystems/Shooter Is Enabled", true); SmartBoolean BACK_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Back Limelight Is Enabled", true); - SmartBoolean LEFT_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Left Limelight Is Enabled", false); + SmartBoolean LEFT_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Left Limelight Is Enabled", true); SmartBoolean RIGHT_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Right Limelight Is Enabled", true); } @@ -206,7 +206,7 @@ private void configureButtonBindings() { driver.getLeftBumper() .onTrue(new IntakeStopRollers()); - // driver.getRightBumper() + // driver.getRightBumper() // .whileTrue(new IntakeOuttake()) // .onFalse(new IntakeRunRollers()); @@ -233,7 +233,7 @@ private void configureButtonBindings() { .alongWith(new SpindexerRun())) .onFalse(new SuperstructureFerry().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); - // FOTM + // FOTM // driver.getDPadRight() // .onTrue(new IntakeRunRollers()) // .onTrue(new ConditionalCommand( diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index c5b2b48d..2fb33454 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -26,7 +26,7 @@ public interface Cameras { new Camera("limelight-left", new Pose3d(Units.inchesToMeters(2.490), Units.inchesToMeters(14.8620), Units.inchesToMeters(7.920157), // new Pose3d(0.0,0.0,0.0, - new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15.0), Units.degreesToRadians(289.0))), + new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15.0), Units.degreesToRadians(71.0))), //289 RobotContainer.EnabledSubsystems.LEFT_LIMELIGHT), new Camera("limelight-back", //11.0845u new Pose3d(Units.inchesToMeters(-10.768823), Units.inchesToMeters(-12.717519), Units.inchesToMeters(8.331714), diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 14895ee7..396bc1bf 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -32,23 +32,23 @@ public interface ClimberHopper { public interface Superstructure { public interface Shooter { - SmartNumber kP = new SmartNumber("Superstructure/Shooter/Gains/kP", 10.0); - SmartNumber kI = new SmartNumber("Superstructure/Shooter/Gains/kI", 0.0); - SmartNumber kD = new SmartNumber("Superstructure/Shooter/Gains/kD", 0.0); - - SmartNumber kS = new SmartNumber("Superstructure/Shooter/Gains/kS", 10.32); - SmartNumber kV = new SmartNumber("Superstructure/Shooter/Gains/kV", 0.0835); - SmartNumber kA = new SmartNumber("Superstructure/Shooter/Gains/kA", 0.0); - - // BELOW ARE VELOCITY PID AND VOLTAGEBASED FF GAINS - // SmartNumber kP = new SmartNumber("Superstructure/Shooter/Gains/kP", 0.45); + // SmartNumber kP = new SmartNumber("Superstructure/Shooter/Gains/kP", 10.0); // SmartNumber kI = new SmartNumber("Superstructure/Shooter/Gains/kI", 0.0); // SmartNumber kD = new SmartNumber("Superstructure/Shooter/Gains/kD", 0.0); - // SmartNumber kS = new SmartNumber("Superstructure/Shooter/Gains/kS", 0.0); - // SmartNumber kV = new SmartNumber("Superstructure/Shooter/Gains/kV", 0.123);//0.11650); + // SmartNumber kS = new SmartNumber("Superstructure/Shooter/Gains/kS", 10.32); + // SmartNumber kV = new SmartNumber("Superstructure/Shooter/Gains/kV", 0.0835); // SmartNumber kA = new SmartNumber("Superstructure/Shooter/Gains/kA", 0.0); + // BELOW ARE VELOCITY PID AND VOLTAGEBASED FF GAINS + SmartNumber kP = new SmartNumber("Superstructure/Shooter/Gains/kP", 0.45); + SmartNumber kI = new SmartNumber("Superstructure/Shooter/Gains/kI", 0.0); + SmartNumber kD = new SmartNumber("Superstructure/Shooter/Gains/kD", 0.0); + + SmartNumber kS = new SmartNumber("Superstructure/Shooter/Gains/kS", 0.0); + SmartNumber kV = new SmartNumber("Superstructure/Shooter/Gains/kV", 0.123);//0.11650); + SmartNumber kA = new SmartNumber("Superstructure/Shooter/Gains/kA", 0.0); + // double kP = 0.45; // double kI = 0.0; // double kD = 0.0; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 1f78fe59..85984b26 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -155,10 +155,10 @@ public interface AngleInterpolation { public interface RPMInterpolation{ double[][] distanceRPMInterpolationValues = { {1.22, 2700.0}, //BLAY-APPROVED, LOCKED IN - {2.15, 2900.0}, //BLAY-APPROVED + {2.15, 2930.0}, //BLAY-APPROVED {3.38, 3200}, //BLAY-APPROVED {4.43, 3550.0}, //BLAY-APPROVED - {5.66, 3900.0} //KEVIN-APPROVED + {5.66, 3850.0} //KEVIN-APPROVED }; } @@ -174,10 +174,10 @@ public interface TOFInterpolation{ public interface FerryRPMInterpolation { double[][] ferryDistanceRPMInterpolation = { - {5.16, 3800.0}, - {6.94, 4050.0}, - {7.87, 4300.0}, - {9.77, 4550.0} + {5.16, 3500.0}, + {6.94, 3700.0}, + {7.87, 4000.0}, + {9.77, 4500.0} }; } 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 c38134b6..109568e2 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -98,9 +98,7 @@ public void periodic() { leadMotor.setVoltage(voltageOverride.get()); } else { /** - * THE FOLLOWING CODE IS NOT A MISTAKE - * - * WHEN STOPPING THE + * THE FOLLOWING LINE IS NOT A MISTAKE */ if (atTolerance() && getState() == SpindexerState.STOP) { leadMotor.stopMotor(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java index c7e04f3a..33121c29 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -32,8 +32,8 @@ public class ShooterImpl extends Shooter { private final TalonFX shooterLeader; private final TalonFX shooterFollower; - // private final VelocityVoltage shooterController; - private final VelocityTorqueCurrentFOC shooterController; + private final VelocityVoltage shooterController; + // private final VelocityTorqueCurrentFOC shooterController; private final Follower follower; private Optional voltageOverride; @@ -61,8 +61,8 @@ public ShooterImpl() { shooterConfig.configure(shooterLeader); shooterConfig.configure(shooterFollower); - // shooterController = new VelocityVoltage(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE).withEnableFOC(true); - shooterController = new VelocityTorqueCurrentFOC(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE); + shooterController = new VelocityVoltage(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE).withEnableFOC(true); + // shooterController = new VelocityTorqueCurrentFOC(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE); follower = new Follower(Ports.Superstructure.Shooter.MOTOR_LEAD, MotorAlignmentValue.Opposed); shooterFollower.setControl(follower); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index be16a1a6..a13ec5c1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -206,7 +206,7 @@ public void periodic() { SmartDashboard.putNumber("Superstructure/Turret/Voltage", turretMotor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Superstructure/Turret/Stator Current", turretMotor.getStatorCurrent().getValueAsDouble()); SmartDashboard.putNumber("Superstructure/Turret/Supply Current", turretMotor.getSupplyCurrent().getValueAsDouble()); - + SmartDashboard.putNumber("Superstructure/Turret/Wrapped Target Angle (deg)", actualTargetDeg); SmartDashboard.putNumber("Current Draws/Turret (amps)", turretMotor.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 fbef970c..d507d386 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -506,6 +506,7 @@ public void periodic() { SmartDashboard.putNumber("Superstructure/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("InterpolationTesting/Turret Dist From Ferry Zone", turretPose.getTranslation().getDistance(Field.getFerryZonePose(pose.getTranslation()).getTranslation())); SmartDashboard.putNumber("Swerve/Pose/X", pose.getX()); SmartDashboard.putNumber("Swerve/Pose/Y", pose.getY()); From 668b16f3f41a42191a73d17c2670444e3fbdfa81 Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Sun, 8 Mar 2026 19:31:09 -0400 Subject: [PATCH 111/150] CLEAN: remove dead code in RobotContainer, push intake and handoff sim --- simgui-ds.json | 29 ++- simgui.json | 15 +- .../com/stuypulse/robot/RobotContainer.java | 154 ++----------- .../handoff/HandoffConditionalCommand.java | 19 +- .../SpindexerConditionalCommand.java | 23 +- .../commands/spindexer/SpindexerDynamic.java | 14 -- .../SuperstructureDefaultCommand.java | 39 ---- .../SuperstructureSOTMConditional.java | 11 +- .../robot/commands/turret/TurretSeed.java | 25 -- .../robot/commands/turret/TurretSetState.java | 1 - .../robot/subsystems/handoff/Handoff.java | 7 +- .../robot/subsystems/handoff/HandoffSim.java | 104 +++++++++ .../robot/subsystems/intake/Intake.java | 7 +- .../robot/subsystems/intake/IntakeImpl.java | 30 +-- .../robot/subsystems/intake/IntakeSim.java | 214 ++++++++++++++++++ .../robot/subsystems/spindexer/Spindexer.java | 18 -- .../subsystems/spindexer/SpindexerImpl.java | 5 +- .../superstructure/hood/HoodImpl.java | 4 - .../superstructure/hood/HoodSim.java | 2 - .../superstructure/shooter/ShooterSim.java | 2 +- .../robot/util/SpindexerInterpolation.java | 28 --- 21 files changed, 427 insertions(+), 324 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDynamic.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureDefaultCommand.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/turret/TurretSeed.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/intake/IntakeSim.java delete mode 100644 src/main/java/com/stuypulse/robot/util/SpindexerInterpolation.java diff --git a/simgui-ds.json b/simgui-ds.json index 20138e7d..f7c505bf 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "Keyboard 0 Settings": { + "window": { + "visible": true + } + }, "keyboardJoysticks": [ { "axisConfig": [ @@ -18,13 +23,20 @@ } ], "axisCount": 3, - "buttonCount": 5, + "buttonCount": 12, "buttonKeys": [ 90, 88, 67, 86, - 66 + 66, + 78, + 77, + 71, + 72, + 74, + 75, + 76 ], "povConfig": [ { @@ -42,19 +54,15 @@ }, { "axisConfig": [ + {}, { - "decKey": 74, - "incKey": 76 - }, - { - "decKey": 73, - "incKey": 75 + "decKey": 73 } ], "axisCount": 2, "buttonCount": 4, "buttonKeys": [ - 77, + -1, 44, 46, 47 @@ -92,8 +100,7 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "Keyboard0" } ] } diff --git a/simgui.json b/simgui.json index 8f995dc1..df4fe77c 100644 --- a/simgui.json +++ b/simgui.json @@ -42,6 +42,16 @@ "window": { "visible": true } + }, + "/SmartDashboard/Visualizers/Hood": { + "window": { + "visible": true + } + }, + "/SmartDashboard/Visualizers/Turret": { + "window": { + "visible": true + } } } }, @@ -57,7 +67,7 @@ "Enabled Subsystems": { "open": true }, - "FieldPositions": { + "Handoff": { "open": true }, "HoodedShooter": { @@ -73,9 +83,6 @@ "open": true }, "Superstructure": { - "Hood": { - "open": true - }, "open": true }, "Swerve": { diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 570f4558..8962c733 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -14,20 +14,14 @@ import com.stuypulse.robot.commands.auton.regular.DepotAuton; import com.stuypulse.robot.commands.auton.regular.EightFuel; import com.stuypulse.robot.commands.auton.regular.TopTwoCycle; -import com.stuypulse.robot.commands.climberhopper.ClimberDown; -// import com.stuypulse.robot.commands.auton.test.BoxTest; import com.stuypulse.robot.commands.climberhopper.ClimberOverrideDown; import com.stuypulse.robot.commands.climberhopper.ClimberOverrideStop; import com.stuypulse.robot.commands.climberhopper.ClimberOverrideUp; import com.stuypulse.robot.commands.handoff.HandoffConditionalCommand; -import com.stuypulse.robot.commands.climberhopper.ClimberUp; -import com.stuypulse.robot.commands.climberhopper.HopperDown; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.hood.ZeroHoodEncoderAtUpperHardstop; import com.stuypulse.robot.commands.intake.IntakeDeploy; -import com.stuypulse.robot.commands.intake.IntakeDigestion; -import com.stuypulse.robot.commands.intake.IntakeOuttake; import com.stuypulse.robot.commands.intake.IntakeRunRollers; import com.stuypulse.robot.commands.intake.IntakeStopRollers; import com.stuypulse.robot.commands.intake.IntakeStow; @@ -36,35 +30,22 @@ import com.stuypulse.robot.commands.spindexer.SpindexerConditionalCommand; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.superstructure.SuperstructureDefaultCommand; import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; import com.stuypulse.robot.commands.superstructure.SuperstructureFerry; import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; -import com.stuypulse.robot.commands.superstructure.SuperstructureKB; -import com.stuypulse.robot.commands.superstructure.SuperstructureLeftCorner; -import com.stuypulse.robot.commands.superstructure.SuperstructureRightCorner; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; -import com.stuypulse.robot.commands.superstructure.SuperstructureSOTMConditional; import com.stuypulse.robot.commands.superstructure.SuperstructureShoot; import com.stuypulse.robot.commands.superstructure.SuperstructureStow; -import com.stuypulse.robot.commands.swerve.SwerveDriveAlignTurretToHub; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveWheelRadiusCharacterization; import com.stuypulse.robot.commands.swerve.SwerveXMode; -import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; -import com.stuypulse.robot.commands.turret.TurretDefaultCommand; -import com.stuypulse.robot.commands.turret.TurretIdle; -import com.stuypulse.robot.commands.turret.TurretShoot; import com.stuypulse.robot.commands.turret.ZeroTurret; import com.stuypulse.robot.commands.vision.ResetLimelightIMU; import com.stuypulse.robot.commands.vision.SetIMUMode; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Ports; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Gains.Intake.Pivot; import com.stuypulse.robot.subsystems.handoff.Handoff; -import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; import com.stuypulse.robot.subsystems.superstructure.hood.Hood; @@ -142,7 +123,6 @@ public RobotContainer() { SmartDashboard.putData("Robot/Override Up", new ClimberOverrideUp()); SmartDashboard.putData("Robot/Override Down", new ClimberOverrideDown()); SmartDashboard.putData("Robot/Override Stop", new ClimberOverrideStop()); - } /****************/ @@ -161,20 +141,7 @@ private void configureDefaultCommands() { /***************/ private void configureButtonBindings() { - // Scoring Routine - driver.getBottomButton() - .whileTrue(new SwerveXMode()) - .onTrue(new IntakeRunRollers()) - .whileTrue(new SuperstructureShoot() - .andThen(new WaitUntilCommand(superstructure::atTolerance)) - .andThen(new HandoffRun()) - .andThen(new WaitUntilCommand(handoff::atTolerance)) - .andThen(new SpindexerRun())) - .onFalse(new SpindexerStop() - .alongWith(new SuperstructureStow()) - .alongWith(new HandoffStop())); - - // Interpolation-based Scoring + // Scoring Routine driver.getTopButton() .whileTrue(new SwerveXMode()) .onTrue(new IntakeRunRollers()) @@ -210,6 +177,15 @@ private void configureButtonBindings() { // .whileTrue(new IntakeOuttake()) // .onFalse(new IntakeRunRollers()); + // Ferrying In Place + driver.getDPadRight() + .whileTrue(new SwerveXMode()) + .whileTrue(new SuperstructureFerry().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + .andThen(new HandoffRun()) + .andThen(new WaitUntilCommand(handoff::atTolerance)) + .alongWith(new SpindexerRun())) + .onFalse(new SuperstructureFerry().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); + // SOTM driver.getRightMenuButton() .onTrue(new IntakeRunRollers()) @@ -224,78 +200,22 @@ private void configureButtonBindings() { () -> superstructure.getState() == SuperstructureState.SOTM )); - // Ferrying In Place - driver.getDPadRight() - .whileTrue(new SwerveXMode()) - .whileTrue(new SuperstructureFerry().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) - .andThen(new HandoffRun()) - .andThen(new WaitUntilCommand(handoff::atTolerance)) - .alongWith(new SpindexerRun())) - .onFalse(new SuperstructureFerry().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); - // FOTM - // driver.getDPadRight() - // .onTrue(new IntakeRunRollers()) - // .onTrue(new ConditionalCommand( - // new ParallelCommandGroup( - // new SuperstructureInterpolation(), - // new SpindexerStop(), - // new HandoffStop() - // ), - // new SuperstructureFOTM().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) - // .andThen(new SpindexerRun()).alongWith(new HandoffRun()), - // () -> superstructure.getState() == SuperstructureState.FOTM - // )); - - // digestion by moving the pivot - // driver.getDPadDown() - // .whileTrue(new IntakeDigestion()) - // .onFalse(new IntakeDeploy()); - - // Test Turret - // driver.getBottomButton() - // .whileTrue(new TurretShoot()) - // .onFalse(new TurretIdle()); - - // Scoring Routine - // driver.getBottomButton() - // .whileTrue(new SuperstructureShoot())//.onlyIf(() -> !superstructure.isHoodUnderTrench())) - // // .alongWith(new SwerveDriveAlignTurretToHub()) - // // .alongWith(new TurretShoot()) - // .whileTrue(new SuperstructureShoot().onlyIf(() -> !swerve.isUnderTrench()) - // .alongWith(new SwerveDriveAlignTurretToHub()) - // // .alongWith(new TurretShoot()) - // .andThen(new WaitUntilCommand(superstructure::atTolerance)) - // .andThen(new HandoffConditionalCommand().onlyIf(superstructure::atTolerance) - // .alongWith(new WaitUntilCommand(handoff::atTolerance)) - // .andThen(new SpindexerConditionalCommand().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) - // .onFalse(new SpindexerStop() - // .alongWith(new SuperstructureStow()) - // .alongWith(new HandoffStop())); - - // driver.getRightBumper() - // .whileTrue(new SwerveWheelRadiusCharacterization()); - - // driver.getRightButton() - // .whileTrue(new SuperstructureFerry().onlyIf(() -> !swerve.isUnderTrench()) - // // .alongWith(new TurretShoot()) - // .andThen(new WaitUntilCommand(superstructure::atTolerance)) - // .andThen(new HandoffRun().onlyIf(superstructure::atTolerance) - // .alongWith(new WaitUntilCommand(handoff::atTolerance)) - // .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && superstructure.atTolerance())))) - // .onFalse(new SpindexerStop() - // .alongWith(new SuperstructureStow()) - // .alongWith(new HandoffStop())); + driver.getLeftMenuButton() + .onTrue(new IntakeRunRollers()) + .onTrue(new ConditionalCommand( + new ParallelCommandGroup( + new SuperstructureInterpolation(), + new SpindexerStop(), + new HandoffStop() + ), + new SuperstructureFOTM().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + .andThen(new SpindexerRun()).alongWith(new HandoffRun()), + () -> superstructure.getState() == SuperstructureState.FOTM + )); //--------------------------------------------------------------------------------------------------------------------------\\ - // // Scoring In Place - // driver.getTopButton() - // .whileTrue(new SwerveXMode()) - // .whileTrue(new SuperstructureInterpolation().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) - // .andThen(new SpindexerRun()).alongWith(new HandoffRun())) - // .onFalse(new SuperstructureInterpolation().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); - // // Manual Left Corner Scoring // driver.getLeftButton() // .whileTrue(new SwerveXMode()) @@ -317,27 +237,6 @@ private void configureButtonBindings() { // .andThen(new SpindexerRun()).alongWith(new HandoffRun())) // .onFalse(new SuperstructureInterpolation().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); - // // Reset Heading - // driver.getDPadUp() - // .onTrue(new SwerveResetHeading()) - // .onTrue(new ResetLimelightIMU()) - // .onFalse(new SetIMUMode(0)); - - - // /** - // // Ferrying SOTM - // driver.getLeftMenuButton() - // .onTrue(new SuperstructureSOTM().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) - // .andThen(new SpindexerRun()).alongWith(new HandoffRun())) - // .onFalse(new SuperstructureSOTM().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); - // **/ - - // // Scoring SOTM - // driver.getRightMenuButton() - // .onTrue(new SuperstructureSOTM().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) - // .andThen(new SpindexerRun()).alongWith(new HandoffRun())) - // .onFalse(new SuperstructureSOTM().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); - // // Swerve X Wheels // driver.getLeftBumper() // .whileTrue(new SwerveXMode()); @@ -349,15 +248,6 @@ private void configureButtonBindings() { // .andThen(new SwerveClimbAlign())) // .onFalse(new ClimberDown()); // **/ - - // // Stow Intake - // driver.getLeftTriggerButton() - // .onTrue(new IntakeStow()); - - // // Deploy Intake - // driver.getRightTriggerButton() - // .onTrue(new IntakeDeploy()); - } /**************/ diff --git a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffConditionalCommand.java b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffConditionalCommand.java index 5b79f3a2..0a620f1e 100644 --- a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffConditionalCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffConditionalCommand.java @@ -1,14 +1,23 @@ +/************************ 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.commands.handoff; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.ConditionalCommand; public class HandoffConditionalCommand extends ConditionalCommand { public HandoffConditionalCommand() { - super(new HandoffStop(), new HandoffRun(), () -> (CommandSwerveDrivetrain.getInstance().isBehindTower() || CommandSwerveDrivetrain.getInstance().isBehindHub())); - // DEBUG - SmartDashboard.putBoolean("FieldPositions/Should Handoff Stop", (CommandSwerveDrivetrain.getInstance().isBehindTower() || CommandSwerveDrivetrain.getInstance().isBehindHub())); + super( + new HandoffStop(), + new HandoffRun(), + () -> { + CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); + return swerve.isBehindTower() || swerve.isBehindHub(); + } + ); } } diff --git a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java index ea022156..7e1dc06b 100644 --- a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java @@ -1,18 +1,23 @@ /************************ 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. */ -/***************************************************************/ +/* 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.commands.spindexer; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj2.command.ConditionalCommand; public class SpindexerConditionalCommand extends ConditionalCommand { public SpindexerConditionalCommand() { - super(new SpindexerStop(), new SpindexerRun() ,() -> (CommandSwerveDrivetrain.getInstance().isBehindTower() || CommandSwerveDrivetrain.getInstance().isBehindHub())); - // DEBUG - SmartDashboard.putBoolean("FieldPositions/Should Spindexer Stop", (CommandSwerveDrivetrain.getInstance().isBehindTower() || CommandSwerveDrivetrain.getInstance().isBehindHub())); + super( + new SpindexerStop(), + new SpindexerRun(), + () -> { + CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); + return swerve.isBehindTower() || swerve.isBehindHub(); + } + ); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDynamic.java b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDynamic.java deleted file mode 100644 index c17ab7e8..00000000 --- a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDynamic.java +++ /dev/null @@ -1,14 +0,0 @@ -/************************ 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.commands.spindexer; - -import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; - -public class SpindexerDynamic extends SpindexerSetState { - public SpindexerDynamic() { - super(SpindexerState.DYNAMIC); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureDefaultCommand.java deleted file mode 100644 index 150b8ade..00000000 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureDefaultCommand.java +++ /dev/null @@ -1,39 +0,0 @@ -package com.stuypulse.robot.commands.superstructure; - -import com.stuypulse.robot.commands.spindexer.SpindexerRun; -import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class SuperstructureDefaultCommand extends Command { - private final CommandSwerveDrivetrain swerve; - private final Superstructure superstructure; - private boolean trench = false; - - public SuperstructureDefaultCommand() { - swerve = CommandSwerveDrivetrain.getInstance(); - superstructure = Superstructure.getInstance(); - - addRequirements(superstructure); - } - - @Override - public void execute() { - if (swerve.isUnderTrench() && trench == false) { - CommandScheduler.getInstance().schedule(new SuperstructureStow()); - trench = true; - } else { - trench = false; - } - - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTMConditional.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTMConditional.java index cf694fae..b735e6e0 100644 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTMConditional.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTMConditional.java @@ -1,16 +1,15 @@ package com.stuypulse.robot.commands.superstructure; -import com.stuypulse.robot.commands.spindexer.SpindexerRun; -import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.ConditionalCommand; public class SuperstructureSOTMConditional extends ConditionalCommand { public SuperstructureSOTMConditional() { - super(new SuperstructureInterpolation(), new SuperstructureSOTM(), () -> (Superstructure.getInstance().getState() == SuperstructureState.SOTM)); + super( + new SuperstructureInterpolation(), + new SuperstructureSOTM(), + () -> Superstructure.getInstance().getState() == SuperstructureState.SOTM + ); } } diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretSeed.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretSeed.java deleted file mode 100644 index 0bcb7a57..00000000 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretSeed.java +++ /dev/null @@ -1,25 +0,0 @@ -/************************ 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.commands.turret; - -import com.stuypulse.robot.subsystems.superstructure.turret.Turret; - -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class TurretSeed extends InstantCommand { - private Turret turret; - - public TurretSeed() { - turret = Turret.getInstance(); - - addRequirements(turret); - } - - @Override - public void initialize() { - turret.seedTurret(); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretSetState.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretSetState.java index 09bbb1b1..479fa504 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretSetState.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretSetState.java @@ -8,7 +8,6 @@ import com.stuypulse.robot.subsystems.superstructure.turret.Turret; import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; public class TurretSetState extends InstantCommand { diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java index db900f03..d6abba2d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java @@ -5,6 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.handoff; +import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -18,7 +19,11 @@ public abstract class Handoff extends SubsystemBase { private HandoffState state; static { - instance = new HandoffImpl(); + if (Robot.isReal()) { + instance = new HandoffImpl(); + } else { + instance = new HandoffSim(); + } } public static Handoff getInstance() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java new file mode 100644 index 00000000..59a734f4 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java @@ -0,0 +1,104 @@ +/************************ 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.subsystems.handoff; + +import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.SysId; + +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.LinearQuadraticRegulator; +import edu.wpi.first.math.estimator.KalmanFilter; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.LinearSystemLoop; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.LinearSystemSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +import java.util.Optional; + +public class HandoffSim extends Handoff { + + private LinearSystemSim sim; + private final LinearSystemLoop controller; + + private Optional voltageOverride; + + public HandoffSim() { + LinearSystem flywheel = LinearSystemId.createFlywheelSystem(DCMotor.getKrakenX44(1), 0.01, 1.0); + + sim = new LinearSystemSim<>(flywheel); + + LinearQuadraticRegulator lqr = new LinearQuadraticRegulator( + flywheel, + VecBuilder.fill(8.0), + VecBuilder.fill(12.0), + Settings.DT); + + KalmanFilter kalmanFilter = new KalmanFilter<>( + Nat.N1(), + Nat.N1(), + flywheel, + VecBuilder.fill(3.0), + VecBuilder.fill(0.01), + Settings.DT); + + controller = new LinearSystemLoop<>(flywheel, lqr, kalmanFilter, 12.0, Settings.DT); + + voltageOverride = Optional.empty(); + } + + @Override + public double getCurrentRPM() { + return sim.getOutput(0) * 60.0 / (2.0 * Math.PI); // convert to RPM + } + + @Override + public void periodic() { + super.periodic(); + + controller.setNextR(VecBuilder.fill(getTargetRPM() * 2.0 * Math.PI / 60.0)); + controller.correct(VecBuilder.fill(sim.getOutput(0))); + controller.predict(Settings.DT); + + if (EnabledSubsystems.SHOOTER.get()) { + if (voltageOverride.isPresent()) { + sim.setInput(voltageOverride.get()); + SmartDashboard.putNumber("Handoff/Input Voltage", voltageOverride.get()); + } else { + SmartDashboard.putNumber("Handoff/Input Voltage", controller.getU(0)); + sim.setInput(controller.getU(0)); + } + } else { + sim.setInput(0); + SmartDashboard.putNumber("Handoff/Input Voltage", 0.0); + } + + sim.update(Settings.DT); + } + + @Override + public void setVoltageOverride(Optional volts) { + voltageOverride = volts; + } + + @Override + public SysIdRoutine getSysIdRoutine() { + return SysId.getRoutine( + 2, + 6, + "Handoff", + voltage -> setVoltageOverride(Optional.of(voltage)), + () -> 0.0, + () -> 0.0, + () -> sim.getInput(0), + getInstance()); + } +} \ 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 14623cbb..8d9dca46 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -5,6 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.intake; +import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; import edu.wpi.first.math.geometry.Rotation2d; @@ -21,7 +22,11 @@ public abstract class Intake extends SubsystemBase { private RollerState rollerState; static { - instance = new IntakeImpl(); + if (Robot.isReal()) { + instance = new IntakeImpl(); + } else { + instance = new IntakeSim(); + } } public static Intake getInstance() { 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 da21a016..807a4f9a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -163,8 +163,7 @@ public void periodic() { pivot.setVoltage(pivotVoltageOverride.get()); } else { // PIVOT - if (pivotState == PivotState.DEPLOY - && getPivotAngle().getDegrees() <= Settings.Intake.ARBITRARY_VOLTAGE_THRESHOLD.getDegrees()) { + if (pivotState == PivotState.DEPLOY && getPivotAngle().getDegrees() <= Settings.Intake.ARBITRARY_VOLTAGE_THRESHOLD.getDegrees()) { pivot.setControl(new VoltageOut(-Settings.Intake.PUSHDOWN_VOLTAGE)); // applying 3 volts } else if (pivotState == PivotState.DIGESTION_DOWN || pivotState == PivotState.DIGESTION_UP) { pivot.setControl(new MotionMagicVoltage(pivotState.getTargetAngle().getRotations())); //TODO: verify motion profile works @@ -173,8 +172,7 @@ && getPivotAngle().getDegrees() <= Settings.Intake.ARBITRARY_VOLTAGE_THRESHOLD.g } // ROLLERS - if (pivotState == PivotState.DEPLOY - && getPivotAngle().getDegrees() <= Settings.Intake.THRESHOLD_TO_START_ROLLERS.getDegrees()) { + if (pivotState == PivotState.DEPLOY && getPivotAngle().getDegrees() <= Settings.Intake.THRESHOLD_TO_START_ROLLERS.getDegrees()) { rollerLeader.setControl(rollerController.withOutput(getRollerState().getTargetDutyCycle())); } else { rollerLeader.stopMotor(); @@ -203,30 +201,22 @@ && getPivotAngle().getDegrees() <= Settings.Intake.THRESHOLD_TO_START_ROLLERS.ge SmartDashboard.putNumber("Intake/Pivot Angle Error (deg)", Math.abs(getPivotState().getTargetAngle().getDegrees() - getPivotAngle().getDegrees())); - SmartDashboard.putNumber("Intake/Pivot Closed Loop Error (deg)", - pivot.getClosedLoopError().getValueAsDouble() * 360.0); + SmartDashboard.putNumber("Intake/Pivot Closed Loop Error (deg)", pivot.getClosedLoopError().getValueAsDouble() * 360.0); // ROLLERS - SmartDashboard.putNumber("Intake/Roller Leader Voltage (volts)", - rollerLeader.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("Intake/Roller Follower Voltage (volts)", - rollerFollower.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Roller Leader Voltage (volts)", rollerLeader.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Roller Follower Voltage (volts)", rollerFollower.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("Intake/Roller Leader Current (amps)", - rollerLeader.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("Intake/Roller Follower Current (amps)", - rollerFollower.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Roller Leader Current (amps)", rollerLeader.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Roller Follower Current (amps)", rollerFollower.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Intake/Pivot Temperature (C)", pivot.getDeviceTemp().getValueAsDouble()); SmartDashboard.putNumber("Intake/Leader Temperature (C)", rollerLeader.getDeviceTemp().getValueAsDouble()); - SmartDashboard.putNumber("Intake/Follower Temperature (C)", - rollerFollower.getDeviceTemp().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Follower Temperature (C)", rollerFollower.getDeviceTemp().getValueAsDouble()); SmartDashboard.putNumber("Current Draws/Intake Pivot (amps)", pivot.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("Current Draws/Intake Roller Leader (amps)", - rollerLeader.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("Current Draws/Intake Roller Follower (amps)", - rollerFollower.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Current Draws/Intake Roller Leader (amps)", rollerLeader.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Current Draws/Intake Roller Follower (amps)", rollerFollower.getSupplyCurrent().getValueAsDouble()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeSim.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeSim.java new file mode 100644 index 00000000..c41a6e6a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeSim.java @@ -0,0 +1,214 @@ +/************************ 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.subsystems.intake; + +import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.SysId; + +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.LinearQuadraticRegulator; +import edu.wpi.first.math.estimator.KalmanFilter; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.LinearSystemLoop; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.LinearSystemSim; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +import java.util.Optional; + +public class IntakeSim extends Intake { + + private static final double ARM_LENGTH_METERS = 0.4; + private static final double ARM_MASS_KG = 2.0; + private static final double MOI = SingleJointedArmSim.estimateMOI(ARM_LENGTH_METERS, ARM_MASS_KG); + + private final SingleJointedArmSim pivotSim; + private final LinearSystemLoop pivotController; + + private final LinearSystemSim rollerLeaderSim; + private final LinearSystemSim rollerFollowerSim; + private final LinearSystemLoop rollerLeaderController; + private final LinearSystemLoop rollerFollowerController; + + private Optional pivotVoltageOverride; + + public IntakeSim() { + LinearSystem pivotSystem = LinearSystemId.createSingleJointedArmSystem( + DCMotor.getKrakenX60(1), + MOI, + Settings.Intake.GEAR_RATIO + ); + + KalmanFilter kalmanFilter = new KalmanFilter<>( + Nat.N2(), + Nat.N2(), + pivotSystem, + VecBuilder.fill(3.0, 3.0), + VecBuilder.fill(0.01, 0.01), + Settings.DT + ); + + LinearQuadraticRegulator lqr = new LinearQuadraticRegulator<>( + pivotSystem, + VecBuilder.fill(0.00001, 100), + VecBuilder.fill(12), + Settings.DT + ); + + pivotController = new LinearSystemLoop<>(pivotSystem, lqr, kalmanFilter, 12.0, Settings.DT); + + pivotSim = new SingleJointedArmSim( + DCMotor.getKrakenX60(1), + Settings.Intake.GEAR_RATIO, + MOI, + ARM_LENGTH_METERS, + Settings.Intake.PIVOT_MIN_ANGLE.getRadians(), + Settings.Intake.PIVOT_MAX_ANGLE.getRadians(), + true, + Settings.Intake.PIVOT_MAX_ANGLE.getRadians() // start stowed + ); + + LinearSystem rollerSystem = LinearSystemId.createFlywheelSystem( + DCMotor.getKrakenX60(1), 0.01, 1.0 + ); + + rollerLeaderSim = new LinearSystemSim<>(rollerSystem); + rollerFollowerSim = new LinearSystemSim<>(rollerSystem); + + LinearQuadraticRegulator rollerLQR = new LinearQuadraticRegulator<>( + rollerSystem, + VecBuilder.fill(8.0), + VecBuilder.fill(12.0), + Settings.DT + ); + + KalmanFilter rollerLeaderKalman = new KalmanFilter<>( + Nat.N1(), Nat.N1(), + rollerSystem, + VecBuilder.fill(3.0), + VecBuilder.fill(0.01), + Settings.DT + ); + + KalmanFilter rollerFollowerKalman = new KalmanFilter<>( + Nat.N1(), Nat.N1(), + rollerSystem, + VecBuilder.fill(3.0), + VecBuilder.fill(0.01), + Settings.DT + ); + + rollerLeaderController = new LinearSystemLoop<>(rollerSystem, rollerLQR, rollerLeaderKalman, 12.0, Settings.DT); + rollerFollowerController = new LinearSystemLoop<>(rollerSystem, rollerLQR, rollerFollowerKalman, 12.0, Settings.DT); + + pivotVoltageOverride = Optional.empty(); + } + + @Override + public Rotation2d getPivotAngle() { + return Rotation2d.fromRadians(pivotSim.getAngleRads()); + } + + @Override + public boolean pivotAtTolerance() { + double error = getPivotAngle().minus(getPivotState().getTargetAngle()).getRotations(); + return Math.abs(error) < Settings.Intake.PIVOT_ANGLE_TOLERANCE.getRotations(); + } + + @Override + public boolean pivotStalling() { + return false; + } + + @Override + public void zeroPivotStowed() { + pivotSim.setState(Settings.Intake.PIVOT_MAX_ANGLE.getRadians(), 0.0); + } + + @Override + public void zeroPivotDeployed() { + pivotSim.setState(Settings.Intake.PIVOT_MIN_ANGLE.getRadians(), 0.0); + } + + @Override + public void periodic() { + super.periodic(); + + PivotState pivotState = getPivotState(); + + double targetRadPerSec = getRollerState().getTargetDutyCycle() * 2.0 * Math.PI / 60.0 * 6000.0; + + rollerLeaderController.setNextR(VecBuilder.fill(targetRadPerSec)); + rollerLeaderController.correct(VecBuilder.fill(rollerLeaderSim.getOutput(0))); + rollerLeaderController.predict(Settings.DT); + + rollerFollowerController.setNextR(VecBuilder.fill(targetRadPerSec)); + rollerFollowerController.correct(VecBuilder.fill(rollerFollowerSim.getOutput(0))); + rollerFollowerController.predict(Settings.DT); + + if (EnabledSubsystems.INTAKE.get()) { + if (pivotVoltageOverride.isPresent()) { + pivotSim.setInputVoltage(pivotVoltageOverride.get()); + } else { + pivotController.setNextR(VecBuilder.fill(pivotState.getTargetAngle().getRadians(), 0.0)); + pivotController.correct(VecBuilder.fill(pivotSim.getAngleRads(), pivotSim.getVelocityRadPerSec())); + pivotController.predict(Settings.DT); + pivotSim.setInputVoltage(pivotController.getU(0)); + } + + if (pivotState == PivotState.DEPLOY && getPivotAngle().getDegrees() <= Settings.Intake.THRESHOLD_TO_START_ROLLERS.getDegrees()) { + rollerLeaderSim.setInput(rollerLeaderController.getU(0)); + rollerFollowerSim.setInput(rollerFollowerController.getU(0)); + } else { + rollerLeaderSim.setInput(0.0); + rollerFollowerSim.setInput(0.0); + } + } else { + pivotSim.setInputVoltage(0.0); + rollerLeaderSim.setInput(0.0); + rollerFollowerSim.setInput(0.0); + } + + pivotSim.update(Settings.DT); + rollerLeaderSim.update(Settings.DT); + rollerFollowerSim.update(Settings.DT); + + if (Settings.DEBUG_MODE) { + SmartDashboard.putNumber("Intake/Sim Pivot Angle (deg)", getPivotAngle().getDegrees()); + SmartDashboard.putNumber("Intake/Sim Pivot Velocity (deg per s)", Units.radiansToDegrees(pivotSim.getVelocityRadPerSec())); + SmartDashboard.putNumber("Intake/Sim Roller Leader Velocity (RPM)", rollerLeaderSim.getOutput(0) * 60.0 / (2.0 * Math.PI)); + SmartDashboard.putNumber("Intake/Sim Roller Follower Velocity (RPM)", rollerFollowerSim.getOutput(0) * 60.0 / (2.0 * Math.PI)); + } + } + + @Override + public void setPivotVoltageOverride(Optional voltage) { + this.pivotVoltageOverride = voltage; + } + + @Override + public SysIdRoutine getPivotSysIdRoutine() { + return SysId.getRoutine( + 2, + 6, + "Intake Pivot", + voltage -> setPivotVoltageOverride(Optional.of(voltage)), + () -> getPivotAngle().getRotations(), + () -> pivotSim.getVelocityRadPerSec(), + () -> pivotVoltageOverride.orElse(0.0), + getInstance() + ); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/Spindexer.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/Spindexer.java index df2175f2..acb6517d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/Spindexer.java @@ -5,12 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.spindexer; -import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.util.SpindexerInterpolation; - -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -31,7 +26,6 @@ public static Spindexer getInstance() { public enum SpindexerState { STOP, - DYNAMIC, // In case we need for future: interpolates RPM based on distance to hub FORWARD, REVERSE; } @@ -39,7 +33,6 @@ public enum SpindexerState { public double getTargetRPM() { return switch (getState()) { case STOP -> 0; - case DYNAMIC -> getRPMBasedOnDistance(); case FORWARD -> Settings.Spindexer.FORWARD_SPEED; case REVERSE -> Settings.Spindexer.REVERSE_SPEED; }; @@ -57,16 +50,6 @@ public void setState(SpindexerState state) { this.spindexerState = state; } - /** - * @return target RPM interpolated based on distance - */ - public double getRPMBasedOnDistance() { - Translation2d hubPos = Field.getHubPose().getTranslation(); - Translation2d robotPos = CommandSwerveDrivetrain.getInstance().getPose().getTranslation(); - double distance = hubPos.getDistance(robotPos); - return SpindexerInterpolation.getRPM(distance); - } - public abstract SysIdRoutine getSysIdRoutine(); public abstract void setVoltageOverride(Optional voltage); @@ -77,7 +60,6 @@ public void periodic() { SmartDashboard.putString("States/Spindexer", getState().name()); SmartDashboard.putNumber("Spindexer/Target RPM", getTargetRPM()); - SmartDashboard.putNumber("Spindexer/Interpolated RPM Based on Distance", getRPMBasedOnDistance()); } } } \ No newline at end of file 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 109568e2..67271247 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -10,6 +10,7 @@ import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.SysId; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -97,9 +98,7 @@ public void periodic() { if (voltageOverride.isPresent()) { leadMotor.setVoltage(voltageOverride.get()); } else { - /** - * THE FOLLOWING LINE IS NOT A MISTAKE - */ + // DO NOT REMOVE BELOW LINE - needed to brake the motor in STOP state if (atTolerance() && getState() == SpindexerState.STOP) { leadMotor.stopMotor(); } else { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index 1d6a733b..5bac12c3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -6,18 +6,14 @@ package com.stuypulse.robot.subsystems.superstructure.hood; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; -import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Gains; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.SysId; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java index 5a98df2c..5d2a9c56 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java @@ -10,10 +10,8 @@ import com.stuypulse.robot.util.SysId; import com.stuypulse.robot.util.superstructure.VisualizerHood; -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.simulation.ElevatorSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java index 1be77039..cd2b2c2f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java @@ -32,7 +32,7 @@ public class ShooterSim extends Shooter { private Optional voltageOverride; public ShooterSim() { - LinearSystem flywheel = LinearSystemId.createFlywheelSystem(DCMotor.getKrakenX44(2), 0.20, 1.0); + LinearSystem flywheel = LinearSystemId.createFlywheelSystem(DCMotor.getKrakenX44(2), 0.05, 1.0); sim = new LinearSystemSim<>(flywheel); diff --git a/src/main/java/com/stuypulse/robot/util/SpindexerInterpolation.java b/src/main/java/com/stuypulse/robot/util/SpindexerInterpolation.java deleted file mode 100644 index cb83b1b0..00000000 --- a/src/main/java/com/stuypulse/robot/util/SpindexerInterpolation.java +++ /dev/null @@ -1,28 +0,0 @@ -/************************ 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; - -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; - -public class SpindexerInterpolation { - private static final InterpolatingDoubleTreeMap interpolatingDoubleTreeMap; - - private static final double[][] rpmAndDistance = { - /* { RPM, Distance (m) } */ - {1,1} - }; - - static { - interpolatingDoubleTreeMap = new InterpolatingDoubleTreeMap(); - for (double[] data: rpmAndDistance) { - interpolatingDoubleTreeMap.put(data[1], data[0]); - } - } - - public static double getRPM(double distanceInMeters){ - return interpolatingDoubleTreeMap.get(distanceInMeters); - } -} From bba803c867e0a85557c89461130b54f2f4a142e8 Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Sun, 8 Mar 2026 20:07:36 -0400 Subject: [PATCH 112/150] FIX: handoff and spindexer reverse amount --- .../java/com/stuypulse/robot/RobotContainer.java | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index e90b5e52..6c09f540 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -72,6 +72,7 @@ import com.stuypulse.robot.subsystems.intake.Intake; import com.stuypulse.robot.subsystems.intake.Intake.RollerState; import com.stuypulse.robot.subsystems.spindexer.Spindexer; +import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.subsystems.vision.LimelightVision; import com.stuypulse.robot.util.PathUtil.AutonConfig; @@ -84,6 +85,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; @@ -142,9 +144,17 @@ public RobotContainer() { SmartDashboard.putData("Robot/Override Up", new ClimberOverrideUp()); SmartDashboard.putData("Robot/Override Down", new ClimberOverrideDown()); SmartDashboard.putData("Robot/Override Stop", new ClimberOverrideStop()); - SmartDashboard.putData("Handoff Reverse", new HandoffReverse()); + SmartDashboard.putData("Handoff Reverse", + new ConditionalCommand( + new HandoffReverse().andThen(new WaitCommand(0.25)).andThen(new HandoffRun()), + new HandoffReverse().andThen(new WaitCommand(0.25).andThen(new HandoffStop())), + () -> handoff.getState() == HandoffState.FORWARD)); SmartDashboard.putData("Intake Reverse", new IntakeSetState(RollerState.OUTTAKE)); - SmartDashboard.putData("Spindexer Reverse", new SpindexerReverse()); + SmartDashboard.putData("Spindexer Reverse", + new ConditionalCommand( + new SpindexerReverse().andThen(new WaitCommand(0.25)).andThen(new SpindexerRun()), + new SpindexerReverse().andThen(new WaitCommand(0.25).andThen(new SpindexerStop())), + () -> spindexer.getState() == SpindexerState.FORWARD)); } From f267067943a96ae95d85efcfd6ebf2c45e3e4e58 Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Sun, 8 Mar 2026 22:38:26 -0400 Subject: [PATCH 113/150] ADD: Elastic button for turret seedings --- src/main/java/com/stuypulse/robot/RobotContainer.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index c41c128b..2a014a88 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -143,6 +143,7 @@ public RobotContainer() { new SpindexerReverse().andThen(new WaitCommand(0.25)).andThen(new SpindexerRun()), new SpindexerReverse().andThen(new WaitCommand(0.25).andThen(new SpindexerStop())), () -> spindexer.getState() == SpindexerState.FORWARD)); + SmartDashboard.putData("Turret Seed", new ZeroTurret()); } From 6229f98368059827dae5a149466f9b1e2ba02bc3 Mon Sep 17 00:00:00 2001 From: Alex Wang Date: Sun, 8 Mar 2026 23:38:17 -0400 Subject: [PATCH 114/150] feat: (hood) hood homing command --- .../robot/commands/hood/HomingRoutine.java | 17 +++ .../stuypulse/robot/constants/Settings.java | 1 + .../com/stuypulse/robot/subsystems/.gitkeep | 0 .../subsystems/superstructure/hood/Hood.java | 4 +- .../superstructure/hood/HoodImpl.java | 107 +++++++++--------- 5 files changed, 77 insertions(+), 52 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/hood/HomingRoutine.java delete mode 100644 src/main/java/com/stuypulse/robot/subsystems/.gitkeep diff --git a/src/main/java/com/stuypulse/robot/commands/hood/HomingRoutine.java b/src/main/java/com/stuypulse/robot/commands/hood/HomingRoutine.java new file mode 100644 index 00000000..4caa9ac2 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/hood/HomingRoutine.java @@ -0,0 +1,17 @@ +package com.stuypulse.robot.commands.hood; + +import com.stuypulse.robot.subsystems.superstructure.hood.Hood; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class HomingRoutine extends InstantCommand { + private Hood hood; + public HomingRoutine() { + hood = Hood.getInstance(); + } + + @Override + public void initialize() { + hood.setState(Hood.HoodState.HOMING); + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 85984b26..2c9bdfe9 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -261,6 +261,7 @@ public interface Hood { */ public final double GEAR_RATIO = 1064.0 / 9.0; public final double ENCODER_TO_MECH = 32.0 / 3.0; + public final double HOOD_HOMING_VOLTAGE = 2.0; public final Rotation2d ENCODER_OFFSET = Rotation2d.fromRotations(-0.043); diff --git a/src/main/java/com/stuypulse/robot/subsystems/.gitkeep b/src/main/java/com/stuypulse/robot/subsystems/.gitkeep deleted file mode 100644 index e69de29b..00000000 diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java index 9784fb75..4d161ca3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java @@ -9,9 +9,9 @@ import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.util.superstructure.InterpolationCalculator; import com.stuypulse.robot.util.superstructure.SOTMCalculator; import com.stuypulse.robot.util.superstructure.VisualizerHood; -import com.stuypulse.robot.util.superstructure.InterpolationCalculator; import com.stuypulse.stuylib.input.Gamepad; import edu.wpi.first.math.geometry.Rotation2d; @@ -49,6 +49,7 @@ public enum HoodState { SOTM, FOTM, ANALOG, + HOMING, IDLE; } @@ -77,6 +78,7 @@ public Rotation2d getTargetAngle() { case LEFT_CORNER -> Settings.Superstructure.Hood.Angles.LEFT_CORNER; case RIGHT_CORNER -> Settings.Superstructure.Hood.Angles.RIGHT_CORNER; case INTERPOLATION -> InterpolationCalculator.interpolateShotInfo().targetHoodAngle(); + case HOMING -> new Rotation2d(); //should just apply a voltage, not an angle! case SOTM -> SOTMCalculator.calculateHoodAngleSOTM(); case FOTM -> SOTMCalculator.calculateHoodAngleFOTM(); case ANALOG -> hoodAnalogToOutput(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index 5bac12c3..6312a2d3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -1,10 +1,19 @@ -/************************ PROJECT TRIBECBOT *************************/ +/** ********************** 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. */ -/***************************************************************/ + /* 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.subsystems.superstructure.hood; +import java.util.Optional; + +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Gains; import com.stuypulse.robot.constants.Motors; @@ -18,14 +27,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; -import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; -import java.util.Optional; public class HoodImpl extends Hood { private final Motors.TalonFXConfig hoodConfig; private final Motors.CANCoderConfig hoodEncoderConfig; @@ -34,37 +35,33 @@ public class HoodImpl extends Hood { private final CANcoder hoodEncoder; private final PositionVoltage controller; - + private Optional voltageOverride; - + private boolean hasUsedAbsoluteEncoder; private final BStream isStalling; public HoodImpl() { hoodConfig = new Motors.TalonFXConfig() - .withInvertedValue(InvertedValue.CounterClockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake) - - .withSupplyCurrentLimitAmps(80.0) - .withStatorCurrentLimitEnabled(false) - .withRampRate(0.25) - - .withPIDConstants(Gains.Superstructure.Hood.kP, Gains.Superstructure.Hood.kI, Gains.Superstructure.Hood.kD, 0) - .withFFConstants(Gains.Superstructure.Hood.kS, Gains.Superstructure.Hood.kV, Gains.Superstructure.Hood.kA, 0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 0) - - .withSensorToMechanismRatio(Settings.Superstructure.Hood.GEAR_RATIO) - - .withSoftLimits( - true, true, - Settings.Superstructure.Hood.FORWARD_SOFT_LIMIT.getRotations(), - Settings.Superstructure.Hood.REVERSE_SOFT_LIMIT.getRotations()); + .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) + .withSupplyCurrentLimitAmps(80.0) + .withStatorCurrentLimitEnabled(false) + .withRampRate(0.25) + .withPIDConstants(Gains.Superstructure.Hood.kP, Gains.Superstructure.Hood.kI, Gains.Superstructure.Hood.kD, 0) + .withFFConstants(Gains.Superstructure.Hood.kS, Gains.Superstructure.Hood.kV, Gains.Superstructure.Hood.kA, 0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 0) + .withSensorToMechanismRatio(Settings.Superstructure.Hood.GEAR_RATIO) + .withSoftLimits( + true, true, + Settings.Superstructure.Hood.FORWARD_SOFT_LIMIT.getRotations(), + Settings.Superstructure.Hood.REVERSE_SOFT_LIMIT.getRotations()); hoodEncoderConfig = new Motors.CANCoderConfig() - .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) - .withAbsoluteSensorDiscontinuityPoint(1.0) - .withMagnetOffset(Settings.Superstructure.Hood.ENCODER_OFFSET.getRotations()); + .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) + .withAbsoluteSensorDiscontinuityPoint(1.0) + .withMagnetOffset(Settings.Superstructure.Hood.ENCODER_OFFSET.getRotations()); hoodMotor = new TalonFX(Ports.Superstructure.Hood.MOTOR, Ports.RIO); hoodEncoder = new CANcoder(Ports.Superstructure.Hood.THROUGHBORE_ENCODER, Ports.RIO); @@ -73,12 +70,12 @@ public HoodImpl() { hoodEncoderConfig.configure(hoodEncoder); controller = new PositionVoltage(getTargetAngle().getRotations()) - .withEnableFOC(true); + .withEnableFOC(true); voltageOverride = Optional.empty(); isStalling = BStream.create(() -> hoodMotor.getSupplyCurrent().getValueAsDouble() > Settings.Superstructure.Hood.STALL_CURRENT_LIMIT) //TODO: update value in Settings after testing - .filtered(new BDebounce.Both(Settings.Superstructure.Hood.STALL_DEBOUNCE)); + .filtered(new BDebounce.Both(Settings.Superstructure.Hood.STALL_DEBOUNCE)); } @Override @@ -89,7 +86,7 @@ public Rotation2d getAngle() { //TODO: Implementation as of 3/3 but not yet tested // Should ONLY be called at the lower hardstop! @Override - public void zeroHoodEncoderAtUpperHardstop() { + public void zeroHoodEncoderAtUpperHardstop() { hoodEncoder.getConfigurator().refresh(hoodEncoderConfig.getConfiguration().MagnetSensor); double currentOffset = hoodEncoderConfig.getConfiguration().MagnetSensor.MagnetOffset; @@ -98,7 +95,7 @@ public void zeroHoodEncoderAtUpperHardstop() { double newOffset = -((positionWithCurrentOffset - currentOffset) - Settings.Superstructure.Hood.Angles.MAX.getRotations()); hoodEncoderConfig.withMagnetOffset(newOffset); - + hoodEncoderConfig.configure(hoodEncoder); } @@ -106,12 +103,12 @@ public void zeroHoodEncoderAtUpperHardstop() { public boolean isStalling() { return isStalling.getAsBoolean(); } - + /* * 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 - */ + */ @Override public void seedHood() { hoodMotor.setPosition(getAbsoluteHoodAngleDeg() / 360.0); @@ -121,7 +118,7 @@ private double getAbsoluteHoodAngleDeg() { return Settings.Superstructure.Hood.MIN_FROM_HORIZON.getDegrees() + hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.Superstructure.Hood.ENCODER_TO_MECH; } - @Override + @Override public void periodic() { super.periodic(); @@ -130,9 +127,17 @@ public void periodic() { hasUsedAbsoluteEncoder = true; } + if (isStalling() && getState() == HoodState.HOMING) { + seedHood(); + setState(HoodState.IDLE); + SmartDashboard.putBoolean("Superstructure/Hood/SUCCESFULLY HOMED", true); + } + if (EnabledSubsystems.HOOD.get()) { if (voltageOverride.isPresent()) { hoodMotor.setVoltage(voltageOverride.get()); + } else if (getState() == HoodState.HOMING && !isStalling()) { + hoodMotor.setVoltage(Settings.Superstructure.Hood.HOOD_HOMING_VOLTAGE); } else { hoodMotor.setControl(controller.withPosition(getTargetAngle().getRotations())); } @@ -151,7 +156,7 @@ public void periodic() { SmartDashboard.putBoolean("Superstructure/Hood/Has Used Absolute Encoder", hasUsedAbsoluteEncoder); SmartDashboard.putNumber("Superstructure/Hood/Raw Motor Encoder Value", hoodMotor.getPosition().getValueAsDouble()); - + SmartDashboard.putNumber("Current Draws/Hood (amps)", hoodMotor.getSupplyCurrent().getValueAsDouble()); } } @@ -163,15 +168,15 @@ private void setVoltageOverride(Optional voltageOverride) { @Override public SysIdRoutine getHoodSysIdRoutine() { return SysId.getRoutine( - .45, - 2, - "Hood", - voltage -> setVoltageOverride(Optional.of(voltage)), - () -> hoodMotor.getPosition().getValueAsDouble(), - () -> hoodMotor.getVelocity().getValueAsDouble(), - () -> hoodMotor.getMotorVoltage().getValueAsDouble(), - getInstance() + .45, + 2, + "Hood", + voltage -> setVoltageOverride(Optional.of(voltage)), + () -> hoodMotor.getPosition().getValueAsDouble(), + () -> hoodMotor.getVelocity().getValueAsDouble(), + () -> hoodMotor.getMotorVoltage().getValueAsDouble(), + getInstance() ); } - + } \ No newline at end of file From 196852f9bbab4bb2898c10666a259456ada22d49 Mon Sep 17 00:00:00 2001 From: Alex Wang Date: Sun, 8 Mar 2026 23:38:56 -0400 Subject: [PATCH 115/150] feat: (ll) place expensive debugging calls under debug flag --- .../subsystems/vision/LimelightVision.java | 20 +++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index c9f1cadc..cb2f010e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -198,15 +198,19 @@ public void periodic() { SmartDashboard.putNumber("Vision/Limelight Yaw", LimelightHelpers.getIMUData(limelightName).Yaw); } - String limelightName = names[i]; - SmartDashboard.putString("Vision/MegaTag Mode", megaTagMode.toString()); - // this yaw is seems to be the robot yaw passed into the LL - SmartDashboard.putNumber("Vision/Limelight Robot Yaw " + limelightName, LimelightHelpers.getIMUData(limelightName).robotYaw); - // this is just the yaw of the internal imu - SmartDashboard.putNumber("Vision/Limelight Yaw " + limelightName, LimelightHelpers.getIMUData(limelightName).Yaw); - SmartDashboard.putNumber("Vision/Limelight Robot Yaw Passed in", (CommandSwerveDrivetrain.getInstance().getPose().getRotation().getDegrees() + (Robot.isBlue() ? 0 : 180)) % 360); + if (Settings.DEBUG_MODE) { + String limelightName = names[i]; + SmartDashboard.putString("Vision/MegaTag Mode", megaTagMode.toString()); + // this yaw is seems to be the robot yaw passed into the LL + SmartDashboard.putNumber("Vision/Limelight Robot Yaw " + limelightName, LimelightHelpers.getIMUData(limelightName).robotYaw); + // this is just the yaw of the internal imu + SmartDashboard.putNumber("Vision/Limelight Yaw " + limelightName, LimelightHelpers.getIMUData(limelightName).Yaw); + SmartDashboard.putNumber("Vision/Limelight Robot Yaw Passed in", (CommandSwerveDrivetrain.getInstance().getPose().getRotation().getDegrees() + (Robot.isBlue() ? 0 : 180)) % 360); + } + } + if (Settings.DEBUG_MODE) { + arrayPublisher.set(arrayOfLimelightPoses); } - arrayPublisher.set(arrayOfLimelightPoses); } } } \ No newline at end of file From cc5bfa1b20029f7b22dc330d6edab2b0e516e095 Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Mon, 9 Mar 2026 00:16:26 -0400 Subject: [PATCH 116/150] CLEAN: kill climberhopper, eliminate more dead code --- src/main/java/com/stuypulse/robot/Robot.java | 17 --- .../com/stuypulse/robot/RobotContainer.java | 34 +---- .../commands/auton/regular/DepotAuton.java | 5 - .../commands/auton/regular/EightFuel.java | 1 - .../commands/auton/regular/LeftOneCycle.java | 6 - .../commands/auton/regular/LeftTwoCycle.java | 3 - .../commands/auton/regular/RightOneCycle.java | 5 - .../commands/auton/regular/RightTwoCycle.java | 3 - .../robot/commands/auton/test/BoxTest.java | 4 - .../commands/climberhopper/ClimberDown.java | 14 -- .../ClimberHopperDefaultCommand.java | 78 ----------- .../climberhopper/ClimberHopperReset.java | 46 ------ .../climberhopper/ClimberHopperSetState.java | 26 ---- .../climberhopper/ClimberOverrideDown.java | 27 ---- .../climberhopper/ClimberOverrideStop.java | 25 ---- .../climberhopper/ClimberOverrideUp.java | 26 ---- .../commands/climberhopper/ClimberUp.java | 14 -- .../commands/climberhopper/HopperDown.java | 14 -- .../commands/climberhopper/HopperUp.java | 14 -- .../commands/intake/IntakeDigestion.java | 29 ---- .../commands/intake/IntakeDigestionDown.java | 9 -- .../commands/intake/IntakeDigestionup.java | 9 -- .../robot/commands/turret/SeedTurret.java | 26 ++++ .../stuypulse/robot/constants/Settings.java | 107 -------------- .../climberhopper/ClimberHopper.java | 84 ----------- .../climberhopper/ClimberHopperImpl.java | 132 ------------------ .../climberhopper/ClimberHopperSim.java | 105 -------------- .../ClimberHopperVisualizer.java | 69 --------- .../robot/subsystems/intake/Intake.java | 4 +- .../robot/subsystems/intake/IntakeImpl.java | 35 +---- .../superstructure/Superstructure.java | 8 -- .../superstructure/shooter/ShooterImpl.java | 5 - .../superstructure/turret/TurretImpl.java | 2 - 33 files changed, 32 insertions(+), 954 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberDown.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperSetState.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideDown.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideStop.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideUp.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberUp.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/climberhopper/HopperDown.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/climberhopper/HopperUp.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestion.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestionDown.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestionup.java create mode 100644 src/main/java/com/stuypulse/robot/commands/turret/SeedTurret.java delete mode 100644 src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java delete mode 100644 src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java delete mode 100644 src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java delete mode 100644 src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperVisualizer.java diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 65a21d24..d0c01474 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -9,12 +9,9 @@ import com.stuypulse.robot.commands.vision.SetMegaTagMode; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.vision.LimelightVision; -import com.stuypulse.robot.util.FMSUtil; - import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -27,9 +24,6 @@ public class Robot extends TimedRobot { private RobotContainer robot; private Command auto; private static Alliance alliance; - // private PowerDistribution powerDistribution; - - private FMSUtil fms; public static boolean isBlue() { return alliance == Alliance.Blue; @@ -42,8 +36,6 @@ public static boolean isBlue() { @Override public void robotInit() { robot = new RobotContainer(); - // powerDistribution = new PowerDistribution(); - fms = new FMSUtil(false); DataLogManager.start(); SignalLogger.start(); @@ -53,7 +45,6 @@ public void robotInit() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); - // SmartDashboard.putNumber("Robot/Voltage of Robot", powerDistribution.getVoltage()); if (!Robot.isReal()) { SmartDashboard.putData(CommandScheduler.getInstance()); } @@ -63,10 +54,6 @@ public void robotPeriodic() { if (DriverStation.getAlliance().isPresent()) { alliance = DriverStation.getAlliance().get(); } - - SmartDashboard.putNumber("FMSUtil/Time Left In Shift", fms.getTimeLeftInShift()); - SmartDashboard.putBoolean("FMSUtil/Is Active Shift?", fms.isActiveShift()); - SmartDashboard.putString("FMSUtil/Field State", fms.getCurrentFieldState().toString()); } /*********************/ @@ -89,7 +76,6 @@ public void disabledPeriodic() { @Override public void autonomousInit() { - fms.restartTimer(true); CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); CommandScheduler.getInstance().schedule(new SetIMUMode(Settings.Vision.INTERNAL_EXTERNAL_ASSIST_INDEX)); @@ -115,17 +101,14 @@ public void autonomousExit( @Override public void teleopInit() { - fms.restartTimer(false); CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); - // CommandScheduler.getInstance().schedule(new SetIMUMode(Settings.Vision.INTERNAL_EXTERNAL_ASSIST_INDEX)); CommandScheduler.getInstance().schedule(new SetIMUMode(0)); if (auto != null) { auto.cancel(); } - SmartDashboard.putBoolean("FMSUtil/Won Auto?", fms.didWinAuto()); } @Override diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 2a014a88..bf9d1c45 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -7,20 +7,13 @@ import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; -// import com.stuypulse.robot.commands.auton.test.BoxTest; import com.stuypulse.robot.commands.auton.regular.DepotAuton; import com.stuypulse.robot.commands.auton.regular.EightFuel; import com.stuypulse.robot.commands.auton.regular.LeftOneCycle; import com.stuypulse.robot.commands.auton.regular.LeftTwoCycle; import com.stuypulse.robot.commands.auton.regular.RightOneCycle; -import com.stuypulse.robot.commands.climberhopper.ClimberDown; -import com.stuypulse.robot.commands.climberhopper.ClimberOverrideDown; -import com.stuypulse.robot.commands.climberhopper.ClimberOverrideStop; -import com.stuypulse.robot.commands.climberhopper.ClimberOverrideUp; import com.stuypulse.robot.commands.handoff.HandoffConditionalCommand; import com.stuypulse.robot.commands.handoff.HandoffReverse; -import com.stuypulse.robot.commands.climberhopper.ClimberUp; -import com.stuypulse.robot.commands.climberhopper.HopperDown; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.hood.ZeroHoodEncoderAtUpperHardstop; @@ -45,6 +38,7 @@ import com.stuypulse.robot.commands.swerve.SwerveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveWheelRadiusCharacterization; import com.stuypulse.robot.commands.swerve.SwerveXMode; +import com.stuypulse.robot.commands.turret.SeedTurret; import com.stuypulse.robot.commands.turret.ZeroTurret; import com.stuypulse.robot.commands.vision.ResetLimelightIMU; import com.stuypulse.robot.commands.vision.SetIMUMode; @@ -86,7 +80,6 @@ public interface EnabledSubsystems { SmartBoolean HANDOFF = new SmartBoolean("Enabled Subsystems/Handoff Is Enabled", true); SmartBoolean INTAKE = new SmartBoolean("Enabled Subsystems/Intake Is Enabled", true); SmartBoolean SPINDEXER = new SmartBoolean("Enabled Subsystems/Spindexer Is Enabled", true); - SmartBoolean CLIMBER_HOPPER = new SmartBoolean("Enabled Subsystems/Climber-Hopper Is Enabled", false); SmartBoolean HOOD = new SmartBoolean("Enabled Subsystems/Hood Is Enabled", true); SmartBoolean SHOOTER = new SmartBoolean("Enabled Subsystems/Shooter Is Enabled", true); @@ -99,7 +92,6 @@ public interface EnabledSubsystems { public final Gamepad driver = new AutoGamepad(Ports.Gamepad.DRIVER); // Subsystem - // private final ClimberHopper climberHopper = ClimberHopper.getInstance(); private final Handoff handoff = Handoff.getInstance(); private final Intake intake = Intake.getInstance(); private final Spindexer spindexer = Spindexer.getInstance(); @@ -127,24 +119,22 @@ public RobotContainer() { SmartDashboard.putData("Robot/Zero Pivot Encoder at Lower Limit (Deployed)", new ZeroPivotDeployed().ignoringDisable(true)); SmartDashboard.putData("Robot/Zero Pivot Encoder at Upper Limit (Stowed)", new ZeroPivotStowed().ignoringDisable(true)); SmartDashboard.putData("Robot/Zero Turret Encoders", new ZeroTurret().ignoringDisable(true)); + SmartDashboard.putData("Robot/Seed Turret", new SeedTurret().ignoringDisable(true)); SmartDashboard.putData("Robot/Zero Hood Encoder", new ZeroHoodEncoderAtUpperHardstop().ignoringDisable(true)); - SmartDashboard.putData("Robot/Override Up", new ClimberOverrideUp()); - SmartDashboard.putData("Robot/Override Down", new ClimberOverrideDown()); - SmartDashboard.putData("Robot/Override Stop", new ClimberOverrideStop()); SmartDashboard.putData("Handoff Reverse", new ConditionalCommand( new HandoffReverse().andThen(new WaitCommand(0.25)).andThen(new HandoffRun()), new HandoffReverse().andThen(new WaitCommand(0.25).andThen(new HandoffStop())), () -> handoff.getState() == HandoffState.FORWARD)); + SmartDashboard.putData("Intake Reverse", new IntakeSetState(RollerState.OUTTAKE)); + SmartDashboard.putData("Spindexer Reverse", new ConditionalCommand( new SpindexerReverse().andThen(new WaitCommand(0.25)).andThen(new SpindexerRun()), new SpindexerReverse().andThen(new WaitCommand(0.25).andThen(new SpindexerStop())), () -> spindexer.getState() == SpindexerState.FORWARD)); - SmartDashboard.putData("Turret Seed", new ZeroTurret()); - } /****************/ @@ -153,8 +143,6 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); - // superstructure.setDefaultCommand(new SuperstructureDefaultCommand()); - // climberHopper.setDefaultCommand(new ClimberHopperDefaultCommand()); // turret.setDefaultCommand(new TurretDefaultCommand()); } @@ -262,14 +250,6 @@ private void configureButtonBindings() { // // Swerve X Wheels // driver.getLeftBumper() // .whileTrue(new SwerveXMode()); - - // /** - // // Climb - // driver.getRightBumper() - // .whileTrue(new ClimberUp().alongWith(new IntakeDeploy()) - // .andThen(new SwerveClimbAlign())) - // .onFalse(new ClimberDown()); - // **/ } /**************/ @@ -280,11 +260,6 @@ public void configureAutons() { autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); - // TESTS - // AutonConfig BOX_TEST = new AutonConfig("Box Test", BoxTest::new, - // "Box 1", "Box 2", "Box 3", "Box 4"); - // BOX_TEST.register(autonChooser); - // BASE AutonConfig EIGHT_FUEL = new AutonConfig("Eight Fuel", EightFuel::new, ""); @@ -329,7 +304,6 @@ public void configureSysids() { // autonChooser.addOption("SysID Rotation Translation Quasi Forwards", swerve.sysidRotationQuasiStatic(Direction.kForward)); // autonChooser.addOption("SysID Rotation Translation Quasi Backwards", swerve.sysidRotationQuasiStatic(Direction.kReverse)); - // autonChooser.addOption("SysID Turret Dynamic Forwards", turret.getSysIdRoutine().dynamic(Direction.kForward)); // autonChooser.addOption("SysID Turret Dynamic Reverse", turret.getSysIdRoutine().dynamic(Direction.kReverse)); // autonChooser.addOption("SysID Turret Quasistatic Forwards", turret.getSysIdRoutine().quasistatic(Direction.kForward)); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java index abf66b34..e5c0d484 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java @@ -1,18 +1,13 @@ package com.stuypulse.robot.commands.auton.regular; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.climberhopper.ClimberDown; import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.intake.IntakeDeploy; 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.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java index 423ce0ff..0c25efd1 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java @@ -4,7 +4,6 @@ import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.superstructure.SuperstructureKB; -import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java index cf6ba0f0..e7b5d7e2 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java @@ -1,19 +1,13 @@ package com.stuypulse.robot.commands.auton.regular; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.climberhopper.ClimberDown; import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.intake.IntakeDeploy; -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.superstructure.SuperstructureInterpolation; -import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java index 516bbf08..19159e99 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java @@ -1,7 +1,6 @@ package com.stuypulse.robot.commands.auton.regular; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.climberhopper.ClimberDown; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.intake.IntakeDeploy; @@ -9,11 +8,9 @@ import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; -import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java index dddfad45..d78b29c5 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java @@ -1,15 +1,10 @@ package com.stuypulse.robot.commands.auton.regular; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.climberhopper.ClimberDown; import com.stuypulse.robot.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.intake.IntakeDeploy; -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.superstructure.SuperstructureInterpolation; -import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java index bbb56974..1f284a8f 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java @@ -1,7 +1,6 @@ package com.stuypulse.robot.commands.auton.regular; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.climberhopper.ClimberDown; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.intake.IntakeDeploy; @@ -9,11 +8,9 @@ import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; -import com.stuypulse.robot.commands.swerve.climbAlign.SwerveClimbAlign; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java b/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java index 08160941..7a6a4071 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java @@ -1,10 +1,8 @@ package com.stuypulse.robot.commands.auton.test; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.superstructure.SuperstructureKB; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; public class BoxTest extends SequentialCommandGroup { @@ -12,12 +10,10 @@ public class BoxTest extends SequentialCommandGroup { public BoxTest(PathPlannerPath... paths) { addCommands( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]) - ); } diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberDown.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberDown.java deleted file mode 100644 index 57036b6f..00000000 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberDown.java +++ /dev/null @@ -1,14 +0,0 @@ -/************************ 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.commands.climberhopper; - -import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; - -public class ClimberDown extends ClimberHopperSetState { - public ClimberDown() { - super(ClimberHopperState.CLIMBER_DOWN); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java deleted file mode 100644 index 434b1b25..00000000 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java +++ /dev/null @@ -1,78 +0,0 @@ -/************************ 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.commands.climberhopper; - -import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; -import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; - -import edu.wpi.first.wpilibj2.command.Command; - - -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 - - public ClimberHopperDefaultCommand() { - climberHopper = ClimberHopper.getInstance(); - // swerve = CommandSwerveDrivetrain.getInstance(); - // pose = swerve.getPose(); - - - addRequirements(climberHopper); - } - - @Override - 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(); - - // 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 isCloseToNearTrenchesX = Math.abs(pose.getX() - Field.NearRightTrench.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 - - // // === 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); - } - } -} \ 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 deleted file mode 100644 index fd669f38..00000000 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java +++ /dev/null @@ -1,46 +0,0 @@ -/************************ 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.commands.climberhopper; - -// 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; - -public class ClimberHopperReset extends Command { - // 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() { - - // } -} diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperSetState.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperSetState.java deleted file mode 100644 index 6849ef97..00000000 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperSetState.java +++ /dev/null @@ -1,26 +0,0 @@ -/************************ 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.commands.climberhopper; - -import com.stuypulse.robot.subsystems.climberhopper.*; -import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; - -import edu.wpi.first.wpilibj2.command.Command; - -public class ClimberHopperSetState extends Command { - private final ClimberHopper climberHopper = ClimberHopper.getInstance(); - private final ClimberHopperState state; - - public ClimberHopperSetState(ClimberHopperState state) { - this.state = state; - addRequirements(climberHopper); - } - - @Override - public void initialize() { - climberHopper.setState(state); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideDown.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideDown.java deleted file mode 100644 index e2e137bd..00000000 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideDown.java +++ /dev/null @@ -1,27 +0,0 @@ - -package com.stuypulse.robot.commands.climberhopper; - -import java.util.Optional; - -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; - -import edu.wpi.first.wpilibj2.command.Command; - -public class ClimberOverrideDown extends Command { - private final ClimberHopper climberHopper; - - public ClimberOverrideDown() { - climberHopper = ClimberHopper.getInstance(); - } - - @Override - public void initialize() { - climberHopper.setVoltageOverride(Optional.of(-Settings.ClimberHopper.MOTOR_VOLTAGE)); - } - - @Override - public void end(boolean interrupted) { - climberHopper.setVoltageOverride(Optional.empty()); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideStop.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideStop.java deleted file mode 100644 index de1fc321..00000000 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideStop.java +++ /dev/null @@ -1,25 +0,0 @@ -package com.stuypulse.robot.commands.climberhopper; - -import java.util.Optional; - -import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; - -import edu.wpi.first.wpilibj2.command.Command; - -public class ClimberOverrideStop extends Command { - private final ClimberHopper climberHopper; - - public ClimberOverrideStop() { - climberHopper = ClimberHopper.getInstance(); - } - - @Override - public void initialize() { - climberHopper.setVoltageOverride(Optional.of(0.0)); - } - - @Override - public void end(boolean interrupted) { - climberHopper.setVoltageOverride(Optional.empty()); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideUp.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideUp.java deleted file mode 100644 index a0f25dbe..00000000 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberOverrideUp.java +++ /dev/null @@ -1,26 +0,0 @@ -package com.stuypulse.robot.commands.climberhopper; - -import java.util.Optional; - -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; - -import edu.wpi.first.wpilibj2.command.Command; - -public class ClimberOverrideUp extends Command { - private final ClimberHopper climberHopper; - - public ClimberOverrideUp() { - climberHopper = ClimberHopper.getInstance(); - } - - @Override - public void initialize() { - climberHopper.setVoltageOverride(Optional.of(Settings.ClimberHopper.MOTOR_VOLTAGE)); - } - - @Override - public void end(boolean interrupted) { - climberHopper.setVoltageOverride(Optional.empty()); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberUp.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberUp.java deleted file mode 100644 index d9db87ce..00000000 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberUp.java +++ /dev/null @@ -1,14 +0,0 @@ -/************************ 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.commands.climberhopper; - -import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; - -public class ClimberUp extends ClimberHopperSetState { - public ClimberUp() { - super(ClimberHopperState.CLIMBER_UP); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperDown.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperDown.java deleted file mode 100644 index 5eb46595..00000000 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperDown.java +++ /dev/null @@ -1,14 +0,0 @@ -/************************ 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.commands.climberhopper; - -import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; - -public class HopperDown extends ClimberHopperSetState { - public HopperDown() { - super(ClimberHopperState.HOPPER_DOWN); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperUp.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperUp.java deleted file mode 100644 index 03fca2a9..00000000 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperUp.java +++ /dev/null @@ -1,14 +0,0 @@ -/************************ 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.commands.climberhopper; - -// 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 diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestion.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestion.java deleted file mode 100644 index c2e89853..00000000 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestion.java +++ /dev/null @@ -1,29 +0,0 @@ -package com.stuypulse.robot.commands.intake; - -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.intake.Intake; -import com.stuypulse.robot.subsystems.intake.Intake.PivotState; -import com.stuypulse.stuylib.streams.booleans.BStream; -import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; -import com.stuypulse.robot.commands.intake.IntakeDigestionup; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RepeatCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class IntakeDigestion extends RepeatCommand { - public IntakeDigestion() { - super( - new SequentialCommandGroup( - new IntakeDigestionup(), - new WaitUntilCommand(() -> Intake.getInstance().pivotAtTolerance()).withTimeout(0.4), - new WaitCommand(1.0), - new IntakeDigestionDown(), - new WaitUntilCommand(() -> Intake.getInstance().pivotAtTolerance()).withTimeout(0.4), - new WaitCommand(0.5) - ) - ); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestionDown.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestionDown.java deleted file mode 100644 index 66cbd528..00000000 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestionDown.java +++ /dev/null @@ -1,9 +0,0 @@ -package com.stuypulse.robot.commands.intake; - -import com.stuypulse.robot.subsystems.intake.Intake.PivotState; - -public class IntakeDigestionDown extends IntakeSetState{ - public IntakeDigestionDown() { - super(PivotState.DIGESTION_DOWN); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestionup.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestionup.java deleted file mode 100644 index e6fbd71f..00000000 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeDigestionup.java +++ /dev/null @@ -1,9 +0,0 @@ -package com.stuypulse.robot.commands.intake; - -import com.stuypulse.robot.subsystems.intake.Intake.PivotState; - -public class IntakeDigestionup extends IntakeSetState{ - public IntakeDigestionup(){ - super(PivotState.DIGESTION_UP); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/turret/SeedTurret.java b/src/main/java/com/stuypulse/robot/commands/turret/SeedTurret.java new file mode 100644 index 00000000..ed961ec3 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/turret/SeedTurret.java @@ -0,0 +1,26 @@ +/************************ 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.commands.turret; + +import com.stuypulse.robot.subsystems.superstructure.turret.Turret; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class SeedTurret extends InstantCommand { + + private final Turret turret; + + public SeedTurret() { + this.turret = Turret.getInstance(); + + addRequirements(turret); + } + + @Override + public void initialize() { + turret.seedTurret(); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 2c9bdfe9..95ceb528 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -30,59 +30,6 @@ public interface Settings { public final boolean DEBUG_MODE = true; public final CANBus CANIVORE = new CANBus("canivore", "./logs/example.hoot"); - public interface HubDATA { - public final double[] shiftStartTimes = {0.0, 10.0, 35.0, 60.0, 85.0, 110.0}; - public final double[] shiftEndTimes = {10.0, 35.0, 60.0, 85.0, 110.0, 140.0}; - - } - - public interface ClimberHopper { - /* CONSTANTS */ - public final double GEAR_RATIO = 45.0; - - public final double MIN_HEIGHT_METERS = 0.0; - - // public final double MIN_ROTATIONS = -0.1; - public final double MAX_HEIGHT_METERS = 2.884; - // public final double MAX_ROTATIONS = 20; - - public final double MASS_KG = 1.0; - - // public final double NUM_ROTATIONS_TO_REACH_TOP = MAX_ROTATIONS - MIN_ROTATIONS; - // public final double NUM_ROTATIONS_TO_REACH_TOP = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (0.480 / 13.0); // TODO: verify this - public final double NUM_ROTATIONS_TO_REACH_TOP = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / Units.inchesToMeters(2 * Math.PI * 0.75); // TODO: verify this - 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; - - - public final double DRUM_RADIUS_METERS = ((MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (NUM_ROTATIONS_TO_REACH_TOP / GEAR_RATIO)) / 2.0 / Math.PI; - /* CONSTANTS */ - - public final double CLIMBER_UP_HEIGHT_METERS = MAX_HEIGHT_METERS; - // public final double CLIMBER_UP_ROTATIONS = MAX_ROTATIONS; // TODO: FIND - public final double CLIMBER_DOWN_HEIGHT_METERS = MIN_HEIGHT_METERS; - // public final double CLIMBER_DOWN_ROTATIONS = MIN_ROTATIONS; - public final double HOPPER_DOWN_HEIGHT_METERS = MIN_HEIGHT_METERS; - // public final double HOPPER_DOWN_ROTATIONS = MIN_ROTATIONS; - public final double HOPPER_UP_HEIGHT_METERS = MAX_HEIGHT_METERS; - // public final double HOPPER_UP_ROTATIONS = MAX_ROTATIONS; - - public final double STALL = 10.0; - - public final double ROTATIONS_AT_BOTTOM = 0.0; - - public final double DEBOUNCE = 0.25; - - public final double GYRO_TOLERANCE = 0.0; - - public final double HEIGHT_TOLERANCE_METERS = 0.1; - // public final double TOLERANCE_ROTATIONS = 0.1; - - public final double RAMP_RATE = 50.0; - - public final double MOTOR_VOLTAGE = 1.0; - } - public interface Handoff { public final double GEAR_RATIO = 3.0 / 1.0; @@ -97,20 +44,11 @@ public interface Intake { Rotation2d PIVOT_STOW_ANGLE = Rotation2d.fromDegrees(90.0); Rotation2d PIVOT_DEPLOY_ANGLE = Rotation2d.fromDegrees(0.0); - Rotation2d DIGESTION_UP_ANGLE = Rotation2d.fromDegrees(70); // TODO: verify - Rotation2d DIGESTION_DOWN_ANGLE = Rotation2d.fromDegrees(20); - Rotation2d PIVOT_ANGLE_TOLERANCE = Rotation2d.fromDegrees(5.0); Rotation2d PIVOT_MAX_ANGLE = Rotation2d.fromDegrees(88.0); Rotation2d PIVOT_MIN_ANGLE = Rotation2d.fromDegrees(-10.0); - Rotation2d PIVOT_MAX_VEL_DEPLOY = Rotation2d.fromDegrees(720.0); - Rotation2d PIVOT_MAX_ACCEL_DEPLOY = Rotation2d.fromDegrees(1440.0); - - Rotation2d PIVOT_MAX_VEL_STOW = Rotation2d.fromDegrees(360.0); - Rotation2d PIVOT_MAX_ACCEL_STOW = Rotation2d.fromDegrees(600.0); - Rotation2d THRESHOLD_TO_START_ROLLERS = Rotation2d.fromDegrees(10.0); Rotation2d ARBITRARY_VOLTAGE_THRESHOLD = Rotation2d.fromDegrees(15.0); @@ -119,12 +57,8 @@ public interface Intake { double GEAR_RATIO = 37.93; - double debugVoltage = 0; //TODO: set value - SmartNumber debugDutyCycle = new SmartNumber("Intake/Debug Duty Cycle", 0.1); double STALL_CURRENT_LIMIT = 0; //TODO: set value double STALL_DEBOUNCE = 1.0; //TODO: VERIFY - double DIGESTION_DEBOUNCE = 0.2; //TODO: verify - double CURRENT_LIMIT = 45.0; } public interface Spindexer { @@ -190,47 +124,6 @@ public interface FerryTOFInterpolation { }; } - // public interface AngleInterpolation { - // public final double[][] distanceAngleInterpolationValues = { - // {1.22, Units.degreesToRadians(20)}, //AGAINST THE HUB - // {1.43, Units.degreesToRadians(21.0)}, - // {2.15, Units.degreesToRadians(23.23)}, - - // //NEW - // {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 { - // public final double[][] distanceRPMInterpolationValues = { - // {1.22, 2850.0}, //AGAINST THE HUB - // {1.43, 3000.0}, - // {2.15, 3050.0}, - - // //NEW - // {2.864967, 3215.271125}, - // {3.65, 3400.0}, - // {4.43, 3650.0}, - // {5.32, 3950.0} - // }; - // } - - // public interface TOFInterpolation{ - // public final double[][] distanceTOFInterpolationValues = { // COLLECT THESE - // // {1.22, 0.0}, - // {1.30, 1.01}, // seconds - // // {1.43, 0.0}, - // // {2.15, 0.0}, - // {2.864967, 1.1}, - // // {3.65, 0.0}, - // {4.43, 1.234}, - // {5.32, 1.267} - // }; - // } - 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/climberhopper/ClimberHopper.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java deleted file mode 100644 index 97fb6487..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java +++ /dev/null @@ -1,84 +0,0 @@ -/************************ 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.subsystems.climberhopper; - -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; - - static { - if (Robot.isReal()) { - instance = new ClimberHopperImpl(); - } else { - instance = new ClimberHopperSim(); - } - } - - public static ClimberHopper getInstance() { - return instance; - } - - public enum ClimberHopperState { - // CLIMBER_UP(Settings.ClimberHopper.CLIMBER_UP_ROTATIONS), - // CLIMBER_DOWN(Settings.ClimberHopper.CLIMBER_DOWN_ROTATIONS), - // // HOPPER_UP(Settings.ClimberHopper.HOPPER_UP_ROTATIONS), - // HOPPER_DOWN(Settings.ClimberHopper.HOPPER_DOWN_ROTATIONS), - CLIMBER_UP(Settings.ClimberHopper.CLIMBER_UP_HEIGHT_METERS), - CLIMBER_DOWN(Settings.ClimberHopper.CLIMBER_DOWN_HEIGHT_METERS), - // HOPPER_UP(Settings.ClimberHopper.HOPPER_UP_ROTATIONS), - HOPPER_DOWN(Settings.ClimberHopper.HOPPER_UP_HEIGHT_METERS), - STOP(0.0); - - private double targetHeight; - - private ClimberHopperState(double targetHeight) { - this.targetHeight = targetHeight; - } - - public double getTargetHeight() { - return targetHeight; - } - - } - - private ClimberHopperState state; - - protected ClimberHopper() { - this.state = ClimberHopperState.HOPPER_DOWN; - } - - public ClimberHopperState getState() { - return state; - } - - public void setState(ClimberHopperState state) { - this.state = state; - } - - public abstract boolean getStalling(); - public abstract double getCurrentHeight(); - public abstract double getCurrentRotations(); - public abstract boolean atTargetHeight(); - - /** - * Resets the encoder postition to the upper hardstop - */ - public abstract void resetPostionUpper(); - - public abstract void setVoltageOverride(Optional voltage); - - @Override - public void periodic() { - SmartDashboard.putString("ClimberHopper/State", getState().toString()); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java deleted file mode 100644 index 5b072d62..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ /dev/null @@ -1,132 +0,0 @@ -/************************ 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.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; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import java.util.Optional; - - -public class ClimberHopperImpl extends ClimberHopper { - private final Motors.TalonFXConfig climberHopperConfig; - - private final TalonFX motor; - private final VoltageOut controller; - - private final BStream stalling; - private double voltage; - - private Optional voltageOverride; - - public ClimberHopperImpl() { - super(); - - climberHopperConfig = new Motors.TalonFXConfig() - .withInvertedValue(InvertedValue.CounterClockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake) - - .withSupplyCurrentLimitAmps(50.0) - .withStatorCurrentLimitEnabled(false) - .withRampRate(Settings.ClimberHopper.RAMP_RATE) - .withSensorToMechanismRatio(Settings.ClimberHopper.GEAR_RATIO) - .withSoftLimits( - false, false, - Settings.ClimberHopper.ROTATIONS_AT_BOTTOM + Settings.ClimberHopper.NUM_ROTATIONS_TO_REACH_TOP, - Settings.ClimberHopper.ROTATIONS_AT_BOTTOM); - - motor = new TalonFX(Ports.ClimberHopper.CLIMBER_HOPPER, Ports.CANIVORE); - climberHopperConfig.configure(motor); - - motor.setPosition(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM); - stalling = BStream.create(() -> motor.getStatorCurrent().getValueAsDouble() > Settings.ClimberHopper.STALL) - .filtered(new BDebounce.Both(Settings.ClimberHopper.DEBOUNCE)); - - controller = new VoltageOut(0) - .withEnableFOC(true); - - voltageOverride = Optional.empty(); - } - - @Override - public boolean getStalling() { - return stalling.getAsBoolean(); - } - - @Override - public double getCurrentHeight() { - return this.motor.getPosition().getValueAsDouble() * Settings.ClimberHopper.POSITION_CONVERSION_FACTOR; - } - - @Override - public double getCurrentRotations() { - return motor.getPosition().getValueAsDouble(); - } - - private boolean isWithinTolerance(double toleranceMeters) { - return Math.abs(getState().getTargetHeight() - getCurrentHeight()) < toleranceMeters; - } - - @Override - public boolean atTargetHeight() { - return isWithinTolerance(Settings.ClimberHopper.HEIGHT_TOLERANCE_METERS); - } - - public void resetPostionUpper() { - motor.setPosition(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM + Settings.ClimberHopper.NUM_ROTATIONS_TO_REACH_TOP); - } - - @Override - public void periodic() { - super.periodic(); - - if (voltageOverride.isPresent()) { - voltage = voltageOverride.get(); - } else { - if (!atTargetHeight()) { - if (getState() == ClimberHopperState.CLIMBER_DOWN) voltage = -Settings.ClimberHopper.MOTOR_VOLTAGE; - else if (getState() == ClimberHopperState.CLIMBER_UP) voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; - else if (getState() == ClimberHopperState.HOPPER_DOWN) voltage = -Settings.ClimberHopper.MOTOR_VOLTAGE; - } else { - voltage = 0; - } - } - - if (EnabledSubsystems.CLIMBER_HOPPER.get()) { - motor.setControl(controller.withOutput(voltage)); - } else { - motor.stopMotor(); - } - - if (Settings.DEBUG_MODE) { - SmartDashboard.putBoolean("ClimberHopper/Stalling", getStalling()); - - SmartDashboard.putNumber("ClimberHopper/Current Height", getCurrentHeight()); - SmartDashboard.putBoolean("Climber/At Target Height?", atTargetHeight()); - SmartDashboard.putNumber("ClimberHopper/Voltage", voltage); - SmartDashboard.putNumber("ClimberHopper/Applied Voltage", motor.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("ClimberHopper/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("ClimberHopper/Stator Current", motor.getStatorCurrent().getValueAsDouble()); - - SmartDashboard.putNumber("Current Draws/ClimberHopper (amps)", motor.getSupplyCurrent().getValueAsDouble()); - } - } - - @Override - public void setVoltageOverride(Optional voltage) { - this.voltageOverride = voltage; - } -} \ 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 deleted file mode 100644 index cd39369a..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java +++ /dev/null @@ -1,105 +0,0 @@ -/************************ 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.subsystems.climberhopper; - - -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 { - - private final ElevatorSim sim; - private final ClimberHopperVisualizer visualizer; - private double voltage; - - private Optional voltageOverride; - - public ClimberHopperSim() { - visualizer = ClimberHopperVisualizer.getInstance(); - - sim = new ElevatorSim( - DCMotor.getKrakenX60(1), - Settings.ClimberHopper.GEAR_RATIO, - Settings.ClimberHopper.MASS_KG, - Settings.ClimberHopper.DRUM_RADIUS_METERS, - Settings.ClimberHopper.MIN_HEIGHT_METERS, - Settings.ClimberHopper.MAX_HEIGHT_METERS, - false, - Settings.ClimberHopper.MIN_HEIGHT_METERS - ); - - voltageOverride = Optional.empty(); - } - - public boolean getStalling() { - return sim.getCurrentDrawAmps() > Settings.ClimberHopper.STALL; - } - - @Override - public double getCurrentHeight() { - return sim.getPositionMeters(); - } - - @Override - public double getCurrentRotations() { - return sim.getPositionMeters() / Settings.ClimberHopper.POSITION_CONVERSION_FACTOR; - } - - private boolean isWithinTolerance(double toleranceMeters) { - return Math.abs(getState().getTargetHeight() - getCurrentHeight()) < toleranceMeters; - } - - @Override - public boolean atTargetHeight() { - return isWithinTolerance(Settings.ClimberHopper.HEIGHT_TOLERANCE_METERS); - } - - @Override - public void setVoltageOverride(Optional voltage) { - this.voltageOverride = voltage; - } - - @Override - public void resetPostionUpper() { - // No encoder reset for sim - } - - - @Override - public void periodic() { - super.periodic(); - - 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 = 0; - } - } - - sim.setInputVoltage(voltage); - - SmartDashboard.putNumber("ClimberHopper/Voltage", voltage); - SmartDashboard.putNumber("ClimberHopper/Current", sim.getCurrentDrawAmps()); - SmartDashboard.putBoolean("ClimberHopper/Stalling", getStalling()); - SmartDashboard.putNumber("ClimberHopper/Height", getCurrentHeight()); - visualizer.update(getCurrentHeight()); - - sim.update(0.02); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperVisualizer.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperVisualizer.java deleted file mode 100644 index cc890cf5..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperVisualizer.java +++ /dev/null @@ -1,69 +0,0 @@ -/************************ 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.subsystems.climberhopper; - -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; - -public class ClimberHopperVisualizer { - private static final ClimberHopperVisualizer instance; - - static { - instance = new ClimberHopperVisualizer(); - } - - public static ClimberHopperVisualizer getInstance() { - return instance; - } - - private final Mechanism2d canvas; - private final MechanismRoot2d hopper; - private final MechanismRoot2d bot; - private final MechanismRoot2d climber; - - ClimberHopperVisualizer() { - canvas = new Mechanism2d(2.5, 2.5); - hopper = canvas.getRoot("Hopper", 0.8, 1); - bot = canvas.getRoot("Bot", 0.8, 0.5); - climber = canvas.getRoot("Climber", 0.4, 0.5); - - hopper.append(new MechanismLigament2d( - "Hopper", - 0.5, - 0, - 0.5, - new Color8Bit(Color.kYellow) - ) - ); - - bot.append(new MechanismLigament2d( - "Bot", - 1, - 0, - 1, - new Color8Bit(Color.kRed) - )); - - climber.append(new MechanismLigament2d( - "Climber", - 0.2, - 90, - 0.5, - new Color8Bit(Color.kOrange) - )); - } - - public void update(double height) { - - hopper.setPosition(0.8, height + 0.5); - - SmartDashboard.putData("Visualizers/ClimberHopper", canvas); - } -} 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 8d9dca46..e80bdbe1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -40,9 +40,7 @@ protected Intake() { public enum PivotState { DEPLOY(Settings.Intake.PIVOT_DEPLOY_ANGLE), - STOW(Settings.Intake.PIVOT_STOW_ANGLE), - DIGESTION_UP(Settings.Intake.DIGESTION_UP_ANGLE), - DIGESTION_DOWN(Settings.Intake.DIGESTION_DOWN_ANGLE); + STOW(Settings.Intake.PIVOT_STOW_ANGLE); private final Rotation2d targetAngle; 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 807a4f9a..839094d4 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -9,7 +9,6 @@ import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; @@ -23,7 +22,6 @@ import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Gains.Intake.Pivot; import com.stuypulse.robot.util.SettableNumber; import com.stuypulse.robot.util.SysId; import com.stuypulse.stuylib.streams.booleans.BStream; @@ -41,8 +39,6 @@ public class IntakeImpl extends Intake { private final TalonFX rollerLeader; private final TalonFX rollerFollower; - private final MotionMagicVoltage pivotController; - private final DutyCycleOut rollerController; private final Follower follower; @@ -67,16 +63,13 @@ public IntakeImpl() { Gains.Intake.Pivot.kG, 0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign, 0) .withGravityType(GravityTypeValue.Arm_Cosine) - .withMotionProfile(Settings.Intake.PIVOT_MAX_VEL_STOW.getRotations(), - Settings.Intake.PIVOT_MAX_ACCEL_STOW.getRotations()) .withSensorToMechanismRatio(Settings.Intake.GEAR_RATIO); rollerConfig = new Motors.TalonFXConfig() .withInvertedValue(InvertedValue.CounterClockwise_Positive) .withNeutralMode(NeutralModeValue.Brake) - - .withSupplyCurrentLimitAmps(Settings.Intake.CURRENT_LIMIT) + .withSupplyCurrentLimitAmps(45.0) .withStatorCurrentLimitEnabled(false) .withRampRate(0.50); @@ -89,15 +82,11 @@ public IntakeImpl() { rollerFollower = new TalonFX(Ports.Intake.ROLLER_FOLLOWER, Ports.RIO); rollerConfig.configure(rollerFollower); - pivotController = new MotionMagicVoltage(getPivotState().getTargetAngle().getRotations()).withEnableFOC(true); rollerController = new DutyCycleOut(getRollerState().getTargetDutyCycle()).withEnableFOC(true); follower = new Follower(Ports.Intake.ROLLER_LEADER, MotorAlignmentValue.Aligned); rollerFollower.setControl(follower); - velLimit = new SettableNumber(Settings.Intake.PIVOT_MAX_VEL_DEPLOY.getDegrees()); - accelLimit = new SettableNumber(Settings.Intake.PIVOT_MAX_ACCEL_DEPLOY.getDegrees()); - pivotVoltageOverride = Optional.empty(); pivot.setPosition(Settings.Intake.PIVOT_MAX_ANGLE.getRotations()); @@ -123,26 +112,6 @@ public Rotation2d getPivotAngle() { return Rotation2d.fromRotations(pivot.getPosition().getValueAsDouble()); } - private void setMotionProfileConstraints(Rotation2d velLimit, Rotation2d accelLimit) { - this.velLimit.set(velLimit.getDegrees()); - this.accelLimit.set(accelLimit.getDegrees()); - pivotConfig.withMotionProfile(velLimit.getRotations(), accelLimit.getRotations()); - pivotConfig.configure(pivot); - } - - @Override - public void setPivotState(PivotState pivotState) { - super.setPivotState(pivotState); - - if (getPivotState() == PivotState.STOW) { - setMotionProfileConstraints(Settings.Intake.PIVOT_MAX_VEL_STOW, Settings.Intake.PIVOT_MAX_ACCEL_STOW); - } else if (getPivotState() == PivotState.DEPLOY) { - setMotionProfileConstraints(Settings.Intake.PIVOT_MAX_VEL_DEPLOY, Settings.Intake.PIVOT_MAX_ACCEL_DEPLOY); - } - - SmartDashboard.putString("Intake/Profile Constraints", getPivotState().name()); - } - @Override public void zeroPivotStowed() { pivot.setPosition(Settings.Intake.PIVOT_MAX_ANGLE.getRotations()); @@ -165,8 +134,6 @@ public void periodic() { // PIVOT if (pivotState == PivotState.DEPLOY && getPivotAngle().getDegrees() <= Settings.Intake.ARBITRARY_VOLTAGE_THRESHOLD.getDegrees()) { pivot.setControl(new VoltageOut(-Settings.Intake.PUSHDOWN_VOLTAGE)); // applying 3 volts - } else if (pivotState == PivotState.DIGESTION_DOWN || pivotState == PivotState.DIGESTION_UP) { - pivot.setControl(new MotionMagicVoltage(pivotState.getTargetAngle().getRotations())); //TODO: verify motion profile works } else { pivot.setControl(new PositionVoltage(pivotState.getTargetAngle().getRotations())); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index f1ddf68e..44ae483e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -95,10 +95,6 @@ public boolean atTolerance() { return isShooterAtTolerance() && isHoodAtTolerance() && isTurretAtTolerance(); } - // public boolean isHoodUnderTrench() { - // return hood.isUnderTrench(); - // } - public boolean isShooterAtTolerance() { return shooter.atTolerance(); } @@ -144,9 +140,5 @@ public void periodic() { SmartDashboard.putBoolean("Superstructure/Shooter At Tolerance?", isShooterAtTolerance()); SmartDashboard.putBoolean("Superstructure/Hood At Tolerance?", isHoodAtTolerance()); SmartDashboard.putBoolean("Superstructure/Turret At Tolerance?", isTurretAtTolerance()); - SmartDashboard.putBoolean("Superstructure/Everything At Tolerance?", atTolerance()); - - SmartDashboard.putNumber("InterpolationTesting/Current Hood Angle", getHoodAngle().getDegrees()); - SmartDashboard.putNumber("InterpolationTesting/Current Shooter RPM", getShooterRPM()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java index 33121c29..20bc231a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -15,11 +15,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.MotorAlignmentValue; @@ -33,7 +30,6 @@ public class ShooterImpl extends Shooter { private final TalonFX shooterFollower; private final VelocityVoltage shooterController; - // private final VelocityTorqueCurrentFOC shooterController; private final Follower follower; private Optional voltageOverride; @@ -62,7 +58,6 @@ public ShooterImpl() { shooterConfig.configure(shooterFollower); shooterController = new VelocityVoltage(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE).withEnableFOC(true); - // shooterController = new VelocityTorqueCurrentFOC(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE); follower = new Follower(Ports.Superstructure.Shooter.MOTOR_LEAD, MotorAlignmentValue.Opposed); shooterFollower.setControl(follower); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index a13ec5c1..490905bd 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -147,11 +147,9 @@ private double getDelta(double target, double current) { if (delta > 180.0) delta -= 360; else if (delta < -180) delta += 360; - // if (Math.abs(current + delta) < Settings.Superstructure.Turret.RANGE) return delta; if (current + delta < Settings.Superstructure.Turret.RANGE_LEFT) return delta + 360; if (current + delta > Settings.Superstructure.Turret.RANGE_RIGHT) return delta - 360; - // return delta < 0 ? delta + 360 : delta - 360; return delta; } From 86732cfa6533863276ea7c179e7d5eb67baac511 Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Mon, 9 Mar 2026 01:22:20 -0400 Subject: [PATCH 117/150] FIX: turretSim now has wrapping logic + fix scoring routine button binding --- .../com/stuypulse/robot/RobotContainer.java | 24 ++++----- .../commands/handoff/HandoffSetState.java | 9 +++- .../commands/spindexer/SpindexerSetState.java | 9 +++- .../subsystems/superstructure/hood/Hood.java | 2 +- .../superstructure/turret/TurretImpl.java | 7 +-- .../superstructure/turret/TurretSim.java | 49 +++++++++++++++---- 6 files changed, 69 insertions(+), 31 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index bf9d1c45..5cf9bc20 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -13,6 +13,7 @@ import com.stuypulse.robot.commands.auton.regular.LeftTwoCycle; import com.stuypulse.robot.commands.auton.regular.RightOneCycle; import com.stuypulse.robot.commands.handoff.HandoffConditionalCommand; +import com.stuypulse.robot.commands.handoff.HandoffDefaultCommand; import com.stuypulse.robot.commands.handoff.HandoffReverse; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; @@ -25,6 +26,7 @@ import com.stuypulse.robot.commands.intake.ZeroPivotDeployed; import com.stuypulse.robot.commands.intake.ZeroPivotStowed; import com.stuypulse.robot.commands.spindexer.SpindexerConditionalCommand; +import com.stuypulse.robot.commands.spindexer.SpindexerDefaultCommand; import com.stuypulse.robot.commands.spindexer.SpindexerReverse; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; @@ -122,15 +124,15 @@ public RobotContainer() { SmartDashboard.putData("Robot/Seed Turret", new SeedTurret().ignoringDisable(true)); SmartDashboard.putData("Robot/Zero Hood Encoder", new ZeroHoodEncoderAtUpperHardstop().ignoringDisable(true)); - SmartDashboard.putData("Handoff Reverse", + SmartDashboard.putData("Robot/Handoff Reverse", new ConditionalCommand( new HandoffReverse().andThen(new WaitCommand(0.25)).andThen(new HandoffRun()), new HandoffReverse().andThen(new WaitCommand(0.25).andThen(new HandoffStop())), () -> handoff.getState() == HandoffState.FORWARD)); - SmartDashboard.putData("Intake Reverse", new IntakeSetState(RollerState.OUTTAKE)); + SmartDashboard.putData("Robot/Intake Reverse", new IntakeSetState(RollerState.OUTTAKE)); - SmartDashboard.putData("Spindexer Reverse", + SmartDashboard.putData("Robot/Spindexer Reverse", new ConditionalCommand( new SpindexerReverse().andThen(new WaitCommand(0.25)).andThen(new SpindexerRun()), new SpindexerReverse().andThen(new WaitCommand(0.25).andThen(new SpindexerStop())), @@ -156,9 +158,9 @@ private void configureButtonBindings() { .whileTrue(new SwerveXMode()) .onTrue(new IntakeRunRollers()) .whileTrue(new SuperstructureInterpolation() - .andThen(new WaitUntilCommand(superstructure::atTolerance)) + .alongWith(new WaitUntilCommand(() -> superstructure.atTolerance() && superstructure.getState() == SuperstructureState.INTERPOLATION)) .andThen(new HandoffRun()) - .andThen(new WaitUntilCommand(handoff::atTolerance)) + .alongWith(new WaitUntilCommand(() -> handoff.atTolerance() && handoff.getState() == HandoffState.FORWARD)) .andThen(new SpindexerRun())) .onFalse(new SpindexerStop() .alongWith(new SuperstructureStow()) @@ -190,10 +192,11 @@ private void configureButtonBindings() { // Ferrying In Place driver.getDPadRight() .whileTrue(new SwerveXMode()) - .whileTrue(new SuperstructureFerry().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) - .andThen(new HandoffRun()) - .andThen(new WaitUntilCommand(handoff::atTolerance)) - .alongWith(new SpindexerRun())) + .whileTrue(new SuperstructureFerry() + .alongWith(new WaitUntilCommand(() -> superstructure.atTolerance() && superstructure.getState() == SuperstructureState.INTERPOLATION)) + .andThen(new HandoffRun()) + .alongWith(new WaitUntilCommand(() -> handoff.atTolerance() && handoff.getState() == HandoffState.FORWARD)) + .andThen(new SpindexerRun())) .onFalse(new SuperstructureFerry().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); // SOTM @@ -205,8 +208,7 @@ private void configureButtonBindings() { new SpindexerStop(), new HandoffStop() ), - new SuperstructureSOTM().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) - .andThen(new SpindexerRun()).alongWith(new HandoffRun()), + new SuperstructureSOTM(), () -> superstructure.getState() == SuperstructureState.SOTM )); diff --git a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffSetState.java b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffSetState.java index 72885181..ce13ffd6 100644 --- a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffSetState.java +++ b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffSetState.java @@ -8,9 +8,9 @@ import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Command; -public class HandoffSetState extends InstantCommand{ +public class HandoffSetState extends Command { private final Handoff handoff; private HandoffState state; @@ -24,4 +24,9 @@ public HandoffSetState(HandoffState state) { public void initialize() { handoff.setState(state); } + + @Override + public boolean isFinished() { + return true; + } } diff --git a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerSetState.java b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerSetState.java index 415a4de1..81474ebe 100644 --- a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerSetState.java +++ b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerSetState.java @@ -8,9 +8,9 @@ import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Command; -public class SpindexerSetState extends InstantCommand { +public class SpindexerSetState extends Command { private final Spindexer spindexer; private SpindexerState state; @@ -24,4 +24,9 @@ public SpindexerSetState(SpindexerState state) { public void initialize() { spindexer.setState(state); } + + @Override + public boolean isFinished() { + return true; + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java index 4d161ca3..b9547d98 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java @@ -91,7 +91,7 @@ public boolean atTolerance() { if (Robot.isReal()) { return Math.abs(error) < Settings.Superstructure.HOOD_TOLERANCE.getRotations(); } else { - return Math.abs(error) < Settings.Superstructure.HOOD_TOLERANCE.getRotations() + (0.5 / 360.0); + return Math.abs(error) < Settings.Superstructure.HOOD_TOLERANCE.getRotations() + (2 / 360.0); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index 490905bd..c6c27434 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -49,7 +49,6 @@ public TurretImpl() { .withSupplyCurrentLimitAmps(80) .withStatorCurrentLimitEnabled(false) .withRampRate(0.25) - // .withVoltageLimits(6, -6) //TODO: VERIFY MAX VOLTAGE .withPIDConstants(Gains.Superstructure.Turret.slot0.kP, Gains.Superstructure.Turret.slot0.kI, Gains.Superstructure.Turret.slot0.kD, 0) .withFFConstants(Gains.Superstructure.Turret.slot0.kS, Gains.Superstructure.Turret.slot0.kV, Gains.Superstructure.Turret.slot0.kA, 0) @@ -122,7 +121,6 @@ public void zeroEncoders() { } public void seedTurret() { - // motor.setPosition(0); //TODO: SEED USING CRT INSTEAD OF TS, TS IS TEMP turretMotor.setPosition(getVectorSpaceAngle().getRotations()); } @@ -174,11 +172,10 @@ public void periodic() { double currentAngle = getAngle().getDegrees(); double actualTargetDeg = currentAngle + getDelta(getTargetAngle().getDegrees(), currentAngle); - isWrapping = Math.abs(actualTargetDeg - currentAngle) > - Settings.Superstructure.Turret.GAIN_SWITCHING_THRESHOLD.getDegrees(); + isWrapping = Math.abs(actualTargetDeg - currentAngle) > Settings.Superstructure.Turret.GAIN_SWITCHING_THRESHOLD.getDegrees(); int slot = 0; - if(isWrapping) { + if (isWrapping) { slot = 1; } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java index ec68b67e..e4e02aa0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java @@ -40,6 +40,8 @@ public class TurretSim extends Turret { private double maxAngularVelRadiansPerSecond; private double maxAngularAccelRadiansPerSecondSquared; + private boolean isWrapping; + private Optional voltageOverride; public TurretSim() { @@ -72,6 +74,7 @@ public TurretSim() { setpoint = new TrapezoidProfile.State(); voltageOverride = Optional.empty(); + isWrapping = false; } @Override @@ -89,6 +92,17 @@ public void zeroEncoders() { return; } + @Override + public boolean isWrapping() { + return isWrapping; + } + + @Override + public boolean atTolerance() { + double error = getAngle().minus(getTargetAngle()).getRotations(); + return Math.abs(error) < Settings.Superstructure.Turret.TOLERANCE.getRotations(); + } + private double getAngularVelocityRadPerSec() { return sim.getOutput(1); } @@ -97,11 +111,31 @@ private void setVoltageOverride(Optional volts) { voltageOverride = volts; } + private double getDelta(double target, double current) { + double delta = (target - current) % 360; + + if (delta > 180.0) delta -= 360; + else if (delta < -180) delta += 360; + + if (current + delta < Settings.Superstructure.Turret.RANGE_LEFT) return delta + 360; + if (current + delta > Settings.Superstructure.Turret.RANGE_RIGHT) return delta - 360; + + return delta; + } + @Override public void periodic() { super.periodic(); + + double currentAngle = getAngle().getDegrees(); + double targetAngle = getTargetAngle().getDegrees(); + double delta = getDelta(targetAngle, currentAngle); + double actualTargetDeg = currentAngle + delta; + + isWrapping = Math.abs(actualTargetDeg - currentAngle) > + Settings.Superstructure.Turret.GAIN_SWITCHING_THRESHOLD.getDegrees(); - goal = new TrapezoidProfile.State(getTargetAngle().getRadians(), 0.0); + goal = new TrapezoidProfile.State(Units.degreesToRadians(actualTargetDeg), 0.0); setpoint = profile.calculate(Settings.DT, setpoint, goal); SmartDashboard.putNumber("Superstructure/Turret/Constraints/Max Vel (deg per s)", Units.radiansToDegrees(maxAngularVelRadiansPerSecond)); @@ -109,10 +143,11 @@ public void periodic() { SmartDashboard.putNumber("Superstructure/Turret/Motion Profile Setpoint (deg)", Units.radiansToDegrees(setpoint.position)); SmartDashboard.putNumber("Superstructure/Turret/Error: abs(turret - target) (deg)", Math.abs(getAngle().minus(getTargetAngle()).getDegrees())); + SmartDashboard.putNumber("Superstructure/Turret/Current Angle (deg)", Units.radiansToDegrees(sim.getOutput(0))); + SmartDashboard.putNumber("Superstructure/Turret/Wrapped Target Angle (deg)", actualTargetDeg); + SmartDashboard.putBoolean("Superstructure/Turret/Is Wrapping", isWrapping); - SmartDashboard.putNumber("Superstructure/Turret/Current Angle (deg)", sim.getOutput(0)); - - controller.setNextR(VecBuilder.fill(setpoint.position, 0.0)); + controller.setNextR(VecBuilder.fill(setpoint.position, 0.0)); controller.correct(VecBuilder.fill(sim.getOutput(0), sim.getOutput(1))); controller.predict(Settings.DT); @@ -142,10 +177,4 @@ public SysIdRoutine getSysIdRoutine() { () -> sim.getInput(0), getInstance()); } - - @Override - public boolean isWrapping() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'isWrapping'"); - } } \ No newline at end of file From 185a2fddc5a98c0bedf319cd64e95f1626de5424 Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Mon, 9 Mar 2026 01:35:59 -0400 Subject: [PATCH 118/150] FEAT: add HandoffDefaultCommand and SpindexerDefaultCommand to stop when robot is behind tower, hub, or when turret is wrapping; separate all superstructure tolerances from SOTM --- simgui.json | 96 +------------------ .../com/stuypulse/robot/RobotContainer.java | 3 +- .../handoff/HandoffDefaultCommand.java | 53 ++++++++++ .../spindexer/SpindexerDefaultCommand.java | 55 +++++++++++ .../stuypulse/robot/constants/Settings.java | 14 +-- .../superstructure/Superstructure.java | 4 +- .../subsystems/superstructure/hood/Hood.java | 8 +- .../superstructure/shooter/Shooter.java | 9 +- .../superstructure/turret/Turret.java | 7 +- 9 files changed, 143 insertions(+), 106 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/handoff/HandoffDefaultCommand.java create mode 100644 src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDefaultCommand.java diff --git a/simgui.json b/simgui.json index 33cbc814..16a565ca 100644 --- a/simgui.json +++ b/simgui.json @@ -1,11 +1,4 @@ { - "HALProvider": { - "Other Devices": { - "window": { - "visible": false - } - } - }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo", @@ -14,28 +7,20 @@ "/SmartDashboard/Handoff Reverse": "Command", "/SmartDashboard/Intake Reverse": "Command", "/SmartDashboard/PathPlanner": "Alerts", - "/SmartDashboard/Robot/Override Down": "Command", - "/SmartDashboard/Robot/Override Stop": "Command", - "/SmartDashboard/Robot/Override Up": "Command", - "/SmartDashboard/Robot/Reset Pivot": "Command", - "/SmartDashboard/Robot/Zero Encoders": "Command", + "/SmartDashboard/Robot/Handoff Reverse": "Command", + "/SmartDashboard/Robot/Intake Reverse": "Command", + "/SmartDashboard/Robot/Seed Turret": "Command", + "/SmartDashboard/Robot/Spindexer Reverse": "Command", "/SmartDashboard/Robot/Zero Hood Encoder": "Command", "/SmartDashboard/Robot/Zero Pivot Encoder at Lower Limit (Deployed)": "Command", "/SmartDashboard/Robot/Zero Pivot Encoder at Upper Limit (Stowed)": "Command", "/SmartDashboard/Robot/Zero Turret Encoders": "Command", "/SmartDashboard/Scheduler": "Scheduler", - "/SmartDashboard/Scheduler": "Scheduler", "/SmartDashboard/Spindexer Reverse": "Command", - "/SmartDashboard/Visualizers/ClimberHopper": "Mechanism2d", "/SmartDashboard/Visualizers/Hood": "Mechanism2d", "/SmartDashboard/Visualizers/Turret": "Mechanism2d" }, "windows": { - "/SmartDashboard/Autonomous": { - "window": { - "visible": true - } - }, "/SmartDashboard/Field": { "bottom": 1914, "height": 8.069275856018066, @@ -46,91 +31,18 @@ "window": { "visible": true } - }, - "/SmartDashboard/Scheduler": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Visualizers/Hood": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Visualizers/Turret": { - "window": { - "visible": true - } } } }, "NetworkTables": { - "Persistent Values": { - "open": false - }, - "Retained Values": { - "open": false - }, "transitory": { "SmartDashboard": { - "Enabled Subsystems": { - "open": true - }, - "FMSUtil": { - "open": true - }, "FieldPositions": { - "Handoff": { - "open": true - }, - "HoodedShooter": { - "Hood": { - "open": true - }, - "open": true - }, - "Intake": { - "open": true - }, - "States": { "open": true }, "States": { "open": true }, - "Superstructure": { - "Hood": { - "open": true - }, - "Turret": { - "open": true - }, - "open": true - }, - "Swerve": { - "Modules": { - "Module 0": { - "open": true - }, - "Module 1": { - "open": true - }, - "Module 2": { - "open": true - }, - "Module 3": { - "open": true - }, - "open": true - }, - "open": true - }, - "Turret": { - "open": true - }, - "open": true - }, - "limelight-right": { "open": true } } diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 5cf9bc20..623a9d02 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -145,7 +145,8 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); - // turret.setDefaultCommand(new TurretDefaultCommand()); + spindexer.setDefaultCommand(new SpindexerDefaultCommand()); + handoff.setDefaultCommand(new HandoffDefaultCommand()); } /***************/ diff --git a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffDefaultCommand.java new file mode 100644 index 00000000..5c5c266b --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffDefaultCommand.java @@ -0,0 +1,53 @@ +/************************ 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.commands.handoff; + +import com.stuypulse.robot.subsystems.handoff.Handoff; +import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; +import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Command; + +public class HandoffDefaultCommand extends Command { + private final Handoff handoff; + private final Superstructure superstructure; + private final CommandSwerveDrivetrain swerve; + + public HandoffDefaultCommand() { + this.handoff = Handoff.getInstance(); + this.superstructure = Superstructure.getInstance(); + this.swerve = CommandSwerveDrivetrain.getInstance(); + + addRequirements(handoff); + } + + @Override + public void execute() { + boolean shouldStop = swerve.isBehindTower() + || swerve.isBehindHub() + || swerve.isUnderTrench() + || superstructure.isTurretWrapping(); + + boolean prerequisiteMet = superstructure.atTolerance(); + + boolean shouldRun = !shouldStop && prerequisiteMet; + + if (shouldStop || !prerequisiteMet) { + handoff.setState(HandoffState.STOP); + } else if (shouldRun && superstructure.getState() == SuperstructureState.SOTM) { + handoff.setState(HandoffState.FORWARD); + } + } + + @Override + public boolean runsWhenDisabled() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDefaultCommand.java new file mode 100644 index 00000000..03386244 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDefaultCommand.java @@ -0,0 +1,55 @@ +/************************ 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.commands.spindexer; + +import com.stuypulse.robot.subsystems.handoff.Handoff; +import com.stuypulse.robot.subsystems.spindexer.Spindexer; +import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Command; + +public class SpindexerDefaultCommand extends Command { + private final Spindexer spindexer; + private final Handoff handoff; + private final Superstructure superstructure; + private final CommandSwerveDrivetrain swerve; + + public SpindexerDefaultCommand() { + this.spindexer = Spindexer.getInstance(); + this.handoff = Handoff.getInstance(); + this.superstructure = Superstructure.getInstance(); + this.swerve = CommandSwerveDrivetrain.getInstance(); + + addRequirements(spindexer); + } + + @Override + public void execute() { + boolean shouldStop = swerve.isBehindTower() + || swerve.isBehindHub() + || swerve.isUnderTrench() + || superstructure.isTurretWrapping(); + + boolean prerequisitesMet = handoff.atTolerance() && superstructure.atTolerance(); + + boolean shouldRun = !shouldStop && prerequisitesMet; + + if (!shouldRun) { + spindexer.setState(SpindexerState.STOP); + } else if (shouldRun && superstructure.getState() == SuperstructureState.SOTM) { + spindexer.setState(SpindexerState.FORWARD); + } + } + + @Override + public boolean runsWhenDisabled() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 95ceb528..2617e005 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -76,6 +76,9 @@ public interface Superstructure { public final double SHOOTER_TOLERANCE_RPM = 100.0; public final Rotation2d HOOD_TOLERANCE = Rotation2d.fromDegrees(0.5); + public final double SHOOTER_SOTM_TOLERANCE_RPM = 150.0; + public final Rotation2d HOOD_SOTM_TOLERANCE = Rotation2d.fromDegrees(2.0); + public interface AngleInterpolation { double[][] distanceAngleInterpolationValues = { {1.22, Units.degreesToRadians(22.5)}, //BLAY-APPROVED (ALMOST AGAINST HUB), LOCKED IN @@ -183,6 +186,7 @@ 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 Rotation2d TOLERANCE = Rotation2d.fromDegrees(2.0); + public final Rotation2d SOTM_TOLERANCE = Rotation2d.fromDegrees(4.0); public final Rotation2d KB = Rotation2d.fromDegrees(0.0); public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(0.0); @@ -194,10 +198,8 @@ public interface Turret { Rotation2d MIN_THEORETICAL_ROTATION = Rotation2d.fromDegrees(-612); /* CONSTANTS */ - public final double RANGE_LEFT = -360.0; // -120 - public final double RANGE_RIGHT = 85.0; //390; // 410 - - // 85, -360 + public final double RANGE_LEFT = -360.0; + public final double RANGE_RIGHT = 85.0; public final Rotation2d GAIN_SWITCHING_THRESHOLD = Rotation2d.fromDegrees(30); @@ -212,12 +214,12 @@ public interface BigGear { public interface Encoder17t { public final int TEETH = 17; - public final Rotation2d OFFSET = Rotation2d.fromRotations(-0.716);//(-0.28125);//.fromRotations(-0.279541015625);//(-0.86962890625); //0.6787109375 + public final Rotation2d OFFSET = Rotation2d.fromRotations(-0.716); } public interface Encoder18t { public final int TEETH = 18; - public final Rotation2d OFFSET = Rotation2d.fromRotations(-0.559);//(-0.442);//(0.58203125);//.fromRotations(-0.42822265625);//(-0.700927734375); //0.53564453125 + public final Rotation2d OFFSET = Rotation2d.fromRotations(-0.559); } public interface SoftwareLimit { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index 44ae483e..be7f6892 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -94,7 +94,7 @@ public SuperstructureState getState(){ public boolean atTolerance() { return isShooterAtTolerance() && isHoodAtTolerance() && isTurretAtTolerance(); } - + public boolean isShooterAtTolerance() { return shooter.atTolerance(); } @@ -123,7 +123,7 @@ public Rotation2d getTurretAngle(){ return turret.getAngle(); } - public boolean isWrapping() { + public boolean isTurretWrapping() { return turret.isWrapping(); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java index b9547d98..177573fd 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java @@ -89,9 +89,13 @@ public Rotation2d getTargetAngle() { public boolean atTolerance() { double error = getAngle().minus(getTargetAngle()).getRotations(); if (Robot.isReal()) { - return Math.abs(error) < Settings.Superstructure.HOOD_TOLERANCE.getRotations(); + if (state == HoodState.SOTM) { + return Math.abs(error) < Settings.Superstructure.HOOD_SOTM_TOLERANCE.getRotations(); + } else { + return Math.abs(error) < Settings.Superstructure.HOOD_TOLERANCE.getRotations(); + } } else { - return Math.abs(error) < Settings.Superstructure.HOOD_TOLERANCE.getRotations() + (2 / 360.0); + return Math.abs(error) < Settings.Superstructure.HOOD_TOLERANCE.getRotations() + (5 / 360.0); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index 2fe70245..fa46ba23 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java @@ -76,8 +76,13 @@ public double getShootRPM() { } public boolean atTolerance() { - double diff = Math.abs(getTargetRPM() - getRPM()); - return diff < Settings.Superstructure.SHOOTER_TOLERANCE_RPM; + double error = Math.abs(getTargetRPM() - getRPM()); + + if (state == ShooterState.SOTM) { + return error < Settings.Superstructure.SHOOTER_SOTM_TOLERANCE_RPM; + } else { + return error < Settings.Superstructure.SHOOTER_TOLERANCE_RPM; + } } public abstract double getRPM(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java index 4c38d54c..64133dad 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java @@ -81,7 +81,12 @@ public Rotation2d driverInputToAngle() { public boolean atTolerance() { double error = getAngle().minus(getTargetAngle()).getRotations(); - return Math.abs(error) < Settings.Superstructure.Turret.TOLERANCE.getRotations(); + + if (state == TurretState.SOTM) { + return Math.abs(error) < Settings.Superstructure.Turret.SOTM_TOLERANCE.getRotations(); + } else { + return Math.abs(error) < Settings.Superstructure.Turret.TOLERANCE.getRotations(); + } } public Rotation2d getScoringAngle() { From 386f9cb3a0ca9a7a849692b7f35c4b2ad2ef84ff Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Mon, 9 Mar 2026 02:14:10 -0400 Subject: [PATCH 119/150] FEAT: SOTM turns off when leaving alliance zone, intake deploy sets superstructure to stow (can also be used to exit SOTM) --- .../java/com/stuypulse/robot/RobotContainer.java | 4 ++-- .../commands/handoff/HandoffDefaultCommand.java | 2 +- .../java/com/stuypulse/robot/constants/Field.java | 14 ++++++++------ .../subsystems/superstructure/Superstructure.java | 4 ++++ .../subsystems/swerve/CommandSwerveDrivetrain.java | 14 +++++++++----- 5 files changed, 24 insertions(+), 14 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 623a9d02..62dba236 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -173,8 +173,8 @@ private void configureButtonBindings() { // Intake Deploy driver.getRightTriggerButton() - .onTrue(new IntakeDeploy()); - // .onTrue(new SuperstructureShoot()); // TODO: put shooter in shoot --> turn off sotm + .onTrue(new IntakeDeploy()) + .onTrue(new SuperstructureStow()); // TURNS OFF SOTM // Reset Heading driver.getDPadUp() diff --git a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffDefaultCommand.java index 5c5c266b..8af8476a 100644 --- a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffDefaultCommand.java @@ -39,7 +39,7 @@ public void execute() { boolean shouldRun = !shouldStop && prerequisiteMet; - if (shouldStop || !prerequisiteMet) { + if (!shouldRun) { handoff.setState(HandoffState.STOP); } else if (shouldRun && superstructure.getState() == SuperstructureState.SOTM) { handoff.setState(HandoffState.FORWARD); diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 8f12e456..11c93e2f 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -30,8 +30,7 @@ public interface Field { double WIDTH = Units.inchesToMeters(317.000); double LENGTH = Units.inchesToMeters(651.200); - public static final double trenchHopperTolerance = Units.inchesToMeters(50); - public static final double trenchHoodTolerance = Units.inchesToMeters(20); + public static final double TRENCH_HOOD_TOLERANCE = Units.inchesToMeters(20); // Alliance relative hub center coordinates public static final Pose2d hubCenter = new Pose2d(Units.inchesToMeters(182.11), WIDTH / 2.0, new Rotation2d()); @@ -74,19 +73,22 @@ public static Pose2d getFerryZonePose(Translation2d robot) { } /*** TRENCH COORDINATES ***/ - public interface NearLeftTrench { + public interface AllianceLeftTrench { public static final Pose2d leftEdge = new Pose2d(Units.inchesToMeters(182.11), WIDTH, new Rotation2d()); public static final Pose2d rightEdge = new Pose2d(Units.inchesToMeters(182.11), WIDTH - Units.inchesToMeters(50.59), new Rotation2d()); } - public interface NearRightTrench { + public interface AllianceRightTrench { public static final Pose2d leftEdge = new Pose2d(Units.inchesToMeters(182.11), Units.inchesToMeters(50.59), new Rotation2d()); public static final Pose2d rightEdge = new Pose2d(Units.inchesToMeters(182.11), Units.inchesToMeters(0), new Rotation2d()); } - public interface FarLeftTrench { + + // OPPONENT SIDE, BUT LEFT/RIGHT RELATIVE TO YOUR ALLIANCE POV + public interface OpponentLeftTrench { public static final Pose2d leftEdge = new Pose2d(LENGTH - Units.inchesToMeters(182.11), WIDTH, new Rotation2d()); public static final Pose2d rightEdge = new Pose2d(LENGTH - Units.inchesToMeters(182.11), WIDTH - Units.inchesToMeters(50.59), new Rotation2d()); } - public interface FarRightTrench { + // OPPONENT SIDE, BUT LEFT/RIGHT RELATIVE TO YOUR ALLIANCE POV + public interface OpponentRightTrench { public static final Pose2d leftEdge = new Pose2d(LENGTH - Units.inchesToMeters(182.11), Units.inchesToMeters(50.59), new Rotation2d()); public static final Pose2d rightEdge = new Pose2d(LENGTH - Units.inchesToMeters(182.11), Units.inchesToMeters(0), new Rotation2d()); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index be7f6892..bb03be31 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -133,6 +133,10 @@ public void periodic() { if (state == SuperstructureState.SOTM || state == SuperstructureState.FOTM) { SOTMCalculator.updateSOTMSolution(); } + + if (CommandSwerveDrivetrain.getInstance().isOutsideAllianceZone() && state == SuperstructureState.SOTM) { + setState(SuperstructureState.FERRY); + } SmartDashboard.putString("Superstructure/State", state.name()); SmartDashboard.putString("States/SuperStructure", state.name()); 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 d507d386..fdc2159a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -454,15 +454,15 @@ public double[] getWheelDrivePositionsRadians(){ public boolean isUnderTrench() { Translation2d turretTranslation = getTurretPose().getTranslation(); - boolean isBetweenRightTrenchesY = Field.NearRightTrench.rightEdge.getY() < turretTranslation.getY() && Field.NearRightTrench.leftEdge.getY() > turretTranslation.getY(); + boolean isBetweenRightTrenchesY = Field.AllianceRightTrench.rightEdge.getY() < turretTranslation.getY() && Field.AllianceRightTrench.leftEdge.getY() > turretTranslation.getY(); - boolean isBetweenLeftTrenchesY = Field.NearLeftTrench.rightEdge.getY() < turretTranslation.getY() && Field.NearLeftTrench.leftEdge.getY() > turretTranslation.getY(); + boolean isBetweenLeftTrenchesY = Field.AllianceLeftTrench.rightEdge.getY() < turretTranslation.getY() && Field.AllianceLeftTrench.leftEdge.getY() > turretTranslation.getY(); - boolean isCloseToAllianceSideTrenchX = Math.abs(turretTranslation.getX() - Field.NearRightTrench.rightEdge.getX()) < Field.trenchHoodTolerance; + boolean isUnderAllianceTrenchX = Math.abs(turretTranslation.getX() - Field.AllianceRightTrench.rightEdge.getX()) < Field.TRENCH_HOOD_TOLERANCE; - boolean isCloseToNeutralSideTrenchX = Math.abs(turretTranslation.getX() - Field.FarRightTrench.rightEdge.getX()) < Field.trenchHoodTolerance; + boolean isUnderOpponentTrenchX = Math.abs(turretTranslation.getX() - Field.OpponentRightTrench.rightEdge.getX()) < Field.TRENCH_HOOD_TOLERANCE; - boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) && (isCloseToAllianceSideTrenchX || isCloseToNeutralSideTrenchX); + boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) && (isUnderAllianceTrenchX || isUnderOpponentTrenchX); return isUnderTrench; } @@ -487,6 +487,10 @@ public boolean isBehindHub() { return behindHubX && withinHubY; } + + public boolean isOutsideAllianceZone() { + return getPose().getX() > Field.AllianceRightTrench.rightEdge.getX() + Field.TRENCH_HOOD_TOLERANCE; + } @Override public void periodic() { From 55b7412a36f3d396dbd7eea9b0faa6fb1bd5465b Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Mon, 9 Mar 2026 02:21:21 -0400 Subject: [PATCH 120/150] FEAT: fotm max velocity and add angular velocity limiting for SOTM/FOTM, cleanup conditional commands (note: they are currently unused) --- .../commands/handoff/HandoffConditionalCommand.java | 4 +++- .../spindexer/SpindexerConditionalCommand.java | 4 +++- .../robot/commands/swerve/SwerveDriveDrive.java | 12 +++++++----- .../java/com/stuypulse/robot/constants/Settings.java | 6 ++++-- 4 files changed, 17 insertions(+), 9 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffConditionalCommand.java b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffConditionalCommand.java index 0a620f1e..35687603 100644 --- a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffConditionalCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffConditionalCommand.java @@ -6,6 +6,7 @@ package com.stuypulse.robot.commands.handoff; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj2.command.ConditionalCommand; @@ -16,7 +17,8 @@ public HandoffConditionalCommand() { new HandoffRun(), () -> { CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); - return swerve.isBehindTower() || swerve.isBehindHub(); + Superstructure superstructure = Superstructure.getInstance(); + return swerve.isBehindTower() || swerve.isBehindHub() || superstructure.isTurretWrapping(); } ); } diff --git a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java index 7e1dc06b..16a53441 100644 --- a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java @@ -6,6 +6,7 @@ package com.stuypulse.robot.commands.spindexer; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj2.command.ConditionalCommand; @@ -16,7 +17,8 @@ public SpindexerConditionalCommand() { new SpindexerRun(), () -> { CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); - return swerve.isBehindTower() || swerve.isBehindHub(); + Superstructure superstructure = Superstructure.getInstance(); + return swerve.isBehindTower() || swerve.isBehindHub() || superstructure.isTurretWrapping(); } ); } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java index fadd8c7f..3f39723a 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java @@ -34,8 +34,6 @@ public class SwerveDriveDrive extends Command { private final Gamepad driver; - private double maxVelocity; - private final VStream speed; private final IStream turn; @@ -75,13 +73,17 @@ public void execute() { Vector2D speedVector = speed.get(); double angularVel = turn.get(); + double maxAngularVel = Swerve.Constraints.MAX_ANGULAR_VEL_RAD_PER_S; + if (speedVector.magnitude() > 0.05 && superstructure.getState() == SuperstructureState.SOTM) { speedVector = speedVector.normalize().mul(Settings.Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S); - // angularVel = (angularVel / Swerve.Constraints.MAX_ANGULAR_VEL_RAD_PER_S) * Settings.Swerve.Constraints.MAX_ANGULAR_VEL_SOTM_RAD_PER_S; + maxAngularVel = Settings.Swerve.Constraints.MAX_ANGULAR_VEL_SOTM_RAD_PER_S; + } else if (speedVector.magnitude() > 0.05 && superstructure.getState() == SuperstructureState.FOTM) { + speedVector = speedVector.normalize().mul(Settings.Swerve.Constraints.MAX_VELOCITY_FOTM_M_PER_S); + maxAngularVel = Settings.Swerve.Constraints.MAX_ANGULAR_VEL_FOTM_RAD_PER_S; } - SmartDashboard.putNumber("Swerve/SOTM velocity x", speedVector.x * Settings.Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S); - SmartDashboard.putNumber("Swerve/SOTM velocity y", speedVector.y * Settings.Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S); + angularVel = MathUtil.clamp(angularVel, -maxAngularVel, maxAngularVel); swerve.setControl(swerve.getFieldCentricSwerveRequest() .withVelocityX(speedVector.x) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 2617e005..80cb650c 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -242,12 +242,14 @@ public interface Swerve { public interface Constraints { public final double MAX_VELOCITY_M_PER_S = 4.93; - public final double MAX_VELOCITY_SOTM_M_PER_S = 1.00; + public final double MAX_VELOCITY_SOTM_M_PER_S = 1.00; + public final double MAX_VELOCITY_FOTM_M_PER_S = 2.00; - public final double MAX_ACCEL_M_PER_S_SQUARED = 15.0; public final double MAX_ANGULAR_VEL_RAD_PER_S = Units.degreesToRadians(300.0); public final double MAX_ANGULAR_VEL_SOTM_RAD_PER_S = Units.degreesToRadians(75.0); + public final double MAX_ANGULAR_VEL_FOTM_RAD_PER_S = Units.degreesToRadians(150.0); + public final double MAX_ACCEL_M_PER_S_SQUARED = 15.0; public final double MAX_ANGULAR_ACCEL_RAD_PER_S_SQUARED = Units.degreesToRadians(900.0); public final PathConstraints DEFAULT_CONSTRAINTS = From a776dabbefcedd469fd73b966fb2328df8c500fc Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Mon, 9 Mar 2026 02:44:49 -0400 Subject: [PATCH 121/150] FIX: ferry zone calculation was broken; also fixed FOTM --- .../com/stuypulse/robot/RobotContainer.java | 3 +-- .../com/stuypulse/robot/constants/Field.java | 27 ++++++++++++------- .../util/superstructure/SOTMCalculator.java | 3 +-- 3 files changed, 20 insertions(+), 13 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 62dba236..1b3f731f 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -222,8 +222,7 @@ private void configureButtonBindings() { new SpindexerStop(), new HandoffStop() ), - new SuperstructureFOTM().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) - .andThen(new SpindexerRun()).alongWith(new HandoffRun()), + new SuperstructureFOTM(), () -> superstructure.getState() == SuperstructureState.FOTM )); diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 11c93e2f..217cc233 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -55,20 +55,29 @@ public static Pose2d getHubPose() { public final double DISTANCE_TO_RUNGS = Units.inchesToMeters(20); // placeholder value, how far away in terms of y-cord from the rung - public static boolean closerToTop(){ - return (CommandSwerveDrivetrain.getInstance().getPose().getY() >= Field.towerFarCenter.getY()); + public static boolean closerToTop() { + return CommandSwerveDrivetrain.getInstance().getPose().getY() >= Field.towerFarCenter.getY(); } - // 1.0 meters from driverstation wall and field wall - public final Pose2d leftFerryZone = new Pose2d(Units.inchesToMeters(31.5), Units.inchesToMeters(WIDTH - 34.5), new Rotation2d()); - public final Pose2d rightFerryZone = new Pose2d(Units.inchesToMeters(20.75), Units.inchesToMeters(76), 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 final Pose2d leftFerryZone = new Pose2d( + Units.inchesToMeters(31.5), + WIDTH - Units.inchesToMeters(34.5), + new Rotation2d() + ); + + public final Pose2d rightFerryZone = new Pose2d( + Units.inchesToMeters(31.5), + Units.inchesToMeters(34.5), + new Rotation2d() + ); public static Pose2d getFerryZonePose(Translation2d robot) { - if (robot.getDistance(leftFerryZone.getTranslation()) > robot.getDistance(rightFerryZone.getTranslation())) { - return rightFerryZone; - } else { + double fieldMidY = WIDTH / 2.0; + + if (robot.getY() > fieldMidY) { return leftFerryZone; + } else { + return rightFerryZone; } } diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java index 5f8babb8..b9e77e82 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java @@ -53,7 +53,7 @@ public record SOTMSolution( Hood.getInstance().getAngle(), Turret.getInstance().getAngle(), Shooter.getInstance().getRPM(), - Field.rightFerryZone, + Field.getFerryZonePose(CommandSwerveDrivetrain.getInstance().getPose().getTranslation()), 0.0 ); @@ -194,7 +194,6 @@ public static void updateSOTMSolution() { 0 ) ); - SOTMSolution hubSolution = solveShootOnTheMove( futureTurretPose, From c8aac01d4d0a0cc292966eb97cfa9bf88101b9b8 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Mon, 9 Mar 2026 09:08:45 -0400 Subject: [PATCH 122/150] FIX: elastic and intakeimpl error. crash looped --- .../com/stuypulse/robot/RobotContainer.java | 59 ++++++++++--------- .../robot/subsystems/intake/IntakeImpl.java | 4 +- 2 files changed, 32 insertions(+), 31 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 1b3f731f..64311bff 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -134,8 +134,8 @@ public RobotContainer() { SmartDashboard.putData("Robot/Spindexer Reverse", new ConditionalCommand( - new SpindexerReverse().andThen(new WaitCommand(0.25)).andThen(new SpindexerRun()), - new SpindexerReverse().andThen(new WaitCommand(0.25).andThen(new SpindexerStop())), + new SpindexerReverse().andThen(new WaitCommand(1)).andThen(new SpindexerRun()), + new SpindexerReverse().andThen(new WaitCommand(1).andThen(new SpindexerStop())), () -> spindexer.getState() == SpindexerState.FORWARD)); } @@ -145,8 +145,8 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); - spindexer.setDefaultCommand(new SpindexerDefaultCommand()); - handoff.setDefaultCommand(new HandoffDefaultCommand()); + // spindexer.setDefaultCommand(new SpindexerDefaultCommand()); + // handoff.setDefaultCommand(new HandoffDefaultCommand()); } /***************/ @@ -157,15 +157,15 @@ private void configureButtonBindings() { // Scoring Routine driver.getTopButton() .whileTrue(new SwerveXMode()) - .onTrue(new IntakeRunRollers()) - .whileTrue(new SuperstructureInterpolation() - .alongWith(new WaitUntilCommand(() -> superstructure.atTolerance() && superstructure.getState() == SuperstructureState.INTERPOLATION)) - .andThen(new HandoffRun()) + .onTrue(new IntakeRunRollers()) + .whileTrue(new SuperstructureInterpolation() + .alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + .andThen(new HandoffRun()) .alongWith(new WaitUntilCommand(() -> handoff.atTolerance() && handoff.getState() == HandoffState.FORWARD)) .andThen(new SpindexerRun())) - .onFalse(new SpindexerStop() - .alongWith(new SuperstructureStow()) - .alongWith(new HandoffStop())); + .onFalse(new SpindexerStop() + .alongWith(new SuperstructureStow()) + .alongWith(new HandoffStop())); // Intake Stow driver.getLeftTriggerButton() @@ -183,8 +183,8 @@ private void configureButtonBindings() { .onFalse(new SetIMUMode(0)); // Stop Rollers - driver.getLeftBumper() - .onTrue(new IntakeStopRollers()); + // driver.getLeftBumper() + // .onTrue(new IntakeStopRollers()); // driver.getRightBumper() // .whileTrue(new IntakeOuttake()) @@ -193,6 +193,7 @@ private void configureButtonBindings() { // Ferrying In Place driver.getDPadRight() .whileTrue(new SwerveXMode()) + .onTrue(new IntakeRunRollers()) .whileTrue(new SuperstructureFerry() .alongWith(new WaitUntilCommand(() -> superstructure.atTolerance() && superstructure.getState() == SuperstructureState.INTERPOLATION)) .andThen(new HandoffRun()) @@ -226,6 +227,9 @@ private void configureButtonBindings() { () -> superstructure.getState() == SuperstructureState.FOTM )); + driver.getLeftBumper() + .whileTrue(new SwerveXMode()); + //--------------------------------------------------------------------------------------------------------------------------\\ // // Manual Left Corner Scoring @@ -249,9 +253,6 @@ private void configureButtonBindings() { // .andThen(new SpindexerRun()).alongWith(new HandoffRun())) // .onFalse(new SuperstructureInterpolation().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); - // // Swerve X Wheels - // driver.getLeftBumper() - // .whileTrue(new SwerveXMode()); } /**************/ @@ -263,9 +264,9 @@ public void configureAutons() { autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); // BASE - AutonConfig EIGHT_FUEL = new AutonConfig("Eight Fuel", EightFuel::new, - ""); - EIGHT_FUEL.register(autonChooser); + // AutonConfig EIGHT_FUEL = new AutonConfig("Eight Fuel", EightFuel::new, + // ""); + // EIGHT_FUEL.register(autonChooser); // DEPOT AutonConfig DEPOT_AUTON = new AutonConfig("Depot Auton", DepotAuton::new, @@ -273,13 +274,13 @@ public void configureAutons() { DEPOT_AUTON.register(autonChooser); // ONE CYCLES - AutonConfig LEFT_ONE_CYCLE = new AutonConfig("Left One Cycle", LeftOneCycle::new, - "Left Trench To NZ", "Left NZ To Score"); - LEFT_ONE_CYCLE.register(autonChooser); + // AutonConfig LEFT_ONE_CYCLE = new AutonConfig("Left One Cycle", LeftOneCycle::new, + // "Left Trench To NZ", "Left NZ To Score"); + // LEFT_ONE_CYCLE.register(autonChooser); - AutonConfig RIGHT_ONE_CYCLE = new AutonConfig("Right One Cycle", RightOneCycle::new, - "Right Trench To NZ", "Right NZ To Score"); - RIGHT_ONE_CYCLE.register(autonChooser); + // AutonConfig RIGHT_ONE_CYCLE = new AutonConfig("Right One Cycle", RightOneCycle::new, + // "Right Trench To NZ", "Right NZ To Score"); + // RIGHT_ONE_CYCLE.register(autonChooser); // TWO CYCLES AutonConfig LEFT_TWO_CYCLE = new AutonConfig("Left Two Cycle", LeftTwoCycle::new, @@ -296,10 +297,10 @@ public void configureAutons() { public void configureSysids() { - // autonChooser.addOption("SysID Module Translation Dynamic Forwards", swerve.sysIdDynamic(Direction.kForward)); - // autonChooser.addOption("SysID Module Translation Dynamic Backwards", swerve.sysIdDynamic(Direction.kReverse)); - // autonChooser.addOption("SysID Module Translation Quasi Forwards", swerve.sysIdQuasistatic(Direction.kForward)); - // autonChooser.addOption("SysID Module Translation Quasi Backwards", swerve.sysIdQuasistatic(Direction.kReverse)); + autonChooser.addOption("SysID Module Translation Dynamic Forwards", swerve.sysIdDynamic(Direction.kForward)); + autonChooser.addOption("SysID Module Translation Dynamic Backwards", swerve.sysIdDynamic(Direction.kReverse)); + autonChooser.addOption("SysID Module Translation Quasi Forwards", swerve.sysIdQuasistatic(Direction.kForward)); + autonChooser.addOption("SysID Module Translation Quasi Backwards", swerve.sysIdQuasistatic(Direction.kReverse)); // autonChooser.addOption("SysID Rotation Translation Dynamic Forwards", swerve.sysidRotationDynamic(Direction.kForward)); // autonChooser.addOption("SysID Rotation Translation Dynamic Backwards", swerve.sysidRotationDynamic(Direction.kReverse)); 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 839094d4..ad793a87 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -162,8 +162,8 @@ public void periodic() { SmartDashboard.putNumber("Intake/Pivot Supply Current (amps)", pivot.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Intake/Pivot Stator Current (amps)", pivot.getStatorCurrent().getValueAsDouble()); - SmartDashboard.putNumber("Intake/Pivot Max Velocity Limit (deg/s)", velLimit.get()); - SmartDashboard.putNumber("Intake/Pivot Max Accel Limit (deg/s^2)", accelLimit.get()); + // SmartDashboard.putNumber("Intake/Pivot Max Velocity Limit (deg/s)", velLimit.get()); Causing issues + // SmartDashboard.putNumber("Intake/Pivot Max Accel Limit (deg/s^2)", accelLimit.get()); Causing issues SmartDashboard.putNumber("Intake/Pivot Angle Error (deg)", Math.abs(getPivotState().getTargetAngle().getDegrees() - getPivotAngle().getDegrees())); From daed7138665a330961b85c5d5b2c09a4a736ac3d Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Mon, 9 Mar 2026 12:02:50 -0400 Subject: [PATCH 123/150] FIX: Updated buttons --- elastic-layout.json | 57 ++++++++++++++++++++++++++++----------------- 1 file changed, 35 insertions(+), 22 deletions(-) diff --git a/elastic-layout.json b/elastic-layout.json index 81632983..c463037c 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -418,6 +418,19 @@ "true_icon": "None", "false_icon": "None" } + }, + { + "title": "Autonomous", + "x": 560.0, + "y": 420.0, + "width": 210.0, + "height": 140.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Autonomous", + "period": 0.06, + "sort_options": false + } } ] } @@ -840,14 +853,31 @@ } }, { - "title": "Handoff Reverse", + "title": "Superstructure At Tol?", "x": 840.0, + "y": 350.0, + "width": 280.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Superstructure/Everything At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Handoff Reverse", + "x": 1120.0, "y": 420.0, "width": 280.0, "height": 280.0, "type": "Command", "properties": { - "topic": "/SmartDashboard/Reverse Handoff", + "topic": "/SmartDashboard/Robot/Handoff Reverse", "period": 0.06, "show_type": false, "maximize_button_space": true @@ -861,7 +891,7 @@ "height": 280.0, "type": "Command", "properties": { - "topic": "/SmartDashboard/Intake Reverse", + "topic": "/SmartDashboard/Robot/Intake Reverse", "period": 0.06, "show_type": false, "maximize_button_space": true @@ -869,34 +899,17 @@ }, { "title": "Spindexer Reverse", - "x": 1120.0, + "x": 840.0, "y": 420.0, "width": 280.0, "height": 280.0, "type": "Command", "properties": { - "topic": "/SmartDashboard/Spindexer Reverse", + "topic": "/SmartDashboard/Robot/Spindexer Reverse", "period": 0.06, "show_type": false, "maximize_button_space": true } - }, - { - "title": "Superstructure At Tol?", - "x": 840.0, - "y": 350.0, - "width": 280.0, - "height": 70.0, - "type": "Boolean Box", - "properties": { - "topic": "/SmartDashboard/Superstructure/Everything At Tolerance?", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } } ] } From d210422e6dfe1b81a8b8b5291e4b4daf6a7ec068 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Mon, 9 Mar 2026 14:23:49 -0400 Subject: [PATCH 124/150] ADD: a bunch of stuff daniel said to push so i lwk did --- simgui-ds.json | 5 +++++ .../com/stuypulse/robot/RobotContainer.java | 8 ++++--- .../handoff/HandoffDefaultCommand.java | 2 +- .../robot/commands/intake/IntakeHoming.java | 11 ++++++++++ .../spindexer/SpindexerDefaultCommand.java | 2 +- .../stuypulse/robot/constants/Cameras.java | 4 ++-- .../com/stuypulse/robot/constants/Field.java | 5 +++-- .../stuypulse/robot/constants/Settings.java | 22 ++++++++++++++----- .../robot/subsystems/handoff/Handoff.java | 1 + .../robot/subsystems/handoff/HandoffImpl.java | 19 +++++++++++++++- .../robot/subsystems/handoff/HandoffSim.java | 6 +++++ .../robot/subsystems/intake/Intake.java | 1 + .../robot/subsystems/intake/IntakeImpl.java | 11 ++++++++-- .../subsystems/superstructure/hood/Hood.java | 4 ++-- .../superstructure/shooter/Shooter.java | 2 +- .../superstructure/turret/Turret.java | 2 +- .../superstructure/turret/TurretImpl.java | 6 ----- 17 files changed, 83 insertions(+), 28 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeHoming.java diff --git a/simgui-ds.json b/simgui-ds.json index f7c505bf..1f8961ec 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -4,6 +4,11 @@ "visible": true } }, + "System Joysticks": { + "window": { + "enabled": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 64311bff..cea1ab84 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -14,6 +14,7 @@ import com.stuypulse.robot.commands.auton.regular.RightOneCycle; import com.stuypulse.robot.commands.handoff.HandoffConditionalCommand; import com.stuypulse.robot.commands.handoff.HandoffDefaultCommand; +// import com.stuypulse.robot.commands.handoff.HandoffDefaultCommand; import com.stuypulse.robot.commands.handoff.HandoffReverse; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; @@ -27,6 +28,7 @@ import com.stuypulse.robot.commands.intake.ZeroPivotStowed; import com.stuypulse.robot.commands.spindexer.SpindexerConditionalCommand; import com.stuypulse.robot.commands.spindexer.SpindexerDefaultCommand; +// import com.stuypulse.robot.commands.spindexer.SpindexerDefaultCommand; import com.stuypulse.robot.commands.spindexer.SpindexerReverse; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; @@ -145,8 +147,8 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); - // spindexer.setDefaultCommand(new SpindexerDefaultCommand()); - // handoff.setDefaultCommand(new HandoffDefaultCommand()); + spindexer.setDefaultCommand(new SpindexerDefaultCommand()); + handoff.setDefaultCommand(new HandoffDefaultCommand()); } /***************/ @@ -195,7 +197,7 @@ private void configureButtonBindings() { .whileTrue(new SwerveXMode()) .onTrue(new IntakeRunRollers()) .whileTrue(new SuperstructureFerry() - .alongWith(new WaitUntilCommand(() -> superstructure.atTolerance() && superstructure.getState() == SuperstructureState.INTERPOLATION)) + .alongWith(new WaitUntilCommand(() -> superstructure.atTolerance() && superstructure.getState() == SuperstructureState.FERRY)) .andThen(new HandoffRun()) .alongWith(new WaitUntilCommand(() -> handoff.atTolerance() && handoff.getState() == HandoffState.FORWARD)) .andThen(new SpindexerRun())) diff --git a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffDefaultCommand.java index 8af8476a..776d2abe 100644 --- a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffDefaultCommand.java @@ -41,7 +41,7 @@ public void execute() { if (!shouldRun) { handoff.setState(HandoffState.STOP); - } else if (shouldRun && superstructure.getState() == SuperstructureState.SOTM) { + } else if (shouldRun && (superstructure.getState() == SuperstructureState.SOTM || superstructure.getState() == SuperstructureState.FOTM)) { handoff.setState(HandoffState.FORWARD); } } diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeHoming.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeHoming.java new file mode 100644 index 00000000..f1ccac36 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeHoming.java @@ -0,0 +1,11 @@ +package com.stuypulse.robot.commands.intake; + +import com.stuypulse.robot.subsystems.intake.Intake.PivotState; + +public class IntakeHoming extends IntakeSetState { + + public IntakeHoming() { //TODO: ensure this works/overrides the Intake Deploy w/o conflicts + super(PivotState.HOMING); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDefaultCommand.java index 03386244..01cb203d 100644 --- a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDefaultCommand.java @@ -43,7 +43,7 @@ public void execute() { if (!shouldRun) { spindexer.setState(SpindexerState.STOP); - } else if (shouldRun && superstructure.getState() == SuperstructureState.SOTM) { + } else if (shouldRun && (superstructure.getState() == SuperstructureState.SOTM || superstructure.getState() == SuperstructureState.FOTM)) { spindexer.setState(SpindexerState.FORWARD); } } diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 2fb33454..261f64a9 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -19,12 +19,12 @@ public interface Cameras { public Camera[] LimelightCameras = { new Camera("limelight-right", //.381, .2325, .2069592 - new Pose3d(Units.inchesToMeters(-14.962), Units.inchesToMeters(9.164), Units.inchesToMeters(8.118), //TODO: check this offset + new Pose3d(Units.inchesToMeters( -9.164), Units.inchesToMeters(14.962), Units.inchesToMeters(8.118), //TODO: check this offset // new Pose3d(0.0,0.0,0.0, new Rotation3d(Units.degreesToRadians(180), Units.degreesToRadians(31.08), Units.degreesToRadians(-80))), RobotContainer.EnabledSubsystems.RIGHT_LIMELIGHT), new Camera("limelight-left", - new Pose3d(Units.inchesToMeters(2.490), Units.inchesToMeters(14.8620), Units.inchesToMeters(7.920157), + new Pose3d(Units.inchesToMeters(2.490), Units.inchesToMeters(-14.8620), Units.inchesToMeters(7.920157), // new Pose3d(0.0,0.0,0.0, new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15.0), Units.degreesToRadians(71.0))), //289 RobotContainer.EnabledSubsystems.LEFT_LIMELIGHT), diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 217cc233..9706a57d 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -15,6 +15,7 @@ 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.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; @@ -66,8 +67,8 @@ public static boolean closerToTop() { ); public final Pose2d rightFerryZone = new Pose2d( - Units.inchesToMeters(31.5), - Units.inchesToMeters(34.5), + Units.inchesToMeters(20.75), + Units.inchesToMeters(76), new Rotation2d() ); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 80cb650c..0041d139 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -38,6 +38,8 @@ public interface Handoff { double HANDOFF_REVERSE = -500.0; double RPM_TOLERANCE = 200.0; SmartNumber HANDOFF_RPM = new SmartNumber("Handoff/Target RPM", HANDOFF_MAX); + + SmartNumber HANDOFF_STALL_CURRENT = new SmartNumber("Handoff/Stall Current Limit for Reverse", 30.0); } public interface Intake { @@ -51,7 +53,8 @@ public interface Intake { Rotation2d THRESHOLD_TO_START_ROLLERS = Rotation2d.fromDegrees(10.0); - Rotation2d ARBITRARY_VOLTAGE_THRESHOLD = Rotation2d.fromDegrees(15.0); + Rotation2d ANGLE_THRESHOLD_FOR_HOLDING_VOLTAGE = Rotation2d.fromDegrees(15.0); + double HOMING_VOLTAGE = 3.0; double PUSHDOWN_VOLTAGE = 3.0; @@ -76,8 +79,8 @@ public interface Superstructure { public final double SHOOTER_TOLERANCE_RPM = 100.0; public final Rotation2d HOOD_TOLERANCE = Rotation2d.fromDegrees(0.5); - public final double SHOOTER_SOTM_TOLERANCE_RPM = 150.0; - public final Rotation2d HOOD_SOTM_TOLERANCE = Rotation2d.fromDegrees(2.0); + public final double SHOOTER_SOTM_TOLERANCE_RPM = 350.0; + public final Rotation2d HOOD_SOTM_TOLERANCE = Rotation2d.fromDegrees(3.0); public interface AngleInterpolation { double[][] distanceAngleInterpolationValues = { @@ -114,7 +117,14 @@ public interface FerryRPMInterpolation { {5.16, 3500.0}, {6.94, 3700.0}, {7.87, 4000.0}, - {9.77, 4500.0} + {9.77, 4500.0}, + {10.694, 4795.0}, //STARTING FROM HERE THE DATA IS UNRELIABLE!!! + {11.516, 5000.0}, + {12.416, 5295.0}, + {13.316, 5500.0}, + {14.216, 5795.0}, + {15.148, 6000.0}, + {16.54, 6300} //FIELD LENGTH }; } @@ -169,7 +179,7 @@ public interface Hood { public final double STALL_DEBOUNCE = 0.5; public interface Angles { - public final SmartNumber SHOOT = new SmartNumber("InterpolationTesting/Shoot State Target Angle (deg)", 15.0); + public final SmartNumber SHOOT = new SmartNumber("InterpolationTesting/Shoot State Target Angle (deg)", 20.0); public final SmartNumber FERRY = new SmartNumber("InterpolationTesting/Ferry State Target Angle (deg)", 20.0); public final Rotation2d MIN = Rotation2d.fromDegrees(20.0); @@ -186,7 +196,7 @@ 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 Rotation2d TOLERANCE = Rotation2d.fromDegrees(2.0); - public final Rotation2d SOTM_TOLERANCE = Rotation2d.fromDegrees(4.0); + public final Rotation2d SOTM_TOLERANCE = Rotation2d.fromDegrees(5.0); public final Rotation2d KB = Rotation2d.fromDegrees(0.0); public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(0.0); diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java index d6abba2d..600c1333 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java @@ -65,6 +65,7 @@ public boolean atTolerance() { public abstract SysIdRoutine getSysIdRoutine(); public abstract void setVoltageOverride(Optional voltage); + public abstract boolean isHandoffStalling(); @Override public void periodic() { 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 74ee0359..49946b48 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -11,6 +11,8 @@ import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.SysId; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -28,6 +30,7 @@ public class HandoffImpl extends Handoff { private final VelocityVoltage controller; private Optional voltageOverride; + private BStream isStalling; public HandoffImpl() { handoffConfig = new Motors.TalonFXConfig() @@ -48,6 +51,14 @@ public HandoffImpl() { controller = new VelocityVoltage(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE).withEnableFOC(true); voltageOverride = Optional.empty(); + + isStalling = BStream.create(() -> motor.getSupplyCurrent().getValueAsDouble() > Settings.Handoff.HANDOFF_STALL_CURRENT.getAsDouble()) + .filtered(new BDebounce.Both(0.5)); + } + + @Override + public boolean isHandoffStalling() { + return isStalling.get(); } public double getCurrentRPM() { @@ -61,7 +72,13 @@ public void periodic() { if (EnabledSubsystems.HANDOFF.get() && getState() != HandoffState.STOP) { if (voltageOverride.isPresent()) { motor.setVoltage(voltageOverride.get()); - } else { + } + // else if (isHandoffStalling()) { //TODO: debug logic + // setState(HandoffState.REVERSE); + // motor.setControl(controller.withVelocity(getTargetRPM() / 60.0).withEnableFOC(true)); + + // } + else { motor.setControl(controller.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE)); } } else { diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java index 59a734f4..d5d3c71f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java @@ -101,4 +101,10 @@ public SysIdRoutine getSysIdRoutine() { () -> sim.getInput(0), getInstance()); } + + @Override + public boolean isHandoffStalling() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'isHandoffStalling'"); + } } \ 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 e80bdbe1..9c0a6cad 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -40,6 +40,7 @@ protected Intake() { public enum PivotState { DEPLOY(Settings.Intake.PIVOT_DEPLOY_ANGLE), + HOMING(Settings.Intake.PIVOT_DEPLOY_ANGLE), STOW(Settings.Intake.PIVOT_STOW_ANGLE); private final Rotation2d targetAngle; 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 ad793a87..b55a54c3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -132,9 +132,12 @@ public void periodic() { pivot.setVoltage(pivotVoltageOverride.get()); } else { // PIVOT - if (pivotState == PivotState.DEPLOY && getPivotAngle().getDegrees() <= Settings.Intake.ARBITRARY_VOLTAGE_THRESHOLD.getDegrees()) { + if (pivotState == PivotState.DEPLOY && getPivotAngle().getDegrees() <= Settings.Intake.ANGLE_THRESHOLD_FOR_HOLDING_VOLTAGE.getDegrees()) { pivot.setControl(new VoltageOut(-Settings.Intake.PUSHDOWN_VOLTAGE)); // applying 3 volts - } else { + } else if (pivotState == PivotState.HOMING) { + pivot.setControl(new VoltageOut(-Settings.Intake.HOMING_VOLTAGE)); + } + else { pivot.setControl(new PositionVoltage(pivotState.getTargetAngle().getRotations())); } @@ -146,6 +149,10 @@ public void periodic() { } rollerFollower.setControl(follower); } + + if (pivotState == PivotState.HOMING && pivotStalling()) { + zeroPivotDeployed(); + } } else { pivot.stopMotor(); rollerLeader.stopMotor(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java index 177573fd..d355a0a0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java @@ -72,7 +72,7 @@ public Rotation2d getTargetAngle() { return switch(state) { case STOW -> Settings.Superstructure.Hood.Angles.STOW; - case FERRY -> Rotation2d.fromDegrees(30); + case FERRY -> Rotation2d.fromDegrees(39); case SHOOT -> Rotation2d.fromDegrees(Settings.Superstructure.Hood.Angles.SHOOT.get()); case KB -> Settings.Superstructure.Hood.Angles.KB; case LEFT_CORNER -> Settings.Superstructure.Hood.Angles.LEFT_CORNER; @@ -89,7 +89,7 @@ public Rotation2d getTargetAngle() { public boolean atTolerance() { double error = getAngle().minus(getTargetAngle()).getRotations(); if (Robot.isReal()) { - if (state == HoodState.SOTM) { + if (state == HoodState.SOTM || state == HoodState.FOTM) { return Math.abs(error) < Settings.Superstructure.HOOD_SOTM_TOLERANCE.getRotations(); } else { return Math.abs(error) < Settings.Superstructure.HOOD_TOLERANCE.getRotations(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index fa46ba23..3ead1687 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java @@ -78,7 +78,7 @@ public double getShootRPM() { public boolean atTolerance() { double error = Math.abs(getTargetRPM() - getRPM()); - if (state == ShooterState.SOTM) { + if (state == ShooterState.SOTM || state == ShooterState.FOTM) { return error < Settings.Superstructure.SHOOTER_SOTM_TOLERANCE_RPM; } else { return error < Settings.Superstructure.SHOOTER_TOLERANCE_RPM; diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java index 64133dad..39597a2f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java @@ -82,7 +82,7 @@ public Rotation2d driverInputToAngle() { public boolean atTolerance() { double error = getAngle().minus(getTargetAngle()).getRotations(); - if (state == TurretState.SOTM) { + if (state == TurretState.SOTM || state == TurretState.FOTM) { return Math.abs(error) < Settings.Superstructure.Turret.SOTM_TOLERANCE.getRotations(); } else { return Math.abs(error) < Settings.Superstructure.Turret.TOLERANCE.getRotations(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index c6c27434..816721dc 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -133,12 +133,6 @@ public Rotation2d getAngle() { return Rotation2d.fromRotations(turretMotor.getPosition().getValueAsDouble()); } - @Override - public boolean atTolerance() { - double error = getAngle().minus(getTargetAngle()).getRotations(); - return Math.abs(error) < Settings.Superstructure.Turret.TOLERANCE.getRotations(); - } - private double getDelta(double target, double current) { double delta = (target - current) % 360; From 07acd0bf2cce0ed13cefe32b05aa7fe139a838ec Mon Sep 17 00:00:00 2001 From: DanTheMan95 <81121522+Danx3mer@users.noreply.github.com> Date: Mon, 9 Mar 2026 14:48:53 -0400 Subject: [PATCH 125/150] feat: option to override rpm via SmartDashboard --- .../java/com/stuypulse/robot/constants/Settings.java | 11 ++++++++++- .../robot/subsystems/superstructure/hood/Hood.java | 4 ++++ .../subsystems/superstructure/shooter/Shooter.java | 4 ++++ .../superstructure/shooter/ShooterImpl.java | 8 ++------ 4 files changed, 20 insertions(+), 7 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 0041d139..9febac38 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -7,6 +7,7 @@ import com.ctre.phoenix6.CANBus; import com.pathplanner.lib.path.PathConstraints; +import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.network.SmartNumber; import edu.wpi.first.math.VecBuilder; @@ -138,12 +139,17 @@ public interface FerryTOFInterpolation { } public interface Shooter { + public final double GEAR_RATIO = 1.0; public final double FLYWHEEL_RADIUS = Units.inchesToMeters(3.965 / 2); - + public interface RPM { + public final SmartBoolean OVERRIDEN = new SmartBoolean("Superstructure/Shooter/RPM Override enabled?", false); + public final SmartNumber OVERRIDE_VALUE = new SmartNumber("Superstructure/Shooter/RPM Override value", 0.0); + public final SmartNumber SHOOT = new SmartNumber("InterpolationTesting/Shoot State Target RPM", 3500.0); public final SmartNumber FERRY = new SmartNumber("InterpolationTesting/Ferry State Target RPM", 2000.0); + public final double REVERSE = 0.0; public final double KB = 0.0; public final double LEFT_CORNER = 0.0; @@ -179,6 +185,9 @@ public interface Hood { public final double STALL_DEBOUNCE = 0.5; public interface Angles { + public final SmartBoolean OVERRIDEN = new SmartBoolean("Superstructure/Hood/Angle Override enabled?", false); + public final SmartNumber OVERRIDE_VALUE_DEG = new SmartNumber("Superstructure/Hood/Angle Override value", 0.0); + public final SmartNumber SHOOT = new SmartNumber("InterpolationTesting/Shoot State Target Angle (deg)", 20.0); public final SmartNumber FERRY = new SmartNumber("InterpolationTesting/Ferry State Target Angle (deg)", 20.0); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java index d355a0a0..cc7b1d36 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java @@ -69,6 +69,10 @@ public Rotation2d getTargetAngle() { if (CommandSwerveDrivetrain.getInstance().isUnderTrench()) { return Settings.Superstructure.Hood.Angles.STOW; } + + if(Settings.Superstructure.Hood.Angles.OVERRIDEN.get()) { + return Rotation2d.fromDegrees(Settings.Superstructure.Hood.Angles.OVERRIDE_VALUE_DEG.get()); + } return switch(state) { case STOW -> Settings.Superstructure.Hood.Angles.STOW; diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index 3ead1687..30d09035 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java @@ -57,6 +57,10 @@ public ShooterState getState() { } public double getTargetRPM() { + if(Settings.Superstructure.Shooter.RPM.OVERRIDEN.get()) { + return Settings.Superstructure.Shooter.RPM.OVERRIDE_VALUE.get(); + } + return switch(state) { case STOP -> 0; case SHOOT -> getShootRPM(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java index 20bc231a..0c274dc7 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -104,15 +104,11 @@ public void periodic() { Gains.Superstructure.Shooter.kA ); - if (EnabledSubsystems.SHOOTER.get()) { - if (getState() == ShooterState.STOP) { - shooterLeader.stopMotor(); - } else if (voltageOverride.isPresent()) { + if (EnabledSubsystems.SHOOTER.get() || getState() == ShooterState.STOP) { + if (voltageOverride.isPresent()) { shooterLeader.setVoltage(voltageOverride.get()); - shooterFollower.setControl(follower); } else { shooterLeader.setControl(shooterController.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE)); - shooterFollower.setControl(follower); } } else { shooterLeader.stopMotor(); From 39fc38e743af314f686cfcc96155fd30b2e917fd Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Mon, 9 Mar 2026 15:10:45 -0400 Subject: [PATCH 126/150] FEAT: working on accel limit for FOTM/SOTM --- .../commands/swerve/SwerveDriveDrive.java | 32 +++++++++++++++---- .../stuypulse/robot/constants/Settings.java | 2 ++ 2 files changed, 28 insertions(+), 6 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java index 3f39723a..e42a3249 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java @@ -37,9 +37,16 @@ public class SwerveDriveDrive extends Command { private final VStream speed; private final IStream turn; + private final VRateLimit normalAcellLimit; + private final VRateLimit stomAcellLimit; + private final VRateLimit fotmAcellLimit; + public SwerveDriveDrive(Gamepad driver) { swerve = CommandSwerveDrivetrain.getInstance(); superstructure = Superstructure.getInstance(); + normalAcellLimit = new VRateLimit(Settings.Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED); + stomAcellLimit = new VRateLimit(Settings.Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED_STOM); + fotmAcellLimit = new VRateLimit(Settings.Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED_FOTM); speed = VStream.create(this::getDriverInputAsVelocity) .filtered( @@ -47,7 +54,7 @@ public SwerveDriveDrive(Gamepad driver) { x -> x.clamp(1), x -> x.pow(Drive.POWER), x -> x.mul(Swerve.Constraints.MAX_VELOCITY_M_PER_S), - new VRateLimit(Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED), + normalAcellLimit, new VLowPassFilter(Drive.RC) ); @@ -70,24 +77,37 @@ private Vector2D getDriverInputAsVelocity() { @Override public void execute() { + SuperstructureState state = superstructure.getState(); + if (state == SuperstructureState.SOTM) { + speed.filtered(stomAcellLimit); + } else if (state == SuperstructureState.FOTM) { + speed.filtered(fotmAcellLimit); + } else { + speed.filtered(normalAcellLimit); + } Vector2D speedVector = speed.get(); double angularVel = turn.get(); - + //TODO: test this double maxAngularVel = Swerve.Constraints.MAX_ANGULAR_VEL_RAD_PER_S; if (speedVector.magnitude() > 0.05 && superstructure.getState() == SuperstructureState.SOTM) { speedVector = speedVector.normalize().mul(Settings.Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S); maxAngularVel = Settings.Swerve.Constraints.MAX_ANGULAR_VEL_SOTM_RAD_PER_S; + } else if (speedVector.magnitude() > 0.05 && superstructure.getState() == SuperstructureState.FOTM) { - speedVector = speedVector.normalize().mul(Settings.Swerve.Constraints.MAX_VELOCITY_FOTM_M_PER_S); - maxAngularVel = Settings.Swerve.Constraints.MAX_ANGULAR_VEL_FOTM_RAD_PER_S; + speedVector = speedVector.normalize().mul(Settings.Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S); + maxAngularVel = Settings.Swerve.Constraints.MAX_ANGULAR_VEL_SOTM_RAD_PER_S; + } angularVel = MathUtil.clamp(angularVel, -maxAngularVel, maxAngularVel); swerve.setControl(swerve.getFieldCentricSwerveRequest() - .withVelocityX(speedVector.x) - .withVelocityY(speedVector.y) + .withVelocityX(speed.get().x) + .withVelocityY(speed.get().y) .withRotationalRate(-angularVel)); + + SmartDashboard.putNumber("Swerve/Speed x", speed.get().x); + SmartDashboard.putNumber("Swerve/Speed y", speed.get().y); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 9febac38..49b2e75a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -269,6 +269,8 @@ public interface Constraints { public final double MAX_ANGULAR_VEL_FOTM_RAD_PER_S = Units.degreesToRadians(150.0); public final double MAX_ACCEL_M_PER_S_SQUARED = 15.0; + public final double MAX_ACCEL_M_PER_S_SQUARED_STOM = 15.0; + public final double MAX_ACCEL_M_PER_S_SQUARED_FOTM = 15.0; public final double MAX_ANGULAR_ACCEL_RAD_PER_S_SQUARED = Units.degreesToRadians(900.0); public final PathConstraints DEFAULT_CONSTRAINTS = From 59223969c47f292164f068445a072ac57e4962ef Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Mon, 9 Mar 2026 16:09:09 -0400 Subject: [PATCH 127/150] ADD: Dynamic RPM tolerance switching for SOTM --- simgui.json | 5 +++++ .../java/com/stuypulse/robot/constants/Settings.java | 6 +++++- .../stuypulse/robot/subsystems/handoff/Handoff.java | 11 +++++++++-- 3 files changed, 19 insertions(+), 3 deletions(-) diff --git a/simgui.json b/simgui.json index 16a565ca..3c0f9e96 100644 --- a/simgui.json +++ b/simgui.json @@ -21,6 +21,11 @@ "/SmartDashboard/Visualizers/Turret": "Mechanism2d" }, "windows": { + "/SmartDashboard/Autonomous": { + "window": { + "visible": true + } + }, "/SmartDashboard/Field": { "bottom": 1914, "height": 8.069275856018066, diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 9febac38..0a1ab232 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -38,6 +38,7 @@ public interface Handoff { double HANDOFF_MAX = 4800.0; double HANDOFF_REVERSE = -500.0; double RPM_TOLERANCE = 200.0; + double RPM_SOTM_TOLERANCE = 700.0; SmartNumber HANDOFF_RPM = new SmartNumber("Handoff/Target RPM", HANDOFF_MAX); SmartNumber HANDOFF_STALL_CURRENT = new SmartNumber("Handoff/Stall Current Limit for Reverse", 30.0); @@ -70,7 +71,8 @@ public interface Spindexer { double REVERSE_SPEED = -4500.0; double STOP_SPEED = 0.0; - double RPM_TOLERANCE = 400.0; + double RPM_TOLERANCE = 800.0; + /* CONSTANTS */ double GEAR_RATIO = 8.0 / 1.0; @@ -269,6 +271,8 @@ public interface Constraints { public final double MAX_ANGULAR_VEL_FOTM_RAD_PER_S = Units.degreesToRadians(150.0); public final double MAX_ACCEL_M_PER_S_SQUARED = 15.0; + public final double MAX_ACCEL_M_PER_S_SQUARED_STOM = 15.0; + public final double MAX_ACCEL_M_PER_S_SQUARED_FOTM = 15.0; public final double MAX_ANGULAR_ACCEL_RAD_PER_S_SQUARED = Units.degreesToRadians(900.0); public final PathConstraints DEFAULT_CONSTRAINTS = diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java index 600c1333..e3f02249 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java @@ -6,7 +6,10 @@ package com.stuypulse.robot.subsystems.handoff; import com.stuypulse.robot.Robot; +import com.stuypulse.robot.commands.superstructure.SuperstructureSetState; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -57,8 +60,12 @@ public void setState(HandoffState state) { } public boolean atTolerance() { - double diff = Math.abs(getTargetRPM() - getCurrentRPM()); - return diff < Settings.Handoff.RPM_TOLERANCE; + double error = Math.abs(getTargetRPM() - getCurrentRPM()); + SuperstructureState superstructureState = Superstructure.getInstance().getState(); + if (superstructureState == SuperstructureState.SOTM || superstructureState == SuperstructureState.FOTM) { + return error < Settings.Handoff.RPM_TOLERANCE; + } + return error < Settings.Handoff.RPM_TOLERANCE; } public abstract double getCurrentRPM(); From 98c200d1feedbd5a1a867494da8e638a840ec821 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Mon, 9 Mar 2026 16:46:41 -0400 Subject: [PATCH 128/150] FIX: fixed max vel and accel in pathplanner --- src/main/deploy/pathplanner/paths/Box 1.path | 6 ++--- src/main/deploy/pathplanner/paths/Box 2.path | 6 ++--- src/main/deploy/pathplanner/paths/Box 3.path | 6 ++--- src/main/deploy/pathplanner/paths/Box 4.path | 6 ++--- .../paths/Depot To Tower Left.path | 6 ++--- .../pathplanner/paths/Left Bump To Depot.path | 6 ++--- .../pathplanner/paths/Left NZ To Score.path | 8 +++---- .../paths/Left NZ To Tower Left.path | 8 +++---- .../paths/Left Score Score Tower.path | 10 ++++----- .../paths/Left Score To Score.path | 10 ++++----- .../Left Trench Score To Tower Left.path | 6 ++--- .../pathplanner/paths/Left Trench To NZ.path | 8 +++---- .../pathplanner/paths/Right NZ To Score.path | 8 +++---- .../paths/Right NZ To Tower Right.path | 14 ++++++------ .../paths/Right Score Score Tower.path | 12 +++++----- .../paths/Right Score To Score.path | 10 ++++----- .../Right Trench Score To Tower Right.path | 10 ++++----- .../pathplanner/paths/Right Trench To NZ.path | 10 ++++----- .../pathplanner/paths/Straight Line.path | 6 ++--- src/main/deploy/pathplanner/settings.json | 6 ++--- .../commands/auton/regular/LeftTwoCycle.java | 22 +++++++++---------- .../commands/auton/regular/RightTwoCycle.java | 21 +++++++++--------- .../com/stuypulse/robot/constants/Gains.java | 7 ++++-- 23 files changed, 107 insertions(+), 105 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Box 1.path b/src/main/deploy/pathplanner/paths/Box 1.path index fa4bd833..2887ab86 100644 --- a/src/main/deploy/pathplanner/paths/Box 1.path +++ b/src/main/deploy/pathplanner/paths/Box 1.path @@ -33,9 +33,9 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, + "maxVelocity": 4.93, + "maxAcceleration": 15.0, + "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/Box 2.path b/src/main/deploy/pathplanner/paths/Box 2.path index c539bd92..ee818335 100644 --- a/src/main/deploy/pathplanner/paths/Box 2.path +++ b/src/main/deploy/pathplanner/paths/Box 2.path @@ -33,9 +33,9 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, + "maxVelocity": 4.93, + "maxAcceleration": 15.0, + "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/Box 3.path b/src/main/deploy/pathplanner/paths/Box 3.path index 5d16b2e8..82070849 100644 --- a/src/main/deploy/pathplanner/paths/Box 3.path +++ b/src/main/deploy/pathplanner/paths/Box 3.path @@ -33,9 +33,9 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, + "maxVelocity": 4.93, + "maxAcceleration": 15.0, + "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/Box 4.path b/src/main/deploy/pathplanner/paths/Box 4.path index 69af045f..ff63e346 100644 --- a/src/main/deploy/pathplanner/paths/Box 4.path +++ b/src/main/deploy/pathplanner/paths/Box 4.path @@ -33,9 +33,9 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, + "maxVelocity": 4.93, + "maxAcceleration": 15.0, + "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/Depot To Tower Left.path b/src/main/deploy/pathplanner/paths/Depot To Tower Left.path index afda93a2..3b896d57 100644 --- a/src/main/deploy/pathplanner/paths/Depot To Tower Left.path +++ b/src/main/deploy/pathplanner/paths/Depot To Tower Left.path @@ -38,9 +38,9 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, + "maxVelocity": 4.93, + "maxAcceleration": 15.0, + "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/Left Bump To Depot.path b/src/main/deploy/pathplanner/paths/Left Bump To Depot.path index 9ed4afa7..6748ccf8 100644 --- a/src/main/deploy/pathplanner/paths/Left Bump To Depot.path +++ b/src/main/deploy/pathplanner/paths/Left Bump To Depot.path @@ -33,9 +33,9 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, + "maxVelocity": 4.93, + "maxAcceleration": 15.0, + "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 3aa7f9af..33a3edbd 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -33,9 +33,9 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, + "maxVelocity": 4.93, + "maxAcceleration": 15.0, + "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, "unlimited": true @@ -47,7 +47,7 @@ "reversed": false, "folder": "To Score", "idealStartingState": { - "velocity": 5.75, + "velocity": 4.93, "rotation": -90.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path b/src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path index 7627554b..b2377a9d 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path @@ -54,9 +54,9 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, + "maxVelocity": 4.93, + "maxAcceleration": 15.0, + "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, "unlimited": true @@ -68,7 +68,7 @@ "reversed": false, "folder": "To Tower", "idealStartingState": { - "velocity": 5.75, + "velocity": 4.93, "rotation": -90.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Left Score Score Tower.path b/src/main/deploy/pathplanner/paths/Left Score Score Tower.path index ce5994d6..7391671f 100644 --- a/src/main/deploy/pathplanner/paths/Left Score Score Tower.path +++ b/src/main/deploy/pathplanner/paths/Left Score Score Tower.path @@ -110,21 +110,21 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, + "maxVelocity": 4.93, + "maxAcceleration": 15.0, + "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, "unlimited": true }, "goalEndState": { - "velocity": 5.75, + "velocity": 0.0, "rotation": 0.0 }, "reversed": false, "folder": "To Tower", "idealStartingState": { - "velocity": 5.75, + "velocity": 4.93, "rotation": -90.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path index 673e46b3..b700551a 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -90,21 +90,21 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, + "maxVelocity": 4.93, + "maxAcceleration": 15.0, + "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, "unlimited": true }, "goalEndState": { - "velocity": 5.75, + "velocity": 0.0, "rotation": 90.0 }, "reversed": false, "folder": "To NZ", "idealStartingState": { - "velocity": 5.75, + "velocity": 0.0, "rotation": -90.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path b/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path index b3a53574..7e8b83d5 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path +++ b/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path @@ -38,9 +38,9 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, + "maxVelocity": 4.93, + "maxAcceleration": 15.0, + "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, "unlimited": true diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path index c8d423ab..52bcaaff 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -33,15 +33,15 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, + "maxVelocity": 4.93, + "maxAcceleration": 15.0, + "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, "unlimited": true }, "goalEndState": { - "velocity": 5.75, + "velocity": 4.93, "rotation": -90.0 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index aabe421e..2b325c07 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -33,9 +33,9 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, + "maxVelocity": 4.93, + "maxAcceleration": 15.0, + "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, "unlimited": true @@ -47,7 +47,7 @@ "reversed": false, "folder": "To Score", "idealStartingState": { - "velocity": 5.75, + "velocity": 4.93, "rotation": 90.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path b/src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path index 433b2592..8f9b222f 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 6.053606168446025, - "y": 0.4898457888493475 + "y": 0.48984578884934704 }, "isLocked": false, "linkedName": null @@ -25,7 +25,7 @@ }, "nextControl": { "x": 3.1632509361049994, - "y": 0.412579921032048 + "y": 0.4125799210320481 }, "isLocked": false, "linkedName": null @@ -54,21 +54,21 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, + "maxVelocity": 4.93, + "maxAcceleration": 15.0, + "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, "unlimited": true }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "reversed": false, "folder": "To Tower", "idealStartingState": { - "velocity": 5.75, + "velocity": 4.93, "rotation": 90.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Right Score Score Tower.path b/src/main/deploy/pathplanner/paths/Right Score Score Tower.path index 6ad8008c..200a1482 100644 --- a/src/main/deploy/pathplanner/paths/Right Score Score Tower.path +++ b/src/main/deploy/pathplanner/paths/Right Score Score Tower.path @@ -52,7 +52,7 @@ "y": 0.6512336892052202 }, "prevControl": { - "x": 7.013067438601092, + "x": 7.013067438601093, "y": 0.6691872138236445 }, "nextControl": { @@ -110,16 +110,16 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, + "maxVelocity": 4.93, + "maxAcceleration": 15.0, + "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, "unlimited": true }, "goalEndState": { - "velocity": 5.75, - "rotation": 180.0 + "velocity": 4.93, + "rotation": 0.0 }, "reversed": false, "folder": "To Tower", diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index 9ff8fb78..30dbaf24 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -53,7 +53,7 @@ }, "prevControl": { "x": 7.118173588922177, - "y": 1.6648785526089203 + "y": 1.6648785526089207 }, "nextControl": { "x": 7.172562277580072, @@ -90,15 +90,15 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, + "maxVelocity": 4.93, + "maxAcceleration": 15.0, + "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, "unlimited": true }, "goalEndState": { - "velocity": 5.75, + "velocity": 0.0, "rotation": -90.0 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path b/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path index 25a9b4e4..793f7b1a 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path +++ b/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 1.4104440475220295, + "x": 1.41044404752203, "y": 0.712825661260673 }, "isLocked": false, @@ -38,16 +38,16 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, + "maxVelocity": 4.93, + "maxAcceleration": 15.0, + "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, "unlimited": true }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "reversed": false, "folder": "To Tower", diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index 84afa546..1e141bb1 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -21,7 +21,7 @@ }, "prevControl": { "x": 8.205444839857652, - "y": 0.37149466192170744 + "y": 0.371494661921707 }, "nextControl": null, "isLocked": false, @@ -33,15 +33,15 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, + "maxVelocity": 4.93, + "maxAcceleration": 15.0, + "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, "unlimited": true }, "goalEndState": { - "velocity": 5.75, + "velocity": 4.93, "rotation": 90.0 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Straight Line.path b/src/main/deploy/pathplanner/paths/Straight Line.path index bb367655..ad031ac1 100644 --- a/src/main/deploy/pathplanner/paths/Straight Line.path +++ b/src/main/deploy/pathplanner/paths/Straight Line.path @@ -33,9 +33,9 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.75, - "maxAcceleration": 6.75, - "maxAngularVelocity": 400.0, + "maxVelocity": 4.93, + "maxAcceleration": 15.0, + "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 818357d9..f5934b40 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -12,9 +12,9 @@ "autoFolders": [ "Tests" ], - "defaultMaxVel": 5.75, - "defaultMaxAccel": 6.75, - "defaultMaxAngVel": 400.0, + "defaultMaxVel": 4.93, + "defaultMaxAccel": 15.0, + "defaultMaxAngVel": 300.0, "defaultMaxAngAccel": 900.0, "defaultNominalVoltage": 12.0, "robotMass": 64.86, diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java index 19159e99..1d86c4be 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java @@ -8,6 +8,8 @@ import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; +import com.stuypulse.robot.subsystems.handoff.Handoff; +import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -28,16 +30,12 @@ public LeftTwoCycle(PathPlannerPath... paths) { new SuperstructureInterpolation(), // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new IntakeStow() - ), - new ParallelCommandGroup( - new WaitUntilCommand(() -> Superstructure.getInstance().isShooterAtTolerance()), - new WaitUntilCommand(() -> Superstructure.getInstance().isHoodAtTolerance()) - ), - new SpindexerRun().alongWith( - new HandoffRun() - ).withTimeout(5.0), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new HandoffRun().alongWith(new WaitUntilCommand(() -> Handoff.getInstance().atTolerance())).andThen( + new SpindexerRun() + ).withTimeout(10.0), + // NZ Trip 2 new ParallelCommandGroup( @@ -50,8 +48,8 @@ public LeftTwoCycle(PathPlannerPath... paths) { new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) // new SwerveClimbAlign() ), - new SpindexerRun().alongWith( - new HandoffRun() + new HandoffRun().alongWith(new WaitUntilCommand(() -> Handoff.getInstance().atTolerance())).andThen( + new SpindexerRun() ) // .until(() -> DriverStation.getMatchTime() < 2).andThen( // new ParallelCommandGroup( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java index 1f284a8f..b603f0b7 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java @@ -8,6 +8,8 @@ import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; +import com.stuypulse.robot.subsystems.handoff.Handoff; +import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -28,13 +30,12 @@ public RightTwoCycle(PathPlannerPath... paths) { new SuperstructureInterpolation(), // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new IntakeStow() - ), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new SpindexerRun().alongWith( - new HandoffRun() - ).withTimeout(5.0), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new HandoffRun().alongWith(new WaitUntilCommand(() -> Handoff.getInstance().atTolerance())).andThen( + new SpindexerRun() + ).withTimeout(10.0), + // NZ Trip 2 new ParallelCommandGroup( @@ -47,8 +48,8 @@ public RightTwoCycle(PathPlannerPath... paths) { new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) // new SwerveClimbAlign() ), - new SpindexerRun().alongWith( - new HandoffRun() + new HandoffRun().alongWith(new WaitUntilCommand(() -> Handoff.getInstance().atTolerance())).andThen( + new SpindexerRun() ) // .until(() -> DriverStation.getMatchTime() < 2).andThen( // new ParallelCommandGroup( @@ -57,7 +58,7 @@ public RightTwoCycle(PathPlannerPath... paths) { // new ClimberDown() // ) // ) - + ); } diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 396bc1bf..d058142e 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -163,8 +163,11 @@ public interface Rotation { double akI = 0.0; double akD = 0.0; - PIDConstants XY = new PIDConstants(3.0, 0.0, 0.2); - PIDConstants THETA = new PIDConstants(13.0, 0.0, 0.5); + PIDConstants XY = new PIDConstants(6.5, 0, 0.2); + PIDConstants THETA = new PIDConstants(15.0, 0, 0.2); + + // PIDConstants XY = new PIDConstants(3.0, 0.0, 0.2); + // PIDConstants THETA = new PIDConstants(13.0, 0.0, 0.5); } } } From d4e8309b2ee06a152e11566152a15883d8bdd8aa Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Mon, 9 Mar 2026 16:50:15 -0400 Subject: [PATCH 129/150] FEAT: added spindexer tolerance and state on elastic --- elastic-layout.json | 48 +++++++++++++++++++++++++++++---------------- 1 file changed, 31 insertions(+), 17 deletions(-) diff --git a/elastic-layout.json b/elastic-layout.json index c463037c..182f36ed 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -852,23 +852,6 @@ "false_icon": "None" } }, - { - "title": "Superstructure At Tol?", - "x": 840.0, - "y": 350.0, - "width": 280.0, - "height": 70.0, - "type": "Boolean Box", - "properties": { - "topic": "/SmartDashboard/Superstructure/Everything At Tolerance?", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, { "title": "Handoff Reverse", "x": 1120.0, @@ -910,6 +893,37 @@ "show_type": false, "maximize_button_space": true } + }, + { + "title": "Spin At Tol?", + "x": 980.0, + "y": 350.0, + "width": 140.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Spindexer/At Tolerance", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Spin State", + "x": 840.0, + "y": 350.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Spindexer/State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } } ] } From 48a50cc6d2047dfb0548b52c7bb8fab478d46d89 Mon Sep 17 00:00:00 2001 From: Apetrock24 Date: Mon, 9 Mar 2026 17:05:21 -0400 Subject: [PATCH 130/150] ADD: swerve drive acceleration limiting commands for SOTM and FOTM --- .../com/stuypulse/robot/RobotContainer.java | 8 +- .../commands/swerve/SwerveDriveFOTM.java | 111 +++++++++++++++++ .../commands/swerve/SwerveDriveSOTM.java | 113 ++++++++++++++++++ .../stuypulse/robot/constants/Settings.java | 4 +- .../swerve/CommandSwerveDrivetrain.java | 1 - 5 files changed, 231 insertions(+), 6 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index cea1ab84..ebf76e38 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -39,6 +39,8 @@ import com.stuypulse.robot.commands.superstructure.SuperstructureShoot; import com.stuypulse.robot.commands.superstructure.SuperstructureStow; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; +import com.stuypulse.robot.commands.swerve.SwerveDriveFOTM; +import com.stuypulse.robot.commands.swerve.SwerveDriveSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveWheelRadiusCharacterization; import com.stuypulse.robot.commands.swerve.SwerveXMode; @@ -208,11 +210,11 @@ private void configureButtonBindings() { .onTrue(new IntakeRunRollers()) .onTrue(new ConditionalCommand( new ParallelCommandGroup( - new SuperstructureInterpolation(), + new SuperstructureInterpolation(), new SpindexerStop(), new HandoffStop() ), - new SuperstructureSOTM(), + new SuperstructureSOTM().alongWith(new SwerveDriveSOTM(driver)), () -> superstructure.getState() == SuperstructureState.SOTM )); @@ -225,7 +227,7 @@ private void configureButtonBindings() { new SpindexerStop(), new HandoffStop() ), - new SuperstructureFOTM(), + new SuperstructureFOTM().alongWith(new SwerveDriveFOTM(driver)), () -> superstructure.getState() == SuperstructureState.FOTM )); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java new file mode 100644 index 00000000..7ddb4ce2 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java @@ -0,0 +1,111 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; +import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.math.Vector2D; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; +import com.stuypulse.stuylib.streams.vectors.VStream; +import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; +import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; +import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveFOTM extends Command{ + + private final CommandSwerveDrivetrain swerve; + private final Superstructure superstructure; + + private final Gamepad driver; + + private final VStream speed; + private final IStream turn; + + private final VRateLimit fotmAcellLimit; + private double previous; + private double current; + + public SwerveDriveFOTM(Gamepad driver) { + swerve = CommandSwerveDrivetrain.getInstance(); + superstructure = Superstructure.getInstance(); + fotmAcellLimit = new VRateLimit(Settings.Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED_FOTM); + + previous = 0.0; + current = 0.0; + + speed = VStream.create(this::getDriverInputAsVelocity) + .filtered( + new VDeadZone(Drive.DEADBAND), + x -> x.clamp(1), + x -> x.pow(Drive.POWER), + x -> x.mul(Swerve.Constraints.MAX_VELOCITY_M_PER_S), + fotmAcellLimit, + new VLowPassFilter(Drive.RC) + ); + + turn = IStream.create(driver::getRightX) + .filtered( + x -> SLMath.deadband(x, Turn.DEADBAND), + x -> SLMath.spow(x, Turn.POWER), + x -> x * Swerve.Constraints.MAX_ANGULAR_VEL_RAD_PER_S, + new LowPassFilter(Turn.RC) + ); + + this.driver = driver; + + addRequirements(swerve); + } + + private Vector2D getDriverInputAsVelocity() { + return new Vector2D(driver.getLeftStick().y, -driver.getLeftStick().x); + } + + @Override + public void execute() { + Vector2D speedVector = speed.get(); + double angularVel = turn.get(); + double maxAngularVel = Swerve.Constraints.MAX_ANGULAR_VEL_RAD_PER_S; + previous = current; + current = speed.get().x; + + + if (speedVector.magnitude() > 0.05 && superstructure.getState() == SuperstructureState.SOTM) { + speedVector = speedVector.normalize().mul(Settings.Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S); + maxAngularVel = Settings.Swerve.Constraints.MAX_ANGULAR_VEL_SOTM_RAD_PER_S; + + } else if (speedVector.magnitude() > 0.05 && superstructure.getState() == SuperstructureState.FOTM) { + speedVector = speedVector.normalize().mul(Settings.Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S); + maxAngularVel = Settings.Swerve.Constraints.MAX_ANGULAR_VEL_SOTM_RAD_PER_S; + } + + angularVel = MathUtil.clamp(angularVel, -maxAngularVel, maxAngularVel); + + swerve.setControl(swerve.getFieldCentricSwerveRequest() + .withVelocityX(speed.get().x) + .withVelocityY(speed.get().y) + .withRotationalRate(-angularVel)); + + SmartDashboard.putNumber("Swerve/Speed x", speed.get().x); + SmartDashboard.putNumber("Swerve/Speed y", speed.get().y); + } + + @Override + public boolean isFinished() { + SuperstructureState state = superstructure.getState(); + if (state == SuperstructureState.FOTM) { + return false; + } else { + return true; + } + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java new file mode 100644 index 00000000..7fed8548 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java @@ -0,0 +1,113 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; +import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.math.Vector2D; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; +import com.stuypulse.stuylib.streams.vectors.VStream; +import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; +import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; +import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveSOTM extends Command { + + private final CommandSwerveDrivetrain swerve; + private final Superstructure superstructure; + + private final Gamepad driver; + + private final VStream speed; + private final IStream turn; + + private final VRateLimit stomAcellLimit; + private double previous; + private double current; + + public SwerveDriveSOTM(Gamepad driver) { + swerve = CommandSwerveDrivetrain.getInstance(); + superstructure = Superstructure.getInstance(); + stomAcellLimit = new VRateLimit(Settings.Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED_STOM); + + previous = 0.0; + current = 0.0; + + speed = VStream.create(this::getDriverInputAsVelocity) + .filtered( + new VDeadZone(Drive.DEADBAND), + x -> x.clamp(1), + x -> x.pow(Drive.POWER), + x -> x.mul(Swerve.Constraints.MAX_VELOCITY_M_PER_S), + stomAcellLimit, + new VLowPassFilter(Drive.RC) + ); + + turn = IStream.create(driver::getRightX) + .filtered( + x -> SLMath.deadband(x, Turn.DEADBAND), + x -> SLMath.spow(x, Turn.POWER), + x -> x * Swerve.Constraints.MAX_ANGULAR_VEL_RAD_PER_S, + new LowPassFilter(Turn.RC) + ); + + this.driver = driver; + + addRequirements(swerve); + } + + private Vector2D getDriverInputAsVelocity() { + return new Vector2D(driver.getLeftStick().y, -driver.getLeftStick().x); + } + + @Override + public void execute() { + + Vector2D speedVector = speed.get(); + double angularVel = turn.get(); + //TODO: test this + double maxAngularVel = Swerve.Constraints.MAX_ANGULAR_VEL_RAD_PER_S; + previous = current; + current = speed.get().x; + + + if (speedVector.magnitude() > 0.05 && superstructure.getState() == SuperstructureState.SOTM) { + speedVector = speedVector.normalize().mul(Settings.Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S); + maxAngularVel = Settings.Swerve.Constraints.MAX_ANGULAR_VEL_SOTM_RAD_PER_S; + + } else if (speedVector.magnitude() > 0.05 && superstructure.getState() == SuperstructureState.FOTM) { + speedVector = speedVector.normalize().mul(Settings.Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S); + maxAngularVel = Settings.Swerve.Constraints.MAX_ANGULAR_VEL_SOTM_RAD_PER_S; + } + + angularVel = MathUtil.clamp(angularVel, -maxAngularVel, maxAngularVel); + + swerve.setControl(swerve.getFieldCentricSwerveRequest() + .withVelocityX(speed.get().x) + .withVelocityY(speed.get().y) + .withRotationalRate(-angularVel)); + + SmartDashboard.putNumber("Swerve/Speed x", speed.get().x); + SmartDashboard.putNumber("Swerve/Speed y", speed.get().y); + } + + @Override + public boolean isFinished() { + SuperstructureState state = superstructure.getState(); + if (state == SuperstructureState.SOTM) { + return false; + } else { + return true; + } + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 49b2e75a..0af634b8 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -269,8 +269,8 @@ public interface Constraints { public final double MAX_ANGULAR_VEL_FOTM_RAD_PER_S = Units.degreesToRadians(150.0); public final double MAX_ACCEL_M_PER_S_SQUARED = 15.0; - public final double MAX_ACCEL_M_PER_S_SQUARED_STOM = 15.0; - public final double MAX_ACCEL_M_PER_S_SQUARED_FOTM = 15.0; + public final double MAX_ACCEL_M_PER_S_SQUARED_STOM = 4.0; //TODO: find this value + public final double MAX_ACCEL_M_PER_S_SQUARED_FOTM = 2.0; //TODO: find this value public final double MAX_ANGULAR_ACCEL_RAD_PER_S_SQUARED = Units.degreesToRadians(900.0); public final PathConstraints DEFAULT_CONSTRAINTS = 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 fdc2159a..d34f23d4 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -546,7 +546,6 @@ public void periodic() { SmartDashboard.putNumber("Swerve/Velocity Field Relative Y (m per s)", getFieldRelativeSpeeds().y); SmartDashboard.putNumber("Swerve/Angular Velocity (rad per s)", chassisSpeeds.omegaRadiansPerSecond); - SmartDashboard.putNumber("Swerve/Distance From Hub (meters)", Field.hubCenter.getTranslation().getDistance(getPose().getTranslation())); } } \ No newline at end of file From d3a7cd8b3cd75f6be47f786648a06474b795499d Mon Sep 17 00:00:00 2001 From: Apetrock24 Date: Mon, 9 Mar 2026 17:11:43 -0400 Subject: [PATCH 131/150] Clean: fix dead code --- .../commands/swerve/SwerveDriveFOTM.java | 30 +++---------------- .../commands/swerve/SwerveDriveSOTM.java | 30 ++----------------- 2 files changed, 7 insertions(+), 53 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java index 7ddb4ce2..4b8aa570 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java @@ -32,23 +32,18 @@ public class SwerveDriveFOTM extends Command{ private final IStream turn; private final VRateLimit fotmAcellLimit; - private double previous; - private double current; - + public SwerveDriveFOTM(Gamepad driver) { swerve = CommandSwerveDrivetrain.getInstance(); superstructure = Superstructure.getInstance(); fotmAcellLimit = new VRateLimit(Settings.Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED_FOTM); - - previous = 0.0; - current = 0.0; speed = VStream.create(this::getDriverInputAsVelocity) .filtered( new VDeadZone(Drive.DEADBAND), x -> x.clamp(1), x -> x.pow(Drive.POWER), - x -> x.mul(Swerve.Constraints.MAX_VELOCITY_M_PER_S), + x -> x.mul(Swerve.Constraints.MAX_VELOCITY_FOTM_M_PER_S), fotmAcellLimit, new VLowPassFilter(Drive.RC) ); @@ -57,7 +52,7 @@ public SwerveDriveFOTM(Gamepad driver) { .filtered( x -> SLMath.deadband(x, Turn.DEADBAND), x -> SLMath.spow(x, Turn.POWER), - x -> x * Swerve.Constraints.MAX_ANGULAR_VEL_RAD_PER_S, + x -> x * Swerve.Constraints.MAX_VELOCITY_FOTM_M_PER_S, new LowPassFilter(Turn.RC) ); @@ -72,28 +67,11 @@ private Vector2D getDriverInputAsVelocity() { @Override public void execute() { - Vector2D speedVector = speed.get(); - double angularVel = turn.get(); - double maxAngularVel = Swerve.Constraints.MAX_ANGULAR_VEL_RAD_PER_S; - previous = current; - current = speed.get().x; - - - if (speedVector.magnitude() > 0.05 && superstructure.getState() == SuperstructureState.SOTM) { - speedVector = speedVector.normalize().mul(Settings.Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S); - maxAngularVel = Settings.Swerve.Constraints.MAX_ANGULAR_VEL_SOTM_RAD_PER_S; - - } else if (speedVector.magnitude() > 0.05 && superstructure.getState() == SuperstructureState.FOTM) { - speedVector = speedVector.normalize().mul(Settings.Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S); - maxAngularVel = Settings.Swerve.Constraints.MAX_ANGULAR_VEL_SOTM_RAD_PER_S; - } - - angularVel = MathUtil.clamp(angularVel, -maxAngularVel, maxAngularVel); swerve.setControl(swerve.getFieldCentricSwerveRequest() .withVelocityX(speed.get().x) .withVelocityY(speed.get().y) - .withRotationalRate(-angularVel)); + .withRotationalRate(-turn.get())); SmartDashboard.putNumber("Swerve/Speed x", speed.get().x); SmartDashboard.putNumber("Swerve/Speed y", speed.get().y); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java index 7fed8548..3ad0244c 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java @@ -32,23 +32,18 @@ public class SwerveDriveSOTM extends Command { private final IStream turn; private final VRateLimit stomAcellLimit; - private double previous; - private double current; public SwerveDriveSOTM(Gamepad driver) { swerve = CommandSwerveDrivetrain.getInstance(); superstructure = Superstructure.getInstance(); stomAcellLimit = new VRateLimit(Settings.Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED_STOM); - previous = 0.0; - current = 0.0; - speed = VStream.create(this::getDriverInputAsVelocity) .filtered( new VDeadZone(Drive.DEADBAND), x -> x.clamp(1), x -> x.pow(Drive.POWER), - x -> x.mul(Swerve.Constraints.MAX_VELOCITY_M_PER_S), + x -> x.mul(Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S), stomAcellLimit, new VLowPassFilter(Drive.RC) ); @@ -57,7 +52,7 @@ public SwerveDriveSOTM(Gamepad driver) { .filtered( x -> SLMath.deadband(x, Turn.DEADBAND), x -> SLMath.spow(x, Turn.POWER), - x -> x * Swerve.Constraints.MAX_ANGULAR_VEL_RAD_PER_S, + x -> x * Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S, new LowPassFilter(Turn.RC) ); @@ -73,29 +68,10 @@ private Vector2D getDriverInputAsVelocity() { @Override public void execute() { - Vector2D speedVector = speed.get(); - double angularVel = turn.get(); - //TODO: test this - double maxAngularVel = Swerve.Constraints.MAX_ANGULAR_VEL_RAD_PER_S; - previous = current; - current = speed.get().x; - - - if (speedVector.magnitude() > 0.05 && superstructure.getState() == SuperstructureState.SOTM) { - speedVector = speedVector.normalize().mul(Settings.Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S); - maxAngularVel = Settings.Swerve.Constraints.MAX_ANGULAR_VEL_SOTM_RAD_PER_S; - - } else if (speedVector.magnitude() > 0.05 && superstructure.getState() == SuperstructureState.FOTM) { - speedVector = speedVector.normalize().mul(Settings.Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S); - maxAngularVel = Settings.Swerve.Constraints.MAX_ANGULAR_VEL_SOTM_RAD_PER_S; - } - - angularVel = MathUtil.clamp(angularVel, -maxAngularVel, maxAngularVel); - swerve.setControl(swerve.getFieldCentricSwerveRequest() .withVelocityX(speed.get().x) .withVelocityY(speed.get().y) - .withRotationalRate(-angularVel)); + .withRotationalRate(-turn.get())); SmartDashboard.putNumber("Swerve/Speed x", speed.get().x); SmartDashboard.putNumber("Swerve/Speed y", speed.get().y); From da7a5202806dec6ddb720f8895bee07ca0191971 Mon Sep 17 00:00:00 2001 From: Brian Zheng Date: Mon, 9 Mar 2026 17:30:26 -0400 Subject: [PATCH 132/150] FIX: SOTM / FOTM velocity cap --- .../commands/swerve/SwerveDriveDrive.java | 6 ++-- .../util/superstructure/SOTMCalculator.java | 28 ++++++++++++------- 2 files changed, 21 insertions(+), 13 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java index e42a3249..6823ced3 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java @@ -91,12 +91,12 @@ public void execute() { double maxAngularVel = Swerve.Constraints.MAX_ANGULAR_VEL_RAD_PER_S; if (speedVector.magnitude() > 0.05 && superstructure.getState() == SuperstructureState.SOTM) { - speedVector = speedVector.normalize().mul(Settings.Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S); + speedVector = speedVector.mul(Settings.Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S / Swerve.Constraints.MAX_VELOCITY_M_PER_S); maxAngularVel = Settings.Swerve.Constraints.MAX_ANGULAR_VEL_SOTM_RAD_PER_S; } else if (speedVector.magnitude() > 0.05 && superstructure.getState() == SuperstructureState.FOTM) { - speedVector = speedVector.normalize().mul(Settings.Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S); - maxAngularVel = Settings.Swerve.Constraints.MAX_ANGULAR_VEL_SOTM_RAD_PER_S; + speedVector = speedVector.mul(Settings.Swerve.Constraints.MAX_VELOCITY_FOTM_M_PER_S / Swerve.Constraints.MAX_VELOCITY_M_PER_S); + maxAngularVel = Settings.Swerve.Constraints.MAX_ANGULAR_VEL_FOTM_RAD_PER_S; } diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java index b9e77e82..ad3a613c 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -160,6 +161,7 @@ public static void updateSOTMSolution() { CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); + Pose2d turretPose = swerve.getTurretPose(); Pose2d robotPose = swerve.getPose(); Pose2d hubPose = Field.getHubPose(); Pose2d ferryPose = Field.getFerryZonePose(robotPose.getTranslation()); @@ -170,21 +172,27 @@ public static void updateSOTMSolution() { robotPose.getRotation() ); - Pose2d futureTurretPose = swerve.getTurretPose().exp( + Transform2d robotToTurret = turretPose.minus(robotPose); + + Pose2d futureTurretPose = robotPose.exp( new Twist2d( robotRelativeSpeeds.vxMetersPerSecond * Settings.Superstructure.SOTM.UPDATE_DELAY.doubleValue(), robotRelativeSpeeds.vyMetersPerSecond * Settings.Superstructure.SOTM.UPDATE_DELAY.doubleValue(), 0 ) - ); + ).transformBy(robotToTurret); - Vector2D oppositeDirection = Vector2D.kOrigin; + + Vector2D oppositeDirection = new Vector2D(new Translation2d( + -fieldRelativeSpeeds.vxMetersPerSecond, + -fieldRelativeSpeeds.vyMetersPerSecond + )); - if (Math.abs(robotRelativeSpeeds.vxMetersPerSecond) > Settings.Swerve.MODULE_VELOCITY_DEADBAND_M_PER_S && Math.abs(robotRelativeSpeeds.vyMetersPerSecond) > Settings.Swerve.MODULE_VELOCITY_DEADBAND_M_PER_S) { - oppositeDirection = new Vector2D(new Translation2d( - -robotRelativeSpeeds.vxMetersPerSecond, - -robotRelativeSpeeds.vyMetersPerSecond - )); + if (oppositeDirection.magnitude() < Settings.Swerve.MODULE_VELOCITY_DEADBAND_M_PER_S) { + oppositeDirection = Vector2D.kOrigin; + } + else { + oppositeDirection = oppositeDirection.normalize(); } hubPose = hubPose.exp( @@ -217,8 +225,8 @@ public static void updateSOTMSolution() { ferryPose2d.setPose(Robot.isBlue() ? ferryPose : Field.transformToOppositeAlliance(ferryPose)); virtualFerryPose2d.setPose((Robot.isBlue() ? ferrySol.virtualPose() : Field.transformToOppositeAlliance(ferrySol.virtualPose()))); - // hubPose2d.setPose(Robot.isBlue() ? hubPose : Field.transformToOppositeAlliance(hubPose)); - // virtualHubPose2d.setPose((Robot.isBlue() ? hubSol.virtualPose() : Field.transformToOppositeAlliance(hubSol.virtualPose()))); + hubPose2d.setPose(Robot.isBlue() ? hubPose : Field.transformToOppositeAlliance(hubPose)); + virtualHubPose2d.setPose((Robot.isBlue() ? hubSol.virtualPose() : Field.transformToOppositeAlliance(hubSol.virtualPose()))); futureTurretPose2d.setPose((Robot.isBlue() ? futureTurretPose : Field.transformToOppositeAlliance(futureTurretPose))); From a3bdf87c4174e54c82e7b654a24a44c15d4540e4 Mon Sep 17 00:00:00 2001 From: Material-Energy Date: Mon, 9 Mar 2026 17:42:58 -0400 Subject: [PATCH 133/150] FIX: Use ferry data instead for fotm --- .../superstructure/Superstructure.java | 4 +- .../superstructure/shooter/Shooter.java | 2 +- .../InterpolationCalculator.java | 39 ++++-- .../util/superstructure/SOTMCalculator.java | 117 ++++++++++++++---- 4 files changed, 132 insertions(+), 30 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index bb03be31..601cbf8e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -130,8 +130,10 @@ public boolean isTurretWrapping() { @Override public void periodic() { SuperstructureState state = getState(); - if (state == SuperstructureState.SOTM || state == SuperstructureState.FOTM) { + if (state == SuperstructureState.SOTM) { SOTMCalculator.updateSOTMSolution(); + } else if (state == SuperstructureState.FOTM){ + SOTMCalculator.updateFOTMSolution(); } if (CommandSwerveDrivetrain.getInstance().isOutsideAllianceZone() && state == SuperstructureState.SOTM) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index 30d09035..24030b71 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java @@ -64,7 +64,7 @@ public double getTargetRPM() { return switch(state) { case STOP -> 0; case SHOOT -> getShootRPM(); - case FERRY -> InterpolationCalculator.interpolateFerryingRPM(); + case FERRY -> InterpolationCalculator.interpolateFerryingInfo().targetRPM(); case REVERSE -> Settings.Superstructure.Shooter.RPM.REVERSE; case KB -> Settings.Superstructure.Shooter.RPM.KB; case LEFT_CORNER -> Settings.Superstructure.Shooter.RPM.LEFT_CORNER; diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java index b3960495..4b0dd585 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java @@ -1,6 +1,7 @@ package com.stuypulse.robot.util.superstructure; import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Superstructure.AngleInterpolation; import com.stuypulse.robot.constants.Settings.Superstructure.FerryRPMInterpolation; import com.stuypulse.robot.constants.Settings.Superstructure.RPMInterpolation; @@ -27,6 +28,12 @@ public record InterpolatedShotInfo( double flightTimeSeconds) { } + public record InterpolatedFerryInfo( + Rotation2d targetHoodAngle, + double targetRPM, + double flightTimeSeconds) { + } + static { distanceAngleInterpolator = new InterpolatingDoubleTreeMap(); @@ -78,20 +85,38 @@ public static InterpolatedShotInfo interpolateShotInfo(Pose2d turretPose, Pose2d flightTime ); } + - public static double interpolateFerryingRPM() { + public static InterpolatedFerryInfo interpolateFerryingInfo() { CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); + Pose2d turretPose = swerve.getTurretPose(); + + return interpolateFerryingInfo( + turretPose, + Field.getFerryZonePose(turretPose.getTranslation()) + ); + } - Translation2d currentPose = swerve.getTurretPose().getTranslation(); - Translation2d cornerPose = Field.getFerryZonePose(currentPose).getTranslation(); + public static InterpolatedFerryInfo interpolateFerryingInfo(Pose2d turretPose, Pose2d targetPose) { + + Translation2d currentPose = turretPose.getTranslation(); + Translation2d ferryPose = targetPose.getTranslation(); - double distanceMeters = cornerPose.getDistance(currentPose); + double distanceMeters = currentPose.getDistance(ferryPose); + Rotation2d targetAngle = Rotation2d.fromDegrees(Settings.Superstructure.Hood.Angles.FERRY.getAsDouble()); double targetRPM = ferryingDistanceRPMInterpolator.get(distanceMeters); - - SmartDashboard.putNumber("Superstructure/Interpolated Ferrying RPM", targetRPM); + double flightTime = 1.3; - return targetRPM; + SmartDashboard.putNumber("Superstructure/Interpolated Ferry Target Angle", targetAngle.getDegrees()); + SmartDashboard.putNumber("Superstructure/Interpolated Ferry RPM", targetRPM); + SmartDashboard.putNumber("Superstructure/Interpolated Ferry TOF", flightTime); + + return new InterpolatedFerryInfo( + targetAngle, + targetRPM, + flightTime + ); } diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java index ad3a613c..7245fdc8 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java @@ -7,6 +7,7 @@ import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.subsystems.superstructure.turret.Turret; +import com.stuypulse.robot.util.superstructure.InterpolationCalculator.InterpolatedFerryInfo; import com.stuypulse.robot.util.superstructure.InterpolationCalculator.InterpolatedShotInfo; import com.stuypulse.stuylib.math.Vector2D; @@ -23,8 +24,8 @@ public class SOTMCalculator { public static final double g = 9.81; - public static SOTMSolution hubSol; - public static SOTMSolution ferrySol; + public static MoveSolution hubSol; + public static MoveSolution ferrySol; private static FieldObject2d hubPose2d; private static FieldObject2d virtualHubPose2d; @@ -33,7 +34,7 @@ public class SOTMCalculator { private static FieldObject2d ferryPose2d; private static FieldObject2d virtualFerryPose2d; - public record SOTMSolution( + public record MoveSolution( Rotation2d targetHoodAngle, Rotation2d targetTurretAngle, double targetShooterRPM, @@ -42,7 +43,7 @@ public record SOTMSolution( } static { - hubSol = new SOTMSolution( + hubSol = new MoveSolution( Hood.getInstance().getAngle(), Turret.getInstance().getAngle(), Shooter.getInstance().getRPM(), @@ -50,7 +51,7 @@ public record SOTMSolution( 0.0 ); - ferrySol = new SOTMSolution( + ferrySol = new MoveSolution( Hood.getInstance().getAngle(), Turret.getInstance().getAngle(), Shooter.getInstance().getRPM(), @@ -68,7 +69,7 @@ public record SOTMSolution( } - public static SOTMSolution solveShootOnTheMove( + public static MoveSolution solveSOTM( Pose2d turretPose, Pose2d targetPose, ChassisSpeeds fieldRelativeSpeeds, @@ -148,7 +149,59 @@ but looking at the second equation, to get the flight time we need to solveBalli // Rotation2d.fromRadians(-yaw).plus(robotPose.getRotation()) : // Rotation2d.fromRadians(yaw).minus(robotPose.getRotation()); - return new SOTMSolution( + return new MoveSolution( + sol.targetHoodAngle(), + TurretAngleCalculator.getPointAtTargetAngle(virtualTranslation, turretTranslation), + sol.targetRPM(), + virtualPose, + sol.flightTimeSeconds() + ); + } + + public static MoveSolution solveFOTM( + Pose2d turretPose, + Pose2d targetPose, + ChassisSpeeds fieldRelativeSpeeds, + int maxIterations, + double timeTolerance) { + + InterpolatedFerryInfo sol = InterpolationCalculator.interpolateFerryingInfo(turretPose, targetPose); + + + double t_guess = sol.flightTimeSeconds(); + + Pose2d virtualPose = targetPose; + + + for (int i = 0; i < maxIterations; i++) { + + SmartDashboard.putNumber("Superstructure/SOTM/iteration #", i); + + double dx = fieldRelativeSpeeds.vxMetersPerSecond * t_guess; + double dy = fieldRelativeSpeeds.vyMetersPerSecond * t_guess; + + virtualPose = new Pose2d( + targetPose.getX() - dx, + targetPose.getY() - dy, + targetPose.getRotation()); + + + InterpolatedFerryInfo newSol = InterpolationCalculator.interpolateFerryingInfo(turretPose, virtualPose); + + if (Math.abs(newSol.flightTimeSeconds() - t_guess) < timeTolerance) { + break; + } + + t_guess = newSol.flightTimeSeconds(); + + sol = newSol; + + } + + Translation2d virtualTranslation = virtualPose.getTranslation(); + Translation2d turretTranslation = turretPose.getTranslation(); + + return new MoveSolution( sol.targetHoodAngle(), TurretAngleCalculator.getPointAtTargetAngle(virtualTranslation, turretTranslation), sol.targetRPM(), @@ -164,7 +217,6 @@ public static void updateSOTMSolution() { Pose2d turretPose = swerve.getTurretPose(); Pose2d robotPose = swerve.getPose(); Pose2d hubPose = Field.getHubPose(); - Pose2d ferryPose = Field.getFerryZonePose(robotPose.getTranslation()); ChassisSpeeds robotRelativeSpeeds = swerve.getChassisSpeeds(); ChassisSpeeds fieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds( @@ -203,7 +255,7 @@ public static void updateSOTMSolution() { ) ); - SOTMSolution hubSolution = solveShootOnTheMove( + hubSol = solveSOTM( futureTurretPose, hubPose, fieldRelativeSpeeds, @@ -211,7 +263,41 @@ public static void updateSOTMSolution() { Settings.Superstructure.SOTM.TIME_TOLERANCE ); - SOTMSolution ferrySolution = solveShootOnTheMove( + hubPose2d.setPose(Robot.isBlue() ? hubPose : Field.transformToOppositeAlliance(hubPose)); + virtualHubPose2d.setPose((Robot.isBlue() ? hubSol.virtualPose() : Field.transformToOppositeAlliance(hubSol.virtualPose()))); + futureTurretPose2d.setPose((Robot.isBlue() ? futureTurretPose : Field.transformToOppositeAlliance(futureTurretPose))); + + + SmartDashboard.putNumber("Superstructure/SOTM/calculated turret angle", hubSol.targetTurretAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/SOTM/calculated hood angle", hubSol.targetHoodAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/SOTM/calculated flight time", hubSol.flightTime()); + SmartDashboard.putNumber("Superstructure/SOTM/turret dist to virtual pose", futureTurretPose.getTranslation().getDistance(hubSol.virtualPose().getTranslation())); + } + + public static void updateFOTMSolution() { + CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); + + Pose2d turretPose = swerve.getTurretPose(); + Pose2d robotPose = swerve.getPose(); + Pose2d ferryPose = Field.getFerryZonePose(robotPose.getTranslation()); + + ChassisSpeeds robotRelativeSpeeds = swerve.getChassisSpeeds(); + ChassisSpeeds fieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds( + robotRelativeSpeeds, + robotPose.getRotation() + ); + + Transform2d robotToTurret = turretPose.minus(robotPose); + + Pose2d futureTurretPose = robotPose.exp( + new Twist2d( + robotRelativeSpeeds.vxMetersPerSecond * Settings.Superstructure.SOTM.UPDATE_DELAY.doubleValue(), + robotRelativeSpeeds.vyMetersPerSecond * Settings.Superstructure.SOTM.UPDATE_DELAY.doubleValue(), + 0 + ) + ).transformBy(robotToTurret); + + ferrySol = solveFOTM( futureTurretPose, ferryPose, fieldRelativeSpeeds, @@ -219,21 +305,10 @@ public static void updateSOTMSolution() { Settings.Superstructure.SOTM.TIME_TOLERANCE ); - hubSol = hubSolution; - ferrySol = ferrySolution; ferryPose2d.setPose(Robot.isBlue() ? ferryPose : Field.transformToOppositeAlliance(ferryPose)); virtualFerryPose2d.setPose((Robot.isBlue() ? ferrySol.virtualPose() : Field.transformToOppositeAlliance(ferrySol.virtualPose()))); - - hubPose2d.setPose(Robot.isBlue() ? hubPose : Field.transformToOppositeAlliance(hubPose)); - virtualHubPose2d.setPose((Robot.isBlue() ? hubSol.virtualPose() : Field.transformToOppositeAlliance(hubSol.virtualPose()))); futureTurretPose2d.setPose((Robot.isBlue() ? futureTurretPose : Field.transformToOppositeAlliance(futureTurretPose))); - - - SmartDashboard.putNumber("Superstructure/SOTM/calculated turret angle", hubSol.targetTurretAngle().getDegrees()); - SmartDashboard.putNumber("Superstructure/SOTM/calculated hood angle", hubSol.targetHoodAngle().getDegrees()); - SmartDashboard.putNumber("Superstructure/SOTM/calculated flight time", hubSol.flightTime()); - SmartDashboard.putNumber("Superstructure/SOTM/turret dist to virtual pose", futureTurretPose.getTranslation().getDistance(hubSol.virtualPose().getTranslation())); SmartDashboard.putNumber("Superstructure/FOTM/calculated turret angle", ferrySol.targetTurretAngle().getDegrees()); SmartDashboard.putNumber("Superstructure/FOTM/calculated hood angle", ferrySol.targetHoodAngle().getDegrees()); From cea5fba88b006ad06413cb43f2d4a5671a3e1cf4 Mon Sep 17 00:00:00 2001 From: Material-Energy Date: Mon, 9 Mar 2026 17:51:38 -0400 Subject: [PATCH 134/150] FIX: update the flight time to be more reasonable --- .../robot/util/superstructure/InterpolationCalculator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java index 4b0dd585..83a72148 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java @@ -106,7 +106,7 @@ public static InterpolatedFerryInfo interpolateFerryingInfo(Pose2d turretPose, P Rotation2d targetAngle = Rotation2d.fromDegrees(Settings.Superstructure.Hood.Angles.FERRY.getAsDouble()); double targetRPM = ferryingDistanceRPMInterpolator.get(distanceMeters); - double flightTime = 1.3; + double flightTime = 2.1; SmartDashboard.putNumber("Superstructure/Interpolated Ferry Target Angle", targetAngle.getDegrees()); SmartDashboard.putNumber("Superstructure/Interpolated Ferry RPM", targetRPM); From 24bc172de676d1f28311a58aef87638ddee42194 Mon Sep 17 00:00:00 2001 From: Brian Zheng Date: Mon, 9 Mar 2026 18:12:05 -0400 Subject: [PATCH 135/150] FIX: get rid of prev stuff in swervedrivedrive --- .../commands/swerve/SwerveDriveDrive.java | 47 ++----------------- .../commands/swerve/SwerveDriveFOTM.java | 6 +-- .../commands/swerve/SwerveDriveSOTM.java | 6 +-- .../stuypulse/robot/constants/Settings.java | 2 +- 4 files changed, 8 insertions(+), 53 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java index 6823ced3..33fd1e47 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java @@ -17,36 +17,22 @@ import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; -import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Swerve; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public class SwerveDriveDrive extends Command { private final CommandSwerveDrivetrain swerve; - private final Superstructure superstructure; private final Gamepad driver; private final VStream speed; private final IStream turn; - private final VRateLimit normalAcellLimit; - private final VRateLimit stomAcellLimit; - private final VRateLimit fotmAcellLimit; - public SwerveDriveDrive(Gamepad driver) { swerve = CommandSwerveDrivetrain.getInstance(); - superstructure = Superstructure.getInstance(); - normalAcellLimit = new VRateLimit(Settings.Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED); - stomAcellLimit = new VRateLimit(Settings.Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED_STOM); - fotmAcellLimit = new VRateLimit(Settings.Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED_FOTM); speed = VStream.create(this::getDriverInputAsVelocity) .filtered( @@ -54,7 +40,7 @@ public SwerveDriveDrive(Gamepad driver) { x -> x.clamp(1), x -> x.pow(Drive.POWER), x -> x.mul(Swerve.Constraints.MAX_VELOCITY_M_PER_S), - normalAcellLimit, + new VRateLimit(Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED), new VLowPassFilter(Drive.RC) ); @@ -77,37 +63,10 @@ private Vector2D getDriverInputAsVelocity() { @Override public void execute() { - SuperstructureState state = superstructure.getState(); - if (state == SuperstructureState.SOTM) { - speed.filtered(stomAcellLimit); - } else if (state == SuperstructureState.FOTM) { - speed.filtered(fotmAcellLimit); - } else { - speed.filtered(normalAcellLimit); - } - Vector2D speedVector = speed.get(); - double angularVel = turn.get(); - //TODO: test this - double maxAngularVel = Swerve.Constraints.MAX_ANGULAR_VEL_RAD_PER_S; - - if (speedVector.magnitude() > 0.05 && superstructure.getState() == SuperstructureState.SOTM) { - speedVector = speedVector.mul(Settings.Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S / Swerve.Constraints.MAX_VELOCITY_M_PER_S); - maxAngularVel = Settings.Swerve.Constraints.MAX_ANGULAR_VEL_SOTM_RAD_PER_S; - - } else if (speedVector.magnitude() > 0.05 && superstructure.getState() == SuperstructureState.FOTM) { - speedVector = speedVector.mul(Settings.Swerve.Constraints.MAX_VELOCITY_FOTM_M_PER_S / Swerve.Constraints.MAX_VELOCITY_M_PER_S); - maxAngularVel = Settings.Swerve.Constraints.MAX_ANGULAR_VEL_FOTM_RAD_PER_S; - - } - - angularVel = MathUtil.clamp(angularVel, -maxAngularVel, maxAngularVel); - swerve.setControl(swerve.getFieldCentricSwerveRequest() .withVelocityX(speed.get().x) .withVelocityY(speed.get().y) - .withRotationalRate(-angularVel)); - - SmartDashboard.putNumber("Swerve/Speed x", speed.get().x); - SmartDashboard.putNumber("Swerve/Speed y", speed.get().y); + .withRotationalRate(-turn.getAsDouble()) + ); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java index 4b8aa570..17cc810b 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java @@ -31,12 +31,10 @@ public class SwerveDriveFOTM extends Command{ private final VStream speed; private final IStream turn; - private final VRateLimit fotmAcellLimit; public SwerveDriveFOTM(Gamepad driver) { swerve = CommandSwerveDrivetrain.getInstance(); superstructure = Superstructure.getInstance(); - fotmAcellLimit = new VRateLimit(Settings.Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED_FOTM); speed = VStream.create(this::getDriverInputAsVelocity) .filtered( @@ -44,7 +42,7 @@ public SwerveDriveFOTM(Gamepad driver) { x -> x.clamp(1), x -> x.pow(Drive.POWER), x -> x.mul(Swerve.Constraints.MAX_VELOCITY_FOTM_M_PER_S), - fotmAcellLimit, + new VRateLimit(Settings.Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED_FOTM), new VLowPassFilter(Drive.RC) ); @@ -52,7 +50,7 @@ public SwerveDriveFOTM(Gamepad driver) { .filtered( x -> SLMath.deadband(x, Turn.DEADBAND), x -> SLMath.spow(x, Turn.POWER), - x -> x * Swerve.Constraints.MAX_VELOCITY_FOTM_M_PER_S, + x -> x * Swerve.Constraints.MAX_ANGULAR_VEL_FOTM_RAD_PER_S, new LowPassFilter(Turn.RC) ); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java index 3ad0244c..af3978af 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java @@ -31,12 +31,10 @@ public class SwerveDriveSOTM extends Command { private final VStream speed; private final IStream turn; - private final VRateLimit stomAcellLimit; public SwerveDriveSOTM(Gamepad driver) { swerve = CommandSwerveDrivetrain.getInstance(); superstructure = Superstructure.getInstance(); - stomAcellLimit = new VRateLimit(Settings.Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED_STOM); speed = VStream.create(this::getDriverInputAsVelocity) .filtered( @@ -44,7 +42,7 @@ public SwerveDriveSOTM(Gamepad driver) { x -> x.clamp(1), x -> x.pow(Drive.POWER), x -> x.mul(Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S), - stomAcellLimit, + new VRateLimit(Settings.Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED_SOTM), new VLowPassFilter(Drive.RC) ); @@ -52,7 +50,7 @@ public SwerveDriveSOTM(Gamepad driver) { .filtered( x -> SLMath.deadband(x, Turn.DEADBAND), x -> SLMath.spow(x, Turn.POWER), - x -> x * Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S, + x -> x * Swerve.Constraints.MAX_ANGULAR_VEL_SOTM_RAD_PER_S, new LowPassFilter(Turn.RC) ); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 0a1ab232..d231806d 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -271,7 +271,7 @@ public interface Constraints { public final double MAX_ANGULAR_VEL_FOTM_RAD_PER_S = Units.degreesToRadians(150.0); public final double MAX_ACCEL_M_PER_S_SQUARED = 15.0; - public final double MAX_ACCEL_M_PER_S_SQUARED_STOM = 15.0; + public final double MAX_ACCEL_M_PER_S_SQUARED_SOTM = 15.0; public final double MAX_ACCEL_M_PER_S_SQUARED_FOTM = 15.0; public final double MAX_ANGULAR_ACCEL_RAD_PER_S_SQUARED = Units.degreesToRadians(900.0); From 67a412900c7299de9d711ec8c0e15adcaa9860f7 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Mon, 9 Mar 2026 22:31:54 -0400 Subject: [PATCH 136/150] feat: auto spindexer unjam, hood rezeroing logic fix, intake dynamic gain switching --- .../com/stuypulse/robot/RobotContainer.java | 59 +++++++++++-------- .../com/stuypulse/robot/constants/Gains.java | 20 +++++-- .../stuypulse/robot/constants/Settings.java | 19 +++--- .../robot/subsystems/intake/IntakeImpl.java | 16 ++++- .../subsystems/spindexer/SpindexerImpl.java | 26 ++++++++ .../superstructure/hood/HoodImpl.java | 2 +- 6 files changed, 99 insertions(+), 43 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index ebf76e38..79eea1ce 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -35,6 +35,9 @@ import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; import com.stuypulse.robot.commands.superstructure.SuperstructureFerry; import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureKB; +import com.stuypulse.robot.commands.superstructure.SuperstructureLeftCorner; +import com.stuypulse.robot.commands.superstructure.SuperstructureRightCorner; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; import com.stuypulse.robot.commands.superstructure.SuperstructureShoot; import com.stuypulse.robot.commands.superstructure.SuperstructureStow; @@ -149,8 +152,8 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); - spindexer.setDefaultCommand(new SpindexerDefaultCommand()); - handoff.setDefaultCommand(new HandoffDefaultCommand()); + // spindexer.setDefaultCommand(new SpindexerDefaultCommand()); + // handoff.setDefaultCommand(new HandoffDefaultCommand()); } /***************/ @@ -165,7 +168,7 @@ private void configureButtonBindings() { .whileTrue(new SuperstructureInterpolation() .alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) .andThen(new HandoffRun()) - .alongWith(new WaitUntilCommand(() -> handoff.atTolerance() && handoff.getState() == HandoffState.FORWARD)) + .alongWith(new WaitUntilCommand(() -> handoff.getState() == HandoffState.FORWARD)) .andThen(new SpindexerRun())) .onFalse(new SpindexerStop() .alongWith(new SuperstructureStow()) @@ -187,8 +190,8 @@ private void configureButtonBindings() { .onFalse(new SetIMUMode(0)); // Stop Rollers - // driver.getLeftBumper() - // .onTrue(new IntakeStopRollers()); + driver.getLeftBumper() + .onTrue(new IntakeStopRollers()); // driver.getRightBumper() // .whileTrue(new IntakeOuttake()) @@ -236,26 +239,32 @@ private void configureButtonBindings() { //--------------------------------------------------------------------------------------------------------------------------\\ - // // Manual Left Corner Scoring - // driver.getLeftButton() - // .whileTrue(new SwerveXMode()) - // .whileTrue(new SuperstructureLeftCorner().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) - // .andThen(new SpindexerRun()).alongWith(new HandoffRun())) - // .onFalse(new SuperstructureInterpolation().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); - - // // Manual Right Corner Scoring - // driver.getRightButton() - // .whileTrue(new SwerveXMode()) - // .whileTrue(new SuperstructureRightCorner().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) - // .andThen(new SpindexerRun()).alongWith(new HandoffRun())) - // .onFalse(new SuperstructureInterpolation().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); - - // // Manual KB Distance Scoring - // driver.getBottomButton() - // .whileTrue(new SwerveXMode()) - // .whileTrue(new SuperstructureKB().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) - // .andThen(new SpindexerRun()).alongWith(new HandoffRun())) - // .onFalse(new SuperstructureInterpolation().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); + // Manual Left Corner Scoring + driver.getLeftButton() + .whileTrue(new SwerveXMode()) + .onTrue(new IntakeDeploy()) + .whileTrue(new SuperstructureLeftCorner().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + .andThen(new HandoffRun()).alongWith(new WaitUntilCommand(() -> handoff.atTolerance()) + .andThen(new SpindexerRun()))) + .onFalse(new SuperstructureInterpolation().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); + + // Manual Right Corner Scoring + driver.getRightButton() + .whileTrue(new SwerveXMode()) + .onTrue(new IntakeDeploy()) + .whileTrue(new SuperstructureRightCorner().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + .andThen(new HandoffRun()).alongWith(new WaitUntilCommand(() -> handoff.atTolerance()) + .andThen(new SpindexerRun()))) + .onFalse(new SuperstructureInterpolation().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); + + // Manual KB Distance Scoring + driver.getBottomButton() + .whileTrue(new SwerveXMode()) + .onTrue(new IntakeDeploy()) + .whileTrue(new SuperstructureKB().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + .andThen(new HandoffRun()).alongWith(new WaitUntilCommand(() -> handoff.atTolerance()) + .andThen(new SpindexerRun()))) + .onFalse(new SuperstructureInterpolation().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); } diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index d058142e..73027060 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -103,13 +103,21 @@ public interface Spindexer { public interface Intake { public interface Pivot { - double kP = 100.0; - double kI = 0.0; - double kD = 10.0; + // double kP = 100.0; + // double kI = 0.0; + // double kD = 10.0; - double kS = 0.0; - double kV = 0.12; - double kA = 0.0; + // double kS = 0.0; + // double kV = 0.12; + // double kA = 0.0; + + SmartNumber kP = new SmartNumber("Intake/Pivot/Gains/kP", 100.0); + SmartNumber kI = new SmartNumber("Intake/Pivot/Gains/kI", 0.0); + SmartNumber kD = new SmartNumber("Intake/Pivot/Gains/kD", 10.0); + + SmartNumber kS = new SmartNumber("Intake/Pivot/Gains/kS", 0.0); + SmartNumber kV = new SmartNumber("Intake/Pivot/Gains/kV", 0.12); + SmartNumber kA = new SmartNumber("Intake/Pivot/Gains/kA", 0.0); double kG = 0.5; } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index d231806d..b28929e3 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -72,6 +72,7 @@ public interface Spindexer { double STOP_SPEED = 0.0; double RPM_TOLERANCE = 800.0; + double STALL_CURRENT_LIMIT = 40.0; // random number as of 3/9 /* CONSTANTS */ @@ -153,9 +154,9 @@ public interface RPM { public final SmartNumber FERRY = new SmartNumber("InterpolationTesting/Ferry State Target RPM", 2000.0); public final double REVERSE = 0.0; - public final double KB = 0.0; - public final double LEFT_CORNER = 0.0; - public final double RIGHT_CORNER = 0.0; + public final double KB = 2720.0; + public final double LEFT_CORNER = 3850.0; + public final double RIGHT_CORNER = 3850.0; } } @@ -177,7 +178,7 @@ public interface Hood { public final double ENCODER_TO_MECH = 32.0 / 3.0; public final double HOOD_HOMING_VOLTAGE = 2.0; - public final Rotation2d ENCODER_OFFSET = Rotation2d.fromRotations(-0.043); + public final Rotation2d ENCODER_OFFSET = Rotation2d.fromRotations(-0.052); public final Rotation2d FORWARD_SOFT_LIMIT = Rotation2d.fromDegrees(39.0); public final Rotation2d REVERSE_SOFT_LIMIT = Rotation2d.fromDegrees(20.0); @@ -197,9 +198,9 @@ public interface Angles { public final Rotation2d MAX = Rotation2d.fromDegrees(40.0); public final Rotation2d STOW = Rotation2d.fromDegrees(21.0); - public final Rotation2d KB = Rotation2d.fromDegrees(12.0); - public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(10.0); - public final Rotation2d RIGHT_CORNER = Rotation2d.fromDegrees(10.0); + public final Rotation2d KB = Rotation2d.fromDegrees(22.0); + public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(38.0); + public final Rotation2d RIGHT_CORNER = Rotation2d.fromDegrees(38.0); } } @@ -210,8 +211,8 @@ public interface Turret { public final Rotation2d SOTM_TOLERANCE = Rotation2d.fromDegrees(5.0); public final Rotation2d KB = Rotation2d.fromDegrees(0.0); - public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(0.0); - public final Rotation2d RIGHT_CORNER = Rotation2d.fromDegrees(0.0); + public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(-127); + public final Rotation2d RIGHT_CORNER = Rotation2d.fromDegrees(-60); double RESOLUTION_OF_ABSOLUTE_ENCODER = 0.1; double WRAP_DEBOUNCE = 0.5; 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 b55a54c3..45fdb341 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -58,8 +58,8 @@ public IntakeImpl() { .withStatorCurrentLimitEnabled(false) .withRampRate(0.25) - .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, + .withPIDConstants(Gains.Intake.Pivot.kP.get(), Gains.Intake.Pivot.kI.get(), Gains.Intake.Pivot.kD.get(), 0) + .withFFConstants(Gains.Intake.Pivot.kS.get(), Gains.Intake.Pivot.kV.get(), Gains.Intake.Pivot.kA.get(), Gains.Intake.Pivot.kG, 0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign, 0) .withGravityType(GravityTypeValue.Arm_Cosine) @@ -125,7 +125,19 @@ public void zeroPivotDeployed() { @Override public void periodic() { super.periodic(); + PivotState pivotState = getPivotState(); + + pivotConfig.updateGainsConfig( + pivot, + 0, + Gains.Intake.Pivot.kP, + Gains.Intake.Pivot.kI, + Gains.Intake.Pivot.kD, + Gains.Intake.Pivot.kS, + Gains.Intake.Pivot.kV, + Gains.Intake.Pivot.kA + ); if (EnabledSubsystems.INTAKE.get()) { if (pivotVoltageOverride.isPresent()) { 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 67271247..8488655b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -12,7 +12,10 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.SysId; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -30,9 +33,12 @@ public class SpindexerImpl extends Spindexer { private final TalonFX leadMotor; private final TalonFX followerMotor; + private final Timer timer; + private boolean timerTriggered; private final VelocityVoltage controller; private final Follower follower; + private final BStream isStalling; private Optional voltageOverride; @@ -72,8 +78,15 @@ public SpindexerImpl() { controller = new VelocityVoltage(getTargetRPM()).withEnableFOC(true); follower = new Follower(Ports.Spindexer.SPINDEXER_LEAD_MOTOR, MotorAlignmentValue.Aligned); + isStalling = BStream.create(() -> leadMotor.getSupplyCurrent().getValueAsDouble() > Settings.Spindexer.STALL_CURRENT_LIMIT) + .filtered(new BDebounce.Both(Settings.Superstructure.Hood.STALL_DEBOUNCE)); + followerMotor.setControl(follower); + timer = new Timer(); + + timerTriggered = false; + voltageOverride = Optional.empty(); } @@ -104,6 +117,15 @@ public void periodic() { } else { leadMotor.setControl(controller.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE)); } + + // if (isStalling()) { + // if (timer.hasElapsed(0.25)) { + // timer.reset(); + // } + // else { + // leadMotor.setControl(controller.withVelocity(Settings.Spindexer.REVERSE_SPEED)); + // } + // } } } else { leadMotor.stopMotor(); @@ -128,6 +150,10 @@ public void periodic() { } } + public boolean isStalling() { + return isStalling.get(); + } + @Override public void setVoltageOverride(Optional voltage) { this.voltageOverride = voltage; diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index 6312a2d3..aa3a57c9 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -115,7 +115,7 @@ public void seedHood() { } private double getAbsoluteHoodAngleDeg() { - return Settings.Superstructure.Hood.MIN_FROM_HORIZON.getDegrees() + hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.Superstructure.Hood.ENCODER_TO_MECH; + return hoodEncoder.getPosition().getValueAsDouble() * 360.0; //Settings.Superstructure.Hood.MIN_FROM_HORIZON.getDegrees() + hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.Superstructure.Hood.ENCODER_TO_MECH; } @Override From 7670330738e49194cb82927f2f04b890129321b5 Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Mon, 9 Mar 2026 22:46:23 -0400 Subject: [PATCH 137/150] FIX: Auto shoot time --- .../stuypulse/robot/commands/auton/regular/LeftTwoCycle.java | 3 ++- .../stuypulse/robot/commands/auton/regular/RightTwoCycle.java | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java index 1d86c4be..83bcf954 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class LeftTwoCycle extends SequentialCommandGroup { @@ -34,7 +35,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new HandoffRun().alongWith(new WaitUntilCommand(() -> Handoff.getInstance().atTolerance())).andThen( new SpindexerRun() - ).withTimeout(10.0), + ).andThen(new WaitCommand(5.0)), // NZ Trip 2 diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java index b603f0b7..3e090f53 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class RightTwoCycle extends SequentialCommandGroup { @@ -34,7 +35,7 @@ public RightTwoCycle(PathPlannerPath... paths) { new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new HandoffRun().alongWith(new WaitUntilCommand(() -> Handoff.getInstance().atTolerance())).andThen( new SpindexerRun() - ).withTimeout(10.0), + ).andThen(new WaitCommand(5.0)), // NZ Trip 2 From 537500a94e80c130abed25295fcd98d14a4f5f19 Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Mon, 9 Mar 2026 23:56:14 -0400 Subject: [PATCH 138/150] FIX: handoff and spindexer default cmds, intake --- simgui.json | 9 +++ .../com/stuypulse/robot/RobotContainer.java | 67 ++++++++++++++----- 2 files changed, 61 insertions(+), 15 deletions(-) diff --git a/simgui.json b/simgui.json index 3c0f9e96..0d02d095 100644 --- a/simgui.json +++ b/simgui.json @@ -45,9 +45,18 @@ "FieldPositions": { "open": true }, + "Handoff": { + "open": true + }, + "Spindexer": { + "open": true + }, "States": { "open": true }, + "Superstructure": { + "open": true + }, "open": true } } diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 79eea1ce..d85ebe19 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -5,6 +5,8 @@ /***************************************************************/ package com.stuypulse.robot; +import java.util.concurrent.locks.Condition; + import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; import com.stuypulse.robot.commands.auton.regular.DepotAuton; @@ -152,8 +154,31 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); - // spindexer.setDefaultCommand(new SpindexerDefaultCommand()); - // handoff.setDefaultCommand(new HandoffDefaultCommand()); + spindexer.setDefaultCommand( + new ConditionalCommand( + new SpindexerStop(), + new ConditionalCommand( + new ConditionalCommand( + new SpindexerRun(), + new SpindexerReverse(), + () -> spindexer.getState() == SpindexerState.FORWARD), + new SpindexerStop(), + () -> spindexer.getState() != SpindexerState.STOP + ), + () -> swerve.isBehindHub() || swerve.isBehindTower() || turret.isWrapping() || swerve.isUnderTrench()) + ); + handoff.setDefaultCommand( + new ConditionalCommand( + new HandoffStop(), + new ConditionalCommand( + new ConditionalCommand( + new HandoffRun(), + new HandoffReverse(), + () -> handoff.getState() == HandoffState.FORWARD), + new HandoffStop(), + () -> handoff.getState() != HandoffState.STOP + ), + () -> swerve.isBehindHub() || swerve.isBehindTower() || turret.isWrapping() || swerve.isUnderTrench())); } /***************/ @@ -168,7 +193,7 @@ private void configureButtonBindings() { .whileTrue(new SuperstructureInterpolation() .alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) .andThen(new HandoffRun()) - .alongWith(new WaitUntilCommand(() -> handoff.getState() == HandoffState.FORWARD)) + .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) .andThen(new SpindexerRun())) .onFalse(new SpindexerStop() .alongWith(new SuperstructureStow()) @@ -202,9 +227,9 @@ private void configureButtonBindings() { .whileTrue(new SwerveXMode()) .onTrue(new IntakeRunRollers()) .whileTrue(new SuperstructureFerry() - .alongWith(new WaitUntilCommand(() -> superstructure.atTolerance() && superstructure.getState() == SuperstructureState.FERRY)) + .alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) .andThen(new HandoffRun()) - .alongWith(new WaitUntilCommand(() -> handoff.atTolerance() && handoff.getState() == HandoffState.FORWARD)) + .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) .andThen(new SpindexerRun())) .onFalse(new SuperstructureFerry().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); @@ -213,11 +238,17 @@ private void configureButtonBindings() { .onTrue(new IntakeRunRollers()) .onTrue(new ConditionalCommand( new ParallelCommandGroup( - new SuperstructureInterpolation(), + new SuperstructureStow(), new SpindexerStop(), new HandoffStop() ), - new SuperstructureSOTM().alongWith(new SwerveDriveSOTM(driver)), + new ParallelCommandGroup( + new SuperstructureSOTM().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + .andThen(new HandoffRun()) + .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) + .andThen(new SpindexerRun()), + new SwerveDriveSOTM(driver) + ), () -> superstructure.getState() == SuperstructureState.SOTM )); @@ -226,11 +257,17 @@ private void configureButtonBindings() { .onTrue(new IntakeRunRollers()) .onTrue(new ConditionalCommand( new ParallelCommandGroup( - new SuperstructureInterpolation(), + new SuperstructureStow(), new SpindexerStop(), new HandoffStop() ), - new SuperstructureFOTM().alongWith(new SwerveDriveFOTM(driver)), + new ParallelCommandGroup( + new SuperstructureFOTM().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + .andThen(new HandoffRun()) + .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) + .andThen(new SpindexerRun()), + new SwerveDriveFOTM(driver) + ), () -> superstructure.getState() == SuperstructureState.FOTM )); @@ -242,29 +279,29 @@ private void configureButtonBindings() { // Manual Left Corner Scoring driver.getLeftButton() .whileTrue(new SwerveXMode()) - .onTrue(new IntakeDeploy()) + .onTrue(new IntakeRunRollers()) .whileTrue(new SuperstructureLeftCorner().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) .andThen(new HandoffRun()).alongWith(new WaitUntilCommand(() -> handoff.atTolerance()) .andThen(new SpindexerRun()))) - .onFalse(new SuperstructureInterpolation().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); + .onFalse(new SuperstructureStow().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); // Manual Right Corner Scoring driver.getRightButton() .whileTrue(new SwerveXMode()) - .onTrue(new IntakeDeploy()) + .onTrue(new IntakeRunRollers()) .whileTrue(new SuperstructureRightCorner().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) .andThen(new HandoffRun()).alongWith(new WaitUntilCommand(() -> handoff.atTolerance()) .andThen(new SpindexerRun()))) - .onFalse(new SuperstructureInterpolation().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); + .onFalse(new SuperstructureStow().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); // Manual KB Distance Scoring driver.getBottomButton() .whileTrue(new SwerveXMode()) - .onTrue(new IntakeDeploy()) + .onTrue(new IntakeRunRollers()) .whileTrue(new SuperstructureKB().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) .andThen(new HandoffRun()).alongWith(new WaitUntilCommand(() -> handoff.atTolerance()) .andThen(new SpindexerRun()))) - .onFalse(new SuperstructureInterpolation().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); + .onFalse(new SuperstructureStow().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); } From a18a4aa0cf41f6a7a46519109f3e9228014475be Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Tue, 10 Mar 2026 00:06:46 -0400 Subject: [PATCH 139/150] ADD: outtake button --- src/main/java/com/stuypulse/robot/RobotContainer.java | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index d85ebe19..0e7e8fce 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -22,6 +22,7 @@ import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.hood.ZeroHoodEncoderAtUpperHardstop; import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.intake.IntakeOuttake; import com.stuypulse.robot.commands.intake.IntakeRunRollers; import com.stuypulse.robot.commands.intake.IntakeSetState; import com.stuypulse.robot.commands.intake.IntakeStopRollers; @@ -215,12 +216,12 @@ private void configureButtonBindings() { .onFalse(new SetIMUMode(0)); // Stop Rollers - driver.getLeftBumper() - .onTrue(new IntakeStopRollers()); + // driver.getLeftBumper() + // .onTrue(new IntakeStopRollers()); - // driver.getRightBumper() - // .whileTrue(new IntakeOuttake()) - // .onFalse(new IntakeRunRollers()); + driver.getRightBumper() + .whileTrue(new IntakeOuttake()) + .onFalse(new IntakeRunRollers()); // Ferrying In Place driver.getDPadRight() From 601e2f811983a357f51950755e3548cd7c5a0c5d Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Tue, 10 Mar 2026 10:38:08 -0400 Subject: [PATCH 140/150] FIX; zero hood and stop spindexer --- src/main/java/com/stuypulse/robot/constants/Settings.java | 2 +- .../com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java | 2 +- .../robot/subsystems/superstructure/hood/HoodImpl.java | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index b28929e3..b3485c75 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -178,7 +178,7 @@ public interface Hood { public final double ENCODER_TO_MECH = 32.0 / 3.0; public final double HOOD_HOMING_VOLTAGE = 2.0; - public final Rotation2d ENCODER_OFFSET = Rotation2d.fromRotations(-0.052); + public final Rotation2d ENCODER_OFFSET = Rotation2d.fromRotations(-0.14); public final Rotation2d FORWARD_SOFT_LIMIT = Rotation2d.fromDegrees(39.0); public final Rotation2d REVERSE_SOFT_LIMIT = Rotation2d.fromDegrees(20.0); 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 8488655b..0dc40d70 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -112,7 +112,7 @@ public void periodic() { leadMotor.setVoltage(voltageOverride.get()); } else { // DO NOT REMOVE BELOW LINE - needed to brake the motor in STOP state - if (atTolerance() && getState() == SpindexerState.STOP) { + if (getState() == SpindexerState.STOP) { leadMotor.stopMotor(); } else { leadMotor.setControl(controller.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE)); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index aa3a57c9..6312a2d3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -115,7 +115,7 @@ public void seedHood() { } private double getAbsoluteHoodAngleDeg() { - return hoodEncoder.getPosition().getValueAsDouble() * 360.0; //Settings.Superstructure.Hood.MIN_FROM_HORIZON.getDegrees() + hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.Superstructure.Hood.ENCODER_TO_MECH; + return Settings.Superstructure.Hood.MIN_FROM_HORIZON.getDegrees() + hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.Superstructure.Hood.ENCODER_TO_MECH; } @Override From b73f5055134d7822e5d6023fbdac41ecd44c3b74 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Tue, 10 Mar 2026 15:26:15 -0400 Subject: [PATCH 141/150] FIX: sysid module translation, remove default commands, change intake buttons, fix scoring in place logic, remove opposite robot velocity vector from SOTM pose calculation (works better now) --- simgui-ds.json | 5 -- simgui.json | 12 ++-- .../com/stuypulse/robot/RobotContainer.java | 55 +++++++------------ .../commands/hood/NewZeroAtUpperHardstop.java | 23 ++++++++ ...eedHoodRelativeEncoderAtUpperHardstop.java | 20 +++++++ .../com/stuypulse/robot/constants/Gains.java | 8 +-- .../stuypulse/robot/constants/Settings.java | 9 +-- .../subsystems/spindexer/SpindexerImpl.java | 12 +--- .../superstructure/Superstructure.java | 4 +- .../subsystems/superstructure/hood/Hood.java | 4 ++ .../superstructure/hood/HoodImpl.java | 25 ++++++++- .../superstructure/hood/HoodSim.java | 12 ++++ .../superstructure/turret/TurretImpl.java | 2 +- .../subsystems/swerve/TunerConstants.java | 4 +- .../util/superstructure/SOTMCalculator.java | 41 +++++++------- 15 files changed, 146 insertions(+), 90 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/hood/NewZeroAtUpperHardstop.java create mode 100644 src/main/java/com/stuypulse/robot/commands/hood/SeedHoodRelativeEncoderAtUpperHardstop.java diff --git a/simgui-ds.json b/simgui-ds.json index 1f8961ec..ff81e307 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,9 +1,4 @@ { - "Keyboard 0 Settings": { - "window": { - "visible": true - } - }, "System Joysticks": { "window": { "enabled": false diff --git a/simgui.json b/simgui.json index 0d02d095..c9f35a38 100644 --- a/simgui.json +++ b/simgui.json @@ -9,6 +9,7 @@ "/SmartDashboard/PathPlanner": "Alerts", "/SmartDashboard/Robot/Handoff Reverse": "Command", "/SmartDashboard/Robot/Intake Reverse": "Command", + "/SmartDashboard/Robot/Seed Hood Relative Encoder At Upper Hardstop": "Command", "/SmartDashboard/Robot/Seed Turret": "Command", "/SmartDashboard/Robot/Spindexer Reverse": "Command", "/SmartDashboard/Robot/Zero Hood Encoder": "Command", @@ -36,6 +37,11 @@ "window": { "visible": true } + }, + "/SmartDashboard/Scheduler": { + "window": { + "visible": true + } } } }, @@ -48,15 +54,9 @@ "Handoff": { "open": true }, - "Spindexer": { - "open": true - }, "States": { "open": true }, - "Superstructure": { - "open": true - }, "open": true } } diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 0e7e8fce..50dd4b14 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -20,6 +20,8 @@ import com.stuypulse.robot.commands.handoff.HandoffReverse; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.hood.NewZeroAtUpperHardstop; +import com.stuypulse.robot.commands.hood.SeedHoodRelativeEncoderAtUpperHardstop; import com.stuypulse.robot.commands.hood.ZeroHoodEncoderAtUpperHardstop; import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.intake.IntakeOuttake; @@ -51,6 +53,7 @@ import com.stuypulse.robot.commands.swerve.SwerveWheelRadiusCharacterization; import com.stuypulse.robot.commands.swerve.SwerveXMode; import com.stuypulse.robot.commands.turret.SeedTurret; +import com.stuypulse.robot.commands.turret.TurretLeftCorner; import com.stuypulse.robot.commands.turret.ZeroTurret; import com.stuypulse.robot.commands.vision.ResetLimelightIMU; import com.stuypulse.robot.commands.vision.SetIMUMode; @@ -132,7 +135,8 @@ public RobotContainer() { SmartDashboard.putData("Robot/Zero Pivot Encoder at Upper Limit (Stowed)", new ZeroPivotStowed().ignoringDisable(true)); SmartDashboard.putData("Robot/Zero Turret Encoders", new ZeroTurret().ignoringDisable(true)); SmartDashboard.putData("Robot/Seed Turret", new SeedTurret().ignoringDisable(true)); - SmartDashboard.putData("Robot/Zero Hood Encoder", new ZeroHoodEncoderAtUpperHardstop().ignoringDisable(true)); + SmartDashboard.putData("Robot/Seed Hood Relative Encoder At Upper Hardstop", new SeedHoodRelativeEncoderAtUpperHardstop().ignoringDisable(true)); + SmartDashboard.putData("Robot/Ryan Testing Seed Hood Encoder (NEW)", new NewZeroAtUpperHardstop()); SmartDashboard.putData("Robot/Handoff Reverse", new ConditionalCommand( @@ -155,31 +159,6 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); - spindexer.setDefaultCommand( - new ConditionalCommand( - new SpindexerStop(), - new ConditionalCommand( - new ConditionalCommand( - new SpindexerRun(), - new SpindexerReverse(), - () -> spindexer.getState() == SpindexerState.FORWARD), - new SpindexerStop(), - () -> spindexer.getState() != SpindexerState.STOP - ), - () -> swerve.isBehindHub() || swerve.isBehindTower() || turret.isWrapping() || swerve.isUnderTrench()) - ); - handoff.setDefaultCommand( - new ConditionalCommand( - new HandoffStop(), - new ConditionalCommand( - new ConditionalCommand( - new HandoffRun(), - new HandoffReverse(), - () -> handoff.getState() == HandoffState.FORWARD), - new HandoffStop(), - () -> handoff.getState() != HandoffState.STOP - ), - () -> swerve.isBehindHub() || swerve.isBehindTower() || turret.isWrapping() || swerve.isUnderTrench())); } /***************/ @@ -192,9 +171,9 @@ private void configureButtonBindings() { .whileTrue(new SwerveXMode()) .onTrue(new IntakeRunRollers()) .whileTrue(new SuperstructureInterpolation() - .alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + .alongWith(new WaitUntilCommand(() -> superstructure.getState() == SuperstructureState.INTERPOLATION && superstructure.atTolerance())) .andThen(new HandoffRun()) - .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) + .alongWith(new WaitUntilCommand(() -> handoff.getState() == HandoffState.FORWARD && handoff.atTolerance())) .andThen(new SpindexerRun())) .onFalse(new SpindexerStop() .alongWith(new SuperstructureStow()) @@ -207,7 +186,9 @@ private void configureButtonBindings() { // Intake Deploy driver.getRightTriggerButton() .onTrue(new IntakeDeploy()) - .onTrue(new SuperstructureStow()); // TURNS OFF SOTM + .onTrue(new SuperstructureStow() + .alongWith(new SpindexerStop()) //TODO: test this logic + .alongWith(new HandoffStop())); // TURNS OFF SOTM // Reset Heading driver.getDPadUp() @@ -216,8 +197,8 @@ private void configureButtonBindings() { .onFalse(new SetIMUMode(0)); // Stop Rollers - // driver.getLeftBumper() - // .onTrue(new IntakeStopRollers()); + driver.getLeftBumper() + .onTrue(new IntakeDeploy().andThen(new IntakeStopRollers())); driver.getRightBumper() .whileTrue(new IntakeOuttake()) @@ -227,12 +208,14 @@ private void configureButtonBindings() { driver.getDPadRight() .whileTrue(new SwerveXMode()) .onTrue(new IntakeRunRollers()) - .whileTrue(new SuperstructureFerry() - .alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + .whileTrue(new SuperstructureInterpolation() // CURRENTLY, THIS PROVES THE FERRYING STATE IS BLOCKING SPINDEXER AND HANDOFF SOMEWHERE IN THE CODE + .alongWith(new WaitUntilCommand(() -> superstructure.getState() == SuperstructureState.INTERPOLATION && superstructure.atTolerance())) .andThen(new HandoffRun()) - .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) + .alongWith(new WaitUntilCommand(() -> handoff.getState() == HandoffState.FORWARD && handoff.atTolerance())) .andThen(new SpindexerRun())) - .onFalse(new SuperstructureFerry().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); + .onFalse(new SpindexerStop() + .alongWith(new SuperstructureStow()) + .alongWith(new HandoffStop())); // SOTM driver.getRightMenuButton() @@ -272,7 +255,7 @@ private void configureButtonBindings() { () -> superstructure.getState() == SuperstructureState.FOTM )); - driver.getLeftBumper() + driver.getDPadDown() .whileTrue(new SwerveXMode()); //--------------------------------------------------------------------------------------------------------------------------\\ diff --git a/src/main/java/com/stuypulse/robot/commands/hood/NewZeroAtUpperHardstop.java b/src/main/java/com/stuypulse/robot/commands/hood/NewZeroAtUpperHardstop.java new file mode 100644 index 00000000..e8536c50 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/hood/NewZeroAtUpperHardstop.java @@ -0,0 +1,23 @@ +package com.stuypulse.robot.commands.hood; + + + +import com.stuypulse.robot.subsystems.superstructure.hood.Hood; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class NewZeroAtUpperHardstop extends InstantCommand { + private final Hood hood; + + public NewZeroAtUpperHardstop() { + this.hood = Hood.getInstance(); + + addRequirements(hood); + } + + @Override + public void initialize() { + hood.seedHoodAtUpperHardStop(); + hood.zeroHoodEncodersAfterSeed(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/hood/SeedHoodRelativeEncoderAtUpperHardstop.java b/src/main/java/com/stuypulse/robot/commands/hood/SeedHoodRelativeEncoderAtUpperHardstop.java new file mode 100644 index 00000000..95bcaabb --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/hood/SeedHoodRelativeEncoderAtUpperHardstop.java @@ -0,0 +1,20 @@ +package com.stuypulse.robot.commands.hood; + +import com.stuypulse.robot.subsystems.superstructure.hood.Hood; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class SeedHoodRelativeEncoderAtUpperHardstop extends InstantCommand { + private final Hood hood; + + public SeedHoodRelativeEncoderAtUpperHardstop() { + this.hood = Hood.getInstance(); + + addRequirements(hood); + } + + @Override + public void initialize() { + hood.seedHoodAtUpperHardStop(); + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 73027060..a4abbeda 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -135,13 +135,13 @@ public interface Handoff { public interface Swerve { public interface Drive { - double kP = 0.3838; + double kP = 0.10224; double kI = 0.0; double kD = 0.0; - double kS = 0.20896; - double kV = 0.12464; - double kA = 0.014877; + double kS = 0.19896; + double kV = 0.12528; + double kA = 0.011662; } public interface Turn { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index b3485c75..49784e8a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -102,7 +102,7 @@ public interface RPMInterpolation{ {2.15, 2930.0}, //BLAY-APPROVED {3.38, 3200}, //BLAY-APPROVED {4.43, 3550.0}, //BLAY-APPROVED - {5.66, 3850.0} //KEVIN-APPROVED + {5.66, 3900.0} //KEVIN-APPROVED }; } @@ -178,11 +178,12 @@ public interface Hood { public final double ENCODER_TO_MECH = 32.0 / 3.0; public final double HOOD_HOMING_VOLTAGE = 2.0; - public final Rotation2d ENCODER_OFFSET = Rotation2d.fromRotations(-0.14); + public final Rotation2d ENCODER_OFFSET = Rotation2d.fromRotations(0.795); public final Rotation2d FORWARD_SOFT_LIMIT = Rotation2d.fromDegrees(39.0); public final Rotation2d REVERSE_SOFT_LIMIT = Rotation2d.fromDegrees(20.0); public final Rotation2d MIN_FROM_HORIZON = Rotation2d.fromDegrees(7.0); + public final Rotation2d MAX_FROM_HORIZON = Rotation2d.fromDegrees(40.0); public final double STALL_CURRENT_LIMIT = 20.0; public final double STALL_DEBOUNCE = 0.5; @@ -211,8 +212,8 @@ public interface Turret { public final Rotation2d SOTM_TOLERANCE = Rotation2d.fromDegrees(5.0); public final Rotation2d KB = Rotation2d.fromDegrees(0.0); - public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(-127); - public final Rotation2d RIGHT_CORNER = Rotation2d.fromDegrees(-60); + public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(-127.0); + public final Rotation2d RIGHT_CORNER = Rotation2d.fromDegrees(-60.0); double RESOLUTION_OF_ABSOLUTE_ENCODER = 0.1; double WRAP_DEBOUNCE = 0.5; 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 0dc40d70..1f79deb8 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -10,6 +10,7 @@ import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.SysId; import com.stuypulse.stuylib.streams.booleans.BStream; @@ -114,18 +115,11 @@ public void periodic() { // DO NOT REMOVE BELOW LINE - needed to brake the motor in STOP state if (getState() == SpindexerState.STOP) { leadMotor.stopMotor(); + } else if (Superstructure.getInstance().isTurretWrapping()) { + leadMotor.stopMotor(); } else { leadMotor.setControl(controller.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE)); } - - // if (isStalling()) { - // if (timer.hasElapsed(0.25)) { - // timer.reset(); - // } - // else { - // leadMotor.setControl(controller.withVelocity(Settings.Spindexer.REVERSE_SPEED)); - // } - // } } } else { leadMotor.stopMotor(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index 601cbf8e..63a02279 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -52,8 +52,8 @@ public enum SuperstructureState { FOTM(HoodState.FOTM, ShooterState.FOTM, TurretState.FOTM), REVERSE(HoodState.SHOOT, ShooterState.REVERSE, TurretState.SHOOT), KB(HoodState.KB, ShooterState.KB, TurretState.KB), - LEFT_CORNER(HoodState.LEFT_CORNER, ShooterState.LEFT_CORNER, TurretState.SHOOT), - RIGHT_CORNER(HoodState.RIGHT_CORNER, ShooterState.RIGHT_CORNER, TurretState.SHOOT), + LEFT_CORNER(HoodState.LEFT_CORNER, ShooterState.LEFT_CORNER, TurretState.LEFT_CORNER), + RIGHT_CORNER(HoodState.RIGHT_CORNER, ShooterState.RIGHT_CORNER, TurretState.RIGHT_CORNER), INTERPOLATION(HoodState.INTERPOLATION, ShooterState.INTERPOLATION, TurretState.SHOOT), SOTM(HoodState.SOTM, ShooterState.SOTM, TurretState.SOTM); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java index cc7b1d36..7bb8a770 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java @@ -123,6 +123,10 @@ public Rotation2d hoodAnalogToOutput() { public abstract void zeroHoodEncoderAtUpperHardstop(); public abstract void seedHood(); + public abstract void seedHoodAtUpperHardStop(); + public abstract void zeroHoodEncodersAfterSeed(); + + @Override public void periodic() { SmartDashboard.putString("Superstructure/Hood/State", state.name()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index 6312a2d3..dc6507f7 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -114,8 +114,13 @@ public void seedHood() { hoodMotor.setPosition(getAbsoluteHoodAngleDeg() / 360.0); } + @Override + public void seedHoodAtUpperHardStop() { + hoodMotor.setPosition(Rotation2d.fromDegrees(40).getRotations()); + } + private double getAbsoluteHoodAngleDeg() { - return Settings.Superstructure.Hood.MIN_FROM_HORIZON.getDegrees() + hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.Superstructure.Hood.ENCODER_TO_MECH; + return Settings.Superstructure.Hood.MIN_FROM_HORIZON.getDegrees() + hoodEncoder.getAbsolutePosition().getValueAsDouble() * 360.0 / Settings.Superstructure.Hood.ENCODER_TO_MECH; } @Override @@ -179,4 +184,22 @@ public SysIdRoutine getHoodSysIdRoutine() { ); } + + @Override + public void zeroHoodEncodersAfterSeed() { //only use if you are seeded -> might add a boolean to double check that we are in seed at Upper Hardstop ^^ + hoodEncoder.getConfigurator().refresh(hoodEncoderConfig.getConfiguration().MagnetSensor); + + double currentOffset = hoodEncoderConfig.getConfiguration().MagnetSensor.MagnetOffset; + + double encoderPositionWithCurrentOffset = hoodEncoder.getPosition().getValueAsDouble(); + double encoderPositionWithOutOffset = encoderPositionWithCurrentOffset - currentOffset; + + //double newOffset = -((positionWithCurrentOffset - currentOffset) - Settings.Superstructure.Hood.Angles.MAX.getRotations()); + double newOffset = encoderPositionWithOutOffset - hoodMotor.getPosition().getValueAsDouble(); + + hoodEncoderConfig.withMagnetOffset(newOffset); + + hoodEncoderConfig.configure(hoodEncoder); + } + } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java index 5d2a9c56..7b140b29 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java @@ -115,4 +115,16 @@ public void periodic() { SmartDashboard.putNumber("Superstructure/Hood/Sim Height (m)", sim.getPositionMeters()); } } + + @Override + public void seedHoodAtUpperHardStop() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'seedHoodAtUpperHardStop'"); + } + + @Override + public void zeroHoodEncodersAfterSeed() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'zeroHoodEncodersAfterSeed'"); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index 816721dc..892e9ec4 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -160,7 +160,7 @@ public void periodic() { if (!hasUsedAbsoluteEncoder) { seedTurret(); hasUsedAbsoluteEncoder = true; - System.out.println("Absolute Encoder Reset triggered"); + System.out.println("Absolute Encoder Reset triggered"); //TODO: remove, printing consumes resouces on driver station (source wpilib themselves) } double currentAngle = getAngle().getDegrees(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java index a51f6c1c..f8ad7f30 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java @@ -34,8 +34,8 @@ public class TunerConstants { // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.3838).withKI(0).withKD(0) - .withKS(0.20896).withKV(0.12464).withKA(0.014877); + .withKP(0.115).withKI(0).withKD(0) + .withKS(0.19896).withKV(0.12528).withKA(0.011662); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java index 7245fdc8..c72e6da3 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java @@ -235,25 +235,25 @@ public static void updateSOTMSolution() { ).transformBy(robotToTurret); - Vector2D oppositeDirection = new Vector2D(new Translation2d( - -fieldRelativeSpeeds.vxMetersPerSecond, - -fieldRelativeSpeeds.vyMetersPerSecond - )); - - if (oppositeDirection.magnitude() < Settings.Swerve.MODULE_VELOCITY_DEADBAND_M_PER_S) { - oppositeDirection = Vector2D.kOrigin; - } - else { - oppositeDirection = oppositeDirection.normalize(); - } - - hubPose = hubPose.exp( - new Twist2d( - oppositeDirection.x * Field.HUB_RADIUS, - oppositeDirection.y * Field.HUB_RADIUS, - 0 - ) - ); + // Vector2D oppositeDirection = new Vector2D(new Translation2d( + // -fieldRelativeSpeeds.vxMetersPerSecond, + // -fieldRelativeSpeeds.vyMetersPerSecond + // )); + + // if (oppositeDirection.magnitude() < Settings.Swerve.MODULE_VELOCITY_DEADBAND_M_PER_S) { + // oppositeDirection = Vector2D.kOrigin; + // } + // else { + // oppositeDirection = oppositeDirection.normalize(); + // } + + // hubPose = hubPose.exp( + // new Twist2d( + // oppositeDirection.x * Field.HUB_RADIUS, + // oppositeDirection.y * Field.HUB_RADIUS, + // 0 + // ) + // ); hubSol = solveSOTM( futureTurretPose, @@ -329,7 +329,8 @@ public static double calculateShooterRPMSOTM() { } public static Rotation2d calculateHoodAngleFOTM() { - return ferrySol.targetHoodAngle(); + // return ferrySol.targetHoodAngle(); + return Rotation2d.fromDegrees(39); } public static Rotation2d calculateTurretAngleFOTM() { From e306ac5b422d7d75e8ad83d59c1936760d8aaad6 Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Tue, 10 Mar 2026 17:17:40 -0400 Subject: [PATCH 142/150] FEAT: updated auto order --- src/main/java/com/stuypulse/robot/RobotContainer.java | 2 +- .../robot/commands/auton/regular/LeftTwoCycle.java | 8 +++++--- .../robot/commands/auton/regular/RightTwoCycle.java | 8 +++++--- 3 files changed, 11 insertions(+), 7 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 0e7e8fce..b53d8dbf 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -303,7 +303,7 @@ private void configureButtonBindings() { .andThen(new HandoffRun()).alongWith(new WaitUntilCommand(() -> handoff.atTolerance()) .andThen(new SpindexerRun()))) .onFalse(new SuperstructureStow().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); - + } /**************/ diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java index 83bcf954..87ebf450 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java @@ -4,6 +4,8 @@ import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.intake.IntakeRunRollers; +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; @@ -28,15 +30,15 @@ public LeftTwoCycle(PathPlannerPath... paths) { new IntakeDeploy().alongWith( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) ), - new SuperstructureInterpolation(), + new SuperstructureInterpolation().alongWith(new IntakeStopRollers()), // Trip 1 To Score CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new HandoffRun().alongWith(new WaitUntilCommand(() -> Handoff.getInstance().atTolerance())).andThen( new SpindexerRun() ).andThen(new WaitCommand(5.0)), - + new IntakeRunRollers(), // NZ Trip 2 new ParallelCommandGroup( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java index 3e090f53..b6218af7 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java @@ -4,6 +4,8 @@ import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.intake.IntakeRunRollers; +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; @@ -28,15 +30,15 @@ public RightTwoCycle(PathPlannerPath... paths) { new IntakeDeploy().alongWith( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) ), - new SuperstructureInterpolation(), + new SuperstructureInterpolation().alongWith(new IntakeStopRollers()), // Trip 1 To Score CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new HandoffRun().alongWith(new WaitUntilCommand(() -> Handoff.getInstance().atTolerance())).andThen( new SpindexerRun() ).andThen(new WaitCommand(5.0)), - + new IntakeRunRollers(), // NZ Trip 2 new ParallelCommandGroup( From 6921691854f09dd0e600a101182c305d4c863f99 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Tue, 10 Mar 2026 17:30:35 -0400 Subject: [PATCH 143/150] FEAT: add pid gains for rotation and translation alignment (pidtopose) --- .../com/stuypulse/robot/RobotContainer.java | 26 +++++++++++-------- .../pidToPose/SwerveDrivePIDToPose.java | 4 ++- .../com/stuypulse/robot/constants/Gains.java | 4 +-- .../superstructure/hood/HoodImpl.java | 2 ++ .../swerve/CommandSwerveDrivetrain.java | 6 ++--- 5 files changed, 25 insertions(+), 17 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 50dd4b14..3cd77980 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -52,6 +52,7 @@ import com.stuypulse.robot.commands.swerve.SwerveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveWheelRadiusCharacterization; import com.stuypulse.robot.commands.swerve.SwerveXMode; +import com.stuypulse.robot.commands.swerve.pidToPose.SwerveDrivePIDToPose; import com.stuypulse.robot.commands.turret.SeedTurret; import com.stuypulse.robot.commands.turret.TurretLeftCorner; import com.stuypulse.robot.commands.turret.ZeroTurret; @@ -77,6 +78,8 @@ import com.stuypulse.stuylib.input.gamepads.AutoGamepad; import com.stuypulse.stuylib.network.SmartBoolean; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -138,6 +141,7 @@ public RobotContainer() { SmartDashboard.putData("Robot/Seed Hood Relative Encoder At Upper Hardstop", new SeedHoodRelativeEncoderAtUpperHardstop().ignoringDisable(true)); SmartDashboard.putData("Robot/Ryan Testing Seed Hood Encoder (NEW)", new NewZeroAtUpperHardstop()); + SmartDashboard.putData("Robot/Handoff Reverse", new ConditionalCommand( new HandoffReverse().andThen(new WaitCommand(0.25)).andThen(new HandoffRun()), @@ -205,17 +209,17 @@ private void configureButtonBindings() { .onFalse(new IntakeRunRollers()); // Ferrying In Place - driver.getDPadRight() - .whileTrue(new SwerveXMode()) - .onTrue(new IntakeRunRollers()) - .whileTrue(new SuperstructureInterpolation() // CURRENTLY, THIS PROVES THE FERRYING STATE IS BLOCKING SPINDEXER AND HANDOFF SOMEWHERE IN THE CODE - .alongWith(new WaitUntilCommand(() -> superstructure.getState() == SuperstructureState.INTERPOLATION && superstructure.atTolerance())) - .andThen(new HandoffRun()) - .alongWith(new WaitUntilCommand(() -> handoff.getState() == HandoffState.FORWARD && handoff.atTolerance())) - .andThen(new SpindexerRun())) - .onFalse(new SpindexerStop() - .alongWith(new SuperstructureStow()) - .alongWith(new HandoffStop())); + // driver.getDPadRight() + // .whileTrue(new SwerveXMode()) + // .onTrue(new IntakeRunRollers()) + // .whileTrue(new SuperstructureInterpolation() // CURRENTLY, THIS PROVES THE FERRYING STATE IS BLOCKING SPINDEXER AND HANDOFF SOMEWHERE IN THE CODE + // .alongWith(new WaitUntilCommand(() -> superstructure.getState() == SuperstructureState.INTERPOLATION && superstructure.atTolerance())) + // .andThen(new HandoffRun()) + // .alongWith(new WaitUntilCommand(() -> handoff.getState() == HandoffState.FORWARD && handoff.atTolerance())) + // .andThen(new SpindexerRun())) + // .onFalse(new SpindexerStop() + // .alongWith(new SuperstructureStow()) + // .alongWith(new HandoffStop())); // SOTM driver.getRightMenuButton() 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 401726f5..f2936cd6 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 @@ -142,7 +142,7 @@ private boolean isAlignedY() { } private boolean isAlignedTheta() { - return Math.abs(targetPose.get().getRotation().minus(swerve.getPose().getRotation()).getRadians()) < thetaTolerance.doubleValue(); + return Math.abs(targetPose.get().getRotation().minus(swerve.getPose().getRotation()).getDegrees()) < thetaTolerance.doubleValue(); } private boolean isAligned() { @@ -164,6 +164,8 @@ public void execute() { SmartDashboard.putNumber("Alignment/Target y", targetPose.get().getY()); SmartDashboard.putNumber("Alignment/Target angle", targetPose.get().getRotation().getDegrees()); + SmartDashboard.putNumber("Alignment/Error of Angle Controller)", controller.getError().omegaRadiansPerSecond); + SmartDashboard.putNumber("Alignment/Target Velocity Robot Relative X (m per s)", controller.getOutput().vxMetersPerSecond); SmartDashboard.putNumber("Alignment/Target Velocity Robot Relative Y (m per s)", controller.getOutput().vyMetersPerSecond); SmartDashboard.putNumber("Alignment/Target Angular Velocity (rad per s)", controller.getOutput().omegaRadiansPerSecond); diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index a4abbeda..0eecca1a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -171,8 +171,8 @@ public interface Rotation { double akI = 0.0; double akD = 0.0; - PIDConstants XY = new PIDConstants(6.5, 0, 0.2); - PIDConstants THETA = new PIDConstants(15.0, 0, 0.2); + PIDConstants XY = new PIDConstants(2.2, 0, 0.0); + PIDConstants THETA = new PIDConstants(3, 0, 0.0); // PIDConstants XY = new PIDConstants(3.0, 0.0, 0.2); // PIDConstants THETA = new PIDConstants(13.0, 0.0, 0.5); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index dc6507f7..c83cdbc0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -163,6 +163,8 @@ public void periodic() { SmartDashboard.putNumber("Superstructure/Hood/Raw Motor Encoder Value", hoodMotor.getPosition().getValueAsDouble()); SmartDashboard.putNumber("Current Draws/Hood (amps)", hoodMotor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putBoolean("Prematch Checks/Hood at Top?", getAngle().getDegrees() > 39.0); + } } 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 d34f23d4..ebec6f48 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -116,7 +116,7 @@ public SwerveRequest.RobotCentric getRobotCentricSwerveRequest() { private final SysIdRoutine m_sysIdRoutineChassisTranslation = new SysIdRoutine( new SysIdRoutine.Config( /* This is in meters per second², but SysId only supports "volts per second" */ - Volts.of(1).per(Second), + Volts.of(0.5).per(Second), /* This is in meters per second, but SysId only supports "volts" */ Volts.of(Settings.Swerve.Constraints.MAX_VELOCITY_M_PER_S), null, // Use default timeout (10 s) @@ -126,7 +126,7 @@ public SwerveRequest.RobotCentric getRobotCentricSwerveRequest() { new SysIdRoutine.Mechanism( output -> { /* output is actually meters per second, but SysId only supports "volts" */ - setControl(getFieldCentricSwerveRequest().withVelocityX(output.in(Volts)).withVelocityY(0).withRotationalRate(0)); + setControl(getRobotCentricSwerveRequest().withVelocityX(output.in(Volts)).withVelocityY(0).withRotationalRate(0)); /* also log the requested output for SysId */ SignalLogger.writeDouble("Target X Velocity ('voltage')", output.in(Volts)); SignalLogger.writeDouble("X Position", getPose().getX()); @@ -183,7 +183,7 @@ public SwerveRequest.RobotCentric getRobotCentricSwerveRequest() { ); /* The SysId routine to test */ - private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineModuleTranslation; + private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineChassisTranslation; /** * Constructs a CTRE SwerveDrivetrain using the specified constants. From bad9691f5c75bccac65eda1a1de717db3406ec87 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Tue, 10 Mar 2026 19:34:08 -0400 Subject: [PATCH 144/150] FIX: Autos --- simgui.json | 1 + src/main/deploy/pathplanner/paths/Box 1.path | 4 +-- src/main/deploy/pathplanner/paths/Box 2.path | 4 +-- src/main/deploy/pathplanner/paths/Box 3.path | 4 +-- src/main/deploy/pathplanner/paths/Box 4.path | 4 +-- .../paths/Depot To Tower Left.path | 4 +-- .../pathplanner/paths/Left Bump To Depot.path | 4 +-- .../pathplanner/paths/Left NZ To Score.path | 26 ++++++++--------- .../paths/Left NZ To Tower Left.path | 16 +++++------ .../paths/Left Score Score Tower.path | 16 +++++------ .../paths/Left Score To Score.path | 28 +++++++++---------- .../Left Trench Score To Tower Left.path | 14 +++++----- .../pathplanner/paths/Left Trench To NZ.path | 20 ++++++------- .../pathplanner/paths/Right NZ To Score.path | 28 +++++++++---------- .../paths/Right NZ To Tower Right.path | 8 +++--- .../paths/Right Score Score Tower.path | 8 +++--- .../paths/Right Score To Score.path | 8 +++--- .../Right Trench Score To Tower Right.path | 14 +++++----- .../pathplanner/paths/Right Trench To NZ.path | 22 +++++++-------- .../pathplanner/paths/Straight Line.path | 4 +-- src/main/deploy/pathplanner/settings.json | 4 +-- .../com/stuypulse/robot/RobotContainer.java | 5 ++++ .../commands/auton/regular/LeftTwoCycle.java | 8 ++++-- .../commands/auton/regular/RightTwoCycle.java | 10 ++++--- 24 files changed, 137 insertions(+), 127 deletions(-) diff --git a/simgui.json b/simgui.json index c9f35a38..6a90a0ba 100644 --- a/simgui.json +++ b/simgui.json @@ -9,6 +9,7 @@ "/SmartDashboard/PathPlanner": "Alerts", "/SmartDashboard/Robot/Handoff Reverse": "Command", "/SmartDashboard/Robot/Intake Reverse": "Command", + "/SmartDashboard/Robot/Ryan Testing Seed Hood Encoder (NEW)": "Command", "/SmartDashboard/Robot/Seed Hood Relative Encoder At Upper Hardstop": "Command", "/SmartDashboard/Robot/Seed Turret": "Command", "/SmartDashboard/Robot/Spindexer Reverse": "Command", diff --git a/src/main/deploy/pathplanner/paths/Box 1.path b/src/main/deploy/pathplanner/paths/Box 1.path index 2887ab86..d8930a67 100644 --- a/src/main/deploy/pathplanner/paths/Box 1.path +++ b/src/main/deploy/pathplanner/paths/Box 1.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.93, - "maxAcceleration": 15.0, + "maxVelocity": 2.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Box 2.path b/src/main/deploy/pathplanner/paths/Box 2.path index ee818335..8ac93dd7 100644 --- a/src/main/deploy/pathplanner/paths/Box 2.path +++ b/src/main/deploy/pathplanner/paths/Box 2.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.93, - "maxAcceleration": 15.0, + "maxVelocity": 2.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Box 3.path b/src/main/deploy/pathplanner/paths/Box 3.path index 82070849..3bfc33d1 100644 --- a/src/main/deploy/pathplanner/paths/Box 3.path +++ b/src/main/deploy/pathplanner/paths/Box 3.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.93, - "maxAcceleration": 15.0, + "maxVelocity": 2.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Box 4.path b/src/main/deploy/pathplanner/paths/Box 4.path index ff63e346..e38a13d5 100644 --- a/src/main/deploy/pathplanner/paths/Box 4.path +++ b/src/main/deploy/pathplanner/paths/Box 4.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.93, - "maxAcceleration": 15.0, + "maxVelocity": 2.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Depot To Tower Left.path b/src/main/deploy/pathplanner/paths/Depot To Tower Left.path index 3b896d57..a86d903e 100644 --- a/src/main/deploy/pathplanner/paths/Depot To Tower Left.path +++ b/src/main/deploy/pathplanner/paths/Depot To Tower Left.path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.93, - "maxAcceleration": 15.0, + "maxVelocity": 2.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Left Bump To Depot.path b/src/main/deploy/pathplanner/paths/Left Bump To Depot.path index 6748ccf8..7d2e13cb 100644 --- a/src/main/deploy/pathplanner/paths/Left Bump To Depot.path +++ b/src/main/deploy/pathplanner/paths/Left Bump To Depot.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.93, - "maxAcceleration": 15.0, + "maxVelocity": 2.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 33a3edbd..0e26724e 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 8.259240806642943, - "y": 4.43846975088968 + "x": 8.231184022824536, + "y": 4.6366476462196875 }, "prevControl": null, "nextControl": { - "x": 6.376381969157771, - "y": 7.612431791221827 + "x": 7.0796433666191145, + "y": 7.237318116975751 }, "isLocked": false, "linkedName": "NZ Left Center" }, { "anchor": { - "x": 4.020118623962041, - "y": 7.590913404507711 + "x": 3.52151212553495, + "y": 7.547845934379458 }, "prevControl": { - "x": 6.462455516014234, - "y": 7.6985053380782915 + "x": 6.619852845367487, + "y": 7.647103713894571 }, "nextControl": null, "isLocked": false, @@ -33,12 +33,12 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.93, - "maxAcceleration": 15.0, + "maxVelocity": 2.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { "velocity": 0, @@ -47,8 +47,8 @@ "reversed": false, "folder": "To Score", "idealStartingState": { - "velocity": 4.93, + "velocity": 0.0, "rotation": -90.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path b/src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path index b2377a9d..0b6065d3 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.259240806642943, - "y": 4.43846975088968 + "x": 8.231184022824536, + "y": 4.6366476462196875 }, "prevControl": null, "nextControl": { - "x": 6.161198102016607, - "y": 7.515599051008303 + "x": 6.1331413181982, + "y": 7.713776946338313 }, "isLocked": false, "linkedName": "NZ Left Center" @@ -54,12 +54,12 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.93, - "maxAcceleration": 15.0, + "maxVelocity": 2.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { "velocity": 0, @@ -68,7 +68,7 @@ "reversed": false, "folder": "To Tower", "idealStartingState": { - "velocity": 4.93, + "velocity": 0.0, "rotation": -90.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Left Score Score Tower.path b/src/main/deploy/pathplanner/paths/Left Score Score Tower.path index 7391671f..dcbf7d18 100644 --- a/src/main/deploy/pathplanner/paths/Left Score Score Tower.path +++ b/src/main/deploy/pathplanner/paths/Left Score Score Tower.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.020118623962041, - "y": 7.590913404507711 + "x": 3.52151212553495, + "y": 7.547845934379458 }, "prevControl": null, "nextControl": { - "x": 6.376381969162431, - "y": 7.5909134045129925 + "x": 5.877775470735339, + "y": 7.547845934384739 }, "isLocked": false, "linkedName": "Left Trench Score" @@ -110,12 +110,12 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.93, - "maxAcceleration": 15.0, + "maxVelocity": 2.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { "velocity": 0.0, @@ -124,7 +124,7 @@ "reversed": false, "folder": "To Tower", "idealStartingState": { - "velocity": 4.93, + "velocity": 0.0, "rotation": -90.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path index b700551a..e3019e3b 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 4.020118623962041, - "y": 7.590913404507711 + "x": 3.52151212553495, + "y": 7.547845934379458 }, "prevControl": null, "nextControl": { - "x": 6.376381969162431, - "y": 7.5909134045129925 + "x": 6.613851640513552, + "y": 7.754864479315264 }, "isLocked": false, "linkedName": "Left Trench Score" }, { "anchor": { - "x": 5.924495848161329, - "y": 6.3858837485172 + "x": 6.1351212553495005, + "y": 6.357489300998574 }, "prevControl": { - "x": 5.981334656466404, - "y": 7.488556629635666 + "x": 6.252261841806642, + "y": 7.741022388272439 }, "nextControl": { - "x": 5.870699881376039, - "y": 5.3422419928825615 + "x": 6.035218845133962, + "y": 5.177554197274526 }, "isLocked": false, "linkedName": null @@ -90,19 +90,19 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.93, - "maxAcceleration": 15.0, + "maxVelocity": 2.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { "velocity": 0.0, "rotation": 90.0 }, "reversed": false, - "folder": "To NZ", + "folder": "To Score", "idealStartingState": { "velocity": 0.0, "rotation": -90.0 diff --git a/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path b/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path index 7e8b83d5..e5ca1cc2 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path +++ b/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.020118623962041, - "y": 7.590913404507711 + "x": 3.52151212553495, + "y": 7.547845934379458 }, "prevControl": null, "nextControl": { - "x": 1.642336892052195, - "y": 7.655468564650059 + "x": 1.1437303936251038, + "y": 7.612401094521806 }, "isLocked": false, "linkedName": "Left Trench Score" @@ -38,12 +38,12 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.93, - "maxAcceleration": 15.0, + "maxVelocity": 2.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path index 52bcaaff..cbc223fc 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 8.646571767497036, - "y": 7.483321470937129 + "x": 9.162767475035661, + "y": 7.651355206847361 }, "isLocked": false, "linkedName": "Left Trench Start" }, { "anchor": { - "x": 8.259240806642943, - "y": 4.43846975088968 + "x": 8.231184022824536, + "y": 4.6366476462196875 }, "prevControl": { - "x": 8.302277580071175, - "y": 7.8060972716488735 + "x": 8.20530670470756, + "y": 6.706833095577747 }, "nextControl": null, "isLocked": false, @@ -33,15 +33,15 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.93, - "maxAcceleration": 15.0, + "maxVelocity": 2.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { - "velocity": 4.93, + "velocity": 0.0, "rotation": -90.0 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index 2b325c07..598f2629 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 8.28075919335706, - "y": 3.6638078291814944 + "x": 8.282938659058486, + "y": 3.4333523537803137 }, "prevControl": null, "nextControl": { - "x": 6.634602609727167, - "y": 0.5221233689205218 + "x": 8.347631954350925, + "y": 1.0008844507845942 }, "isLocked": false, - "linkedName": null + "linkedName": "Right Center NZ" }, { "anchor": { - "x": 4.461245551601425, - "y": 0.5006049822064051 + "x": 3.508573466476462, + "y": 0.44452211126961516 }, "prevControl": { - "x": 6.656120996441283, - "y": 0.4683274021352317 + "x": 7.299600570613409, + "y": 0.4574607703281026 }, "nextControl": null, "isLocked": false, @@ -33,12 +33,12 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.93, - "maxAcceleration": 15.0, + "maxVelocity": 2.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { "velocity": 0.0, @@ -47,8 +47,8 @@ "reversed": false, "folder": "To Score", "idealStartingState": { - "velocity": 4.93, + "velocity": 0.0, "rotation": 90.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path b/src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path index 8f9b222f..3611ebc6 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path @@ -54,12 +54,12 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.93, - "maxAcceleration": 15.0, + "maxVelocity": 2.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { "velocity": 0, @@ -68,7 +68,7 @@ "reversed": false, "folder": "To Tower", "idealStartingState": { - "velocity": 4.93, + "velocity": 0.0, "rotation": 90.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Right Score Score Tower.path b/src/main/deploy/pathplanner/paths/Right Score Score Tower.path index 200a1482..08c50d19 100644 --- a/src/main/deploy/pathplanner/paths/Right Score Score Tower.path +++ b/src/main/deploy/pathplanner/paths/Right Score Score Tower.path @@ -110,15 +110,15 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.93, - "maxAcceleration": 15.0, + "maxVelocity": 2.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { - "velocity": 4.93, + "velocity": 0.0, "rotation": 0.0 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index 30dbaf24..1e20d396 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -90,19 +90,19 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.93, - "maxAcceleration": 15.0, + "maxVelocity": 2.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { "velocity": 0.0, "rotation": -90.0 }, "reversed": false, - "folder": "To NZ", + "folder": "To Score", "idealStartingState": { "velocity": 0, "rotation": 90.0 diff --git a/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path b/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path index 793f7b1a..38a11739 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path +++ b/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.461245551601425, - "y": 0.5006049822064051 + "x": 3.508573466476462, + "y": 0.44452211126961516 }, "prevControl": null, "nextControl": { - "x": 1.41044404752203, - "y": 0.712825661260673 + "x": 0.45777196239706797, + "y": 0.6567427903238829 }, "isLocked": false, "linkedName": "Right Trench Score" @@ -38,12 +38,12 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.93, - "maxAcceleration": 15.0, + "maxVelocity": 2.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index 1e141bb1..38c981b4 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -8,24 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 8.74821391703567, - "y": 0.5681744021261762 + "x": 9.188644793152639, + "y": 0.48333808844507875 }, "isLocked": false, "linkedName": "Right Trench Start" }, { "anchor": { - "x": 8.28075919335706, - "y": 3.6638078291814944 + "x": 8.282938659058486, + "y": 3.4333523537803137 }, "prevControl": { - "x": 8.205444839857652, - "y": 0.371494661921707 + "x": 8.153552068473608, + "y": 1.182025677603424 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Right Center NZ" } ], "rotationTargets": [], @@ -33,15 +33,15 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.93, - "maxAcceleration": 15.0, + "maxVelocity": 2.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { - "velocity": 4.93, + "velocity": 0.0, "rotation": 90.0 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Straight Line.path b/src/main/deploy/pathplanner/paths/Straight Line.path index ad031ac1..f77ecd7b 100644 --- a/src/main/deploy/pathplanner/paths/Straight Line.path +++ b/src/main/deploy/pathplanner/paths/Straight Line.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.93, - "maxAcceleration": 15.0, + "maxVelocity": 2.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index f5934b40..23f131a1 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -12,8 +12,8 @@ "autoFolders": [ "Tests" ], - "defaultMaxVel": 4.93, - "defaultMaxAccel": 15.0, + "defaultMaxVel": 2.0, + "defaultMaxAccel": 5.0, "defaultMaxAngVel": 300.0, "defaultMaxAngAccel": 900.0, "defaultNominalVoltage": 12.0, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 90e9428c..a46250a3 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -58,6 +58,7 @@ import com.stuypulse.robot.commands.turret.ZeroTurret; import com.stuypulse.robot.commands.vision.ResetLimelightIMU; import com.stuypulse.robot.commands.vision.SetIMUMode; +import com.stuypulse.robot.commands.vision.SetMegaTagMode; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.subsystems.handoff.Handoff; @@ -73,6 +74,7 @@ import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.subsystems.vision.LimelightVision; +import com.stuypulse.robot.subsystems.vision.LimelightVision.MegaTagMode; import com.stuypulse.robot.util.PathUtil.AutonConfig; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; @@ -140,6 +142,9 @@ public RobotContainer() { SmartDashboard.putData("Robot/Seed Turret", new SeedTurret().ignoringDisable(true)); SmartDashboard.putData("Robot/Seed Hood Relative Encoder At Upper Hardstop", new SeedHoodRelativeEncoderAtUpperHardstop().ignoringDisable(true)); SmartDashboard.putData("Robot/Ryan Testing Seed Hood Encoder (NEW)", new NewZeroAtUpperHardstop()); + SmartDashboard.putData("Robot/Set Megatag 1", new SetMegaTagMode(MegaTagMode.MEGATAG1).ignoringDisable(true)); + SmartDashboard.putData("Robot/Set Megatag 2", new SetMegaTagMode(MegaTagMode.MEGATAG2).ignoringDisable(true)); + SmartDashboard.putData("Robot/Handoff Reverse", diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java index 87ebf450..de66ebe4 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java @@ -27,13 +27,15 @@ public LeftTwoCycle(PathPlannerPath... paths) { addCommands( // NZ Trip 1 - new IntakeDeploy().alongWith( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.5).andThen(new IntakeDeploy()) ), new SuperstructureInterpolation().alongWith(new IntakeStopRollers()), // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new WaitCommand(0.5).andThen(new SuperstructureInterpolation()) + ), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new HandoffRun().alongWith(new WaitUntilCommand(() -> Handoff.getInstance().atTolerance())).andThen( new SpindexerRun() diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java index b6218af7..7d908669 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java @@ -27,13 +27,15 @@ public RightTwoCycle(PathPlannerPath... paths) { addCommands( // NZ Trip 1 - new IntakeDeploy().alongWith( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.5).andThen(new IntakeDeploy()) ), - new SuperstructureInterpolation().alongWith(new IntakeStopRollers()), + new IntakeStopRollers(), // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new WaitCommand(0.5).andThen(new SuperstructureInterpolation()) + ), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new HandoffRun().alongWith(new WaitUntilCommand(() -> Handoff.getInstance().atTolerance())).andThen( new SpindexerRun() From c97ff42cbe7a0870336857d6f7e8315afd58d9bd Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Tue, 10 Mar 2026 23:11:40 -0400 Subject: [PATCH 145/150] CLEAN: cleanup, remove dead code --- .../com/stuypulse/robot/RobotContainer.java | 37 +--------- .../handoff/HandoffConditionalCommand.java | 25 ------- .../handoff/HandoffDefaultCommand.java | 53 -------------- .../commands/hood/NewZeroAtUpperHardstop.java | 23 ------ .../hood/ZeroHoodEncoderAtUpperHardstop.java | 21 ------ .../SpindexerConditionalCommand.java | 25 ------- .../spindexer/SpindexerDefaultCommand.java | 55 -------------- .../SuperstructureSOTMConditional.java | 15 ---- .../commands/swerve/SwerveResetHeading.java | 5 -- .../swerve/climbAlign/SwerveClimbAlign.java | 16 ---- .../climbAlign/SwerveClimbAlignBot.java | 23 ------ .../climbAlign/SwerveClimbAlignTop.java | 25 ------- .../commands/turret/TurretDefaultCommand.java | 36 --------- .../robot/constants/DriverConstants.java | 4 +- .../com/stuypulse/robot/constants/Gains.java | 73 ++----------------- .../subsystems/spindexer/SpindexerImpl.java | 6 -- .../superstructure/Superstructure.java | 2 - .../superstructure/hood/HoodImpl.java | 40 +++++----- .../superstructure/turret/TurretImpl.java | 1 - .../subsystems/vision/LimelightVision.java | 8 -- 20 files changed, 32 insertions(+), 461 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/commands/handoff/HandoffConditionalCommand.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/handoff/HandoffDefaultCommand.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/hood/NewZeroAtUpperHardstop.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/hood/ZeroHoodEncoderAtUpperHardstop.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDefaultCommand.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTMConditional.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlign.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlignBot.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlignTop.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index a46250a3..baeffd33 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -5,24 +5,14 @@ /***************************************************************/ package com.stuypulse.robot; -import java.util.concurrent.locks.Condition; - import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; import com.stuypulse.robot.commands.auton.regular.DepotAuton; -import com.stuypulse.robot.commands.auton.regular.EightFuel; -import com.stuypulse.robot.commands.auton.regular.LeftOneCycle; import com.stuypulse.robot.commands.auton.regular.LeftTwoCycle; -import com.stuypulse.robot.commands.auton.regular.RightOneCycle; -import com.stuypulse.robot.commands.handoff.HandoffConditionalCommand; -import com.stuypulse.robot.commands.handoff.HandoffDefaultCommand; -// import com.stuypulse.robot.commands.handoff.HandoffDefaultCommand; import com.stuypulse.robot.commands.handoff.HandoffReverse; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.hood.NewZeroAtUpperHardstop; import com.stuypulse.robot.commands.hood.SeedHoodRelativeEncoderAtUpperHardstop; -import com.stuypulse.robot.commands.hood.ZeroHoodEncoderAtUpperHardstop; import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.intake.IntakeOuttake; import com.stuypulse.robot.commands.intake.IntakeRunRollers; @@ -31,30 +21,22 @@ import com.stuypulse.robot.commands.intake.IntakeStow; import com.stuypulse.robot.commands.intake.ZeroPivotDeployed; import com.stuypulse.robot.commands.intake.ZeroPivotStowed; -import com.stuypulse.robot.commands.spindexer.SpindexerConditionalCommand; -import com.stuypulse.robot.commands.spindexer.SpindexerDefaultCommand; -// import com.stuypulse.robot.commands.spindexer.SpindexerDefaultCommand; import com.stuypulse.robot.commands.spindexer.SpindexerReverse; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; -import com.stuypulse.robot.commands.superstructure.SuperstructureFerry; import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureKB; import com.stuypulse.robot.commands.superstructure.SuperstructureLeftCorner; import com.stuypulse.robot.commands.superstructure.SuperstructureRightCorner; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; -import com.stuypulse.robot.commands.superstructure.SuperstructureShoot; import com.stuypulse.robot.commands.superstructure.SuperstructureStow; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveFOTM; import com.stuypulse.robot.commands.swerve.SwerveDriveSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; -import com.stuypulse.robot.commands.swerve.SwerveWheelRadiusCharacterization; import com.stuypulse.robot.commands.swerve.SwerveXMode; -import com.stuypulse.robot.commands.swerve.pidToPose.SwerveDrivePIDToPose; import com.stuypulse.robot.commands.turret.SeedTurret; -import com.stuypulse.robot.commands.turret.TurretLeftCorner; import com.stuypulse.robot.commands.turret.ZeroTurret; import com.stuypulse.robot.commands.vision.ResetLimelightIMU; import com.stuypulse.robot.commands.vision.SetIMUMode; @@ -80,8 +62,6 @@ import com.stuypulse.stuylib.input.gamepads.AutoGamepad; import com.stuypulse.stuylib.network.SmartBoolean; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -89,7 +69,6 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; @@ -141,12 +120,9 @@ public RobotContainer() { SmartDashboard.putData("Robot/Zero Turret Encoders", new ZeroTurret().ignoringDisable(true)); SmartDashboard.putData("Robot/Seed Turret", new SeedTurret().ignoringDisable(true)); SmartDashboard.putData("Robot/Seed Hood Relative Encoder At Upper Hardstop", new SeedHoodRelativeEncoderAtUpperHardstop().ignoringDisable(true)); - SmartDashboard.putData("Robot/Ryan Testing Seed Hood Encoder (NEW)", new NewZeroAtUpperHardstop()); SmartDashboard.putData("Robot/Set Megatag 1", new SetMegaTagMode(MegaTagMode.MEGATAG1).ignoringDisable(true)); SmartDashboard.putData("Robot/Set Megatag 2", new SetMegaTagMode(MegaTagMode.MEGATAG2).ignoringDisable(true)); - - SmartDashboard.putData("Robot/Handoff Reverse", new ConditionalCommand( new HandoffReverse().andThen(new WaitCommand(0.25)).andThen(new HandoffRun()), @@ -306,11 +282,6 @@ public void configureAutons() { autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); - // BASE - // AutonConfig EIGHT_FUEL = new AutonConfig("Eight Fuel", EightFuel::new, - // ""); - // EIGHT_FUEL.register(autonChooser); - // DEPOT AutonConfig DEPOT_AUTON = new AutonConfig("Depot Auton", DepotAuton::new, "Left Bump To Depot", "Depot To Tower Left"); @@ -340,10 +311,10 @@ public void configureAutons() { public void configureSysids() { - autonChooser.addOption("SysID Module Translation Dynamic Forwards", swerve.sysIdDynamic(Direction.kForward)); - autonChooser.addOption("SysID Module Translation Dynamic Backwards", swerve.sysIdDynamic(Direction.kReverse)); - autonChooser.addOption("SysID Module Translation Quasi Forwards", swerve.sysIdQuasistatic(Direction.kForward)); - autonChooser.addOption("SysID Module Translation Quasi Backwards", swerve.sysIdQuasistatic(Direction.kReverse)); + // autonChooser.addOption("SysID Module Translation Dynamic Forwards", swerve.sysIdDynamic(Direction.kForward)); + // autonChooser.addOption("SysID Module Translation Dynamic Backwards", swerve.sysIdDynamic(Direction.kReverse)); + // autonChooser.addOption("SysID Module Translation Quasi Forwards", swerve.sysIdQuasistatic(Direction.kForward)); + // autonChooser.addOption("SysID Module Translation Quasi Backwards", swerve.sysIdQuasistatic(Direction.kReverse)); // autonChooser.addOption("SysID Rotation Translation Dynamic Forwards", swerve.sysidRotationDynamic(Direction.kForward)); // autonChooser.addOption("SysID Rotation Translation Dynamic Backwards", swerve.sysidRotationDynamic(Direction.kReverse)); diff --git a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffConditionalCommand.java b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffConditionalCommand.java deleted file mode 100644 index 35687603..00000000 --- a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffConditionalCommand.java +++ /dev/null @@ -1,25 +0,0 @@ -/************************ 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.commands.handoff; - -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; - -public class HandoffConditionalCommand extends ConditionalCommand { - public HandoffConditionalCommand() { - super( - new HandoffStop(), - new HandoffRun(), - () -> { - CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); - Superstructure superstructure = Superstructure.getInstance(); - return swerve.isBehindTower() || swerve.isBehindHub() || superstructure.isTurretWrapping(); - } - ); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffDefaultCommand.java deleted file mode 100644 index 776d2abe..00000000 --- a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffDefaultCommand.java +++ /dev/null @@ -1,53 +0,0 @@ -/************************ 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.commands.handoff; - -import com.stuypulse.robot.subsystems.handoff.Handoff; -import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; -import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj2.command.Command; - -public class HandoffDefaultCommand extends Command { - private final Handoff handoff; - private final Superstructure superstructure; - private final CommandSwerveDrivetrain swerve; - - public HandoffDefaultCommand() { - this.handoff = Handoff.getInstance(); - this.superstructure = Superstructure.getInstance(); - this.swerve = CommandSwerveDrivetrain.getInstance(); - - addRequirements(handoff); - } - - @Override - public void execute() { - boolean shouldStop = swerve.isBehindTower() - || swerve.isBehindHub() - || swerve.isUnderTrench() - || superstructure.isTurretWrapping(); - - boolean prerequisiteMet = superstructure.atTolerance(); - - boolean shouldRun = !shouldStop && prerequisiteMet; - - if (!shouldRun) { - handoff.setState(HandoffState.STOP); - } else if (shouldRun && (superstructure.getState() == SuperstructureState.SOTM || superstructure.getState() == SuperstructureState.FOTM)) { - handoff.setState(HandoffState.FORWARD); - } - } - - @Override - public boolean runsWhenDisabled() { - return false; - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/hood/NewZeroAtUpperHardstop.java b/src/main/java/com/stuypulse/robot/commands/hood/NewZeroAtUpperHardstop.java deleted file mode 100644 index e8536c50..00000000 --- a/src/main/java/com/stuypulse/robot/commands/hood/NewZeroAtUpperHardstop.java +++ /dev/null @@ -1,23 +0,0 @@ -package com.stuypulse.robot.commands.hood; - - - -import com.stuypulse.robot.subsystems.superstructure.hood.Hood; - -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class NewZeroAtUpperHardstop extends InstantCommand { - private final Hood hood; - - public NewZeroAtUpperHardstop() { - this.hood = Hood.getInstance(); - - addRequirements(hood); - } - - @Override - public void initialize() { - hood.seedHoodAtUpperHardStop(); - hood.zeroHoodEncodersAfterSeed(); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/hood/ZeroHoodEncoderAtUpperHardstop.java b/src/main/java/com/stuypulse/robot/commands/hood/ZeroHoodEncoderAtUpperHardstop.java deleted file mode 100644 index 926739e9..00000000 --- a/src/main/java/com/stuypulse/robot/commands/hood/ZeroHoodEncoderAtUpperHardstop.java +++ /dev/null @@ -1,21 +0,0 @@ -package com.stuypulse.robot.commands.hood; - -import com.stuypulse.robot.subsystems.superstructure.hood.Hood; - -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class ZeroHoodEncoderAtUpperHardstop extends InstantCommand{ - private final Hood hood; - - public ZeroHoodEncoderAtUpperHardstop() { - hood = Hood.getInstance(); - - addRequirements(hood); - } - - @Override - public void initialize() { - hood.zeroHoodEncoderAtUpperHardstop(); - hood.seedHood(); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java deleted file mode 100644 index 16a53441..00000000 --- a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerConditionalCommand.java +++ /dev/null @@ -1,25 +0,0 @@ -/************************ 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.commands.spindexer; - -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; - -public class SpindexerConditionalCommand extends ConditionalCommand { - public SpindexerConditionalCommand() { - super( - new SpindexerStop(), - new SpindexerRun(), - () -> { - CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); - Superstructure superstructure = Superstructure.getInstance(); - return swerve.isBehindTower() || swerve.isBehindHub() || superstructure.isTurretWrapping(); - } - ); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDefaultCommand.java deleted file mode 100644 index 01cb203d..00000000 --- a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDefaultCommand.java +++ /dev/null @@ -1,55 +0,0 @@ -/************************ 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.commands.spindexer; - -import com.stuypulse.robot.subsystems.handoff.Handoff; -import com.stuypulse.robot.subsystems.spindexer.Spindexer; -import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj2.command.Command; - -public class SpindexerDefaultCommand extends Command { - private final Spindexer spindexer; - private final Handoff handoff; - private final Superstructure superstructure; - private final CommandSwerveDrivetrain swerve; - - public SpindexerDefaultCommand() { - this.spindexer = Spindexer.getInstance(); - this.handoff = Handoff.getInstance(); - this.superstructure = Superstructure.getInstance(); - this.swerve = CommandSwerveDrivetrain.getInstance(); - - addRequirements(spindexer); - } - - @Override - public void execute() { - boolean shouldStop = swerve.isBehindTower() - || swerve.isBehindHub() - || swerve.isUnderTrench() - || superstructure.isTurretWrapping(); - - boolean prerequisitesMet = handoff.atTolerance() && superstructure.atTolerance(); - - boolean shouldRun = !shouldStop && prerequisitesMet; - - if (!shouldRun) { - spindexer.setState(SpindexerState.STOP); - } else if (shouldRun && (superstructure.getState() == SuperstructureState.SOTM || superstructure.getState() == SuperstructureState.FOTM)) { - spindexer.setState(SpindexerState.FORWARD); - } - } - - @Override - public boolean runsWhenDisabled() { - return false; - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTMConditional.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTMConditional.java deleted file mode 100644 index b735e6e0..00000000 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTMConditional.java +++ /dev/null @@ -1,15 +0,0 @@ -package com.stuypulse.robot.commands.superstructure; - -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; - -public class SuperstructureSOTMConditional extends ConditionalCommand { - public SuperstructureSOTMConditional() { - super( - new SuperstructureInterpolation(), - new SuperstructureSOTM(), - () -> Superstructure.getInstance().getState() == SuperstructureState.SOTM - ); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetHeading.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetHeading.java index b2f10daa..72a623d3 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetHeading.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetHeading.java @@ -14,9 +14,4 @@ public class SwerveResetHeading extends InstantCommand { public SwerveResetHeading() { super(() -> CommandSwerveDrivetrain.getInstance().resetRotation(Rotation2d.kZero)); } - - // @Override - // public void initialize() { - // CommandSwerveDrivetrain.getInstance().getPigeon2().reset(); - // } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlign.java b/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlign.java deleted file mode 100644 index 2cc837de..00000000 --- a/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlign.java +++ /dev/null @@ -1,16 +0,0 @@ -/************************ 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.commands.swerve.climbAlign; - -import com.stuypulse.robot.constants.Field; - -import edu.wpi.first.wpilibj2.command.ConditionalCommand; - -public class SwerveClimbAlign extends ConditionalCommand{ - public SwerveClimbAlign(){ - super(new SwerveClimbAlignTop(), new SwerveClimbAlignBot(), () -> Field.closerToTop()); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlignBot.java b/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlignBot.java deleted file mode 100644 index e7c23408..00000000 --- a/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlignBot.java +++ /dev/null @@ -1,23 +0,0 @@ -/************************ 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.commands.swerve.climbAlign; - -import com.stuypulse.robot.commands.swerve.pidToPose.SwerveDrivePIDToPose; -import com.stuypulse.robot.constants.Field; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; - -public class SwerveClimbAlignBot extends SwerveDrivePIDToPose{ - public SwerveClimbAlignBot(){ - super(new Pose2d(Field.towerFarCenter.getX(), Field.towerFarCenter.getY() - Field.barDisplacement - Field.DISTANCE_TO_RUNGS, new Rotation2d(0))); - } - - @Override - public void execute(){ - super.execute(); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlignTop.java b/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlignTop.java deleted file mode 100644 index afb9c3db..00000000 --- a/src/main/java/com/stuypulse/robot/commands/swerve/climbAlign/SwerveClimbAlignTop.java +++ /dev/null @@ -1,25 +0,0 @@ -/************************ 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.commands.swerve.climbAlign; - -import com.stuypulse.robot.commands.swerve.pidToPose.SwerveDrivePIDToPose; -import com.stuypulse.robot.constants.Field; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; - - -public class SwerveClimbAlignTop extends SwerveDrivePIDToPose{ - public SwerveClimbAlignTop(){ - super(new Pose2d(Field.towerFarCenter.getX(), Field.towerFarCenter.getY() + Field.barDisplacement + Field.DISTANCE_TO_RUNGS, new Rotation2d(Units.degreesToRadians(180)))); - } - - @Override - public void execute(){ - super.execute(); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java deleted file mode 100644 index 11901db9..00000000 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretDefaultCommand.java +++ /dev/null @@ -1,36 +0,0 @@ -package com.stuypulse.robot.commands.turret; - -import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.subsystems.superstructure.turret.Turret; -import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj2.command.Command; - -public class TurretDefaultCommand extends Command { - private final Turret turret; - private final CommandSwerveDrivetrain swerve; - - public TurretDefaultCommand() { - turret = Turret.getInstance(); - swerve = CommandSwerveDrivetrain.getInstance(); - - addRequirements(turret); - } - - @Override - public void execute() { - // === Robot Position Logic === - // Reminder from driver's perspective, positive X to the opposite alliance and positive Y points to the left. - - boolean isInAllianceZone = swerve.getPose().getX() < Field.getHubPose().getX(); - - if (isInAllianceZone) { - turret.setState(TurretState.SHOOT); - } - else { - turret.setState(TurretState.FERRY); - } - } - -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/DriverConstants.java b/src/main/java/com/stuypulse/robot/constants/DriverConstants.java index ccdc7681..c1ef99da 100644 --- a/src/main/java/com/stuypulse/robot/constants/DriverConstants.java +++ b/src/main/java/com/stuypulse/robot/constants/DriverConstants.java @@ -12,14 +12,14 @@ public interface Driver { double BUZZ_INTENSITY = 1.0; public interface Drive { - double DEADBAND = 0.05; //TODO: tune to philip's request + double DEADBAND = 0.05; double RC = 0.05; int POWER = 3; } public interface Turn { double DEADBAND = 0.05; double RC = 0.05; - int POWER = 2; + int POWER = 3; } } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 0eecca1a..b9d3b06c 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -11,57 +11,21 @@ public class Gains { - public interface ClimberHopper { - SmartNumber kP = new SmartNumber("ClimberHopper/Gains/kP", 1.0); - SmartNumber kI = new SmartNumber("ClimberHopper/Gains/kI", 0.0); - SmartNumber kD = new SmartNumber("ClimberHopper/Gains/kD", 0.20); - - SmartNumber kS = new SmartNumber("ClimberHopper/Gains/kS", 0.0); - SmartNumber kV = new SmartNumber("ClimberHopper/Gains/kV", 0.123); - SmartNumber kA = new SmartNumber("ClimberHopper/Gains/kA", 0.0); - - // double kP = 1.0; - // double kI = 0.0; - // double kD = 0.20; - - // double kS = 0.0; - // double kV = 0.123; - // double kA = 0.0; - } - public interface Superstructure { public interface Shooter { - - // SmartNumber kP = new SmartNumber("Superstructure/Shooter/Gains/kP", 10.0); - // SmartNumber kI = new SmartNumber("Superstructure/Shooter/Gains/kI", 0.0); - // SmartNumber kD = new SmartNumber("Superstructure/Shooter/Gains/kD", 0.0); - - // SmartNumber kS = new SmartNumber("Superstructure/Shooter/Gains/kS", 10.32); - // SmartNumber kV = new SmartNumber("Superstructure/Shooter/Gains/kV", 0.0835); - // SmartNumber kA = new SmartNumber("Superstructure/Shooter/Gains/kA", 0.0); - - // BELOW ARE VELOCITY PID AND VOLTAGEBASED FF GAINS SmartNumber kP = new SmartNumber("Superstructure/Shooter/Gains/kP", 0.45); SmartNumber kI = new SmartNumber("Superstructure/Shooter/Gains/kI", 0.0); SmartNumber kD = new SmartNumber("Superstructure/Shooter/Gains/kD", 0.0); SmartNumber kS = new SmartNumber("Superstructure/Shooter/Gains/kS", 0.0); - SmartNumber kV = new SmartNumber("Superstructure/Shooter/Gains/kV", 0.123);//0.11650); + SmartNumber kV = new SmartNumber("Superstructure/Shooter/Gains/kV", 0.123); SmartNumber kA = new SmartNumber("Superstructure/Shooter/Gains/kA", 0.0); - - // double kP = 0.45; - // double kI = 0.0; - // double kD = 0.0; - - // double kS = 0.0; - // double kV = 0.123; - // double kA = 0.0; } public interface Hood { - double kP = 250.0; //300.0; + double kP = 250.0; double kI = 0.0; - double kD = 2.0; //0.5 + double kD = 2.0; double kS = 0.25; double kV = 0.0; @@ -97,20 +61,12 @@ public interface Spindexer { double kD = 10.0; double kS = 0.25; - double kV = 1.2; //0.9413 + double kV = 1.2; double kA = 0.010876; } public interface Intake { public interface Pivot { - // double kP = 100.0; - // double kI = 0.0; - // double kD = 10.0; - - // double kS = 0.0; - // double kV = 0.12; - // double kA = 0.0; - SmartNumber kP = new SmartNumber("Intake/Pivot/Gains/kP", 100.0); SmartNumber kI = new SmartNumber("Intake/Pivot/Gains/kI", 0.0); SmartNumber kD = new SmartNumber("Intake/Pivot/Gains/kD", 10.0); @@ -124,7 +80,7 @@ public interface Pivot { } public interface Handoff { - double kP = 0.00015508; // 0.016973 from sysid + double kP = 0.00015508; double kI = 0.0; double kD = 0.0; @@ -155,27 +111,8 @@ public interface Turn { } public interface Alignment { - public interface Rotation { - double kp = 112.3; - double ki = 0.0; - double kd = 2.3758; - double ks = 0.31395; - double kv = 0.10969; - double ka = 0.026589; - } - - double kP = 0.0; - double kI = 0.0; - double kD = 0.0; - double akP = 0.0; - double akI = 0.0; - double akD = 0.0; - PIDConstants XY = new PIDConstants(2.2, 0, 0.0); PIDConstants THETA = new PIDConstants(3, 0, 0.0); - - // PIDConstants XY = new PIDConstants(3.0, 0.0, 0.2); - // PIDConstants THETA = new PIDConstants(13.0, 0.0, 0.5); } } } 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 1f79deb8..079d1ec6 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -34,8 +34,6 @@ public class SpindexerImpl extends Spindexer { private final TalonFX leadMotor; private final TalonFX followerMotor; - private final Timer timer; - private boolean timerTriggered; private final VelocityVoltage controller; private final Follower follower; @@ -84,10 +82,6 @@ public SpindexerImpl() { followerMotor.setControl(follower); - timer = new Timer(); - - timerTriggered = false; - voltageOverride = Optional.empty(); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index 63a02279..370012ad 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -5,7 +5,6 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure; -import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.subsystems.superstructure.hood.Hood; import com.stuypulse.robot.subsystems.superstructure.hood.Hood.HoodState; import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter; @@ -15,7 +14,6 @@ import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.superstructure.SOTMCalculator; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index c83cdbc0..2456196b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -83,22 +83,6 @@ public Rotation2d getAngle() { return Rotation2d.fromRotations(hoodMotor.getPosition().getValueAsDouble()); } - //TODO: Implementation as of 3/3 but not yet tested - // Should ONLY be called at the lower hardstop! - @Override - public void zeroHoodEncoderAtUpperHardstop() { - hoodEncoder.getConfigurator().refresh(hoodEncoderConfig.getConfiguration().MagnetSensor); - - double currentOffset = hoodEncoderConfig.getConfiguration().MagnetSensor.MagnetOffset; - - double positionWithCurrentOffset = hoodEncoder.getPosition().getValueAsDouble(); - double newOffset = -((positionWithCurrentOffset - currentOffset) - Settings.Superstructure.Hood.Angles.MAX.getRotations()); - - hoodEncoderConfig.withMagnetOffset(newOffset); - - hoodEncoderConfig.configure(hoodEncoder); - } - @Override public boolean isStalling() { return isStalling.getAsBoolean(); @@ -128,12 +112,14 @@ public void periodic() { super.periodic(); if (!hasUsedAbsoluteEncoder) { - seedHood(); + // seedHood(); + seedHoodAtUpperHardStop(); hasUsedAbsoluteEncoder = true; } if (isStalling() && getState() == HoodState.HOMING) { - seedHood(); + // seedHood(); + seedHoodAtUpperHardStop(); setState(HoodState.IDLE); SmartDashboard.putBoolean("Superstructure/Hood/SUCCESFULLY HOMED", true); } @@ -186,7 +172,23 @@ public SysIdRoutine getHoodSysIdRoutine() { ); } - + + //TODO: Implementation as of 3/3 but not yet tested + // Should ONLY be called at the lower hardstop! + @Override + public void zeroHoodEncoderAtUpperHardstop() { + hoodEncoder.getConfigurator().refresh(hoodEncoderConfig.getConfiguration().MagnetSensor); + + double currentOffset = hoodEncoderConfig.getConfiguration().MagnetSensor.MagnetOffset; + + double positionWithCurrentOffset = hoodEncoder.getPosition().getValueAsDouble(); + double newOffset = -((positionWithCurrentOffset - currentOffset) - Settings.Superstructure.Hood.Angles.MAX.getRotations()); + + hoodEncoderConfig.withMagnetOffset(newOffset); + + hoodEncoderConfig.configure(hoodEncoder); + } + @Override public void zeroHoodEncodersAfterSeed() { //only use if you are seeded -> might add a boolean to double check that we are in seed at Upper Hardstop ^^ hoodEncoder.getConfigurator().refresh(hoodEncoderConfig.getConfiguration().MagnetSensor); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index 892e9ec4..79839e48 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -160,7 +160,6 @@ public void periodic() { if (!hasUsedAbsoluteEncoder) { seedTurret(); hasUsedAbsoluteEncoder = true; - System.out.println("Absolute Encoder Reset triggered"); //TODO: remove, printing consumes resouces on driver station (source wpilib themselves) } double currentAngle = getAngle().getDegrees(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index cb2f010e..260e18af 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -95,14 +95,6 @@ public void disable() { enabled.set(false); } - // public void setCameraEnabled(String name, boolean enabled) { - // for (int i = 0; i < names.length; i++) { - // if (names[i].equals(name)) { - // camerasEnabled[i].set(enabled); - // } - // } - // } - public void setMegaTagMode(MegaTagMode mode) { this.megaTagMode = mode; } From 3e4fb4300024612d743b31898fda772e3de7637a Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Tue, 10 Mar 2026 23:12:46 -0400 Subject: [PATCH 146/150] CLEAN: spotless --- src/main/java/com/stuypulse/robot/Robot.java | 1 + .../com/stuypulse/robot/RobotContainer.java | 18 +- .../commands/auton/regular/DepotAuton.java | 8 +- .../commands/auton/regular/EightFuel.java | 8 +- .../commands/auton/regular/LeftOneCycle.java | 8 +- .../commands/auton/regular/LeftTwoCycle.java | 10 +- .../commands/auton/regular/RightOneCycle.java | 8 +- .../commands/auton/regular/RightTwoCycle.java | 10 +- .../robot/commands/auton/test/BoxTest.java | 8 +- .../robot/commands/hood/HomingRoutine.java | 5 + .../robot/commands/hood/HoodAnalog.java | 8 +- ...eedHoodRelativeEncoderAtUpperHardstop.java | 5 + .../robot/commands/intake/IntakeHoming.java | 5 + .../commands/intake/ZeroPivotDeployed.java | 5 + .../commands/intake/ZeroPivotStowed.java | 5 + .../swerve/SwerveDriveAlignTurretToHub.java | 14 +- .../commands/swerve/SwerveDriveFOTM.java | 21 +- .../commands/swerve/SwerveDriveSOTM.java | 21 +- .../SwerveWheelRadiusCharacterization.java | 5 + .../robot/commands/swerve/SwerveXMode.java | 8 +- .../robot/commands/turret/TurretAnalog.java | 3 +- .../commands/vision/SetIMUAssistValue.java | 5 + .../stuypulse/robot/constants/Cameras.java | 1 - .../com/stuypulse/robot/constants/Field.java | 1 - .../stuypulse/robot/constants/Settings.java | 5 +- .../robot/subsystems/handoff/Handoff.java | 1 - .../robot/subsystems/handoff/HandoffImpl.java | 5 +- .../robot/subsystems/intake/IntakeImpl.java | 27 +- .../robot/subsystems/spindexer/Spindexer.java | 1 + .../subsystems/spindexer/SpindexerImpl.java | 7 +- .../superstructure/Superstructure.java | 294 +++++++++--------- .../subsystems/superstructure/hood/Hood.java | 3 +- .../superstructure/hood/HoodImpl.java | 29 +- .../superstructure/shooter/Shooter.java | 2 +- .../swerve/CommandSwerveDrivetrain.java | 35 ++- .../subsystems/vision/LimelightVision.java | 3 +- .../com/stuypulse/robot/util/FMSUtil.java | 9 +- .../InterpolationCalculator.java | 5 + .../util/superstructure/SOTMCalculator.java | 9 +- 39 files changed, 371 insertions(+), 255 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index d0c01474..ad54144d 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -9,6 +9,7 @@ import com.stuypulse.robot.commands.vision.SetMegaTagMode; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.vision.LimelightVision; + import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index baeffd33..bc90e9f4 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -5,10 +5,14 @@ /***************************************************************/ package com.stuypulse.robot; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.input.gamepads.AutoGamepad; +import com.stuypulse.stuylib.network.SmartBoolean; + import com.stuypulse.robot.commands.auton.DoNothingAuton; -import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; import com.stuypulse.robot.commands.auton.regular.DepotAuton; import com.stuypulse.robot.commands.auton.regular.LeftTwoCycle; +import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; import com.stuypulse.robot.commands.handoff.HandoffReverse; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; @@ -45,22 +49,19 @@ import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.intake.Intake.RollerState; +import com.stuypulse.robot.subsystems.spindexer.Spindexer; +import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; import com.stuypulse.robot.subsystems.superstructure.hood.Hood; import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter; import com.stuypulse.robot.subsystems.superstructure.turret.Turret; -import com.stuypulse.robot.subsystems.intake.Intake; -import com.stuypulse.robot.subsystems.intake.Intake.RollerState; -import com.stuypulse.robot.subsystems.spindexer.Spindexer; -import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.subsystems.vision.LimelightVision; import com.stuypulse.robot.subsystems.vision.LimelightVision.MegaTagMode; import com.stuypulse.robot.util.PathUtil.AutonConfig; -import com.stuypulse.stuylib.input.Gamepad; -import com.stuypulse.stuylib.input.gamepads.AutoGamepad; -import com.stuypulse.stuylib.network.SmartBoolean; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -69,7 +70,6 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; public class RobotContainer { diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java index e5c0d484..0cbe7062 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java @@ -1,6 +1,10 @@ +/************************ 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.commands.auton.regular; -import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.intake.IntakeStow; @@ -11,6 +15,8 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import com.pathplanner.lib.path.PathPlannerPath; + public class DepotAuton extends SequentialCommandGroup { public DepotAuton(PathPlannerPath... paths) { diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java index 0c25efd1..541e3a30 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java @@ -1,6 +1,10 @@ +/************************ 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.commands.auton.regular; -import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.superstructure.SuperstructureKB; @@ -9,6 +13,8 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import com.pathplanner.lib.path.PathPlannerPath; + public class EightFuel extends SequentialCommandGroup { public EightFuel(PathPlannerPath... paths) { diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java index e7b5d7e2..806244ef 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java @@ -1,6 +1,10 @@ +/************************ 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.commands.auton.regular; -import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.spindexer.SpindexerRun; @@ -12,6 +16,8 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import com.pathplanner.lib.path.PathPlannerPath; + public class LeftOneCycle extends SequentialCommandGroup { public LeftOneCycle(PathPlannerPath... paths) { diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java index de66ebe4..d14fbf90 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java @@ -1,17 +1,19 @@ +/************************ 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.commands.auton.regular; -import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.intake.IntakeRunRollers; 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.superstructure.SuperstructureInterpolation; import com.stuypulse.robot.subsystems.handoff.Handoff; -import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -20,6 +22,8 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import com.pathplanner.lib.path.PathPlannerPath; + public class LeftTwoCycle extends SequentialCommandGroup { public LeftTwoCycle(PathPlannerPath... paths) { diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java index d78b29c5..0a4d5fb2 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java @@ -1,6 +1,10 @@ +/************************ 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.commands.auton.regular; -import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.spindexer.SpindexerRun; @@ -12,6 +16,8 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import com.pathplanner.lib.path.PathPlannerPath; + public class RightOneCycle extends SequentialCommandGroup { public RightOneCycle(PathPlannerPath... paths) { diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java index 7d908669..ef6aa64a 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java @@ -1,17 +1,19 @@ +/************************ 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.commands.auton.regular; -import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.intake.IntakeRunRollers; 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.superstructure.SuperstructureInterpolation; import com.stuypulse.robot.subsystems.handoff.Handoff; -import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -20,6 +22,8 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import com.pathplanner.lib.path.PathPlannerPath; + public class RightTwoCycle extends SequentialCommandGroup { public RightTwoCycle(PathPlannerPath... paths) { diff --git a/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java b/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java index 7a6a4071..1ad35b24 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java @@ -1,10 +1,16 @@ +/************************ 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.commands.auton.test; -import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import com.pathplanner.lib.path.PathPlannerPath; + public class BoxTest extends SequentialCommandGroup { public BoxTest(PathPlannerPath... paths) { diff --git a/src/main/java/com/stuypulse/robot/commands/hood/HomingRoutine.java b/src/main/java/com/stuypulse/robot/commands/hood/HomingRoutine.java index 4caa9ac2..28c32fee 100644 --- a/src/main/java/com/stuypulse/robot/commands/hood/HomingRoutine.java +++ b/src/main/java/com/stuypulse/robot/commands/hood/HomingRoutine.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.commands.hood; import com.stuypulse.robot.subsystems.superstructure.hood.Hood; diff --git a/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java b/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java index d0bcf8b2..ecac6c79 100644 --- a/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java +++ b/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java @@ -1,8 +1,14 @@ +/************************ 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.commands.hood; -import com.stuypulse.robot.subsystems.superstructure.hood.Hood; import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.robot.subsystems.superstructure.hood.Hood; + import edu.wpi.first.wpilibj2.command.Command; public class HoodAnalog extends Command{ diff --git a/src/main/java/com/stuypulse/robot/commands/hood/SeedHoodRelativeEncoderAtUpperHardstop.java b/src/main/java/com/stuypulse/robot/commands/hood/SeedHoodRelativeEncoderAtUpperHardstop.java index 95bcaabb..5a518fa0 100644 --- a/src/main/java/com/stuypulse/robot/commands/hood/SeedHoodRelativeEncoderAtUpperHardstop.java +++ b/src/main/java/com/stuypulse/robot/commands/hood/SeedHoodRelativeEncoderAtUpperHardstop.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.commands.hood; import com.stuypulse.robot.subsystems.superstructure.hood.Hood; diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeHoming.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeHoming.java index f1ccac36..f1f51a83 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeHoming.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeHoming.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.commands.intake; import com.stuypulse.robot.subsystems.intake.Intake.PivotState; diff --git a/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotDeployed.java b/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotDeployed.java index e0902543..ec51b2bc 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotDeployed.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotDeployed.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.commands.intake; import com.stuypulse.robot.subsystems.intake.Intake; diff --git a/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotStowed.java b/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotStowed.java index d39698d4..43a9093c 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotStowed.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotStowed.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.commands.intake; import com.stuypulse.robot.subsystems.intake.Intake; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java index 7c8ff025..b49bfed3 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java @@ -3,21 +3,19 @@ /* 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.commands.swerve; -import com.ctre.phoenix6.swerve.SwerveRequest; -import com.ctre.phoenix6.swerve.SwerveRequest.SwerveDriveBrake; +import com.stuypulse.stuylib.control.angle.AngleController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.streams.angles.filters.AMotionProfile; + import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Gains.Swerve.Alignment; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.superstructure.turret.Turret; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.superstructure.TurretAngleCalculator; -import com.stuypulse.stuylib.control.angle.AngleController; -import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; -import com.stuypulse.stuylib.math.Angle; -import com.stuypulse.stuylib.streams.angles.filters.AMotionProfile; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -25,6 +23,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import com.ctre.phoenix6.swerve.SwerveRequest; + public class SwerveDriveAlignTurretToHub extends Command { private final CommandSwerveDrivetrain swerve; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java index 17cc810b..792d9711 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java @@ -1,12 +1,10 @@ +/************************ 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.commands.swerve; -import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; -import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Settings.Swerve; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.math.Vector2D; @@ -17,7 +15,14 @@ import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; -import edu.wpi.first.math.MathUtil; +import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; +import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java index af3978af..c28bcbe9 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java @@ -1,12 +1,10 @@ +/************************ 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.commands.swerve; -import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; -import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Settings.Swerve; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.math.Vector2D; @@ -17,7 +15,14 @@ import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; -import edu.wpi.first.math.MathUtil; +import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; +import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java index 504e8603..4f480ae0 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.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.commands.swerve; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveXMode.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveXMode.java index f299b20e..fdc7e67e 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveXMode.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveXMode.java @@ -1,10 +1,16 @@ +/************************ 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.commands.swerve; -import com.ctre.phoenix6.swerve.SwerveRequest; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj2.command.Command; +import com.ctre.phoenix6.swerve.SwerveRequest; + public class SwerveXMode extends Command { private CommandSwerveDrivetrain swerve; diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretAnalog.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretAnalog.java index 62569756..3ba3bbcf 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretAnalog.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretAnalog.java @@ -5,9 +5,10 @@ /***************************************************************/ package com.stuypulse.robot.commands.turret; +import com.stuypulse.stuylib.input.Gamepad; + import com.stuypulse.robot.subsystems.superstructure.turret.Turret; import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; -import com.stuypulse.stuylib.input.Gamepad; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/com/stuypulse/robot/commands/vision/SetIMUAssistValue.java b/src/main/java/com/stuypulse/robot/commands/vision/SetIMUAssistValue.java index 864e8a32..39172070 100644 --- a/src/main/java/com/stuypulse/robot/commands/vision/SetIMUAssistValue.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/SetIMUAssistValue.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.commands.vision; import com.stuypulse.robot.subsystems.vision.LimelightVision; diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 261f64a9..56400b19 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -12,7 +12,6 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.Unit; /** This interface stores information about each camera. */ public interface Cameras { diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 9706a57d..5c20aae3 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -15,7 +15,6 @@ 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.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 49784e8a..32e950b7 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -5,8 +5,6 @@ /***************************************************************/ package com.stuypulse.robot.constants; -import com.ctre.phoenix6.CANBus; -import com.pathplanner.lib.path.PathConstraints; import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.network.SmartNumber; @@ -18,6 +16,9 @@ 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. * diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java index e3f02249..a14aa844 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java @@ -6,7 +6,6 @@ package com.stuypulse.robot.subsystems.handoff; import com.stuypulse.robot.Robot; -import com.stuypulse.robot.commands.superstructure.SuperstructureSetState; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; 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 49946b48..e80f97cb 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -5,14 +5,15 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.handoff; +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.Gains; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.SysId; -import com.stuypulse.stuylib.streams.booleans.BStream; -import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; 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 45fdb341..ad2c2b4d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -5,18 +5,9 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.intake; -import java.util.Optional; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; -import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.GravityTypeValue; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.MotorAlignmentValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Gains; import com.stuypulse.robot.constants.Motors; @@ -24,13 +15,23 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.SettableNumber; import com.stuypulse.robot.util.SysId; -import com.stuypulse.stuylib.streams.booleans.BStream; -import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import java.util.Optional; + public class IntakeImpl extends Intake { private final Motors.TalonFXConfig pivotConfig; private final Motors.TalonFXConfig rollerConfig; diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/Spindexer.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/Spindexer.java index acb6517d..77a24a2e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/Spindexer.java @@ -6,6 +6,7 @@ package com.stuypulse.robot.subsystems.spindexer; import com.stuypulse.robot.constants.Settings; + import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; 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 079d1ec6..36c7a196 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -5,18 +5,17 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.spindexer; +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.Gains; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.SysId; -import com.stuypulse.stuylib.streams.booleans.BStream; -import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index 370012ad..a437398c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -1,148 +1,148 @@ -/************************ 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.subsystems.superstructure; - -import com.stuypulse.robot.subsystems.superstructure.hood.Hood; -import com.stuypulse.robot.subsystems.superstructure.hood.Hood.HoodState; -import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter; -import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter.ShooterState; -import com.stuypulse.robot.subsystems.superstructure.turret.Turret; -import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.util.superstructure.SOTMCalculator; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class Superstructure extends SubsystemBase { - - private static final Superstructure instance; - - static { - instance = new Superstructure(); - } - - public static Superstructure getInstance(){ - return instance; - } - - private SuperstructureState state; - - private final Hood hood; - private final Shooter shooter; - private final Turret turret; - - public Superstructure() { - state = SuperstructureState.INTERPOLATION; - hood = Hood.getInstance(); - shooter = Shooter.getInstance(); - turret = Turret.getInstance(); - } - - public enum SuperstructureState { - STOW(HoodState.STOW, ShooterState.SHOOT, TurretState.SHOOT), - SHOOT(HoodState.SHOOT, ShooterState.SHOOT, TurretState.SHOOT), - FERRY(HoodState.FERRY, ShooterState.FERRY, TurretState.FERRY), - FOTM(HoodState.FOTM, ShooterState.FOTM, TurretState.FOTM), - REVERSE(HoodState.SHOOT, ShooterState.REVERSE, TurretState.SHOOT), - KB(HoodState.KB, ShooterState.KB, TurretState.KB), - LEFT_CORNER(HoodState.LEFT_CORNER, ShooterState.LEFT_CORNER, TurretState.LEFT_CORNER), - RIGHT_CORNER(HoodState.RIGHT_CORNER, ShooterState.RIGHT_CORNER, TurretState.RIGHT_CORNER), - INTERPOLATION(HoodState.INTERPOLATION, ShooterState.INTERPOLATION, TurretState.SHOOT), - SOTM(HoodState.SOTM, ShooterState.SOTM, TurretState.SOTM); - - private HoodState hoodState; - private ShooterState shooterState; - private TurretState turretState; - - private SuperstructureState(HoodState hoodState, ShooterState shooterState, TurretState TurretState) { - this.hoodState = hoodState; - this.shooterState = shooterState; - this.turretState = TurretState; - } - - public HoodState getHoodState() { - return hoodState; - } - - public ShooterState getShooterState() { - return shooterState; - } - - public TurretState getTurretState(){ - return turretState; - } - } - - public void setState(SuperstructureState state){ - this.state = state; - hood.setState(state.getHoodState()); - shooter.setState(state.getShooterState()); - turret.setState(state.getTurretState()); - } - - public SuperstructureState getState(){ - return state; - } - - public boolean atTolerance() { - return isShooterAtTolerance() && isHoodAtTolerance() && isTurretAtTolerance(); - } - - public boolean isShooterAtTolerance() { - return shooter.atTolerance(); - } - - public boolean isHoodAtTolerance() { - return hood.atTolerance(); - } - - public boolean isTurretAtTolerance(){ - return turret.atTolerance(); - } - - public double getTargetRPM() { - return shooter.getTargetRPM(); - } - - public double getShooterRPM() { - return shooter.getRPM(); - } - - public Rotation2d getHoodAngle() { - return hood.getAngle(); - } - - public Rotation2d getTurretAngle(){ - return turret.getAngle(); - } - - public boolean isTurretWrapping() { - return turret.isWrapping(); - } - - @Override - public void periodic() { - SuperstructureState state = getState(); - if (state == SuperstructureState.SOTM) { - SOTMCalculator.updateSOTMSolution(); - } else if (state == SuperstructureState.FOTM){ - SOTMCalculator.updateFOTMSolution(); - } - - if (CommandSwerveDrivetrain.getInstance().isOutsideAllianceZone() && state == SuperstructureState.SOTM) { - setState(SuperstructureState.FERRY); - } - - SmartDashboard.putString("Superstructure/State", state.name()); - SmartDashboard.putString("States/SuperStructure", state.name()); - - SmartDashboard.putBoolean("Superstructure/Shooter At Tolerance?", isShooterAtTolerance()); - SmartDashboard.putBoolean("Superstructure/Hood At Tolerance?", isHoodAtTolerance()); - SmartDashboard.putBoolean("Superstructure/Turret At Tolerance?", isTurretAtTolerance()); - } +/************************ 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.subsystems.superstructure; + +import com.stuypulse.robot.subsystems.superstructure.hood.Hood; +import com.stuypulse.robot.subsystems.superstructure.hood.Hood.HoodState; +import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter; +import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter.ShooterState; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.util.superstructure.SOTMCalculator; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Superstructure extends SubsystemBase { + + private static final Superstructure instance; + + static { + instance = new Superstructure(); + } + + public static Superstructure getInstance(){ + return instance; + } + + private SuperstructureState state; + + private final Hood hood; + private final Shooter shooter; + private final Turret turret; + + public Superstructure() { + state = SuperstructureState.INTERPOLATION; + hood = Hood.getInstance(); + shooter = Shooter.getInstance(); + turret = Turret.getInstance(); + } + + public enum SuperstructureState { + STOW(HoodState.STOW, ShooterState.SHOOT, TurretState.SHOOT), + SHOOT(HoodState.SHOOT, ShooterState.SHOOT, TurretState.SHOOT), + FERRY(HoodState.FERRY, ShooterState.FERRY, TurretState.FERRY), + FOTM(HoodState.FOTM, ShooterState.FOTM, TurretState.FOTM), + REVERSE(HoodState.SHOOT, ShooterState.REVERSE, TurretState.SHOOT), + KB(HoodState.KB, ShooterState.KB, TurretState.KB), + LEFT_CORNER(HoodState.LEFT_CORNER, ShooterState.LEFT_CORNER, TurretState.LEFT_CORNER), + RIGHT_CORNER(HoodState.RIGHT_CORNER, ShooterState.RIGHT_CORNER, TurretState.RIGHT_CORNER), + INTERPOLATION(HoodState.INTERPOLATION, ShooterState.INTERPOLATION, TurretState.SHOOT), + SOTM(HoodState.SOTM, ShooterState.SOTM, TurretState.SOTM); + + private HoodState hoodState; + private ShooterState shooterState; + private TurretState turretState; + + private SuperstructureState(HoodState hoodState, ShooterState shooterState, TurretState TurretState) { + this.hoodState = hoodState; + this.shooterState = shooterState; + this.turretState = TurretState; + } + + public HoodState getHoodState() { + return hoodState; + } + + public ShooterState getShooterState() { + return shooterState; + } + + public TurretState getTurretState(){ + return turretState; + } + } + + public void setState(SuperstructureState state){ + this.state = state; + hood.setState(state.getHoodState()); + shooter.setState(state.getShooterState()); + turret.setState(state.getTurretState()); + } + + public SuperstructureState getState(){ + return state; + } + + public boolean atTolerance() { + return isShooterAtTolerance() && isHoodAtTolerance() && isTurretAtTolerance(); + } + + public boolean isShooterAtTolerance() { + return shooter.atTolerance(); + } + + public boolean isHoodAtTolerance() { + return hood.atTolerance(); + } + + public boolean isTurretAtTolerance(){ + return turret.atTolerance(); + } + + public double getTargetRPM() { + return shooter.getTargetRPM(); + } + + public double getShooterRPM() { + return shooter.getRPM(); + } + + public Rotation2d getHoodAngle() { + return hood.getAngle(); + } + + public Rotation2d getTurretAngle(){ + return turret.getAngle(); + } + + public boolean isTurretWrapping() { + return turret.isWrapping(); + } + + @Override + public void periodic() { + SuperstructureState state = getState(); + if (state == SuperstructureState.SOTM) { + SOTMCalculator.updateSOTMSolution(); + } else if (state == SuperstructureState.FOTM){ + SOTMCalculator.updateFOTMSolution(); + } + + if (CommandSwerveDrivetrain.getInstance().isOutsideAllianceZone() && state == SuperstructureState.SOTM) { + setState(SuperstructureState.FERRY); + } + + SmartDashboard.putString("Superstructure/State", state.name()); + SmartDashboard.putString("States/SuperStructure", state.name()); + + SmartDashboard.putBoolean("Superstructure/Shooter At Tolerance?", isShooterAtTolerance()); + SmartDashboard.putBoolean("Superstructure/Hood At Tolerance?", isHoodAtTolerance()); + SmartDashboard.putBoolean("Superstructure/Turret At Tolerance?", isTurretAtTolerance()); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java index 7bb8a770..85f5075f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java @@ -5,6 +5,8 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure.hood; +import com.stuypulse.stuylib.input.Gamepad; + import com.stuypulse.robot.Robot; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Settings; @@ -12,7 +14,6 @@ import com.stuypulse.robot.util.superstructure.InterpolationCalculator; import com.stuypulse.robot.util.superstructure.SOTMCalculator; import com.stuypulse.robot.util.superstructure.VisualizerHood; -import com.stuypulse.stuylib.input.Gamepad; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index 2456196b..085d5568 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -1,32 +1,33 @@ -/** ********************** PROJECT TRIBECBOT ************************ */ +/************************ 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. */ -/** ************************************************************ */ +/* 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.subsystems.superstructure.hood; -import java.util.Optional; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; -import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Gains; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.SysId; -import com.stuypulse.stuylib.streams.booleans.BStream; -import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import java.util.Optional; + public class HoodImpl extends Hood { private final Motors.TalonFXConfig hoodConfig; private final Motors.CANCoderConfig hoodEncoderConfig; diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index 24030b71..41737758 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java @@ -7,8 +7,8 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.util.superstructure.SOTMCalculator; import com.stuypulse.robot.util.superstructure.InterpolationCalculator; +import com.stuypulse.robot.util.superstructure.SOTMCalculator; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; 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 ebec6f48..851e4cf1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -5,17 +5,12 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.swerve; -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.swerve.SwerveRequest; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.util.PathPlannerLogging; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Volts; + +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.math.Vector2D; + import com.stuypulse.robot.Robot; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Field; @@ -23,8 +18,6 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.superstructure.turret.Turret; import com.stuypulse.robot.subsystems.swerve.TunerConstants.TunerSwerveDrivetrain; -import com.stuypulse.stuylib.math.Angle; -import com.stuypulse.stuylib.math.Vector2D; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; @@ -35,10 +28,6 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.util.Units; - -import static edu.wpi.first.units.Units.Second; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; @@ -46,6 +35,18 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.PathPlannerLogging; /** * Class that extends the Phoenix 6 SwerveDrivetrain class and implements * Subsystem so it can easily be used in command-based projects. diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 260e18af..1b035c46 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -5,6 +5,8 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.vision; +import com.stuypulse.stuylib.network.SmartBoolean; + import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Cameras; import com.stuypulse.robot.constants.Settings; @@ -12,7 +14,6 @@ import com.stuypulse.robot.util.vision.LimelightHelpers; import com.stuypulse.robot.util.vision.LimelightHelpers.IMUData; import com.stuypulse.robot.util.vision.LimelightHelpers.PoseEstimate; -import com.stuypulse.stuylib.network.SmartBoolean; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; diff --git a/src/main/java/com/stuypulse/robot/util/FMSUtil.java b/src/main/java/com/stuypulse/robot/util/FMSUtil.java index bb1de2d5..8158e407 100644 --- a/src/main/java/com/stuypulse/robot/util/FMSUtil.java +++ b/src/main/java/com/stuypulse/robot/util/FMSUtil.java @@ -1,12 +1,17 @@ +/************************ 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; -import java.util.Optional; - import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.Optional; + public class FMSUtil { private final Timer timer = new Timer(); diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java index 83a72148..ac3c0113 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.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.superstructure; import com.stuypulse.robot.constants.Field; diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java index c72e6da3..9c5d77ae 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java @@ -1,15 +1,20 @@ +/************************ 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.superstructure; + import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.superstructure.hood.Hood; import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.subsystems.superstructure.turret.Turret; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.superstructure.InterpolationCalculator.InterpolatedFerryInfo; import com.stuypulse.robot.util.superstructure.InterpolationCalculator.InterpolatedShotInfo; -import com.stuypulse.stuylib.math.Vector2D; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; From c88015182e3b2580c39a247278e959abab30cce3 Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Tue, 10 Mar 2026 23:53:56 -0400 Subject: [PATCH 147/150] FEAT: added intake rolling to shooting --- .../stuypulse/robot/commands/auton/regular/LeftTwoCycle.java | 3 +-- .../stuypulse/robot/commands/auton/regular/RightTwoCycle.java | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java index d14fbf90..968a6b27 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java @@ -34,7 +34,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( new WaitCommand(0.5).andThen(new IntakeDeploy()) ), - new SuperstructureInterpolation().alongWith(new IntakeStopRollers()), + new SuperstructureInterpolation(), // Trip 1 To Score CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( @@ -44,7 +44,6 @@ public LeftTwoCycle(PathPlannerPath... paths) { new HandoffRun().alongWith(new WaitUntilCommand(() -> Handoff.getInstance().atTolerance())).andThen( new SpindexerRun() ).andThen(new WaitCommand(5.0)), - new IntakeRunRollers(), // NZ Trip 2 new ParallelCommandGroup( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java index ef6aa64a..3c2f574f 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java @@ -34,7 +34,7 @@ public RightTwoCycle(PathPlannerPath... paths) { CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( new WaitCommand(0.5).andThen(new IntakeDeploy()) ), - new IntakeStopRollers(), + new SuperstructureInterpolation(), // Trip 1 To Score CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( @@ -44,7 +44,6 @@ public RightTwoCycle(PathPlannerPath... paths) { new HandoffRun().alongWith(new WaitUntilCommand(() -> Handoff.getInstance().atTolerance())).andThen( new SpindexerRun() ).andThen(new WaitCommand(5.0)), - new IntakeRunRollers(), // NZ Trip 2 new ParallelCommandGroup( From 4ef757ff8dd0a748c37e76a90fa722d5174ff29e Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Wed, 11 Mar 2026 00:00:49 -0400 Subject: [PATCH 148/150] FEAT: Updated path speeds and added constraints for speed optimization --- .../pathplanner/paths/Left NZ To Score.path | 16 ++++----- .../paths/Left Score Score Tower.path | 8 ++--- .../paths/Left Score To Score.path | 34 +++++++++---------- .../Left Trench Score To Tower Left.path | 8 ++--- .../pathplanner/paths/Left Trench To NZ.path | 22 +++++++++--- .../pathplanner/paths/Right NZ To Score.path | 12 +++---- .../paths/Right Score To Score.path | 18 +++++----- .../pathplanner/paths/Right Trench To NZ.path | 22 +++++++++--- 8 files changed, 84 insertions(+), 56 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 0e26724e..6548415c 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 7.0796433666191145, - "y": 7.237318116975751 + "x": 7.118766310794782, + "y": 7.590913404507711 }, "isLocked": false, "linkedName": "NZ Left Center" }, { "anchor": { - "x": 3.52151212553495, - "y": 7.547845934379458 + "x": 3.600510083036774, + "y": 7.580154211150653 }, "prevControl": { - "x": 6.619852845367487, - "y": 7.647103713894571 + "x": 6.698850802869312, + "y": 7.679411990665766 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 5.0, + "maxVelocity": 3.5, + "maxAcceleration": 7.5, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Left Score Score Tower.path b/src/main/deploy/pathplanner/paths/Left Score Score Tower.path index dcbf7d18..3765688a 100644 --- a/src/main/deploy/pathplanner/paths/Left Score Score Tower.path +++ b/src/main/deploy/pathplanner/paths/Left Score Score Tower.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.52151212553495, - "y": 7.547845934379458 + "x": 3.600510083036774, + "y": 7.580154211150653 }, "prevControl": null, "nextControl": { - "x": 5.877775470735339, - "y": 7.547845934384739 + "x": 5.956773428237163, + "y": 7.580154211155934 }, "isLocked": false, "linkedName": "Left Trench Score" diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path index e3019e3b..e499cbf8 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 3.52151212553495, - "y": 7.547845934379458 + "x": 3.600510083036774, + "y": 7.580154211150653 }, "prevControl": null, "nextControl": { - "x": 6.613851640513552, - "y": 7.754864479315264 + "x": 6.645361803084224, + "y": 7.558635824436536 }, "isLocked": false, "linkedName": "Left Trench Score" }, { "anchor": { - "x": 6.1351212553495005, - "y": 6.357489300998574 + "x": 5.924495848161329, + "y": 6.3858837485172 }, "prevControl": { - "x": 6.252261841806642, - "y": 7.741022388272439 + "x": 5.9890510083036785, + "y": 7.601672597864768 }, "nextControl": { - "x": 6.035218845133962, - "y": 5.177554197274526 + "x": 5.869085726034151, + "y": 5.342326448455356 }, "isLocked": false, "linkedName": null @@ -36,12 +36,12 @@ "y": 4.4707473309608545 }, "prevControl": { - "x": 5.84048778435412, - "y": 4.1030409776021175 + "x": 5.956773428232504, + "y": 4.2555634638196915 }, "nextControl": { - "x": 7.312431791226488, - "y": 4.944151838676692 + "x": 7.571772343343488, + "y": 4.914746694477237 }, "isLocked": false, "linkedName": null @@ -90,8 +90,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 5.0, + "maxVelocity": 3.0, + "maxAcceleration": 7.5, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, @@ -107,5 +107,5 @@ "velocity": 0.0, "rotation": -90.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path b/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path index e5ca1cc2..88729809 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path +++ b/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.52151212553495, - "y": 7.547845934379458 + "x": 3.600510083036774, + "y": 7.580154211150653 }, "prevControl": null, "nextControl": { - "x": 1.1437303936251038, - "y": 7.612401094521806 + "x": 1.2227283511269276, + "y": 7.6447093712930005 }, "isLocked": false, "linkedName": "Left Trench Score" diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path index cbc223fc..a0cf7e08 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -29,12 +29,26 @@ } ], "rotationTargets": [], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.596175478065245, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 5.0, + "maxVelocity": 3.5, + "maxAcceleration": 7.5, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, @@ -50,5 +64,5 @@ "velocity": 0, "rotation": -90.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index 598f2629..ca26af25 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 8.347631954350925, - "y": 1.0008844507845942 + "x": 7.2371174377224206, + "y": 0.3284578884934768 }, "isLocked": false, "linkedName": "Right Center NZ" @@ -20,8 +20,8 @@ "y": 0.44452211126961516 }, "prevControl": { - "x": 7.299600570613409, - "y": 0.4574607703281026 + "x": 7.64596678529063, + "y": 0.5651601423487547 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 5.0, + "maxVelocity": 3.5, + "maxAcceleration": 7.5, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index 1e20d396..a8fa506e 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.656120996441281, - "y": 0.4145314353499395 + "x": 7.151043890865957, + "y": 0.5006049822064051 }, "isLocked": false, "linkedName": "Right Trench Start" @@ -20,12 +20,12 @@ "y": 3.4271055753262156 }, "prevControl": { - "x": 6.010569395017794, - "y": 0.6727520759193353 + "x": 5.902977461447213, + "y": 0.7050296559905087 }, "nextControl": { - "x": 5.981149519717918, - "y": 4.438496114303611 + "x": 6.0210167361443325, + "y": 4.438021718286884 }, "isLocked": false, "linkedName": null @@ -90,8 +90,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 5.0, + "maxVelocity": 3.0, + "maxAcceleration": 7.5, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, @@ -107,5 +107,5 @@ "velocity": 0, "rotation": 90.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index 38c981b4..d78cfd07 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -29,12 +29,26 @@ } ], "rotationTargets": [], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6276715410573667, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 5.0, + "maxVelocity": 3.5, + "maxAcceleration": 7.5, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, @@ -50,5 +64,5 @@ "velocity": 0, "rotation": 90.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file From d81a0a8c49b3a4658fca0b00e125516e36226794 Mon Sep 17 00:00:00 2001 From: Jophy Wang Date: Wed, 11 Mar 2026 00:05:23 -0400 Subject: [PATCH 149/150] FEAT: one last starting position thing --- .../paths/Right Score To Score.path | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index a8fa506e..008a9baf 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 4.42896797153025, - "y": 0.5006049822064051 + "x": 3.508573466476462, + "y": 0.44452211126961516 }, "prevControl": null, "nextControl": { - "x": 7.151043890865957, - "y": 0.5006049822064051 + "x": 6.871304863582445, + "y": 0.37149466192170844 }, "isLocked": false, - "linkedName": "Right Trench Start" + "linkedName": "Right Trench Score" }, { "anchor": { @@ -20,12 +20,12 @@ "y": 3.4271055753262156 }, "prevControl": { - "x": 5.902977461447213, - "y": 0.7050296559905087 + "x": 5.956269193231364, + "y": 0.28005132838418056 }, "nextControl": { - "x": 6.0210167361443325, - "y": 4.438021718286884 + "x": 5.999810201660737, + "y": 4.459988137603796 }, "isLocked": false, "linkedName": null From c9a14632d785ff2ccebdf8ad26ce13afc23a1cc7 Mon Sep 17 00:00:00 2001 From: SP-COMPuter Date: Wed, 11 Mar 2026 12:21:34 -0400 Subject: [PATCH 150/150] FEAT: add left and right corner turret angles --- simgui.json | 2 + .../pathplanner/paths/Left NZ To Score.path | 24 +++++++--- .../paths/Left Score To Score.path | 34 +++++++++----- .../pathplanner/paths/Left Trench To NZ.path | 2 +- .../pathplanner/paths/Right NZ To Score.path | 28 +++++++++--- .../paths/Right Score To Score.path | 44 ++++++++++++------- .../Right Trench Score To Tower Right.path | 8 ++-- .../pathplanner/paths/Right Trench To NZ.path | 2 +- .../com/stuypulse/robot/RobotContainer.java | 6 +-- .../commands/auton/regular/LeftTwoCycle.java | 9 ++-- .../commands/auton/regular/RightTwoCycle.java | 9 ++-- .../SuperstructureAutoInterpolation.java | 14 ++++++ .../stuypulse/robot/constants/Settings.java | 4 +- .../superstructure/Superstructure.java | 3 +- 14 files changed, 134 insertions(+), 55 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureAutoInterpolation.java diff --git a/simgui.json b/simgui.json index 6a90a0ba..84499020 100644 --- a/simgui.json +++ b/simgui.json @@ -12,6 +12,8 @@ "/SmartDashboard/Robot/Ryan Testing Seed Hood Encoder (NEW)": "Command", "/SmartDashboard/Robot/Seed Hood Relative Encoder At Upper Hardstop": "Command", "/SmartDashboard/Robot/Seed Turret": "Command", + "/SmartDashboard/Robot/Set Megatag 1": "Command", + "/SmartDashboard/Robot/Set Megatag 2": "Command", "/SmartDashboard/Robot/Spindexer Reverse": "Command", "/SmartDashboard/Robot/Zero Hood Encoder": "Command", "/SmartDashboard/Robot/Zero Pivot Encoder at Lower Limit (Deployed)": "Command", diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 6548415c..b7b1758a 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 7.118766310794782, - "y": 7.590913404507711 + "x": 7.6360057061340925, + "y": 7.703109843081313 }, "isLocked": false, "linkedName": "NZ Left Center" @@ -20,8 +20,8 @@ "y": 7.580154211150653 }, "prevControl": { - "x": 6.698850802869312, - "y": 7.679411990665766 + "x": 7.7653922967189715, + "y": 7.6125392296718974 }, "nextControl": null, "isLocked": false, @@ -29,7 +29,21 @@ } ], "rotationTargets": [], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7829408020369174, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path index e499cbf8..273e9436 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -52,24 +52,24 @@ "y": 7.408007117437722 }, "prevControl": { - "x": 7.215599051012967, - "y": 7.386488730728886 + "x": 7.216188798669032, + "y": 7.40370263955874 }, "nextControl": { - "x": 4.90513878751039, - "y": 7.518515031468064 + "x": 4.789500713266761, + "y": 7.431398002853067 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.472004744958482, + "x": 3.7544079885877313, "y": 7.408007117437722 }, "prevControl": { - "x": 5.913736654808932, - "y": 7.42952550415712 + "x": 5.41055634807418, + "y": 7.366704707560627 }, "nextControl": null, "isLocked": false, @@ -86,12 +86,26 @@ "rotationDegrees": 90.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 3.2132399745385114, + "maxWaypointRelativePos": 4.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 7.5, + "maxVelocity": 2.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path index a0cf7e08..5bf2b1b4 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -35,7 +35,7 @@ "minWaypointRelativePos": 0.596175478065245, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 2.0, + "maxVelocity": 1.5, "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index ca26af25..4daa9b7c 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 7.2371174377224206, - "y": 0.3284578884934768 + "x": 7.90771754636234, + "y": 0.3280741797432247 }, "isLocked": false, "linkedName": "Right Center NZ" }, { "anchor": { - "x": 3.508573466476462, - "y": 0.44452211126961516 + "x": 3.042781740370898, + "y": 0.5221540656205423 }, "prevControl": { - "x": 7.64596678529063, - "y": 0.5651601423487547 + "x": 7.46780313837375, + "y": 0.5221540656205423 }, "nextControl": null, "isLocked": false, @@ -29,7 +29,21 @@ } ], "rotationTargets": [], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.782940802036919, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index 008a9baf..f8bb4623 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.508573466476462, - "y": 0.44452211126961516 + "x": 3.042781740370898, + "y": 0.5221540656205423 }, "prevControl": null, "nextControl": { - "x": 6.871304863582445, - "y": 0.37149466192170844 + "x": 6.859686162624821, + "y": 0.4703994293865924 }, "isLocked": false, "linkedName": "Right Trench Score" @@ -20,12 +20,12 @@ "y": 3.4271055753262156 }, "prevControl": { - "x": 5.956269193231364, - "y": 0.28005132838418056 + "x": 6.186875891583452, + "y": 0.2504422253922973 }, "nextControl": { - "x": 5.999810201660737, - "y": 4.459988137603796 + "x": 5.924849720295674, + "y": 4.458047058785347 }, "isLocked": false, "linkedName": null @@ -64,12 +64,12 @@ }, { "anchor": { - "x": 4.052396204033215, - "y": 0.6512336892052202 + "x": 3.5991440798858774, + "y": 0.71623395149786 }, "prevControl": { - "x": 6.171957295373667, - "y": 0.6512336892052202 + "x": 5.718705171226329, + "y": 0.71623395149786 }, "nextControl": null, "isLocked": false, @@ -86,12 +86,26 @@ "rotationDegrees": -90.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 3.467854869509843, + "maxWaypointRelativePos": 4.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 7.5, + "maxVelocity": 2.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path b/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path index 38a11739..d55f75b1 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path +++ b/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.508573466476462, - "y": 0.44452211126961516 + "x": 3.042781740370898, + "y": 0.5221540656205423 }, "prevControl": null, "nextControl": { - "x": 0.45777196239706797, - "y": 0.6567427903238829 + "x": -0.008019763708495553, + "y": 0.7343747446748101 }, "isLocked": false, "linkedName": "Right Trench Score" diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index d78cfd07..288b8ca0 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -35,7 +35,7 @@ "minWaypointRelativePos": 0.6276715410573667, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 2.0, + "maxVelocity": 1.5, "maxAcceleration": 5.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index bc90e9f4..e2147423 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -250,7 +250,7 @@ private void configureButtonBindings() { .whileTrue(new SwerveXMode()) .onTrue(new IntakeRunRollers()) .whileTrue(new SuperstructureLeftCorner().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) - .andThen(new HandoffRun()).alongWith(new WaitUntilCommand(() -> handoff.atTolerance()) + .andThen(new HandoffRun()).alongWith(new WaitUntilCommand(() -> handoff.getState() == HandoffState.FORWARD && handoff.atTolerance()) .andThen(new SpindexerRun()))) .onFalse(new SuperstructureStow().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); @@ -259,7 +259,7 @@ private void configureButtonBindings() { .whileTrue(new SwerveXMode()) .onTrue(new IntakeRunRollers()) .whileTrue(new SuperstructureRightCorner().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) - .andThen(new HandoffRun()).alongWith(new WaitUntilCommand(() -> handoff.atTolerance()) + .andThen(new HandoffRun()).alongWith(new WaitUntilCommand(() -> handoff.getState() == HandoffState.FORWARD && handoff.atTolerance()) .andThen(new SpindexerRun()))) .onFalse(new SuperstructureStow().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); @@ -268,7 +268,7 @@ private void configureButtonBindings() { .whileTrue(new SwerveXMode()) .onTrue(new IntakeRunRollers()) .whileTrue(new SuperstructureKB().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) - .andThen(new HandoffRun()).alongWith(new WaitUntilCommand(() -> handoff.atTolerance()) + .andThen(new HandoffRun()).alongWith(new WaitUntilCommand(() -> handoff.getState() == HandoffState.FORWARD && handoff.atTolerance()) .andThen(new SpindexerRun()))) .onFalse(new SuperstructureStow().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java index 968a6b27..4d2dc494 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java @@ -12,6 +12,7 @@ import com.stuypulse.robot.commands.intake.IntakeStopRollers; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.superstructure.Superstructure; @@ -34,16 +35,17 @@ public LeftTwoCycle(PathPlannerPath... paths) { CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( new WaitCommand(0.5).andThen(new IntakeDeploy()) ), - new SuperstructureInterpolation(), // Trip 1 To Score CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new WaitCommand(0.5).andThen(new SuperstructureInterpolation()) + new WaitCommand(0.5).andThen(new SuperstructureAutoInterpolation()) ), + new SuperstructureInterpolation(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new HandoffRun().alongWith(new WaitUntilCommand(() -> Handoff.getInstance().atTolerance())).andThen( new SpindexerRun() - ).andThen(new WaitCommand(5.0)), + ).andThen(new WaitCommand(4.0)), + new SuperstructureAutoInterpolation(), // NZ Trip 2 new ParallelCommandGroup( @@ -51,6 +53,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { new HandoffStop(), new SpindexerStop() ), + new SuperstructureInterpolation(), new ParallelCommandGroup( new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java index 3c2f574f..6d96ddf5 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java @@ -12,6 +12,7 @@ import com.stuypulse.robot.commands.intake.IntakeStopRollers; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.superstructure.Superstructure; @@ -34,16 +35,17 @@ public RightTwoCycle(PathPlannerPath... paths) { CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( new WaitCommand(0.5).andThen(new IntakeDeploy()) ), - new SuperstructureInterpolation(), // Trip 1 To Score CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new WaitCommand(0.5).andThen(new SuperstructureInterpolation()) + new WaitCommand(0.5).andThen(new SuperstructureAutoInterpolation()) ), + new SuperstructureInterpolation(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new HandoffRun().alongWith(new WaitUntilCommand(() -> Handoff.getInstance().atTolerance())).andThen( new SpindexerRun() - ).andThen(new WaitCommand(5.0)), + ).andThen(new WaitCommand(4.0)), + new SuperstructureAutoInterpolation(), // NZ Trip 2 new ParallelCommandGroup( @@ -51,6 +53,7 @@ public RightTwoCycle(PathPlannerPath... paths) { new HandoffStop(), new SpindexerStop() ), + new SuperstructureInterpolation(), new ParallelCommandGroup( new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureAutoInterpolation.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureAutoInterpolation.java new file mode 100644 index 00000000..589eaa7f --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureAutoInterpolation.java @@ -0,0 +1,14 @@ +/************************ 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.commands.superstructure; + +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; + +public class SuperstructureAutoInterpolation extends SuperstructureSetState { + public SuperstructureAutoInterpolation() { + super(SuperstructureState.AUTO_INTERPOLATION); + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 32e950b7..dc08e433 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -213,8 +213,8 @@ public interface Turret { public final Rotation2d SOTM_TOLERANCE = Rotation2d.fromDegrees(5.0); public final Rotation2d KB = Rotation2d.fromDegrees(0.0); - public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(-127.0); - public final Rotation2d RIGHT_CORNER = Rotation2d.fromDegrees(-60.0); + public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(-233.0); + public final Rotation2d RIGHT_CORNER = Rotation2d.fromDegrees(53.0); double RESOLUTION_OF_ABSOLUTE_ENCODER = 0.1; double WRAP_DEBOUNCE = 0.5; diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index a437398c..9a8cd38b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -44,7 +44,7 @@ public Superstructure() { } public enum SuperstructureState { - STOW(HoodState.STOW, ShooterState.SHOOT, TurretState.SHOOT), + STOW(HoodState.STOW, ShooterState.INTERPOLATION, TurretState.SHOOT), SHOOT(HoodState.SHOOT, ShooterState.SHOOT, TurretState.SHOOT), FERRY(HoodState.FERRY, ShooterState.FERRY, TurretState.FERRY), FOTM(HoodState.FOTM, ShooterState.FOTM, TurretState.FOTM), @@ -53,6 +53,7 @@ public enum SuperstructureState { LEFT_CORNER(HoodState.LEFT_CORNER, ShooterState.LEFT_CORNER, TurretState.LEFT_CORNER), RIGHT_CORNER(HoodState.RIGHT_CORNER, ShooterState.RIGHT_CORNER, TurretState.RIGHT_CORNER), INTERPOLATION(HoodState.INTERPOLATION, ShooterState.INTERPOLATION, TurretState.SHOOT), + AUTO_INTERPOLATION(HoodState.STOW, ShooterState.INTERPOLATION, TurretState.SHOOT), SOTM(HoodState.SOTM, ShooterState.SOTM, TurretState.SOTM); private HoodState hoodState;