--- /dev/null
+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
+ }
+}
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
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;
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) {
hood.forceHoodDown(false);
}));
}
+
+ controller.get(PS5Button.TOUCHPAD).onTrue(new InstantCommand(() -> {
+ new BrownOutControl(shooter, spindexer, turret, intake, hood, getDrivetrain());
+ }));
}
@Override
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...
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);
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;
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 {
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)));
}
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;
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
}
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;
--- /dev/null
+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
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;
}
+
}
--- /dev/null
+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() {}
+}