package frc.robot;
-import java.util.concurrent.ScheduledExecutorService;
import java.util.function.BooleanSupplier;
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.ScheduleCommand;
+import frc.robot.commands.LogCommand;
import frc.robot.commands.drive_comm.DefaultDriveCommand;
import frc.robot.commands.gpm.AutoShootCommand;
-import frc.robot.commands.gpm.BrownOutControl;
import frc.robot.commands.gpm.ClimbDriveCommand;
import frc.robot.commands.gpm.IntakeMovementCommand;
import frc.robot.commands.gpm.RunSpindexer;
turret.setDefaultCommand(new Superstructure(turret, drive, hood, shooter, spindexer));
}
- if (shooter != null && spindexer != null && turret != null && intake != null && hood != null && drive != null) {
- CommandScheduler.getInstance().schedule(new BrownOutControl(shooter, spindexer, turret, intake, hood, drive));
- }
-
drive.setDefaultCommand(new DefaultDriveCommand(drive, driver));
break;
}
+++ /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.
- // voltage 6.3 is brownout where issues occur, but 4.75 is dead robot
- double batteryVoltage = RobotController.getBatteryVoltage();
- if (batteryVoltage > 7.5) { // normal
- return levels[0];
- } else if (batteryVoltage > 6.75) { // if 7.5 to 6.75
- return levels[1]; // lower drivetrain
- } else if (batteryVoltage > 6.0) { // if 6.75 to 6.0 (browning out)
- return levels[2];
- } else if (batteryVoltage > 5.25) { // if 6.0 to 5.0 (mayday)
- return levels[3];
- } else { // were are on life support at this point 5.25 to 4.75
- 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;
-
- // apply them / set them
- shooter.setNewCurrentLimit(shooterCurrent);
- turret.setCurrentLimits(turretCurrent);
- hood.setCurrentLimits(hoodCurrent);
- spindexer.setNewCurrentLimit(spindexerCurrent);
- intake.setCurrentLimits(intakeCurrent);
- drivetrain.applyNewModuleCurrents(steerCurrent, driveCurrent);
- }
-
- @Override
- public void end(boolean interrupted) {
- // Nothing
- applyLevel(levels[0]); // disable
- }
-}
*
* @return Whether Y coordinate is in the upper half (left side on blue alliance)
*/
- public static boolean isOnLeftSideOfField(Translation2d drivepose) {
+ public static boolean isOnLeftSideOfField(Translation2d drivepose){
return drivepose.getY() > FIELD_WIDTH/2;
}
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
public static final double STEER_PEAK_CURRENT_DURATION = 0.01;
public static final boolean STEER_ENABLE_CURRENT_LIMIT = true;
- public static final int DRIVE_CONTINUOUS_CURRENT_LIMIT = 40;
- public static final int DRIVE_PEAK_CURRENT_LIMIT = 40;
+ public static final int DRIVE_CONTINUOUS_CURRENT_LIMIT = 60;
+ public static final int DRIVE_PEAK_CURRENT_LIMIT = 60;
public static final double DRIVE_PEAK_CURRENT_DURATION = 0.01;
public static final boolean DRIVE_ENABLE_CURRENT_LIMIT = true;
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;
hood.forceHoodDown(false);
}));
}
-
- // turns it on
- // 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);
trenchAlign = target;
}
- // for current limit setting (brownout protection)
- public void applyNewModuleCurrents(double steerCurrent, double driveCurren) {
- for (Module module : modules) { // iterate over our modules
- module.setNewCurrentLimit(steerCurrent, driveCurren);
- }
- }
-
/**
* Sets the desired states for all swerve modules.
*
public double getDriveStatorCurrent(){
return inputs.driveCurrentAmps;
}
-
- // I took the config things straight from this file
- public void setNewCurrentLimit(double currentSteer, double currentDrive) {
- CurrentLimitsConfigs steerConfig = new CurrentLimitsConfigs();
- // steer
- steerConfig.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT;
- steerConfig.SupplyCurrentLimit = currentSteer;
- steerConfig.SupplyCurrentLowerLimit = currentSteer;
- steerConfig.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
- angleMotor.getConfigurator().apply(steerConfig); // apply
-
- // drive
- CurrentLimitsConfigs dirveConfig = new CurrentLimitsConfigs();
- dirveConfig.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
- dirveConfig.SupplyCurrentLimit = currentDrive;
- dirveConfig.SupplyCurrentLowerLimit = currentDrive;
- dirveConfig.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
- dirveConfig.StatorCurrentLimit = currentDrive;
- dirveConfig.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
- driveMotor.getConfigurator().apply(dirveConfig); // apply
- }
-
private void configDriveMotor() {
var talonFXConfigs = new TalonFXConfiguration();
// set Motion Magic settings
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 = 40.0;
}
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 * 1.0, // keep as same
- HoodConstants.NORMAL_CURRENT_LIMIT * 1.0, // preserve aiming speed
- SpindexerConstants.currentLimit * 1.0, // preserve indexing speed
- TurretConstants.NORMAL_CURRENT_LIMIT * 1.0, // preserve aiming speed
- IntakeConstants.NORMAL_CURRENT_LIMIT * 1.0, // preserve indexing speed
- DriveConstants.STEER_PEAK_CURRENT_LIMIT * 0.8, // lower drive rotation
- DriveConstants.DRIVE_PEAK_CURRENT_LIMIT * 0.8 // lower drive movement
- );
-
- // 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 * 1.0, // keep as same
- HoodConstants.NORMAL_CURRENT_LIMIT * 1.0, // preserve aiming speed
- SpindexerConstants.currentLimit * 0.8, // preserve indexing speed
- TurretConstants.NORMAL_CURRENT_LIMIT * 1.0, // preserve aiming speed
- IntakeConstants.NORMAL_CURRENT_LIMIT * 0.8, // preserve indexing speed
- DriveConstants.STEER_PEAK_CURRENT_LIMIT * 0.7, // lower drive rotation
- DriveConstants.DRIVE_PEAK_CURRENT_LIMIT * 0.7 // lower drive movement
- );
-
- // 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 * 1.0, // keep as same
- HoodConstants.NORMAL_CURRENT_LIMIT * 0.8, // preserve aiming speed
- SpindexerConstants.currentLimit * 0.6, // preserve indexing speed
- TurretConstants.NORMAL_CURRENT_LIMIT * 0.8, // preserve aiming speed
- IntakeConstants.NORMAL_CURRENT_LIMIT * 0.6, // preserve indexing speed
- DriveConstants.STEER_PEAK_CURRENT_LIMIT * 0.5, // lower drive rotation
- DriveConstants.DRIVE_PEAK_CURRENT_LIMIT * 0.5 // lower drive movement
- );
-
- // 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 * 0.8, // keep as same
- HoodConstants.NORMAL_CURRENT_LIMIT * 0.7, // preserve aiming speed
- SpindexerConstants.currentLimit * 0.5, // preserve indexing speed
- TurretConstants.NORMAL_CURRENT_LIMIT * 0.7, // preserve aiming speed
- IntakeConstants.NORMAL_CURRENT_LIMIT * 0.5, // preserve indexing speed
- DriveConstants.STEER_PEAK_CURRENT_LIMIT * 0.45, // lower drive rotation
- DriveConstants.DRIVE_PEAK_CURRENT_LIMIT * 0.45 // lower drive movement
- );
-
-}
\ No newline at end of file
+++ /dev/null
-package frc.robot.util.BrownOut;
-
-public class BrownOutLevel {
- // in percentage values (0.0-1.0)
- 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;
- }
-
-}
-