From: Wesley28w Date: Sat, 21 Mar 2026 15:53:59 +0000 (-0700) Subject: Initial Shell X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=3743775d8d136045ff3102f55d069f5f48a634dd;p=FRC2026.git Initial Shell --- diff --git a/src/main/java/frc/robot/commands/gpm/BrownOutControl.java b/src/main/java/frc/robot/commands/gpm/BrownOutControl.java new file mode 100644 index 0000000..8f6f270 --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/BrownOutControl.java @@ -0,0 +1,87 @@ +package frc.robot.commands.gpm; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Intake.Intake; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.subsystems.hood.Hood; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.spindexer.Spindexer; +import frc.robot.subsystems.spindexer.SpindexerConstants; +import frc.robot.subsystems.turret.Turret; +import frc.robot.util.BrownOut.BrownOutConstants; +import frc.robot.util.BrownOut.BrownOutLevel; + +public class BrownOutControl extends Command { + private Shooter shooter; + private Spindexer spindexer; + private Turret turret; + private Intake intake; + private Hood hood; + private Drivetrain drivetrain; + + public BrownOutLevel[] levels = { + BrownOutConstants.BROWNOUT_LVL_ONE, + BrownOutConstants.BROWNOUT_LVL_TWO, + BrownOutConstants.BROWNOUT_LVL_THREE, + BrownOutConstants.BROWNOUT_LVL_FOUR, + BrownOutConstants.BROWNOUT_LVL_FIVE, + }; + + public BrownOutControl(Shooter shooter, Spindexer spindexer, Turret turret, Intake intake, Hood hood, Drivetrain drivetrain) { + this.shooter = shooter; + this.spindexer = spindexer; + this.turret = turret; + this.intake = intake; + this.hood = hood; + this.drivetrain = drivetrain; + } + + @Override + public void execute() { + BrownOutLevel level = monitor(); + applyLevel(level); + } + + public BrownOutLevel monitor() { + // pretty sure this is where you get it. Need to check if this is same as logs. + double batteryVoltage = RobotController.getBatteryVoltage(); + if (batteryVoltage > 8.25) { + return levels[0]; + } else if (batteryVoltage > 7.75) { + return levels[1]; + } else if (batteryVoltage > 7.25) { + return levels[2]; + } else if (batteryVoltage > 6.75) { + return levels[3]; + } else { + return levels[4]; + } + } + + public void applyLevel(BrownOutLevel level) { + double shooterCurrent = level.shooterCurrent; + double turretCurrent = level.turretCurrent; + double hoodCurrent = level.hoodCurrent; + double spindexerCurrent = level.spindexerCurrent; + double intakeCurrent = level.intakeCurrent; + double steerCurrent = level.steerCurrent; + double driveCurrent = level.driveCurrent; + + shooter.setNewCurrentLimit(shooterCurrent); + turret.setCurrentLimits(turretCurrent); + hood.setCurrentLimits(hoodCurrent); + spindexer.setNewCurrentLimit(spindexerCurrent); + intake.setCurrentLimits(intakeCurrent); + + // TODO: set drivetrain currents. I'll do it once we fix drivetrain. + } + + @Override + public void end(boolean interrupted) { + // Nothing + applyLevel(levels[0]); // disable + } +} diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java index b58189c..3584ff8 100644 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ b/src/main/java/frc/robot/constants/IntakeConstants.java @@ -15,12 +15,14 @@ public class IntakeConstants { public static final double CALIBRATING_CURRENT_LIMITS = 10.0; public static final double CALIBRATING_CURRENT_THRESHOLD = 9.0; + /**Current */ + public static final double NORMAL_CURRENT_LIMIT = 100.0; + public static final double ROLLER_MOI_KG_M_SQ = 0.5 * 0.020 * 0.020; // 0.5kg roller, 20mm radius for now public static final double ROLLER_GEARING = 2.0; public static final double CARRIAGE_MASS_KG = 3.0; public static final double DRUM_RADIUS_METERS = Units.inchesToMeters(1.0); - /** max extension in inches */ public static final double MAX_EXTENSION = 10.5; // inches diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index abc8eb7..0cf022b 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.robot.Robot; +import frc.robot.commands.gpm.BrownOutControl; import frc.robot.commands.gpm.IntakeMovementCommand; import frc.robot.commands.gpm.ReverseMotors; import frc.robot.commands.gpm.RunSpindexer; @@ -155,30 +156,30 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { controller.get(PS5Button.SQUARE).toggleOnTrue(autoShoot); } - // Climb - if (climb != null) { - // Calibration - controller.get(PS5Button.OPTIONS).onTrue(new InstantCommand(() -> { - climb.hardstopCalibration(); - })).onFalse(new InstantCommand(() -> { - climb.stopCalibrating(); - })); - - // Climb retract - controller.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> { - climb.retract(); - })); - - // Go to up position - controller.get(PS5Button.TRIANGLE).onTrue(new InstantCommand(() -> { - climb.goUp(); - })); - - // Go to climb position - controller.get(PS5Button.TOUCHPAD).onTrue(new InstantCommand(() -> { - climb.climbPosition(); - })); - } + // // Climb + // if (climb != null) { + // // Calibration + // controller.get(PS5Button.OPTIONS).onTrue(new InstantCommand(() -> { + // climb.hardstopCalibration(); + // })).onFalse(new InstantCommand(() -> { + // climb.stopCalibrating(); + // })); + + // // Climb retract + // controller.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> { + // climb.retract(); + // })); + + // // Go to up position + // controller.get(PS5Button.TRIANGLE).onTrue(new InstantCommand(() -> { + // climb.goUp(); + // })); + + // // Go to climb position + // controller.get(PS5Button.TOUCHPAD).onTrue(new InstantCommand(() -> { + // climb.climbPosition(); + // })); + // } // Hood if (hood != null) { @@ -196,6 +197,10 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { hood.forceHoodDown(false); })); } + + controller.get(PS5Button.TOUCHPAD).onTrue(new InstantCommand(() -> { + new BrownOutControl(shooter, spindexer, turret, intake, hood, getDrivetrain()); + })); } @Override diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index f4bfdee..0dd0bae 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; import frc.robot.constants.IdConstants; import frc.robot.constants.IntakeConstants; +import frc.robot.subsystems.shooter.ShooterConstants; public class Intake extends SubsystemBase implements IntakeIO{ // Mechanism Display... @@ -133,6 +134,12 @@ public class Intake extends SubsystemBase implements IntakeIO{ new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast) ); + CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); + limitConfig.StatorCurrentLimit = IntakeConstants.NORMAL_CURRENT_LIMIT; + limitConfig.StatorCurrentLimitEnable = true; + leftMotor.getConfigurator().apply(limitConfig); + rightMotor.getConfigurator().apply(limitConfig); + leftMotor.setPosition(0.0); rightMotor.setPosition(0.0); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 5e751e2..f39dfb4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -3,6 +3,7 @@ package frc.robot.subsystems.shooter; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.VelocityVoltage; @@ -16,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; import frc.robot.constants.IdConstants; +import frc.robot.subsystems.spindexer.SpindexerConstants; import frc.robot.util.HubActive; public class Shooter extends SubsystemBase implements ShooterIO { @@ -54,6 +56,12 @@ public class Shooter extends SubsystemBase implements ShooterIO { new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast) ); + CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); + limitConfig.StatorCurrentLimit = ShooterConstants.SHOOTER_CURRENT_LIMIT; + limitConfig.StatorCurrentLimitEnable = true; + shooterMotorLeft.getConfigurator().apply(limitConfig); + shooterMotorRight.getConfigurator().apply(limitConfig); + SmartDashboard.putData("Turn on shooter", new InstantCommand(()-> setShooter(12.0))); } @@ -97,6 +105,14 @@ public class Shooter extends SubsystemBase implements ShooterIO { return inputs.shooterSpeedLeft; } + public void setNewCurrentLimit(double newCurrentLimit) { + CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); + limitConfig.StatorCurrentLimit = newCurrentLimit; + limitConfig.StatorCurrentLimitEnable = true; + shooterMotorLeft.getConfigurator().apply(limitConfig); + shooterMotorRight.getConfigurator().apply(limitConfig); + } + @Override public void updateInputs(){ inputs.shooterSpeedLeft = Units.rotationsToRadians(shooterMotorLeft.getVelocity().getValueAsDouble()) * ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index e320c55..cdcf89d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -5,4 +5,5 @@ import edu.wpi.first.math.util.Units; public class ShooterConstants { public static final double SHOOTER_VELOCITY = 1.0; public static final double SHOOTER_LAUNCH_DIAMETER = Units.inchesToMeters(4.0); + public static final double SHOOTER_CURRENT_LIMIT = 100.0; // TODO: tune } diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java index 53ed3e9..6ad6279 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java @@ -84,6 +84,15 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { return inputs.spindexerCurrent; } + public void setNewCurrentLimit(double newCurrentLimit) { + CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); + limitConfig.StatorCurrentLimit = SpindexerConstants.CURRENT_SPIKE_LIMIT; + limitConfig.StatorCurrentLimitEnable = true; + limitConfig.SupplyCurrentLowerLimit = newCurrentLimit; + limitConfig.SupplyCurrentLowerTime = 1.5; + motor.getConfigurator().apply(limitConfig); + } + @Override public void updateInputs() { inputs.spindexerVelocity = motor.getVelocity().getValueAsDouble(); //SpindexerConstants.gearRatio; diff --git a/src/main/java/frc/robot/util/BrownOut/BrownOutConstants.java b/src/main/java/frc/robot/util/BrownOut/BrownOutConstants.java new file mode 100644 index 0000000..eb0c3d4 --- /dev/null +++ b/src/main/java/frc/robot/util/BrownOut/BrownOutConstants.java @@ -0,0 +1,80 @@ +package frc.robot.util.BrownOut; + +import frc.robot.constants.IntakeConstants; +import frc.robot.constants.swerve.DriveConstants; +import frc.robot.subsystems.hood.HoodConstants; +import frc.robot.subsystems.shooter.ShooterConstants; +import frc.robot.subsystems.spindexer.SpindexerConstants; +import frc.robot.subsystems.turret.TurretConstants; + +public class BrownOutConstants { + /* level one should be less protected that level two and so on. So the current limits will be higher at lvl one. + Order is: + Shooter, (applied to both motors) + Hood, + Spindexer, + Turret, + Intake, + Steer (main power draw) (is applied to continous and peak) + Drive (main power draw) (is applied to continous and peak) + */ + + // currently for show. I would imagine u would decrease movement: drivetrain, then bps impacters: intake/indexing speed, and then a bit on aiming: turret/hood. + // I don't see a world where you would decrease shooter current, but we need to do some testing to see how much current we are at when shooting + + // normal + public static final BrownOutLevel BROWNOUT_LVL_ONE = new BrownOutLevel( + ShooterConstants.SHOOTER_CURRENT_LIMIT, + HoodConstants.NORMAL_CURRENT_LIMIT, + SpindexerConstants.currentLimit, + TurretConstants.NORMAL_CURRENT_LIMIT, + IntakeConstants.NORMAL_CURRENT_LIMIT, + DriveConstants.STEER_PEAK_CURRENT_LIMIT, + DriveConstants.DRIVE_PEAK_CURRENT_LIMIT + ); + + // should deplete drivetrain a bit and lower everything else slightly. Preserve Shooter. + public static final BrownOutLevel BROWNOUT_LVL_TWO = new BrownOutLevel( + ShooterConstants.SHOOTER_CURRENT_LIMIT, + HoodConstants.NORMAL_CURRENT_LIMIT, + SpindexerConstants.currentLimit, + TurretConstants.NORMAL_CURRENT_LIMIT, + IntakeConstants.NORMAL_CURRENT_LIMIT, + DriveConstants.STEER_PEAK_CURRENT_LIMIT, + DriveConstants.DRIVE_PEAK_CURRENT_LIMIT + ); + + // lower bps systems: intake & spindexer. Preserve Shooter. Slight lower on evertthing else + public static final BrownOutLevel BROWNOUT_LVL_THREE = new BrownOutLevel( + ShooterConstants.SHOOTER_CURRENT_LIMIT, + HoodConstants.NORMAL_CURRENT_LIMIT, + SpindexerConstants.currentLimit, + TurretConstants.NORMAL_CURRENT_LIMIT, + IntakeConstants.NORMAL_CURRENT_LIMIT, + DriveConstants.STEER_PEAK_CURRENT_LIMIT, + DriveConstants.DRIVE_PEAK_CURRENT_LIMIT + ); + + // lower aiming systems: turret & hood. Preserve Shooter. Slight lower on everything else + public static final BrownOutLevel BROWNOUT_LVL_FOUR = new BrownOutLevel( + ShooterConstants.SHOOTER_CURRENT_LIMIT, + HoodConstants.NORMAL_CURRENT_LIMIT, + SpindexerConstants.currentLimit, + TurretConstants.NORMAL_CURRENT_LIMIT, + IntakeConstants.NORMAL_CURRENT_LIMIT, + DriveConstants.STEER_PEAK_CURRENT_LIMIT, + DriveConstants.DRIVE_PEAK_CURRENT_LIMIT + ); + + // now we have to deplete shooter... THIS IS REALLY BAD IS IT COMES TO THIS. + public static final BrownOutLevel BROWNOUT_LVL_FIVE = new BrownOutLevel( + ShooterConstants.SHOOTER_CURRENT_LIMIT, + HoodConstants.NORMAL_CURRENT_LIMIT, + SpindexerConstants.currentLimit, + TurretConstants.NORMAL_CURRENT_LIMIT, + IntakeConstants.NORMAL_CURRENT_LIMIT, + DriveConstants.STEER_PEAK_CURRENT_LIMIT, + DriveConstants.DRIVE_PEAK_CURRENT_LIMIT + ); + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/BrownOut/BrownOutLevel.java b/src/main/java/frc/robot/util/BrownOut/BrownOutLevel.java index 69943d0..11b1992 100644 --- a/src/main/java/frc/robot/util/BrownOut/BrownOutLevel.java +++ b/src/main/java/frc/robot/util/BrownOut/BrownOutLevel.java @@ -2,14 +2,31 @@ package frc.robot.util.BrownOut; public class BrownOutLevel { // in percentage values (0.0-1.0) - double shooterCurrent; - double hoodCurrent; - double spindexerCurrent; - double intakeCurrent; - double drivetrainCurrent; - - public BrownOutLevel() { - + public double shooterCurrent; // A -> priority one + public double hoodCurrent; // A -> priority two + public double turretCurrent; // A -> priority two + public double spindexerCurrent; // A -> priority three + public double intakeCurrent; // A -> priority three + public double steerCurrent; // A -> priority four? (idk) + public double driveCurrent; // A -> priority four? (idk) + + public BrownOutLevel( + double shooterCurrent, + double hoodCurrent, + double turretCurrent, + double spindexerCurrent, + double intakeCurrent, + double steerCurrent, + double driveCurrent + ) { + this.shooterCurrent = shooterCurrent; + this.hoodCurrent = hoodCurrent; + this.spindexerCurrent = spindexerCurrent; + this.intakeCurrent = intakeCurrent; + this.turretCurrent = turretCurrent; + this.steerCurrent = steerCurrent; + this.driveCurrent = driveCurrent; } + } diff --git a/src/main/java/frc/robot/util/BrownOut/BrownOutManager.java b/src/main/java/frc/robot/util/BrownOut/BrownOutManager.java new file mode 100644 index 0000000..5cf942b --- /dev/null +++ b/src/main/java/frc/robot/util/BrownOut/BrownOutManager.java @@ -0,0 +1,17 @@ +package frc.robot.util.BrownOut; + +public class BrownOutManager { + public BrownOutLevel[] levels = { + BrownOutConstants.BROWNOUT_LVL_ONE, + BrownOutConstants.BROWNOUT_LVL_TWO, + BrownOutConstants.BROWNOUT_LVL_THREE, + BrownOutConstants.BROWNOUT_LVL_FOUR, + BrownOutConstants.BROWNOUT_LVL_FIVE, + }; + + public void monitor() { + + } + + public void apply() {} +}